Add Support for Various Command Interfaces

[Woojin-Crive]

Changes

• Added proper command interface support with ROS2-Dynamixel interface mapping
• Improved error handling and robustness throughout the codebase
• Implemented per-device torque enable control (replacing global control)
• Added support for new sensor model (sensorxel_joy)
• Enhanced joint state-command synchronization
• Improved parameter initialization organization
• Added robust error handling for model file reading

Technical Details

1. Command Interface Mapping:

   • Added ros2_to_dxl_cmd_map and dxl_to_ros2_cmd_map for standardized interface name conversion
   • Improved handling of position, velocity, and effort commands

2. Torque Control:

   • Replaced global torque enable with per-device control
   • Added dxl_torque_enable_ map to track individual device states

3. Error Handling:

   • Added try-catch blocks in critical sections
   • Improved error reporting and logging
   • Enhanced model file reading with proper error handling

4. State Management:

   • Streamlined state calculation logic
   • Improved synchronization between joint states and commands
   • Better organization of parameter initialization

5. Model Support:

   • Added support for sensorxel_joy model
   • Updated existing model files with new parameters

Testing

Please test the following scenarios:

1. Command interface functionality for position, velocity, and effort
2. Per-device torque control
3. Error handling with invalid configurations
4. State synchronization
5. New sensor model integration

Breaking Changes

• Removed global torque enable parameter in favor of per-device control
• Updated command interface naming convention

[gemini-code-assist]

Hello @Woojin-Crive, I'm Gemini Code Assist¹! I'm currently reviewing this pull request and will post my feedback shortly. In the meantime, here's a summary to help you and other reviewers quickly get up to speed!

Summary of Changes

As Gemini, your code assistant, I've reviewed this pull request. This PR introduces significant enhancements to the dynamixel hardware interface, primarily focusing on adding robust support for various standard ROS2 command interfaces (position, velocity, and effort) on a per-device level. It moves away from a global torque enable setting to individual control for each Dynamixel. The changes also include substantial improvements to error handling, particularly around reading and parsing model files, and a more structured approach to parameter initialization. Support for a new sensor model, sensorxel_joy, has been integrated, and the synchronization logic between joint states and commands upon startup has been refined.

Highlights

• Command Interface Support: Implemented mapping between standard ROS2 command interfaces (position, velocity, effort) and corresponding Dynamixel control items (Goal Position, Goal Velocity, Goal Current). This allows for more flexible and standard control modes per device.
• Per-Device Torque Control: Replaced the single global torque enable parameter with individual torque enable control for each Dynamixel. This state is now managed per device based on parameters.
• Enhanced Error Handling: Added robust error handling using try-catch blocks in critical sections, especially during model file parsing and device initialization, making the interface more resilient to configuration errors.
• New Sensor Model Integration: Added support for the sensorxel_joy model, including its control table definition and integration into the hardware interface's read loop for sensor data.
• Improved State/Command Synchronization: Refined the logic for synchronizing initial joint commands with the current joint states upon startup, specifically targeting the position interface.
• Parameter Initialization Refactor: Reorganized the process of initializing Dynamixel parameters from the configuration, ensuring Operating Mode and Limit parameters are written before other control items.

Changelog

Click here to see the changelog

• include/dynamixel_hardware_interface/dynamixel_hardware_interface.hpp
  • Added include for <unordered_map>.
  • Removed old #define constants for GOAL_POSITION_INDEX, GOAL_VELOCITY_INDEX, and GOAL_CURRENT_INDEX.
  • Added a std::map<uint8_t, bool> dxl_torque_enable_ member to track the torque enable state for each Dynamixel ID.
  • Removed the bool global_torque_enable_ member.
  • Added inline const std::unordered_map constants ros2_to_dxl_cmd_map and dxl_to_ros2_cmd_map for converting between ROS2 and Dynamixel command interface names.
• param/dxl_model/dynamixel.model
  • Added a new entry mapping model number 536 to the model file sensorxel_joy.model.
• param/dxl_model/ph42_020_s300.model
  • Modified entries in the control table definition, specifically adding Indirect Address 1 and Indirect Data 1, and changing the address for Indirect Address Read.
• param/dxl_model/sensorxel_joy.model
  • Added a new model file defining the control table for the sensorxel_joy device, including addresses and sizes for various control items like Model Number, Firmware, ID, Baud Rate, Error Code, Timestamps, Joystick values (Tact Switch, X, Y), and IMU data (Acc X, Y, Z, Sum).
• src/dynamixel/dynamixel.cpp
  • Wrapped calls to dxl_info_.ReadDxlModelFile and dxl_info_.CheckDxlControlItem in InitDxlComm with try-catch blocks to handle potential exceptions during initialization.
  • Added a check for write_data_list_.empty() before accessing its first element in SetMultiDxlWrite to prevent potential crashes if the list is empty.
  • Reordered the handling of "Goal Velocity" and "Goal Current" when setting values for SyncWrite and BulkWrite, ensuring "Goal Velocity" is checked before "Goal Current".
• src/dynamixel/dynamixel_info.cpp
  • Replaced return and exit(-1) with throw std::runtime_error in ReadDxlModelFile for better error propagation when model files are not found or cannot be opened.
  • Added checks for minimum column count (strs.size() < 2 or < 3) and try-catch blocks when parsing lines in the model file to handle malformed entries gracefully.
  • Added a check to ensure the control table is not empty after parsing the model file.
• src/dynamixel_hardware_interface.cpp
  • Added includes for <map> and <unordered_map>.
  • Removed the parsing of the global_torque_enable parameter in the on_init method.
  • Added try-catch blocks and size checks in export_state_interfaces and export_command_interfaces to improve robustness during interface export.
  • Moved the call to ReadSensorData for GPIO sensors to occur before the main Dynamixel read loop in the read method.
  • Modified the start method to remove the old manual joint state-to-command sync loop and the global torque enable check. It now calls the new SyncJointCommandWithStates function and enables torque based on the per-device dxl_torque_enable_ map.
  • Refactored the initItems method to iterate through parameters in a specific order (Operating Mode, then Limits, then others) and to handle the per-device "Torque Enable" parameter.
  • Refactored InitDxlReadItems to simplify the loop for initializing sensor states.
  • Refactored InitDxlWriteItems to iterate through the component's command interfaces and add only the supported ROS2 interfaces (Goal Position, Goal Velocity, Goal Current) to the internal handler list, filtering out non-dxl types.
  • Refactored CalcTransmissionToJoint to use a single loop iterating over state indices (Position, Velocity, Effort) for calculating joint states from transmission states.
  • Refactored CalcJointToTransmission to iterate through transmission command interfaces and apply the matrix multiplication for each, including the prismatic conversion for the Position interface.
  • Implemented the SyncJointCommandWithStates function to find and sync the position interface value between joint states and commands using the standard ROS2 interface name (hardware_interface::HW_IF_POSITION).

Using Gemini Code Assist

The full guide for Gemini Code Assist can be found on our documentation page, here are some quick tips.

Invoking Gemini

You can request assistance from Gemini at any point in your pull request via creating an issue comment (i.e. comment on the pull request page) using either /gemini <command> or @gemini-code-assist <command>. Below is a summary of the supported commands.

Feature	Command	Description
Code Review	/gemini review	Performs a code review for the current pull request in its current state.
Pull Request Summary	/gemini summary	Provides a summary of the current pull request in its current state.
Comment	@gemini-code-assist	Responds in comments when explicitly tagged, both in issue comments and review comments.
Help	/gemini help	Displays a list of available commands.

Customization

To customize Gemini Code Assist for GitHub experience, repository maintainers can create a configuration file and/or provide a custom code review style guide (such as PEP-8 for Python) by creating and adding files to a .gemini/ folder in the base of the repository. Detailed instructions can be found here.

Limitations & Feedback

Gemini Code Assist is currently in preview and may make mistakes. Please leave feedback on any instances where its feedback is incorrect or counter productive. You can react with 👍 and 👎 on @gemini-code-assist comments to provide feedback.

You can also get AI-powered code generation, chat, as well as code reviews directly in the IDE at no cost with the Gemini Code Assist IDE Extension.

---

Interfaces align,
Position, speed, and force,
Hardware takes command.

Footnotes

1. Review the Privacy Notices, Generative AI Prohibited Use Policy, Terms of Service, and learn how to configure Gemini Code Assist in GitHub here. Gemini can make mistakes, so double check it and use code with caution. ↩

[gemini-code-assist]

Code Review

This pull request introduces significant improvements, including per-device torque control, enhanced error handling, and better organization of parameter initialization. The addition of the sensorxel_joy model is also a valuable contribution. The refactoring of state and command interface handling is generally positive, improving clarity and reducing duplication in some areas.

Summary of Findings

• Incorrect Transmission Calculation: The CalcJointToTransmission function incorrectly sums values from all joint command interfaces when calculating transmission commands. This needs to be revised to correctly map specific joint command types (position, velocity, effort) to corresponding transmission command types using the transmission matrix.
• Potential Velocity Truncation: In SetDxlValueToSyncWrite, the converted velocity value is cast to int16_t, which could lead to truncation if the value exceeds this range. The corresponding BulkWrite function uses int32_t, which is safer. This inconsistency and potential data loss should be addressed.
• Arbitrary Command Ordering: The order of handling "Goal Velocity" and "Goal Current" in the if-else if chains within SetDxlValueToSyncWrite and SetDxlValueToBulkWrite is arbitrary. Consistent ordering (e.g., alphabetical or by control table address) would improve code maintainability.
• Suppressed Error Logging: Several fprintf(stderr, ...) calls within catch blocks in src/dynamixel/dynamixel_info.cpp are commented out. This prevents important error messages related to model file parsing from being logged, hindering debugging. (Commented on)
• Inconsistent Logging: The code uses a mix of fprintf(stderr, ...) and ROS2 logging macros (RCLCPP_INFO_STREAM, etc.). Using ROS2 logging consistently throughout the codebase is recommended for better integration with the ROS2 ecosystem. (Not commented on due to settings)
• Magic Numbers for Error Bits: The CheckError function uses magic numbers (e.g., 0x01, 0x04) to check hardware error status bits. Replacing these with named constants would improve code readability and maintainability. (Not commented on due to settings)
• Raw Pointer Usage for Matrices: The use of raw double ** pointers for the transmission matrices requires manual memory management (new/delete). Using C++ standard library containers like std::vector<std::vector<double>> or a dedicated matrix library would be more idiomatic and safer. (Not commented on due to settings)
• Use of usleep: The code uses usleep for delays. Using std::this_thread::sleep_for or ROS2 timers is generally preferred for better portability and integration with the ROS2 event loop. (Not commented on due to settings)
• Redundant is_set_hdl_ Flag: The is_set_hdl_ flag in InitDxlReadItems and InitDxlWriteItems appears redundant as these initialization functions are typically called only once during the on_init lifecycle transition. (Not commented on due to settings)

Merge Readiness

This pull request introduces valuable features and improvements, particularly in error handling and parameter initialization. However, a critical correctness issue has been identified in the CalcJointToTransmission function, which needs to be resolved before merging. Additionally, a high-severity issue related to potential velocity command truncation in SetDxlValueToSyncWrite should be addressed. Please make the necessary changes and request another review. I am unable to approve this pull request, and users should have others review and approve this code before merging.

[yun-goon]

Version work is required.

[robotpilot]

Please include changelog changes and bump!

[robotpilot]

@Woojin-Crive Please see the PR below and include the handling of ament_target_dependencies issues in this PR.
#37

[yun-goon]

Good!

[sunghowoo]

LGTM!