24 #include <interfaces/JacoBimanualInterface.h>
26 #include <core/exceptions/software.h>
54 JacoBimanualInterface::JacoBimanualInterface() : Interface()
56 data_size =
sizeof(JacoBimanualInterface_data_t);
57 data_ptr = malloc(data_size);
58 data = (JacoBimanualInterface_data_t *)data_ptr;
59 data_ts = (interface_data_ts_t *)data_ptr;
60 memset(data_ptr, 0, data_size);
61 add_fieldinfo(IFT_UINT32,
"msgid", 1, &data->msgid);
62 add_fieldinfo(IFT_BOOL,
"final", 1, &data->final);
63 add_fieldinfo(IFT_UINT32,
"error_code", 1, &data->error_code);
64 add_fieldinfo(IFT_BOOL,
"constrained", 1, &data->constrained);
65 add_messageinfo(
"CartesianGotoMessage");
66 add_messageinfo(
"MoveGripperMessage");
67 add_messageinfo(
"SetPlannerParamsMessage");
68 add_messageinfo(
"SetConstrainedMessage");
69 unsigned char tmp_hash[] = {0x7c, 0x62, 0x7a, 0x5a, 0xc6, 0xc1, 0xb4, 0x12, 0x6f, 0xa4, 0x89, 0x89, 0xb8, 0xe5, 0x1, 0x66};
74 JacoBimanualInterface::~JacoBimanualInterface()
85 JacoBimanualInterface::msgid()
const
95 JacoBimanualInterface::maxlenof_msgid()
const
106 JacoBimanualInterface::set_msgid(
const uint32_t new_msgid)
108 data->msgid = new_msgid;
118 JacoBimanualInterface::is_final()
const
128 JacoBimanualInterface::maxlenof_final()
const
139 JacoBimanualInterface::set_final(
const bool new_final)
141 data->final = new_final;
152 JacoBimanualInterface::error_code()
const
154 return data->error_code;
162 JacoBimanualInterface::maxlenof_error_code()
const
174 JacoBimanualInterface::set_error_code(
const uint32_t new_error_code)
176 data->error_code = new_error_code;
186 JacoBimanualInterface::is_constrained()
const
188 return data->constrained;
196 JacoBimanualInterface::maxlenof_constrained()
const
207 JacoBimanualInterface::set_constrained(
const bool new_constrained)
209 data->constrained = new_constrained;
215 JacoBimanualInterface::create_message(
const char *type)
const
217 if ( strncmp(
"CartesianGotoMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
219 }
else if ( strncmp(
"MoveGripperMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
221 }
else if ( strncmp(
"SetPlannerParamsMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
222 return new SetPlannerParamsMessage();
223 }
else if ( strncmp(
"SetConstrainedMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
224 return new SetConstrainedMessage();
226 throw UnknownTypeException(
"The given type '%s' does not match any known "
227 "message type for this interface type.", type);
236 JacoBimanualInterface::copy_values(
const Interface *other)
238 const JacoBimanualInterface *oi =
dynamic_cast<const JacoBimanualInterface *
>(other);
240 throw TypeMismatchException(
"Can only copy values from interface of same type (%s vs. %s)",
241 type(), other->type());
243 memcpy(data, oi->data,
sizeof(JacoBimanualInterface_data_t));
247 JacoBimanualInterface::enum_tostring(
const char *enumtype,
int val)
const
249 throw UnknownTypeException(
"Unknown enum type %s", enumtype);
274 JacoBimanualInterface::CartesianGotoMessage::CartesianGotoMessage(
const float ini_l_x,
const float ini_l_y,
const float ini_l_z,
const float ini_l_e1,
const float ini_l_e2,
const float ini_l_e3,
const float ini_r_x,
const float ini_r_y,
const float ini_r_z,
const float ini_r_e1,
const float ini_r_e2,
const float ini_r_e3) :
Message(
"CartesianGotoMessage")
276 data_size =
sizeof(CartesianGotoMessage_data_t);
279 data = (CartesianGotoMessage_data_t *)
data_ptr;
284 data->l_e1 = ini_l_e1;
285 data->l_e2 = ini_l_e2;
286 data->l_e3 = ini_l_e3;
290 data->r_e1 = ini_r_e1;
291 data->r_e2 = ini_r_e2;
292 data->r_e3 = ini_r_e3;
309 data_size =
sizeof(CartesianGotoMessage_data_t);
312 data = (CartesianGotoMessage_data_t *)
data_ptr;
342 data = (CartesianGotoMessage_data_t *)
data_ptr;
464 data->l_e1 = new_l_e1;
494 data->l_e2 = new_l_e2;
524 data->l_e3 = new_l_e3;
644 data->r_e1 = new_r_e1;
674 data->r_e2 = new_r_e2;
704 data->r_e3 = new_r_e3;
734 data_size =
sizeof(MoveGripperMessage_data_t);
737 data = (MoveGripperMessage_data_t *)
data_ptr;
739 data->l_finger1 = ini_l_finger1;
740 data->l_finger2 = ini_l_finger2;
741 data->l_finger3 = ini_l_finger3;
742 data->r_finger1 = ini_r_finger1;
743 data->r_finger2 = ini_r_finger2;
744 data->r_finger3 = ini_r_finger3;
755 data_size =
sizeof(MoveGripperMessage_data_t);
758 data = (MoveGripperMessage_data_t *)
data_ptr;
782 data = (MoveGripperMessage_data_t *)
data_ptr;
794 return data->l_finger1;
814 data->l_finger1 = new_l_finger1;
824 return data->l_finger2;
844 data->l_finger2 = new_l_finger2;
854 return data->l_finger3;
874 data->l_finger3 = new_l_finger3;
884 return data->r_finger1;
904 data->r_finger1 = new_r_finger1;
914 return data->r_finger2;
934 data->r_finger2 = new_r_finger2;
944 return data->r_finger3;
964 data->r_finger3 = new_r_finger3;
989 data_size =
sizeof(SetPlannerParamsMessage_data_t);
992 data = (SetPlannerParamsMessage_data_t *)
data_ptr;
994 strncpy(data->params, ini_params, 1024-1);
995 data->params[1024-1] = 0;
1001 data_size =
sizeof(SetPlannerParamsMessage_data_t);
1004 data = (SetPlannerParamsMessage_data_t *)
data_ptr;
1023 data = (SetPlannerParamsMessage_data_t *)
data_ptr;
1035 return data->params;
1055 strncpy(data->params, new_params,
sizeof(data->params)-1);
1056 data->params[
sizeof(data->params)-1] = 0;
1081 data_size =
sizeof(SetConstrainedMessage_data_t);
1084 data = (SetConstrainedMessage_data_t *)
data_ptr;
1086 data->constrained = ini_constrained;
1092 data_size =
sizeof(SetConstrainedMessage_data_t);
1095 data = (SetConstrainedMessage_data_t *)
data_ptr;
1114 data = (SetConstrainedMessage_data_t *)
data_ptr;
1127 return data->constrained;
1148 data->constrained = new_constrained;
1176 const SetPlannerParamsMessage *m2 =
dynamic_cast<const SetPlannerParamsMessage *
>(message);
1180 const SetConstrainedMessage *m3 =
dynamic_cast<const SetConstrainedMessage *
>(message);
1188 EXPORT_INTERFACE(JacoBimanualInterface)