single_dynamixel_monitor.cpp
Go to the documentation of this file.
1 /*******************************************************************************
2 * Copyright 2016 ROBOTIS CO., LTD.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *******************************************************************************/
16 
17 /* Authors: Taehun Lim (Darby) */
18 
19 #include "dynamixel_workbench_single_manager/single_dynamixel_monitor.h"
20 
21 using namespace single_dynamixel_monitor;
22 
23 inline void millis(uint16_t msec)
24 {
25  usleep(1000*msec);
26 }
27 
28 SingleDynamixelMonitor::SingleDynamixelMonitor(void)
29  :dxl_baud_rate_(0),
30  dxl_id_(0)
31 {
32  // Check Dynamixel Ping or Scan (default : Scan (1~253))
33  const char* log = NULL;
34  bool result = false;
35 
36  // Dynamixel Monitor variable
37  bool use_ping = node_handle_.param<bool>("ping", false);
38  int ping_id = node_handle_.param<int>("ping_id", 1);
39  int scan_range = node_handle_.param<int>("scan_range", 253);
40 
41  device_name_ = node_handle_.param<std::string>("device_name", "/dev/ttyUSB0");
42  dxl_baud_rate_ = node_handle_.param<int>("baud_rate", 57600);
43 
44  // Init Dynamixel Driver
45  dynamixel_workbench_ = new DynamixelWorkbench;
46 
47  result = dynamixel_workbench_->init(device_name_.c_str(), dxl_baud_rate_, &log);
48  if (result == false)
49  {
50  printf("%s\n", log);
51  printf("Failed to init\n");
52 
53  ros::shutdown();
54  exit(1);
55  }
56  else
57  printf("Succeed to init(%d)\n", dxl_baud_rate_);
58 
59  if(use_ping == true)
60  {
61  uint16_t model_number = 0;
62  result = dynamixel_workbench_->ping(ping_id, &model_number, &log);
63  if (result == false)
64  {
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);
69 
70  ros::shutdown();
71  exit(1);
72  }
73  else
74  {
75  dxl_id_ = ping_id;
76  printf("[ID] %u, [Model Name] %s, [BAUD RATE] %d [VERSION] %.1f\n",
77  dxl_id_, dynamixel_workbench_->getModelName(dxl_id_), dxl_baud_rate_, dynamixel_workbench_->getProtocolVersion());
78  }
79  }
80  else
81  {
82  uint8_t id_cnt = 0;
83  result = dynamixel_workbench_->scan(&dxl_id_, &id_cnt, scan_range, &log);
84  if (result == false)
85  {
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);
90 
91  ros::shutdown();
92  exit(1);
93  }
94  else
95  {
96  printf("[ID] %u, [Model Name] %s, [BAUD RATE] %d [VERSION] %.1f\n",
97  dxl_id_, dynamixel_workbench_->getModelName(dxl_id_), dxl_baud_rate_, dynamixel_workbench_->getProtocolVersion());
98  }
99  }
100 
101  initDynamixelStatePublisher();
102  initDynamixelInfoServer();
103  initDynamixelCommandServer();
104 
105  printf("dynamixel_workbench_single_manager : Init Success!\n");
106 }
107 
108 SingleDynamixelMonitor::~SingleDynamixelMonitor(void)
109 {
110 
111 }
112 
113 void SingleDynamixelMonitor::initSingleDynamixelMonitor(void)
114 {
115 
116 }
117 
118 void SingleDynamixelMonitor::shutdownSingleDynamixelMonitor(void)
119 {
120  dynamixel_workbench_->writeRegister(dxl_id_, "Torque_Enable", (uint8_t)0);
121 
122  ros::shutdown();
123 }
124 
125 void SingleDynamixelMonitor::initDynamixelStatePublisher()
126 {
127  const char* model_name = dynamixel_workbench_->getModelName(dxl_id_);
128 
129  if (!strncmp(model_name, "AX", strlen("AX")))
130  {
131  dynamixel_status_pub_ = node_handle_.advertise<dynamixel_workbench_msgs::AX>("dynamixel/" + std::string("AX"), 10);
132  }
133  else if (!strncmp(model_name, "RX", strlen("RX")))
134  {
135  dynamixel_status_pub_ = node_handle_.advertise<dynamixel_workbench_msgs::RX>("dynamixel/" + std::string("RX"), 10);
136  }
137  else if (!strncmp(model_name, "MX", strlen("MX")))
138  {
139  if (!strncmp(model_name, "MX-12W", strlen(model_name)) ||
140  !strncmp(model_name, "MX-28" , strlen(model_name)))
141  {
142  dynamixel_status_pub_ = node_handle_.advertise<dynamixel_workbench_msgs::MX>("dynamixel/" + std::string("MX"), 10);
143  }
144  else if (!strncmp(model_name, "MX-64", strlen(model_name)) ||
145  !strncmp(model_name, "MX-106" , strlen(model_name)))
146  {
147  dynamixel_status_pub_ = node_handle_.advertise<dynamixel_workbench_msgs::MXExt>("dynamixel/" + std::string("MX"), 10);
148  }
149  else if (!strncmp(model_name, "MX-28-2", strlen(model_name)))
150  {
151  dynamixel_status_pub_ = node_handle_.advertise<dynamixel_workbench_msgs::MX2>("dynamixel/" + std::string("MX"), 10);
152  }
153  else if (!strncmp(model_name, "MX-64-2", strlen(model_name)) ||
154  !strncmp(model_name, "MX-106-2" , strlen(model_name)))
155  {
156  dynamixel_status_pub_ = node_handle_.advertise<dynamixel_workbench_msgs::MX2Ext>("dynamixel/" + std::string("MX"), 10);
157  }
158  }
159  else if (!strncmp(model_name, "EX", strlen("EX")))
160  {
161  dynamixel_status_pub_ = node_handle_.advertise<dynamixel_workbench_msgs::EX>("dynamixel/" + std::string("EX"), 10);
162  }
163  else if (!strncmp(model_name, "XL", strlen("XL")))
164  {
165  if (!strncmp(model_name, "XL-320", strlen(model_name)))
166  {
167  dynamixel_status_pub_ = node_handle_.advertise<dynamixel_workbench_msgs::XL320>("dynamixel/" + std::string("XL"), 10);
168  }
169  else if (!strncmp(model_name, "XL430-W250", strlen(model_name)))
170  {
171  dynamixel_status_pub_ = node_handle_.advertise<dynamixel_workbench_msgs::XL>("dynamixel/" + std::string("XL"), 10);
172  }
173  }
174  else if (!strncmp(model_name, "XM", strlen("XM")))
175  {
176  if (!strncmp(model_name, "XM430-W210", strlen(model_name)) ||
177  !strncmp(model_name, "XM430-W350" , strlen(model_name)))
178  {
179  dynamixel_status_pub_ = node_handle_.advertise<dynamixel_workbench_msgs::XM>("dynamixel/" + std::string("XM"), 10);
180  }
181  else if (!strncmp(model_name, "XM540-W150", strlen(model_name)) ||
182  !strncmp(model_name, "XM540-W270" , strlen(model_name)))
183  {
184  dynamixel_status_pub_ = node_handle_.advertise<dynamixel_workbench_msgs::XMExt>("dynamixel/" + std::string("XM"), 10);
185  }
186  }
187  else if (!strncmp(model_name, "XH", strlen("XH")))
188  {
189  dynamixel_status_pub_ = node_handle_.advertise<dynamixel_workbench_msgs::XH>("dynamixel/" + std::string("XH"), 10);
190  }
191  else if (!strncmp(model_name, "PRO", strlen("PRO")))
192  {
193  if ((!strncmp(model_name, "PRO_L42_10_S300_R", strlen(model_name))))
194  {
195  dynamixel_status_pub_ = node_handle_.advertise<dynamixel_workbench_msgs::PRO>("dynamixel/" + std::string("PRO"), 10);
196  }
197  else
198  {
199  dynamixel_status_pub_ = node_handle_.advertise<dynamixel_workbench_msgs::PROExt>("dynamixel/" + std::string("PRO"), 10);
200  }
201  }
202 }
203 
204 void SingleDynamixelMonitor::initDynamixelInfoServer(void)
205 {
206  dynamixel_info_server_ = node_handle_.advertiseService("dynamixel/info", &SingleDynamixelMonitor::dynamixelInfoMsgCallback, this);
207 }
208 
209 void SingleDynamixelMonitor::initDynamixelCommandServer(void)
210 {
211  dynamixel_command_server_ = node_handle_.advertiseService("dynamixel/command", &SingleDynamixelMonitor::dynamixelCommandMsgCallback, this);
212 }
213 
214 bool SingleDynamixelMonitor::showDynamixelControlTable(void)
215 {
216  bool result = false;
217  int32_t torque_status = 0;
218  uint16_t torque_enable_address = 0;
219  const ControlItem* item_ptr = dynamixel_workbench_->getControlTable(dxl_id_);
220 
221  for (int item_num = 0; item_num < dynamixel_workbench_->getTheNumberOfControlItem(dxl_id_); item_num++)
222  {
223  if (!strncmp(item_ptr[item_num].item_name, "Torque_Enable", strlen("Torque_Enable")))
224  {
225  torque_enable_address = item_num;
226  }
227  }
228 
229  result = dynamixel_workbench_->readRegister(dxl_id_, "Torque_Enable", &torque_status);
230 
231  for (int item_num = 0; item_num < dynamixel_workbench_->getTheNumberOfControlItem(dxl_id_); item_num++)
232  {
233  if (torque_status == false)
234  {
235  printf("%s\n", item_ptr[item_num].item_name);
236  }
237  else
238  {
239  if (item_num >= torque_enable_address)
240  printf("%s\n", item_ptr[item_num].item_name);
241  }
242  }
243 
244  return result;
245 }
246 
247 bool SingleDynamixelMonitor::checkValidationCommand(std::string cmd)
248 {
249  const ControlItem* item_ptr = dynamixel_workbench_->getControlTable(dxl_id_);
250 
251  for (int item_num = 0; item_num < dynamixel_workbench_->getTheNumberOfControlItem(dxl_id_); item_num++)
252  {
253  if (!strncmp(item_ptr[item_num].item_name, cmd.c_str(), strlen(item_ptr[item_num].item_name)))
254  return true;
255  }
256 
257  printf("Please Check DYNAMXEL Address Name('table')\n");
258  return false;
259 }
260 
261 bool SingleDynamixelMonitor::changeId(uint8_t new_id)
262 {
263  const char* log = NULL;
264  bool result = false;
265 
266  if (new_id > 0 && new_id < 254)
267  {
268  result = dynamixel_workbench_->changeID(dxl_id_, new_id, &log);
269  if (result == false)
270  {
271  printf("%s\n", log);
272  printf("...Failed to change ID!\n");
273  return false;
274  }
275  else
276  {
277  printf("%s\n", log);
278  uint16_t model_number = 0;
279  result = dynamixel_workbench_->ping(new_id, &model_number, &log);
280  if (result == false)
281  {
282  printf("%s\n", log);
283  printf("...Failed to ping Dynamixel ID [%u]\n", dxl_id_);
284  return false;
285  }
286  else
287  {
288  dxl_id_ = new_id;
289 
290  printf("...Succeeded to ping Dynamixel ID [%u]\n", dxl_id_);
291  return true;
292  }
293 
294  return false;
295  }
296  }
297  else
298  {
299  printf("Dynamixel ID can be set 1~253\n");
300  return false;
301  }
302 
303  return false;
304 }
305 
306 bool SingleDynamixelMonitor::changeBaudrate(uint32_t new_baud_rate)
307 {
308  const char* log = NULL;
309  bool result = false;
310  bool check_baud_rate = false;
311 
312  uint64_t baud_rate_list[14] = {9600, 19200, 57600, 115200, 200000, 250000, 400000, 500000,
313  1000000, 2000000, 3000000, 4000000, 4500000, 10500000};
314 
315  for (int i = 0; i < 14; i++)
316  {
317  if (baud_rate_list[i] == new_baud_rate)
318  check_baud_rate = true;
319  }
320 
321  if (check_baud_rate == false)
322  {
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");
326 
327  return false;
328  }
329  else
330  {
331  result = dynamixel_workbench_->changeBaudrate(dxl_id_, new_baud_rate, &log);
332  if (result == false)
333  {
334  printf("%s\n", log);
335  printf("Failed to change baudrate!\n");
336 
337  return false;
338  }
339  else
340  {
341  result = dynamixel_workbench_->setBaudrate(new_baud_rate, &log);
342  printf("%s\n", log);
343  printf("Success to change baudrate! [ BAUD RATE: %d ]\n", new_baud_rate);
344  return true;
345  }
346  }
347 
348  return false;
349 }
350 
351 bool SingleDynamixelMonitor::changeProtocolVersion(float ver)
352 {
353  const char* log = NULL;
354  bool result = false;
355 
356  if (ver == 1.0f || ver == 2.0f)
357  {
358  result = dynamixel_workbench_->changeProtocolVersion(dxl_id_, ver, &log);
359  if (result == false)
360  {
361  printf("%s\n", log);
362  printf("Failed to change protocol version\n");
363 
364  return false;
365  }
366  else
367  {
368  result = dynamixel_workbench_->setPacketHandler(ver, &log);
369  printf("Success to change protocol version [PROTOCOL VERSION: %.1f]\n", dynamixel_workbench_->getProtocolVersion());
370  return true;
371  }
372  }
373  else
374  {
375  printf("Dynamixel supports protocol version [1.0] or [2.0]\n");
376  return false;
377  }
378 
379  return false;
380 }
381 
382 bool SingleDynamixelMonitor::controlLoop(void)
383 {
384  dynamixelStatePublish();
385 
386  return true;
387 }
388 
389 bool SingleDynamixelMonitor::dynamixelInfoMsgCallback(dynamixel_workbench_msgs::GetDynamixelInfo::Request &req,
390  dynamixel_workbench_msgs::GetDynamixelInfo::Response &res)
391 {
392  res.dynamixel_info.load_info.device_name = device_name_;
393  res.dynamixel_info.load_info.baud_rate = dynamixel_workbench_->getBaudrate();
394  res.dynamixel_info.load_info.protocol_version = dynamixel_workbench_->getProtocolVersion();
395 
396  res.dynamixel_info.model_id = dxl_id_;
397  res.dynamixel_info.model_name = dynamixel_workbench_->getModelName(dxl_id_);
398  res.dynamixel_info.model_number = dynamixel_workbench_->getModelNumber(dxl_id_);
399 
400  return true;
401 }
402 
403 bool SingleDynamixelMonitor::dynamixelCommandMsgCallback(dynamixel_workbench_msgs::DynamixelCommand::Request &req,
404  dynamixel_workbench_msgs::DynamixelCommand::Response &res)
405 {
406  const char* log = NULL;
407  bool result = false;
408 
409  if (req.command == "table")
410  {
411  if (showDynamixelControlTable())
412  res.comm_result = true;
413  else
414  res.comm_result = false;
415  }
416  else if (req.command == "reboot")
417  {
418  result = dynamixel_workbench_->reboot(dxl_id_, &log);
419  if (result == false)
420  {
421  printf("%s\n", log);
422  printf("Failed to reboot\n");
423  res.comm_result = false;
424  }
425  else
426  {
427  printf("%s\n", log);
428  printf("Succeed to reboot\n");
429  res.comm_result = true;
430  }
431  }
432  else if (req.command == "factory_reset")
433  {
434  result = dynamixel_workbench_->reset(dxl_id_, &log);
435  if (result == false)
436  {
437  printf("%s\n", log);
438  printf("Failed to reset\n");
439  res.comm_result = false;
440  }
441  else
442  {
443  dxl_id_ = 1;
444 
445  printf("%s\n", log);
446  printf("Succeed to reset\n");
447  res.comm_result = true;
448  }
449  }
450  else if (req.command == "torque")
451  {
452  uint8_t value = req.value;
453 
454  result = dynamixel_workbench_->torque(dxl_id_, (bool)value, &log);
455  if (result == false)
456  {
457  res.comm_result = false;
458  printf("%s\n", log);
459  }
460  else
461  {
462  res.comm_result = true;
463  printf("%s\n", log);
464  }
465  }
466  else if (req.command == "Joint")
467  {
468  int32_t value = req.value;
469 
470  result = dynamixel_workbench_->jointMode(dxl_id_, 0, 0, &log);
471  if (result == false)
472  {
473  printf("%s\n", log);
474  res.comm_result = false;
475  }
476  else
477  {
478  printf("%s\n", log);
479  }
480 
481  result = dynamixel_workbench_->goalPosition(dxl_id_, value, &log);
482  if (result == false)
483  {
484  printf("%s\n", log);
485  res.comm_result = false;
486  }
487  else
488  {
489  printf("%s\n", log);
490  }
491 
492  res.comm_result = true;
493  }
494  else if (req.command == "Wheel")
495  {
496  int32_t value = req.value;
497 
498  result = dynamixel_workbench_->wheelMode(dxl_id_, 0, &log);
499  if (result == false)
500  {
501  printf("%s\n", log);
502  res.comm_result = false;
503  }
504  else
505  {
506  printf("%s\n", log);
507  }
508 
509  result = dynamixel_workbench_->goalVelocity(dxl_id_, value, &log);
510  if (result == false)
511  {
512  printf("%s\n", log);
513  res.comm_result = false;
514  }
515  else
516  {
517  printf("%s\n", log);
518  }
519 
520  res.comm_result = true;
521  }
522  else if (req.command == "exit")
523  {
524  shutdownSingleDynamixelMonitor();
525  }
526  else if (req.command == "addr")
527  {
528  std::string addr = req.addr_name;
529  int32_t value = req.value;
530 
531  if (checkValidationCommand(addr))
532  {
533  res.comm_result = true;
534  }
535  else
536  {
537  res.comm_result = false;
538  return false;
539  }
540 
541  if (addr == "ID")
542  {
543  if (changeId(value))
544  res.comm_result = true;
545  else
546  res.comm_result = false;
547  }
548  else if (addr == "Baud_Rate")
549  {
550  if (changeBaudrate(value))
551  res.comm_result = true;
552  else
553  res.comm_result = false;
554  }
555  else if (addr == "Protocol_Version")
556  {
557  if (changeProtocolVersion(value))
558  res.comm_result = true;
559  else
560  res.comm_result = false;
561  }
562  else
563  {
564  result = dynamixel_workbench_->itemWrite(dxl_id_, addr.c_str(), value, &log);
565  if (result == false)
566  {
567  printf("%s\n", log);
568  res.comm_result = false;
569  }
570  else
571  {
572  printf("%s\n", log);
573  res.comm_result = true;
574  }
575  }
576  }
577  else
578  {
579  return false;
580  }
581 
582  return true;
583 }
584 
585 int main(int argc, char **argv)
586 {
587  // Init ROS node
588  ros::init(argc, argv, "single_dynamixel_monitor");
589 
590  SingleDynamixelMonitor single_dynamixel_monitor;
591  ros::Rate loop_rate(1000);
592 
593  while (ros::ok())
594  {
595  single_dynamixel_monitor.controlLoop();
596 
597  ros::spinOnce();
598  loop_rate.sleep();
599  }
600 
601  return 0;
602 }
603 
604 
605 void SingleDynamixelMonitor::dynamixelStatePublish(void)
606 {
607  const char* model_name = dynamixel_workbench_->getModelName(dxl_id_);
608 
609  if (!strncmp(model_name, "AX", strlen("AX")))
610  {
611  AX();
612  }
613  else if (!strncmp(model_name, "RX", strlen("RX")))
614  {
615  RX();
616  }
617  else if (!strncmp(model_name, "MX-12W", strlen(model_name)) ||
618  !strncmp(model_name, "MX-28" , strlen(model_name)))
619  {
620  MX();
621  }
622  else if (!strncmp(model_name, "MX-64", strlen(model_name)) ||
623  !strncmp(model_name, "MX-106" , strlen(model_name)))
624  {
625  MXExt();
626  }
627  else if (!strncmp(model_name, "MX-28-2", strlen(model_name)))
628  {
629  MX2();
630  }
631  else if (!strncmp(model_name, "MX-64-2", strlen(model_name)) ||
632  !strncmp(model_name, "MX-106-2" , strlen(model_name)))
633  {
634  MX2Ext();
635  }
636  else if (!strncmp(model_name, "EX", strlen("EX")))
637  {
638  EX();
639  }
640  else if (!strncmp(model_name, "XL-320", strlen(model_name)))
641  {
642  XL320();
643  }
644  else if (!strncmp(model_name, "XL430-W250", strlen(model_name)))
645  {
646  XL();
647  }
648  else if (!strncmp(model_name, "XM430-W210", strlen(model_name)) ||
649  !strncmp(model_name, "XM430-W350" , strlen(model_name)))
650  {
651  XM();
652  }
653  else if (!strncmp(model_name, "XM540-W150", strlen(model_name)) ||
654  !strncmp(model_name, "XM540-W270" , strlen(model_name)))
655  {
656  XMExt();
657  }
658  else if (!strncmp(model_name, "XH", strlen("XH")))
659  {
660  XH();
661  }
662  else if (!strncmp(model_name, "PRO_L42_10_S300_R", strlen("PRO_L42_10_S300_R")))
663  {
664  PRO();
665  }
666  else if (!strncmp(model_name, "PRO", strlen("PRO")))
667  {
668  PROExt();
669  }
670 }
671 
672 void SingleDynamixelMonitor::AX(void)
673 {
674  const ControlItem* item_ptr = dynamixel_workbench_->getControlTable(dxl_id_);
675  dynamixel_workbench_msgs::AX ax_state;
676 
677  uint16_t last_register_addr = item_ptr[dynamixel_workbench_->getTheNumberOfControlItem(dxl_id_)-1].address;
678  uint16_t last_register_addr_length = item_ptr[dynamixel_workbench_->getTheNumberOfControlItem(dxl_id_)-1].data_length;
679  uint32_t getAllRegisteredData[last_register_addr+last_register_addr_length] = {0, };
680 
681  dynamixel_workbench_->readRegister(dxl_id_, (uint16_t)0, last_register_addr+last_register_addr_length, getAllRegisteredData);
682 
683  for (int index = 0; index < dynamixel_workbench_->getTheNumberOfControlItem(dxl_id_); index++)
684  {
685  int32_t read_value = 0;
686  read_value = getAllRegisteredData[item_ptr[index].address];
687 
688  switch (item_ptr[index].data_length)
689  {
690  case BYTE:
691  read_value = getAllRegisteredData[item_ptr[index].address];
692  break;
693 
694  case WORD:
695  read_value = DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address], getAllRegisteredData[item_ptr[index].address+1]);
696  break;
697 
698  case DWORD:
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]));
701  break;
702 
703  default:
704  read_value = getAllRegisteredData[item_ptr[index].address];
705  break;
706  }
707 
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;
772  }
773 
774  dynamixel_status_pub_.publish(ax_state);
775 }
776 
777 void SingleDynamixelMonitor::RX(void)
778 {
779  const ControlItem* item_ptr = dynamixel_workbench_->getControlTable(dxl_id_);
780  dynamixel_workbench_msgs::RX rx_state;
781 
782  uint16_t last_register_addr = item_ptr[dynamixel_workbench_->getTheNumberOfControlItem(dxl_id_)-1].address;
783  uint16_t last_register_addr_length = item_ptr[dynamixel_workbench_->getTheNumberOfControlItem(dxl_id_)-1].data_length;
784  uint32_t getAllRegisteredData[last_register_addr+last_register_addr_length] = {0, };
785 
786  dynamixel_workbench_->readRegister(dxl_id_, (uint16_t)0, last_register_addr+last_register_addr_length, getAllRegisteredData);
787 
788  for (int index = 0; index < dynamixel_workbench_->getTheNumberOfControlItem(dxl_id_); index++)
789  {
790  int32_t read_value = 0;
791  read_value = getAllRegisteredData[item_ptr[index].address];
792 
793  switch (item_ptr[index].data_length)
794  {
795  case BYTE:
796  read_value = getAllRegisteredData[item_ptr[index].address];
797  break;
798 
799  case WORD:
800  read_value = DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address], getAllRegisteredData[item_ptr[index].address+1]);
801  break;
802 
803  case DWORD:
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]));
806  break;
807 
808  default:
809  read_value = getAllRegisteredData[item_ptr[index].address];
810  break;
811  }
812 
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;
877  }
878 
879  dynamixel_status_pub_.publish(rx_state);
880 }
881 
882 void SingleDynamixelMonitor::MX(void)
883 {
884  const ControlItem* item_ptr = dynamixel_workbench_->getControlTable(dxl_id_);
885  dynamixel_workbench_msgs::MX mx_state;
886 
887  uint16_t last_register_addr = item_ptr[dynamixel_workbench_->getTheNumberOfControlItem(dxl_id_)-1].address;
888  uint16_t last_register_addr_length = item_ptr[dynamixel_workbench_->getTheNumberOfControlItem(dxl_id_)-1].data_length;
889  uint32_t getAllRegisteredData[last_register_addr+last_register_addr_length] = {0, };
890 
891  dynamixel_workbench_->readRegister(dxl_id_, (uint16_t)0, last_register_addr+last_register_addr_length, getAllRegisteredData);
892 
893  for (int index = 0; index < dynamixel_workbench_->getTheNumberOfControlItem(dxl_id_); index++)
894  {
895  int32_t read_value = 0;
896  read_value = getAllRegisteredData[item_ptr[index].address];
897 
898  switch (item_ptr[index].data_length)
899  {
900  case BYTE:
901  read_value = getAllRegisteredData[item_ptr[index].address];
902  break;
903 
904  case WORD:
905  read_value = DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address], getAllRegisteredData[item_ptr[index].address+1]);
906  break;
907 
908  case DWORD:
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]));
911  break;
912 
913  default:
914  read_value = getAllRegisteredData[item_ptr[index].address];
915  break;
916  }
917 
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;
986  }
987 
988  dynamixel_status_pub_.publish(mx_state);
989 }
990 
991 void SingleDynamixelMonitor::MXExt(void)
992 {
993  const ControlItem* item_ptr = dynamixel_workbench_->getControlTable(dxl_id_);
994  dynamixel_workbench_msgs::MXExt mxext_state;
995 
996  uint16_t last_register_addr = item_ptr[dynamixel_workbench_->getTheNumberOfControlItem(dxl_id_)-1].address;
997  uint16_t last_register_addr_length = item_ptr[dynamixel_workbench_->getTheNumberOfControlItem(dxl_id_)-1].data_length;
998  uint32_t getAllRegisteredData[last_register_addr+last_register_addr_length] = {0, };
999 
1000  dynamixel_workbench_->readRegister(dxl_id_, (uint16_t)0, last_register_addr+last_register_addr_length, getAllRegisteredData);
1001 
1002  for (int index = 0; index < dynamixel_workbench_->getTheNumberOfControlItem(dxl_id_); index++)
1003  {
1004  int32_t read_value = 0;
1005  read_value = getAllRegisteredData[item_ptr[index].address];
1006 
1007  switch (item_ptr[index].data_length)
1008  {
1009  case BYTE:
1010  read_value = getAllRegisteredData[item_ptr[index].address];
1011  break;
1012 
1013  case WORD:
1014  read_value = DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address], getAllRegisteredData[item_ptr[index].address+1]);
1015  break;
1016 
1017  case DWORD:
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]));
1020  break;
1021 
1022  default:
1023  read_value = getAllRegisteredData[item_ptr[index].address];
1024  break;
1025  }
1026 
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;
1101  }
1102 
1103  dynamixel_status_pub_.publish(mxext_state);
1104 }
1105 
1106 void SingleDynamixelMonitor::MX2(void)
1107 {
1108  const ControlItem* item_ptr = dynamixel_workbench_->getControlTable(dxl_id_);
1109  dynamixel_workbench_msgs::MX2 mx2_state;
1110 
1111  uint16_t last_register_addr = item_ptr[dynamixel_workbench_->getTheNumberOfControlItem(dxl_id_)-1].address;
1112  uint16_t last_register_addr_length = item_ptr[dynamixel_workbench_->getTheNumberOfControlItem(dxl_id_)-1].data_length;
1113  uint32_t getAllRegisteredData[last_register_addr+last_register_addr_length] = {0, };
1114 
1115  dynamixel_workbench_->readRegister(dxl_id_, (uint16_t)0, last_register_addr+last_register_addr_length, getAllRegisteredData);
1116 
1117  for (int index = 0; index < dynamixel_workbench_->getTheNumberOfControlItem(dxl_id_); index++)
1118  {
1119  int32_t read_value = 0;
1120  read_value = getAllRegisteredData[item_ptr[index].address];
1121 
1122  switch (item_ptr[index].data_length)
1123  {
1124  case BYTE:
1125  read_value = getAllRegisteredData[item_ptr[index].address];
1126  break;
1127 
1128  case WORD:
1129  read_value = DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address], getAllRegisteredData[item_ptr[index].address+1]);
1130  break;
1131 
1132  case DWORD:
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]));
1135  break;
1136 
1137  default:
1138  read_value = getAllRegisteredData[item_ptr[index].address];
1139  break;
1140  }
1141 
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;
1240  }
1241 
1242  dynamixel_status_pub_.publish(mx2_state);
1243 }
1244 
1245 void SingleDynamixelMonitor::MX2Ext(void)
1246 {
1247  const ControlItem* item_ptr = dynamixel_workbench_->getControlTable(dxl_id_);
1248  dynamixel_workbench_msgs::MX2Ext mx2ext_state;
1249 
1250  uint16_t last_register_addr = item_ptr[dynamixel_workbench_->getTheNumberOfControlItem(dxl_id_)-1].address;
1251  uint16_t last_register_addr_length = item_ptr[dynamixel_workbench_->getTheNumberOfControlItem(dxl_id_)-1].data_length;
1252  uint32_t getAllRegisteredData[last_register_addr+last_register_addr_length] = {0, };
1253 
1254  dynamixel_workbench_->readRegister(dxl_id_, (uint16_t)0, last_register_addr+last_register_addr_length, getAllRegisteredData);
1255 
1256  for (int index = 0; index < dynamixel_workbench_->getTheNumberOfControlItem(dxl_id_); index++)
1257  {
1258  int32_t read_value = 0;
1259  read_value = getAllRegisteredData[item_ptr[index].address];
1260 
1261  switch (item_ptr[index].data_length)
1262  {
1263  case BYTE:
1264  read_value = getAllRegisteredData[item_ptr[index].address];
1265  break;
1266 
1267  case WORD:
1268  read_value = DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address], getAllRegisteredData[item_ptr[index].address+1]);
1269  break;
1270 
1271  case DWORD:
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]));
1274  break;
1275 
1276  default:
1277  read_value = getAllRegisteredData[item_ptr[index].address];
1278  break;
1279  }
1280 
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;
1383  }
1384 
1385  dynamixel_status_pub_.publish(mx2ext_state);
1386 }
1387 
1388 void SingleDynamixelMonitor::EX(void)
1389 {
1390  const ControlItem* item_ptr = dynamixel_workbench_->getControlTable(dxl_id_);
1391  dynamixel_workbench_msgs::EX ex_state;
1392 
1393  uint16_t last_register_addr = item_ptr[dynamixel_workbench_->getTheNumberOfControlItem(dxl_id_)-1].address;
1394  uint16_t last_register_addr_length = item_ptr[dynamixel_workbench_->getTheNumberOfControlItem(dxl_id_)-1].data_length;
1395  uint32_t getAllRegisteredData[last_register_addr+last_register_addr_length] = {0, };
1396 
1397  dynamixel_workbench_->readRegister(dxl_id_, (uint16_t)0, last_register_addr+last_register_addr_length, getAllRegisteredData);
1398 
1399  for (int index = 0; index < dynamixel_workbench_->getTheNumberOfControlItem(dxl_id_); index++)
1400  {
1401  int32_t read_value = 0;
1402  read_value = getAllRegisteredData[item_ptr[index].address];
1403 
1404  switch (item_ptr[index].data_length)
1405  {
1406  case BYTE:
1407  read_value = getAllRegisteredData[item_ptr[index].address];
1408  break;
1409 
1410  case WORD:
1411  read_value = DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address], getAllRegisteredData[item_ptr[index].address+1]);
1412  break;
1413 
1414  case DWORD:
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]));
1417  break;
1418 
1419  default:
1420  read_value = getAllRegisteredData[item_ptr[index].address];
1421  break;
1422  }
1423 
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;
1492  }
1493 
1494  dynamixel_status_pub_.publish(ex_state);
1495 }
1496 
1497 void SingleDynamixelMonitor::XL320(void)
1498 {
1499  const ControlItem* item_ptr = dynamixel_workbench_->getControlTable(dxl_id_);
1500  dynamixel_workbench_msgs::XL320 xl320_state;
1501 
1502  uint16_t last_register_addr = item_ptr[dynamixel_workbench_->getTheNumberOfControlItem(dxl_id_)-1].address;
1503  uint16_t last_register_addr_length = item_ptr[dynamixel_workbench_->getTheNumberOfControlItem(dxl_id_)-1].data_length;
1504  uint32_t getAllRegisteredData[last_register_addr+last_register_addr_length] = {0, };
1505 
1506  dynamixel_workbench_->readRegister(dxl_id_, (uint16_t)0, last_register_addr+last_register_addr_length, getAllRegisteredData);
1507 
1508  for (int index = 0; index < dynamixel_workbench_->getTheNumberOfControlItem(dxl_id_); index++)
1509  {
1510  int32_t read_value = 0;
1511  read_value = getAllRegisteredData[item_ptr[index].address];
1512 
1513  switch (item_ptr[index].data_length)
1514  {
1515  case BYTE:
1516  read_value = getAllRegisteredData[item_ptr[index].address];
1517  break;
1518 
1519  case WORD:
1520  read_value = DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address], getAllRegisteredData[item_ptr[index].address+1]);
1521  break;
1522 
1523  case DWORD:
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]));
1526  break;
1527 
1528  default:
1529  read_value = getAllRegisteredData[item_ptr[index].address];
1530  break;
1531  }
1532 
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;
1593  }
1594 
1595  dynamixel_status_pub_.publish(xl320_state);
1596 }
1597 
1598 void SingleDynamixelMonitor::XL(void)
1599 {
1600  const ControlItem* item_ptr = dynamixel_workbench_->getControlTable(dxl_id_);
1601  dynamixel_workbench_msgs::XL xl_state;
1602 
1603  uint16_t last_register_addr = item_ptr[dynamixel_workbench_->getTheNumberOfControlItem(dxl_id_)-1].address;
1604  uint16_t last_register_addr_length = item_ptr[dynamixel_workbench_->getTheNumberOfControlItem(dxl_id_)-1].data_length;
1605  uint32_t getAllRegisteredData[last_register_addr+last_register_addr_length] = {0, };
1606 
1607  dynamixel_workbench_->readRegister(dxl_id_, (uint16_t)0, last_register_addr+last_register_addr_length, getAllRegisteredData);
1608 
1609  for (int index = 0; index < dynamixel_workbench_->getTheNumberOfControlItem(dxl_id_); index++)
1610  {
1611  int32_t read_value = 0;
1612  read_value = getAllRegisteredData[item_ptr[index].address];
1613 
1614  switch (item_ptr[index].data_length)
1615  {
1616  case BYTE:
1617  read_value = getAllRegisteredData[item_ptr[index].address];
1618  break;
1619 
1620  case WORD:
1621  read_value = DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address], getAllRegisteredData[item_ptr[index].address+1]);
1622  break;
1623 
1624  case DWORD:
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]));
1627  break;
1628 
1629  default:
1630  read_value = getAllRegisteredData[item_ptr[index].address];
1631  break;
1632  }
1633 
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;
1732  }
1733 
1734  dynamixel_status_pub_.publish(xl_state);
1735 }
1736 
1737 void SingleDynamixelMonitor::XM(void)
1738 {
1739  const ControlItem* item_ptr = dynamixel_workbench_->getControlTable(dxl_id_);
1740  dynamixel_workbench_msgs::XM xm_state;
1741 
1742  uint16_t last_register_addr = item_ptr[dynamixel_workbench_->getTheNumberOfControlItem(dxl_id_)-1].address;
1743  uint16_t last_register_addr_length = item_ptr[dynamixel_workbench_->getTheNumberOfControlItem(dxl_id_)-1].data_length;
1744  uint32_t getAllRegisteredData[last_register_addr+last_register_addr_length] = {0, };
1745 
1746  dynamixel_workbench_->readRegister(dxl_id_, (uint16_t)0, last_register_addr+last_register_addr_length, getAllRegisteredData);
1747 
1748  for (int index = 0; index < dynamixel_workbench_->getTheNumberOfControlItem(dxl_id_); index++)
1749  {
1750  int32_t read_value = 0;
1751  read_value = getAllRegisteredData[item_ptr[index].address];
1752 
1753  switch (item_ptr[index].data_length)
1754  {
1755  case BYTE:
1756  read_value = getAllRegisteredData[item_ptr[index].address];
1757  break;
1758 
1759  case WORD:
1760  read_value = DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address], getAllRegisteredData[item_ptr[index].address+1]);
1761  break;
1762 
1763  case DWORD:
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]));
1766  break;
1767 
1768  default:
1769  read_value = getAllRegisteredData[item_ptr[index].address];
1770  break;
1771  }
1772 
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;
1875  }
1876 
1877  dynamixel_status_pub_.publish(xm_state);
1878 }
1879 
1880 void SingleDynamixelMonitor::XMExt(void)
1881 {
1882  const ControlItem* item_ptr = dynamixel_workbench_->getControlTable(dxl_id_);
1883  dynamixel_workbench_msgs::XMExt xmext_state;
1884 
1885  uint16_t last_register_addr = item_ptr[dynamixel_workbench_->getTheNumberOfControlItem(dxl_id_)-1].address;
1886  uint16_t last_register_addr_length = item_ptr[dynamixel_workbench_->getTheNumberOfControlItem(dxl_id_)-1].data_length;
1887  uint32_t getAllRegisteredData[last_register_addr+last_register_addr_length] = {0, };
1888 
1889  dynamixel_workbench_->readRegister(dxl_id_, (uint16_t)0, last_register_addr+last_register_addr_length, getAllRegisteredData);
1890 
1891  for (int index = 0; index < dynamixel_workbench_->getTheNumberOfControlItem(dxl_id_); index++)
1892  {
1893  int32_t read_value = 0;
1894  read_value = getAllRegisteredData[item_ptr[index].address];
1895 
1896  switch (item_ptr[index].data_length)
1897  {
1898  case BYTE:
1899  read_value = getAllRegisteredData[item_ptr[index].address];
1900  break;
1901 
1902  case WORD:
1903  read_value = DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address], getAllRegisteredData[item_ptr[index].address+1]);
1904  break;
1905 
1906  case DWORD:
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]));
1909  break;
1910 
1911  default:
1912  read_value = getAllRegisteredData[item_ptr[index].address];
1913  break;
1914  }
1915 
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;
2024  }
2025 
2026  dynamixel_status_pub_.publish(xmext_state);
2027 }
2028 
2029 void SingleDynamixelMonitor::XH(void)
2030 {
2031  const ControlItem* item_ptr = dynamixel_workbench_->getControlTable(dxl_id_);
2032  dynamixel_workbench_msgs::XH xh_state;
2033 
2034  uint16_t last_register_addr = item_ptr[dynamixel_workbench_->getTheNumberOfControlItem(dxl_id_)-1].address;
2035  uint16_t last_register_addr_length = item_ptr[dynamixel_workbench_->getTheNumberOfControlItem(dxl_id_)-1].data_length;
2036  uint32_t getAllRegisteredData[last_register_addr+last_register_addr_length] = {0, };
2037 
2038  dynamixel_workbench_->readRegister(dxl_id_, (uint16_t)0, last_register_addr+last_register_addr_length, getAllRegisteredData);
2039 
2040  for (int index = 0; index < dynamixel_workbench_->getTheNumberOfControlItem(dxl_id_); index++)
2041  {
2042  int32_t read_value = 0;
2043  read_value = getAllRegisteredData[item_ptr[index].address];
2044 
2045  switch (item_ptr[index].data_length)
2046  {
2047  case BYTE:
2048  read_value = getAllRegisteredData[item_ptr[index].address];
2049  break;
2050 
2051  case WORD:
2052  read_value = DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address], getAllRegisteredData[item_ptr[index].address+1]);
2053  break;
2054 
2055  case DWORD:
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]));
2058  break;
2059 
2060  default:
2061  read_value = getAllRegisteredData[item_ptr[index].address];
2062  break;
2063  }
2064 
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;
2167  }
2168 
2169  dynamixel_status_pub_.publish(xh_state);
2170 }
2171 
2172 void SingleDynamixelMonitor::PRO(void)
2173 {
2174  const ControlItem* item_ptr = dynamixel_workbench_->getControlTable(dxl_id_);
2175  dynamixel_workbench_msgs::PRO pro_state;
2176 
2177  uint16_t last_register_addr = item_ptr[dynamixel_workbench_->getTheNumberOfControlItem(dxl_id_)-1].address;
2178  uint16_t last_register_addr_length = item_ptr[dynamixel_workbench_->getTheNumberOfControlItem(dxl_id_)-1].data_length;
2179  uint32_t getAllRegisteredData[last_register_addr+last_register_addr_length] = {0, };
2180 
2181  dynamixel_workbench_->readRegister(dxl_id_, (uint16_t)0, last_register_addr+last_register_addr_length, getAllRegisteredData);
2182 
2183  for (int index = 0; index < dynamixel_workbench_->getTheNumberOfControlItem(dxl_id_); index++)
2184  {
2185  int32_t read_value = 0;
2186  read_value = getAllRegisteredData[item_ptr[index].address];
2187 
2188  switch (item_ptr[index].data_length)
2189  {
2190  case BYTE:
2191  read_value = getAllRegisteredData[item_ptr[index].address];
2192  break;
2193 
2194  case WORD:
2195  read_value = DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address], getAllRegisteredData[item_ptr[index].address+1]);
2196  break;
2197 
2198  case DWORD:
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]));
2201  break;
2202 
2203  default:
2204  read_value = getAllRegisteredData[item_ptr[index].address];
2205  break;
2206  }
2207 
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;
2288  }
2289 
2290  dynamixel_status_pub_.publish(pro_state);
2291 }
2292 
2293 void SingleDynamixelMonitor::PROExt(void)
2294 {
2295  const ControlItem* item_ptr = dynamixel_workbench_->getControlTable(dxl_id_);
2296  dynamixel_workbench_msgs::PROExt proext_state;
2297 
2298  uint16_t last_register_addr = item_ptr[dynamixel_workbench_->getTheNumberOfControlItem(dxl_id_)-1].address;
2299  uint16_t last_register_addr_length = item_ptr[dynamixel_workbench_->getTheNumberOfControlItem(dxl_id_)-1].data_length;
2300  uint32_t getAllRegisteredData[last_register_addr+last_register_addr_length] = {0, };
2301 
2302  dynamixel_workbench_->readRegister(dxl_id_, (uint16_t)0, last_register_addr+last_register_addr_length, getAllRegisteredData);
2303 
2304  for (int index = 0; index < dynamixel_workbench_->getTheNumberOfControlItem(dxl_id_); index++)
2305  {
2306  int32_t read_value = 0;
2307  read_value = getAllRegisteredData[item_ptr[index].address];
2308 
2309  switch (item_ptr[index].data_length)
2310  {
2311  case BYTE:
2312  read_value = getAllRegisteredData[item_ptr[index].address];
2313  break;
2314 
2315  case WORD:
2316  read_value = DXL_MAKEWORD(getAllRegisteredData[item_ptr[index].address], getAllRegisteredData[item_ptr[index].address+1]);
2317  break;
2318 
2319  case DWORD:
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]));
2322  break;
2323 
2324  default:
2325  read_value = getAllRegisteredData[item_ptr[index].address];
2326  break;
2327  }
2328 
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;
2411  }
2412 
2413  dynamixel_status_pub_.publish(proext_state);
2414 }
single_dynamixel_monitor::SingleDynamixelMonitor::dxl_id_
uint8_t dxl_id_
Definition: single_dynamixel_monitor.h:54
main
int main(int argc, char **argv)
Definition: single_dynamixel_monitor.cpp:585
single_dynamixel_monitor::SingleDynamixelMonitor::initDynamixelInfoServer
void initDynamixelInfoServer(void)
Definition: single_dynamixel_monitor.cpp:204
single_dynamixel_monitor::SingleDynamixelMonitor::XM
void XM(void)
Definition: single_dynamixel_monitor.cpp:1737
DynamixelWorkbench::changeID
bool changeID(uint8_t id, uint8_t new_id, const char **log=NULL)
single_dynamixel_monitor::SingleDynamixelMonitor::changeId
bool changeId(uint8_t new_id)
Definition: single_dynamixel_monitor.cpp:261
DynamixelDriver::getBaudrate
uint32_t getBaudrate(void)
single_dynamixel_monitor::SingleDynamixelMonitor::MXExt
void MXExt(void)
Definition: single_dynamixel_monitor.cpp:991
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
f
f
ControlItem::address
uint16_t address
DynamixelDriver::setPacketHandler
bool setPacketHandler(float protocol_version, const char **log=NULL)
DynamixelDriver::init
bool init(const char *device_name="/dev/ttyUSB0", uint32_t baud_rate=57600, const char **log=NULL)
DynamixelWorkbench::changeProtocolVersion
bool changeProtocolVersion(uint8_t id, uint8_t version, const char **log=NULL)
single_dynamixel_monitor::SingleDynamixelMonitor::shutdownSingleDynamixelMonitor
void shutdownSingleDynamixelMonitor(void)
Definition: single_dynamixel_monitor.cpp:118
single_dynamixel_monitor.h
DynamixelWorkbench::torque
bool torque(uint8_t id, int32_t onoff, const char **log=NULL)
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
DynamixelWorkbench::itemWrite
bool itemWrite(uint8_t id, const char *item_name, int32_t data, const char **log=NULL)
DynamixelDriver::readRegister
bool readRegister(uint8_t id, uint16_t address, uint16_t length, uint32_t *data, const char **log=NULL)
DynamixelDriver::getModelName
const char * getModelName(uint8_t id, const char **log=NULL)
DynamixelDriver::writeRegister
bool writeRegister(uint8_t id, uint16_t address, uint16_t length, uint8_t *data, const char **log=NULL)
single_dynamixel_monitor::SingleDynamixelMonitor::PROExt
void PROExt(void)
Definition: single_dynamixel_monitor.cpp:2293
single_dynamixel_monitor::SingleDynamixelMonitor::dxl_baud_rate_
uint32_t dxl_baud_rate_
Definition: single_dynamixel_monitor.h:53
single_dynamixel_monitor::SingleDynamixelMonitor::AX
void AX(void)
Definition: single_dynamixel_monitor.cpp:672
ros::NodeHandle::advertiseService
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
single_dynamixel_monitor::SingleDynamixelMonitor::changeProtocolVersion
bool changeProtocolVersion(float ver)
Definition: single_dynamixel_monitor.cpp:351
single_dynamixel_monitor::SingleDynamixelMonitor::MX2
void MX2(void)
Definition: single_dynamixel_monitor.cpp:1106
single_dynamixel_monitor::SingleDynamixelMonitor::changeBaudrate
bool changeBaudrate(uint32_t new_baud_rate)
Definition: single_dynamixel_monitor.cpp:306
DynamixelDriver::reset
bool reset(uint8_t id, const char **log=NULL)
DXL_MAKEWORD
def DXL_MAKEWORD(a, b)
single_dynamixel_monitor::SingleDynamixelMonitor::dynamixelCommandMsgCallback
bool dynamixelCommandMsgCallback(dynamixel_workbench_msgs::DynamixelCommand::Request &req, dynamixel_workbench_msgs::DynamixelCommand::Response &res)
Definition: single_dynamixel_monitor.cpp:403
single_dynamixel_monitor::SingleDynamixelMonitor::XL320
void XL320(void)
Definition: single_dynamixel_monitor.cpp:1497
single_dynamixel_monitor::SingleDynamixelMonitor::dynamixel_workbench_
DynamixelWorkbench * dynamixel_workbench_
Definition: single_dynamixel_monitor.h:50
single_dynamixel_monitor::SingleDynamixelMonitor::MX
void MX(void)
Definition: single_dynamixel_monitor.cpp:882
DynamixelWorkbench::goalVelocity
bool goalVelocity(uint8_t id, int32_t value, const char **log=NULL)
DynamixelDriver::ping
bool ping(uint8_t id, uint16_t *get_model_number, const char **log=NULL)
single_dynamixel_monitor::SingleDynamixelMonitor::dynamixel_status_pub_
ros::Publisher dynamixel_status_pub_
Definition: single_dynamixel_monitor.h:42
single_dynamixel_monitor::SingleDynamixelMonitor::controlLoop
bool controlLoop()
Definition: single_dynamixel_monitor.cpp:382
ros::NodeHandle::param
bool param(const std::string &param_name, T &param_val, const T &default_val) const
single_dynamixel_monitor::SingleDynamixelMonitor::initDynamixelCommandServer
void initDynamixelCommandServer(void)
Definition: single_dynamixel_monitor.cpp:209
ros::ok
ROSCPP_DECL bool ok()
single_dynamixel_monitor::SingleDynamixelMonitor::XL
void XL(void)
Definition: single_dynamixel_monitor.cpp:1598
single_dynamixel_monitor::SingleDynamixelMonitor::XMExt
void XMExt(void)
Definition: single_dynamixel_monitor.cpp:1880
single_dynamixel_monitor::SingleDynamixelMonitor
Definition: single_dynamixel_monitor.h:33
single_dynamixel_monitor::SingleDynamixelMonitor::showDynamixelControlTable
bool showDynamixelControlTable(void)
Definition: single_dynamixel_monitor.cpp:214
DynamixelWorkbench::goalPosition
bool goalPosition(uint8_t id, int32_t value, const char **log=NULL)
DWORD
#define DWORD
BYTE
#define BYTE
ros::NodeHandle::advertise
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
single_dynamixel_monitor::SingleDynamixelMonitor::node_handle_
ros::NodeHandle node_handle_
Definition: single_dynamixel_monitor.h:37
single_dynamixel_monitor::SingleDynamixelMonitor::XH
void XH(void)
Definition: single_dynamixel_monitor.cpp:2029
single_dynamixel_monitor::SingleDynamixelMonitor::MX2Ext
void MX2Ext(void)
Definition: single_dynamixel_monitor.cpp:1245
ros::Rate::sleep
bool sleep()
WORD
#define WORD
single_dynamixel_monitor::SingleDynamixelMonitor::dynamixelStatePublish
void dynamixelStatePublish(void)
Definition: single_dynamixel_monitor.cpp:605
single_dynamixel_monitor::SingleDynamixelMonitor::device_name_
std::string device_name_
Definition: single_dynamixel_monitor.h:52
single_dynamixel_monitor::SingleDynamixelMonitor::RX
void RX(void)
Definition: single_dynamixel_monitor.cpp:777
DynamixelWorkbench::changeBaudrate
bool changeBaudrate(uint8_t id, uint32_t new_baudrate, const char **log=NULL)
single_dynamixel_monitor::SingleDynamixelMonitor::initSingleDynamixelMonitor
void initSingleDynamixelMonitor(void)
Definition: single_dynamixel_monitor.cpp:113
DynamixelWorkbench::wheelMode
bool wheelMode(uint8_t id, int32_t acceleration=0, const char **log=NULL)
single_dynamixel_monitor
Definition: single_dynamixel_monitor.h:31
DynamixelDriver::getControlTable
const ControlItem * getControlTable(uint8_t id, const char **log=NULL)
single_dynamixel_monitor::SingleDynamixelMonitor::PRO
void PRO(void)
Definition: single_dynamixel_monitor.cpp:2172
single_dynamixel_monitor::SingleDynamixelMonitor::checkValidationCommand
bool checkValidationCommand(std::string cmd)
Definition: single_dynamixel_monitor.cpp:247
single_dynamixel_monitor::SingleDynamixelMonitor::dynamixel_info_server_
ros::ServiceServer dynamixel_info_server_
Definition: single_dynamixel_monitor.h:47
DynamixelDriver::reboot
bool reboot(uint8_t id, const char **log=NULL)
millis
void millis(uint16_t msec)
Definition: single_dynamixel_monitor.cpp:23
ros::shutdown
ROSCPP_DECL void shutdown()
DynamixelDriver::getTheNumberOfControlItem
uint8_t getTheNumberOfControlItem(uint8_t id, const char **log=NULL)
single_dynamixel_monitor::SingleDynamixelMonitor::SingleDynamixelMonitor
SingleDynamixelMonitor(void)
Definition: single_dynamixel_monitor.cpp:28
single_dynamixel_monitor::SingleDynamixelMonitor::~SingleDynamixelMonitor
~SingleDynamixelMonitor(void)
Definition: single_dynamixel_monitor.cpp:108
single_dynamixel_monitor::SingleDynamixelMonitor::dynamixelInfoMsgCallback
bool dynamixelInfoMsgCallback(dynamixel_workbench_msgs::GetDynamixelInfo::Request &req, dynamixel_workbench_msgs::GetDynamixelInfo::Response &res)
Definition: single_dynamixel_monitor.cpp:389
ControlItem::data_length
uint16_t data_length
DXL_MAKEDWORD
def DXL_MAKEDWORD(a, b)
single_dynamixel_monitor::SingleDynamixelMonitor::EX
void EX(void)
Definition: single_dynamixel_monitor.cpp:1388
ros::spinOnce
ROSCPP_DECL void spinOnce()
DynamixelWorkbench::jointMode
bool jointMode(uint8_t id, int32_t velocity=0, int32_t acceleration=0, const char **log=NULL)
DynamixelDriver::getModelNumber
uint16_t getModelNumber(uint8_t id, const char **log=NULL)
DynamixelDriver::getProtocolVersion
float getProtocolVersion(void)
single_dynamixel_monitor::SingleDynamixelMonitor::dynamixel_command_server_
ros::ServiceServer dynamixel_command_server_
Definition: single_dynamixel_monitor.h:48
single_dynamixel_monitor::SingleDynamixelMonitor::initDynamixelStatePublisher
void initDynamixelStatePublisher(void)
Definition: single_dynamixel_monitor.cpp:125
DynamixelDriver::setBaudrate
bool setBaudrate(uint32_t baud_rate, const char **log=NULL)

dynamixel_workbench_single_manager
Author(s): Darby Lim
autogenerated on Mon Jun 10 2019 13:06:13