24 #include <interfaces/NaoSensorInterface.h>
26 #include <core/exceptions/software.h>
46 NaoSensorInterface::NaoSensorInterface() : Interface()
48 data_size =
sizeof(NaoSensorInterface_data_t);
50 data = (NaoSensorInterface_data_t *)
data_ptr;
53 enum_map_UltrasonicDirection[(int)USD_NONE] =
"USD_NONE";
54 enum_map_UltrasonicDirection[(int)USD_LEFT_LEFT] =
"USD_LEFT_LEFT";
55 enum_map_UltrasonicDirection[(int)USD_LEFT_RIGHT] =
"USD_LEFT_RIGHT";
56 enum_map_UltrasonicDirection[(int)USD_RIGHT_RIGHT] =
"USD_RIGHT_RIGHT";
57 enum_map_UltrasonicDirection[(int)USD_RIGHT_LEFT] =
"USD_RIGHT_LEFT";
58 enum_map_UltrasonicDirection[(int)USD_BOTH_BOTH] =
"USD_BOTH_BOTH";
83 add_fieldinfo(
IFT_ENUM,
"ultrasonic_direction", 1, &data->ultrasonic_direction,
"UltrasonicDirection", &enum_map_UltrasonicDirection);
96 unsigned char tmp_hash[] = {0x41, 0x41, 0x54, 0x94, 0xca, 0xe8, 0x3d, 0x7a, 0xb8, 0xaa, 0xc2, 0x4e, 0x2c, 0xac, 0xcb, 0x15};
101 NaoSensorInterface::~NaoSensorInterface()
119 default:
return "UNKNOWN";
130 return data->accel_x;
150 data->accel_x = new_accel_x;
161 return data->accel_y;
181 data->accel_y = new_accel_y;
192 return data->accel_z;
212 data->accel_z = new_accel_z;
243 data->gyro_x = new_gyro_x;
274 data->gyro_y = new_gyro_y;
285 return data->gyro_ref;
305 data->gyro_ref = new_gyro_ref;
316 return data->angle_x;
336 data->angle_x = new_angle_x;
347 return data->angle_y;
367 data->angle_y = new_angle_y;
378 return data->l_fsr_fl;
398 data->l_fsr_fl = new_l_fsr_fl;
409 return data->l_fsr_fr;
429 data->l_fsr_fr = new_l_fsr_fr;
440 return data->l_fsr_rl;
460 data->l_fsr_rl = new_l_fsr_rl;
471 return data->l_fsr_rr;
491 data->l_fsr_rr = new_l_fsr_rr;
502 return data->r_fsr_fl;
522 data->r_fsr_fl = new_r_fsr_fl;
533 return data->r_fsr_fr;
553 data->r_fsr_fr = new_r_fsr_fr;
564 return data->r_fsr_rl;
584 data->r_fsr_rl = new_r_fsr_rl;
595 return data->r_fsr_rr;
615 data->r_fsr_rr = new_r_fsr_rr;
626 return data->l_total_weight;
646 data->l_total_weight = new_l_total_weight;
657 return data->r_total_weight;
677 data->r_total_weight = new_r_total_weight;
688 return data->l_cop_x;
708 data->l_cop_x = new_l_cop_x;
719 return data->l_cop_y;
739 data->l_cop_y = new_l_cop_y;
750 return data->r_cop_x;
770 data->r_cop_x = new_r_cop_x;
781 return data->r_cop_y;
801 data->r_cop_y = new_r_cop_y;
815 return data->ultrasonic_distance_left;
831 throw Exception(
"Index value %u out of bounds (0..3)", index);
833 return data->ultrasonic_distance_left[index];
856 memcpy(data->ultrasonic_distance_left, new_ultrasonic_distance_left,
sizeof(
float) * 4);
872 throw Exception(
"Index value %u out of bounds (0..3)", index);
874 data->ultrasonic_distance_left[index] = new_ultrasonic_distance_left;
887 return data->ultrasonic_distance_right;
903 throw Exception(
"Index value %u out of bounds (0..3)", index);
905 return data->ultrasonic_distance_right[index];
928 memcpy(data->ultrasonic_distance_right, new_ultrasonic_distance_right,
sizeof(
float) * 4);
944 throw Exception(
"Index value %u out of bounds (0..3)", index);
946 data->ultrasonic_distance_right[index] = new_ultrasonic_distance_right;
980 data->ultrasonic_direction = new_ultrasonic_direction;
991 return data->l_foot_bumper_l;
1011 data->l_foot_bumper_l = new_l_foot_bumper_l;
1022 return data->l_foot_bumper_r;
1042 data->l_foot_bumper_r = new_l_foot_bumper_r;
1053 return data->r_foot_bumper_l;
1073 data->r_foot_bumper_l = new_r_foot_bumper_l;
1084 return data->r_foot_bumper_r;
1104 data->r_foot_bumper_r = new_r_foot_bumper_r;
1115 return data->head_touch_front;
1135 data->head_touch_front = new_head_touch_front;
1146 return data->head_touch_middle;
1166 data->head_touch_middle = new_head_touch_middle;
1177 return data->head_touch_rear;
1197 data->head_touch_rear = new_head_touch_rear;
1208 return data->chest_button;
1228 data->chest_button = new_chest_button;
1239 return data->battery_charge;
1259 data->battery_charge = new_battery_charge;
1267 if ( strncmp(
"EmitUltrasonicWaveMessage",
type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1268 return new EmitUltrasonicWaveMessage();
1269 }
else if ( strncmp(
"StartUltrasonicMessage",
type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1270 return new StartUltrasonicMessage();
1271 }
else if ( strncmp(
"StopUltrasonicMessage",
type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
1272 return new StopUltrasonicMessage();
1274 throw UnknownTypeException(
"The given type '%s' does not match any known "
1275 "message type for this interface type.",
type);
1286 const NaoSensorInterface *oi = dynamic_cast<const NaoSensorInterface *>(other);
1289 type(), other->type());
1291 memcpy(data, oi->data,
sizeof(NaoSensorInterface_data_t));
1297 if (strcmp(enumtype,
"UltrasonicDirection") == 0) {
1316 data_size =
sizeof(EmitUltrasonicWaveMessage_data_t);
1319 data = (EmitUltrasonicWaveMessage_data_t *)
data_ptr;
1321 data->ultrasonic_direction = ini_ultrasonic_direction;
1322 enum_map_UltrasonicDirection[(int)
USD_NONE] =
"USD_NONE";
1323 enum_map_UltrasonicDirection[(int)
USD_LEFT_LEFT] =
"USD_LEFT_LEFT";
1324 enum_map_UltrasonicDirection[(int)
USD_LEFT_RIGHT] =
"USD_LEFT_RIGHT";
1325 enum_map_UltrasonicDirection[(int)
USD_RIGHT_RIGHT] =
"USD_RIGHT_RIGHT";
1326 enum_map_UltrasonicDirection[(int)
USD_RIGHT_LEFT] =
"USD_RIGHT_LEFT";
1327 enum_map_UltrasonicDirection[(int)
USD_BOTH_BOTH] =
"USD_BOTH_BOTH";
1328 add_fieldinfo(
IFT_ENUM,
"ultrasonic_direction", 1, &data->ultrasonic_direction,
"UltrasonicDirection", &enum_map_UltrasonicDirection);
1333 data_size =
sizeof(EmitUltrasonicWaveMessage_data_t);
1336 data = (EmitUltrasonicWaveMessage_data_t *)
data_ptr;
1338 enum_map_UltrasonicDirection[(int)
USD_NONE] =
"USD_NONE";
1339 enum_map_UltrasonicDirection[(int)
USD_LEFT_LEFT] =
"USD_LEFT_LEFT";
1340 enum_map_UltrasonicDirection[(int)
USD_LEFT_RIGHT] =
"USD_LEFT_RIGHT";
1341 enum_map_UltrasonicDirection[(int)
USD_RIGHT_RIGHT] =
"USD_RIGHT_RIGHT";
1342 enum_map_UltrasonicDirection[(int)
USD_RIGHT_LEFT] =
"USD_RIGHT_LEFT";
1343 enum_map_UltrasonicDirection[(int)
USD_BOTH_BOTH] =
"USD_BOTH_BOTH";
1344 add_fieldinfo(
IFT_ENUM,
"ultrasonic_direction", 1, &data->ultrasonic_direction,
"UltrasonicDirection", &enum_map_UltrasonicDirection);
1361 data = (EmitUltrasonicWaveMessage_data_t *)
data_ptr;
1397 data->ultrasonic_direction = new_ultrasonic_direction;
1422 data_size =
sizeof(StartUltrasonicMessage_data_t);
1425 data = (StartUltrasonicMessage_data_t *)
data_ptr;
1427 data->ultrasonic_direction = ini_ultrasonic_direction;
1428 enum_map_UltrasonicDirection[(int)
USD_NONE] =
"USD_NONE";
1429 enum_map_UltrasonicDirection[(int)
USD_LEFT_LEFT] =
"USD_LEFT_LEFT";
1430 enum_map_UltrasonicDirection[(int)
USD_LEFT_RIGHT] =
"USD_LEFT_RIGHT";
1431 enum_map_UltrasonicDirection[(int)
USD_RIGHT_RIGHT] =
"USD_RIGHT_RIGHT";
1432 enum_map_UltrasonicDirection[(int)
USD_RIGHT_LEFT] =
"USD_RIGHT_LEFT";
1433 enum_map_UltrasonicDirection[(int)
USD_BOTH_BOTH] =
"USD_BOTH_BOTH";
1434 add_fieldinfo(
IFT_ENUM,
"ultrasonic_direction", 1, &data->ultrasonic_direction,
"UltrasonicDirection", &enum_map_UltrasonicDirection);
1439 data_size =
sizeof(StartUltrasonicMessage_data_t);
1442 data = (StartUltrasonicMessage_data_t *)
data_ptr;
1444 enum_map_UltrasonicDirection[(int)
USD_NONE] =
"USD_NONE";
1445 enum_map_UltrasonicDirection[(int)
USD_LEFT_LEFT] =
"USD_LEFT_LEFT";
1446 enum_map_UltrasonicDirection[(int)
USD_LEFT_RIGHT] =
"USD_LEFT_RIGHT";
1447 enum_map_UltrasonicDirection[(int)
USD_RIGHT_RIGHT] =
"USD_RIGHT_RIGHT";
1448 enum_map_UltrasonicDirection[(int)
USD_RIGHT_LEFT] =
"USD_RIGHT_LEFT";
1449 enum_map_UltrasonicDirection[(int)
USD_BOTH_BOTH] =
"USD_BOTH_BOTH";
1450 add_fieldinfo(
IFT_ENUM,
"ultrasonic_direction", 1, &data->ultrasonic_direction,
"UltrasonicDirection", &enum_map_UltrasonicDirection);
1467 data = (StartUltrasonicMessage_data_t *)
data_ptr;
1503 data->ultrasonic_direction = new_ultrasonic_direction;
1526 data_size =
sizeof(StopUltrasonicMessage_data_t);
1529 data = (StopUltrasonicMessage_data_t *)
data_ptr;
1531 enum_map_UltrasonicDirection[(int)
USD_NONE] =
"USD_NONE";
1532 enum_map_UltrasonicDirection[(int)
USD_LEFT_LEFT] =
"USD_LEFT_LEFT";
1533 enum_map_UltrasonicDirection[(int)
USD_LEFT_RIGHT] =
"USD_LEFT_RIGHT";
1534 enum_map_UltrasonicDirection[(int)
USD_RIGHT_RIGHT] =
"USD_RIGHT_RIGHT";
1535 enum_map_UltrasonicDirection[(int)
USD_RIGHT_LEFT] =
"USD_RIGHT_LEFT";
1536 enum_map_UltrasonicDirection[(int)
USD_BOTH_BOTH] =
"USD_BOTH_BOTH";
1553 data = (StopUltrasonicMessage_data_t *)
data_ptr;
1579 const StartUltrasonicMessage *m1 = dynamic_cast<const StartUltrasonicMessage *>(message);
1583 const StopUltrasonicMessage *m2 = dynamic_cast<const StopUltrasonicMessage *>(message);
1591 EXPORT_INTERFACE(NaoSensorInterface)