24 #include <interfaces/MotorInterface.h>
26 #include <core/exceptions/software.h>
62 MotorInterface::MotorInterface() : Interface()
64 data_size =
sizeof(MotorInterface_data_t);
65 data_ptr = malloc(data_size);
66 data = (MotorInterface_data_t *)data_ptr;
67 data_ts = (interface_data_ts_t *)data_ptr;
68 memset(data_ptr, 0, data_size);
69 add_fieldinfo(IFT_UINT32,
"motor_state", 1, &data->motor_state);
70 add_fieldinfo(IFT_UINT32,
"drive_mode", 1, &data->drive_mode);
71 add_fieldinfo(IFT_INT32,
"right_rpm", 1, &data->right_rpm);
72 add_fieldinfo(IFT_INT32,
"rear_rpm", 1, &data->rear_rpm);
73 add_fieldinfo(IFT_INT32,
"left_rpm", 1, &data->left_rpm);
74 add_fieldinfo(IFT_FLOAT,
"odometry_path_length", 1, &data->odometry_path_length);
75 add_fieldinfo(IFT_FLOAT,
"odometry_position_x", 1, &data->odometry_position_x);
76 add_fieldinfo(IFT_FLOAT,
"odometry_position_y", 1, &data->odometry_position_y);
77 add_fieldinfo(IFT_FLOAT,
"odometry_orientation", 1, &data->odometry_orientation);
78 add_fieldinfo(IFT_FLOAT,
"vx", 1, &data->vx);
79 add_fieldinfo(IFT_FLOAT,
"vy", 1, &data->vy);
80 add_fieldinfo(IFT_FLOAT,
"omega", 1, &data->omega);
81 add_fieldinfo(IFT_FLOAT,
"des_vx", 1, &data->des_vx);
82 add_fieldinfo(IFT_FLOAT,
"des_vy", 1, &data->des_vy);
83 add_fieldinfo(IFT_FLOAT,
"des_omega", 1, &data->des_omega);
84 add_fieldinfo(IFT_UINT32,
"controller", 1, &data->controller);
85 add_fieldinfo(IFT_STRING,
"controller_thread_name", 64, data->controller_thread_name);
86 add_messageinfo(
"SetMotorStateMessage");
87 add_messageinfo(
"AcquireControlMessage");
88 add_messageinfo(
"ResetOdometryMessage");
89 add_messageinfo(
"SetOdometryMessage");
90 add_messageinfo(
"DriveRPMMessage");
91 add_messageinfo(
"GotoMessage");
92 add_messageinfo(
"TransMessage");
93 add_messageinfo(
"RotMessage");
94 add_messageinfo(
"TransRotMessage");
95 add_messageinfo(
"OrbitMessage");
96 add_messageinfo(
"LinTransRotMessage");
97 unsigned char tmp_hash[] = {0x62, 0x6c, 0x3f, 0x33, 0x1c, 0x3a, 0x9e, 0x18, 0xd5, 0xee, 0xab, 0x30, 0xfb, 0x10, 0xf0, 0x79};
102 MotorInterface::~MotorInterface()
114 MotorInterface::motor_state()
const
116 return data->motor_state;
124 MotorInterface::maxlenof_motor_state()
const
136 MotorInterface::set_motor_state(
const uint32_t new_motor_state)
138 set_field(data->motor_state, new_motor_state);
148 MotorInterface::drive_mode()
const
150 return data->drive_mode;
158 MotorInterface::maxlenof_drive_mode()
const
170 MotorInterface::set_drive_mode(
const uint32_t new_drive_mode)
172 set_field(data->drive_mode, new_drive_mode);
182 MotorInterface::right_rpm()
const
184 return data->right_rpm;
192 MotorInterface::maxlenof_right_rpm()
const
204 MotorInterface::set_right_rpm(
const int32_t new_right_rpm)
206 set_field(data->right_rpm, new_right_rpm);
216 MotorInterface::rear_rpm()
const
218 return data->rear_rpm;
226 MotorInterface::maxlenof_rear_rpm()
const
238 MotorInterface::set_rear_rpm(
const int32_t new_rear_rpm)
240 set_field(data->rear_rpm, new_rear_rpm);
250 MotorInterface::left_rpm()
const
252 return data->left_rpm;
260 MotorInterface::maxlenof_left_rpm()
const
272 MotorInterface::set_left_rpm(
const int32_t new_left_rpm)
274 set_field(data->left_rpm, new_left_rpm);
284 MotorInterface::odometry_path_length()
const
286 return data->odometry_path_length;
294 MotorInterface::maxlenof_odometry_path_length()
const
306 MotorInterface::set_odometry_path_length(
const float new_odometry_path_length)
308 set_field(data->odometry_path_length, new_odometry_path_length);
318 MotorInterface::odometry_position_x()
const
320 return data->odometry_position_x;
328 MotorInterface::maxlenof_odometry_position_x()
const
340 MotorInterface::set_odometry_position_x(
const float new_odometry_position_x)
342 set_field(data->odometry_position_x, new_odometry_position_x);
352 MotorInterface::odometry_position_y()
const
354 return data->odometry_position_y;
362 MotorInterface::maxlenof_odometry_position_y()
const
374 MotorInterface::set_odometry_position_y(
const float new_odometry_position_y)
376 set_field(data->odometry_position_y, new_odometry_position_y);
386 MotorInterface::odometry_orientation()
const
388 return data->odometry_orientation;
396 MotorInterface::maxlenof_odometry_orientation()
const
408 MotorInterface::set_odometry_orientation(
const float new_odometry_orientation)
410 set_field(data->odometry_orientation, new_odometry_orientation);
420 MotorInterface::vx()
const
430 MotorInterface::maxlenof_vx()
const
442 MotorInterface::set_vx(
const float new_vx)
444 set_field(data->vx, new_vx);
454 MotorInterface::vy()
const
464 MotorInterface::maxlenof_vy()
const
476 MotorInterface::set_vy(
const float new_vy)
478 set_field(data->vy, new_vy);
488 MotorInterface::omega()
const
498 MotorInterface::maxlenof_omega()
const
510 MotorInterface::set_omega(
const float new_omega)
512 set_field(data->omega, new_omega);
522 MotorInterface::des_vx()
const
532 MotorInterface::maxlenof_des_vx()
const
544 MotorInterface::set_des_vx(
const float new_des_vx)
546 set_field(data->des_vx, new_des_vx);
556 MotorInterface::des_vy()
const
566 MotorInterface::maxlenof_des_vy()
const
578 MotorInterface::set_des_vy(
const float new_des_vy)
580 set_field(data->des_vy, new_des_vy);
590 MotorInterface::des_omega()
const
592 return data->des_omega;
600 MotorInterface::maxlenof_des_omega()
const
612 MotorInterface::set_des_omega(
const float new_des_omega)
614 set_field(data->des_omega, new_des_omega);
625 MotorInterface::controller()
const
627 return data->controller;
635 MotorInterface::maxlenof_controller()
const
648 MotorInterface::set_controller(
const uint32_t new_controller)
650 set_field(data->controller, new_controller);
662 MotorInterface::controller_thread_name()
const
664 return data->controller_thread_name;
672 MotorInterface::maxlenof_controller_thread_name()
const
686 MotorInterface::set_controller_thread_name(
const char * new_controller_thread_name)
688 set_field(data->controller_thread_name, new_controller_thread_name);
693 MotorInterface::create_message(
const char *type)
const
695 if ( strncmp(
"SetMotorStateMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
697 }
else if ( strncmp(
"AcquireControlMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
699 }
else if ( strncmp(
"ResetOdometryMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
701 }
else if ( strncmp(
"SetOdometryMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
703 }
else if ( strncmp(
"DriveRPMMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
705 }
else if ( strncmp(
"GotoMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
707 }
else if ( strncmp(
"TransMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
709 }
else if ( strncmp(
"RotMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
711 }
else if ( strncmp(
"TransRotMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
713 }
else if ( strncmp(
"OrbitMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
715 }
else if ( strncmp(
"LinTransRotMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
719 "message type for this interface type.", type);
733 type(), other->
type());
735 memcpy(data, oi->data,
sizeof(MotorInterface_data_t));
739 MotorInterface::enum_tostring(
const char *enumtype,
int val)
const
755 MotorInterface::SetMotorStateMessage::SetMotorStateMessage(
const uint32_t ini_motor_state) :
Message(
"SetMotorStateMessage")
757 data_size =
sizeof(SetMotorStateMessage_data_t);
760 data = (SetMotorStateMessage_data_t *)
data_ptr;
762 data->motor_state = ini_motor_state;
768 data_size =
sizeof(SetMotorStateMessage_data_t);
771 data = (SetMotorStateMessage_data_t *)
data_ptr;
790 data = (SetMotorStateMessage_data_t *)
data_ptr;
804 return data->motor_state;
826 set_field(data->motor_state, new_motor_state);
852 data_size =
sizeof(AcquireControlMessage_data_t);
855 data = (AcquireControlMessage_data_t *)
data_ptr;
857 data->controller = ini_controller;
858 strncpy(data->controller_thread_name, ini_controller_thread_name, 64-1);
859 data->controller_thread_name[64-1] = 0;
866 data_size =
sizeof(AcquireControlMessage_data_t);
869 data = (AcquireControlMessage_data_t *)
data_ptr;
889 data = (AcquireControlMessage_data_t *)
data_ptr;
904 return data->controller;
927 set_field(data->controller, new_controller);
941 return data->controller_thread_name;
965 set_field(data->controller_thread_name, new_controller_thread_name);
988 data_size =
sizeof(ResetOdometryMessage_data_t);
991 data = (ResetOdometryMessage_data_t *)
data_ptr;
1009 data = (ResetOdometryMessage_data_t *)
data_ptr;
1038 data_size =
sizeof(SetOdometryMessage_data_t);
1041 data = (SetOdometryMessage_data_t *)
data_ptr;
1045 data->odometry_orientation = ini_odometry_orientation;
1053 data_size =
sizeof(SetOdometryMessage_data_t);
1056 data = (SetOdometryMessage_data_t *)
data_ptr;
1077 data = (SetOdometryMessage_data_t *)
data_ptr;
1149 return data->odometry_orientation;
1169 set_field(data->odometry_orientation, new_odometry_orientation);
1196 data_size =
sizeof(DriveRPMMessage_data_t);
1199 data = (DriveRPMMessage_data_t *)
data_ptr;
1201 data->front_right = ini_front_right;
1202 data->front_left = ini_front_left;
1203 data->rear = ini_rear;
1211 data_size =
sizeof(DriveRPMMessage_data_t);
1214 data = (DriveRPMMessage_data_t *)
data_ptr;
1235 data = (DriveRPMMessage_data_t *)
data_ptr;
1247 return data->front_right;
1267 set_field(data->front_right, new_front_right);
1277 return data->front_left;
1297 set_field(data->front_left, new_front_left);
1358 data = (GotoMessage_data_t *)
data_ptr;
1362 data->phi = ini_phi;
1363 data->time_sec = ini_time_sec;
1375 data = (GotoMessage_data_t *)
data_ptr;
1397 data = (GotoMessage_data_t *)
data_ptr;
1499 return data->time_sec;
1519 set_field(data->time_sec, new_time_sec);
1545 data_size =
sizeof(TransMessage_data_t);
1548 data = (TransMessage_data_t *)
data_ptr;
1558 data_size =
sizeof(TransMessage_data_t);
1561 data = (TransMessage_data_t *)
data_ptr;
1581 data = (TransMessage_data_t *)
data_ptr;
1671 data = (RotMessage_data_t *)
data_ptr;
1673 data->omega = ini_omega;
1682 data = (RotMessage_data_t *)
data_ptr;
1701 data = (RotMessage_data_t *)
data_ptr;
1760 data_size =
sizeof(TransRotMessage_data_t);
1763 data = (TransRotMessage_data_t *)
data_ptr;
1767 data->omega = ini_omega;
1775 data_size =
sizeof(TransRotMessage_data_t);
1778 data = (TransRotMessage_data_t *)
data_ptr;
1799 data = (TransRotMessage_data_t *)
data_ptr;
1918 data_size =
sizeof(OrbitMessage_data_t);
1921 data = (OrbitMessage_data_t *)
data_ptr;
1925 data->omega = ini_omega;
1933 data_size =
sizeof(OrbitMessage_data_t);
1936 data = (OrbitMessage_data_t *)
data_ptr;
1957 data = (OrbitMessage_data_t *)
data_ptr;
2076 data_size =
sizeof(LinTransRotMessage_data_t);
2079 data = (LinTransRotMessage_data_t *)
data_ptr;
2083 data->omega = ini_omega;
2091 data_size =
sizeof(LinTransRotMessage_data_t);
2094 data = (LinTransRotMessage_data_t *)
data_ptr;
2115 data = (LinTransRotMessage_data_t *)
data_ptr;
2268 if ( m10 != NULL ) {
Base class for all Fawkes BlackBoard interfaces.
const char * type() const
Get type of interface.
void * data_ptr
Pointer to local memory storage.
void set_field(FieldT &field, DataT &data)
Set a field, set data_changed to true and update data_changed accordingly.
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.
AcquireControlMessage Fawkes BlackBoard Interface Message.
size_t maxlenof_controller() const
Get maximum length of controller value.
void set_controller(const uint32_t new_controller)
Set controller value.
size_t maxlenof_controller_thread_name() const
Get maximum length of controller_thread_name value.
AcquireControlMessage()
Constructor.
char * controller_thread_name() const
Get controller_thread_name value.
virtual Message * clone() const
Clone this message.
~AcquireControlMessage()
Destructor.
uint32_t controller() const
Get controller value.
void set_controller_thread_name(const char *new_controller_thread_name)
Set controller_thread_name value.
DriveRPMMessage Fawkes BlackBoard Interface Message.
float front_left() const
Get front_left value.
~DriveRPMMessage()
Destructor.
void set_front_right(const float new_front_right)
Set front_right value.
float rear() const
Get rear value.
size_t maxlenof_front_right() const
Get maximum length of front_right value.
size_t maxlenof_rear() const
Get maximum length of rear value.
void set_front_left(const float new_front_left)
Set front_left value.
virtual Message * clone() const
Clone this message.
DriveRPMMessage()
Constructor.
void set_rear(const float new_rear)
Set rear value.
size_t maxlenof_front_left() const
Get maximum length of front_left value.
float front_right() const
Get front_right value.
GotoMessage Fawkes BlackBoard Interface Message.
float phi() const
Get phi value.
void set_y(const float new_y)
Set y value.
size_t maxlenof_time_sec() const
Get maximum length of time_sec value.
GotoMessage()
Constructor.
void set_time_sec(const float new_time_sec)
Set time_sec value.
void set_phi(const float new_phi)
Set phi value.
size_t maxlenof_y() const
Get maximum length of y value.
float time_sec() const
Get time_sec value.
~GotoMessage()
Destructor.
size_t maxlenof_x() const
Get maximum length of x value.
float x() const
Get x value.
virtual Message * clone() const
Clone this message.
float y() const
Get y value.
void set_x(const float new_x)
Set x value.
size_t maxlenof_phi() const
Get maximum length of phi value.
LinTransRotMessage Fawkes BlackBoard Interface Message.
~LinTransRotMessage()
Destructor.
LinTransRotMessage()
Constructor.
size_t maxlenof_vy() const
Get maximum length of vy value.
float omega() const
Get omega value.
float vy() const
Get vy value.
size_t maxlenof_vx() const
Get maximum length of vx value.
float vx() const
Get vx value.
void set_vx(const float new_vx)
Set vx value.
virtual Message * clone() const
Clone this message.
void set_vy(const float new_vy)
Set vy value.
size_t maxlenof_omega() const
Get maximum length of omega value.
void set_omega(const float new_omega)
Set omega value.
OrbitMessage Fawkes BlackBoard Interface Message.
float omega() const
Get omega value.
void set_omega(const float new_omega)
Set omega value.
void set_px(const float new_px)
Set px value.
void set_py(const float new_py)
Set py value.
size_t maxlenof_omega() const
Get maximum length of omega value.
float px() const
Get px value.
size_t maxlenof_px() const
Get maximum length of px value.
OrbitMessage()
Constructor.
float py() const
Get py value.
~OrbitMessage()
Destructor.
virtual Message * clone() const
Clone this message.
size_t maxlenof_py() const
Get maximum length of py value.
ResetOdometryMessage Fawkes BlackBoard Interface Message.
virtual Message * clone() const
Clone this message.
~ResetOdometryMessage()
Destructor.
ResetOdometryMessage()
Constructor.
RotMessage Fawkes BlackBoard Interface Message.
float omega() const
Get omega value.
void set_omega(const float new_omega)
Set omega value.
virtual Message * clone() const
Clone this message.
size_t maxlenof_omega() const
Get maximum length of omega value.
SetMotorStateMessage Fawkes BlackBoard Interface Message.
uint32_t motor_state() const
Get motor_state value.
SetMotorStateMessage()
Constructor.
virtual Message * clone() const
Clone this message.
~SetMotorStateMessage()
Destructor.
void set_motor_state(const uint32_t new_motor_state)
Set motor_state value.
size_t maxlenof_motor_state() const
Get maximum length of motor_state value.
SetOdometryMessage Fawkes BlackBoard Interface Message.
size_t maxlenof_odometry_orientation() const
Get maximum length of odometry_orientation value.
void set_y(const float new_y)
Set y value.
~SetOdometryMessage()
Destructor.
size_t maxlenof_x() const
Get maximum length of x value.
void set_odometry_orientation(const float new_odometry_orientation)
Set odometry_orientation value.
size_t maxlenof_y() const
Get maximum length of y value.
SetOdometryMessage()
Constructor.
float x() const
Get x value.
float y() const
Get y value.
virtual Message * clone() const
Clone this message.
float odometry_orientation() const
Get odometry_orientation value.
void set_x(const float new_x)
Set x value.
TransMessage Fawkes BlackBoard Interface Message.
~TransMessage()
Destructor.
virtual Message * clone() const
Clone this message.
void set_vx(const float new_vx)
Set vx value.
void set_vy(const float new_vy)
Set vy value.
TransMessage()
Constructor.
size_t maxlenof_vy() const
Get maximum length of vy value.
float vx() const
Get vx value.
size_t maxlenof_vx() const
Get maximum length of vx value.
float vy() const
Get vy value.
TransRotMessage Fawkes BlackBoard Interface Message.
TransRotMessage()
Constructor.
size_t maxlenof_vy() const
Get maximum length of vy value.
float vx() const
Get vx value.
void set_omega(const float new_omega)
Set omega value.
size_t maxlenof_vx() const
Get maximum length of vx value.
void set_vx(const float new_vx)
Set vx value.
~TransRotMessage()
Destructor.
void set_vy(const float new_vy)
Set vy value.
virtual Message * clone() const
Clone this message.
size_t maxlenof_omega() const
Get maximum length of omega value.
float omega() const
Get omega value.
float vy() const
Get vy value.
MotorInterface Fawkes BlackBoard Interface.
static const uint32_t DRIVE_MODE_RPM
DRIVE_MODE_RPM constant.
virtual bool message_valid(const Message *message) const
Check if message is valid and can be enqueued.
static const uint32_t MOTOR_DISABLED
MOTOR_DISABLED constant.
static const uint32_t DRIVE_MODE_TRANS_ROT
DRIVE_MODE_TRANS_ROT constant.
static const uint32_t DRIVE_MODE_ORBIT
DRIVE_MODE_ORBIT constant.
static const uint32_t DRIVE_MODE_LINE_TRANS_ROT
DRIVE_MODE_LINE_TRANS_ROT constant.
static const uint32_t DRIVE_MODE_TRANS
DRIVE_MODE_TRANS constant.
static const uint32_t MOTOR_ENABLED
MOTOR_ENABLED constant.
static const uint32_t DRIVE_MODE_ROT
DRIVE_MODE_ROT constant.
Fawkes library namespace.
@ IFT_UINT32
32 bit unsigned integer field
Timestamp data, must be present and first entries for each interface data structs!...