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 data_changed |=
change_field(data->sensor_value, new_sensor_value);
195 KatanaInterface::set_sensor_value(
unsigned int index,
const uint8_t new_sensor_value)
197 data_changed |=
change_field(data->sensor_value, index, new_sensor_value);
205 KatanaInterface::x()
const
215 KatanaInterface::maxlenof_x()
const
226 KatanaInterface::set_x(
const float new_x)
237 KatanaInterface::y()
const
247 KatanaInterface::maxlenof_y()
const
258 KatanaInterface::set_y(
const float new_y)
269 KatanaInterface::z()
const
279 KatanaInterface::maxlenof_z()
const
290 KatanaInterface::set_z(
const float new_z)
300 KatanaInterface::phi()
const
310 KatanaInterface::maxlenof_phi()
const
320 KatanaInterface::set_phi(
const float new_phi)
330 KatanaInterface::theta()
const
340 KatanaInterface::maxlenof_theta()
const
350 KatanaInterface::set_theta(
const float new_theta)
360 KatanaInterface::psi()
const
370 KatanaInterface::maxlenof_psi()
const
380 KatanaInterface::set_psi(
const float new_psi)
390 KatanaInterface::encoders()
const
392 return data->encoders;
402 KatanaInterface::encoders(
unsigned int index)
const
405 throw Exception(
"Index value %u out of bounds (0..5)", index);
407 return data->encoders[index];
415 KatanaInterface::maxlenof_encoders()
const
425 KatanaInterface::set_encoders(
const int32_t * new_encoders)
427 data_changed |=
change_field(data->encoders, new_encoders);
436 KatanaInterface::set_encoders(
unsigned int index,
const int32_t new_encoders)
438 data_changed |=
change_field(data->encoders, index, new_encoders);
445 KatanaInterface::angles()
const
457 KatanaInterface::angles(
unsigned int index)
const
460 throw Exception(
"Index value %u out of bounds (0..5)", index);
462 return data->angles[index];
470 KatanaInterface::maxlenof_angles()
const
480 KatanaInterface::set_angles(
const float * new_angles)
491 KatanaInterface::set_angles(
unsigned int index,
const float new_angles)
493 data_changed |=
change_field(data->angles, index, new_angles);
501 KatanaInterface::msgid()
const
511 KatanaInterface::maxlenof_msgid()
const
522 KatanaInterface::set_msgid(
const uint32_t new_msgid)
533 KatanaInterface::is_final()
const
543 KatanaInterface::maxlenof_final()
const
554 KatanaInterface::set_final(
const bool new_final)
566 KatanaInterface::error_code()
const
568 return data->error_code;
576 KatanaInterface::maxlenof_error_code()
const
588 KatanaInterface::set_error_code(
const uint32_t new_error_code)
590 data_changed |=
change_field(data->error_code, new_error_code);
598 KatanaInterface::is_enabled()
const
600 return data->enabled;
608 KatanaInterface::maxlenof_enabled()
const
618 KatanaInterface::set_enabled(
const bool new_enabled)
620 data_changed |=
change_field(data->enabled, new_enabled);
628 KatanaInterface::is_calibrated()
const
630 return data->calibrated;
638 KatanaInterface::maxlenof_calibrated()
const
648 KatanaInterface::set_calibrated(
const bool new_calibrated)
650 data_changed |=
change_field(data->calibrated, new_calibrated);
658 KatanaInterface::max_velocity()
const
660 return data->max_velocity;
668 KatanaInterface::maxlenof_max_velocity()
const
678 KatanaInterface::set_max_velocity(
const uint8_t new_max_velocity)
680 data_changed |=
change_field(data->max_velocity, new_max_velocity);
688 KatanaInterface::num_motors()
const
690 return data->num_motors;
698 KatanaInterface::maxlenof_num_motors()
const
708 KatanaInterface::set_num_motors(
const uint8_t new_num_motors)
710 data_changed |=
change_field(data->num_motors, new_num_motors);
715 KatanaInterface::create_message(
const char *type)
const
717 if ( strncmp(
"StopMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
719 }
else if ( strncmp(
"FlushMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
721 }
else if ( strncmp(
"ParkMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
723 }
else if ( strncmp(
"LinearGotoMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
725 }
else if ( strncmp(
"LinearGotoKniMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
727 }
else if ( strncmp(
"ObjectGotoMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
729 }
else if ( strncmp(
"CalibrateMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
731 }
else if ( strncmp(
"OpenGripperMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
733 }
else if ( strncmp(
"CloseGripperMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
735 }
else if ( strncmp(
"SetEnabledMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
737 }
else if ( strncmp(
"SetMaxVelocityMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
739 }
else if ( strncmp(
"SetPlannerParamsMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
741 }
else if ( strncmp(
"SetMotorEncoderMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
743 }
else if ( strncmp(
"MoveMotorEncoderMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
745 }
else if ( strncmp(
"SetMotorAngleMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
747 }
else if ( strncmp(
"MoveMotorAngleMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
751 "message type for this interface type.", type);
765 type(), other->
type());
767 memcpy(data, oi->data,
sizeof(KatanaInterface_data_t));
771 KatanaInterface::enum_tostring(
const char *enumtype,
int val)
const
785 KatanaInterface::StopMessage::StopMessage() :
Message(
"StopMessage")
790 data = (StopMessage_data_t *)
data_ptr;
808 data = (StopMessage_data_t *)
data_ptr;
836 data = (FlushMessage_data_t *)
data_ptr;
854 data = (FlushMessage_data_t *)
data_ptr;
882 data = (ParkMessage_data_t *)
data_ptr;
900 data = (ParkMessage_data_t *)
data_ptr;
935 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")
937 data_size =
sizeof(LinearGotoMessage_data_t);
940 data = (LinearGotoMessage_data_t *)
data_ptr;
942 data->theta_error = ini_theta_error;
943 data->offset_xy = ini_offset_xy;
944 data->straight = ini_straight;
945 strncpy(data->trans_frame, ini_trans_frame, 32-1);
946 data->trans_frame[32-1] = 0;
947 strncpy(data->rot_frame, ini_rot_frame, 32-1);
948 data->rot_frame[32-1] = 0;
953 data->theta = ini_theta;
970 data_size =
sizeof(LinearGotoMessage_data_t);
973 data = (LinearGotoMessage_data_t *)
data_ptr;
1002 data = (LinearGotoMessage_data_t *)
data_ptr;
1015 return data->theta_error;
1046 return data->offset_xy;
1076 return data->straight;
1107 return data->trans_frame;
1139 return data->rot_frame;
1376 data_size =
sizeof(LinearGotoKniMessage_data_t);
1379 data = (LinearGotoKniMessage_data_t *)
data_ptr;
1384 data->phi = ini_phi;
1385 data->theta = ini_theta;
1386 data->psi = ini_psi;
1397 data_size =
sizeof(LinearGotoKniMessage_data_t);
1400 data = (LinearGotoKniMessage_data_t *)
data_ptr;
1424 data = (LinearGotoKniMessage_data_t *)
data_ptr;
1638 data_size =
sizeof(ObjectGotoMessage_data_t);
1641 data = (ObjectGotoMessage_data_t *)
data_ptr;
1643 strncpy(data->object, ini_object, 32-1);
1644 data->object[32-1] = 0;
1645 data->rot_x = ini_rot_x;
1652 data_size =
sizeof(ObjectGotoMessage_data_t);
1655 data = (ObjectGotoMessage_data_t *)
data_ptr;
1675 data = (ObjectGotoMessage_data_t *)
data_ptr;
1687 return data->object;
1760 data_size =
sizeof(CalibrateMessage_data_t);
1763 data = (CalibrateMessage_data_t *)
data_ptr;
1781 data = (CalibrateMessage_data_t *)
data_ptr;
1806 data_size =
sizeof(OpenGripperMessage_data_t);
1809 data = (OpenGripperMessage_data_t *)
data_ptr;
1827 data = (OpenGripperMessage_data_t *)
data_ptr;
1852 data_size =
sizeof(CloseGripperMessage_data_t);
1855 data = (CloseGripperMessage_data_t *)
data_ptr;
1873 data = (CloseGripperMessage_data_t *)
data_ptr;
1900 data_size =
sizeof(SetEnabledMessage_data_t);
1903 data = (SetEnabledMessage_data_t *)
data_ptr;
1905 data->enabled = ini_enabled;
1911 data_size =
sizeof(SetEnabledMessage_data_t);
1914 data = (SetEnabledMessage_data_t *)
data_ptr;
1933 data = (SetEnabledMessage_data_t *)
data_ptr;
1945 return data->enabled;
1990 data_size =
sizeof(SetMaxVelocityMessage_data_t);
1993 data = (SetMaxVelocityMessage_data_t *)
data_ptr;
1995 data->max_velocity = ini_max_velocity;
2001 data_size =
sizeof(SetMaxVelocityMessage_data_t);
2004 data = (SetMaxVelocityMessage_data_t *)
data_ptr;
2023 data = (SetMaxVelocityMessage_data_t *)
data_ptr;
2035 return data->max_velocity;
2081 data_size =
sizeof(SetPlannerParamsMessage_data_t);
2084 data = (SetPlannerParamsMessage_data_t *)
data_ptr;
2086 strncpy(data->plannerparams, ini_plannerparams, 1024-1);
2087 data->plannerparams[1024-1] = 0;
2088 data->straight = ini_straight;
2095 data_size =
sizeof(SetPlannerParamsMessage_data_t);
2098 data = (SetPlannerParamsMessage_data_t *)
data_ptr;
2118 data = (SetPlannerParamsMessage_data_t *)
data_ptr;
2130 return data->plannerparams;
2160 return data->straight;
2206 data_size =
sizeof(SetMotorEncoderMessage_data_t);
2209 data = (SetMotorEncoderMessage_data_t *)
data_ptr;
2212 data->enc = ini_enc;
2219 data_size =
sizeof(SetMotorEncoderMessage_data_t);
2222 data = (SetMotorEncoderMessage_data_t *)
data_ptr;
2242 data = (SetMotorEncoderMessage_data_t *)
data_ptr;
2330 data_size =
sizeof(MoveMotorEncoderMessage_data_t);
2333 data = (MoveMotorEncoderMessage_data_t *)
data_ptr;
2336 data->enc = ini_enc;
2343 data_size =
sizeof(MoveMotorEncoderMessage_data_t);
2346 data = (MoveMotorEncoderMessage_data_t *)
data_ptr;
2366 data = (MoveMotorEncoderMessage_data_t *)
data_ptr;
2454 data_size =
sizeof(SetMotorAngleMessage_data_t);
2457 data = (SetMotorAngleMessage_data_t *)
data_ptr;
2460 data->angle = ini_angle;
2467 data_size =
sizeof(SetMotorAngleMessage_data_t);
2470 data = (SetMotorAngleMessage_data_t *)
data_ptr;
2490 data = (SetMotorAngleMessage_data_t *)
data_ptr;
2578 data_size =
sizeof(MoveMotorAngleMessage_data_t);
2581 data = (MoveMotorAngleMessage_data_t *)
data_ptr;
2584 data->angle = ini_angle;
2591 data_size =
sizeof(MoveMotorAngleMessage_data_t);
2594 data = (MoveMotorAngleMessage_data_t *)
data_ptr;
2614 data = (MoveMotorAngleMessage_data_t *)
data_ptr;
2737 if ( m10 != NULL ) {
2741 if ( m11 != NULL ) {
2745 if ( m12 != NULL ) {
2749 if ( m13 != NULL ) {
2753 if ( m14 != NULL ) {
2757 if ( m15 != NULL ) {