From 8f180f83ee5b98f726108c7d2a59bbb6206656e2 Mon Sep 17 00:00:00 2001 From: Zheng Qu Date: Wed, 7 Jan 2026 12:32:46 +0100 Subject: [PATCH 1/2] refactor(dynamixel_hardware): update on_init function to use new parameter type - Changed the `on_init` function signature to accept `HardwareComponentInterfaceParams` --- .../include/dynamixel_hardware/dynamixel_hardware.hpp | 3 ++- dynamixel_hardware/src/dynamixel_hardware.cpp | 5 +++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp b/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp index d3d5746..bf585d0 100644 --- a/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp +++ b/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp @@ -65,7 +65,8 @@ class DynamixelHardware : public hardware_interface::SystemInterface RCLCPP_SHARED_PTR_DEFINITIONS(DynamixelHardware) DYNAMIXEL_HARDWARE_PUBLIC - CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; + CallbackReturn on_init( + const hardware_interface::HardwareComponentInterfaceParams & params) override; DYNAMIXEL_HARDWARE_PUBLIC std::vector export_state_interfaces() override; diff --git a/dynamixel_hardware/src/dynamixel_hardware.cpp b/dynamixel_hardware/src/dynamixel_hardware.cpp index 720db9e..e59ee69 100644 --- a/dynamixel_hardware/src/dynamixel_hardware.cpp +++ b/dynamixel_hardware/src/dynamixel_hardware.cpp @@ -48,10 +48,11 @@ constexpr const char * const kExtraJointParameters[] = { "Velocity_I_Gain", }; -CallbackReturn DynamixelHardware::on_init(const hardware_interface::HardwareInfo & info) +CallbackReturn DynamixelHardware::on_init( + const hardware_interface::HardwareComponentInterfaceParams & params) { RCLCPP_DEBUG(rclcpp::get_logger(kDynamixelHardware), "configure"); - if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) { + if (hardware_interface::SystemInterface::on_init(params) != CallbackReturn::SUCCESS) { return CallbackReturn::ERROR; } From 956c4641c1157786c02af66d809ac68823837a1d Mon Sep 17 00:00:00 2001 From: Zheng Qu Date: Wed, 30 Jul 2025 00:02:41 +0200 Subject: [PATCH 2/2] Add interface to conditionally enable torque --- .../dynamixel_hardware/dynamixel_hardware.hpp | 3 ++- dynamixel_hardware/src/dynamixel_hardware.cpp | 17 ++++++++++++++--- 2 files changed, 16 insertions(+), 4 deletions(-) diff --git a/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp b/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp index bf585d0..84fc64f 100644 --- a/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp +++ b/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp @@ -101,7 +101,8 @@ class DynamixelHardware : public hardware_interface::SystemInterface std::map control_items_; std::vector joints_; std::vector joint_ids_; - bool torque_enabled_{false}; + bool should_enable_torque_{false}; // True if torque should be enabled. False otherwise. + bool torque_enabled_{false}; // True if torque is enabled. False otherwise. ControlMode control_mode_{ControlMode::Position}; bool mode_changed_{false}; bool use_dummy_{false}; diff --git a/dynamixel_hardware/src/dynamixel_hardware.cpp b/dynamixel_hardware/src/dynamixel_hardware.cpp index e59ee69..493df6f 100644 --- a/dynamixel_hardware/src/dynamixel_hardware.cpp +++ b/dynamixel_hardware/src/dynamixel_hardware.cpp @@ -82,6 +82,15 @@ CallbackReturn DynamixelHardware::on_init( return CallbackReturn::SUCCESS; } + if ( + info_.hardware_parameters.find("enable_torque") != info_.hardware_parameters.end() && + info_.hardware_parameters.at("enable_torque") == "false") + { + should_enable_torque_ = false; + } else { + should_enable_torque_ = true; + } + auto usb_port = info_.hardware_parameters.at("usb_port"); auto baud_rate = std::stoi(info_.hardware_parameters.at("baud_rate")); const char * log = nullptr; @@ -105,7 +114,9 @@ CallbackReturn DynamixelHardware::on_init( enable_torque(false); set_control_mode(ControlMode::Position, true); set_joint_params(); - enable_torque(true); + if (should_enable_torque_) { + enable_torque(true); + } const ControlItem * goal_position = dynamixel_workbench_.getItemInfo(joint_ids_[0], kGoalPositionItem); @@ -411,7 +422,7 @@ return_type DynamixelHardware::set_control_mode(const ControlMode & mode, const control_mode_ = ControlMode::Velocity; } - if (torque_enabled) { + if (torque_enabled && should_enable_torque_) { enable_torque(true); } return return_type::OK; @@ -435,7 +446,7 @@ return_type DynamixelHardware::set_control_mode(const ControlMode & mode, const control_mode_ = ControlMode::Position; } - if (torque_enabled) { + if (torque_enabled && should_enable_torque_) { enable_torque(true); } return return_type::OK;