Combined status and state read managers (#13)

* Combined status and state read managers

* Updated Readme

Author: Joschi3

README.md

@@ -174,6 +174,29 @@ external_joint_measurements:
 '
 ```
 
+## Troubleshooting
+
+### Slow communication
+
+You can inspect read/write performance via the controller manager statistics topic. Look for the hardware interface’s
+reported read and write durations.
+If communication appears slow or irregular, check the following:
+
+* **USB latency timer**
+    * Many USB–TTL adapters buffer data by default (often 16 ms).
+    * Verify with:
+      ```bash
+      cat /sys/bus/usb-serial/devices/<ttyUSB0>/latency_timer
+      ```
+      It should show **1 ms**.
+* **Baud rate**
+    * Ensure the configured baud rate matches all motors.
+    * Higher baud rates (e.g. 3 M or 4.5 M) significantly reduce cycle times.
+
+* **Return Delay Time (per motor)**
+    * Each Dynamixel adds this delay before replying.
+    * Keep it **near 0** for fastest SyncRead performance
+
 ## Contribution
 
 Feel free to contribute to this project by opening an issue or a pull request.

dynamixel_ros_control/include/dynamixel_ros_control/dynamixel_hardware_interface.hpp

@@ -57,9 +57,8 @@ class DynamixelHardwareInterface : public hardware_interface::SystemInterface
 private:
   bool loadTransmissionConfiguration();
   bool processCommandInterfaceUpdates(const std::vector<std::string>& interface_updates, bool stopping);
-  bool setUpStateReadManager();
+  bool setUpStateAndStatusReadManager();
   bool setUpCmdReadManager();
-  bool setUpStatusReadManager();
   bool setUpTorqueWriteManager();
   bool setUpControlWriteManager();
 
@@ -83,7 +82,6 @@ class DynamixelHardwareInterface : public hardware_interface::SystemInterface
 
   // Read
   SyncReadManager read_manager_;
-  SyncReadManager status_read_manager_;
   SyncReadManager cmd_read_manager_;
   rclcpp::Time last_successful_read_time_;
   bool first_read_successful_{false};

dynamixel_ros_control/src/dynamixel_hardware_interface.cpp

@@ -214,8 +214,8 @@ DynamixelHardwareInterface::on_configure(const rclcpp_lifecycle::State& previous
   // }
 
   // Set up sync read / write managers
-  if (!setUpStatusReadManager() || !setUpStateReadManager() || !setUpTorqueWriteManager() ||
-      !setUpControlWriteManager() || !setUpCmdReadManager()) {
+  if (!setUpStateAndStatusReadManager() || !setUpTorqueWriteManager() || !setUpControlWriteManager() ||
+      !setUpCmdReadManager()) {
     return hardware_interface::CallbackReturn::FAILURE;
   }
 
@@ -463,13 +463,12 @@ hardware_interface::return_type DynamixelHardwareInterface::read(const rclcpp::T
     // Another operation is holding dynamixel_comm_mutex_; skipping read
     return hardware_interface::return_type::OK;
   }
-  // Check for hardware errors
-  status_read_manager_.read();
+
+  read_manager_.read();
   if (!isHardwareOk()) {
     return hardware_interface::return_type::ERROR;
   }
 
-  read_manager_.read();
   if (!read_manager_.isOk()) {
     DXL_LOG_ERROR("Read manager lost connection");
     return hardware_interface::return_type::ERROR;
@@ -616,7 +615,7 @@ bool DynamixelHardwareInterface::processCommandInterfaceUpdates(const std::vecto
   return true;
 }
 
-bool DynamixelHardwareInterface::setUpStateReadManager()
+bool DynamixelHardwareInterface::setUpStateAndStatusReadManager()
 {
   read_manager_ = SyncReadManager();
   std::unordered_map<std::string, DxlValueMappingList> register_dynamixel_mappings;
@@ -628,6 +627,8 @@ bool DynamixelHardwareInterface::setUpStateReadManager()
       register_dynamixel_mappings[register_name].push_back(
           std::make_pair<Dynamixel*, DxlValue>(joint.dynamixel.get(), DxlValue(&interface_value)));
     }
+    register_dynamixel_mappings[DXL_REGISTER_HARDWARE_ERROR].push_back(
+        std::make_pair<Dynamixel*, DxlValue>(joint.dynamixel.get(), DxlValue(&joint.dynamixel->hardware_error_status)));
   }
 
   for (const auto& [register_name, dynamixel_mapping] : register_dynamixel_mappings) {
@@ -637,19 +638,6 @@ bool DynamixelHardwareInterface::setUpStateReadManager()
   return read_manager_.init(driver_);
 }
 
-bool DynamixelHardwareInterface::setUpStatusReadManager()
-{
-  status_read_manager_ = SyncReadManager();
-  DxlValueMappingList status_mapping;
-  for (auto& [name, joint] : joints_) {
-    status_read_manager_.addDynamixel(joint.dynamixel.get());
-    status_mapping.push_back(
-        std::make_pair<Dynamixel*, DxlValue>(joint.dynamixel.get(), DxlValue(&joint.dynamixel->hardware_error_status)));
-  }
-
-  status_read_manager_.addRegister(DXL_REGISTER_HARDWARE_ERROR, status_mapping);
-  return status_read_manager_.init(driver_);
-}
 bool DynamixelHardwareInterface::setUpCmdReadManager()
 {
   cmd_read_manager_ = SyncReadManager();