24 #include <interfaces/KatanaInterface.h>
26 #include <core/exceptions/software.h>
91 KatanaInterface::KatanaInterface() : Interface()
93 data_size =
sizeof(KatanaInterface_data_t);
94 data_ptr = malloc(data_size);
95 data = (KatanaInterface_data_t *)data_ptr;
96 data_ts = (interface_data_ts_t *)data_ptr;
97 memset(data_ptr, 0, data_size);
98 add_fieldinfo(IFT_BYTE,
"sensor_value", 16, &data->sensor_value);
99 add_fieldinfo(IFT_FLOAT,
"x", 1, &data->x);
100 add_fieldinfo(IFT_FLOAT,
"y", 1, &data->y);
101 add_fieldinfo(IFT_FLOAT,
"z", 1, &data->z);
102 add_fieldinfo(IFT_FLOAT,
"phi", 1, &data->phi);
103 add_fieldinfo(IFT_FLOAT,
"theta", 1, &data->theta);
104 add_fieldinfo(IFT_FLOAT,
"psi", 1, &data->psi);
105 add_fieldinfo(IFT_INT32,
"encoders", 6, &data->encoders);
106 add_fieldinfo(IFT_FLOAT,
"angles", 6, &data->angles);
107 add_fieldinfo(IFT_UINT32,
"msgid", 1, &data->msgid);
108 add_fieldinfo(IFT_BOOL,
"final", 1, &data->final);
109 add_fieldinfo(IFT_UINT32,
"error_code", 1, &data->error_code);
110 add_fieldinfo(IFT_BOOL,
"enabled", 1, &data->enabled);
111 add_fieldinfo(IFT_BOOL,
"calibrated", 1, &data->calibrated);
112 add_fieldinfo(IFT_BYTE,
"max_velocity", 1, &data->max_velocity);
113 add_fieldinfo(IFT_BYTE,
"num_motors", 1, &data->num_motors);
114 add_messageinfo(
"StopMessage");
115 add_messageinfo(
"FlushMessage");
116 add_messageinfo(
"ParkMessage");
117 add_messageinfo(
"LinearGotoMessage");
118 add_messageinfo(
"LinearGotoKniMessage");
119 add_messageinfo(
"ObjectGotoMessage");
120 add_messageinfo(
"CalibrateMessage");
121 add_messageinfo(
"OpenGripperMessage");
122 add_messageinfo(
"CloseGripperMessage");
123 add_messageinfo(
"SetEnabledMessage");
124 add_messageinfo(
"SetMaxVelocityMessage");
125 add_messageinfo(
"SetPlannerParamsMessage");
126 add_messageinfo(
"SetMotorEncoderMessage");
127 add_messageinfo(
"MoveMotorEncoderMessage");
128 add_messageinfo(
"SetMotorAngleMessage");
129 add_messageinfo(
"MoveMotorAngleMessage");
130 unsigned char tmp_hash[] = {0x63, 0x62, 0xb0, 0x97, 0x9, 0x8f, 0x58, 0x40, 0x61, 0xdc, 0x9a, 0xcc, 0xa, 0x97, 0xf8, 0xcd};
135 KatanaInterface::~KatanaInterface()
146 KatanaInterface::sensor_value()
const
148 return data->sensor_value;
159 KatanaInterface::sensor_value(
unsigned int index)
const
162 throw Exception(
"Index value %u out of bounds (0..15)", index);
164 return data->sensor_value[index];
172 KatanaInterface::maxlenof_sensor_value()
const
183 KatanaInterface::set_sensor_value(
const uint8_t * new_sensor_value)
185 memcpy(data->sensor_value, new_sensor_value,
sizeof(uint8_t) * 16);
196 KatanaInterface::set_sensor_value(
unsigned int index,
const uint8_t new_sensor_value)
199 throw Exception(
"Index value %u out of bounds (0..15)", index);
201 data->sensor_value[index] = new_sensor_value;
210 KatanaInterface::x()
const
220 KatanaInterface::maxlenof_x()
const
231 KatanaInterface::set_x(
const float new_x)
243 KatanaInterface::y()
const
253 KatanaInterface::maxlenof_y()
const
264 KatanaInterface::set_y(
const float new_y)
276 KatanaInterface::z()
const
286 KatanaInterface::maxlenof_z()
const
297 KatanaInterface::set_z(
const float new_z)
308 KatanaInterface::phi()
const
318 KatanaInterface::maxlenof_phi()
const
328 KatanaInterface::set_phi(
const float new_phi)
339 KatanaInterface::theta()
const
349 KatanaInterface::maxlenof_theta()
const
359 KatanaInterface::set_theta(
const float new_theta)
361 data->theta = new_theta;
370 KatanaInterface::psi()
const
380 KatanaInterface::maxlenof_psi()
const
390 KatanaInterface::set_psi(
const float new_psi)
401 KatanaInterface::encoders()
const
403 return data->encoders;
413 KatanaInterface::encoders(
unsigned int index)
const
416 throw Exception(
"Index value %u out of bounds (0..5)", index);
418 return data->encoders[index];
426 KatanaInterface::maxlenof_encoders()
const
436 KatanaInterface::set_encoders(
const int32_t * new_encoders)
438 memcpy(data->encoders, new_encoders,
sizeof(int32_t) * 6);
448 KatanaInterface::set_encoders(
unsigned int index,
const int32_t new_encoders)
451 throw Exception(
"Index value %u out of bounds (0..5)", index);
453 data->encoders[index] = new_encoders;
461 KatanaInterface::angles()
const
473 KatanaInterface::angles(
unsigned int index)
const
476 throw Exception(
"Index value %u out of bounds (0..5)", index);
478 return data->angles[index];
486 KatanaInterface::maxlenof_angles()
const
496 KatanaInterface::set_angles(
const float * new_angles)
498 memcpy(data->angles, new_angles,
sizeof(
float) * 6);
508 KatanaInterface::set_angles(
unsigned int index,
const float new_angles)
511 throw Exception(
"Index value %u out of bounds (0..5)", index);
513 data->angles[index] = new_angles;
522 KatanaInterface::msgid()
const
532 KatanaInterface::maxlenof_msgid()
const
543 KatanaInterface::set_msgid(
const uint32_t new_msgid)
545 data->msgid = new_msgid;
555 KatanaInterface::is_final()
const
565 KatanaInterface::maxlenof_final()
const
576 KatanaInterface::set_final(
const bool new_final)
578 data->final = new_final;
589 KatanaInterface::error_code()
const
591 return data->error_code;
599 KatanaInterface::maxlenof_error_code()
const
611 KatanaInterface::set_error_code(
const uint32_t new_error_code)
613 data->error_code = new_error_code;
622 KatanaInterface::is_enabled()
const
624 return data->enabled;
632 KatanaInterface::maxlenof_enabled()
const
642 KatanaInterface::set_enabled(
const bool new_enabled)
644 data->enabled = new_enabled;
653 KatanaInterface::is_calibrated()
const
655 return data->calibrated;
663 KatanaInterface::maxlenof_calibrated()
const
673 KatanaInterface::set_calibrated(
const bool new_calibrated)
675 data->calibrated = new_calibrated;
684 KatanaInterface::max_velocity()
const
686 return data->max_velocity;
694 KatanaInterface::maxlenof_max_velocity()
const
704 KatanaInterface::set_max_velocity(
const uint8_t new_max_velocity)
706 data->max_velocity = new_max_velocity;
715 KatanaInterface::num_motors()
const
717 return data->num_motors;
725 KatanaInterface::maxlenof_num_motors()
const
735 KatanaInterface::set_num_motors(
const uint8_t new_num_motors)
737 data->num_motors = new_num_motors;
743 KatanaInterface::create_message(
const char *type)
const
745 if ( strncmp(
"StopMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
746 return new StopMessage();
747 }
else if ( strncmp(
"FlushMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
748 return new FlushMessage();
749 }
else if ( strncmp(
"ParkMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
750 return new ParkMessage();
751 }
else if ( strncmp(
"LinearGotoMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
752 return new LinearGotoMessage();
753 }
else if ( strncmp(
"LinearGotoKniMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
754 return new LinearGotoKniMessage();
755 }
else if ( strncmp(
"ObjectGotoMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
756 return new ObjectGotoMessage();
757 }
else if ( strncmp(
"CalibrateMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
758 return new CalibrateMessage();
759 }
else if ( strncmp(
"OpenGripperMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
760 return new OpenGripperMessage();
761 }
else if ( strncmp(
"CloseGripperMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
762 return new CloseGripperMessage();
763 }
else if ( strncmp(
"SetEnabledMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
764 return new SetEnabledMessage();
765 }
else if ( strncmp(
"SetMaxVelocityMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
766 return new SetMaxVelocityMessage();
767 }
else if ( strncmp(
"SetPlannerParamsMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
768 return new SetPlannerParamsMessage();
769 }
else if ( strncmp(
"SetMotorEncoderMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
770 return new SetMotorEncoderMessage();
771 }
else if ( strncmp(
"MoveMotorEncoderMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
772 return new MoveMotorEncoderMessage();
773 }
else if ( strncmp(
"SetMotorAngleMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
774 return new SetMotorAngleMessage();
775 }
else if ( strncmp(
"MoveMotorAngleMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
776 return new MoveMotorAngleMessage();
778 throw UnknownTypeException(
"The given type '%s' does not match any known "
779 "message type for this interface type.", type);
788 KatanaInterface::copy_values(
const Interface *other)
790 const KatanaInterface *oi = dynamic_cast<const KatanaInterface *>(other);
793 type(), other->type());
795 memcpy(data, oi->data,
sizeof(KatanaInterface_data_t));
799 KatanaInterface::enum_tostring(
const char *enumtype,
int val)
const
801 throw UnknownTypeException(
"Unknown enum type %s", enumtype);
813 KatanaInterface::StopMessage::StopMessage() :
Message(
"StopMessage")
818 data = (StopMessage_data_t *)
data_ptr;
836 data = (StopMessage_data_t *)
data_ptr;
864 data = (FlushMessage_data_t *)
data_ptr;
882 data = (FlushMessage_data_t *)
data_ptr;
910 data = (ParkMessage_data_t *)
data_ptr;
928 data = (ParkMessage_data_t *)
data_ptr;
963 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")
965 data_size =
sizeof(LinearGotoMessage_data_t);
968 data = (LinearGotoMessage_data_t *)
data_ptr;
970 data->theta_error = ini_theta_error;
971 data->offset_xy = ini_offset_xy;
972 data->straight = ini_straight;
973 strncpy(data->trans_frame, ini_trans_frame, 32-1);
974 data->trans_frame[32-1] = 0;
975 strncpy(data->rot_frame, ini_rot_frame, 32-1);
976 data->rot_frame[32-1] = 0;
981 data->theta = ini_theta;
998 data_size =
sizeof(LinearGotoMessage_data_t);
1001 data = (LinearGotoMessage_data_t *)
data_ptr;
1030 data = (LinearGotoMessage_data_t *)
data_ptr;
1043 return data->theta_error;
1064 data->theta_error = new_theta_error;
1074 return data->offset_xy;
1094 data->offset_xy = new_offset_xy;
1104 return data->straight;
1124 data->straight = new_straight;
1135 return data->trans_frame;
1156 strncpy(data->trans_frame, new_trans_frame,
sizeof(data->trans_frame)-1);
1157 data->trans_frame[
sizeof(data->trans_frame)-1] = 0;
1168 return data->rot_frame;
1189 strncpy(data->rot_frame, new_rot_frame,
sizeof(data->rot_frame)-1);
1190 data->rot_frame[
sizeof(data->rot_frame)-1] = 0;
1316 data->phi = new_phi;
1346 data->theta = new_theta;
1376 data->psi = new_psi;
1406 data_size =
sizeof(LinearGotoKniMessage_data_t);
1409 data = (LinearGotoKniMessage_data_t *)
data_ptr;
1414 data->phi = ini_phi;
1415 data->theta = ini_theta;
1416 data->psi = ini_psi;
1427 data_size =
sizeof(LinearGotoKniMessage_data_t);
1430 data = (LinearGotoKniMessage_data_t *)
data_ptr;
1454 data = (LinearGotoKniMessage_data_t *)
data_ptr;
1582 data->phi = new_phi;
1612 data->theta = new_theta;
1642 data->psi = new_psi;
1668 data_size =
sizeof(ObjectGotoMessage_data_t);
1671 data = (ObjectGotoMessage_data_t *)
data_ptr;
1673 strncpy(data->object, ini_object, 32-1);
1674 data->object[32-1] = 0;
1675 data->rot_x = ini_rot_x;
1682 data_size =
sizeof(ObjectGotoMessage_data_t);
1685 data = (ObjectGotoMessage_data_t *)
data_ptr;
1705 data = (ObjectGotoMessage_data_t *)
data_ptr;
1717 return data->object;
1737 strncpy(data->object, new_object,
sizeof(data->object)-1);
1738 data->object[
sizeof(data->object)-1] = 0;
1768 data->rot_x = new_rot_x;
1791 data_size =
sizeof(CalibrateMessage_data_t);
1794 data = (CalibrateMessage_data_t *)
data_ptr;
1812 data = (CalibrateMessage_data_t *)
data_ptr;
1837 data_size =
sizeof(OpenGripperMessage_data_t);
1840 data = (OpenGripperMessage_data_t *)
data_ptr;
1858 data = (OpenGripperMessage_data_t *)
data_ptr;
1883 data_size =
sizeof(CloseGripperMessage_data_t);
1886 data = (CloseGripperMessage_data_t *)
data_ptr;
1904 data = (CloseGripperMessage_data_t *)
data_ptr;
1931 data_size =
sizeof(SetEnabledMessage_data_t);
1934 data = (SetEnabledMessage_data_t *)
data_ptr;
1936 data->enabled = ini_enabled;
1942 data_size =
sizeof(SetEnabledMessage_data_t);
1945 data = (SetEnabledMessage_data_t *)
data_ptr;
1964 data = (SetEnabledMessage_data_t *)
data_ptr;
1976 return data->enabled;
1996 data->enabled = new_enabled;
2021 data_size =
sizeof(SetMaxVelocityMessage_data_t);
2024 data = (SetMaxVelocityMessage_data_t *)
data_ptr;
2026 data->max_velocity = ini_max_velocity;
2032 data_size =
sizeof(SetMaxVelocityMessage_data_t);
2035 data = (SetMaxVelocityMessage_data_t *)
data_ptr;
2054 data = (SetMaxVelocityMessage_data_t *)
data_ptr;
2066 return data->max_velocity;
2086 data->max_velocity = new_max_velocity;
2112 data_size =
sizeof(SetPlannerParamsMessage_data_t);
2115 data = (SetPlannerParamsMessage_data_t *)
data_ptr;
2117 strncpy(data->plannerparams, ini_plannerparams, 1024-1);
2118 data->plannerparams[1024-1] = 0;
2119 data->straight = ini_straight;
2126 data_size =
sizeof(SetPlannerParamsMessage_data_t);
2129 data = (SetPlannerParamsMessage_data_t *)
data_ptr;
2149 data = (SetPlannerParamsMessage_data_t *)
data_ptr;
2161 return data->plannerparams;
2181 strncpy(data->plannerparams, new_plannerparams,
sizeof(data->plannerparams)-1);
2182 data->plannerparams[
sizeof(data->plannerparams)-1] = 0;
2192 return data->straight;
2212 data->straight = new_straight;
2238 data_size =
sizeof(SetMotorEncoderMessage_data_t);
2241 data = (SetMotorEncoderMessage_data_t *)
data_ptr;
2244 data->enc = ini_enc;
2251 data_size =
sizeof(SetMotorEncoderMessage_data_t);
2254 data = (SetMotorEncoderMessage_data_t *)
data_ptr;
2274 data = (SetMotorEncoderMessage_data_t *)
data_ptr;
2336 data->enc = new_enc;
2362 data_size =
sizeof(MoveMotorEncoderMessage_data_t);
2365 data = (MoveMotorEncoderMessage_data_t *)
data_ptr;
2368 data->enc = ini_enc;
2375 data_size =
sizeof(MoveMotorEncoderMessage_data_t);
2378 data = (MoveMotorEncoderMessage_data_t *)
data_ptr;
2398 data = (MoveMotorEncoderMessage_data_t *)
data_ptr;
2460 data->enc = new_enc;
2486 data_size =
sizeof(SetMotorAngleMessage_data_t);
2489 data = (SetMotorAngleMessage_data_t *)
data_ptr;
2492 data->angle = ini_angle;
2499 data_size =
sizeof(SetMotorAngleMessage_data_t);
2502 data = (SetMotorAngleMessage_data_t *)
data_ptr;
2522 data = (SetMotorAngleMessage_data_t *)
data_ptr;
2584 data->angle = new_angle;
2610 data_size =
sizeof(MoveMotorAngleMessage_data_t);
2613 data = (MoveMotorAngleMessage_data_t *)
data_ptr;
2616 data->angle = ini_angle;
2623 data_size =
sizeof(MoveMotorAngleMessage_data_t);
2626 data = (MoveMotorAngleMessage_data_t *)
data_ptr;
2646 data = (MoveMotorAngleMessage_data_t *)
data_ptr;
2708 data->angle = new_angle;
2728 const StopMessage *m0 = dynamic_cast<const StopMessage *>(message);
2732 const FlushMessage *m1 = dynamic_cast<const FlushMessage *>(message);
2736 const ParkMessage *m2 = dynamic_cast<const ParkMessage *>(message);
2740 const LinearGotoMessage *m3 = dynamic_cast<const LinearGotoMessage *>(message);
2744 const LinearGotoKniMessage *m4 = dynamic_cast<const LinearGotoKniMessage *>(message);
2748 const ObjectGotoMessage *m5 = dynamic_cast<const ObjectGotoMessage *>(message);
2752 const CalibrateMessage *m6 = dynamic_cast<const CalibrateMessage *>(message);
2756 const OpenGripperMessage *m7 = dynamic_cast<const OpenGripperMessage *>(message);
2760 const CloseGripperMessage *m8 = dynamic_cast<const CloseGripperMessage *>(message);
2764 const SetEnabledMessage *m9 = dynamic_cast<const SetEnabledMessage *>(message);
2768 const SetMaxVelocityMessage *m10 = dynamic_cast<const SetMaxVelocityMessage *>(message);
2769 if ( m10 != NULL ) {
2772 const SetPlannerParamsMessage *m11 = dynamic_cast<const SetPlannerParamsMessage *>(message);
2773 if ( m11 != NULL ) {
2776 const SetMotorEncoderMessage *m12 = dynamic_cast<const SetMotorEncoderMessage *>(message);
2777 if ( m12 != NULL ) {
2780 const MoveMotorEncoderMessage *m13 = dynamic_cast<const MoveMotorEncoderMessage *>(message);
2781 if ( m13 != NULL ) {
2784 const SetMotorAngleMessage *m14 = dynamic_cast<const SetMotorAngleMessage *>(message);
2785 if ( m14 != NULL ) {
2788 const MoveMotorAngleMessage *m15 = dynamic_cast<const MoveMotorAngleMessage *>(message);
2789 if ( m15 != NULL ) {
2796 EXPORT_INTERFACE(KatanaInterface)