24 #include <interfaces/JacoInterface.h>
26 #include <core/exceptions/software.h>
54 JacoInterface::JacoInterface() : Interface()
56 data_size =
sizeof(JacoInterface_data_t);
57 data_ptr = malloc(data_size);
58 data = (JacoInterface_data_t *)data_ptr;
59 data_ts = (interface_data_ts_t *)data_ptr;
60 memset(data_ptr, 0, data_size);
61 add_fieldinfo(IFT_BOOL,
"connected", 1, &data->connected);
62 add_fieldinfo(IFT_BOOL,
"initialized", 1, &data->initialized);
63 add_fieldinfo(IFT_FLOAT,
"x", 1, &data->x);
64 add_fieldinfo(IFT_FLOAT,
"y", 1, &data->y);
65 add_fieldinfo(IFT_FLOAT,
"z", 1, &data->z);
66 add_fieldinfo(IFT_FLOAT,
"euler1", 1, &data->euler1);
67 add_fieldinfo(IFT_FLOAT,
"euler2", 1, &data->euler2);
68 add_fieldinfo(IFT_FLOAT,
"euler3", 1, &data->euler3);
69 add_fieldinfo(IFT_FLOAT,
"joints", 6, &data->joints);
70 add_fieldinfo(IFT_FLOAT,
"finger1", 1, &data->finger1);
71 add_fieldinfo(IFT_FLOAT,
"finger2", 1, &data->finger2);
72 add_fieldinfo(IFT_FLOAT,
"finger3", 1, &data->finger3);
73 add_fieldinfo(IFT_UINT32,
"msgid", 1, &data->msgid);
74 add_fieldinfo(IFT_BOOL,
"final", 1, &data->final);
75 add_fieldinfo(IFT_UINT32,
"error_code", 1, &data->error_code);
76 add_messageinfo(
"CalibrateMessage");
77 add_messageinfo(
"RetractMessage");
78 add_messageinfo(
"StopMessage");
79 add_messageinfo(
"CartesianGotoMessage");
80 add_messageinfo(
"AngularGotoMessage");
81 add_messageinfo(
"MoveGripperMessage");
82 add_messageinfo(
"SetPlannerParamsMessage");
83 add_messageinfo(
"JoystickPushMessage");
84 add_messageinfo(
"JoystickReleaseMessage");
85 unsigned char tmp_hash[] = {0xff, 0x31, 0x58, 0x1c, 0x31, 0x7a, 0xc5, 0xb4, 0x2a, 0x56, 0x87, 0x5, 0x34, 0xfe, 0x6, 0x93};
90 JacoInterface::~JacoInterface()
100 JacoInterface::is_connected()
const
102 return data->connected;
110 JacoInterface::maxlenof_connected()
const
120 JacoInterface::set_connected(
const bool new_connected)
122 set_field(data->connected, new_connected);
130 JacoInterface::is_initialized()
const
132 return data->initialized;
140 JacoInterface::maxlenof_initialized()
const
150 JacoInterface::set_initialized(
const bool new_initialized)
152 set_field(data->initialized, new_initialized);
160 JacoInterface::x()
const
170 JacoInterface::maxlenof_x()
const
180 JacoInterface::set_x(
const float new_x)
182 set_field(data->x, new_x);
190 JacoInterface::y()
const
200 JacoInterface::maxlenof_y()
const
210 JacoInterface::set_y(
const float new_y)
212 set_field(data->y, new_y);
220 JacoInterface::z()
const
230 JacoInterface::maxlenof_z()
const
240 JacoInterface::set_z(
const float new_z)
242 set_field(data->z, new_z);
250 JacoInterface::euler1()
const
260 JacoInterface::maxlenof_euler1()
const
270 JacoInterface::set_euler1(
const float new_euler1)
272 set_field(data->euler1, new_euler1);
280 JacoInterface::euler2()
const
290 JacoInterface::maxlenof_euler2()
const
300 JacoInterface::set_euler2(
const float new_euler2)
302 set_field(data->euler2, new_euler2);
310 JacoInterface::euler3()
const
320 JacoInterface::maxlenof_euler3()
const
330 JacoInterface::set_euler3(
const float new_euler3)
332 set_field(data->euler3, new_euler3);
340 JacoInterface::joints()
const
352 JacoInterface::joints(
unsigned int index)
const
355 throw Exception(
"Index value %u out of bounds (0..5)", index);
357 return data->joints[index];
365 JacoInterface::maxlenof_joints()
const
375 JacoInterface::set_joints(
const float * new_joints)
377 set_field(data->joints, new_joints);
386 JacoInterface::set_joints(
unsigned int index,
const float new_joints)
388 set_field(data->joints, index, new_joints);
395 JacoInterface::finger1()
const
397 return data->finger1;
405 JacoInterface::maxlenof_finger1()
const
415 JacoInterface::set_finger1(
const float new_finger1)
417 set_field(data->finger1, new_finger1);
425 JacoInterface::finger2()
const
427 return data->finger2;
435 JacoInterface::maxlenof_finger2()
const
445 JacoInterface::set_finger2(
const float new_finger2)
447 set_field(data->finger2, new_finger2);
455 JacoInterface::finger3()
const
457 return data->finger3;
465 JacoInterface::maxlenof_finger3()
const
475 JacoInterface::set_finger3(
const float new_finger3)
477 set_field(data->finger3, new_finger3);
486 JacoInterface::msgid()
const
496 JacoInterface::maxlenof_msgid()
const
507 JacoInterface::set_msgid(
const uint32_t new_msgid)
509 set_field(data->msgid, new_msgid);
518 JacoInterface::is_final()
const
528 JacoInterface::maxlenof_final()
const
539 JacoInterface::set_final(
const bool new_final)
541 set_field(data->final, new_final);
551 JacoInterface::error_code()
const
553 return data->error_code;
561 JacoInterface::maxlenof_error_code()
const
573 JacoInterface::set_error_code(
const uint32_t new_error_code)
575 set_field(data->error_code, new_error_code);
580 JacoInterface::create_message(
const char *type)
const
582 if ( strncmp(
"CalibrateMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
584 }
else if ( strncmp(
"RetractMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
586 }
else if ( strncmp(
"StopMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
588 }
else if ( strncmp(
"CartesianGotoMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
590 }
else if ( strncmp(
"AngularGotoMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
592 }
else if ( strncmp(
"MoveGripperMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
594 }
else if ( strncmp(
"SetPlannerParamsMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
596 }
else if ( strncmp(
"JoystickPushMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
598 }
else if ( strncmp(
"JoystickReleaseMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
602 "message type for this interface type.", type);
616 type(), other->
type());
618 memcpy(data, oi->data,
sizeof(JacoInterface_data_t));
622 JacoInterface::enum_tostring(
const char *enumtype,
int val)
const
636 JacoInterface::CalibrateMessage::CalibrateMessage() :
Message(
"CalibrateMessage")
638 data_size =
sizeof(CalibrateMessage_data_t);
641 data = (CalibrateMessage_data_t *)
data_ptr;
659 data = (CalibrateMessage_data_t *)
data_ptr;
684 data_size =
sizeof(RetractMessage_data_t);
687 data = (RetractMessage_data_t *)
data_ptr;
705 data = (RetractMessage_data_t *)
data_ptr;
733 data = (StopMessage_data_t *)
data_ptr;
751 data = (StopMessage_data_t *)
data_ptr;
783 data_size =
sizeof(CartesianGotoMessage_data_t);
786 data = (CartesianGotoMessage_data_t *)
data_ptr;
804 data_size =
sizeof(CartesianGotoMessage_data_t);
807 data = (CartesianGotoMessage_data_t *)
data_ptr;
831 data = (CartesianGotoMessage_data_t *)
data_ptr;
1043 data_size =
sizeof(AngularGotoMessage_data_t);
1046 data = (AngularGotoMessage_data_t *)
data_ptr;
1064 data_size =
sizeof(AngularGotoMessage_data_t);
1067 data = (AngularGotoMessage_data_t *)
data_ptr;
1091 data = (AngularGotoMessage_data_t *)
data_ptr;
1300 data_size =
sizeof(MoveGripperMessage_data_t);
1303 data = (MoveGripperMessage_data_t *)
data_ptr;
1305 data->finger1 = ini_finger1;
1306 data->finger2 = ini_finger2;
1307 data->finger3 = ini_finger3;
1315 data_size =
sizeof(MoveGripperMessage_data_t);
1318 data = (MoveGripperMessage_data_t *)
data_ptr;
1339 data = (MoveGripperMessage_data_t *)
data_ptr;
1351 return data->finger1;
1381 return data->finger2;
1411 return data->finger3;
1456 data_size =
sizeof(SetPlannerParamsMessage_data_t);
1459 data = (SetPlannerParamsMessage_data_t *)
data_ptr;
1461 strncpy(data->params, ini_params, 1024-1);
1462 data->params[1024-1] = 0;
1468 data_size =
sizeof(SetPlannerParamsMessage_data_t);
1471 data = (SetPlannerParamsMessage_data_t *)
data_ptr;
1490 data = (SetPlannerParamsMessage_data_t *)
data_ptr;
1502 return data->params;
1547 data_size =
sizeof(JoystickPushMessage_data_t);
1550 data = (JoystickPushMessage_data_t *)
data_ptr;
1552 data->button = ini_button;
1558 data_size =
sizeof(JoystickPushMessage_data_t);
1561 data = (JoystickPushMessage_data_t *)
data_ptr;
1580 data = (JoystickPushMessage_data_t *)
data_ptr;
1592 return data->button;
1635 data_size =
sizeof(JoystickReleaseMessage_data_t);
1638 data = (JoystickReleaseMessage_data_t *)
data_ptr;
1656 data = (JoystickReleaseMessage_data_t *)
data_ptr;
Base class for exceptions in Fawkes.
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.
AngularGotoMessage Fawkes BlackBoard Interface Message.
AngularGotoMessage()
Constructor.
void set_j3(const float new_j3)
Set j3 value.
float j1() const
Get j1 value.
size_t maxlenof_j6() const
Get maximum length of j6 value.
void set_j5(const float new_j5)
Set j5 value.
float j4() const
Get j4 value.
void set_j1(const float new_j1)
Set j1 value.
float j5() const
Get j5 value.
void set_j6(const float new_j6)
Set j6 value.
size_t maxlenof_j2() const
Get maximum length of j2 value.
~AngularGotoMessage()
Destructor.
void set_j2(const float new_j2)
Set j2 value.
float j3() const
Get j3 value.
float j6() const
Get j6 value.
float j2() const
Get j2 value.
size_t maxlenof_j3() const
Get maximum length of j3 value.
size_t maxlenof_j1() const
Get maximum length of j1 value.
virtual Message * clone() const
Clone this message.
void set_j4(const float new_j4)
Set j4 value.
size_t maxlenof_j4() const
Get maximum length of j4 value.
size_t maxlenof_j5() const
Get maximum length of j5 value.
CalibrateMessage Fawkes BlackBoard Interface Message.
virtual Message * clone() const
Clone this message.
CalibrateMessage()
Constructor.
~CalibrateMessage()
Destructor.
CartesianGotoMessage Fawkes BlackBoard Interface Message.
size_t maxlenof_y() const
Get maximum length of y value.
size_t maxlenof_e2() const
Get maximum length of e2 value.
size_t maxlenof_z() const
Get maximum length of z value.
float e1() const
Get e1 value.
float x() const
Get x value.
~CartesianGotoMessage()
Destructor.
float y() const
Get y value.
size_t maxlenof_x() const
Get maximum length of x value.
size_t maxlenof_e1() const
Get maximum length of e1 value.
float z() const
Get z value.
CartesianGotoMessage()
Constructor.
void set_e1(const float new_e1)
Set e1 value.
void set_x(const float new_x)
Set x value.
void set_e2(const float new_e2)
Set e2 value.
virtual Message * clone() const
Clone this message.
void set_e3(const float new_e3)
Set e3 value.
void set_y(const float new_y)
Set y value.
float e2() const
Get e2 value.
size_t maxlenof_e3() const
Get maximum length of e3 value.
void set_z(const float new_z)
Set z value.
float e3() const
Get e3 value.
JoystickPushMessage Fawkes BlackBoard Interface Message.
void set_button(const uint32_t new_button)
Set button value.
uint32_t button() const
Get button value.
size_t maxlenof_button() const
Get maximum length of button value.
virtual Message * clone() const
Clone this message.
JoystickPushMessage()
Constructor.
~JoystickPushMessage()
Destructor.
JoystickReleaseMessage Fawkes BlackBoard Interface Message.
~JoystickReleaseMessage()
Destructor.
JoystickReleaseMessage()
Constructor.
virtual Message * clone() const
Clone this message.
MoveGripperMessage Fawkes BlackBoard Interface Message.
float finger1() const
Get finger1 value.
size_t maxlenof_finger1() const
Get maximum length of finger1 value.
virtual Message * clone() const
Clone this message.
MoveGripperMessage()
Constructor.
void set_finger3(const float new_finger3)
Set finger3 value.
size_t maxlenof_finger2() const
Get maximum length of finger2 value.
void set_finger2(const float new_finger2)
Set finger2 value.
float finger3() const
Get finger3 value.
size_t maxlenof_finger3() const
Get maximum length of finger3 value.
void set_finger1(const float new_finger1)
Set finger1 value.
float finger2() const
Get finger2 value.
~MoveGripperMessage()
Destructor.
RetractMessage Fawkes BlackBoard Interface Message.
~RetractMessage()
Destructor.
virtual Message * clone() const
Clone this message.
RetractMessage()
Constructor.
SetPlannerParamsMessage Fawkes BlackBoard Interface Message.
size_t maxlenof_params() const
Get maximum length of params value.
~SetPlannerParamsMessage()
Destructor.
void set_params(const char *new_params)
Set params value.
char * params() const
Get params value.
SetPlannerParamsMessage()
Constructor.
virtual Message * clone() const
Clone this message.
StopMessage Fawkes BlackBoard Interface Message.
virtual Message * clone() const
Clone this message.
~StopMessage()
Destructor.
StopMessage()
Constructor.
JacoInterface Fawkes BlackBoard Interface.
static const uint32_t ERROR_NONE
ERROR_NONE constant.
virtual bool message_valid(const Message *message) const
Check if message is valid and can be enqueued.
static const uint32_t ERROR_NO_IK
ERROR_NO_IK constant.
static const uint32_t ERROR_UNSPECIFIC
ERROR_UNSPECIFIC constant.
static const uint32_t ERROR_PLANNING
ERROR_PLANNING constant.
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_UINT32
32 bit unsigned integer field
Timestamp data, must be present and first entries for each interface data structs!...