24 #include <interfaces/KatanaInterface.h>
26 #include <core/exceptions/software.h>
89 KatanaInterface::KatanaInterface() : Interface()
91 data_size =
sizeof(KatanaInterface_data_t);
92 data_ptr = malloc(data_size);
93 data = (KatanaInterface_data_t *)data_ptr;
94 data_ts = (interface_data_ts_t *)data_ptr;
95 memset(data_ptr, 0, data_size);
96 add_fieldinfo(IFT_BYTE,
"sensor_value", 16, &data->sensor_value);
97 add_fieldinfo(IFT_FLOAT,
"x", 1, &data->x);
98 add_fieldinfo(IFT_FLOAT,
"y", 1, &data->y);
99 add_fieldinfo(IFT_FLOAT,
"z", 1, &data->z);
100 add_fieldinfo(IFT_FLOAT,
"phi", 1, &data->phi);
101 add_fieldinfo(IFT_FLOAT,
"theta", 1, &data->theta);
102 add_fieldinfo(IFT_FLOAT,
"psi", 1, &data->psi);
103 add_fieldinfo(IFT_INT32,
"encoders", 6, &data->encoders);
104 add_fieldinfo(IFT_FLOAT,
"angles", 6, &data->angles);
105 add_fieldinfo(IFT_UINT32,
"msgid", 1, &data->msgid);
106 add_fieldinfo(IFT_BOOL,
"final", 1, &data->final);
107 add_fieldinfo(IFT_UINT32,
"error_code", 1, &data->error_code);
108 add_fieldinfo(IFT_BOOL,
"enabled", 1, &data->enabled);
109 add_fieldinfo(IFT_BOOL,
"calibrated", 1, &data->calibrated);
110 add_fieldinfo(IFT_BYTE,
"max_velocity", 1, &data->max_velocity);
111 add_fieldinfo(IFT_BYTE,
"num_motors", 1, &data->num_motors);
112 add_messageinfo(
"StopMessage");
113 add_messageinfo(
"FlushMessage");
114 add_messageinfo(
"ParkMessage");
115 add_messageinfo(
"LinearGotoMessage");
116 add_messageinfo(
"LinearGotoKniMessage");
117 add_messageinfo(
"ObjectGotoMessage");
118 add_messageinfo(
"CalibrateMessage");
119 add_messageinfo(
"OpenGripperMessage");
120 add_messageinfo(
"CloseGripperMessage");
121 add_messageinfo(
"SetEnabledMessage");
122 add_messageinfo(
"SetMaxVelocityMessage");
123 add_messageinfo(
"SetPlannerParamsMessage");
124 add_messageinfo(
"SetMotorEncoderMessage");
125 add_messageinfo(
"MoveMotorEncoderMessage");
126 add_messageinfo(
"SetMotorAngleMessage");
127 add_messageinfo(
"MoveMotorAngleMessage");
128 unsigned char tmp_hash[] = {0x63, 0x62, 0xb0, 0x97, 0x9, 0x8f, 0x58, 0x40, 0x61, 0xdc, 0x9a, 0xcc, 0xa, 0x97, 0xf8, 0xcd};
133 KatanaInterface::~KatanaInterface()
144 KatanaInterface::sensor_value()
const
146 return data->sensor_value;
157 KatanaInterface::sensor_value(
unsigned int index)
const
160 throw Exception(
"Index value %u out of bounds (0..16)", index);
162 return data->sensor_value[index];
170 KatanaInterface::maxlenof_sensor_value()
const
181 KatanaInterface::set_sensor_value(
const uint8_t * new_sensor_value)
183 memcpy(data->sensor_value, new_sensor_value,
sizeof(uint8_t) * 16);
194 KatanaInterface::set_sensor_value(
unsigned int index,
const uint8_t new_sensor_value)
197 throw Exception(
"Index value %u out of bounds (0..16)", index);
199 data->sensor_value[index] = new_sensor_value;
208 KatanaInterface::x()
const
218 KatanaInterface::maxlenof_x()
const
229 KatanaInterface::set_x(
const float new_x)
241 KatanaInterface::y()
const
251 KatanaInterface::maxlenof_y()
const
262 KatanaInterface::set_y(
const float new_y)
274 KatanaInterface::z()
const
284 KatanaInterface::maxlenof_z()
const
295 KatanaInterface::set_z(
const float new_z)
306 KatanaInterface::phi()
const
316 KatanaInterface::maxlenof_phi()
const
326 KatanaInterface::set_phi(
const float new_phi)
337 KatanaInterface::theta()
const
347 KatanaInterface::maxlenof_theta()
const
357 KatanaInterface::set_theta(
const float new_theta)
359 data->theta = new_theta;
368 KatanaInterface::psi()
const
378 KatanaInterface::maxlenof_psi()
const
388 KatanaInterface::set_psi(
const float new_psi)
399 KatanaInterface::encoders()
const
401 return data->encoders;
411 KatanaInterface::encoders(
unsigned int index)
const
414 throw Exception(
"Index value %u out of bounds (0..6)", index);
416 return data->encoders[index];
424 KatanaInterface::maxlenof_encoders()
const
434 KatanaInterface::set_encoders(
const int32_t * new_encoders)
436 memcpy(data->encoders, new_encoders,
sizeof(int32_t) * 6);
446 KatanaInterface::set_encoders(
unsigned int index,
const int32_t new_encoders)
449 throw Exception(
"Index value %u out of bounds (0..6)", index);
451 data->encoders[index] = new_encoders;
459 KatanaInterface::angles()
const
471 KatanaInterface::angles(
unsigned int index)
const
474 throw Exception(
"Index value %u out of bounds (0..6)", index);
476 return data->angles[index];
484 KatanaInterface::maxlenof_angles()
const
494 KatanaInterface::set_angles(
const float * new_angles)
496 memcpy(data->angles, new_angles,
sizeof(
float) * 6);
506 KatanaInterface::set_angles(
unsigned int index,
const float new_angles)
509 throw Exception(
"Index value %u out of bounds (0..6)", index);
511 data->angles[index] = new_angles;
520 KatanaInterface::msgid()
const
530 KatanaInterface::maxlenof_msgid()
const
541 KatanaInterface::set_msgid(
const uint32_t new_msgid)
543 data->msgid = new_msgid;
553 KatanaInterface::is_final()
const
563 KatanaInterface::maxlenof_final()
const
574 KatanaInterface::set_final(
const bool new_final)
576 data->final = new_final;
587 KatanaInterface::error_code()
const
589 return data->error_code;
597 KatanaInterface::maxlenof_error_code()
const
609 KatanaInterface::set_error_code(
const uint32_t new_error_code)
611 data->error_code = new_error_code;
620 KatanaInterface::is_enabled()
const
622 return data->enabled;
630 KatanaInterface::maxlenof_enabled()
const
640 KatanaInterface::set_enabled(
const bool new_enabled)
642 data->enabled = new_enabled;
651 KatanaInterface::is_calibrated()
const
653 return data->calibrated;
661 KatanaInterface::maxlenof_calibrated()
const
671 KatanaInterface::set_calibrated(
const bool new_calibrated)
673 data->calibrated = new_calibrated;
682 KatanaInterface::max_velocity()
const
684 return data->max_velocity;
692 KatanaInterface::maxlenof_max_velocity()
const
702 KatanaInterface::set_max_velocity(
const uint8_t new_max_velocity)
704 data->max_velocity = new_max_velocity;
713 KatanaInterface::num_motors()
const
715 return data->num_motors;
723 KatanaInterface::maxlenof_num_motors()
const
733 KatanaInterface::set_num_motors(
const uint8_t new_num_motors)
735 data->num_motors = new_num_motors;
741 KatanaInterface::create_message(
const char *type)
const
743 if ( strncmp(
"StopMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
745 }
else if ( strncmp(
"FlushMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
747 }
else if ( strncmp(
"ParkMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
749 }
else if ( strncmp(
"LinearGotoMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
751 }
else if ( strncmp(
"LinearGotoKniMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
753 }
else if ( strncmp(
"ObjectGotoMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
755 }
else if ( strncmp(
"CalibrateMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
757 }
else if ( strncmp(
"OpenGripperMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
759 }
else if ( strncmp(
"CloseGripperMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
761 }
else if ( strncmp(
"SetEnabledMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
763 }
else if ( strncmp(
"SetMaxVelocityMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
765 }
else if ( strncmp(
"SetPlannerParamsMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
767 }
else if ( strncmp(
"SetMotorEncoderMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
769 }
else if ( strncmp(
"MoveMotorEncoderMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
771 }
else if ( strncmp(
"SetMotorAngleMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
773 }
else if ( strncmp(
"MoveMotorAngleMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
777 "message type for this interface type.", type);
791 type(), other->
type());
793 memcpy(data, oi->data,
sizeof(KatanaInterface_data_t));
797 KatanaInterface::enum_tostring(
const char *enumtype,
int val)
const
811 KatanaInterface::StopMessage::StopMessage() :
Message(
"StopMessage")
816 data = (StopMessage_data_t *)
data_ptr;
834 data = (StopMessage_data_t *)
data_ptr;
862 data = (FlushMessage_data_t *)
data_ptr;
880 data = (FlushMessage_data_t *)
data_ptr;
908 data = (ParkMessage_data_t *)
data_ptr;
926 data = (ParkMessage_data_t *)
data_ptr;
961 KatanaInterface::LinearGotoMessage::LinearGotoMessage(
const float ini_theta_error,
const float ini_offset_xy,
const bool ini_straight,
const char * ini_trans_frame,
const char * ini_rot_frame,
const float ini_x,
const float ini_y,
const float ini_z,
const float ini_phi,
const float ini_theta,
const float ini_psi) :
Message(
"LinearGotoMessage")
963 data_size =
sizeof(LinearGotoMessage_data_t);
966 data = (LinearGotoMessage_data_t *)
data_ptr;
968 data->theta_error = ini_theta_error;
969 data->offset_xy = ini_offset_xy;
970 data->straight = ini_straight;
971 strncpy(data->trans_frame, ini_trans_frame, 32);
972 strncpy(data->rot_frame, ini_rot_frame, 32);
977 data->theta = ini_theta;
994 data_size =
sizeof(LinearGotoMessage_data_t);
997 data = (LinearGotoMessage_data_t *)
data_ptr;
1026 data = (LinearGotoMessage_data_t *)
data_ptr;
1039 return data->theta_error;
1060 data->theta_error = new_theta_error;
1070 return data->offset_xy;
1090 data->offset_xy = new_offset_xy;
1100 return data->straight;
1120 data->straight = new_straight;
1131 return data->trans_frame;
1152 strncpy(data->trans_frame, new_trans_frame,
sizeof(data->trans_frame));
1163 return data->rot_frame;
1184 strncpy(data->rot_frame, new_rot_frame,
sizeof(data->rot_frame));
1310 data->phi = new_phi;
1340 data->theta = new_theta;
1370 data->psi = new_psi;
1400 data_size =
sizeof(LinearGotoKniMessage_data_t);
1403 data = (LinearGotoKniMessage_data_t *)
data_ptr;
1408 data->phi = ini_phi;
1409 data->theta = ini_theta;
1410 data->psi = ini_psi;
1421 data_size =
sizeof(LinearGotoKniMessage_data_t);
1424 data = (LinearGotoKniMessage_data_t *)
data_ptr;
1448 data = (LinearGotoKniMessage_data_t *)
data_ptr;
1576 data->phi = new_phi;
1606 data->theta = new_theta;
1636 data->psi = new_psi;
1662 data_size =
sizeof(ObjectGotoMessage_data_t);
1665 data = (ObjectGotoMessage_data_t *)
data_ptr;
1667 strncpy(data->object, ini_object, 32);
1668 data->rot_x = ini_rot_x;
1675 data_size =
sizeof(ObjectGotoMessage_data_t);
1678 data = (ObjectGotoMessage_data_t *)
data_ptr;
1698 data = (ObjectGotoMessage_data_t *)
data_ptr;
1710 return data->object;
1730 strncpy(data->object, new_object,
sizeof(data->object));
1760 data->rot_x = new_rot_x;
1783 data_size =
sizeof(CalibrateMessage_data_t);
1786 data = (CalibrateMessage_data_t *)
data_ptr;
1804 data = (CalibrateMessage_data_t *)
data_ptr;
1829 data_size =
sizeof(OpenGripperMessage_data_t);
1832 data = (OpenGripperMessage_data_t *)
data_ptr;
1850 data = (OpenGripperMessage_data_t *)
data_ptr;
1875 data_size =
sizeof(CloseGripperMessage_data_t);
1878 data = (CloseGripperMessage_data_t *)
data_ptr;
1896 data = (CloseGripperMessage_data_t *)
data_ptr;
1923 data_size =
sizeof(SetEnabledMessage_data_t);
1926 data = (SetEnabledMessage_data_t *)
data_ptr;
1928 data->enabled = ini_enabled;
1934 data_size =
sizeof(SetEnabledMessage_data_t);
1937 data = (SetEnabledMessage_data_t *)
data_ptr;
1956 data = (SetEnabledMessage_data_t *)
data_ptr;
1968 return data->enabled;
1988 data->enabled = new_enabled;
2013 data_size =
sizeof(SetMaxVelocityMessage_data_t);
2016 data = (SetMaxVelocityMessage_data_t *)
data_ptr;
2018 data->max_velocity = ini_max_velocity;
2024 data_size =
sizeof(SetMaxVelocityMessage_data_t);
2027 data = (SetMaxVelocityMessage_data_t *)
data_ptr;
2046 data = (SetMaxVelocityMessage_data_t *)
data_ptr;
2058 return data->max_velocity;
2078 data->max_velocity = new_max_velocity;
2104 data_size =
sizeof(SetPlannerParamsMessage_data_t);
2107 data = (SetPlannerParamsMessage_data_t *)
data_ptr;
2109 strncpy(data->plannerparams, ini_plannerparams, 1024);
2110 data->straight = ini_straight;
2117 data_size =
sizeof(SetPlannerParamsMessage_data_t);
2120 data = (SetPlannerParamsMessage_data_t *)
data_ptr;
2140 data = (SetPlannerParamsMessage_data_t *)
data_ptr;
2152 return data->plannerparams;
2172 strncpy(data->plannerparams, new_plannerparams,
sizeof(data->plannerparams));
2182 return data->straight;
2202 data->straight = new_straight;
2228 data_size =
sizeof(SetMotorEncoderMessage_data_t);
2231 data = (SetMotorEncoderMessage_data_t *)
data_ptr;
2234 data->enc = ini_enc;
2241 data_size =
sizeof(SetMotorEncoderMessage_data_t);
2244 data = (SetMotorEncoderMessage_data_t *)
data_ptr;
2264 data = (SetMotorEncoderMessage_data_t *)
data_ptr;
2326 data->enc = new_enc;
2352 data_size =
sizeof(MoveMotorEncoderMessage_data_t);
2355 data = (MoveMotorEncoderMessage_data_t *)
data_ptr;
2358 data->enc = ini_enc;
2365 data_size =
sizeof(MoveMotorEncoderMessage_data_t);
2368 data = (MoveMotorEncoderMessage_data_t *)
data_ptr;
2388 data = (MoveMotorEncoderMessage_data_t *)
data_ptr;
2450 data->enc = new_enc;
2476 data_size =
sizeof(SetMotorAngleMessage_data_t);
2479 data = (SetMotorAngleMessage_data_t *)
data_ptr;
2482 data->angle = ini_angle;
2489 data_size =
sizeof(SetMotorAngleMessage_data_t);
2492 data = (SetMotorAngleMessage_data_t *)
data_ptr;
2512 data = (SetMotorAngleMessage_data_t *)
data_ptr;
2574 data->angle = new_angle;
2600 data_size =
sizeof(MoveMotorAngleMessage_data_t);
2603 data = (MoveMotorAngleMessage_data_t *)
data_ptr;
2606 data->angle = ini_angle;
2613 data_size =
sizeof(MoveMotorAngleMessage_data_t);
2616 data = (MoveMotorAngleMessage_data_t *)
data_ptr;
2636 data = (MoveMotorAngleMessage_data_t *)
data_ptr;
2698 data->angle = new_angle;
2759 if ( m10 != NULL ) {
2763 if ( m11 != NULL ) {
2767 if ( m12 != NULL ) {
2771 if ( m13 != NULL ) {
2775 if ( m14 != NULL ) {
2779 if ( m15 != NULL ) {