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 ) {
size_t maxlenof_distance() const
Get maximum length of distance value.
size_t maxlenof_strength() const
Get maximum length of strength value.
float speed() const
Get speed value.
void set_theta(const float new_theta)
Set theta value.
void * data_ptr
Pointer to memory that contains local data.
~GetUpMessage()
Destructor.
~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.
float y() const
Get y value.
size_t maxlenof_angle() const
Get maximum length of angle value.
KickMessage Fawkes BlackBoard Interface Message.
~WalkSidewaysMessage()
Destructor.
~StandupMessage()
Destructor.
void set_x(const float new_x)
Set x value.
size_t maxlenof_speed() const
Get maximum length of speed value.
GetUpMessage()
Constructor.
float pitch() const
Get pitch value.
StopMessage()
Constructor.
WalkArcMessage()
Constructor.
Fawkes library namespace.
float angle() const
Get angle value.
virtual Message * clone() const
Clone this message.
Timestamp data, must be present and first entries for each interface data structs! This leans on time...
size_t maxlenof_x() const
Get maximum length of x value.
~TurnMessage()
Destructor.
void set_speed(const float new_speed)
Set speed value.
size_t maxlenof_speed() const
Get maximum length of speed value.
WalkVelocityMessage Fawkes BlackBoard Interface Message.
size_t maxlenof_leg() const
Get maximum length of leg value.
TurnMessage()
Constructor.
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.
virtual Message * clone() const
Clone this message.
virtual Message * clone() const
Clone this message.
virtual Message * clone() const
Clone this message.
~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.
float yaw() const
Get yaw value.
StandupMessage()
Constructor.
WalkArcMessage Fawkes BlackBoard Interface Message.
HumanoidMotionInterface Fawkes BlackBoard Interface.
void * data_ptr
Pointer to local memory storage.
float angle() const
Get angle value.
size_t maxlenof_yaw() const
Get maximum length of yaw value.
virtual Message * clone() const
Clone this message.
~WalkVelocityMessage()
Destructor.
MoveHeadMessage()
Constructor.
void set_radius(const float new_radius)
Set radius value.
float speed() const
Get speed value.
void set_speed(const float new_speed)
Set speed value.
float radius() const
Get radius value.
size_t maxlenof_theta() const
Get maximum length of theta value.
WalkSidewaysMessage Fawkes BlackBoard Interface Message.
virtual Message * clone() const
Clone this message.
float x() const
Get x value.
size_t maxlenof_radius() const
Get maximum length of radius value.
KickMessage()
Constructor.
virtual Message * clone() const
Clone this message.
StandupEnum from_pos() const
Get from_pos value.
float strength() const
Get strength value.
~KickMessage()
Destructor.
void set_yaw(const float new_yaw)
Set yaw value.
ParkMessage()
Constructor.
size_t maxlenof_distance() const
Get maximum length of distance value.
WalkStraightMessage()
Constructor.
void set_from_pos(const StandupEnum new_from_pos)
Set from_pos value.
WalkVelocityMessage()
Constructor.
virtual Message * clone() const
Clone this message.
ParkMessage Fawkes BlackBoard Interface Message.
size_t maxlenof_pitch() const
Get maximum length of pitch value.
WalkStraightMessage Fawkes BlackBoard Interface Message.
virtual bool message_valid(const Message *message) const
Check if message is valid and can be enqueued.
virtual Message * clone() const
Clone this message.
size_t maxlenof_angle() const
Get maximum length of angle value.
void add_fieldinfo(interface_fieldtype_t type, const char *name, size_t length, void *value, const char *enumtype=0)
Add an entry to the info list.
~ParkMessage()
Destructor.
void set_distance(const float new_distance)
Set distance value.
virtual Message * clone() const
Clone this message.
WalkSidewaysMessage()
Constructor.
float distance() const
Get distance value.
MoveHeadMessage Fawkes BlackBoard Interface Message.
float theta() const
Get theta value.
void set_strength(const float new_strength)
Set strength value.
StopMessage Fawkes BlackBoard Interface Message.
size_t maxlenof_y() const
Get maximum length of y value.
float distance() const
Get distance value.
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_from_pos() const
Get maximum length of from_pos value.
const char * type() const
Get type of interface.
LegEnum leg() const
Get leg value.
field with interface specific enum type
void set_leg(const LegEnum new_leg)
Set leg value.