24 #include <interfaces/HumanoidMotionInterface.h>
26 #include <core/exceptions/software.h>
46 HumanoidMotionInterface::HumanoidMotionInterface() : Interface()
48 data_size =
sizeof(HumanoidMotionInterface_data_t);
49 data_ptr = malloc(data_size);
50 data = (HumanoidMotionInterface_data_t *)data_ptr;
51 data_ts = (interface_data_ts_t *)data_ptr;
52 memset(data_ptr, 0, data_size);
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";
58 add_fieldinfo(IFT_BOOL,
"moving", 1, &data->moving);
59 add_fieldinfo(IFT_BOOL,
"arms_enabled", 1, &data->arms_enabled);
60 add_fieldinfo(IFT_UINT32,
"msgid", 1, &data->msgid);
61 add_messageinfo(
"StopMessage");
62 add_messageinfo(
"WalkStraightMessage");
63 add_messageinfo(
"WalkSidewaysMessage");
64 add_messageinfo(
"WalkArcMessage");
65 add_messageinfo(
"WalkVelocityMessage");
66 add_messageinfo(
"TurnMessage");
67 add_messageinfo(
"KickMessage");
68 add_messageinfo(
"ParkMessage");
69 add_messageinfo(
"GetUpMessage");
70 add_messageinfo(
"StandupMessage");
71 add_messageinfo(
"MoveHeadMessage");
72 unsigned char tmp_hash[] = {0x58, 0x4e, 0xd5, 0x1c, 0xbb, 0xf7, 0x6d, 0x85, 0x15, 0x52, 0x3b, 0x5a, 0x2b, 0x99, 0x5d, 0xc6};
77 HumanoidMotionInterface::~HumanoidMotionInterface()
86 HumanoidMotionInterface::tostring_LegEnum(
LegEnum value)
const
89 case LEG_LEFT:
return "LEG_LEFT";
90 case LEG_RIGHT:
return "LEG_RIGHT";
91 default:
return "UNKNOWN";
99 HumanoidMotionInterface::tostring_StandupEnum(
StandupEnum value)
const
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";
114 HumanoidMotionInterface::is_moving()
const
124 HumanoidMotionInterface::maxlenof_moving()
const
134 HumanoidMotionInterface::set_moving(
const bool new_moving)
146 HumanoidMotionInterface::is_arms_enabled()
const
148 return data->arms_enabled;
156 HumanoidMotionInterface::maxlenof_arms_enabled()
const
168 HumanoidMotionInterface::set_arms_enabled(
const bool new_arms_enabled)
170 data_changed |=
change_field(data->arms_enabled, new_arms_enabled);
181 HumanoidMotionInterface::msgid()
const
191 HumanoidMotionInterface::maxlenof_msgid()
const
204 HumanoidMotionInterface::set_msgid(
const uint32_t new_msgid)
211 HumanoidMotionInterface::create_message(
const char *type)
const
213 if ( strncmp(
"StopMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
215 }
else if ( strncmp(
"WalkStraightMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
217 }
else if ( strncmp(
"WalkSidewaysMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
219 }
else if ( strncmp(
"WalkArcMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
221 }
else if ( strncmp(
"WalkVelocityMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
223 }
else if ( strncmp(
"TurnMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
225 }
else if ( strncmp(
"KickMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
227 }
else if ( strncmp(
"ParkMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
229 }
else if ( strncmp(
"GetUpMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
231 }
else if ( strncmp(
"StandupMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
233 }
else if ( strncmp(
"MoveHeadMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
237 "message type for this interface type.", type);
246 HumanoidMotionInterface::copy_values(
const Interface *other)
251 type(), other->
type());
253 memcpy(data, oi->data,
sizeof(HumanoidMotionInterface_data_t));
257 HumanoidMotionInterface::enum_tostring(
const char *enumtype,
int val)
const
259 if (strcmp(enumtype,
"LegEnum") == 0) {
260 return tostring_LegEnum((
LegEnum)val);
262 if (strcmp(enumtype,
"StandupEnum") == 0) {
277 HumanoidMotionInterface::StopMessage::StopMessage() :
Message(
"StopMessage")
282 data = (StopMessage_data_t *)
data_ptr;
284 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
285 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
287 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
305 data = (StopMessage_data_t *)
data_ptr;
332 data_size =
sizeof(WalkStraightMessage_data_t);
335 data = (WalkStraightMessage_data_t *)
data_ptr;
337 data->distance = ini_distance;
338 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
339 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
341 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
348 data_size =
sizeof(WalkStraightMessage_data_t);
351 data = (WalkStraightMessage_data_t *)
data_ptr;
353 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
354 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
356 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
375 data = (WalkStraightMessage_data_t *)
data_ptr;
387 return data->distance;
432 data_size =
sizeof(WalkSidewaysMessage_data_t);
435 data = (WalkSidewaysMessage_data_t *)
data_ptr;
437 data->distance = ini_distance;
438 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
439 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
441 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
448 data_size =
sizeof(WalkSidewaysMessage_data_t);
451 data = (WalkSidewaysMessage_data_t *)
data_ptr;
453 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
454 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
456 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
475 data = (WalkSidewaysMessage_data_t *)
data_ptr;
487 return data->distance;
533 data_size =
sizeof(WalkArcMessage_data_t);
536 data = (WalkArcMessage_data_t *)
data_ptr;
538 data->angle = ini_angle;
539 data->radius = ini_radius;
540 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
541 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
543 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
551 data_size =
sizeof(WalkArcMessage_data_t);
554 data = (WalkArcMessage_data_t *)
data_ptr;
556 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
557 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
559 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
579 data = (WalkArcMessage_data_t *)
data_ptr;
669 data_size =
sizeof(WalkVelocityMessage_data_t);
672 data = (WalkVelocityMessage_data_t *)
data_ptr;
676 data->theta = ini_theta;
677 data->speed = ini_speed;
678 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
679 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
681 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
691 data_size =
sizeof(WalkVelocityMessage_data_t);
694 data = (WalkVelocityMessage_data_t *)
data_ptr;
696 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
697 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
699 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
721 data = (WalkVelocityMessage_data_t *)
data_ptr;
887 data = (TurnMessage_data_t *)
data_ptr;
889 data->angle = ini_angle;
890 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
891 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
893 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
903 data = (TurnMessage_data_t *)
data_ptr;
905 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
906 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
908 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
927 data = (TurnMessage_data_t *)
data_ptr;
988 data = (KickMessage_data_t *)
data_ptr;
991 data->strength = ini_strength;
992 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
993 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
995 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
1006 data = (KickMessage_data_t *)
data_ptr;
1008 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
1009 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
1011 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
1031 data = (KickMessage_data_t *)
data_ptr;
1073 return data->strength;
1119 data = (ParkMessage_data_t *)
data_ptr;
1121 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
1122 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
1124 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
1142 data = (ParkMessage_data_t *)
data_ptr;
1167 data_size =
sizeof(GetUpMessage_data_t);
1170 data = (GetUpMessage_data_t *)
data_ptr;
1172 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
1173 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
1175 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
1193 data = (GetUpMessage_data_t *)
data_ptr;
1220 data_size =
sizeof(StandupMessage_data_t);
1223 data = (StandupMessage_data_t *)
data_ptr;
1225 data->from_pos = ini_from_pos;
1226 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
1227 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
1229 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
1236 data_size =
sizeof(StandupMessage_data_t);
1239 data = (StandupMessage_data_t *)
data_ptr;
1241 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
1242 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
1244 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
1263 data = (StandupMessage_data_t *)
data_ptr;
1322 data_size =
sizeof(MoveHeadMessage_data_t);
1325 data = (MoveHeadMessage_data_t *)
data_ptr;
1327 data->yaw = ini_yaw;
1328 data->pitch = ini_pitch;
1329 data->speed = ini_speed;
1330 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
1331 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
1333 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
1342 data_size =
sizeof(MoveHeadMessage_data_t);
1345 data = (MoveHeadMessage_data_t *)
data_ptr;
1347 enum_map_LegEnum[(int)
LEG_LEFT] =
"LEG_LEFT";
1348 enum_map_LegEnum[(int)
LEG_RIGHT] =
"LEG_RIGHT";
1350 enum_map_StandupEnum[(int)
STANDUP_BACK] =
"STANDUP_BACK";
1371 data = (MoveHeadMessage_data_t *)
data_ptr;
1524 if ( m10 != NULL ) {
GetUpMessage Fawkes BlackBoard Interface Message.
virtual Message * clone() const
Clone this message.
~GetUpMessage()
Destructor.
GetUpMessage()
Constructor.
KickMessage Fawkes BlackBoard Interface Message.
virtual Message * clone() const
Clone this message.
void set_strength(const float new_strength)
Set strength value.
size_t maxlenof_strength() const
Get maximum length of strength value.
float strength() const
Get strength value.
LegEnum leg() const
Get leg value.
~KickMessage()
Destructor.
void set_leg(const LegEnum new_leg)
Set leg value.
KickMessage()
Constructor.
size_t maxlenof_leg() const
Get maximum length of leg value.
MoveHeadMessage Fawkes BlackBoard Interface Message.
float speed() const
Get speed value.
float pitch() const
Get pitch value.
size_t maxlenof_speed() const
Get maximum length of speed value.
MoveHeadMessage()
Constructor.
virtual Message * clone() const
Clone this message.
size_t maxlenof_pitch() const
Get maximum length of pitch value.
~MoveHeadMessage()
Destructor.
float yaw() const
Get yaw value.
void set_speed(const float new_speed)
Set speed value.
void set_pitch(const float new_pitch)
Set pitch value.
void set_yaw(const float new_yaw)
Set yaw value.
size_t maxlenof_yaw() const
Get maximum length of yaw value.
ParkMessage Fawkes BlackBoard Interface Message.
ParkMessage()
Constructor.
~ParkMessage()
Destructor.
virtual Message * clone() const
Clone this message.
StandupMessage Fawkes BlackBoard Interface Message.
void set_from_pos(const StandupEnum new_from_pos)
Set from_pos value.
virtual Message * clone() const
Clone this message.
StandupEnum from_pos() const
Get from_pos value.
~StandupMessage()
Destructor.
StandupMessage()
Constructor.
size_t maxlenof_from_pos() const
Get maximum length of from_pos value.
StopMessage Fawkes BlackBoard Interface Message.
~StopMessage()
Destructor.
virtual Message * clone() const
Clone this message.
StopMessage()
Constructor.
TurnMessage Fawkes BlackBoard Interface Message.
float angle() const
Get angle value.
size_t maxlenof_angle() const
Get maximum length of angle value.
TurnMessage()
Constructor.
~TurnMessage()
Destructor.
virtual Message * clone() const
Clone this message.
void set_angle(const float new_angle)
Set angle value.
WalkArcMessage Fawkes BlackBoard Interface Message.
size_t maxlenof_angle() const
Get maximum length of angle value.
void set_angle(const float new_angle)
Set angle value.
float angle() const
Get angle value.
~WalkArcMessage()
Destructor.
float radius() const
Get radius value.
void set_radius(const float new_radius)
Set radius value.
size_t maxlenof_radius() const
Get maximum length of radius value.
WalkArcMessage()
Constructor.
virtual Message * clone() const
Clone this message.
WalkSidewaysMessage Fawkes BlackBoard Interface Message.
void set_distance(const float new_distance)
Set distance value.
size_t maxlenof_distance() const
Get maximum length of distance value.
WalkSidewaysMessage()
Constructor.
float distance() const
Get distance value.
~WalkSidewaysMessage()
Destructor.
virtual Message * clone() const
Clone this message.
WalkStraightMessage Fawkes BlackBoard Interface Message.
size_t maxlenof_distance() const
Get maximum length of distance value.
void set_distance(const float new_distance)
Set distance value.
WalkStraightMessage()
Constructor.
virtual Message * clone() const
Clone this message.
float distance() const
Get distance value.
~WalkStraightMessage()
Destructor.
WalkVelocityMessage Fawkes BlackBoard Interface Message.
void set_y(const float new_y)
Set y value.
void set_x(const float new_x)
Set x value.
float x() const
Get x value.
float speed() const
Get speed value.
size_t maxlenof_speed() const
Get maximum length of speed value.
void set_speed(const float new_speed)
Set speed value.
void set_theta(const float new_theta)
Set theta value.
float y() const
Get y value.
float theta() const
Get theta value.
size_t maxlenof_x() const
Get maximum length of x value.
WalkVelocityMessage()
Constructor.
size_t maxlenof_theta() const
Get maximum length of theta value.
~WalkVelocityMessage()
Destructor.
size_t maxlenof_y() const
Get maximum length of y value.
virtual Message * clone() const
Clone this message.
HumanoidMotionInterface Fawkes BlackBoard Interface.
virtual bool message_valid(const Message *message) const
Check if message is valid and can be enqueued.
LegEnum
Type to determinate leg side.
StandupEnum
From which position to standup.
@ STANDUP_DETECT
Detect via accelerometer.
@ STANDUP_BACK
Standup from lying on the back.
@ STANDUP_FRONT
Standup from lying on the tummy.
Base class for all Fawkes BlackBoard interfaces.
const char * type() const
Get type of interface.
void * data_ptr
Pointer to local memory storage.
Base class for all messages passed through interfaces in Fawkes BlackBoard.
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.
void * data_ptr
Pointer to memory that contains local data.
message_data_ts_t * data_ts
data timestamp aliasing pointer
unsigned int data_size
Size of memory needed to hold all data.
Fawkes library namespace.
@ IFT_ENUM
field with interface specific enum type
bool change_field(FieldT &field, const DataT &value)
Set a field and return whether it changed.
Timestamp data, must be present and first entries for each interface data structs!...