read all registers at once in dynamixel_driver (improved single_dynam…

…ixel_monitor state publishing from 1Hz to 55Hz)
jysung · Jul 17, 2017 · 26fc2a2 · 26fc2a2
1 parent abdf325
commit 26fc2a2
Show file tree

Hide file tree

Showing 3 changed files with 63 additions and 3 deletions.
diff --git a/dynamixel_workbench_single_manager/src/single_dynamixel_monitor.cpp b/dynamixel_workbench_single_manager/src/single_dynamixel_monitor.cpp
@@ -1016,17 +1016,24 @@ bool SingleDynamixelMonitor::XL()
 
 bool SingleDynamixelMonitor::XM()
 {
-  int32_t read_value = 0;
+
+  uint8_t read_value_array[200] = {0};
+
+  // read all registers (upto "Present Temperature")
+  dynamixel_driver_->readAllRegister(147, read_value_array);
 
   dynamixel_workbench_msgs::XM xm_state;
   dynamixel_tool::DynamixelTool *dynamixel = dynamixel_driver_->dynamixel_;
 
+  int32_t read_value = 0;
   for (dynamixel->it_ctrl_ = dynamixel->ctrl_table_.begin();
        dynamixel->it_ctrl_ != dynamixel->ctrl_table_.end();
        dynamixel->it_ctrl_++)
   {
     dynamixel->item_ = dynamixel->ctrl_table_[dynamixel->it_ctrl_->first.c_str()];
-    dynamixel_driver_->readRegister(dynamixel->item_->item_name ,&read_value);
+
+    // lookup each value of register
+    dynamixel_driver_->lookupLoadedRegisterValue(read_value_array, dynamixel->item_->item_name, &read_value);
 
     if ("model_number" == dynamixel->item_->item_name)
       xm_state.model_number = read_value;
@@ -1130,11 +1137,12 @@ bool SingleDynamixelMonitor::XM()
       xm_state.present_input_voltage = read_value;
     else if ("present_temperature" == dynamixel->item_->item_name)
       xm_state.present_temperature = read_value;
-  }
 
+  }
   dynamixel_status_pub_.publish(xm_state);
 }
 
+
 bool SingleDynamixelMonitor::XH()
 {
   int32_t read_value = 0;

diff --git a/dynamixel_workbench_toolbox/include/dynamixel_workbench_toolbox/dynamixel_driver.h b/dynamixel_workbench_toolbox/include/dynamixel_workbench_toolbox/dynamixel_driver.h
@@ -70,6 +70,9 @@ class DynamixelDriver
 
   bool writeRegister(std::string addr_name, uint32_t value);
   bool readRegister(std::string addr_name, int32_t *value);
+
+  bool readAllRegister(int last_address, uint8_t *value_array);
+  bool lookupLoadedRegisterValue(uint8_t *read_register_array, std::string addr_name, int32_t *value);
 };
 }
 

diff --git a/dynamixel_workbench_toolbox/src/dynamixel_driver.cpp b/dynamixel_workbench_toolbox/src/dynamixel_driver.cpp
@@ -214,6 +214,55 @@ bool DynamixelDriver::readRegister(std::string addr_name, int32_t *value)
   }
 }
 
+bool DynamixelDriver::readAllRegister(int last_address, uint8_t *value_array)
+{
+  uint8_t error = 0;
+  int comm_result = COMM_RX_FAIL;
+
+  // read all reigsters starting from 0
+  comm_result = packetHandler_->readTxRx(portHandler_, dynamixel_->id_, 0, last_address, value_array, &error);
+
+  if (comm_result == COMM_SUCCESS)
+  {
+    if (error != 0)
+    {
+      packetHandler_->printRxPacketError(error);
+    }
+
+    return true;
+  }
+  else
+  {
+    packetHandler_->printTxRxResult(comm_result);
+
+    ROS_ERROR("[ID] %u, Fail to read all registers!", dynamixel_->id_);
+    return false;
+  }
+
+}
+
+bool DynamixelDriver::lookupLoadedRegisterValue(uint8_t *read_register_array, std::string addr_name, int32_t *value)
+{
+  dynamixel_->item_ = dynamixel_->ctrl_table_[addr_name];
+  dynamixel_tool::ControlTableItem *addr_item = dynamixel_->item_;
+
+  if (addr_item->data_length == 1)
+  {
+    *value = read_register_array[addr_item->address];
+  }
+  else if (addr_item->data_length == 2)
+  {
+    *value = DXL_MAKEWORD(read_register_array[addr_item->address], read_register_array[addr_item->address + 1]);
+  }
+  else if (addr_item->data_length == 4)
+  {
+    *value = DXL_MAKEDWORD(DXL_MAKEWORD(read_register_array[addr_item->address], read_register_array[addr_item->address + 1]),
+                           DXL_MAKEWORD(read_register_array[addr_item->address + 2], read_register_array[addr_item->address + 3]));
+  }
+
+  return true;
+}
+
 bool DynamixelDriver::reboot()
 {
   if (getProtocolVersion() != 2.0)