From da732e6da8fb401cba97a5505759399ed04ecdbf Mon Sep 17 00:00:00 2001 From: ahb Date: Sat, 1 Apr 2023 16:25:47 +0200 Subject: [PATCH 1/3] migrate to ros2_control humble API --- CMakeLists.txt | 2 + include/system_odri.hpp | 29 ++++++------ package.xml | 1 + src/system_odri.cpp | 98 ++++++++++++++++++++--------------------- 4 files changed, 63 insertions(+), 67 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index f5dbdd6..6a70d9a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -51,6 +51,7 @@ check_minimal_cxx_standard(11 ENFORCE) cmake_policy(SET CMP0057 NEW) find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) find_package(hardware_interface REQUIRED) find_package(odri_control_interface REQUIRED) find_package(controller_interface REQUIRED) @@ -99,6 +100,7 @@ ament_target_dependencies( hardware_interface pluginlib rclcpp + rclcpp_lifecycle ) target_include_directories( diff --git a/include/system_odri.hpp b/include/system_odri.hpp index 5f86f9e..610dfae 100644 --- a/include/system_odri.hpp +++ b/include/system_odri.hpp @@ -23,22 +23,20 @@ #include #include -#include "hardware_interface/base_interface.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "hardware_interface/types/hardware_interface_status_values.hpp" #include "master_board_sdk/master_board_interface.h" #include "odri_control_interface/imu.hpp" #include "odri_control_interface/robot.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" #include "semantic_components/imu_sensor.hpp" #include "system_interface_odri.hpp" #include "visibility_control.h" -using hardware_interface::return_type; - #define rt_printf printf /** @@ -73,13 +71,12 @@ typedef Matrix Vector4l; namespace ros2_control_odri { -class SystemOdriHardware : public hardware_interface::BaseInterface< - hardware_interface::SystemInterface> { +class SystemOdriHardware : public hardware_interface::SystemInterface { public: RCLCPP_SHARED_PTR_DEFINITIONS(SystemOdriHardware) ROS2_CONTROL_ODRI_PUBLIC - hardware_interface::return_type configure( + hardware_interface::CallbackReturn on_init( const hardware_interface::HardwareInfo &system_info) override; ROS2_CONTROL_ODRI_PUBLIC @@ -91,27 +88,27 @@ class SystemOdriHardware : public hardware_interface::BaseInterface< override; ROS2_CONTROL_ODRI_PUBLIC - return_type prepare_command_mode_switch( + hardware_interface::return_type prepare_command_mode_switch( const std::vector &start_interfaces, const std::vector &stop_interfaces) override; ROS2_CONTROL_ODRI_PUBLIC - return_type calibration(); + hardware_interface::return_type calibration(); ROS2_CONTROL_ODRI_PUBLIC - return_type start() override; + hardware_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; ROS2_CONTROL_ODRI_PUBLIC - return_type stop() override; + hardware_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; ROS2_CONTROL_ODRI_PUBLIC - return_type read() override; + hardware_interface::return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override; ROS2_CONTROL_ODRI_PUBLIC - return_type write() override; + hardware_interface::return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override; ROS2_CONTROL_ODRI_PUBLIC - return_type display(); + hardware_interface::return_type display(); private: // Parameters for the RRBot simulation @@ -123,10 +120,10 @@ class SystemOdriHardware : public hardware_interface::BaseInterface< void display_robot_state(); // Read desired starting position. - return_type read_desired_starting_position(); + hardware_interface::return_type read_desired_starting_position(); // Read default joint cmd and state values - return_type read_default_cmd_state_value(std::string &default_joint_cs); + hardware_interface::return_type read_default_cmd_state_value(std::string &default_joint_cs); // Read default cmd or state value. // default_joint_cs: "default_joint_cmd" or "default_joint_state" diff --git a/package.xml b/package.xml index a4c96dc..b883562 100644 --- a/package.xml +++ b/package.xml @@ -37,6 +37,7 @@ hardware_interface pluginlib rclcpp + rclcpp_lifecycle odri_control_interface controller_interface diff --git a/src/system_odri.cpp b/src/system_odri.cpp index bfc7e5c..61c8d00 100644 --- a/src/system_odri.cpp +++ b/src/system_odri.cpp @@ -44,7 +44,7 @@ namespace ros2_control_odri { Eigen::Vector6d desired_joint_position = Eigen::Vector6d::Zero(); Eigen::Vector6d desired_torque = Eigen::Vector6d::Zero(); -return_type SystemOdriHardware::read_default_cmd_state_value( +hardware_interface::return_type SystemOdriHardware::read_default_cmd_state_value( std::string &default_joint_cs) { // Hardware parameters provides a string if (info_.hardware_parameters.find(default_joint_cs) == @@ -53,7 +53,7 @@ return_type SystemOdriHardware::read_default_cmd_state_value( rclcpp::get_logger("SystemOdriHardware"), "%s not in the parameter list of ros2_control_odri/SystemOdriHardware!", default_joint_cs.c_str()); - return return_type::ERROR; + return hardware_interface::return_type::ERROR; } std::string str_des_start_pos = info_.hardware_parameters[default_joint_cs]; @@ -64,7 +64,7 @@ return_type SystemOdriHardware::read_default_cmd_state_value( } else if (default_joint_cs == "default_joint_state") { hw_cs = hw_states_; } else - return return_type::ERROR; + return hardware_interface::return_type::ERROR; // Read the parameter through a stream of strings. std::istringstream iss_def_cmd_val; @@ -91,40 +91,40 @@ return_type SystemOdriHardware::read_default_cmd_state_value( RCLCPP_FATAL(rclcpp::get_logger("SystemOdriHardware"), "default_joint_cmd '%s' no '%s'.", joint_name.c_str(), msg.c_str()); - return return_type::ERROR; + return hardware_interface::return_type::ERROR; } - return return_type::OK; + return hardware_interface::return_type::OK; }; std::string amsg("position"); if (handle_dbl_and_msg(iss_def_cmd_val, joint_name, hw_cs.at(joint_name).position, - amsg) == return_type::ERROR) - return return_type::ERROR; + amsg) == hardware_interface::return_type::ERROR) + return hardware_interface::return_type::ERROR; amsg = "velocity"; if (handle_dbl_and_msg(iss_def_cmd_val, joint_name, hw_cs.at(joint_name).velocity, - amsg) == return_type::ERROR) - return return_type::ERROR; + amsg) == hardware_interface::return_type::ERROR) + return hardware_interface::return_type::ERROR; amsg = "effort"; if (handle_dbl_and_msg(iss_def_cmd_val, joint_name, hw_cs.at(joint_name).effort, - amsg) == return_type::ERROR) - return return_type::ERROR; + amsg) == hardware_interface::return_type::ERROR) + return hardware_interface::return_type::ERROR; amsg = "Kp"; if (handle_dbl_and_msg(iss_def_cmd_val, joint_name, hw_cs.at(joint_name).Kp, - amsg) == return_type::ERROR) - return return_type::ERROR; + amsg) == hardware_interface::return_type::ERROR) + return hardware_interface::return_type::ERROR; amsg = "Kd"; if (handle_dbl_and_msg(iss_def_cmd_val, joint_name, hw_cs.at(joint_name).Kd, - amsg) == return_type::ERROR) - return return_type::ERROR; + amsg) == hardware_interface::return_type::ERROR) + return hardware_interface::return_type::ERROR; found_joint = true; break; // Found the joint break the loop @@ -135,7 +135,7 @@ return_type SystemOdriHardware::read_default_cmd_state_value( RCLCPP_FATAL(rclcpp::get_logger("SystemOdriHardware"), "Joint '%s' not found in '%s'.", joint_name.c_str(), default_joint_cs.c_str()); - return return_type::ERROR; + return hardware_interface::return_type::ERROR; } } if (default_joint_cs == "default_joint_cmd") @@ -143,11 +143,11 @@ return_type SystemOdriHardware::read_default_cmd_state_value( else if (default_joint_cs == "default_joint_state") hw_states_ = hw_cs; - return return_type::OK; + return hardware_interface::return_type::OK; } /// Reading desired position -return_type SystemOdriHardware::read_desired_starting_position() { +hardware_interface::return_type SystemOdriHardware::read_desired_starting_position() { std::vector vec_des_start_pos; if (info_.hardware_parameters.find("desired_starting_position") == @@ -155,7 +155,7 @@ return_type SystemOdriHardware::read_desired_starting_position() { RCLCPP_FATAL(rclcpp::get_logger("SystemOdriHardware"), "desired_starting_positon not in the parameter list of " "ros2_control_odri/SystemOdriHardware!"); - return return_type::ERROR; + return hardware_interface::return_type::ERROR; } // Hardware parameters provides a string @@ -186,13 +186,13 @@ return_type SystemOdriHardware::read_desired_starting_position() { int idx_dsp = 0; for (auto apos : vec_des_start_pos) eig_des_start_pos_[idx_dsp++] = apos; - return return_type::OK; + return hardware_interface::return_type::OK; } -return_type SystemOdriHardware::configure( +hardware_interface::CallbackReturn SystemOdriHardware::on_init( const hardware_interface::HardwareInfo &info) { - if (configure_default(info) != return_type::OK) { - return return_type::ERROR; + if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) { + return hardware_interface::CallbackReturn::ERROR; } // For each sensor. @@ -236,9 +236,9 @@ return_type SystemOdriHardware::configure( if (joint.command_interfaces.size() != odri_list_of_cmd_inter.size()) { RCLCPP_FATAL( rclcpp::get_logger("SystemOdriHardware"), - "Joint '%s' has %d command interfaces found.", // 5 expected.", + "Joint '%s' has %lu command interfaces found.", // 5 expected.", joint.name.c_str(), joint.command_interfaces.size()); - return return_type::ERROR; + return hardware_interface::CallbackReturn::ERROR; } // For each command interface of the joint @@ -255,16 +255,16 @@ return_type SystemOdriHardware::configure( RCLCPP_FATAL(rclcpp::get_logger("SystemOdriHardware"), "'%s' expected.", a_cmd_inter.c_str()); } - return return_type::ERROR; + return hardware_interface::CallbackReturn::ERROR; } } // Check if the state interface list is of the right size if (joint.state_interfaces.size() != odri_list_of_state_inter.size()) { RCLCPP_FATAL(rclcpp::get_logger("SystemOdriHardware"), - "Joint '%s' has %d state interface.", // 5 expected.", + "Joint '%s' has %lu state interface.", // 5 expected.", joint.name.c_str(), joint.state_interfaces.size()); - return return_type::ERROR; + return hardware_interface::CallbackReturn::ERROR; } // For each state interface of the joint @@ -283,14 +283,12 @@ return_type SystemOdriHardware::configure( RCLCPP_FATAL(rclcpp::get_logger("SystemOdriHardware"), "'%s' expected.", a_state_inter.c_str()); } - return return_type::ERROR; + return hardware_interface::CallbackReturn::ERROR; } } } - status_ = hardware_interface::status::CONFIGURED; - - return return_type::OK; + return hardware_interface::CallbackReturn::SUCCESS; } void SystemOdriHardware::display_robot_state() { @@ -305,7 +303,7 @@ void SystemOdriHardware::display_robot_state() { std::cout << " **************************" << std::endl; } -return_type SystemOdriHardware::prepare_command_mode_switch( +hardware_interface::return_type SystemOdriHardware::prepare_command_mode_switch( const std::vector &start_interfaces, const std::vector &stop_interfaces) { // Initialize new modes. @@ -357,14 +355,14 @@ return_type SystemOdriHardware::prepare_command_mode_switch( "Joint '%s' has no valid control mode %d %d", joint.name.c_str(), control_mode_[joint.name], new_modes_[joint.name]); - return return_type::ERROR; + return hardware_interface::return_type::ERROR; } control_mode_[joint.name] = new_modes_[joint.name]; } std::cout << "in prepare_command_mode_switch" << std::endl; display_robot_state(); - return return_type::OK; + return hardware_interface::return_type::OK; } std::vector @@ -454,7 +452,7 @@ SystemOdriHardware::export_command_interfaces() { return command_interfaces; } -return_type SystemOdriHardware::start() { +hardware_interface::CallbackReturn SystemOdriHardware::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { //// Read Parameters //// /// Read odri_config_yaml @@ -462,18 +460,18 @@ return_type SystemOdriHardware::start() { robot_ = RobotFromYamlFile(info_.hardware_parameters["odri_config_yaml"]); /// Read hardware parameter "desired_starting_position" - if (read_desired_starting_position() == return_type::ERROR) - return return_type::ERROR; + if (read_desired_starting_position() == hardware_interface::return_type::ERROR) + return hardware_interface::CallbackReturn::ERROR; /// Read hardware parameter "default_joint_cmd" std::string default_joint_cs("default_joint_cmd"); - if (read_default_cmd_state_value(default_joint_cs) == return_type::ERROR) - return return_type::ERROR; + if (read_default_cmd_state_value(default_joint_cs) == hardware_interface::return_type::ERROR) + return hardware_interface::CallbackReturn::ERROR; /// Read hardware parameter "default_joint_state" default_joint_cs = "default_joint_state"; - if (read_default_cmd_state_value(default_joint_cs) == return_type::ERROR) - return return_type::ERROR; + if (read_default_cmd_state_value(default_joint_cs) == hardware_interface::return_type::ERROR) + return hardware_interface::CallbackReturn::ERROR; /// Initialize the robot to the desired starting position. robot_->Initialize(eig_des_start_pos_); @@ -491,19 +489,17 @@ return_type SystemOdriHardware::start() { joint_name_to_array_index_[it->first] = idx++; } - status_ = hardware_interface::status::STARTED; - - return return_type::OK; + return hardware_interface::CallbackReturn::SUCCESS; } -return_type SystemOdriHardware::stop() { +hardware_interface::CallbackReturn SystemOdriHardware::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) { // Stop the MasterBoard main_board_ptr_->MasterBoardInterface::Stop(); - return return_type::OK; + return hardware_interface::CallbackReturn::SUCCESS; } -hardware_interface::return_type SystemOdriHardware::read() { +hardware_interface::return_type SystemOdriHardware::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { // Data acquisition (with ODRI) robot_->ParseSensorData(); @@ -552,10 +548,10 @@ hardware_interface::return_type SystemOdriHardware::read() { imu_states_["IMU"].quater_z = imu_quater[2]; imu_states_["IMU"].quater_w = imu_quater[3]; - return return_type::OK; + return hardware_interface::return_type::OK; } -hardware_interface::return_type SystemOdriHardware::write() { +hardware_interface::return_type SystemOdriHardware::write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { Eigen::Vector6d positions; Eigen::Vector6d velocities; Eigen::Vector6d torques; @@ -598,7 +594,7 @@ hardware_interface::return_type SystemOdriHardware::write() { robot_->SendCommandAndWaitEndOfCycle(0.00); - return return_type::OK; + return hardware_interface::return_type::OK; } } // namespace ros2_control_odri From be887c44aae6cb5c5faf9737b4f478e8fbf95bff Mon Sep 17 00:00:00 2001 From: ahb Date: Fri, 14 Apr 2023 14:52:52 +0200 Subject: [PATCH 2/3] clang-format --- include/system_odri.hpp | 15 ++++++++++----- src/system_odri.cpp | 30 ++++++++++++++++++++---------- 2 files changed, 30 insertions(+), 15 deletions(-) diff --git a/include/system_odri.hpp b/include/system_odri.hpp index 610dfae..5ba57f4 100644 --- a/include/system_odri.hpp +++ b/include/system_odri.hpp @@ -96,16 +96,20 @@ class SystemOdriHardware : public hardware_interface::SystemInterface { hardware_interface::return_type calibration(); ROS2_CONTROL_ODRI_PUBLIC - hardware_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + hardware_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State &previous_state) override; ROS2_CONTROL_ODRI_PUBLIC - hardware_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + hardware_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State &previous_state) override; ROS2_CONTROL_ODRI_PUBLIC - hardware_interface::return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override; + hardware_interface::return_type read(const rclcpp::Time &time, + const rclcpp::Duration &period) override; ROS2_CONTROL_ODRI_PUBLIC - hardware_interface::return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override; + hardware_interface::return_type write( + const rclcpp::Time &time, const rclcpp::Duration &period) override; ROS2_CONTROL_ODRI_PUBLIC hardware_interface::return_type display(); @@ -123,7 +127,8 @@ class SystemOdriHardware : public hardware_interface::SystemInterface { hardware_interface::return_type read_desired_starting_position(); // Read default joint cmd and state values - hardware_interface::return_type read_default_cmd_state_value(std::string &default_joint_cs); + hardware_interface::return_type read_default_cmd_state_value( + std::string &default_joint_cs); // Read default cmd or state value. // default_joint_cs: "default_joint_cmd" or "default_joint_state" diff --git a/src/system_odri.cpp b/src/system_odri.cpp index 61c8d00..b55a678 100644 --- a/src/system_odri.cpp +++ b/src/system_odri.cpp @@ -44,7 +44,8 @@ namespace ros2_control_odri { Eigen::Vector6d desired_joint_position = Eigen::Vector6d::Zero(); Eigen::Vector6d desired_torque = Eigen::Vector6d::Zero(); -hardware_interface::return_type SystemOdriHardware::read_default_cmd_state_value( +hardware_interface::return_type +SystemOdriHardware::read_default_cmd_state_value( std::string &default_joint_cs) { // Hardware parameters provides a string if (info_.hardware_parameters.find(default_joint_cs) == @@ -147,7 +148,8 @@ hardware_interface::return_type SystemOdriHardware::read_default_cmd_state_value } /// Reading desired position -hardware_interface::return_type SystemOdriHardware::read_desired_starting_position() { +hardware_interface::return_type +SystemOdriHardware::read_desired_starting_position() { std::vector vec_des_start_pos; if (info_.hardware_parameters.find("desired_starting_position") == @@ -191,7 +193,8 @@ hardware_interface::return_type SystemOdriHardware::read_desired_starting_positi hardware_interface::CallbackReturn SystemOdriHardware::on_init( const hardware_interface::HardwareInfo &info) { - if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) { + if (hardware_interface::SystemInterface::on_init(info) != + CallbackReturn::SUCCESS) { return hardware_interface::CallbackReturn::ERROR; } @@ -452,7 +455,8 @@ SystemOdriHardware::export_command_interfaces() { return command_interfaces; } -hardware_interface::CallbackReturn SystemOdriHardware::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { +hardware_interface::CallbackReturn SystemOdriHardware::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) { //// Read Parameters //// /// Read odri_config_yaml @@ -460,17 +464,20 @@ hardware_interface::CallbackReturn SystemOdriHardware::on_activate(const rclcpp_ robot_ = RobotFromYamlFile(info_.hardware_parameters["odri_config_yaml"]); /// Read hardware parameter "desired_starting_position" - if (read_desired_starting_position() == hardware_interface::return_type::ERROR) + if (read_desired_starting_position() == + hardware_interface::return_type::ERROR) return hardware_interface::CallbackReturn::ERROR; /// Read hardware parameter "default_joint_cmd" std::string default_joint_cs("default_joint_cmd"); - if (read_default_cmd_state_value(default_joint_cs) == hardware_interface::return_type::ERROR) + if (read_default_cmd_state_value(default_joint_cs) == + hardware_interface::return_type::ERROR) return hardware_interface::CallbackReturn::ERROR; /// Read hardware parameter "default_joint_state" default_joint_cs = "default_joint_state"; - if (read_default_cmd_state_value(default_joint_cs) == hardware_interface::return_type::ERROR) + if (read_default_cmd_state_value(default_joint_cs) == + hardware_interface::return_type::ERROR) return hardware_interface::CallbackReturn::ERROR; /// Initialize the robot to the desired starting position. @@ -492,14 +499,16 @@ hardware_interface::CallbackReturn SystemOdriHardware::on_activate(const rclcpp_ return hardware_interface::CallbackReturn::SUCCESS; } -hardware_interface::CallbackReturn SystemOdriHardware::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) { +hardware_interface::CallbackReturn SystemOdriHardware::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) { // Stop the MasterBoard main_board_ptr_->MasterBoardInterface::Stop(); return hardware_interface::CallbackReturn::SUCCESS; } -hardware_interface::return_type SystemOdriHardware::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { +hardware_interface::return_type SystemOdriHardware::read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { // Data acquisition (with ODRI) robot_->ParseSensorData(); @@ -551,7 +560,8 @@ hardware_interface::return_type SystemOdriHardware::read(const rclcpp::Time & /* return hardware_interface::return_type::OK; } -hardware_interface::return_type SystemOdriHardware::write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { +hardware_interface::return_type SystemOdriHardware::write( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { Eigen::Vector6d positions; Eigen::Vector6d velocities; Eigen::Vector6d torques; From 17f1299c5308dbe475a699f5a03d189f4e94edb3 Mon Sep 17 00:00:00 2001 From: ahb Date: Sat, 15 Apr 2023 20:37:11 +0200 Subject: [PATCH 3/3] fix joint values being mapped incorrectly due to overwriting a map that StateInterface holds pointers into --- src/system_odri.cpp | 42 ++++++++++++++++++++++++++---------------- 1 file changed, 26 insertions(+), 16 deletions(-) diff --git a/src/system_odri.cpp b/src/system_odri.cpp index b55a678..c6fb620 100644 --- a/src/system_odri.cpp +++ b/src/system_odri.cpp @@ -59,11 +59,11 @@ SystemOdriHardware::read_default_cmd_state_value( std::string str_des_start_pos = info_.hardware_parameters[default_joint_cs]; typedef std::map map_pveg; - map_pveg hw_cs; + map_pveg* hw_cs; if (default_joint_cs == "default_joint_cmd") { - hw_cs = hw_commands_; + hw_cs = &hw_commands_; } else if (default_joint_cs == "default_joint_state") { - hw_cs = hw_states_; + hw_cs = &hw_states_; } else return hardware_interface::return_type::ERROR; @@ -76,8 +76,11 @@ SystemOdriHardware::read_default_cmd_state_value( std::string joint_name; RCLCPP_INFO_STREAM( rclcpp::get_logger("SystemOdriHardware"), - " Current value of iss_def_cmd_val:" << iss_def_cmd_val.str().c_str()); + " Current value of iss_def_cmd_val for " << default_joint_cs << ":" << iss_def_cmd_val.str().c_str()); iss_def_cmd_val >> joint_name; + if (joint_name == "" && iss_def_cmd_val.eof()) { + break; + } // Find the associate joint bool found_joint = false; @@ -99,31 +102,31 @@ SystemOdriHardware::read_default_cmd_state_value( std::string amsg("position"); if (handle_dbl_and_msg(iss_def_cmd_val, joint_name, - hw_cs.at(joint_name).position, + hw_cs->at(joint_name).position, amsg) == hardware_interface::return_type::ERROR) return hardware_interface::return_type::ERROR; amsg = "velocity"; if (handle_dbl_and_msg(iss_def_cmd_val, joint_name, - hw_cs.at(joint_name).velocity, + hw_cs->at(joint_name).velocity, amsg) == hardware_interface::return_type::ERROR) return hardware_interface::return_type::ERROR; amsg = "effort"; if (handle_dbl_and_msg(iss_def_cmd_val, joint_name, - hw_cs.at(joint_name).effort, + hw_cs->at(joint_name).effort, amsg) == hardware_interface::return_type::ERROR) return hardware_interface::return_type::ERROR; amsg = "Kp"; if (handle_dbl_and_msg(iss_def_cmd_val, joint_name, - hw_cs.at(joint_name).Kp, + hw_cs->at(joint_name).Kp, amsg) == hardware_interface::return_type::ERROR) return hardware_interface::return_type::ERROR; amsg = "Kd"; if (handle_dbl_and_msg(iss_def_cmd_val, joint_name, - hw_cs.at(joint_name).Kd, + hw_cs->at(joint_name).Kd, amsg) == hardware_interface::return_type::ERROR) return hardware_interface::return_type::ERROR; @@ -139,11 +142,6 @@ SystemOdriHardware::read_default_cmd_state_value( return hardware_interface::return_type::ERROR; } } - if (default_joint_cs == "default_joint_cmd") - hw_commands_ = hw_cs; - else if (default_joint_cs == "default_joint_state") - hw_states_ = hw_cs; - return hardware_interface::return_type::OK; } @@ -361,6 +359,9 @@ hardware_interface::return_type SystemOdriHardware::prepare_command_mode_switch( return hardware_interface::return_type::ERROR; } control_mode_[joint.name] = new_modes_[joint.name]; + RCLCPP_INFO_STREAM( + rclcpp::get_logger("SystemOdriHardware"), + "control_mode[" << joint.name << "]: " << control_mode_[joint.name]); } std::cout << "in prepare_command_mode_switch" << std::endl; @@ -488,14 +489,20 @@ hardware_interface::CallbackReturn SystemOdriHardware::on_activate( // First set the key joint_name_to_array_index_[joint.name] = 0; } - - // Then build the index. + + /// Then build the index. + // Warning: depends on order of joint tags within ros2_control tag + // and on order in robot.joint_modules.motor_numbers in config_solo12.yaml uint idx = 0; for (auto it = joint_name_to_array_index_.begin(); it != joint_name_to_array_index_.end(); ++it) { + RCLCPP_INFO_STREAM( + rclcpp::get_logger("SystemOdriHardware"), + "joint_name=" << it->first << " -> index=" << idx); joint_name_to_array_index_[it->first] = idx++; } + return hardware_interface::CallbackReturn::SUCCESS; } @@ -582,6 +589,9 @@ hardware_interface::return_type SystemOdriHardware::write( hw_commands_[joint.name].Kp; gain_KD[joint_name_to_array_index_[joint.name]] = hw_commands_[joint.name].Kd; + } else if (control_mode_[joint.name] == control_mode_t::EFFORT) { + torques[joint_name_to_array_index_[joint.name]] = + hw_commands_[joint.name].effort; } }