24 #include <interfaces/HumanoidMotionInterface.h>
26 #include <core/exceptions/software.h>
44 HumanoidMotionInterface::HumanoidMotionInterface() : Interface()
46 data_size =
sizeof(HumanoidMotionInterface_data_t);
47 data_ptr = malloc(data_size);
48 data = (HumanoidMotionInterface_data_t *)data_ptr;
49 data_ts = (interface_data_ts_t *)data_ptr;
50 memset(data_ptr, 0, data_size);
51 add_fieldinfo(IFT_BOOL,
"moving", 1, &data->moving);
52 add_fieldinfo(IFT_BOOL,
"arms_enabled", 1, &data->arms_enabled);
53 add_fieldinfo(IFT_UINT32,
"msgid", 1, &data->msgid);
54 add_messageinfo(
"StopMessage");
55 add_messageinfo(
"WalkStraightMessage");
56 add_messageinfo(
"WalkSidewaysMessage");
57 add_messageinfo(
"WalkArcMessage");
58 add_messageinfo(
"WalkVelocityMessage");
59 add_messageinfo(
"TurnMessage");
60 add_messageinfo(
"KickMessage");
61 add_messageinfo(
"ParkMessage");
62 add_messageinfo(
"GetUpMessage");
63 add_messageinfo(
"StandupMessage");
64 add_messageinfo(
"MoveHeadMessage");
65 unsigned char tmp_hash[] = {0x58, 0x4e, 0xd5, 0x1c, 0xbb, 0xf7, 0x6d, 0x85, 0x15, 0x52, 0x3b, 0x5a, 0x2b, 0x99, 0x5d, 0xc6};
70 HumanoidMotionInterface::~HumanoidMotionInterface()
79 HumanoidMotionInterface::tostring_LegEnum(
LegEnum value)
const
82 case LEG_LEFT:
return "LEG_LEFT";
83 case LEG_RIGHT:
return "LEG_RIGHT";
84 default:
return "UNKNOWN";
92 HumanoidMotionInterface::tostring_StandupEnum(
StandupEnum value)
const
95 case STANDUP_DETECT:
return "STANDUP_DETECT";
96 case STANDUP_BACK:
return "STANDUP_BACK";
97 case STANDUP_FRONT:
return "STANDUP_FRONT";
98 default:
return "UNKNOWN";
107 HumanoidMotionInterface::is_moving()
const
117 HumanoidMotionInterface::maxlenof_moving()
const
127 HumanoidMotionInterface::set_moving(
const bool new_moving)
129 data->moving = new_moving;
140 HumanoidMotionInterface::is_arms_enabled()
const
142 return data->arms_enabled;
150 HumanoidMotionInterface::maxlenof_arms_enabled()
const
162 HumanoidMotionInterface::set_arms_enabled(
const bool new_arms_enabled)
164 data->arms_enabled = new_arms_enabled;
176 HumanoidMotionInterface::msgid()
const
186 HumanoidMotionInterface::maxlenof_msgid()
const
199 HumanoidMotionInterface::set_msgid(
const uint32_t new_msgid)
201 data->msgid = new_msgid;
207 HumanoidMotionInterface::create_message(
const char *type)
const
209 if ( strncmp(
"StopMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
211 }
else if ( strncmp(
"WalkStraightMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
213 }
else if ( strncmp(
"WalkSidewaysMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
215 }
else if ( strncmp(
"WalkArcMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
217 }
else if ( strncmp(
"WalkVelocityMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
219 }
else if ( strncmp(
"TurnMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
221 }
else if ( strncmp(
"KickMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
223 }
else if ( strncmp(
"ParkMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
225 }
else if ( strncmp(
"GetUpMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
227 }
else if ( strncmp(
"StandupMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
229 }
else if ( strncmp(
"MoveHeadMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
233 "message type for this interface type.", type);
242 HumanoidMotionInterface::copy_values(
const Interface *other)
247 type(), other->
type());
249 memcpy(data, oi->data,
sizeof(HumanoidMotionInterface_data_t));
253 HumanoidMotionInterface::enum_tostring(
const char *enumtype,
int val)
const
255 if (strcmp(enumtype,
"LegEnum") == 0) {
256 return tostring_LegEnum((
LegEnum)val);
258 if (strcmp(enumtype,
"StandupEnum") == 0) {
273 HumanoidMotionInterface::StopMessage::StopMessage() :
Message(
"StopMessage")
278 data = (StopMessage_data_t *)
data_ptr;
296 data = (StopMessage_data_t *)
data_ptr;
323 data_size =
sizeof(WalkStraightMessage_data_t);
326 data = (WalkStraightMessage_data_t *)
data_ptr;
328 data->distance = ini_distance;
334 data_size =
sizeof(WalkStraightMessage_data_t);
337 data = (WalkStraightMessage_data_t *)
data_ptr;
356 data = (WalkStraightMessage_data_t *)
data_ptr;
368 return data->distance;
388 data->distance = new_distance;
413 data_size =
sizeof(WalkSidewaysMessage_data_t);
416 data = (WalkSidewaysMessage_data_t *)
data_ptr;
418 data->distance = ini_distance;
424 data_size =
sizeof(WalkSidewaysMessage_data_t);
427 data = (WalkSidewaysMessage_data_t *)
data_ptr;
446 data = (WalkSidewaysMessage_data_t *)
data_ptr;
458 return data->distance;
478 data->distance = new_distance;
504 data_size =
sizeof(WalkArcMessage_data_t);
507 data = (WalkArcMessage_data_t *)
data_ptr;
509 data->angle = ini_angle;
510 data->radius = ini_radius;
517 data_size =
sizeof(WalkArcMessage_data_t);
520 data = (WalkArcMessage_data_t *)
data_ptr;
540 data = (WalkArcMessage_data_t *)
data_ptr;
572 data->angle = new_angle;
602 data->radius = new_radius;
630 data_size =
sizeof(WalkVelocityMessage_data_t);
633 data = (WalkVelocityMessage_data_t *)
data_ptr;
637 data->theta = ini_theta;
638 data->speed = ini_speed;
647 data_size =
sizeof(WalkVelocityMessage_data_t);
650 data = (WalkVelocityMessage_data_t *)
data_ptr;
672 data = (WalkVelocityMessage_data_t *)
data_ptr;
776 data->theta = new_theta;
810 data->speed = new_speed;
838 data = (TurnMessage_data_t *)
data_ptr;
840 data->angle = ini_angle;
849 data = (TurnMessage_data_t *)
data_ptr;
868 data = (TurnMessage_data_t *)
data_ptr;
900 data->angle = new_angle;
929 data = (KickMessage_data_t *)
data_ptr;
932 data->strength = ini_strength;
942 data = (KickMessage_data_t *)
data_ptr;
962 data = (KickMessage_data_t *)
data_ptr;
1004 return data->strength;
1024 data->strength = new_strength;
1050 data = (ParkMessage_data_t *)
data_ptr;
1068 data = (ParkMessage_data_t *)
data_ptr;
1093 data_size =
sizeof(GetUpMessage_data_t);
1096 data = (GetUpMessage_data_t *)
data_ptr;
1114 data = (GetUpMessage_data_t *)
data_ptr;
1141 data_size =
sizeof(StandupMessage_data_t);
1144 data = (StandupMessage_data_t *)
data_ptr;
1146 data->from_pos = ini_from_pos;
1152 data_size =
sizeof(StandupMessage_data_t);
1155 data = (StandupMessage_data_t *)
data_ptr;
1174 data = (StandupMessage_data_t *)
data_ptr;
1206 data->from_pos = new_from_pos;
1233 data_size =
sizeof(MoveHeadMessage_data_t);
1236 data = (MoveHeadMessage_data_t *)
data_ptr;
1238 data->yaw = ini_yaw;
1239 data->pitch = ini_pitch;
1240 data->speed = ini_speed;
1248 data_size =
sizeof(MoveHeadMessage_data_t);
1251 data = (MoveHeadMessage_data_t *)
data_ptr;
1272 data = (MoveHeadMessage_data_t *)
data_ptr;
1304 data->yaw = new_yaw;
1334 data->pitch = new_pitch;
1364 data->speed = new_speed;
1425 if ( m10 != NULL ) {