Merge pull request #15 from ROBOTIS-GIT/bump-1.3.0

Bump 1.3.0

Author: robotpilot

.github/workflows/ros-ci.yml

@@ -1,45 +1,57 @@
-name: ros-ci
+# The name of the workflow
+name: CI
 
-# Controls when the action will run. Triggers the workflow on push or pull request
+# Specifies the events that trigger the workflow
 on:
   push:
-    branches: [ humble ]
+    branches: [ main, humble, jazzy ]
   pull_request:
-    branches: [ humble ]
+    branches: [ main, humble, jazzy ]
 
-# A workflow run is made up of one or more jobs that can run sequentially or in parallel
+# Defines a set of jobs to be run as part of the workflow
 jobs:
-  humble:
+  # The name of the job
+  ROS_CI:
     runs-on: ubuntu-22.04
     strategy:
       fail-fast: false
       matrix:
         ros_distribution:
           - humble
+          - jazzy
+          - rolling
         include:
+          # ROS 2 Humble Hawksbill
           - docker_image: ubuntu:jammy
             ros_distribution: humble
             ros_version: 2
-          #   # Rolling
-          # - docker_image: ubuntu:jammy
-          #   ros_distribution: rolling
-          #   ros_version: 2
+          # ROS 2 Jazzy Jalisco
+          - docker_image: ubuntu:noble
+            ros_distribution: jazzy
+            ros_version: 2
+          # ROS 2 Rolling Ridley
+          - docker_image: ubuntu:noble
+            ros_distribution: rolling
+            ros_version: 2
     container:
       image: ${{ matrix.docker_image }}
     steps:
-      - name: Setup directories
+      - name: Setup workspace
         run: mkdir -p ros_ws/src
-      - name: checkout
-        uses: actions/checkout@v3
+
+      - name: Checkout code
+        uses: actions/checkout@v4
         with:
           path: ros_ws/src
+
       - name: Setup ROS environment
-        uses: ros-tooling/setup-ros@0.7.1
+        uses: ros-tooling/setup-ros@v0.7
         with:
           required-ros-distributions: ${{ matrix.ros_distribution }}
+
       - name: Build and Test
-        uses: ros-tooling/action-ros-ci@0.3.5
+        uses: ros-tooling/action-ros-ci@v0.3
         with:
-          package-name: dynamixel_hardware_interface
           target-ros2-distro: ${{ matrix.ros_distribution }}
           vcs-repo-file-url: "https://raw.githubusercontent.com/ROBOTIS-GIT/dynamixel_hardware_interface/main/dynamixel_hardware_interface_ci.repos"
+          package-name: dynamixel_hardware_interface

.github/workflows/ros-lint.yml

@@ -0,0 +1,32 @@
+# The name of the workflow
+name: Lint
+
+# Specifies the events that trigger the workflow
+on:
+  pull_request:
+
+# Defines a set of jobs to be run as part of the workflow
+jobs:
+  ament_lint:
+    runs-on: ubuntu-latest
+    container:
+      image: rostooling/setup-ros-docker:ubuntu-noble-ros-rolling-ros-base-latest
+    strategy:
+      fail-fast: false
+      matrix:
+        linter: [cppcheck, cpplint, uncrustify, flake8, pep257, lint_cmake, xmllint, copyright]
+    steps:
+      - name: Checkout code
+        uses: actions/checkout@v4
+
+      - name: Setup ROS environment
+        uses: ros-tooling/[email protected]
+
+      - name: Run Linter
+        env:
+          AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: 1
+        uses: ros-tooling/action-ros-lint@master
+        with:
+          linter: ${{ matrix.linter }}
+          distribution: rolling
+          package-name: "*"

CHANGELOG.rst

@@ -2,6 +2,13 @@
 Changelog for package dynamixel_hardware_interface
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.3.0 (2025-02-17)
+------------------
+* Enhance Error Handling and Timeout Management
+* Use GroupFastSyncRead and GroupFastBulkRead
+* Remove deprecated parameter ros_update_freq_ to prevent stoi failure
+* Contributors: Woojin Wie
+
 1.2.0 (2025-01-17)
 ------------------
 * Extend Bulk/Sync Selection Logic to Include Indirect Operations

README.md

@@ -11,13 +11,6 @@ ROS 2 package providing a hardware interface for controlling [Dynamixel](https:/
 
 This package currently supports ROS 2 Humble only. Ensure that ROS 2 Humble is properly installed (ROS 2 Humble installation guide).
 
-- [Dynamixel SDK](https://github.com/ROBOTIS-GIT/DynamixelSDK):
-  - Install the Dynamixel SDK using the following command:
-
-    ```bash
-    sudo apt install ros-humble-dynamixel-sdk
-    ```
-
 - Hardware Requirements:
 
   - Dynamixel servos
@@ -31,6 +24,7 @@ This package currently supports ROS 2 Humble only. Ensure that ROS 2 Humble is p
 
    ```bash
    cd ~/${WORKSPACE}/src
+   git clone -b humble https://github.com/ROBOTIS-GIT/DynamixelSDK.git
    git clone -b humble https://github.com/ROBOTIS-GIT/dynamixel_hardware_interface.git
    git clone -b humble https://github.com/ROBOTIS-GIT/dynamixel_interfaces.git
    ```

dynamixel_hardware_interface_ci.repos

@@ -2,8 +2,8 @@ repositories:
   utils/DynamixelSDK:
     type: git
     url: https://github.com/ROBOTIS-GIT/DynamixelSDK.git
-    version: humble
+    version: main
   dynamixel_hardware_interface/dynamixel_interfaces:
     type: git
     url: https://github.com/ROBOTIS-GIT/dynamixel_interfaces.git
-    version: humble
+    version: main

include/dynamixel_hardware_interface/dynamixel/dynamixel.hpp

@@ -125,12 +125,12 @@ class Dynamixel
   std::vector<RWItemList> read_data_list_;
 
   // sync read
-  dynamixel::GroupSyncRead * group_sync_read_;
+  dynamixel::GroupFastSyncRead * group_sync_read_;
   // indirect inform for sync read
   std::map<uint8_t /*id*/, IndirectInfo> indirect_info_read_;
 
   // bulk read
-  dynamixel::GroupBulkRead * group_bulk_read_;
+  dynamixel::GroupFastBulkRead * group_bulk_read_;
 
   // write item (sync or bulk) variable
   bool write_type_;

include/dynamixel_hardware_interface/dynamixel_hardware_interface.hpp

@@ -38,8 +38,8 @@
 #include "dynamixel_interfaces/srv/set_data_to_dxl.hpp"
 #include "dynamixel_interfaces/srv/reboot_dxl.hpp"
 
-#include "realtime_tools/realtime_publisher.h"
-#include "realtime_tools/realtime_buffer.h"
+#include "realtime_tools/realtime_publisher.hpp"
+#include "realtime_tools/realtime_buffer.hpp"
 
 #include "std_srvs/srv/set_bool.hpp"
 
@@ -181,7 +181,12 @@ class DynamixelHardware : public
   std::map<uint8_t /*id*/, uint8_t /*err*/> dxl_hw_err_;
   DxlTorqueStatus dxl_torque_status_;
   std::map<uint8_t /*id*/, bool /*enable*/> dxl_torque_state_;
-  double err_timeout_sec_;
+  double err_timeout_ms_;
+  rclcpp::Duration read_error_duration_{0, 0};
+  rclcpp::Duration write_error_duration_{0, 0};
+  bool is_read_in_error_{false};
+  bool is_write_in_error_{false};
+
   bool use_revolute_to_prismatic_{false};
   std::string conversion_dxl_name_{""};
   std::string conversion_joint_name_{""};
@@ -322,8 +327,6 @@ class DynamixelHardware : public
   double revoluteToPrismatic(double revolute_value);
 
   double prismaticToRevolute(double prismatic_value);
-
-  int ros_update_freq_;
 };
 
 }  // namespace dynamixel_hardware_interface

package.xml

@@ -1,28 +1,25 @@
 <?xml version='1.0' encoding='UTF-8'?>
 <package format="3">
   <name>dynamixel_hardware_interface</name>
-  <version>1.2.0</version>
+  <version>1.3.0</version>
   <description>
     ROS 2 package providing a hardware interface for controlling Dynamixel motors via the ROS 2 control framework.
   </description>
   <maintainer email="[email protected]">Pyo</maintainer>
   <license>Apache 2.0</license>
   <author email="[email protected]">Hye-Jong KIM</author>
   <author email="[email protected]">Sungho Woo</author>
-
+  <author email="[email protected]">Woojin Wie</author>
   <buildtool_depend>ament_cmake</buildtool_depend>
-
   <depend>rclcpp</depend>
   <depend>hardware_interface</depend>
   <depend>pluginlib</depend>
   <depend>realtime_tools</depend>
   <depend>dynamixel_sdk</depend>
   <depend>std_srvs</depend>
   <depend>dynamixel_interfaces</depend>
-
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
-
   <export>
     <build_type>ament_cmake</build_type>
   </export>

src/dynamixel/dynamixel.cpp

@@ -799,7 +799,7 @@ DxlError Dynamixel::SetSyncReadHandler(std::vector<uint8_t> id_arr)
     IN_ADDR, indirect_info_read_[id_arr.at(0)].size);
 
   group_sync_read_ =
-    new dynamixel::GroupSyncRead(
+    new dynamixel::GroupFastSyncRead(
     port_handler_, packet_handler_,
     IN_ADDR, indirect_info_read_[id_arr.at(0)].size);
 
@@ -917,7 +917,7 @@ DxlError Dynamixel::SetBulkReadHandler(std::vector<uint8_t> id_arr)
       IN_ADDR, indirect_info_read_[id_arr.at(0)].size);
   }
 
-  group_bulk_read_ = new dynamixel::GroupBulkRead(port_handler_, packet_handler_);
+  group_bulk_read_ = new dynamixel::GroupFastBulkRead(port_handler_, packet_handler_);
 
   for (auto it_id : id_arr) {
     uint8_t ID = it_id;

src/dynamixel_hardware_interface.cpp

@@ -35,7 +35,11 @@ DynamixelHardware::DynamixelHardware()
 {
   dxl_status_ = DXL_OK;
   dxl_torque_status_ = TORQUE_ENABLED;
-  err_timeout_sec_ = 3.0;
+  err_timeout_ms_ = 500;
+  is_read_in_error_ = false;
+  is_write_in_error_ = false;
+  read_error_duration_ = rclcpp::Duration(0, 0);
+  write_error_duration_ = rclcpp::Duration(0, 0);
 }
 
 DynamixelHardware::~DynamixelHardware()
@@ -64,7 +68,13 @@ hardware_interface::CallbackReturn DynamixelHardware::on_init(
 
   port_name_ = info_.hardware_parameters["port_name"];
   baud_rate_ = info_.hardware_parameters["baud_rate"];
-  err_timeout_sec_ = stod(info_.hardware_parameters["error_timeout_sec"]);
+  try {
+    err_timeout_ms_ = stod(info_.hardware_parameters["error_timeout_ms"]);
+  } catch (const std::exception & e) {
+    RCLCPP_ERROR(
+      logger_, "Failed to parse error_timeout_ms parameter: %s, using default value",
+      e.what());
+  }
 
   RCLCPP_INFO_STREAM(
     logger_,
@@ -269,8 +279,6 @@ hardware_interface::CallbackReturn DynamixelHardware::on_init(
     str_set_dxl_torque_srv_name,
     std::bind(&DynamixelHardware::set_dxl_torque_srv_callback, this, _1, _2));
 
-  ros_update_freq_ = stoi(info_.hardware_parameters["ros_update_freq"]);
-
   return hardware_interface::CallbackReturn::SUCCESS;
 }
 
@@ -394,11 +402,24 @@ hardware_interface::return_type DynamixelHardware::read(
   } else if (dxl_status_ == DXL_OK || dxl_status_ == COMM_ERROR) {
     dxl_comm_err_ = CheckError(dxl_comm_->ReadMultiDxlData());
     if (dxl_comm_err_ != DxlError::OK) {
+      if (!is_read_in_error_) {
+        is_read_in_error_ = true;
+        read_error_duration_ = rclcpp::Duration(0, 0);
+      }
+      read_error_duration_ = read_error_duration_ + period;
+
       RCLCPP_ERROR_STREAM(
         logger_,
-        "Dynamixel Read Fail :" << Dynamixel::DxlErrorToString(dxl_comm_err_));
-      return hardware_interface::return_type::ERROR;
+        "Dynamixel Read Fail (Duration: " << read_error_duration_.seconds() * 1000 << "ms/" <<
+          err_timeout_ms_ << "ms)");
+
+      if (read_error_duration_.seconds() * 1000 >= err_timeout_ms_) {
+        return hardware_interface::return_type::ERROR;
+      }
+      return hardware_interface::return_type::OK;
     }
+    is_read_in_error_ = false;
+    read_error_duration_ = rclcpp::Duration(0, 0);
   } else if (dxl_status_ == HW_ERROR) {
     dxl_comm_err_ = CheckError(dxl_comm_->ReadMultiDxlData());
     if (dxl_comm_err_ != DxlError::OK) {
@@ -434,7 +455,6 @@ hardware_interface::return_type DynamixelHardware::read(
   }
   return hardware_interface::return_type::OK;
 }
-
 hardware_interface::return_type DynamixelHardware::write(
   const rclcpp::Time & time, const rclcpp::Duration & period)
 {
@@ -447,16 +467,29 @@ hardware_interface::return_type DynamixelHardware::write(
 
     dxl_comm_->WriteMultiDxlData();
 
+    is_write_in_error_ = false;
+    write_error_duration_ = rclcpp::Duration(0, 0);
+
     return hardware_interface::return_type::OK;
   } else {
-    RCLCPP_ERROR_STREAM(logger_, "Dynamixel Write Fail");
-    return hardware_interface::return_type::ERROR;
+    write_error_duration_ = write_error_duration_ + period;
+
+    RCLCPP_ERROR_STREAM(
+      logger_,
+      "Dynamixel Write Fail (Duration: " << write_error_duration_.seconds() * 1000 << "ms/" <<
+        err_timeout_ms_ << "ms)");
+
+    if (write_error_duration_.seconds() * 1000 >= err_timeout_ms_) {
+      return hardware_interface::return_type::ERROR;
+    }
+    return hardware_interface::return_type::OK;
   }
 }
 
 DxlError DynamixelHardware::CheckError(DxlError dxl_comm_err)
 {
   DxlError error_state = DxlError::OK;
+  dxl_status_ = DXL_OK;
 
   // check comm error
   if (dxl_comm_err != DxlError::OK) {