33 const char* log = NULL;
51 printf(
"Failed to init\n");
57 printf(
"Succeed to init(%d)\n", dxl_baud_rate_);
61 uint16_t model_number = 0;
62 result = dynamixel_workbench_->ping(ping_id, &model_number, &log);
65 printf(
"Please Check USB Port authorization and\n");
66 printf(
"Baudrate [ex : 9600, 57600, 115200, 1000000, 2000000]\n");
67 printf(
"...Failed to find dynamixel!\n");
68 printf(
"Log Message : %s\n", log);
76 printf(
"[ID] %u, [Model Name] %s, [BAUD RATE] %d [VERSION] %.1f\n",
83 result = dynamixel_workbench_->scan(&
dxl_id_, &id_cnt, scan_range, &log);
86 printf(
"Please Check USB Port authorization and\n");
87 printf(
"Baudrate [ex : 9600, 57600, 115200, 1000000, 2000000]\n");
88 printf(
"...Failed to find dynamixel!\n");
89 printf(
"Log Message : %s\n", log);
96 printf(
"[ID] %u, [Model Name] %s, [BAUD RATE] %d [VERSION] %.1f\n",
105 printf(
"dynamixel_workbench_single_manager : Init Success!\n");
129 if (!strncmp(model_name,
"AX", strlen(
"AX")))
133 else if (!strncmp(model_name,
"RX", strlen(
"RX")))
137 else if (!strncmp(model_name,
"MX", strlen(
"MX")))
139 if (!strncmp(model_name,
"MX-12W", strlen(model_name)) ||
140 !strncmp(model_name,
"MX-28" , strlen(model_name)))
144 else if (!strncmp(model_name,
"MX-64", strlen(model_name)) ||
145 !strncmp(model_name,
"MX-106" , strlen(model_name)))
149 else if (!strncmp(model_name,
"MX-28-2", strlen(model_name)))
153 else if (!strncmp(model_name,
"MX-64-2", strlen(model_name)) ||
154 !strncmp(model_name,
"MX-106-2" , strlen(model_name)))
159 else if (!strncmp(model_name,
"EX", strlen(
"EX")))
163 else if (!strncmp(model_name,
"XL", strlen(
"XL")))
165 if (!strncmp(model_name,
"XL-320", strlen(model_name)))
169 else if (!strncmp(model_name,
"XL430-W250", strlen(model_name)))
174 else if (!strncmp(model_name,
"XM", strlen(
"XM")))
176 if (!strncmp(model_name,
"XM430-W210", strlen(model_name)) ||
177 !strncmp(model_name,
"XM430-W350" , strlen(model_name)))
181 else if (!strncmp(model_name,
"XM540-W150", strlen(model_name)) ||
182 !strncmp(model_name,
"XM540-W270" , strlen(model_name)))
187 else if (!strncmp(model_name,
"XH", strlen(
"XH")))
191 else if (!strncmp(model_name,
"PRO", strlen(
"PRO")))
193 if ((!strncmp(model_name,
"PRO_L42_10_S300_R", strlen(model_name))))
217 int32_t torque_status = 0;
218 uint16_t torque_enable_address = 0;
223 if (!strncmp(item_ptr[item_num].item_name,
"Torque_Enable", strlen(
"Torque_Enable")))
225 torque_enable_address = item_num;
233 if (torque_status ==
false)
235 printf(
"%s\n", item_ptr[item_num].item_name);
239 if (item_num >= torque_enable_address)
240 printf(
"%s\n", item_ptr[item_num].item_name);
253 if (!strncmp(item_ptr[item_num].item_name, cmd.c_str(), strlen(item_ptr[item_num].item_name)))
257 printf(
"Please Check DYNAMXEL Address Name('table')\n");
263 const char* log = NULL;
266 if (new_id > 0 && new_id < 254)
272 printf(
"...Failed to change ID!\n");
278 uint16_t model_number = 0;
283 printf(
"...Failed to ping Dynamixel ID [%u]\n",
dxl_id_);
290 printf(
"...Succeeded to ping Dynamixel ID [%u]\n",
dxl_id_);
299 printf(
"Dynamixel ID can be set 1~253\n");
308 const char* log = NULL;
310 bool check_baud_rate =
false;
312 uint64_t baud_rate_list[14] = {9600, 19200, 57600, 115200, 200000, 250000, 400000, 500000,
313 1000000, 2000000, 3000000, 4000000, 4500000, 10500000};
315 for (
int i = 0; i < 14; i++)
317 if (baud_rate_list[i] == new_baud_rate)
318 check_baud_rate =
true;
321 if (check_baud_rate ==
false)
323 printf(
"Failed to change [ BAUD RATE: %d ]\n", new_baud_rate);
324 printf(
"Valid baud rate is [9600, 57600, 115200, 1000000, 2000000,...]\n");
325 printf(
"You can choose other baud rate in GUI,\n");
335 printf(
"Failed to change baudrate!\n");
343 printf(
"Success to change baudrate! [ BAUD RATE: %d ]\n", new_baud_rate);
353 const char* log = NULL;
356 if (ver == 1.0
f || ver == 2.0
f)
362 printf(
"Failed to change protocol version\n");
375 printf(
"Dynamixel supports protocol version [1.0] or [2.0]\n");
390 dynamixel_workbench_msgs::GetDynamixelInfo::Response &res)
392 res.dynamixel_info.load_info.device_name =
device_name_;
396 res.dynamixel_info.model_id =
dxl_id_;
404 dynamixel_workbench_msgs::DynamixelCommand::Response &res)
406 const char* log = NULL;
409 if (req.command ==
"table")
412 res.comm_result =
true;
414 res.comm_result =
false;
416 else if (req.command ==
"reboot")
422 printf(
"Failed to reboot\n");
423 res.comm_result =
false;
428 printf(
"Succeed to reboot\n");
429 res.comm_result =
true;
432 else if (req.command ==
"factory_reset")
438 printf(
"Failed to reset\n");
439 res.comm_result =
false;
446 printf(
"Succeed to reset\n");
447 res.comm_result =
true;
450 else if (req.command ==
"torque")
452 uint8_t value = req.value;
457 res.comm_result =
false;
462 res.comm_result =
true;
466 else if (req.command ==
"Joint")
468 int32_t value = req.value;
474 res.comm_result =
false;
485 res.comm_result =
false;
492 res.comm_result =
true;
494 else if (req.command ==
"Wheel")
496 int32_t value = req.value;
502 res.comm_result =
false;
513 res.comm_result =
false;
520 res.comm_result =
true;
522 else if (req.command ==
"exit")
526 else if (req.command ==
"addr")
528 std::string addr = req.addr_name;
529 int32_t value = req.value;
533 res.comm_result =
true;
537 res.comm_result =
false;
544 res.comm_result =
true;
546 res.comm_result =
false;
548 else if (addr ==
"Baud_Rate")
551 res.comm_result =
true;
553 res.comm_result =
false;
555 else if (addr ==
"Protocol_Version")
558 res.comm_result =
true;
560 res.comm_result =
false;
568 res.comm_result =
false;
573 res.comm_result =
true;
585 int main(
int argc,
char **argv)
588 ros::init(argc, argv,
"single_dynamixel_monitor");
609 if (!strncmp(model_name,
"AX", strlen(
"AX")))
613 else if (!strncmp(model_name,
"RX", strlen(
"RX")))
617 else if (!strncmp(model_name,
"MX-12W", strlen(model_name)) ||
618 !strncmp(model_name,
"MX-28" , strlen(model_name)))
622 else if (!strncmp(model_name,
"MX-64", strlen(model_name)) ||
623 !strncmp(model_name,
"MX-106" , strlen(model_name)))
627 else if (!strncmp(model_name,
"MX-28-2", strlen(model_name)))
631 else if (!strncmp(model_name,
"MX-64-2", strlen(model_name)) ||
632 !strncmp(model_name,
"MX-106-2" , strlen(model_name)))
636 else if (!strncmp(model_name,
"EX", strlen(
"EX")))
640 else if (!strncmp(model_name,
"XL-320", strlen(model_name)))
644 else if (!strncmp(model_name,
"XL430-W250", strlen(model_name)))
648 else if (!strncmp(model_name,
"XM430-W210", strlen(model_name)) ||
649 !strncmp(model_name,
"XM430-W350" , strlen(model_name)))
653 else if (!strncmp(model_name,
"XM540-W150", strlen(model_name)) ||
654 !strncmp(model_name,
"XM540-W270" , strlen(model_name)))
658 else if (!strncmp(model_name,
"XH", strlen(
"XH")))
662 else if (!strncmp(model_name,
"PRO_L42_10_S300_R", strlen(
"PRO_L42_10_S300_R")))
666 else if (!strncmp(model_name,
"PRO", strlen(
"PRO")))
675 dynamixel_workbench_msgs::AX ax_state;
679 uint32_t getAllRegisteredData[last_register_addr+last_register_addr_length] = {0, };
685 int32_t read_value = 0;
686 read_value = getAllRegisteredData[item_ptr[index].
address];
688 switch (item_ptr[index].data_length)
691 read_value = getAllRegisteredData[item_ptr[index].address];
695 read_value =
DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address], getAllRegisteredData[item_ptr[index].address+1]);
699 read_value =
DXL_MAKEDWORD(
DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address], getAllRegisteredData[item_ptr[index].address+1]),
700 DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address+2], getAllRegisteredData[item_ptr[index].address+3]));
704 read_value = getAllRegisteredData[item_ptr[index].address];
708 if (!strncmp(item_ptr[index].item_name,
"Model_Number", strlen(
"Model_Number")))
709 ax_state.Model_Number = read_value;
710 else if (!strncmp(item_ptr[index].item_name,
"Firmware_Version", strlen(
"Firmware_Version")))
711 ax_state.Firmware_Version = read_value;
712 else if (!strncmp(item_ptr[index].item_name,
"ID", strlen(
"ID")))
713 ax_state.ID = read_value;
714 else if (!strncmp(item_ptr[index].item_name,
"Baud_Rate", strlen(
"Baud_Rate")))
715 ax_state.Baud_Rate = read_value;
716 else if (!strncmp(item_ptr[index].item_name,
"Return_Delay_Time", strlen(
"Return_Delay_Time")))
717 ax_state.Return_Delay_Time = read_value;
718 else if (!strncmp(item_ptr[index].item_name,
"CW_Angle_Limit", strlen(
"CW_Angle_Limit")))
719 ax_state.CW_Angle_Limit = read_value;
720 else if (!strncmp(item_ptr[index].item_name,
"CCW_Angle_Limit", strlen(
"CCW_Angle_Limit")))
721 ax_state.CCW_Angle_Limit = read_value;
722 else if (!strncmp(item_ptr[index].item_name,
"Temperature_Limit", strlen(
"Temperature_Limit")))
723 ax_state.Temperature_Limit = read_value;
724 else if (!strncmp(item_ptr[index].item_name,
"Min_Voltage_Limit", strlen(
"Min_Voltage_Limit")))
725 ax_state.Min_Voltage_Limit = read_value;
726 else if (!strncmp(item_ptr[index].item_name,
"Max_Voltage_Limit", strlen(
"Max_Voltage_Limit")))
727 ax_state.Max_Voltage_Limit = read_value;
728 else if (!strncmp(item_ptr[index].item_name,
"Max_Torque", strlen(
"Max_Torque")))
729 ax_state.Max_Torque = read_value;
730 else if (!strncmp(item_ptr[index].item_name,
"Status_Return_Level", strlen(
"Status_Return_Level")))
731 ax_state.Status_Return_Level = read_value;
732 else if (!strncmp(item_ptr[index].item_name,
"Alarm_LED", strlen(
"Alarm_LED")))
733 ax_state.Alarm_LED = read_value;
734 else if (!strncmp(item_ptr[index].item_name,
"Shutdown", strlen(
"Shutdown")))
735 ax_state.Shutdown = read_value;
736 else if (!strncmp(item_ptr[index].item_name,
"Torque_Enable", strlen(
"Torque_Enable")))
737 ax_state.Torque_Enable = read_value;
738 else if (!strncmp(item_ptr[index].item_name,
"LED", strlen(
"LED")))
739 ax_state.LED = read_value;
740 else if (!strncmp(item_ptr[index].item_name,
"CW_Compliance_Margin", strlen(
"CW_Compliance_Margin")))
741 ax_state.CW_Compliance_Margin = read_value;
742 else if (!strncmp(item_ptr[index].item_name,
"CCW_Compliance_Margin", strlen(
"CCW_Compliance_Margin")))
743 ax_state.CCW_Compliance_Margin = read_value;
744 else if (!strncmp(item_ptr[index].item_name,
"CW_Compliance_Slope", strlen(
"CW_Compliance_Slope")))
745 ax_state.CW_Compliance_Slope = read_value;
746 else if (!strncmp(item_ptr[index].item_name,
"CCW_Compliance_Slope", strlen(
"CCW_Compliance_Slope")))
747 ax_state.CCW_Compliance_Slope = read_value;
748 else if (!strncmp(item_ptr[index].item_name,
"Goal_Position", strlen(
"Goal_Position")))
749 ax_state.Goal_Position = read_value;
750 else if (!strncmp(item_ptr[index].item_name,
"Moving_Speed", strlen(
"Moving_Speed")))
751 ax_state.Moving_Speed = read_value;
752 else if (!strncmp(item_ptr[index].item_name,
"Torque_Limit", strlen(
"Torque_Limit")))
753 ax_state.Torque_Limit = read_value;
754 else if (!strncmp(item_ptr[index].item_name,
"Present_Position", strlen(
"Present_Position")))
755 ax_state.Present_Position = read_value;
756 else if (!strncmp(item_ptr[index].item_name,
"Present_Speed", strlen(
"Present_Speed")))
757 ax_state.Present_Speed = read_value;
758 else if (!strncmp(item_ptr[index].item_name,
"Present_Load", strlen(
"Present_Load")))
759 ax_state.Present_Load = read_value;
760 else if (!strncmp(item_ptr[index].item_name,
"Present_Voltage", strlen(
"Present_Voltage")))
761 ax_state.Present_Voltage = read_value;
762 else if (!strncmp(item_ptr[index].item_name,
"Present_Temperature", strlen(
"Present_Temperature")))
763 ax_state.Present_Temperature = read_value;
764 else if (!strncmp(item_ptr[index].item_name,
"Registered", strlen(
"Registered")))
765 ax_state.Registered = read_value;
766 else if (!strncmp(item_ptr[index].item_name,
"Moving", strlen(
"Moving")))
767 ax_state.Moving = read_value;
768 else if (!strncmp(item_ptr[index].item_name,
"Lock", strlen(
"Lock")))
769 ax_state.Lock = read_value;
770 else if (!strncmp(item_ptr[index].item_name,
"Punch", strlen(
"Punch")))
771 ax_state.Punch = read_value;
780 dynamixel_workbench_msgs::RX rx_state;
784 uint32_t getAllRegisteredData[last_register_addr+last_register_addr_length] = {0, };
790 int32_t read_value = 0;
791 read_value = getAllRegisteredData[item_ptr[index].
address];
793 switch (item_ptr[index].data_length)
796 read_value = getAllRegisteredData[item_ptr[index].address];
800 read_value =
DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address], getAllRegisteredData[item_ptr[index].address+1]);
804 read_value =
DXL_MAKEDWORD(
DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address], getAllRegisteredData[item_ptr[index].address+1]),
805 DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address+2], getAllRegisteredData[item_ptr[index].address+3]));
809 read_value = getAllRegisteredData[item_ptr[index].address];
813 if (!strncmp(item_ptr[index].item_name,
"Model_Number", strlen(
"Model_Number")))
814 rx_state.Model_Number = read_value;
815 else if (!strncmp(item_ptr[index].item_name,
"Firmware_Version", strlen(
"Firmware_Version")))
816 rx_state.Firmware_Version = read_value;
817 else if (!strncmp(item_ptr[index].item_name,
"ID", strlen(
"ID")))
818 rx_state.ID = read_value;
819 else if (!strncmp(item_ptr[index].item_name,
"Baud_Rate", strlen(
"Baud_Rate")))
820 rx_state.Baud_Rate = read_value;
821 else if (!strncmp(item_ptr[index].item_name,
"Return_Delay_Time", strlen(
"Return_Delay_Time")))
822 rx_state.Return_Delay_Time = read_value;
823 else if (!strncmp(item_ptr[index].item_name,
"CW_Angle_Limit", strlen(
"CW_Angle_Limit")))
824 rx_state.CW_Angle_Limit = read_value;
825 else if (!strncmp(item_ptr[index].item_name,
"CCW_Angle_Limit", strlen(
"CCW_Angle_Limit")))
826 rx_state.CCW_Angle_Limit = read_value;
827 else if (!strncmp(item_ptr[index].item_name,
"Temperature_Limit", strlen(
"Temperature_Limit")))
828 rx_state.Temperature_Limit = read_value;
829 else if (!strncmp(item_ptr[index].item_name,
"Min_Voltage_Limit", strlen(
"Min_Voltage_Limit")))
830 rx_state.Min_Voltage_Limit = read_value;
831 else if (!strncmp(item_ptr[index].item_name,
"Max_Voltage_Limit", strlen(
"Max_Voltage_Limit")))
832 rx_state.Max_Voltage_Limit = read_value;
833 else if (!strncmp(item_ptr[index].item_name,
"Max_Torque", strlen(
"Max_Torque")))
834 rx_state.Max_Torque = read_value;
835 else if (!strncmp(item_ptr[index].item_name,
"Status_Return_Level", strlen(
"Status_Return_Level")))
836 rx_state.Status_Return_Level = read_value;
837 else if (!strncmp(item_ptr[index].item_name,
"Alarm_LED", strlen(
"Alarm_LED")))
838 rx_state.Alarm_LED = read_value;
839 else if (!strncmp(item_ptr[index].item_name,
"Shutdown", strlen(
"Shutdown")))
840 rx_state.Shutdown = read_value;
841 else if (!strncmp(item_ptr[index].item_name,
"Torque_Enable", strlen(
"Torque_Enable")))
842 rx_state.Torque_Enable = read_value;
843 else if (!strncmp(item_ptr[index].item_name,
"LED", strlen(
"LED")))
844 rx_state.LED = read_value;
845 else if (!strncmp(item_ptr[index].item_name,
"CW_Compliance_Margin", strlen(
"CW_Compliance_Margin")))
846 rx_state.CW_Compliance_Margin = read_value;
847 else if (!strncmp(item_ptr[index].item_name,
"CCW_Compliance_Margin", strlen(
"CCW_Compliance_Margin")))
848 rx_state.CCW_Compliance_Margin = read_value;
849 else if (!strncmp(item_ptr[index].item_name,
"CW_Compliance_Slope", strlen(
"CW_Compliance_Slope")))
850 rx_state.CW_Compliance_Slope = read_value;
851 else if (!strncmp(item_ptr[index].item_name,
"CCW_Compliance_Slope", strlen(
"CCW_Compliance_Slope")))
852 rx_state.CCW_Compliance_Slope = read_value;
853 else if (!strncmp(item_ptr[index].item_name,
"Goal_Position", strlen(
"Goal_Position")))
854 rx_state.Goal_Position = read_value;
855 else if (!strncmp(item_ptr[index].item_name,
"Moving_Speed", strlen(
"Moving_Speed")))
856 rx_state.Moving_Speed = read_value;
857 else if (!strncmp(item_ptr[index].item_name,
"Torque_Limit", strlen(
"Torque_Limit")))
858 rx_state.Torque_Limit = read_value;
859 else if (!strncmp(item_ptr[index].item_name,
"Present_Position", strlen(
"Present_Position")))
860 rx_state.Present_Position = read_value;
861 else if (!strncmp(item_ptr[index].item_name,
"Present_Speed", strlen(
"Present_Speed")))
862 rx_state.Present_Speed = read_value;
863 else if (!strncmp(item_ptr[index].item_name,
"Present_Load", strlen(
"Present_Load")))
864 rx_state.Present_Load = read_value;
865 else if (!strncmp(item_ptr[index].item_name,
"Present_Voltage", strlen(
"Present_Voltage")))
866 rx_state.Present_Voltage = read_value;
867 else if (!strncmp(item_ptr[index].item_name,
"Present_Temperature", strlen(
"Present_Temperature")))
868 rx_state.Present_Temperature = read_value;
869 else if (!strncmp(item_ptr[index].item_name,
"Registered", strlen(
"Registered")))
870 rx_state.Registered = read_value;
871 else if (!strncmp(item_ptr[index].item_name,
"Moving", strlen(
"Moving")))
872 rx_state.Moving = read_value;
873 else if (!strncmp(item_ptr[index].item_name,
"Lock", strlen(
"Lock")))
874 rx_state.Lock = read_value;
875 else if (!strncmp(item_ptr[index].item_name,
"Punch", strlen(
"Punch")))
876 rx_state.Punch = read_value;
885 dynamixel_workbench_msgs::MX mx_state;
889 uint32_t getAllRegisteredData[last_register_addr+last_register_addr_length] = {0, };
895 int32_t read_value = 0;
896 read_value = getAllRegisteredData[item_ptr[index].
address];
898 switch (item_ptr[index].data_length)
901 read_value = getAllRegisteredData[item_ptr[index].address];
905 read_value =
DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address], getAllRegisteredData[item_ptr[index].address+1]);
909 read_value =
DXL_MAKEDWORD(
DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address], getAllRegisteredData[item_ptr[index].address+1]),
910 DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address+2], getAllRegisteredData[item_ptr[index].address+3]));
914 read_value = getAllRegisteredData[item_ptr[index].address];
918 if (!strncmp(item_ptr[index].item_name,
"Model_Number", strlen(
"Model_Number")))
919 mx_state.Model_Number = read_value;
920 else if (!strncmp(item_ptr[index].item_name,
"Firmware_Version", strlen(
"Firmware_Version")))
921 mx_state.Firmware_Version = read_value;
922 else if (!strncmp(item_ptr[index].item_name,
"ID", strlen(
"ID")))
923 mx_state.ID = read_value;
924 else if (!strncmp(item_ptr[index].item_name,
"Baud_Rate", strlen(
"Baud_Rate")))
925 mx_state.Baud_Rate = read_value;
926 else if (!strncmp(item_ptr[index].item_name,
"Return_Delay_Time", strlen(
"Return_Delay_Time")))
927 mx_state.Return_Delay_Time = read_value;
928 else if (!strncmp(item_ptr[index].item_name,
"CW_Angle_Limit", strlen(
"CW_Angle_Limit")))
929 mx_state.CW_Angle_Limit = read_value;
930 else if (!strncmp(item_ptr[index].item_name,
"CCW_Angle_Limit", strlen(
"CCW_Angle_Limit")))
931 mx_state.CCW_Angle_Limit = read_value;
932 else if (!strncmp(item_ptr[index].item_name,
"Temperature_Limit", strlen(
"Temperature_Limit")))
933 mx_state.Temperature_Limit = read_value;
934 else if (!strncmp(item_ptr[index].item_name,
"Min_Voltage_Limit", strlen(
"Min_Voltage_Limit")))
935 mx_state.Min_Voltage_Limit = read_value;
936 else if (!strncmp(item_ptr[index].item_name,
"Max_Voltage_Limit", strlen(
"Max_Voltage_Limit")))
937 mx_state.Max_Voltage_Limit = read_value;
938 else if (!strncmp(item_ptr[index].item_name,
"Max_Torque", strlen(
"Max_Torque")))
939 mx_state.Max_Torque = read_value;
940 else if (!strncmp(item_ptr[index].item_name,
"Status_Return_Level", strlen(
"Status_Return_Level")))
941 mx_state.Status_Return_Level = read_value;
942 else if (!strncmp(item_ptr[index].item_name,
"Alarm_LED", strlen(
"Alarm_LED")))
943 mx_state.Alarm_LED = read_value;
944 else if (!strncmp(item_ptr[index].item_name,
"Shutdown", strlen(
"Shutdown")))
945 mx_state.Shutdown = read_value;
946 else if (!strncmp(item_ptr[index].item_name,
"Multi_Turn_Offset", strlen(
"Multi_Turn_Offset")))
947 mx_state.Multi_Turn_Offset = read_value;
948 else if (!strncmp(item_ptr[index].item_name,
"Resolution_Divider", strlen(
"Resolution_Divider")))
949 mx_state.Resolution_Divider = read_value;
950 else if (!strncmp(item_ptr[index].item_name,
"Torque_Enable", strlen(
"Torque_Enable")))
951 mx_state.Torque_Enable = read_value;
952 else if (!strncmp(item_ptr[index].item_name,
"LED", strlen(
"LED")))
953 mx_state.LED = read_value;
954 else if (!strncmp(item_ptr[index].item_name,
"D_gain", strlen(
"D_gain")))
955 mx_state.D_gain = read_value;
956 else if (!strncmp(item_ptr[index].item_name,
"I_gain", strlen(
"I_gain")))
957 mx_state.I_gain = read_value;
958 else if (!strncmp(item_ptr[index].item_name,
"P_gain", strlen(
"P_gain")))
959 mx_state.P_gain = read_value;
960 else if (!strncmp(item_ptr[index].item_name,
"Goal_Position", strlen(
"Goal_Position")))
961 mx_state.Goal_Position = read_value;
962 else if (!strncmp(item_ptr[index].item_name,
"Moving_Speed", strlen(
"Moving_Speed")))
963 mx_state.Moving_Speed = read_value;
964 else if (!strncmp(item_ptr[index].item_name,
"Torque_Limit", strlen(
"Torque_Limit")))
965 mx_state.Torque_Limit = read_value;
966 else if (!strncmp(item_ptr[index].item_name,
"Present_Position", strlen(
"Present_Position")))
967 mx_state.Present_Position = read_value;
968 else if (!strncmp(item_ptr[index].item_name,
"Present_Speed", strlen(
"Present_Speed")))
969 mx_state.Present_Speed = read_value;
970 else if (!strncmp(item_ptr[index].item_name,
"Present_Load", strlen(
"Present_Load")))
971 mx_state.Present_Load = read_value;
972 else if (!strncmp(item_ptr[index].item_name,
"Present_Voltage", strlen(
"Present_Voltage")))
973 mx_state.Present_Voltage = read_value;
974 else if (!strncmp(item_ptr[index].item_name,
"Present_Temperature", strlen(
"Present_Temperature")))
975 mx_state.Present_Temperature = read_value;
976 else if (!strncmp(item_ptr[index].item_name,
"Registered", strlen(
"Registered")))
977 mx_state.Registered = read_value;
978 else if (!strncmp(item_ptr[index].item_name,
"Moving", strlen(
"Moving")))
979 mx_state.Moving = read_value;
980 else if (!strncmp(item_ptr[index].item_name,
"Lock", strlen(
"Lock")))
981 mx_state.Lock = read_value;
982 else if (!strncmp(item_ptr[index].item_name,
"Punch", strlen(
"Punch")))
983 mx_state.Punch = read_value;
984 else if (!strncmp(item_ptr[index].item_name,
"Goal_Acceleration", strlen(
"Goal_Acceleration")))
985 mx_state.Goal_Acceleration = read_value;
994 dynamixel_workbench_msgs::MXExt mxext_state;
998 uint32_t getAllRegisteredData[last_register_addr+last_register_addr_length] = {0, };
1004 int32_t read_value = 0;
1005 read_value = getAllRegisteredData[item_ptr[index].
address];
1007 switch (item_ptr[index].data_length)
1010 read_value = getAllRegisteredData[item_ptr[index].address];
1014 read_value =
DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address], getAllRegisteredData[item_ptr[index].address+1]);
1018 read_value =
DXL_MAKEDWORD(
DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address], getAllRegisteredData[item_ptr[index].address+1]),
1019 DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address+2], getAllRegisteredData[item_ptr[index].address+3]));
1023 read_value = getAllRegisteredData[item_ptr[index].address];
1027 if (!strncmp(item_ptr[index].item_name,
"Model_Number", strlen(
"Model_Number")))
1028 mxext_state.Model_Number = read_value;
1029 else if (!strncmp(item_ptr[index].item_name,
"Firmware_Version", strlen(
"Firmware_Version")))
1030 mxext_state.Firmware_Version = read_value;
1031 else if (!strncmp(item_ptr[index].item_name,
"ID", strlen(
"ID")))
1032 mxext_state.ID = read_value;
1033 else if (!strncmp(item_ptr[index].item_name,
"Baud_Rate", strlen(
"Baud_Rate")))
1034 mxext_state.Baud_Rate = read_value;
1035 else if (!strncmp(item_ptr[index].item_name,
"Return_Delay_Time", strlen(
"Return_Delay_Time")))
1036 mxext_state.Return_Delay_Time = read_value;
1037 else if (!strncmp(item_ptr[index].item_name,
"CW_Angle_Limit", strlen(
"CW_Angle_Limit")))
1038 mxext_state.CW_Angle_Limit = read_value;
1039 else if (!strncmp(item_ptr[index].item_name,
"CCW_Angle_Limit", strlen(
"CCW_Angle_Limit")))
1040 mxext_state.CCW_Angle_Limit = read_value;
1041 else if (!strncmp(item_ptr[index].item_name,
"Temperature_Limit", strlen(
"Temperature_Limit")))
1042 mxext_state.Temperature_Limit = read_value;
1043 else if (!strncmp(item_ptr[index].item_name,
"Min_Voltage_Limit", strlen(
"Min_Voltage_Limit")))
1044 mxext_state.Min_Voltage_Limit = read_value;
1045 else if (!strncmp(item_ptr[index].item_name,
"Max_Voltage_Limit", strlen(
"Max_Voltage_Limit")))
1046 mxext_state.Max_Voltage_Limit = read_value;
1047 else if (!strncmp(item_ptr[index].item_name,
"Max_Torque", strlen(
"Max_Torque")))
1048 mxext_state.Max_Torque = read_value;
1049 else if (!strncmp(item_ptr[index].item_name,
"Status_Return_Level", strlen(
"Status_Return_Level")))
1050 mxext_state.Status_Return_Level = read_value;
1051 else if (!strncmp(item_ptr[index].item_name,
"Alarm_LED", strlen(
"Alarm_LED")))
1052 mxext_state.Alarm_LED = read_value;
1053 else if (!strncmp(item_ptr[index].item_name,
"Shutdown", strlen(
"Shutdown")))
1054 mxext_state.Shutdown = read_value;
1055 else if (!strncmp(item_ptr[index].item_name,
"Multi_Turn_Offset", strlen(
"Multi_Turn_Offset")))
1056 mxext_state.Multi_Turn_Offset = read_value;
1057 else if (!strncmp(item_ptr[index].item_name,
"Resolution_Divider", strlen(
"Resolution_Divider")))
1058 mxext_state.Resolution_Divider = read_value;
1059 else if (!strncmp(item_ptr[index].item_name,
"Torque_Enable", strlen(
"Torque_Enable")))
1060 mxext_state.Torque_Enable = read_value;
1061 else if (!strncmp(item_ptr[index].item_name,
"LED", strlen(
"LED")))
1062 mxext_state.LED = read_value;
1063 else if (!strncmp(item_ptr[index].item_name,
"D_gain", strlen(
"D_gain")))
1064 mxext_state.D_gain = read_value;
1065 else if (!strncmp(item_ptr[index].item_name,
"I_gain", strlen(
"I_gain")))
1066 mxext_state.I_gain = read_value;
1067 else if (!strncmp(item_ptr[index].item_name,
"P_gain", strlen(
"P_gain")))
1068 mxext_state.P_gain = read_value;
1069 else if (!strncmp(item_ptr[index].item_name,
"Goal_Position", strlen(
"Goal_Position")))
1070 mxext_state.Goal_Position = read_value;
1071 else if (!strncmp(item_ptr[index].item_name,
"Moving_Speed", strlen(
"Moving_Speed")))
1072 mxext_state.Moving_Speed = read_value;
1073 else if (!strncmp(item_ptr[index].item_name,
"Torque_Limit", strlen(
"Torque_Limit")))
1074 mxext_state.Torque_Limit = read_value;
1075 else if (!strncmp(item_ptr[index].item_name,
"Present_Position", strlen(
"Present_Position")))
1076 mxext_state.Present_Position = read_value;
1077 else if (!strncmp(item_ptr[index].item_name,
"Present_Speed", strlen(
"Present_Speed")))
1078 mxext_state.Present_Speed = read_value;
1079 else if (!strncmp(item_ptr[index].item_name,
"Present_Load", strlen(
"Present_Load")))
1080 mxext_state.Present_Load = read_value;
1081 else if (!strncmp(item_ptr[index].item_name,
"Present_Voltage", strlen(
"Present_Voltage")))
1082 mxext_state.Present_Voltage = read_value;
1083 else if (!strncmp(item_ptr[index].item_name,
"Present_Temperature", strlen(
"Present_Temperature")))
1084 mxext_state.Present_Temperature = read_value;
1085 else if (!strncmp(item_ptr[index].item_name,
"Registered", strlen(
"Registered")))
1086 mxext_state.Registered = read_value;
1087 else if (!strncmp(item_ptr[index].item_name,
"Moving", strlen(
"Moving")))
1088 mxext_state.Moving = read_value;
1089 else if (!strncmp(item_ptr[index].item_name,
"Lock", strlen(
"Lock")))
1090 mxext_state.Lock = read_value;
1091 else if (!strncmp(item_ptr[index].item_name,
"Punch", strlen(
"Punch")))
1092 mxext_state.Punch = read_value;
1093 else if (!strncmp(item_ptr[index].item_name,
"Current", strlen(
"Current")))
1094 mxext_state.Current = read_value;
1095 else if (!strncmp(item_ptr[index].item_name,
"Torque_Control_Mode_Enable", strlen(
"Torque_Control_Mode_Enable")))
1096 mxext_state.Torque_Control_Mode_Enable = read_value;
1097 else if (!strncmp(item_ptr[index].item_name,
"Goal_Torque", strlen(
"Goal_Torque")))
1098 mxext_state.Goal_Torque = read_value;
1099 else if (!strncmp(item_ptr[index].item_name,
"Goal_Acceleration", strlen(
"Goal_Acceleration")))
1100 mxext_state.Goal_Acceleration = read_value;
1109 dynamixel_workbench_msgs::MX2 mx2_state;
1113 uint32_t getAllRegisteredData[last_register_addr+last_register_addr_length] = {0, };
1119 int32_t read_value = 0;
1120 read_value = getAllRegisteredData[item_ptr[index].
address];
1122 switch (item_ptr[index].data_length)
1125 read_value = getAllRegisteredData[item_ptr[index].address];
1129 read_value =
DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address], getAllRegisteredData[item_ptr[index].address+1]);
1133 read_value =
DXL_MAKEDWORD(
DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address], getAllRegisteredData[item_ptr[index].address+1]),
1134 DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address+2], getAllRegisteredData[item_ptr[index].address+3]));
1138 read_value = getAllRegisteredData[item_ptr[index].address];
1142 if (!strncmp(item_ptr[index].item_name,
"Model_Number", strlen(
"Model_Number")))
1143 mx2_state.Model_Number = read_value;
1144 else if (!strncmp(item_ptr[index].item_name,
"Firmware_Version", strlen(
"Firmware_Version")))
1145 mx2_state.Firmware_Version = read_value;
1146 else if (!strncmp(item_ptr[index].item_name,
"ID", strlen(
"ID")))
1147 mx2_state.ID = read_value;
1148 else if (!strncmp(item_ptr[index].item_name,
"Baud_Rate", strlen(
"Baud_Rate")))
1149 mx2_state.Baud_Rate = read_value;
1150 else if (!strncmp(item_ptr[index].item_name,
"Return_Delay_Time", strlen(
"Return_Delay_Time")))
1151 mx2_state.Return_Delay_Time = read_value;
1152 else if (!strncmp(item_ptr[index].item_name,
"Drive_Mode", strlen(
"Drive_Mode")))
1153 mx2_state.Drive_Mode = read_value;
1154 else if (!strncmp(item_ptr[index].item_name,
"Operating_Mode", strlen(
"Operating_Mode")))
1155 mx2_state.Operating_Mode = read_value;
1156 else if (!strncmp(item_ptr[index].item_name,
"Secondary_ID", strlen(
"Secondary_ID")))
1157 mx2_state.Secondary_ID = read_value;
1158 else if (!strncmp(item_ptr[index].item_name,
"Protocol_Version", strlen(
"Protocol_Version")))
1159 mx2_state.Protocol_Version = read_value;
1160 else if (!strncmp(item_ptr[index].item_name,
"Homing_Offset", strlen(
"Homing_Offset")))
1161 mx2_state.Homing_Offset = read_value;
1162 else if (!strncmp(item_ptr[index].item_name,
"Moving_Threshold", strlen(
"Moving_Threshold")))
1163 mx2_state.Moving_Threshold = read_value;
1164 else if (!strncmp(item_ptr[index].item_name,
"Temperature_Limit", strlen(
"Temperature_Limit")))
1165 mx2_state.Temperature_Limit = read_value;
1166 else if (!strncmp(item_ptr[index].item_name,
"Max_Voltage_Limit", strlen(
"Max_Voltage_Limit")))
1167 mx2_state.Max_Voltage_Limit = read_value;
1168 else if (!strncmp(item_ptr[index].item_name,
"Min_Voltage_Limit", strlen(
"Min_Voltage_Limit")))
1169 mx2_state.Min_Voltage_Limit = read_value;
1170 else if (!strncmp(item_ptr[index].item_name,
"PWM_Limit", strlen(
"PWM_Limit")))
1171 mx2_state.PWM_Limit = read_value;
1172 else if (!strncmp(item_ptr[index].item_name,
"Acceleration_Limit", strlen(
"Acceleration_Limit")))
1173 mx2_state.Acceleration_Limit = read_value;
1174 else if (!strncmp(item_ptr[index].item_name,
"Velocity_Limit", strlen(
"Velocity_Limit")))
1175 mx2_state.Velocity_Limit = read_value;
1176 else if (!strncmp(item_ptr[index].item_name,
"Max_Position_Limit", strlen(
"Max_Position_Limit")))
1177 mx2_state.Max_Position_Limit = read_value;
1178 else if (!strncmp(item_ptr[index].item_name,
"Min_Position_Limit", strlen(
"Min_Position_Limit")))
1179 mx2_state.Min_Position_Limit = read_value;
1180 else if (!strncmp(item_ptr[index].item_name,
"Shutdown", strlen(
"Shutdown")))
1181 mx2_state.Shutdown = read_value;
1182 else if (!strncmp(item_ptr[index].item_name,
"Torque_Enable", strlen(
"Torque_Enable")))
1183 mx2_state.Torque_Enable = read_value;
1184 else if (!strncmp(item_ptr[index].item_name,
"LED", strlen(
"LED")))
1185 mx2_state.LED = read_value;
1186 else if (!strncmp(item_ptr[index].item_name,
"Status_Return_Level", strlen(
"Status_Return_Level")))
1187 mx2_state.Status_Return_Level = read_value;
1188 else if (!strncmp(item_ptr[index].item_name,
"Registered_Instruction", strlen(
"Registered_Instruction")))
1189 mx2_state.Registered_Instruction = read_value;
1190 else if (!strncmp(item_ptr[index].item_name,
"Hardware_Error_Status", strlen(
"Hardware_Error_Status")))
1191 mx2_state.Hardware_Error_Status = read_value;
1192 else if (!strncmp(item_ptr[index].item_name,
"Velocity_I_Gain", strlen(
"Velocity_I_Gain")))
1193 mx2_state.Velocity_I_Gain = read_value;
1194 else if (!strncmp(item_ptr[index].item_name,
"Velocity_P_Gain", strlen(
"Velocity_P_Gain")))
1195 mx2_state.Velocity_P_Gain = read_value;
1196 else if (!strncmp(item_ptr[index].item_name,
"Position_D_Gain", strlen(
"Position_D_Gain")))
1197 mx2_state.Position_D_Gain = read_value;
1198 else if (!strncmp(item_ptr[index].item_name,
"Position_I_Gain", strlen(
"Position_I_Gain")))
1199 mx2_state.Position_I_Gain = read_value;
1200 else if (!strncmp(item_ptr[index].item_name,
"Position_P_Gain", strlen(
"Position_P_Gain")))
1201 mx2_state.Position_P_Gain = read_value;
1202 else if (!strncmp(item_ptr[index].item_name,
"Feedforward_2nd_Gain", strlen(
"Feedforward_2nd_Gain")))
1203 mx2_state.Feedforward_2nd_Gain = read_value;
1204 else if (!strncmp(item_ptr[index].item_name,
"Feedforward_1st_Gain", strlen(
"Feedforward_1st_Gain")))
1205 mx2_state.Feedforward_1st_Gain = read_value;
1206 else if (!strncmp(item_ptr[index].item_name,
"Bus_Watchdog", strlen(
"Bus_Watchdog")))
1207 mx2_state.Bus_Watchdog = read_value;
1208 else if (!strncmp(item_ptr[index].item_name,
"Goal_PWM", strlen(
"Goal_PWM")))
1209 mx2_state.Goal_PWM = read_value;
1210 else if (!strncmp(item_ptr[index].item_name,
"Goal_Velocity", strlen(
"Goal_Velocity")))
1211 mx2_state.Goal_Velocity = read_value;
1212 else if (!strncmp(item_ptr[index].item_name,
"Profile_Acceleration", strlen(
"Profile_Acceleration")))
1213 mx2_state.Profile_Acceleration = read_value;
1214 else if (!strncmp(item_ptr[index].item_name,
"Profile_Velocity", strlen(
"Profile_Velocity")))
1215 mx2_state.Profile_Velocity = read_value;
1216 else if (!strncmp(item_ptr[index].item_name,
"Goal_Position", strlen(
"Goal_Position")))
1217 mx2_state.Goal_Position = read_value;
1218 else if (!strncmp(item_ptr[index].item_name,
"Realtime_Tick", strlen(
"Realtime_Tick")))
1219 mx2_state.Realtime_Tick = read_value;
1220 else if (!strncmp(item_ptr[index].item_name,
"Moving", strlen(
"Moving")))
1221 mx2_state.Moving = read_value;
1222 else if (!strncmp(item_ptr[index].item_name,
"Moving_Status", strlen(
"Moving_Status")))
1223 mx2_state.Moving_Status = read_value;
1224 else if (!strncmp(item_ptr[index].item_name,
"Present_PWM", strlen(
"Present_PWM")))
1225 mx2_state.Present_PWM = read_value;
1226 else if (!strncmp(item_ptr[index].item_name,
"Present_Load", strlen(
"Present_Load")))
1227 mx2_state.Present_Load = read_value;
1228 else if (!strncmp(item_ptr[index].item_name,
"Present_Velocity", strlen(
"Present_Velocity")))
1229 mx2_state.Present_Velocity = read_value;
1230 else if (!strncmp(item_ptr[index].item_name,
"Present_Position", strlen(
"Present_Position")))
1231 mx2_state.Present_Position = read_value;
1232 else if (!strncmp(item_ptr[index].item_name,
"Velocity_Trajectory", strlen(
"Velocity_Trajectory")))
1233 mx2_state.Velocity_Trajectory = read_value;
1234 else if (!strncmp(item_ptr[index].item_name,
"Position_Trajectory", strlen(
"Position_Trajectory")))
1235 mx2_state.Position_Trajectory = read_value;
1236 else if (!strncmp(item_ptr[index].item_name,
"Present_Input_Voltage", strlen(
"Present_Input_Voltage")))
1237 mx2_state.Present_Input_Voltage = read_value;
1238 else if (!strncmp(item_ptr[index].item_name,
"Present_Temperature", strlen(
"Present_Temperature")))
1239 mx2_state.Present_Temperature = read_value;
1248 dynamixel_workbench_msgs::MX2Ext mx2ext_state;
1252 uint32_t getAllRegisteredData[last_register_addr+last_register_addr_length] = {0, };
1258 int32_t read_value = 0;
1259 read_value = getAllRegisteredData[item_ptr[index].
address];
1261 switch (item_ptr[index].data_length)
1264 read_value = getAllRegisteredData[item_ptr[index].address];
1268 read_value =
DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address], getAllRegisteredData[item_ptr[index].address+1]);
1272 read_value =
DXL_MAKEDWORD(
DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address], getAllRegisteredData[item_ptr[index].address+1]),
1273 DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address+2], getAllRegisteredData[item_ptr[index].address+3]));
1277 read_value = getAllRegisteredData[item_ptr[index].address];
1281 if (!strncmp(item_ptr[index].item_name,
"Model_Number", strlen(
"Model_Number")))
1282 mx2ext_state.Model_Number = read_value;
1283 else if (!strncmp(item_ptr[index].item_name,
"Firmware_Version", strlen(
"Firmware_Version")))
1284 mx2ext_state.Firmware_Version = read_value;
1285 else if (!strncmp(item_ptr[index].item_name,
"ID", strlen(
"ID")))
1286 mx2ext_state.ID = read_value;
1287 else if (!strncmp(item_ptr[index].item_name,
"Baud_Rate", strlen(
"Baud_Rate")))
1288 mx2ext_state.Baud_Rate = read_value;
1289 else if (!strncmp(item_ptr[index].item_name,
"Return_Delay_Time", strlen(
"Return_Delay_Time")))
1290 mx2ext_state.Return_Delay_Time = read_value;
1291 else if (!strncmp(item_ptr[index].item_name,
"Drive_Mode", strlen(
"Drive_Mode")))
1292 mx2ext_state.Drive_Mode = read_value;
1293 else if (!strncmp(item_ptr[index].item_name,
"Operating_Mode", strlen(
"Operating_Mode")))
1294 mx2ext_state.Operating_Mode = read_value;
1295 else if (!strncmp(item_ptr[index].item_name,
"Secondary_ID", strlen(
"Secondary_ID")))
1296 mx2ext_state.Secondary_ID = read_value;
1297 else if (!strncmp(item_ptr[index].item_name,
"Protocol_Version", strlen(
"Protocol_Version")))
1298 mx2ext_state.Protocol_Version = read_value;
1299 else if (!strncmp(item_ptr[index].item_name,
"Homing_Offset", strlen(
"Homing_Offset")))
1300 mx2ext_state.Homing_Offset = read_value;
1301 else if (!strncmp(item_ptr[index].item_name,
"Moving_Threshold", strlen(
"Moving_Threshold")))
1302 mx2ext_state.Moving_Threshold = read_value;
1303 else if (!strncmp(item_ptr[index].item_name,
"Temperature_Limit", strlen(
"Temperature_Limit")))
1304 mx2ext_state.Temperature_Limit = read_value;
1305 else if (!strncmp(item_ptr[index].item_name,
"Max_Voltage_Limit", strlen(
"Max_Voltage_Limit")))
1306 mx2ext_state.Max_Voltage_Limit = read_value;
1307 else if (!strncmp(item_ptr[index].item_name,
"Min_Voltage_Limit", strlen(
"Min_Voltage_Limit")))
1308 mx2ext_state.Min_Voltage_Limit = read_value;
1309 else if (!strncmp(item_ptr[index].item_name,
"PWM_Limit", strlen(
"PWM_Limit")))
1310 mx2ext_state.PWM_Limit = read_value;
1311 else if (!strncmp(item_ptr[index].item_name,
"Current_Limit", strlen(
"Current_Limit")))
1312 mx2ext_state.Current_Limit = read_value;
1313 else if (!strncmp(item_ptr[index].item_name,
"Acceleration_Limit", strlen(
"Acceleration_Limit")))
1314 mx2ext_state.Acceleration_Limit = read_value;
1315 else if (!strncmp(item_ptr[index].item_name,
"Velocity_Limit", strlen(
"Velocity_Limit")))
1316 mx2ext_state.Velocity_Limit = read_value;
1317 else if (!strncmp(item_ptr[index].item_name,
"Max_Position_Limit", strlen(
"Max_Position_Limit")))
1318 mx2ext_state.Max_Position_Limit = read_value;
1319 else if (!strncmp(item_ptr[index].item_name,
"Min_Position_Limit", strlen(
"Min_Position_Limit")))
1320 mx2ext_state.Min_Position_Limit = read_value;
1321 else if (!strncmp(item_ptr[index].item_name,
"Shutdown", strlen(
"Shutdown")))
1322 mx2ext_state.Shutdown = read_value;
1323 else if (!strncmp(item_ptr[index].item_name,
"Torque_Enable", strlen(
"Torque_Enable")))
1324 mx2ext_state.Torque_Enable = read_value;
1325 else if (!strncmp(item_ptr[index].item_name,
"LED", strlen(
"LED")))
1326 mx2ext_state.LED = read_value;
1327 else if (!strncmp(item_ptr[index].item_name,
"Status_Return_Level", strlen(
"Status_Return_Level")))
1328 mx2ext_state.Status_Return_Level = read_value;
1329 else if (!strncmp(item_ptr[index].item_name,
"Registered_Instruction", strlen(
"Registered_Instruction")))
1330 mx2ext_state.Registered_Instruction = read_value;
1331 else if (!strncmp(item_ptr[index].item_name,
"Hardware_Error_Status", strlen(
"Hardware_Error_Status")))
1332 mx2ext_state.Hardware_Error_Status = read_value;
1333 else if (!strncmp(item_ptr[index].item_name,
"Velocity_I_Gain", strlen(
"Velocity_I_Gain")))
1334 mx2ext_state.Velocity_I_Gain = read_value;
1335 else if (!strncmp(item_ptr[index].item_name,
"Velocity_P_Gain", strlen(
"Velocity_P_Gain")))
1336 mx2ext_state.Velocity_P_Gain = read_value;
1337 else if (!strncmp(item_ptr[index].item_name,
"Position_D_Gain", strlen(
"Position_D_Gain")))
1338 mx2ext_state.Position_D_Gain = read_value;
1339 else if (!strncmp(item_ptr[index].item_name,
"Position_I_Gain", strlen(
"Position_I_Gain")))
1340 mx2ext_state.Position_I_Gain = read_value;
1341 else if (!strncmp(item_ptr[index].item_name,
"Position_P_Gain", strlen(
"Position_P_Gain")))
1342 mx2ext_state.Position_P_Gain = read_value;
1343 else if (!strncmp(item_ptr[index].item_name,
"Feedforward_2nd_Gain", strlen(
"Feedforward_2nd_Gain")))
1344 mx2ext_state.Feedforward_2nd_Gain = read_value;
1345 else if (!strncmp(item_ptr[index].item_name,
"Feedforward_1st_Gain", strlen(
"Feedforward_1st_Gain")))
1346 mx2ext_state.Feedforward_1st_Gain = read_value;
1347 else if (!strncmp(item_ptr[index].item_name,
"Bus_Watchdog", strlen(
"Bus_Watchdog")))
1348 mx2ext_state.Bus_Watchdog = read_value;
1349 else if (!strncmp(item_ptr[index].item_name,
"Goal_PWM", strlen(
"Goal_PWM")))
1350 mx2ext_state.Goal_PWM = read_value;
1351 else if (!strncmp(item_ptr[index].item_name,
"Goal_Current", strlen(
"Goal_Current")))
1352 mx2ext_state.Goal_Current = read_value;
1353 else if (!strncmp(item_ptr[index].item_name,
"Goal_Velocity", strlen(
"Goal_Velocity")))
1354 mx2ext_state.Goal_Velocity = read_value;
1355 else if (!strncmp(item_ptr[index].item_name,
"Profile_Acceleration", strlen(
"Profile_Acceleration")))
1356 mx2ext_state.Profile_Acceleration = read_value;
1357 else if (!strncmp(item_ptr[index].item_name,
"Profile_Velocity", strlen(
"Profile_Velocity")))
1358 mx2ext_state.Profile_Velocity = read_value;
1359 else if (!strncmp(item_ptr[index].item_name,
"Goal_Position", strlen(
"Goal_Position")))
1360 mx2ext_state.Goal_Position = read_value;
1361 else if (!strncmp(item_ptr[index].item_name,
"Realtime_Tick", strlen(
"Realtime_Tick")))
1362 mx2ext_state.Realtime_Tick = read_value;
1363 else if (!strncmp(item_ptr[index].item_name,
"Moving", strlen(
"Moving")))
1364 mx2ext_state.Moving = read_value;
1365 else if (!strncmp(item_ptr[index].item_name,
"Moving_Status", strlen(
"Moving_Status")))
1366 mx2ext_state.Moving_Status = read_value;
1367 else if (!strncmp(item_ptr[index].item_name,
"Present_PWM", strlen(
"Present_PWM")))
1368 mx2ext_state.Present_PWM = read_value;
1369 else if (!strncmp(item_ptr[index].item_name,
"Present_Current", strlen(
"Present_Current")))
1370 mx2ext_state.Present_Current = read_value;
1371 else if (!strncmp(item_ptr[index].item_name,
"Present_Velocity", strlen(
"Present_Velocity")))
1372 mx2ext_state.Present_Velocity = read_value;
1373 else if (!strncmp(item_ptr[index].item_name,
"Present_Position", strlen(
"Present_Position")))
1374 mx2ext_state.Present_Position = read_value;
1375 else if (!strncmp(item_ptr[index].item_name,
"Velocity_Trajectory", strlen(
"Velocity_Trajectory")))
1376 mx2ext_state.Velocity_Trajectory = read_value;
1377 else if (!strncmp(item_ptr[index].item_name,
"Position_Trajectory", strlen(
"Position_Trajectory")))
1378 mx2ext_state.Position_Trajectory = read_value;
1379 else if (!strncmp(item_ptr[index].item_name,
"Present_Input_Voltage", strlen(
"Present_Input_Voltage")))
1380 mx2ext_state.Present_Input_Voltage = read_value;
1381 else if (!strncmp(item_ptr[index].item_name,
"Present_Temperature", strlen(
"Present_Temperature")))
1382 mx2ext_state.Present_Temperature = read_value;
1391 dynamixel_workbench_msgs::EX ex_state;
1395 uint32_t getAllRegisteredData[last_register_addr+last_register_addr_length] = {0, };
1401 int32_t read_value = 0;
1402 read_value = getAllRegisteredData[item_ptr[index].
address];
1404 switch (item_ptr[index].data_length)
1407 read_value = getAllRegisteredData[item_ptr[index].address];
1411 read_value =
DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address], getAllRegisteredData[item_ptr[index].address+1]);
1415 read_value =
DXL_MAKEDWORD(
DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address], getAllRegisteredData[item_ptr[index].address+1]),
1416 DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address+2], getAllRegisteredData[item_ptr[index].address+3]));
1420 read_value = getAllRegisteredData[item_ptr[index].address];
1424 if (!strncmp(item_ptr[index].item_name,
"Model_Number", strlen(
"Model_Number")))
1425 ex_state.Model_Number = read_value;
1426 else if (!strncmp(item_ptr[index].item_name,
"Firmware_Version", strlen(
"Firmware_Version")))
1427 ex_state.Firmware_Version = read_value;
1428 else if (!strncmp(item_ptr[index].item_name,
"ID", strlen(
"ID")))
1429 ex_state.ID = read_value;
1430 else if (!strncmp(item_ptr[index].item_name,
"Baud_Rate", strlen(
"Baud_Rate")))
1431 ex_state.Baud_Rate = read_value;
1432 else if (!strncmp(item_ptr[index].item_name,
"Return_Delay_Time", strlen(
"Return_Delay_Time")))
1433 ex_state.Return_Delay_Time = read_value;
1434 else if (!strncmp(item_ptr[index].item_name,
"CW_Angle_Limit", strlen(
"CW_Angle_Limit")))
1435 ex_state.CW_Angle_Limit = read_value;
1436 else if (!strncmp(item_ptr[index].item_name,
"CCW_Angle_Limit", strlen(
"CCW_Angle_Limit")))
1437 ex_state.CCW_Angle_Limit = read_value;
1438 else if (!strncmp(item_ptr[index].item_name,
"Drive_Mode", strlen(
"Drive_Mode")))
1439 ex_state.Drive_Mode = read_value;
1440 else if (!strncmp(item_ptr[index].item_name,
"Temperature_Limit", strlen(
"Temperature_Limit")))
1441 ex_state.Temperature_Limit = read_value;
1442 else if (!strncmp(item_ptr[index].item_name,
"Min_Voltage_Limit", strlen(
"Min_Voltage_Limit")))
1443 ex_state.Min_Voltage_Limit = read_value;
1444 else if (!strncmp(item_ptr[index].item_name,
"Max_Voltage_Limit", strlen(
"Max_Voltage_Limit")))
1445 ex_state.Max_Voltage_Limit = read_value;
1446 else if (!strncmp(item_ptr[index].item_name,
"Max_Torque", strlen(
"Max_Torque")))
1447 ex_state.Max_Torque = read_value;
1448 else if (!strncmp(item_ptr[index].item_name,
"Status_Return_Level", strlen(
"Status_Return_Level")))
1449 ex_state.Status_Return_Level = read_value;
1450 else if (!strncmp(item_ptr[index].item_name,
"Alarm_LED", strlen(
"Alarm_LED")))
1451 ex_state.Alarm_LED = read_value;
1452 else if (!strncmp(item_ptr[index].item_name,
"Shutdown", strlen(
"Shutdown")))
1453 ex_state.Shutdown = read_value;
1454 else if (!strncmp(item_ptr[index].item_name,
"Torque_Enable", strlen(
"Torque_Enable")))
1455 ex_state.Torque_Enable = read_value;
1456 else if (!strncmp(item_ptr[index].item_name,
"LED", strlen(
"LED")))
1457 ex_state.LED = read_value;
1458 else if (!strncmp(item_ptr[index].item_name,
"CW_Compliance_Margin", strlen(
"CW_Compliance_Margin")))
1459 ex_state.CW_Compliance_Margin = read_value;
1460 else if (!strncmp(item_ptr[index].item_name,
"CCW_Compliance_Margin", strlen(
"CCW_Compliance_Margin")))
1461 ex_state.CCW_Compliance_Margin = read_value;
1462 else if (!strncmp(item_ptr[index].item_name,
"CW_Compliance_Slope", strlen(
"CW_Compliance_Slope")))
1463 ex_state.CW_Compliance_Slope = read_value;
1464 else if (!strncmp(item_ptr[index].item_name,
"CCW_Compliance_Slope", strlen(
"CCW_Compliance_Slope")))
1465 ex_state.CCW_Compliance_Slope = read_value;
1466 else if (!strncmp(item_ptr[index].item_name,
"Goal_Position", strlen(
"Goal_Position")))
1467 ex_state.Goal_Position = read_value;
1468 else if (!strncmp(item_ptr[index].item_name,
"Moving_Speed", strlen(
"Moving_Speed")))
1469 ex_state.Moving_Speed = read_value;
1470 else if (!strncmp(item_ptr[index].item_name,
"Torque_Limit", strlen(
"Torque_Limit")))
1471 ex_state.Torque_Limit = read_value;
1472 else if (!strncmp(item_ptr[index].item_name,
"Present_Position", strlen(
"Present_Position")))
1473 ex_state.Present_Position = read_value;
1474 else if (!strncmp(item_ptr[index].item_name,
"Present_Speed", strlen(
"Present_Speed")))
1475 ex_state.Present_Speed = read_value;
1476 else if (!strncmp(item_ptr[index].item_name,
"Present_Load", strlen(
"Present_Load")))
1477 ex_state.Present_Load = read_value;
1478 else if (!strncmp(item_ptr[index].item_name,
"Present_Voltage", strlen(
"Present_Voltage")))
1479 ex_state.Present_Voltage = read_value;
1480 else if (!strncmp(item_ptr[index].item_name,
"Present_Temperature", strlen(
"Present_Temperature")))
1481 ex_state.Present_Temperature = read_value;
1482 else if (!strncmp(item_ptr[index].item_name,
"Registered", strlen(
"Registered")))
1483 ex_state.Registered = read_value;
1484 else if (!strncmp(item_ptr[index].item_name,
"Moving", strlen(
"Moving")))
1485 ex_state.Moving = read_value;
1486 else if (!strncmp(item_ptr[index].item_name,
"Lock", strlen(
"Lock")))
1487 ex_state.Lock = read_value;
1488 else if (!strncmp(item_ptr[index].item_name,
"Punch", strlen(
"Punch")))
1489 ex_state.Punch = read_value;
1490 else if (!strncmp(item_ptr[index].item_name,
"Sensored_Current", strlen(
"Sensored_Current")))
1491 ex_state.Sensored_Current = read_value;
1500 dynamixel_workbench_msgs::XL320 xl320_state;
1504 uint32_t getAllRegisteredData[last_register_addr+last_register_addr_length] = {0, };
1510 int32_t read_value = 0;
1511 read_value = getAllRegisteredData[item_ptr[index].
address];
1513 switch (item_ptr[index].data_length)
1516 read_value = getAllRegisteredData[item_ptr[index].address];
1520 read_value =
DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address], getAllRegisteredData[item_ptr[index].address+1]);
1524 read_value =
DXL_MAKEDWORD(
DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address], getAllRegisteredData[item_ptr[index].address+1]),
1525 DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address+2], getAllRegisteredData[item_ptr[index].address+3]));
1529 read_value = getAllRegisteredData[item_ptr[index].address];
1533 if (!strncmp(item_ptr[index].item_name,
"Model_Number", strlen(
"Model_Number")))
1534 xl320_state.Model_Number = read_value;
1535 else if (!strncmp(item_ptr[index].item_name,
"Firmware_Version", strlen(
"Firmware_Version")))
1536 xl320_state.Firmware_Version = read_value;
1537 else if (!strncmp(item_ptr[index].item_name,
"ID", strlen(
"ID")))
1538 xl320_state.ID = read_value;
1539 else if (!strncmp(item_ptr[index].item_name,
"Baud_Rate", strlen(
"Baud_Rate")))
1540 xl320_state.Baud_Rate = read_value;
1541 else if (!strncmp(item_ptr[index].item_name,
"Return_Delay_Time", strlen(
"Return_Delay_Time")))
1542 xl320_state.Return_Delay_Time = read_value;
1543 else if (!strncmp(item_ptr[index].item_name,
"CW_Angle_Limit", strlen(
"CW_Angle_Limit")))
1544 xl320_state.CW_Angle_Limit = read_value;
1545 else if (!strncmp(item_ptr[index].item_name,
"CCW_Angle_Limit", strlen(
"CCW_Angle_Limit")))
1546 xl320_state.CCW_Angle_Limit = read_value;
1547 else if (!strncmp(item_ptr[index].item_name,
"Temperature_Limit", strlen(
"Temperature_Limit")))
1548 xl320_state.Temperature_Limit = read_value;
1549 else if (!strncmp(item_ptr[index].item_name,
"Min_Voltage_Limit", strlen(
"Min_Voltage_Limit")))
1550 xl320_state.Min_Voltage_Limit = read_value;
1551 else if (!strncmp(item_ptr[index].item_name,
"Max_Voltage_Limit", strlen(
"Max_Voltage_Limit")))
1552 xl320_state.Max_Voltage_Limit = read_value;
1553 else if (!strncmp(item_ptr[index].item_name,
"Max_Torque", strlen(
"Max_Torque")))
1554 xl320_state.Max_Torque = read_value;
1555 else if (!strncmp(item_ptr[index].item_name,
"Status_Return_Level", strlen(
"Status_Return_Level")))
1556 xl320_state.Status_Return_Level = read_value;
1557 else if (!strncmp(item_ptr[index].item_name,
"Shutdown", strlen(
"Shutdown")))
1558 xl320_state.Shutdown = read_value;
1559 else if (!strncmp(item_ptr[index].item_name,
"Torque_Enable", strlen(
"Torque_Enable")))
1560 xl320_state.Torque_Enable = read_value;
1561 else if (!strncmp(item_ptr[index].item_name,
"LED", strlen(
"LED")))
1562 xl320_state.LED = read_value;
1563 else if (!strncmp(item_ptr[index].item_name,
"D_gain", strlen(
"D_gain")))
1564 xl320_state.D_gain = read_value;
1565 else if (!strncmp(item_ptr[index].item_name,
"I_gain", strlen(
"I_gain")))
1566 xl320_state.I_gain = read_value;
1567 else if (!strncmp(item_ptr[index].item_name,
"P_gain", strlen(
"P_gain")))
1568 xl320_state.P_gain = read_value;
1569 else if (!strncmp(item_ptr[index].item_name,
"Goal_Position", strlen(
"Goal_Position")))
1570 xl320_state.Goal_Position = read_value;
1571 else if (!strncmp(item_ptr[index].item_name,
"Moving_Speed", strlen(
"Moving_Speed")))
1572 xl320_state.Moving_Speed = read_value;
1573 else if (!strncmp(item_ptr[index].item_name,
"Torque_Limit", strlen(
"Torque_Limit")))
1574 xl320_state.Torque_Limit = read_value;
1575 else if (!strncmp(item_ptr[index].item_name,
"Present_Position", strlen(
"Present_Position")))
1576 xl320_state.Present_Position = read_value;
1577 else if (!strncmp(item_ptr[index].item_name,
"Present_Speed", strlen(
"Present_Speed")))
1578 xl320_state.Present_Speed = read_value;
1579 else if (!strncmp(item_ptr[index].item_name,
"Present_Load", strlen(
"Present_Load")))
1580 xl320_state.Present_Load = read_value;
1581 else if (!strncmp(item_ptr[index].item_name,
"Present_Voltage", strlen(
"Present_Voltage")))
1582 xl320_state.Present_Voltage = read_value;
1583 else if (!strncmp(item_ptr[index].item_name,
"Present_Temperature", strlen(
"Present_Temperature")))
1584 xl320_state.Present_Temperature = read_value;
1585 else if (!strncmp(item_ptr[index].item_name,
"Registered", strlen(
"Registered")))
1586 xl320_state.Registered = read_value;
1587 else if (!strncmp(item_ptr[index].item_name,
"Moving", strlen(
"Moving")))
1588 xl320_state.Moving = read_value;
1589 else if (!strncmp(item_ptr[index].item_name,
"Hardware_Error_Status", strlen(
"Hardware_Error_Status")))
1590 xl320_state.Hardware_Error_Status = read_value;
1591 else if (!strncmp(item_ptr[index].item_name,
"Punch", strlen(
"Punch")))
1592 xl320_state.Punch = read_value;
1601 dynamixel_workbench_msgs::XL xl_state;
1605 uint32_t getAllRegisteredData[last_register_addr+last_register_addr_length] = {0, };
1611 int32_t read_value = 0;
1612 read_value = getAllRegisteredData[item_ptr[index].
address];
1614 switch (item_ptr[index].data_length)
1617 read_value = getAllRegisteredData[item_ptr[index].address];
1621 read_value =
DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address], getAllRegisteredData[item_ptr[index].address+1]);
1625 read_value =
DXL_MAKEDWORD(
DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address], getAllRegisteredData[item_ptr[index].address+1]),
1626 DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address+2], getAllRegisteredData[item_ptr[index].address+3]));
1630 read_value = getAllRegisteredData[item_ptr[index].address];
1634 if (!strncmp(item_ptr[index].item_name,
"Model_Number", strlen(
"Model_Number")))
1635 xl_state.Model_Number = read_value;
1636 else if (!strncmp(item_ptr[index].item_name,
"Firmware_Version", strlen(
"Firmware_Version")))
1637 xl_state.Firmware_Version = read_value;
1638 else if (!strncmp(item_ptr[index].item_name,
"ID", strlen(
"ID")))
1639 xl_state.ID = read_value;
1640 else if (!strncmp(item_ptr[index].item_name,
"Baud_Rate", strlen(
"Baud_Rate")))
1641 xl_state.Baud_Rate = read_value;
1642 else if (!strncmp(item_ptr[index].item_name,
"Return_Delay_Time", strlen(
"Return_Delay_Time")))
1643 xl_state.Return_Delay_Time = read_value;
1644 else if (!strncmp(item_ptr[index].item_name,
"Drive_Mode", strlen(
"Drive_Mode")))
1645 xl_state.Drive_Mode = read_value;
1646 else if (!strncmp(item_ptr[index].item_name,
"Operating_Mode", strlen(
"Operating_Mode")))
1647 xl_state.Operating_Mode = read_value;
1648 else if (!strncmp(item_ptr[index].item_name,
"Secondary_ID", strlen(
"Secondary_ID")))
1649 xl_state.Secondary_ID = read_value;
1650 else if (!strncmp(item_ptr[index].item_name,
"Protocol_Version", strlen(
"Protocol_Version")))
1651 xl_state.Protocol_Version = read_value;
1652 else if (!strncmp(item_ptr[index].item_name,
"Homing_Offset", strlen(
"Homing_Offset")))
1653 xl_state.Homing_Offset = read_value;
1654 else if (!strncmp(item_ptr[index].item_name,
"Moving_Threshold", strlen(
"Moving_Threshold")))
1655 xl_state.Moving_Threshold = read_value;
1656 else if (!strncmp(item_ptr[index].item_name,
"Temperature_Limit", strlen(
"Temperature_Limit")))
1657 xl_state.Temperature_Limit = read_value;
1658 else if (!strncmp(item_ptr[index].item_name,
"Max_Voltage_Limit", strlen(
"Max_Voltage_Limit")))
1659 xl_state.Max_Voltage_Limit = read_value;
1660 else if (!strncmp(item_ptr[index].item_name,
"Min_Voltage_Limit", strlen(
"Min_Voltage_Limit")))
1661 xl_state.Min_Voltage_Limit = read_value;
1662 else if (!strncmp(item_ptr[index].item_name,
"PWM_Limit", strlen(
"PWM_Limit")))
1663 xl_state.PWM_Limit = read_value;
1664 else if (!strncmp(item_ptr[index].item_name,
"Acceleration_Limit", strlen(
"Acceleration_Limit")))
1665 xl_state.Acceleration_Limit = read_value;
1666 else if (!strncmp(item_ptr[index].item_name,
"Velocity_Limit", strlen(
"Velocity_Limit")))
1667 xl_state.Velocity_Limit = read_value;
1668 else if (!strncmp(item_ptr[index].item_name,
"Max_Position_Limit", strlen(
"Max_Position_Limit")))
1669 xl_state.Max_Position_Limit = read_value;
1670 else if (!strncmp(item_ptr[index].item_name,
"Min_Position_Limit", strlen(
"Min_Position_Limit")))
1671 xl_state.Min_Position_Limit = read_value;
1672 else if (!strncmp(item_ptr[index].item_name,
"Shutdown", strlen(
"Shutdown")))
1673 xl_state.Shutdown = read_value;
1674 else if (!strncmp(item_ptr[index].item_name,
"Torque_Enable", strlen(
"Torque_Enable")))
1675 xl_state.Torque_Enable = read_value;
1676 else if (!strncmp(item_ptr[index].item_name,
"LED", strlen(
"LED")))
1677 xl_state.LED = read_value;
1678 else if (!strncmp(item_ptr[index].item_name,
"Status_Return_Level", strlen(
"Status_Return_Level")))
1679 xl_state.Status_Return_Level = read_value;
1680 else if (!strncmp(item_ptr[index].item_name,
"Registered_Instruction", strlen(
"Registered_Instruction")))
1681 xl_state.Registered_Instruction = read_value;
1682 else if (!strncmp(item_ptr[index].item_name,
"Hardware_Error_Status", strlen(
"Hardware_Error_Status")))
1683 xl_state.Hardware_Error_Status = read_value;
1684 else if (!strncmp(item_ptr[index].item_name,
"Velocity_I_Gain", strlen(
"Velocity_I_Gain")))
1685 xl_state.Velocity_I_Gain = read_value;
1686 else if (!strncmp(item_ptr[index].item_name,
"Velocity_P_Gain", strlen(
"Velocity_P_Gain")))
1687 xl_state.Velocity_P_Gain = read_value;
1688 else if (!strncmp(item_ptr[index].item_name,
"Position_D_Gain", strlen(
"Position_D_Gain")))
1689 xl_state.Position_D_Gain = read_value;
1690 else if (!strncmp(item_ptr[index].item_name,
"Position_I_Gain", strlen(
"Position_I_Gain")))
1691 xl_state.Position_I_Gain = read_value;
1692 else if (!strncmp(item_ptr[index].item_name,
"Position_P_Gain", strlen(
"Position_P_Gain")))
1693 xl_state.Position_P_Gain = read_value;
1694 else if (!strncmp(item_ptr[index].item_name,
"Feedforward_2nd_Gain", strlen(
"Feedforward_2nd_Gain")))
1695 xl_state.Feedforward_2nd_Gain = read_value;
1696 else if (!strncmp(item_ptr[index].item_name,
"Feedforward_1st_Gain", strlen(
"Feedforward_1st_Gain")))
1697 xl_state.Feedforward_1st_Gain = read_value;
1698 else if (!strncmp(item_ptr[index].item_name,
"Bus_Watchdog", strlen(
"Bus_Watchdog")))
1699 xl_state.Bus_Watchdog = read_value;
1700 else if (!strncmp(item_ptr[index].item_name,
"Goal_PWM", strlen(
"Goal_PWM")))
1701 xl_state.Goal_PWM = read_value;
1702 else if (!strncmp(item_ptr[index].item_name,
"Goal_Velocity", strlen(
"Goal_Velocity")))
1703 xl_state.Goal_Velocity = read_value;
1704 else if (!strncmp(item_ptr[index].item_name,
"Profile_Acceleration", strlen(
"Profile_Acceleration")))
1705 xl_state.Profile_Acceleration = read_value;
1706 else if (!strncmp(item_ptr[index].item_name,
"Profile_Velocity", strlen(
"Profile_Velocity")))
1707 xl_state.Profile_Velocity = read_value;
1708 else if (!strncmp(item_ptr[index].item_name,
"Goal_Position", strlen(
"Goal_Position")))
1709 xl_state.Goal_Position = read_value;
1710 else if (!strncmp(item_ptr[index].item_name,
"Realtime_Tick", strlen(
"Realtime_Tick")))
1711 xl_state.Realtime_Tick = read_value;
1712 else if (!strncmp(item_ptr[index].item_name,
"Moving", strlen(
"Moving")))
1713 xl_state.Moving = read_value;
1714 else if (!strncmp(item_ptr[index].item_name,
"Moving_Status", strlen(
"Moving_Status")))
1715 xl_state.Moving_Status = read_value;
1716 else if (!strncmp(item_ptr[index].item_name,
"Present_PWM", strlen(
"Present_PWM")))
1717 xl_state.Present_PWM = read_value;
1718 else if (!strncmp(item_ptr[index].item_name,
"Present_Load", strlen(
"Present_Load")))
1719 xl_state.Present_Load = read_value;
1720 else if (!strncmp(item_ptr[index].item_name,
"Present_Velocity", strlen(
"Present_Velocity")))
1721 xl_state.Present_Velocity = read_value;
1722 else if (!strncmp(item_ptr[index].item_name,
"Present_Position", strlen(
"Present_Position")))
1723 xl_state.Present_Position = read_value;
1724 else if (!strncmp(item_ptr[index].item_name,
"Velocity_Trajectory", strlen(
"Velocity_Trajectory")))
1725 xl_state.Velocity_Trajectory = read_value;
1726 else if (!strncmp(item_ptr[index].item_name,
"Position_Trajectory", strlen(
"Position_Trajectory")))
1727 xl_state.Position_Trajectory = read_value;
1728 else if (!strncmp(item_ptr[index].item_name,
"Present_Input_Voltage", strlen(
"Present_Input_Voltage")))
1729 xl_state.Present_Input_Voltage = read_value;
1730 else if (!strncmp(item_ptr[index].item_name,
"Present_Temperature", strlen(
"Present_Temperature")))
1731 xl_state.Present_Temperature = read_value;
1740 dynamixel_workbench_msgs::XM xm_state;
1744 uint32_t getAllRegisteredData[last_register_addr+last_register_addr_length] = {0, };
1750 int32_t read_value = 0;
1751 read_value = getAllRegisteredData[item_ptr[index].
address];
1753 switch (item_ptr[index].data_length)
1756 read_value = getAllRegisteredData[item_ptr[index].address];
1760 read_value =
DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address], getAllRegisteredData[item_ptr[index].address+1]);
1764 read_value =
DXL_MAKEDWORD(
DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address], getAllRegisteredData[item_ptr[index].address+1]),
1765 DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address+2], getAllRegisteredData[item_ptr[index].address+3]));
1769 read_value = getAllRegisteredData[item_ptr[index].address];
1773 if (!strncmp(item_ptr[index].item_name,
"Model_Number", strlen(
"Model_Number")))
1774 xm_state.Model_Number = read_value;
1775 else if (!strncmp(item_ptr[index].item_name,
"Firmware_Version", strlen(
"Firmware_Version")))
1776 xm_state.Firmware_Version = read_value;
1777 else if (!strncmp(item_ptr[index].item_name,
"ID", strlen(
"ID")))
1778 xm_state.ID = read_value;
1779 else if (!strncmp(item_ptr[index].item_name,
"Baud_Rate", strlen(
"Baud_Rate")))
1780 xm_state.Baud_Rate = read_value;
1781 else if (!strncmp(item_ptr[index].item_name,
"Return_Delay_Time", strlen(
"Return_Delay_Time")))
1782 xm_state.Return_Delay_Time = read_value;
1783 else if (!strncmp(item_ptr[index].item_name,
"Drive_Mode", strlen(
"Drive_Mode")))
1784 xm_state.Drive_Mode = read_value;
1785 else if (!strncmp(item_ptr[index].item_name,
"Operating_Mode", strlen(
"Operating_Mode")))
1786 xm_state.Operating_Mode = read_value;
1787 else if (!strncmp(item_ptr[index].item_name,
"Secondary_ID", strlen(
"Secondary_ID")))
1788 xm_state.Secondary_ID = read_value;
1789 else if (!strncmp(item_ptr[index].item_name,
"Protocol_Version", strlen(
"Protocol_Version")))
1790 xm_state.Protocol_Version = read_value;
1791 else if (!strncmp(item_ptr[index].item_name,
"Homing_Offset", strlen(
"Homing_Offset")))
1792 xm_state.Homing_Offset = read_value;
1793 else if (!strncmp(item_ptr[index].item_name,
"Moving_Threshold", strlen(
"Moving_Threshold")))
1794 xm_state.Moving_Threshold = read_value;
1795 else if (!strncmp(item_ptr[index].item_name,
"Temperature_Limit", strlen(
"Temperature_Limit")))
1796 xm_state.Temperature_Limit = read_value;
1797 else if (!strncmp(item_ptr[index].item_name,
"Max_Voltage_Limit", strlen(
"Max_Voltage_Limit")))
1798 xm_state.Max_Voltage_Limit = read_value;
1799 else if (!strncmp(item_ptr[index].item_name,
"Min_Voltage_Limit", strlen(
"Min_Voltage_Limit")))
1800 xm_state.Min_Voltage_Limit = read_value;
1801 else if (!strncmp(item_ptr[index].item_name,
"PWM_Limit", strlen(
"PWM_Limit")))
1802 xm_state.PWM_Limit = read_value;
1803 else if (!strncmp(item_ptr[index].item_name,
"Current_Limit", strlen(
"Current_Limit")))
1804 xm_state.Current_Limit = read_value;
1805 else if (!strncmp(item_ptr[index].item_name,
"Acceleration_Limit", strlen(
"Acceleration_Limit")))
1806 xm_state.Acceleration_Limit = read_value;
1807 else if (!strncmp(item_ptr[index].item_name,
"Velocity_Limit", strlen(
"Velocity_Limit")))
1808 xm_state.Velocity_Limit = read_value;
1809 else if (!strncmp(item_ptr[index].item_name,
"Max_Position_Limit", strlen(
"Max_Position_Limit")))
1810 xm_state.Max_Position_Limit = read_value;
1811 else if (!strncmp(item_ptr[index].item_name,
"Min_Position_Limit", strlen(
"Min_Position_Limit")))
1812 xm_state.Min_Position_Limit = read_value;
1813 else if (!strncmp(item_ptr[index].item_name,
"Shutdown", strlen(
"Shutdown")))
1814 xm_state.Shutdown = read_value;
1815 else if (!strncmp(item_ptr[index].item_name,
"Torque_Enable", strlen(
"Torque_Enable")))
1816 xm_state.Torque_Enable = read_value;
1817 else if (!strncmp(item_ptr[index].item_name,
"LED", strlen(
"LED")))
1818 xm_state.LED = read_value;
1819 else if (!strncmp(item_ptr[index].item_name,
"Status_Return_Level", strlen(
"Status_Return_Level")))
1820 xm_state.Status_Return_Level = read_value;
1821 else if (!strncmp(item_ptr[index].item_name,
"Registered_Instruction", strlen(
"Registered_Instruction")))
1822 xm_state.Registered_Instruction = read_value;
1823 else if (!strncmp(item_ptr[index].item_name,
"Hardware_Error_Status", strlen(
"Hardware_Error_Status")))
1824 xm_state.Hardware_Error_Status = read_value;
1825 else if (!strncmp(item_ptr[index].item_name,
"Velocity_I_Gain", strlen(
"Velocity_I_Gain")))
1826 xm_state.Velocity_I_Gain = read_value;
1827 else if (!strncmp(item_ptr[index].item_name,
"Velocity_P_Gain", strlen(
"Velocity_P_Gain")))
1828 xm_state.Velocity_P_Gain = read_value;
1829 else if (!strncmp(item_ptr[index].item_name,
"Position_D_Gain", strlen(
"Position_D_Gain")))
1830 xm_state.Position_D_Gain = read_value;
1831 else if (!strncmp(item_ptr[index].item_name,
"Position_I_Gain", strlen(
"Position_I_Gain")))
1832 xm_state.Position_I_Gain = read_value;
1833 else if (!strncmp(item_ptr[index].item_name,
"Position_P_Gain", strlen(
"Position_P_Gain")))
1834 xm_state.Position_P_Gain = read_value;
1835 else if (!strncmp(item_ptr[index].item_name,
"Feedforward_2nd_Gain", strlen(
"Feedforward_2nd_Gain")))
1836 xm_state.Feedforward_2nd_Gain = read_value;
1837 else if (!strncmp(item_ptr[index].item_name,
"Feedforward_1st_Gain", strlen(
"Feedforward_1st_Gain")))
1838 xm_state.Feedforward_1st_Gain = read_value;
1839 else if (!strncmp(item_ptr[index].item_name,
"Bus_Watchdog", strlen(
"Bus_Watchdog")))
1840 xm_state.Bus_Watchdog = read_value;
1841 else if (!strncmp(item_ptr[index].item_name,
"Goal_PWM", strlen(
"Goal_PWM")))
1842 xm_state.Goal_PWM = read_value;
1843 else if (!strncmp(item_ptr[index].item_name,
"Goal_Current", strlen(
"Goal_Current")))
1844 xm_state.Goal_Current = read_value;
1845 else if (!strncmp(item_ptr[index].item_name,
"Goal_Velocity", strlen(
"Goal_Velocity")))
1846 xm_state.Goal_Velocity = read_value;
1847 else if (!strncmp(item_ptr[index].item_name,
"Profile_Acceleration", strlen(
"Profile_Acceleration")))
1848 xm_state.Profile_Acceleration = read_value;
1849 else if (!strncmp(item_ptr[index].item_name,
"Profile_Velocity", strlen(
"Profile_Velocity")))
1850 xm_state.Profile_Velocity = read_value;
1851 else if (!strncmp(item_ptr[index].item_name,
"Goal_Position", strlen(
"Goal_Position")))
1852 xm_state.Goal_Position = read_value;
1853 else if (!strncmp(item_ptr[index].item_name,
"Realtime_Tick", strlen(
"Realtime_Tick")))
1854 xm_state.Realtime_Tick = read_value;
1855 else if (!strncmp(item_ptr[index].item_name,
"Moving", strlen(
"Moving")))
1856 xm_state.Moving = read_value;
1857 else if (!strncmp(item_ptr[index].item_name,
"Moving_Status", strlen(
"Moving_Status")))
1858 xm_state.Moving_Status = read_value;
1859 else if (!strncmp(item_ptr[index].item_name,
"Present_PWM", strlen(
"Present_PWM")))
1860 xm_state.Present_PWM = read_value;
1861 else if (!strncmp(item_ptr[index].item_name,
"Present_Current", strlen(
"Present_Current")))
1862 xm_state.Present_Current = read_value;
1863 else if (!strncmp(item_ptr[index].item_name,
"Present_Velocity", strlen(
"Present_Velocity")))
1864 xm_state.Present_Velocity = read_value;
1865 else if (!strncmp(item_ptr[index].item_name,
"Present_Position", strlen(
"Present_Position")))
1866 xm_state.Present_Position = read_value;
1867 else if (!strncmp(item_ptr[index].item_name,
"Velocity_Trajectory", strlen(
"Velocity_Trajectory")))
1868 xm_state.Velocity_Trajectory = read_value;
1869 else if (!strncmp(item_ptr[index].item_name,
"Position_Trajectory", strlen(
"Position_Trajectory")))
1870 xm_state.Position_Trajectory = read_value;
1871 else if (!strncmp(item_ptr[index].item_name,
"Present_Input_Voltage", strlen(
"Present_Input_Voltage")))
1872 xm_state.Present_Input_Voltage = read_value;
1873 else if (!strncmp(item_ptr[index].item_name,
"Present_Temperature", strlen(
"Present_Temperature")))
1874 xm_state.Present_Temperature = read_value;
1883 dynamixel_workbench_msgs::XMExt xmext_state;
1887 uint32_t getAllRegisteredData[last_register_addr+last_register_addr_length] = {0, };
1893 int32_t read_value = 0;
1894 read_value = getAllRegisteredData[item_ptr[index].
address];
1896 switch (item_ptr[index].data_length)
1899 read_value = getAllRegisteredData[item_ptr[index].address];
1903 read_value =
DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address], getAllRegisteredData[item_ptr[index].address+1]);
1907 read_value =
DXL_MAKEDWORD(
DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address], getAllRegisteredData[item_ptr[index].address+1]),
1908 DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address+2], getAllRegisteredData[item_ptr[index].address+3]));
1912 read_value = getAllRegisteredData[item_ptr[index].address];
1916 if (!strncmp(item_ptr[index].item_name,
"Model_Number", strlen(
"Model_Number")))
1917 xmext_state.Model_Number = read_value;
1918 else if (!strncmp(item_ptr[index].item_name,
"Firmware_Version", strlen(
"Firmware_Version")))
1919 xmext_state.Firmware_Version = read_value;
1920 else if (!strncmp(item_ptr[index].item_name,
"ID", strlen(
"ID")))
1921 xmext_state.ID = read_value;
1922 else if (!strncmp(item_ptr[index].item_name,
"Baud_Rate", strlen(
"Baud_Rate")))
1923 xmext_state.Baud_Rate = read_value;
1924 else if (!strncmp(item_ptr[index].item_name,
"Return_Delay_Time", strlen(
"Return_Delay_Time")))
1925 xmext_state.Return_Delay_Time = read_value;
1926 else if (!strncmp(item_ptr[index].item_name,
"Drive_Mode", strlen(
"Drive_Mode")))
1927 xmext_state.Drive_Mode = read_value;
1928 else if (!strncmp(item_ptr[index].item_name,
"Operating_Mode", strlen(
"Operating_Mode")))
1929 xmext_state.Operating_Mode = read_value;
1930 else if (!strncmp(item_ptr[index].item_name,
"Secondary_ID", strlen(
"Secondary_ID")))
1931 xmext_state.Secondary_ID = read_value;
1932 else if (!strncmp(item_ptr[index].item_name,
"Protocol_Version", strlen(
"Protocol_Version")))
1933 xmext_state.Protocol_Version = read_value;
1934 else if (!strncmp(item_ptr[index].item_name,
"Homing_Offset", strlen(
"Homing_Offset")))
1935 xmext_state.Homing_Offset = read_value;
1936 else if (!strncmp(item_ptr[index].item_name,
"Moving_Threshold", strlen(
"Moving_Threshold")))
1937 xmext_state.Moving_Threshold = read_value;
1938 else if (!strncmp(item_ptr[index].item_name,
"Temperature_Limit", strlen(
"Temperature_Limit")))
1939 xmext_state.Temperature_Limit = read_value;
1940 else if (!strncmp(item_ptr[index].item_name,
"Max_Voltage_Limit", strlen(
"Max_Voltage_Limit")))
1941 xmext_state.Max_Voltage_Limit = read_value;
1942 else if (!strncmp(item_ptr[index].item_name,
"Min_Voltage_Limit", strlen(
"Min_Voltage_Limit")))
1943 xmext_state.Min_Voltage_Limit = read_value;
1944 else if (!strncmp(item_ptr[index].item_name,
"PWM_Limit", strlen(
"PWM_Limit")))
1945 xmext_state.PWM_Limit = read_value;
1946 else if (!strncmp(item_ptr[index].item_name,
"Current_Limit", strlen(
"Current_Limit")))
1947 xmext_state.Current_Limit = read_value;
1948 else if (!strncmp(item_ptr[index].item_name,
"Acceleration_Limit", strlen(
"Acceleration_Limit")))
1949 xmext_state.Acceleration_Limit = read_value;
1950 else if (!strncmp(item_ptr[index].item_name,
"Velocity_Limit", strlen(
"Velocity_Limit")))
1951 xmext_state.Velocity_Limit = read_value;
1952 else if (!strncmp(item_ptr[index].item_name,
"Max_Position_Limit", strlen(
"Max_Position_Limit")))
1953 xmext_state.Max_Position_Limit = read_value;
1954 else if (!strncmp(item_ptr[index].item_name,
"Min_Position_Limit", strlen(
"Min_Position_Limit")))
1955 xmext_state.Min_Position_Limit = read_value;
1956 else if (!strncmp(item_ptr[index].item_name,
"External_Port_Mode_1", strlen(
"External_Port_Mode_1")))
1957 xmext_state.External_Port_Mode_1 = read_value;
1958 else if (!strncmp(item_ptr[index].item_name,
"External_Port_Mode_2", strlen(
"External_Port_Mode_2")))
1959 xmext_state.External_Port_Mode_2 = read_value;
1960 else if (!strncmp(item_ptr[index].item_name,
"External_Port_Mode_3", strlen(
"External_Port_Mode_3")))
1961 xmext_state.External_Port_Mode_3 = read_value;
1962 else if (!strncmp(item_ptr[index].item_name,
"Shutdown", strlen(
"Shutdown")))
1963 xmext_state.Shutdown = read_value;
1964 else if (!strncmp(item_ptr[index].item_name,
"Torque_Enable", strlen(
"Torque_Enable")))
1965 xmext_state.Torque_Enable = read_value;
1966 else if (!strncmp(item_ptr[index].item_name,
"LED", strlen(
"LED")))
1967 xmext_state.LED = read_value;
1968 else if (!strncmp(item_ptr[index].item_name,
"Status_Return_Level", strlen(
"Status_Return_Level")))
1969 xmext_state.Status_Return_Level = read_value;
1970 else if (!strncmp(item_ptr[index].item_name,
"Registered_Instruction", strlen(
"Registered_Instruction")))
1971 xmext_state.Registered_Instruction = read_value;
1972 else if (!strncmp(item_ptr[index].item_name,
"Hardware_Error_Status", strlen(
"Hardware_Error_Status")))
1973 xmext_state.Hardware_Error_Status = read_value;
1974 else if (!strncmp(item_ptr[index].item_name,
"Velocity_I_Gain", strlen(
"Velocity_I_Gain")))
1975 xmext_state.Velocity_I_Gain = read_value;
1976 else if (!strncmp(item_ptr[index].item_name,
"Velocity_P_Gain", strlen(
"Velocity_P_Gain")))
1977 xmext_state.Velocity_P_Gain = read_value;
1978 else if (!strncmp(item_ptr[index].item_name,
"Position_D_Gain", strlen(
"Position_D_Gain")))
1979 xmext_state.Position_D_Gain = read_value;
1980 else if (!strncmp(item_ptr[index].item_name,
"Position_I_Gain", strlen(
"Position_I_Gain")))
1981 xmext_state.Position_I_Gain = read_value;
1982 else if (!strncmp(item_ptr[index].item_name,
"Position_P_Gain", strlen(
"Position_P_Gain")))
1983 xmext_state.Position_P_Gain = read_value;
1984 else if (!strncmp(item_ptr[index].item_name,
"Feedforward_2nd_Gain", strlen(
"Feedforward_2nd_Gain")))
1985 xmext_state.Feedforward_2nd_Gain = read_value;
1986 else if (!strncmp(item_ptr[index].item_name,
"Feedforward_1st_Gain", strlen(
"Feedforward_1st_Gain")))
1987 xmext_state.Feedforward_1st_Gain = read_value;
1988 else if (!strncmp(item_ptr[index].item_name,
"Bus_Watchdog", strlen(
"Bus_Watchdog")))
1989 xmext_state.Bus_Watchdog = read_value;
1990 else if (!strncmp(item_ptr[index].item_name,
"Goal_PWM", strlen(
"Goal_PWM")))
1991 xmext_state.Goal_PWM = read_value;
1992 else if (!strncmp(item_ptr[index].item_name,
"Goal_Current", strlen(
"Goal_Current")))
1993 xmext_state.Goal_Current = read_value;
1994 else if (!strncmp(item_ptr[index].item_name,
"Goal_Velocity", strlen(
"Goal_Velocity")))
1995 xmext_state.Goal_Velocity = read_value;
1996 else if (!strncmp(item_ptr[index].item_name,
"Profile_Acceleration", strlen(
"Profile_Acceleration")))
1997 xmext_state.Profile_Acceleration = read_value;
1998 else if (!strncmp(item_ptr[index].item_name,
"Profile_Velocity", strlen(
"Profile_Velocity")))
1999 xmext_state.Profile_Velocity = read_value;
2000 else if (!strncmp(item_ptr[index].item_name,
"Goal_Position", strlen(
"Goal_Position")))
2001 xmext_state.Goal_Position = read_value;
2002 else if (!strncmp(item_ptr[index].item_name,
"Realtime_Tick", strlen(
"Realtime_Tick")))
2003 xmext_state.Realtime_Tick = read_value;
2004 else if (!strncmp(item_ptr[index].item_name,
"Moving", strlen(
"Moving")))
2005 xmext_state.Moving = read_value;
2006 else if (!strncmp(item_ptr[index].item_name,
"Moving_Status", strlen(
"Moving_Status")))
2007 xmext_state.Moving_Status = read_value;
2008 else if (!strncmp(item_ptr[index].item_name,
"Present_PWM", strlen(
"Present_PWM")))
2009 xmext_state.Present_PWM = read_value;
2010 else if (!strncmp(item_ptr[index].item_name,
"Present_Current", strlen(
"Present_Current")))
2011 xmext_state.Present_Current = read_value;
2012 else if (!strncmp(item_ptr[index].item_name,
"Present_Velocity", strlen(
"Present_Velocity")))
2013 xmext_state.Present_Velocity = read_value;
2014 else if (!strncmp(item_ptr[index].item_name,
"Present_Position", strlen(
"Present_Position")))
2015 xmext_state.Present_Position = read_value;
2016 else if (!strncmp(item_ptr[index].item_name,
"Velocity_Trajectory", strlen(
"Velocity_Trajectory")))
2017 xmext_state.Velocity_Trajectory = read_value;
2018 else if (!strncmp(item_ptr[index].item_name,
"Position_Trajectory", strlen(
"Position_Trajectory")))
2019 xmext_state.Position_Trajectory = read_value;
2020 else if (!strncmp(item_ptr[index].item_name,
"Present_Input_Voltage", strlen(
"Present_Input_Voltage")))
2021 xmext_state.Present_Input_Voltage = read_value;
2022 else if (!strncmp(item_ptr[index].item_name,
"Present_Temperature", strlen(
"Present_Temperature")))
2023 xmext_state.Present_Temperature = read_value;
2032 dynamixel_workbench_msgs::XH xh_state;
2036 uint32_t getAllRegisteredData[last_register_addr+last_register_addr_length] = {0, };
2042 int32_t read_value = 0;
2043 read_value = getAllRegisteredData[item_ptr[index].
address];
2045 switch (item_ptr[index].data_length)
2048 read_value = getAllRegisteredData[item_ptr[index].address];
2052 read_value =
DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address], getAllRegisteredData[item_ptr[index].address+1]);
2056 read_value =
DXL_MAKEDWORD(
DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address], getAllRegisteredData[item_ptr[index].address+1]),
2057 DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address+2], getAllRegisteredData[item_ptr[index].address+3]));
2061 read_value = getAllRegisteredData[item_ptr[index].address];
2065 if (!strncmp(item_ptr[index].item_name,
"Model_Number", strlen(
"Model_Number")))
2066 xh_state.Model_Number = read_value;
2067 else if (!strncmp(item_ptr[index].item_name,
"Firmware_Version", strlen(
"Firmware_Version")))
2068 xh_state.Firmware_Version = read_value;
2069 else if (!strncmp(item_ptr[index].item_name,
"ID", strlen(
"ID")))
2070 xh_state.ID = read_value;
2071 else if (!strncmp(item_ptr[index].item_name,
"Baud_Rate", strlen(
"Baud_Rate")))
2072 xh_state.Baud_Rate = read_value;
2073 else if (!strncmp(item_ptr[index].item_name,
"Return_Delay_Time", strlen(
"Return_Delay_Time")))
2074 xh_state.Return_Delay_Time = read_value;
2075 else if (!strncmp(item_ptr[index].item_name,
"Drive_Mode", strlen(
"Drive_Mode")))
2076 xh_state.Drive_Mode = read_value;
2077 else if (!strncmp(item_ptr[index].item_name,
"Operating_Mode", strlen(
"Operating_Mode")))
2078 xh_state.Operating_Mode = read_value;
2079 else if (!strncmp(item_ptr[index].item_name,
"Secondary_ID", strlen(
"Secondary_ID")))
2080 xh_state.Secondary_ID = read_value;
2081 else if (!strncmp(item_ptr[index].item_name,
"Protocol_Version", strlen(
"Protocol_Version")))
2082 xh_state.Protocol_Version = read_value;
2083 else if (!strncmp(item_ptr[index].item_name,
"Homing_Offset", strlen(
"Homing_Offset")))
2084 xh_state.Homing_Offset = read_value;
2085 else if (!strncmp(item_ptr[index].item_name,
"Moving_Threshold", strlen(
"Moving_Threshold")))
2086 xh_state.Moving_Threshold = read_value;
2087 else if (!strncmp(item_ptr[index].item_name,
"Temperature_Limit", strlen(
"Temperature_Limit")))
2088 xh_state.Temperature_Limit = read_value;
2089 else if (!strncmp(item_ptr[index].item_name,
"Max_Voltage_Limit", strlen(
"Max_Voltage_Limit")))
2090 xh_state.Max_Voltage_Limit = read_value;
2091 else if (!strncmp(item_ptr[index].item_name,
"Min_Voltage_Limit", strlen(
"Min_Voltage_Limit")))
2092 xh_state.Min_Voltage_Limit = read_value;
2093 else if (!strncmp(item_ptr[index].item_name,
"PWM_Limit", strlen(
"PWM_Limit")))
2094 xh_state.PWM_Limit = read_value;
2095 else if (!strncmp(item_ptr[index].item_name,
"Current_Limit", strlen(
"Current_Limit")))
2096 xh_state.Current_Limit = read_value;
2097 else if (!strncmp(item_ptr[index].item_name,
"Acceleration_Limit", strlen(
"Acceleration_Limit")))
2098 xh_state.Acceleration_Limit = read_value;
2099 else if (!strncmp(item_ptr[index].item_name,
"Velocity_Limit", strlen(
"Velocity_Limit")))
2100 xh_state.Velocity_Limit = read_value;
2101 else if (!strncmp(item_ptr[index].item_name,
"Max_Position_Limit", strlen(
"Max_Position_Limit")))
2102 xh_state.Max_Position_Limit = read_value;
2103 else if (!strncmp(item_ptr[index].item_name,
"Min_Position_Limit", strlen(
"Min_Position_Limit")))
2104 xh_state.Min_Position_Limit = read_value;
2105 else if (!strncmp(item_ptr[index].item_name,
"Shutdown", strlen(
"Shutdown")))
2106 xh_state.Shutdown = read_value;
2107 else if (!strncmp(item_ptr[index].item_name,
"Torque_Enable", strlen(
"Torque_Enable")))
2108 xh_state.Torque_Enable = read_value;
2109 else if (!strncmp(item_ptr[index].item_name,
"LED", strlen(
"LED")))
2110 xh_state.LED = read_value;
2111 else if (!strncmp(item_ptr[index].item_name,
"Status_Return_Level", strlen(
"Status_Return_Level")))
2112 xh_state.Status_Return_Level = read_value;
2113 else if (!strncmp(item_ptr[index].item_name,
"Registered_Instruction", strlen(
"Registered_Instruction")))
2114 xh_state.Registered_Instruction = read_value;
2115 else if (!strncmp(item_ptr[index].item_name,
"Hardware_Error_Status", strlen(
"Hardware_Error_Status")))
2116 xh_state.Hardware_Error_Status = read_value;
2117 else if (!strncmp(item_ptr[index].item_name,
"Velocity_I_Gain", strlen(
"Velocity_I_Gain")))
2118 xh_state.Velocity_I_Gain = read_value;
2119 else if (!strncmp(item_ptr[index].item_name,
"Velocity_P_Gain", strlen(
"Velocity_P_Gain")))
2120 xh_state.Velocity_P_Gain = read_value;
2121 else if (!strncmp(item_ptr[index].item_name,
"Position_D_Gain", strlen(
"Position_D_Gain")))
2122 xh_state.Position_D_Gain = read_value;
2123 else if (!strncmp(item_ptr[index].item_name,
"Position_I_Gain", strlen(
"Position_I_Gain")))
2124 xh_state.Position_I_Gain = read_value;
2125 else if (!strncmp(item_ptr[index].item_name,
"Position_P_Gain", strlen(
"Position_P_Gain")))
2126 xh_state.Position_P_Gain = read_value;
2127 else if (!strncmp(item_ptr[index].item_name,
"Feedforward_2nd_Gain", strlen(
"Feedforward_2nd_Gain")))
2128 xh_state.Feedforward_2nd_Gain = read_value;
2129 else if (!strncmp(item_ptr[index].item_name,
"Feedforward_1st_Gain", strlen(
"Feedforward_1st_Gain")))
2130 xh_state.Feedforward_1st_Gain = read_value;
2131 else if (!strncmp(item_ptr[index].item_name,
"Bus_Watchdog", strlen(
"Bus_Watchdog")))
2132 xh_state.Bus_Watchdog = read_value;
2133 else if (!strncmp(item_ptr[index].item_name,
"Goal_PWM", strlen(
"Goal_PWM")))
2134 xh_state.Goal_PWM = read_value;
2135 else if (!strncmp(item_ptr[index].item_name,
"Goal_Current", strlen(
"Goal_Current")))
2136 xh_state.Goal_Current = read_value;
2137 else if (!strncmp(item_ptr[index].item_name,
"Goal_Velocity", strlen(
"Goal_Velocity")))
2138 xh_state.Goal_Velocity = read_value;
2139 else if (!strncmp(item_ptr[index].item_name,
"Profile_Acceleration", strlen(
"Profile_Acceleration")))
2140 xh_state.Profile_Acceleration = read_value;
2141 else if (!strncmp(item_ptr[index].item_name,
"Profile_Velocity", strlen(
"Profile_Velocity")))
2142 xh_state.Profile_Velocity = read_value;
2143 else if (!strncmp(item_ptr[index].item_name,
"Goal_Position", strlen(
"Goal_Position")))
2144 xh_state.Goal_Position = read_value;
2145 else if (!strncmp(item_ptr[index].item_name,
"Realtime_Tick", strlen(
"Realtime_Tick")))
2146 xh_state.Realtime_Tick = read_value;
2147 else if (!strncmp(item_ptr[index].item_name,
"Moving", strlen(
"Moving")))
2148 xh_state.Moving = read_value;
2149 else if (!strncmp(item_ptr[index].item_name,
"Moving_Status", strlen(
"Moving_Status")))
2150 xh_state.Moving_Status = read_value;
2151 else if (!strncmp(item_ptr[index].item_name,
"Present_PWM", strlen(
"Present_PWM")))
2152 xh_state.Present_PWM = read_value;
2153 else if (!strncmp(item_ptr[index].item_name,
"Present_Current", strlen(
"Present_Current")))
2154 xh_state.Present_Current = read_value;
2155 else if (!strncmp(item_ptr[index].item_name,
"Present_Velocity", strlen(
"Present_Velocity")))
2156 xh_state.Present_Velocity = read_value;
2157 else if (!strncmp(item_ptr[index].item_name,
"Present_Position", strlen(
"Present_Position")))
2158 xh_state.Present_Position = read_value;
2159 else if (!strncmp(item_ptr[index].item_name,
"Velocity_Trajectory", strlen(
"Velocity_Trajectory")))
2160 xh_state.Velocity_Trajectory = read_value;
2161 else if (!strncmp(item_ptr[index].item_name,
"Position_Trajectory", strlen(
"Position_Trajectory")))
2162 xh_state.Position_Trajectory = read_value;
2163 else if (!strncmp(item_ptr[index].item_name,
"Present_Input_Voltage", strlen(
"Present_Input_Voltage")))
2164 xh_state.Present_Input_Voltage = read_value;
2165 else if (!strncmp(item_ptr[index].item_name,
"Present_Temperature", strlen(
"Present_Temperature")))
2166 xh_state.Present_Temperature = read_value;
2175 dynamixel_workbench_msgs::PRO pro_state;
2179 uint32_t getAllRegisteredData[last_register_addr+last_register_addr_length] = {0, };
2185 int32_t read_value = 0;
2186 read_value = getAllRegisteredData[item_ptr[index].
address];
2188 switch (item_ptr[index].data_length)
2191 read_value = getAllRegisteredData[item_ptr[index].address];
2195 read_value =
DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address], getAllRegisteredData[item_ptr[index].address+1]);
2199 read_value =
DXL_MAKEDWORD(
DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address], getAllRegisteredData[item_ptr[index].address+1]),
2200 DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address+2], getAllRegisteredData[item_ptr[index].address+3]));
2204 read_value = getAllRegisteredData[item_ptr[index].address];
2208 if (!strncmp(item_ptr[index].item_name,
"Model_Number", strlen(
"Model_Number")))
2209 pro_state.Model_Number = read_value;
2210 else if (!strncmp(item_ptr[index].item_name,
"Firmware_Version", strlen(
"Firmware_Version")))
2211 pro_state.Firmware_Version = read_value;
2212 else if (!strncmp(item_ptr[index].item_name,
"ID", strlen(
"ID")))
2213 pro_state.ID = read_value;
2214 else if (!strncmp(item_ptr[index].item_name,
"Baud_Rate", strlen(
"Baud_Rate")))
2215 pro_state.Baud_Rate = read_value;
2216 else if (!strncmp(item_ptr[index].item_name,
"Return_Delay_Time", strlen(
"Return_Delay_Time")))
2217 pro_state.Return_Delay_Time = read_value;
2218 else if (!strncmp(item_ptr[index].item_name,
"Operating_Mode", strlen(
"Operating_Mode")))
2219 pro_state.Operating_Mode = read_value;
2220 else if (!strncmp(item_ptr[index].item_name,
"Moving_Threshold", strlen(
"Moving_Threshold")))
2221 pro_state.Moving_Threshold = read_value;
2222 else if (!strncmp(item_ptr[index].item_name,
"Temperature_Limit", strlen(
"Temperature_Limit")))
2223 pro_state.Temperature_Limit = read_value;
2224 else if (!strncmp(item_ptr[index].item_name,
"Max_Voltage_Limit", strlen(
"Max_Voltage_Limit")))
2225 pro_state.Max_Voltage_Limit = read_value;
2226 else if (!strncmp(item_ptr[index].item_name,
"Min_Voltage_Limit", strlen(
"Min_Voltage_Limit")))
2227 pro_state.Min_Voltage_Limit = read_value;
2228 else if (!strncmp(item_ptr[index].item_name,
"Acceleration_Limit", strlen(
"Acceleration_Limit")))
2229 pro_state.Acceleration_Limit = read_value;
2230 else if (!strncmp(item_ptr[index].item_name,
"Torque_Limit", strlen(
"Torque_Limit")))
2231 pro_state.Torque_Limit = read_value;
2232 else if (!strncmp(item_ptr[index].item_name,
"Velocity_Limit", strlen(
"Velocity_Limit")))
2233 pro_state.Velocity_Limit = read_value;
2234 else if (!strncmp(item_ptr[index].item_name,
"Max_Position_Limit", strlen(
"Max_Position_Limit")))
2235 pro_state.Max_Position_Limit = read_value;
2236 else if (!strncmp(item_ptr[index].item_name,
"Min_Position_Limit", strlen(
"Min_Position_Limit")))
2237 pro_state.Min_Position_Limit = read_value;
2238 else if (!strncmp(item_ptr[index].item_name,
"External_Port_Mode_1", strlen(
"External_Port_Mode_1")))
2239 pro_state.External_Port_Mode_1 = read_value;
2240 else if (!strncmp(item_ptr[index].item_name,
"External_Port_Mode_2", strlen(
"External_Port_Mode_2")))
2241 pro_state.External_Port_Mode_2 = read_value;
2242 else if (!strncmp(item_ptr[index].item_name,
"External_Port_Mode_3", strlen(
"External_Port_Mode_3")))
2243 pro_state.External_Port_Mode_3 = read_value;
2244 else if (!strncmp(item_ptr[index].item_name,
"External_Port_Mode_4", strlen(
"External_Port_Mode_4")))
2245 pro_state.External_Port_Mode_4 = read_value;
2246 else if (!strncmp(item_ptr[index].item_name,
"Shutdown", strlen(
"Shutdown")))
2247 pro_state.Shutdown = read_value;
2248 else if (!strncmp(item_ptr[index].item_name,
"Torque_Enable", strlen(
"Torque_Enable")))
2249 pro_state.Torque_Enable = read_value;
2250 else if (!strncmp(item_ptr[index].item_name,
"LED_RED", strlen(
"LED_RED")))
2251 pro_state.LED_RED = read_value;
2252 else if (!strncmp(item_ptr[index].item_name,
"LED_GREEN", strlen(
"LED_GREEN")))
2253 pro_state.LED_GREEN = read_value;
2254 else if (!strncmp(item_ptr[index].item_name,
"LED_BLUE", strlen(
"LED_BLUE")))
2255 pro_state.LED_BLUE = read_value;
2256 else if (!strncmp(item_ptr[index].item_name,
"Velocity_I_Gain", strlen(
"Velocity_I_Gain")))
2257 pro_state.Velocity_I_Gain = read_value;
2258 else if (!strncmp(item_ptr[index].item_name,
"Velocity_P_Gain", strlen(
"Velocity_P_Gain")))
2259 pro_state.Velocity_P_Gain = read_value;
2260 else if (!strncmp(item_ptr[index].item_name,
"Position_P_Gain", strlen(
"Position_P_Gain")))
2261 pro_state.Position_P_Gain = read_value;
2262 else if (!strncmp(item_ptr[index].item_name,
"Goal_Position", strlen(
"Goal_Position")))
2263 pro_state.Goal_Position = read_value;
2264 else if (!strncmp(item_ptr[index].item_name,
"Goal_Velocity", strlen(
"Goal_Velocity")))
2265 pro_state.Goal_Velocity = read_value;
2266 else if (!strncmp(item_ptr[index].item_name,
"Goal_Torque", strlen(
"Goal_Torque")))
2267 pro_state.Goal_Torque = read_value;
2268 else if (!strncmp(item_ptr[index].item_name,
"Goal_Acceleration", strlen(
"Goal_Acceleration")))
2269 pro_state.Goal_Acceleration = read_value;
2270 else if (!strncmp(item_ptr[index].item_name,
"Moving", strlen(
"Moving")))
2271 pro_state.Moving = read_value;
2272 else if (!strncmp(item_ptr[index].item_name,
"Present_Position", strlen(
"Present_Position")))
2273 pro_state.Present_Position = read_value;
2274 else if (!strncmp(item_ptr[index].item_name,
"Present_Velocity", strlen(
"Present_Velocity")))
2275 pro_state.Present_Velocity = read_value;
2276 else if (!strncmp(item_ptr[index].item_name,
"Present_Current", strlen(
"Present_Current")))
2277 pro_state.Present_Current = read_value;
2278 else if (!strncmp(item_ptr[index].item_name,
"Present_Input_Voltage", strlen(
"Present_Input_Voltage")))
2279 pro_state.Present_Input_Voltage = read_value;
2280 else if (!strncmp(item_ptr[index].item_name,
"Present_Temperature", strlen(
"Present_Temperature")))
2281 pro_state.Present_Temperature = read_value;
2282 else if (!strncmp(item_ptr[index].item_name,
"Registered_Instruction", strlen(
"Registered_Instruction")))
2283 pro_state.Registered_Instruction = read_value;
2284 else if (!strncmp(item_ptr[index].item_name,
"Status_Return_Level", strlen(
"Status_Return_Level")))
2285 pro_state.Status_Return_Level = read_value;
2286 else if (!strncmp(item_ptr[index].item_name,
"Hardware_Error_Status", strlen(
"Hardware_Error_Status")))
2287 pro_state.Hardware_Error_Status = read_value;
2296 dynamixel_workbench_msgs::PROExt proext_state;
2300 uint32_t getAllRegisteredData[last_register_addr+last_register_addr_length] = {0, };
2306 int32_t read_value = 0;
2307 read_value = getAllRegisteredData[item_ptr[index].
address];
2309 switch (item_ptr[index].data_length)
2312 read_value = getAllRegisteredData[item_ptr[index].address];
2316 read_value =
DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address], getAllRegisteredData[item_ptr[index].address+1]);
2320 read_value =
DXL_MAKEDWORD(
DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address], getAllRegisteredData[item_ptr[index].address+1]),
2321 DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address+2], getAllRegisteredData[item_ptr[index].address+3]));
2325 read_value = getAllRegisteredData[item_ptr[index].address];
2329 if (!strncmp(item_ptr[index].item_name,
"Model_Number", strlen(
"Model_Number")))
2330 proext_state.Model_Number = read_value;
2331 else if (!strncmp(item_ptr[index].item_name,
"Firmware_Version", strlen(
"Firmware_Version")))
2332 proext_state.Firmware_Version = read_value;
2333 else if (!strncmp(item_ptr[index].item_name,
"ID", strlen(
"ID")))
2334 proext_state.ID = read_value;
2335 else if (!strncmp(item_ptr[index].item_name,
"Baud_Rate", strlen(
"Baud_Rate")))
2336 proext_state.Baud_Rate = read_value;
2337 else if (!strncmp(item_ptr[index].item_name,
"Return_Delay_Time", strlen(
"Return_Delay_Time")))
2338 proext_state.Return_Delay_Time = read_value;
2339 else if (!strncmp(item_ptr[index].item_name,
"Operating_Mode", strlen(
"Operating_Mode")))
2340 proext_state.Operating_Mode = read_value;
2341 else if (!strncmp(item_ptr[index].item_name,
"Homing_Offset", strlen(
"Homing_Offset")))
2342 proext_state.Homing_Offset = read_value;
2343 else if (!strncmp(item_ptr[index].item_name,
"Moving_Threshold", strlen(
"Moving_Threshold")))
2344 proext_state.Moving_Threshold = read_value;
2345 else if (!strncmp(item_ptr[index].item_name,
"Temperature_Limit", strlen(
"Temperature_Limit")))
2346 proext_state.Temperature_Limit = read_value;
2347 else if (!strncmp(item_ptr[index].item_name,
"Max_Voltage_Limit", strlen(
"Max_Voltage_Limit")))
2348 proext_state.Max_Voltage_Limit = read_value;
2349 else if (!strncmp(item_ptr[index].item_name,
"Min_Voltage_Limit", strlen(
"Min_Voltage_Limit")))
2350 proext_state.Min_Voltage_Limit = read_value;
2351 else if (!strncmp(item_ptr[index].item_name,
"Acceleration_Limit", strlen(
"Acceleration_Limit")))
2352 proext_state.Acceleration_Limit = read_value;
2353 else if (!strncmp(item_ptr[index].item_name,
"Torque_Limit", strlen(
"Torque_Limit")))
2354 proext_state.Torque_Limit = read_value;
2355 else if (!strncmp(item_ptr[index].item_name,
"Velocity_Limit", strlen(
"Velocity_Limit")))
2356 proext_state.Velocity_Limit = read_value;
2357 else if (!strncmp(item_ptr[index].item_name,
"Max_Position_Limit", strlen(
"Max_Position_Limit")))
2358 proext_state.Max_Position_Limit = read_value;
2359 else if (!strncmp(item_ptr[index].item_name,
"Min_Position_Limit", strlen(
"Min_Position_Limit")))
2360 proext_state.Min_Position_Limit = read_value;
2361 else if (!strncmp(item_ptr[index].item_name,
"External_Port_Mode_1", strlen(
"External_Port_Mode_1")))
2362 proext_state.External_Port_Mode_1 = read_value;
2363 else if (!strncmp(item_ptr[index].item_name,
"External_Port_Mode_2", strlen(
"External_Port_Mode_2")))
2364 proext_state.External_Port_Mode_2 = read_value;
2365 else if (!strncmp(item_ptr[index].item_name,
"External_Port_Mode_3", strlen(
"External_Port_Mode_3")))
2366 proext_state.External_Port_Mode_3 = read_value;
2367 else if (!strncmp(item_ptr[index].item_name,
"External_Port_Mode_4", strlen(
"External_Port_Mode_4")))
2368 proext_state.External_Port_Mode_4 = read_value;
2369 else if (!strncmp(item_ptr[index].item_name,
"Shutdown", strlen(
"Shutdown")))
2370 proext_state.Shutdown = read_value;
2371 else if (!strncmp(item_ptr[index].item_name,
"Torque_Enable", strlen(
"Torque_Enable")))
2372 proext_state.Torque_Enable = read_value;
2373 else if (!strncmp(item_ptr[index].item_name,
"LED_RED", strlen(
"LED_RED")))
2374 proext_state.LED_RED = read_value;
2375 else if (!strncmp(item_ptr[index].item_name,
"LED_GREEN", strlen(
"LED_GREEN")))
2376 proext_state.LED_GREEN = read_value;
2377 else if (!strncmp(item_ptr[index].item_name,
"LED_BLUE", strlen(
"LED_BLUE")))
2378 proext_state.LED_BLUE = read_value;
2379 else if (!strncmp(item_ptr[index].item_name,
"Velocity_I_Gain", strlen(
"Velocity_I_Gain")))
2380 proext_state.Velocity_I_Gain = read_value;
2381 else if (!strncmp(item_ptr[index].item_name,
"Velocity_P_Gain", strlen(
"Velocity_P_Gain")))
2382 proext_state.Velocity_P_Gain = read_value;
2383 else if (!strncmp(item_ptr[index].item_name,
"Position_P_Gain", strlen(
"Position_P_Gain")))
2384 proext_state.Position_P_Gain = read_value;
2385 else if (!strncmp(item_ptr[index].item_name,
"Goal_Position", strlen(
"Goal_Position")))
2386 proext_state.Goal_Position = read_value;
2387 else if (!strncmp(item_ptr[index].item_name,
"Goal_Velocity", strlen(
"Goal_Velocity")))
2388 proext_state.Goal_Velocity = read_value;
2389 else if (!strncmp(item_ptr[index].item_name,
"Goal_Torque", strlen(
"Goal_Torque")))
2390 proext_state.Goal_Torque = read_value;
2391 else if (!strncmp(item_ptr[index].item_name,
"Goal_Acceleration", strlen(
"Goal_Acceleration")))
2392 proext_state.Goal_Acceleration = read_value;
2393 else if (!strncmp(item_ptr[index].item_name,
"Moving", strlen(
"Moving")))
2394 proext_state.Moving = read_value;
2395 else if (!strncmp(item_ptr[index].item_name,
"Present_Position", strlen(
"Present_Position")))
2396 proext_state.Present_Position = read_value;
2397 else if (!strncmp(item_ptr[index].item_name,
"Present_Velocity", strlen(
"Present_Velocity")))
2398 proext_state.Present_Velocity = read_value;
2399 else if (!strncmp(item_ptr[index].item_name,
"Present_Current", strlen(
"Present_Current")))
2400 proext_state.Present_Current = read_value;
2401 else if (!strncmp(item_ptr[index].item_name,
"Present_Input_Voltage", strlen(
"Present_Input_Voltage")))
2402 proext_state.Present_Input_Voltage = read_value;
2403 else if (!strncmp(item_ptr[index].item_name,
"Present_Temperature", strlen(
"Present_Temperature")))
2404 proext_state.Present_Temperature = read_value;
2405 else if (!strncmp(item_ptr[index].item_name,
"Registered_Instruction", strlen(
"Registered_Instruction")))
2406 proext_state.Registered_Instruction = read_value;
2407 else if (!strncmp(item_ptr[index].item_name,
"Status_Return_Level", strlen(
"Status_Return_Level")))
2408 proext_state.Status_Return_Level = read_value;
2409 else if (!strncmp(item_ptr[index].item_name,
"Hardware_Error_Status", strlen(
"Hardware_Error_Status")))
2410 proext_state.Hardware_Error_Status = read_value;
int main(int argc, char **argv)
void initDynamixelInfoServer(void)
bool changeID(uint8_t id, uint8_t new_id, const char **log=NULL)
bool changeId(uint8_t new_id)
uint32_t getBaudrate(void)
void publish(const boost::shared_ptr< M > &message) const
bool setPacketHandler(float protocol_version, const char **log=NULL)
bool init(const char *device_name="/dev/ttyUSB0", uint32_t baud_rate=57600, const char **log=NULL)
bool changeProtocolVersion(uint8_t id, uint8_t version, const char **log=NULL)
void shutdownSingleDynamixelMonitor(void)
bool torque(uint8_t id, int32_t onoff, const char **log=NULL)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
bool itemWrite(uint8_t id, const char *item_name, int32_t data, const char **log=NULL)
bool readRegister(uint8_t id, uint16_t address, uint16_t length, uint32_t *data, const char **log=NULL)
const char * getModelName(uint8_t id, const char **log=NULL)
bool writeRegister(uint8_t id, uint16_t address, uint16_t length, uint8_t *data, const char **log=NULL)
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
bool changeProtocolVersion(float ver)
bool changeBaudrate(uint32_t new_baud_rate)
bool reset(uint8_t id, const char **log=NULL)
bool dynamixelCommandMsgCallback(dynamixel_workbench_msgs::DynamixelCommand::Request &req, dynamixel_workbench_msgs::DynamixelCommand::Response &res)
DynamixelWorkbench * dynamixel_workbench_
bool goalVelocity(uint8_t id, int32_t value, const char **log=NULL)
bool ping(uint8_t id, uint16_t *get_model_number, const char **log=NULL)
ros::Publisher dynamixel_status_pub_
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
void initDynamixelCommandServer(void)
bool showDynamixelControlTable(void)
bool goalPosition(uint8_t id, int32_t value, const char **log=NULL)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::NodeHandle node_handle_
void dynamixelStatePublish(void)
bool changeBaudrate(uint8_t id, uint32_t new_baudrate, const char **log=NULL)
void initSingleDynamixelMonitor(void)
bool wheelMode(uint8_t id, int32_t acceleration=0, const char **log=NULL)
const ControlItem * getControlTable(uint8_t id, const char **log=NULL)
bool checkValidationCommand(std::string cmd)
ros::ServiceServer dynamixel_info_server_
bool reboot(uint8_t id, const char **log=NULL)
void millis(uint16_t msec)
ROSCPP_DECL void shutdown()
uint8_t getTheNumberOfControlItem(uint8_t id, const char **log=NULL)
SingleDynamixelMonitor(void)
~SingleDynamixelMonitor(void)
bool dynamixelInfoMsgCallback(dynamixel_workbench_msgs::GetDynamixelInfo::Request &req, dynamixel_workbench_msgs::GetDynamixelInfo::Response &res)
ROSCPP_DECL void spinOnce()
bool jointMode(uint8_t id, int32_t velocity=0, int32_t acceleration=0, const char **log=NULL)
uint16_t getModelNumber(uint8_t id, const char **log=NULL)
float getProtocolVersion(void)
ros::ServiceServer dynamixel_command_server_
void initDynamixelStatePublisher(void)
bool setBaudrate(uint32_t baud_rate, const char **log=NULL)