24 #include <interfaces/JacoInterface.h> 26 #include <core/exceptions/software.h> 54 JacoInterface::JacoInterface() : Interface()
58 data = (JacoInterface_data_t *)
data_ptr;
85 unsigned char tmp_hash[] = {0xff, 0x31, 0x58, 0x1c, 0x31, 0x7a, 0xc5, 0xb4, 0x2a, 0x56, 0x87, 0x5, 0x34, 0xfe, 0x6, 0x93};
90 JacoInterface::~JacoInterface()
102 return data->connected;
122 data->connected = new_connected;
133 return data->initialized;
153 data->initialized = new_initialized;
277 data->euler1 = new_euler1;
308 data->euler2 = new_euler2;
339 data->euler3 = new_euler3;
363 throw Exception(
"Index value %u out of bounds (0..6)", index);
365 return data->joints[index];
385 memcpy(data->joints, new_joints,
sizeof(
float) * 6);
398 throw Exception(
"Index value %u out of bounds (0..6)", index);
400 data->joints[index] = new_joints;
410 return data->finger1;
430 data->finger1 = new_finger1;
441 return data->finger2;
461 data->finger2 = new_finger2;
472 return data->finger3;
492 data->finger3 = new_finger3;
525 data->msgid = new_msgid;
558 data->final = new_final;
571 return data->error_code;
593 data->error_code = new_error_code;
601 if ( strncmp(
"CalibrateMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
603 }
else if ( strncmp(
"RetractMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
605 }
else if ( strncmp(
"StopMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
607 }
else if ( strncmp(
"CartesianGotoMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
609 }
else if ( strncmp(
"AngularGotoMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
611 }
else if ( strncmp(
"MoveGripperMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
613 }
else if ( strncmp(
"SetPlannerParamsMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
615 }
else if ( strncmp(
"JoystickPushMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
617 }
else if ( strncmp(
"JoystickReleaseMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
621 "message type for this interface type.", type);
637 memcpy(data, oi->data,
sizeof(JacoInterface_data_t));
657 data_size =
sizeof(CalibrateMessage_data_t);
660 data = (CalibrateMessage_data_t *)
data_ptr;
678 data = (CalibrateMessage_data_t *)
data_ptr;
703 data_size =
sizeof(RetractMessage_data_t);
706 data = (RetractMessage_data_t *)
data_ptr;
724 data = (RetractMessage_data_t *)
data_ptr;
752 data = (StopMessage_data_t *)
data_ptr;
770 data = (StopMessage_data_t *)
data_ptr;
802 data_size =
sizeof(CartesianGotoMessage_data_t);
805 data = (CartesianGotoMessage_data_t *)
data_ptr;
823 data_size =
sizeof(CartesianGotoMessage_data_t);
826 data = (CartesianGotoMessage_data_t *)
data_ptr;
850 data = (CartesianGotoMessage_data_t *)
data_ptr;
1062 data_size =
sizeof(AngularGotoMessage_data_t);
1065 data = (AngularGotoMessage_data_t *)
data_ptr;
1083 data_size =
sizeof(AngularGotoMessage_data_t);
1086 data = (AngularGotoMessage_data_t *)
data_ptr;
1110 data = (AngularGotoMessage_data_t *)
data_ptr;
1319 data_size =
sizeof(MoveGripperMessage_data_t);
1322 data = (MoveGripperMessage_data_t *)
data_ptr;
1324 data->finger1 = ini_finger1;
1325 data->finger2 = ini_finger2;
1326 data->finger3 = ini_finger3;
1334 data_size =
sizeof(MoveGripperMessage_data_t);
1337 data = (MoveGripperMessage_data_t *)
data_ptr;
1358 data = (MoveGripperMessage_data_t *)
data_ptr;
1370 return data->finger1;
1390 data->finger1 = new_finger1;
1400 return data->finger2;
1420 data->finger2 = new_finger2;
1430 return data->finger3;
1450 data->finger3 = new_finger3;
1475 data_size =
sizeof(SetPlannerParamsMessage_data_t);
1478 data = (SetPlannerParamsMessage_data_t *)
data_ptr;
1480 strncpy(data->params, ini_params, 1024);
1486 data_size =
sizeof(SetPlannerParamsMessage_data_t);
1489 data = (SetPlannerParamsMessage_data_t *)
data_ptr;
1508 data = (SetPlannerParamsMessage_data_t *)
data_ptr;
1520 return data->params;
1540 strncpy(data->params, new_params,
sizeof(data->params));
1565 data_size =
sizeof(JoystickPushMessage_data_t);
1568 data = (JoystickPushMessage_data_t *)
data_ptr;
1570 data->button = ini_button;
1576 data_size =
sizeof(JoystickPushMessage_data_t);
1579 data = (JoystickPushMessage_data_t *)
data_ptr;
1598 data = (JoystickPushMessage_data_t *)
data_ptr;
1610 return data->button;
1630 data->button = new_button;
1653 data_size =
sizeof(JoystickReleaseMessage_data_t);
1656 data = (JoystickReleaseMessage_data_t *)
data_ptr;
1674 data = (JoystickReleaseMessage_data_t *)
data_ptr;
void set_finger1(const float new_finger1)
Set finger1 value.
virtual Message * clone() const
Clone this message.
size_t maxlenof_params() const
Get maximum length of params value.
float z() const
Get z value.
size_t maxlenof_e3() const
Get maximum length of e3 value.
void set_x(const float new_x)
Set x value.
size_t maxlenof_finger3() const
Get maximum length of finger3 value.
float j6() const
Get j6 value.
size_t maxlenof_connected() const
Get maximum length of connected value.
void * data_ptr
Pointer to memory that contains local data.
size_t maxlenof_finger1() const
Get maximum length of finger1 value.
void set_y(const float new_y)
Set y value.
size_t maxlenof_e1() const
Get maximum length of e1 value.
~MoveGripperMessage()
Destructor.
size_t maxlenof_joints() const
Get maximum length of joints value.
Base class for all messages passed through interfaces in Fawkes BlackBoard.
void set_j1(const float new_j1)
Set j1 value.
~RetractMessage()
Destructor.
~SetPlannerParamsMessage()
Destructor.
float z() const
Get z value.
float finger2() const
Get finger2 value.
size_t maxlenof_finger3() const
Get maximum length of finger3 value.
size_t maxlenof_z() const
Get maximum length of z value.
void set_e2(const float new_e2)
Set e2 value.
void set_hash(unsigned char *ihash)
Set hash.
size_t maxlenof_y() const
Get maximum length of y value.
Fawkes library namespace.
SetPlannerParamsMessage Fawkes BlackBoard Interface Message.
void set_j5(const float new_j5)
Set j5 value.
RetractMessage()
Constructor.
Timestamp data, must be present and first entries for each interface data structs! This leans on time...
void set_e3(const float new_e3)
Set e3 value.
size_t maxlenof_finger2() const
Get maximum length of finger2 value.
void set_finger1(const float new_finger1)
Set finger1 value.
unsigned int data_size
Minimal data size to hold data storage.
float finger2() const
Get finger2 value.
size_t maxlenof_msgid() const
Get maximum length of msgid value.
SetPlannerParamsMessage()
Constructor.
virtual Message * clone() const
Clone this message.
size_t maxlenof_e2() const
Get maximum length of e2 value.
float j2() const
Get j2 value.
size_t maxlenof_j5() const
Get maximum length of j5 value.
JoystickPushMessage()
Constructor.
virtual Message * create_message(const char *type) const
Create message based on type name.
MoveGripperMessage Fawkes BlackBoard Interface Message.
bool is_final() const
Get final value.
size_t maxlenof_finger2() const
Get maximum length of finger2 value.
Base class for all Fawkes BlackBoard interfaces.
void set_j3(const float new_j3)
Set j3 value.
uint32_t error_code() const
Get error_code value.
virtual Message * clone() const
Clone this message.
CalibrateMessage()
Constructor.
static const uint32_t ERROR_NO_IK
ERROR_NO_IK constant.
float y() const
Get y value.
float finger3() const
Get finger3 value.
size_t maxlenof_error_code() const
Get maximum length of error_code value.
message_data_ts_t * data_ts
data timestamp aliasing pointer
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 field info list.
unsigned int data_size
Size of memory needed to hold all data.
void set_connected(const bool new_connected)
Set connected value.
float euler3() const
Get euler3 value.
static const uint32_t ERROR_NONE
ERROR_NONE constant.
size_t maxlenof_initialized() const
Get maximum length of initialized value.
void add_messageinfo(const char *name)
Add an entry to the message info list.
virtual Message * clone() const
Clone this message.
bool data_changed
Indicator if data has changed.
float euler2() const
Get euler2 value.
~JoystickPushMessage()
Destructor.
float finger1() const
Get finger1 value.
const char * type() const
Get type of interface.
virtual Message * clone() const
Clone this message.
float x() const
Get x value.
void * data_ptr
Pointer to local memory storage.
static const uint32_t ERROR_UNSPECIFIC
ERROR_UNSPECIFIC constant.
float * joints() const
Get joints value.
Base class for exceptions in Fawkes.
uint32_t msgid() const
Get msgid value.
MoveGripperMessage()
Constructor.
void set_j6(const float new_j6)
Set j6 value.
void set_finger2(const float new_finger2)
Set finger2 value.
virtual Message * clone() const
Clone this message.
size_t maxlenof_j2() const
Get maximum length of j2 value.
JoystickReleaseMessage()
Constructor.
CalibrateMessage Fawkes BlackBoard Interface Message.
StopMessage Fawkes BlackBoard Interface Message.
size_t maxlenof_j6() const
Get maximum length of j6 value.
size_t maxlenof_z() const
Get maximum length of z value.
void set_z(const float new_z)
Set z value.
void set_j4(const float new_j4)
Set j4 value.
void set_e1(const float new_e1)
Set e1 value.
void set_error_code(const uint32_t new_error_code)
Set error_code value.
size_t maxlenof_j4() const
Get maximum length of j4 value.
virtual Message * clone() const
Clone this message.
void set_initialized(const bool new_initialized)
Set initialized value.
size_t maxlenof_finger1() const
Get maximum length of finger1 value.
size_t maxlenof_x() const
Get maximum length of x value.
float e3() const
Get e3 value.
float j4() const
Get j4 value.
size_t maxlenof_button() const
Get maximum length of button value.
float euler1() const
Get euler1 value.
AngularGotoMessage()
Constructor.
bool is_initialized() const
Get initialized value.
void set_joints(unsigned int index, const float new_joints)
Set joints value at given index.
virtual const char * enum_tostring(const char *enumtype, int val) const
Convert arbitrary enum value to string.
float e2() const
Get e2 value.
uint32_t button() const
Get button value.
virtual Message * clone() const
Clone this message.
void set_finger3(const float new_finger3)
Set finger3 value.
virtual bool message_valid(const Message *message) const
Check if message is valid and can be enqueued.
size_t maxlenof_euler3() const
Get maximum length of euler3 value.
void set_final(const bool new_final)
Set final value.
char * params() const
Get params value.
JoystickPushMessage Fawkes BlackBoard Interface Message.
void set_euler3(const float new_euler3)
Set euler3 value.
void set_z(const float new_z)
Set z value.
float j1() const
Get j1 value.
void set_msgid(const uint32_t new_msgid)
Set msgid value.
virtual Message * clone() const
Clone this message.
JacoInterface Fawkes BlackBoard Interface.
void set_j2(const float new_j2)
Set j2 value.
AngularGotoMessage Fawkes BlackBoard Interface Message.
void set_euler1(const float new_euler1)
Set euler1 value.
float x() const
Get x value.
~StopMessage()
Destructor.
JoystickReleaseMessage Fawkes BlackBoard Interface Message.
void set_params(const char *new_params)
Set params value.
float j3() const
Get j3 value.
void set_y(const float new_y)
Set y value.
interface_data_ts_t * data_ts
Pointer to data casted to timestamp struct.
~JoystickReleaseMessage()
Destructor.
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.
size_t maxlenof_final() const
Get maximum length of final value.
size_t maxlenof_x() const
Get maximum length of x value.
~CartesianGotoMessage()
Destructor.
bool is_connected() const
Get connected value.
void set_finger3(const float new_finger3)
Set finger3 value.
float finger3() const
Get finger3 value.
CartesianGotoMessage Fawkes BlackBoard Interface Message.
float j5() const
Get j5 value.
size_t maxlenof_j1() const
Get maximum length of j1 value.
size_t maxlenof_j3() const
Get maximum length of j3 value.
RetractMessage Fawkes BlackBoard Interface Message.
virtual void copy_values(const Interface *other)
Copy values from other interface.
void set_finger2(const float new_finger2)
Set finger2 value.
~AngularGotoMessage()
Destructor.
static const uint32_t ERROR_PLANNING
ERROR_PLANNING constant.
32 bit unsigned integer field
float y() const
Get y value.
void set_button(const uint32_t new_button)
Set button value.
~CalibrateMessage()
Destructor.
size_t maxlenof_euler2() const
Get maximum length of euler2 value.
size_t maxlenof_y() const
Get maximum length of y value.
StopMessage()
Constructor.
float finger1() const
Get finger1 value.
void set_x(const float new_x)
Set x value.
void set_euler2(const float new_euler2)
Set euler2 value.
size_t maxlenof_euler1() const
Get maximum length of euler1 value.
float e1() const
Get e1 value.
CartesianGotoMessage()
Constructor.