From abab217f3d3c2a7a3618062cf50228268b6caab1 Mon Sep 17 00:00:00 2001 From: Soham Patil Date: Sun, 12 May 2024 15:04:49 +0530 Subject: [PATCH 1/2] added delayed read write --- .../dynamixel_hardware/dynamixel_hardware.hpp | 1 + dynamixel_hardware/src/dynamixel_hardware.cpp | 15 ++++++++++++++- 2 files changed, 15 insertions(+), 1 deletion(-) diff --git a/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp b/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp index d3d5746..6ae84d5 100644 --- a/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp +++ b/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp @@ -104,6 +104,7 @@ class DynamixelHardware : public hardware_interface::SystemInterface ControlMode control_mode_{ControlMode::Position}; bool mode_changed_{false}; bool use_dummy_{false}; + bool activated_{false}; }; } // namespace dynamixel_hardware diff --git a/dynamixel_hardware/src/dynamixel_hardware.cpp b/dynamixel_hardware/src/dynamixel_hardware.cpp index 720db9e..fb5ec16 100644 --- a/dynamixel_hardware/src/dynamixel_hardware.cpp +++ b/dynamixel_hardware/src/dynamixel_hardware.cpp @@ -229,12 +229,16 @@ CallbackReturn DynamixelHardware::on_activate(const rclcpp_lifecycle::State & /* reset_command(); write(rclcpp::Time{}, rclcpp::Duration(0, 0)); + activated_ = true; + return CallbackReturn::SUCCESS; } CallbackReturn DynamixelHardware::on_deactivate( const rclcpp_lifecycle::State & /* previous_state */) { + activated_ = false; + RCLCPP_DEBUG(rclcpp::get_logger(kDynamixelHardware), "stop"); return CallbackReturn::SUCCESS; } @@ -243,6 +247,11 @@ return_type DynamixelHardware::read( const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */) { + if(!activated_) + { + return return_type::OK; + } + if (use_dummy_) { return return_type::OK; } @@ -290,7 +299,6 @@ return_type DynamixelHardware::read( joints_[i].state.velocity = dynamixel_workbench_.convertValue2Velocity(ids[i], velocities[i]); joints_[i].state.effort = dynamixel_workbench_.convertValue2Current(currents[i]); } - return return_type::OK; } @@ -298,6 +306,11 @@ return_type DynamixelHardware::write( const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */) { + if(!activated_) + { + return return_type::OK; + } + if (use_dummy_) { for (auto & joint : joints_) { joint.prev_command.position = joint.command.position; From b87f70ae43f5a20ef1959305f4ddb941dd2a0a21 Mon Sep 17 00:00:00 2001 From: Soham Patil Date: Tue, 25 Mar 2025 09:00:21 +0530 Subject: [PATCH 2/2] set activated to true before reset command --- dynamixel_hardware/src/dynamixel_hardware.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/dynamixel_hardware/src/dynamixel_hardware.cpp b/dynamixel_hardware/src/dynamixel_hardware.cpp index fb5ec16..40a30f0 100644 --- a/dynamixel_hardware/src/dynamixel_hardware.cpp +++ b/dynamixel_hardware/src/dynamixel_hardware.cpp @@ -225,12 +225,13 @@ CallbackReturn DynamixelHardware::on_activate(const rclcpp_lifecycle::State & /* joints_[i].state.effort = 0.0; } } + + activated_ = true; + read(rclcpp::Time{}, rclcpp::Duration(0, 0)); reset_command(); write(rclcpp::Time{}, rclcpp::Duration(0, 0)); - activated_ = true; - return CallbackReturn::SUCCESS; }