diff --git a/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp b/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp index d3d5746..84fc64f 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; @@ -100,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 720db9e..493df6f 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; } @@ -81,6 +82,15 @@ CallbackReturn DynamixelHardware::on_init(const hardware_interface::HardwareInfo 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; @@ -104,7 +114,9 @@ CallbackReturn DynamixelHardware::on_init(const hardware_interface::HardwareInfo 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); @@ -410,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; @@ -434,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;