24 #include <interfaces/HumanoidMotionInterface.h>
26 #include <core/exceptions/software.h>
46 HumanoidMotionInterface::HumanoidMotionInterface() : Interface()
48 data_size =
sizeof(HumanoidMotionInterface_data_t);
50 data = (HumanoidMotionInterface_data_t *)
data_ptr;
53 enum_map_LegEnum[(int)LEG_LEFT] =
"LEG_LEFT";
54 enum_map_LegEnum[(int)LEG_RIGHT] =
"LEG_RIGHT";
55 enum_map_StandupEnum[(int)STANDUP_DETECT] =
"STANDUP_DETECT";
56 enum_map_StandupEnum[(int)STANDUP_BACK] =
"STANDUP_BACK";
57 enum_map_StandupEnum[(int)STANDUP_FRONT] =
"STANDUP_FRONT";
72 unsigned char tmp_hash[] = {0x58, 0x4e, 0xd5, 0x1c, 0xbb, 0xf7, 0x6d, 0x85, 0x15, 0x52, 0x3b, 0x5a, 0x2b, 0x99, 0x5d, 0xc6};
77 HumanoidMotionInterface::~HumanoidMotionInterface()
89 case LEG_LEFT:
return "LEG_LEFT";
90 case LEG_RIGHT:
return "LEG_RIGHT";
91 default:
return "UNKNOWN";
102 case STANDUP_DETECT:
return "STANDUP_DETECT";
103 case STANDUP_BACK:
return "STANDUP_BACK";
104 case STANDUP_FRONT:
return "STANDUP_FRONT";
105 default:
return "UNKNOWN";
136 data->moving = new_moving;
149 return data->arms_enabled;
171 data->arms_enabled = new_arms_enabled;
208 data->msgid = new_msgid;
216 if ( strncmp(
"StopMessage",
type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
218 }
else if ( strncmp(
"WalkStraightMessage",
type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
220 }
else if ( strncmp(
"WalkSidewaysMessage",
type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
221 return new WalkSidewaysMessage();
222 }
else if ( strncmp(
"WalkArcMessage",
type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
223 return new WalkArcMessage();
224 }
else if ( strncmp(
"WalkVelocityMessage",
type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
225 return new WalkVelocityMessage();
226 }
else if ( strncmp(
"TurnMessage",
type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
227 return new TurnMessage();
228 }
else if ( strncmp(
"KickMessage",
type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
229 return new KickMessage();
230 }
else if ( strncmp(
"ParkMessage",
type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
231 return new ParkMessage();
232 }
else if ( strncmp(
"GetUpMessage",
type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
233 return new GetUpMessage();
234 }
else if ( strncmp(
"StandupMessage",
type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
235 return new StandupMessage();
236 }
else if ( strncmp(
"MoveHeadMessage",
type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
237 return new MoveHeadMessage();
239 throw UnknownTypeException(
"The given type '%s' does not match any known "
240 "message type for this interface type.",
type);
251 const HumanoidMotionInterface *oi =
dynamic_cast<const HumanoidMotionInterface *
>(other);
253 throw TypeMismatchException(
"Can only copy values from interface of same type (%s vs. %s)",
254 type(), other->type());
256 memcpy(data, oi->data,
sizeof(HumanoidMotionInterface_data_t));
262 if (strcmp(enumtype,
"LegEnum") == 0) {
263 return tostring_LegEnum((LegEnum)val);
265 if (strcmp(enumtype,
"StandupEnum") == 0) {
285 data = (StopMessage_data_t *)
data_ptr;
287 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
288 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
290 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
308 data = (StopMessage_data_t *)
data_ptr;
335 data_size =
sizeof(WalkStraightMessage_data_t);
338 data = (WalkStraightMessage_data_t *)
data_ptr;
340 data->distance = ini_distance;
341 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
342 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
344 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
351 data_size =
sizeof(WalkStraightMessage_data_t);
354 data = (WalkStraightMessage_data_t *)
data_ptr;
356 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
357 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
359 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
378 data = (WalkStraightMessage_data_t *)
data_ptr;
390 return data->distance;
410 data->distance = new_distance;
435 data_size =
sizeof(WalkSidewaysMessage_data_t);
438 data = (WalkSidewaysMessage_data_t *)
data_ptr;
440 data->distance = ini_distance;
441 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
442 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
444 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
451 data_size =
sizeof(WalkSidewaysMessage_data_t);
454 data = (WalkSidewaysMessage_data_t *)
data_ptr;
456 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
457 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
459 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
478 data = (WalkSidewaysMessage_data_t *)
data_ptr;
490 return data->distance;
510 data->distance = new_distance;
536 data_size =
sizeof(WalkArcMessage_data_t);
539 data = (WalkArcMessage_data_t *)
data_ptr;
541 data->angle = ini_angle;
542 data->radius = ini_radius;
543 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
544 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
546 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
554 data_size =
sizeof(WalkArcMessage_data_t);
557 data = (WalkArcMessage_data_t *)
data_ptr;
559 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
560 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
562 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
582 data = (WalkArcMessage_data_t *)
data_ptr;
614 data->angle = new_angle;
644 data->radius = new_radius;
672 data_size =
sizeof(WalkVelocityMessage_data_t);
675 data = (WalkVelocityMessage_data_t *)
data_ptr;
679 data->theta = ini_theta;
680 data->speed = ini_speed;
681 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
682 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
684 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
694 data_size =
sizeof(WalkVelocityMessage_data_t);
697 data = (WalkVelocityMessage_data_t *)
data_ptr;
699 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
700 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
702 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
724 data = (WalkVelocityMessage_data_t *)
data_ptr;
828 data->theta = new_theta;
862 data->speed = new_speed;
890 data = (TurnMessage_data_t *)
data_ptr;
892 data->angle = ini_angle;
893 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
894 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
896 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
906 data = (TurnMessage_data_t *)
data_ptr;
908 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
909 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
911 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
930 data = (TurnMessage_data_t *)
data_ptr;
962 data->angle = new_angle;
991 data = (KickMessage_data_t *)
data_ptr;
994 data->strength = ini_strength;
995 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
996 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
998 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
1009 data = (KickMessage_data_t *)
data_ptr;
1011 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
1012 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
1014 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
1034 data = (KickMessage_data_t *)
data_ptr;
1066 data->leg = new_leg;
1076 return data->strength;
1096 data->strength = new_strength;
1122 data = (ParkMessage_data_t *)
data_ptr;
1124 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
1125 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
1127 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
1145 data = (ParkMessage_data_t *)
data_ptr;
1170 data_size =
sizeof(GetUpMessage_data_t);
1173 data = (GetUpMessage_data_t *)
data_ptr;
1175 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
1176 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
1178 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
1196 data = (GetUpMessage_data_t *)
data_ptr;
1223 data_size =
sizeof(StandupMessage_data_t);
1226 data = (StandupMessage_data_t *)
data_ptr;
1228 data->from_pos = ini_from_pos;
1229 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
1230 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
1232 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
1239 data_size =
sizeof(StandupMessage_data_t);
1242 data = (StandupMessage_data_t *)
data_ptr;
1244 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
1245 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
1247 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
1266 data = (StandupMessage_data_t *)
data_ptr;
1298 data->from_pos = new_from_pos;
1325 data_size =
sizeof(MoveHeadMessage_data_t);
1328 data = (MoveHeadMessage_data_t *)
data_ptr;
1330 data->yaw = ini_yaw;
1331 data->pitch = ini_pitch;
1332 data->speed = ini_speed;
1333 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
1334 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
1336 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
1345 data_size =
sizeof(MoveHeadMessage_data_t);
1348 data = (MoveHeadMessage_data_t *)
data_ptr;
1350 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
1351 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
1353 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
1374 data = (MoveHeadMessage_data_t *)
data_ptr;
1406 data->yaw = new_yaw;
1436 data->pitch = new_pitch;
1466 data->speed = new_speed;
1494 const WalkSidewaysMessage *m2 =
dynamic_cast<const WalkSidewaysMessage *
>(message);
1498 const WalkArcMessage *m3 =
dynamic_cast<const WalkArcMessage *
>(message);
1502 const WalkVelocityMessage *m4 =
dynamic_cast<const WalkVelocityMessage *
>(message);
1506 const TurnMessage *m5 =
dynamic_cast<const TurnMessage *
>(message);
1510 const KickMessage *m6 =
dynamic_cast<const KickMessage *
>(message);
1514 const ParkMessage *m7 =
dynamic_cast<const ParkMessage *
>(message);
1518 const GetUpMessage *m8 =
dynamic_cast<const GetUpMessage *
>(message);
1522 const StandupMessage *m9 =
dynamic_cast<const StandupMessage *
>(message);
1526 const MoveHeadMessage *m10 =
dynamic_cast<const MoveHeadMessage *
>(message);
1527 if ( m10 != NULL ) {
1534 EXPORT_INTERFACE(HumanoidMotionInterface)