24 #include <interfaces/NavigatorInterface.h>
26 #include <core/exceptions/software.h>
76 NavigatorInterface::NavigatorInterface() : Interface()
78 data_size =
sizeof(NavigatorInterface_data_t);
80 data = (NavigatorInterface_data_t *)
data_ptr;
106 unsigned char tmp_hash[] = {0x90, 0x6b, 0x4d, 0xeb, 0x52, 0x4d, 0x53, 0x73, 0x4c, 0xbc, 0x82, 0x5, 0x80, 0x81, 0xf1, 0x39};
111 NavigatorInterface::~NavigatorInterface()
145 data->flags = new_flags;
238 data->dest_x = new_dest_x;
269 data->dest_y = new_dest_y;
280 return data->dest_ori;
300 data->dest_ori = new_dest_ori;
311 return data->dest_dist;
331 data->dest_dist = new_dest_dist;
364 data->msgid = new_msgid;
397 data->final = new_final;
410 return data->error_code;
432 data->error_code = new_error_code;
443 return data->max_velocity;
463 data->max_velocity = new_max_velocity;
475 return data->security_distance;
496 data->security_distance = new_security_distance;
510 return data->escaping_enabled;
533 data->escaping_enabled = new_escaping_enabled;
541 if ( strncmp(
"StopMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
543 }
else if ( strncmp(
"TurnMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
545 }
else if ( strncmp(
"CartesianGotoMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
547 }
else if ( strncmp(
"PolarGotoMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
549 }
else if ( strncmp(
"PlaceGotoMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
551 }
else if ( strncmp(
"ObstacleMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
553 }
else if ( strncmp(
"ResetOdometryMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
555 }
else if ( strncmp(
"SetMaxVelocityMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
557 }
else if ( strncmp(
"SetEscapingMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
559 }
else if ( strncmp(
"SetSecurityDistanceMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
563 "message type for this interface type.", type);
579 memcpy(data, oi->data,
sizeof(NavigatorInterface_data_t));
602 data = (StopMessage_data_t *)
data_ptr;
620 data = (StopMessage_data_t *)
data_ptr;
651 data = (TurnMessage_data_t *)
data_ptr;
653 data->angle = ini_angle;
654 data->velocity = ini_velocity;
664 data = (TurnMessage_data_t *)
data_ptr;
684 data = (TurnMessage_data_t *)
data_ptr;
716 data->angle = new_angle;
727 return data->velocity;
748 data->velocity = new_velocity;
775 data_size =
sizeof(CartesianGotoMessage_data_t);
778 data = (CartesianGotoMessage_data_t *)
data_ptr;
782 data->orientation = ini_orientation;
790 data_size =
sizeof(CartesianGotoMessage_data_t);
793 data = (CartesianGotoMessage_data_t *)
data_ptr;
814 data = (CartesianGotoMessage_data_t *)
data_ptr;
886 return data->orientation;
906 data->orientation = new_orientation;
933 data_size =
sizeof(PolarGotoMessage_data_t);
936 data = (PolarGotoMessage_data_t *)
data_ptr;
939 data->dist = ini_dist;
940 data->orientation = ini_orientation;
948 data_size =
sizeof(PolarGotoMessage_data_t);
951 data = (PolarGotoMessage_data_t *)
data_ptr;
972 data = (PolarGotoMessage_data_t *)
data_ptr;
1004 data->phi = new_phi;
1034 data->dist = new_dist;
1044 return data->orientation;
1064 data->orientation = new_orientation;
1089 data_size =
sizeof(PlaceGotoMessage_data_t);
1092 data = (PlaceGotoMessage_data_t *)
data_ptr;
1094 strncpy(data->place, ini_place, 64);
1100 data_size =
sizeof(PlaceGotoMessage_data_t);
1103 data = (PlaceGotoMessage_data_t *)
data_ptr;
1122 data = (PlaceGotoMessage_data_t *)
data_ptr;
1154 strncpy(data->place, new_place,
sizeof(data->place));
1181 data_size =
sizeof(ObstacleMessage_data_t);
1184 data = (ObstacleMessage_data_t *)
data_ptr;
1188 data->width = ini_width;
1196 data_size =
sizeof(ObstacleMessage_data_t);
1199 data = (ObstacleMessage_data_t *)
data_ptr;
1220 data = (ObstacleMessage_data_t *)
data_ptr;
1312 data->width = new_width;
1335 data_size =
sizeof(ResetOdometryMessage_data_t);
1338 data = (ResetOdometryMessage_data_t *)
data_ptr;
1356 data = (ResetOdometryMessage_data_t *)
data_ptr;
1383 data_size =
sizeof(SetMaxVelocityMessage_data_t);
1386 data = (SetMaxVelocityMessage_data_t *)
data_ptr;
1388 data->max_velocity = ini_max_velocity;
1394 data_size =
sizeof(SetMaxVelocityMessage_data_t);
1397 data = (SetMaxVelocityMessage_data_t *)
data_ptr;
1416 data = (SetMaxVelocityMessage_data_t *)
data_ptr;
1428 return data->max_velocity;
1448 data->max_velocity = new_max_velocity;
1473 data_size =
sizeof(SetEscapingMessage_data_t);
1476 data = (SetEscapingMessage_data_t *)
data_ptr;
1478 data->escaping_enabled = ini_escaping_enabled;
1484 data_size =
sizeof(SetEscapingMessage_data_t);
1487 data = (SetEscapingMessage_data_t *)
data_ptr;
1506 data = (SetEscapingMessage_data_t *)
data_ptr;
1521 return data->escaping_enabled;
1544 data->escaping_enabled = new_escaping_enabled;
1569 data_size =
sizeof(SetSecurityDistanceMessage_data_t);
1572 data = (SetSecurityDistanceMessage_data_t *)
data_ptr;
1574 data->security_distance = ini_security_distance;
1580 data_size =
sizeof(SetSecurityDistanceMessage_data_t);
1583 data = (SetSecurityDistanceMessage_data_t *)
data_ptr;
1602 data = (SetSecurityDistanceMessage_data_t *)
data_ptr;
1615 return data->security_distance;
1636 data->security_distance = new_security_distance;