Fawkes API
Fawkes Development Version
|
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..5)", 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..5)", 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_ - 1) == 0 ) {
603 }
else if ( strncmp(
"RetractMessage",
type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
605 }
else if ( strncmp(
"StopMessage",
type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
606 return new StopMessage();
607 }
else if ( strncmp(
"CartesianGotoMessage",
type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
608 return new CartesianGotoMessage();
609 }
else if ( strncmp(
"AngularGotoMessage",
type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
610 return new AngularGotoMessage();
611 }
else if ( strncmp(
"MoveGripperMessage",
type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
612 return new MoveGripperMessage();
613 }
else if ( strncmp(
"SetPlannerParamsMessage",
type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
614 return new SetPlannerParamsMessage();
615 }
else if ( strncmp(
"JoystickPushMessage",
type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
616 return new JoystickPushMessage();
617 }
else if ( strncmp(
"JoystickReleaseMessage",
type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
618 return new JoystickReleaseMessage();
620 throw UnknownTypeException(
"The given type '%s' does not match any known "
621 "message type for this interface type.",
type);
632 const JacoInterface *oi =
dynamic_cast<const JacoInterface *
>(other);
634 throw TypeMismatchException(
"Can only copy values from interface of same type (%s vs. %s)",
635 type(), other->type());
637 memcpy(data, oi->data,
sizeof(JacoInterface_data_t));
643 throw UnknownTypeException(
"Unknown enum type %s", enumtype);
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-1);
1481 data->params[1024-1] = 0;
1487 data_size =
sizeof(SetPlannerParamsMessage_data_t);
1490 data = (SetPlannerParamsMessage_data_t *)
data_ptr;
1509 data = (SetPlannerParamsMessage_data_t *)
data_ptr;
1521 return data->params;
1541 strncpy(data->params, new_params,
sizeof(data->params)-1);
1542 data->params[
sizeof(data->params)-1] = 0;
1567 data_size =
sizeof(JoystickPushMessage_data_t);
1570 data = (JoystickPushMessage_data_t *)
data_ptr;
1572 data->button = ini_button;
1578 data_size =
sizeof(JoystickPushMessage_data_t);
1581 data = (JoystickPushMessage_data_t *)
data_ptr;
1600 data = (JoystickPushMessage_data_t *)
data_ptr;
1612 return data->button;
1632 data->button = new_button;
1655 data_size =
sizeof(JoystickReleaseMessage_data_t);
1658 data = (JoystickReleaseMessage_data_t *)
data_ptr;
1676 data = (JoystickReleaseMessage_data_t *)
data_ptr;
1706 const StopMessage *m2 =
dynamic_cast<const StopMessage *
>(message);
1710 const CartesianGotoMessage *m3 =
dynamic_cast<const CartesianGotoMessage *
>(message);
1714 const AngularGotoMessage *m4 =
dynamic_cast<const AngularGotoMessage *
>(message);
1718 const MoveGripperMessage *m5 =
dynamic_cast<const MoveGripperMessage *
>(message);
1722 const SetPlannerParamsMessage *m6 =
dynamic_cast<const SetPlannerParamsMessage *
>(message);
1726 const JoystickPushMessage *m7 =
dynamic_cast<const JoystickPushMessage *
>(message);
1730 const JoystickReleaseMessage *m8 =
dynamic_cast<const JoystickReleaseMessage *
>(message);
1738 EXPORT_INTERFACE(JacoInterface)
size_t maxlenof_euler3() const
Get maximum length of euler3 value.
void set_y(const float new_y)
Set y value.
bool is_connected() const
Get connected value.
void set_finger1(const float new_finger1)
Set finger1 value.
float finger3() const
Get finger3 value.
bool is_final() const
Get final value.
~CartesianGotoMessage()
Destructor.
SetPlannerParamsMessage()
Constructor.
~MoveGripperMessage()
Destructor.
size_t maxlenof_j6() const
Get maximum length of j6 value.
virtual Message * clone() const
Clone this message.
float y() const
Get y value.
virtual Message * create_message(const char *type) const
void set_x(const float new_x)
Set x value.
size_t maxlenof_finger2() const
Get maximum length of finger2 value.
~AngularGotoMessage()
Destructor.
size_t maxlenof_j3() const
Get maximum length of j3 value.
void set_connected(const bool new_connected)
Set connected value.
float z() const
Get z value.
uint32_t error_code() const
Get error_code value.
void set_finger1(const float new_finger1)
Set finger1 value.
void set_j2(const float new_j2)
Set j2 value.
size_t maxlenof_e3() const
Get maximum length of e3 value.
void set_y(const float new_y)
Set y value.
float e1() const
Get e1 value.
CartesianGotoMessage()
Constructor.
size_t maxlenof_euler2() const
Get maximum length of euler2 value.
size_t maxlenof_e1() const
Get maximum length of e1 value.
MoveGripperMessage()
Constructor.
message_data_ts_t * data_ts
data timestamp aliasing pointer
float j4() const
Get j4 value.
~SetPlannerParamsMessage()
Destructor.
uint32_t msgid() const
Get msgid value.
float euler3() const
Get euler3 value.
@ IFT_UINT32
32 bit unsigned integer field
void set_x(const float new_x)
Set x value.
static const uint32_t ERROR_NONE
ERROR_NONE constant.
~StopMessage()
Destructor.
size_t maxlenof_initialized() const
Get maximum length of initialized value.
size_t maxlenof_z() const
Get maximum length of z value.
size_t maxlenof_finger3() const
Get maximum length of finger3 value.
const char * type() const
Get type of interface.
void set_j5(const float new_j5)
Set j5 value.
float z() const
Get z value.
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.
JoystickReleaseMessage()
Constructor.
void set_e2(const float new_e2)
Set e2 value.
float finger1() const
Get finger1 value.
float j1() const
Get j1 value.
size_t maxlenof_y() const
Get maximum length of y value.
float x() const
Get x value.
float * joints() const
Get joints value.
float j5() const
Get j5 value.
interface_data_ts_t * data_ts
size_t maxlenof_z() const
Get maximum length of z value.
void set_z(const float new_z)
Set z value.
size_t maxlenof_j4() const
Get maximum length of j4 value.
RetractMessage()
Constructor.
float j3() const
Get j3 value.
size_t maxlenof_e2() const
Get maximum length of e2 value.
Timestamp data, must be present and first entries for each interface data structs!...
size_t maxlenof_finger3() const
Get maximum length of finger3 value.
void set_e1(const float new_e1)
Set e1 value.
bool is_initialized() const
Get initialized value.
virtual bool message_valid(const Message *message) const
Check if message is valid and can be enqueued.
void set_finger3(const float new_finger3)
Set finger3 value.
virtual Message * clone() const
Clone this message.
float finger1() const
Get finger1 value.
virtual Message * clone() const
Clone this message.
void set_j1(const float new_j1)
Set j1 value.
size_t maxlenof_j5() const
Get maximum length of j5 value.
void set_euler3(const float new_euler3)
Set euler3 value.
static const uint32_t ERROR_NO_IK
ERROR_NO_IK constant.
~JoystickPushMessage()
Destructor.
float e2() const
Get e2 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
void set_hash(unsigned char *ihash)
Set hash.
size_t maxlenof_error_code() const
Get maximum length of error_code value.
StopMessage()
Constructor.
float y() const
Get y value.
size_t maxlenof_finger2() const
Get maximum length of finger2 value.
virtual Message * clone() const
Clone this message.
uint32_t button() const
Get button value.
size_t maxlenof_finger1() const
Get maximum length of finger1 value.
float euler2() const
Get euler2 value.
virtual Message * clone() const
Clone this message.
void set_z(const float new_z)
Set z value.
virtual Message * clone() const
Clone this message.
void set_euler1(const float new_euler1)
Set euler1 value.
float finger3() const
Get finger3 value.
size_t maxlenof_j2() const
Get maximum length of j2 value.
float j6() const
Get j6 value.
virtual void copy_values(const Interface *other)
Copy values from other interface.
~CalibrateMessage()
Destructor.
void set_finger2(const float new_finger2)
Set finger2 value.
JoystickPushMessage()
Constructor.
size_t maxlenof_x() const
Get maximum length of x value.
size_t maxlenof_y() const
Get maximum length of y value.
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_euler1() const
Get maximum length of euler1 value.
size_t maxlenof_j1() const
Get maximum length of j1 value.
static const uint32_t ERROR_UNSPECIFIC
ERROR_UNSPECIFIC constant.
virtual Message * clone() const
Clone this message.
void set_error_code(const uint32_t new_error_code)
Set error_code value.
size_t maxlenof_params() const
Get maximum length of params value.
static const uint32_t ERROR_PLANNING
ERROR_PLANNING constant.
size_t maxlenof_finger1() const
Get maximum length of finger1 value.
size_t maxlenof_joints() const
Get maximum length of joints value.
size_t maxlenof_x() const
Get maximum length of x value.
char * params() const
Get params value.
float finger2() const
Get finger2 value.
size_t maxlenof_button() const
Get maximum length of button value.
void set_euler2(const float new_euler2)
Set euler2 value.
~RetractMessage()
Destructor.
float j2() const
Get j2 value.
void set_params(const char *new_params)
Set params value.
void set_finger2(const float new_finger2)
Set finger2 value.
virtual Message * clone() const
Clone this message.
float e3() const
Get e3 value.
size_t maxlenof_connected() const
Get maximum length of connected value.
void add_messageinfo(const char *name)
Add an entry to the message info list.
void set_button(const uint32_t new_button)
Set button value.
void set_j4(const float new_j4)
Set j4 value.
void set_initialized(const bool new_initialized)
Set initialized value.
void set_final(const bool new_final)
Set final value.
void set_msgid(const uint32_t new_msgid)
Set msgid value.
float euler1() const
Get euler1 value.
void set_j3(const float new_j3)
Set j3 value.
float finger2() const
Get finger2 value.
size_t maxlenof_msgid() const
Get maximum length of msgid value.
void set_finger3(const float new_finger3)
Set finger3 value.
void set_j6(const float new_j6)
Set j6 value.
void set_e3(const float new_e3)
Set e3 value.
CalibrateMessage()
Constructor.
float x() const
Get x value.
AngularGotoMessage()
Constructor.
virtual Message * clone() const
Clone this message.
~JoystickReleaseMessage()
Destructor.