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) == 0 ) {
218 }
else if ( strncmp(
"WalkStraightMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
220 }
else if ( strncmp(
"WalkSidewaysMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
222 }
else if ( strncmp(
"WalkArcMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
224 }
else if ( strncmp(
"WalkVelocityMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
226 }
else if ( strncmp(
"TurnMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
228 }
else if ( strncmp(
"KickMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
230 }
else if ( strncmp(
"ParkMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
232 }
else if ( strncmp(
"GetUpMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
234 }
else if ( strncmp(
"StandupMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
236 }
else if ( strncmp(
"MoveHeadMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
240 "message type for this interface type.", 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;
1527 if ( m10 != NULL ) {
uint32_t msgid() const
Get msgid value.
void set_theta(const float new_theta)
Set theta value.
void * data_ptr
Pointer to memory that contains local data.
~GetUpMessage()
Destructor.
float speed() const
Get speed value.
~StopMessage()
Destructor.
Base class for all messages passed through interfaces in Fawkes BlackBoard.
StandupMessage Fawkes BlackBoard Interface Message.
void set_angle(const float new_angle)
Set angle value.
TurnMessage Fawkes BlackBoard Interface Message.
Standup from lying on the back.
virtual Message * create_message(const char *type) const
Create message based on type name.
KickMessage Fawkes BlackBoard Interface Message.
~WalkSidewaysMessage()
Destructor.
~StandupMessage()
Destructor.
void set_x(const float new_x)
Set x value.
GetUpMessage()
Constructor.
void set_hash(unsigned char *ihash)
Set hash.
StopMessage()
Constructor.
virtual Message * clone() const
Clone this message.
size_t maxlenof_theta() const
Get maximum length of theta value.
WalkArcMessage()
Constructor.
size_t maxlenof_strength() const
Get maximum length of strength value.
float distance() const
Get distance value.
Fawkes library namespace.
void set_arms_enabled(const bool new_arms_enabled)
Set arms_enabled value.
Timestamp data, must be present and first entries for each interface data structs! This leans on time...
float angle() const
Get angle value.
float yaw() const
Get yaw value.
LegEnum leg() const
Get leg value.
size_t maxlenof_x() const
Get maximum length of x value.
bool is_moving() const
Get moving value.
float angle() const
Get angle value.
~TurnMessage()
Destructor.
void set_speed(const float new_speed)
Set speed value.
size_t maxlenof_msgid() const
Get maximum length of msgid value.
float theta() const
Get theta value.
virtual bool message_valid(const Message *message) const
Check if message is valid and can be enqueued.
size_t maxlenof_y() const
Get maximum length of y value.
WalkVelocityMessage Fawkes BlackBoard Interface Message.
size_t maxlenof_speed() const
Get maximum length of speed value.
TurnMessage()
Constructor.
void set_moving(const bool new_moving)
Set moving value.
GetUpMessage Fawkes BlackBoard Interface Message.
void set_pitch(const float new_pitch)
Set pitch value.
~MoveHeadMessage()
Destructor.
StandupEnum
From which position to standup.
Base class for all Fawkes BlackBoard interfaces.
size_t maxlenof_speed() const
Get maximum length of speed value.
size_t maxlenof_pitch() const
Get maximum length of pitch value.
size_t maxlenof_yaw() const
Get maximum length of yaw value.
~WalkArcMessage()
Destructor.
~WalkStraightMessage()
Destructor.
LegEnum
Type to determinate leg side.
message_data_ts_t * data_ts
data timestamp aliasing pointer
unsigned int data_size
Size of memory needed to hold all data.
void set_distance(const float new_distance)
Set distance value.
virtual Message * clone() const
Clone this message.
StandupMessage()
Constructor.
size_t maxlenof_radius() const
Get maximum length of radius value.
virtual const char * enum_tostring(const char *enumtype, int val) const
Convert arbitrary enum value to string.
virtual Message * clone() const
Clone this message.
void add_messageinfo(const char *name)
Add an entry to the message info list.
size_t maxlenof_angle() const
Get maximum length of angle value.
void set_msgid(const uint32_t new_msgid)
Set msgid value.
virtual Message * clone() const
Clone this message.
virtual Message * clone() const
Clone this message.
bool data_changed
Indicator if data has changed.
size_t maxlenof_angle() const
Get maximum length of angle value.
WalkArcMessage Fawkes BlackBoard Interface Message.
const char * type() const
Get type of interface.
HumanoidMotionInterface Fawkes BlackBoard Interface.
Standup from lying on the tummy.
size_t maxlenof_leg() const
Get maximum length of leg value.
float y() const
Get y value.
size_t maxlenof_from_pos() const
Get maximum length of from_pos value.
size_t maxlenof_moving() const
Get maximum length of moving value.
bool is_arms_enabled() const
Get arms_enabled value.
StandupEnum from_pos() const
Get from_pos value.
Detect via accelerometer.
~WalkVelocityMessage()
Destructor.
MoveHeadMessage()
Constructor.
virtual Message * clone() const
Clone this message.
void set_radius(const float new_radius)
Set radius value.
void set_speed(const float new_speed)
Set speed value.
float distance() const
Get distance value.
WalkSidewaysMessage Fawkes BlackBoard Interface Message.
size_t maxlenof_arms_enabled() const
Get maximum length of arms_enabled value.
const char * tostring_LegEnum(LegEnum value) const
Convert LegEnum constant to string.
const char * tostring_StandupEnum(StandupEnum value) const
Convert StandupEnum constant to string.
virtual void copy_values(const Interface *other)
Copy values from other interface.
KickMessage()
Constructor.
size_t maxlenof_distance() const
Get maximum length of distance value.
float x() const
Get x value.
float pitch() const
Get pitch value.
~KickMessage()
Destructor.
void set_yaw(const float new_yaw)
Set yaw value.
float strength() const
Get strength value.
ParkMessage()
Constructor.
WalkStraightMessage()
Constructor.
void set_from_pos(const StandupEnum new_from_pos)
Set from_pos value.
WalkVelocityMessage()
Constructor.
ParkMessage Fawkes BlackBoard Interface Message.
WalkStraightMessage Fawkes BlackBoard Interface Message.
virtual Message * clone() const
Clone this message.
virtual Message * clone() const
Clone this message.
~ParkMessage()
Destructor.
void set_distance(const float new_distance)
Set distance value.
WalkSidewaysMessage()
Constructor.
virtual Message * clone() const
Clone this message.
MoveHeadMessage Fawkes BlackBoard Interface Message.
void set_strength(const float new_strength)
Set strength value.
void add_fieldinfo(interface_fieldtype_t type, const char *name, size_t length, void *value, const char *enumtype=0, const interface_enum_map_t *enum_map=0)
Add an entry to the info list.
StopMessage Fawkes BlackBoard Interface Message.
virtual Message * clone() const
Clone this message.
void set_angle(const float new_angle)
Set angle value.
void set_y(const float new_y)
Set y value.
size_t maxlenof_distance() const
Get maximum length of distance value.
const char * type() const
Get message type.
32 bit unsigned integer field
field with interface specific enum type
float radius() const
Get radius value.
virtual Message * clone() const
Clone this message.
void set_leg(const LegEnum new_leg)
Set leg value.
float speed() const
Get speed value.