From 9e37e9dc2c8689f7fc063a38c0e116e66c5e1405 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 7 Aug 2024 14:35:18 +0200 Subject: [PATCH 1/2] start adaptation of demos --- example_10/controllers/gpio_controller.cpp | 6 +- .../ros2_control/diffbot.ros2_control.xacro | 24 +- example_2/hardware/diffbot_system.cpp | 72 ++-- .../diffbot_system.hpp | 6 +- ...ot_system_position_only.ros2_control.xacro | 10 +- .../external_rrbot_force_torque_sensor.cpp | 31 +- .../external_rrbot_force_torque_sensor.hpp | 3 - ...rrbot_modular_actuators.ros2_control.xacro | 10 +- .../rrbot_actuator.hpp | 4 - example_6/hardware/rrbot_actuator.cpp | 77 ++-- .../ros2_control/r6bot.ros2_control.xacro | 66 +++- .../r6bot_hardware.hpp | 27 +- example_7/hardware/r6bot_hardware.cpp | 98 ++--- ...ns_system_position_only.ros2_control.xacro | 10 +- ...bot_transmissions_system_position_only.hpp | 29 +- ...bot_transmissions_system_position_only.cpp | 347 +++++++++--------- 16 files changed, 406 insertions(+), 414 deletions(-) diff --git a/example_10/controllers/gpio_controller.cpp b/example_10/controllers/gpio_controller.cpp index 9bd08ec3a..bcb4a9f2f 100644 --- a/example_10/controllers/gpio_controller.cpp +++ b/example_10/controllers/gpio_controller.cpp @@ -59,8 +59,8 @@ controller_interface::return_type GPIOController::update( { RCLCPP_DEBUG( get_node()->get_logger(), "%s: (%f)", state_interfaces_[i].get_name().c_str(), - state_interfaces_[i].get_value()); - gpio_msg_.values.at(i) = static_cast(state_interfaces_.at(i).get_value()); + state_interfaces_[i].get_value()); + gpio_msg_.values.at(i) = static_cast(state_interfaces_.at(i).get_value()); } gpio_publisher_->publish(gpio_msg_); @@ -84,7 +84,7 @@ controller_interface::return_type GPIOController::update( command_interfaces_[i].set_value(output_cmd_ptr_->data[i]); RCLCPP_DEBUG( get_node()->get_logger(), "%s: (%f)", command_interfaces_[i].get_name().c_str(), - command_interfaces_[i].get_value()); + command_interfaces_[i].get_value()); } return controller_interface::return_type::OK; diff --git a/example_2/description/ros2_control/diffbot.ros2_control.xacro b/example_2/description/ros2_control/diffbot.ros2_control.xacro index f30dcab65..c43c603b2 100644 --- a/example_2/description/ros2_control/diffbot.ros2_control.xacro +++ b/example_2/description/ros2_control/diffbot.ros2_control.xacro @@ -18,14 +18,26 @@ - - - + + double + + + double + + + double + - - - + + double + + + double + + + double + diff --git a/example_2/hardware/diffbot_system.cpp b/example_2/hardware/diffbot_system.cpp index 177b3a4f5..6e229988a 100644 --- a/example_2/hardware/diffbot_system.cpp +++ b/example_2/hardware/diffbot_system.cpp @@ -43,9 +43,6 @@ hardware_interface::CallbackReturn DiffBotSystemHardware::on_init( hw_stop_sec_ = hardware_interface::stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]); // END: This part here is for exemplary purposes - Please do not copy to your production code - hw_positions_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_velocities_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); for (const hardware_interface::ComponentInfo & joint : info_.joints) { @@ -99,32 +96,6 @@ hardware_interface::CallbackReturn DiffBotSystemHardware::on_init( return hardware_interface::CallbackReturn::SUCCESS; } -std::vector DiffBotSystemHardware::export_state_interfaces() -{ - std::vector state_interfaces; - for (auto i = 0u; i < info_.joints.size(); i++) - { - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_positions_[i])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_velocities_[i])); - } - - return state_interfaces; -} - -std::vector DiffBotSystemHardware::export_command_interfaces() -{ - std::vector command_interfaces; - for (auto i = 0u; i < info_.joints.size(); i++) - { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_commands_[i])); - } - - return command_interfaces; -} - hardware_interface::CallbackReturn DiffBotSystemHardware::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { @@ -139,14 +110,20 @@ hardware_interface::CallbackReturn DiffBotSystemHardware::on_activate( } // END: This part here is for exemplary purposes - Please do not copy to your production code - // set some default values - for (auto i = 0u; i < hw_positions_.size(); i++) + // set some default values for joints + for (const auto & [cmd_itf_name, cmd_itf_descr] : joint_command_interfaces_) + { + if (!command_holds_value(cmd_itf_name) || std::isnan(get_command(cmd_itf_name))) + { + set_command(cmd_itf_name, 0.0); + } + } + + for (const auto & [state_itf_name, state_itf_descr] : joint_state_interfaces_) { - if (std::isnan(hw_positions_[i])) + if (!state_holds_value(state_itf_name) || std::isnan(get_state(state_itf_name))) { - hw_positions_[i] = 0; - hw_velocities_[i] = 0; - hw_commands_[i] = 0; + set_state(state_itf_name, 0.0); } } @@ -178,17 +155,18 @@ hardware_interface::return_type DiffBotSystemHardware::read( const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - for (std::size_t i = 0; i < hw_velocities_.size(); i++) + for (const auto & component : info_.joints) { - // Simulate DiffBot wheels's movement as a first-order system - // Update the joint status: this is a revolute joint without any limit. - // Simply integrates - hw_positions_[i] = hw_positions_[i] + period.seconds() * hw_velocities_[i]; + const std::string & joint = component.name; + const std::string joint_pos = joint + pos; + const std::string joint_vel = joint + vel; + double new_pos = get_state(joint_pos) + period.seconds() * get_state(joint_vel); + set_state(joint_pos, new_pos); RCLCPP_INFO( rclcpp::get_logger("DiffBotSystemHardware"), - "Got position state %.5f and velocity state %.5f for '%s'!", hw_positions_[i], - hw_velocities_[i], info_.joints[i].name.c_str()); + "Got position state %.5f and velocity state %.5f for '%s'!", get_state(joint_pos), + get_state(joint_vel), joint.c_str()); } // END: This part here is for exemplary purposes - Please do not copy to your production code @@ -201,14 +179,16 @@ hardware_interface::return_type ros2_control_demo_example_2 ::DiffBotSystemHardw // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "Writing..."); - for (auto i = 0u; i < hw_commands_.size(); i++) + for (const auto & component : info_.joints) { + const std::string & joint = component.name; + const std::string joint_vel = joint + vel; // Simulate sending commands to the hardware RCLCPP_INFO( - rclcpp::get_logger("DiffBotSystemHardware"), "Got command %.5f for '%s'!", hw_commands_[i], - info_.joints[i].name.c_str()); + rclcpp::get_logger("DiffBotSystemHardware"), "Got command %.5f for '%s'!", + get_command(joint + vel), joint.c_str()); - hw_velocities_[i] = hw_commands_[i]; + set_state(joint_vel, get_command(joint_vel)); } RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "Joints successfully written!"); // END: This part here is for exemplary purposes - Please do not copy to your production code diff --git a/example_2/hardware/include/ros2_control_demo_example_2/diffbot_system.hpp b/example_2/hardware/include/ros2_control_demo_example_2/diffbot_system.hpp index 13490e5e0..e4a5ad32b 100644 --- a/example_2/hardware/include/ros2_control_demo_example_2/diffbot_system.hpp +++ b/example_2/hardware/include/ros2_control_demo_example_2/diffbot_system.hpp @@ -70,10 +70,8 @@ class DiffBotSystemHardware : public hardware_interface::SystemInterface double hw_start_sec_; double hw_stop_sec_; - // Store the command for the simulated robot - std::vector hw_commands_; - std::vector hw_positions_; - std::vector hw_velocities_; + const std::string vel = "/" + std::string(hardware_interface::HW_IF_VELOCITY); + const std::string pos = "/" + std::string(hardware_interface::HW_IF_POSITION); }; } // namespace ros2_control_demo_example_2 diff --git a/example_5/description/ros2_control/rrbot_system_position_only.ros2_control.xacro b/example_5/description/ros2_control/rrbot_system_position_only.ros2_control.xacro index 4ca6c5d2f..685ca27e9 100644 --- a/example_5/description/ros2_control/rrbot_system_position_only.ros2_control.xacro +++ b/example_5/description/ros2_control/rrbot_system_position_only.ros2_control.xacro @@ -21,17 +21,23 @@ + double -1 1 - + + double + + double -1 1 - + + double + diff --git a/example_5/hardware/external_rrbot_force_torque_sensor.cpp b/example_5/hardware/external_rrbot_force_torque_sensor.cpp index 3cd56adfc..559c86868 100644 --- a/example_5/hardware/external_rrbot_force_torque_sensor.cpp +++ b/example_5/hardware/external_rrbot_force_torque_sensor.cpp @@ -45,27 +45,9 @@ hardware_interface::CallbackReturn ExternalRRBotForceTorqueSensorHardware::on_in hw_sensor_change_ = stod(info_.hardware_parameters["example_param_max_sensor_change"]); // END: This part here is for exemplary purposes - Please do not copy to your production code - hw_sensor_states_.resize( - info_.sensors[0].state_interfaces.size(), std::numeric_limits::quiet_NaN()); - return hardware_interface::CallbackReturn::SUCCESS; } -std::vector -ExternalRRBotForceTorqueSensorHardware::export_state_interfaces() -{ - std::vector state_interfaces; - - // export sensor state interface - for (uint i = 0; i < info_.sensors[0].state_interfaces.size(); i++) - { - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.sensors[0].name, info_.sensors[0].state_interfaces[i].name, &hw_sensor_states_[i])); - } - - return state_interfaces; -} - hardware_interface::CallbackReturn ExternalRRBotForceTorqueSensorHardware::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { @@ -116,15 +98,18 @@ hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::read( // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code RCLCPP_INFO(rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "Reading..."); - for (uint i = 0; i < hw_sensor_states_.size(); i++) + size_t i = 0; + for (const auto & [itf_name, itf_descr] : sensor_state_interfaces_) { // Simulate RRBot's sensor data unsigned int seed = time(NULL) + i; - hw_sensor_states_[i] = - static_cast(rand_r(&seed)) / (static_cast(RAND_MAX / hw_sensor_change_)); + set_state( + itf_name, + static_cast(rand_r(&seed)) / (static_cast(RAND_MAX / hw_sensor_change_))); RCLCPP_INFO( - rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "Got state %e for sensor %u!", - hw_sensor_states_[i], i); + rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "Got state %e for sensor %s!", + get_state(itf_name), itf_name.c_str()); + ++i; } RCLCPP_INFO( rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "Joints successfully read!"); diff --git a/example_5/hardware/include/ros2_control_demo_example_5/external_rrbot_force_torque_sensor.hpp b/example_5/hardware/include/ros2_control_demo_example_5/external_rrbot_force_torque_sensor.hpp index 97dd8a4fb..34b678454 100644 --- a/example_5/hardware/include/ros2_control_demo_example_5/external_rrbot_force_torque_sensor.hpp +++ b/example_5/hardware/include/ros2_control_demo_example_5/external_rrbot_force_torque_sensor.hpp @@ -61,9 +61,6 @@ class ExternalRRBotForceTorqueSensorHardware : public hardware_interface::Sensor double hw_start_sec_; double hw_stop_sec_; double hw_sensor_change_; - - // Store the sensor states for the simulated robot - std::vector hw_sensor_states_; }; } // namespace ros2_control_demo_example_5 diff --git a/example_6/description/ros2_control/rrbot_modular_actuators.ros2_control.xacro b/example_6/description/ros2_control/rrbot_modular_actuators.ros2_control.xacro index 41918baae..9b5c40aed 100644 --- a/example_6/description/ros2_control/rrbot_modular_actuators.ros2_control.xacro +++ b/example_6/description/ros2_control/rrbot_modular_actuators.ros2_control.xacro @@ -12,10 +12,13 @@ + double -100 100 - + + double + @@ -27,10 +30,13 @@ + double -100 100 - + + double + diff --git a/example_6/hardware/include/ros2_control_demo_example_6/rrbot_actuator.hpp b/example_6/hardware/include/ros2_control_demo_example_6/rrbot_actuator.hpp index 0ceaabaa1..ae15d686b 100644 --- a/example_6/hardware/include/ros2_control_demo_example_6/rrbot_actuator.hpp +++ b/example_6/hardware/include/ros2_control_demo_example_6/rrbot_actuator.hpp @@ -69,10 +69,6 @@ class RRBotModularJoint : public hardware_interface::ActuatorInterface double hw_start_sec_; double hw_stop_sec_; double hw_slowdown_; - - // Store the command for the simulated robot - double hw_joint_command_; - double hw_joint_state_; }; } // namespace ros2_control_demo_example_6 diff --git a/example_6/hardware/rrbot_actuator.cpp b/example_6/hardware/rrbot_actuator.cpp index b48e6c0d7..2fe1fdf7c 100644 --- a/example_6/hardware/rrbot_actuator.cpp +++ b/example_6/hardware/rrbot_actuator.cpp @@ -45,9 +45,6 @@ hardware_interface::CallbackReturn RRBotModularJoint::on_init( hw_slowdown_ = stod(info_.hardware_parameters["example_param_hw_slowdown"]); // END: This part here is for exemplary purposes - Please do not copy to your production code - hw_joint_state_ = std::numeric_limits::quiet_NaN(); - hw_joint_command_ = std::numeric_limits::quiet_NaN(); - const hardware_interface::ComponentInfo & joint = info_.joints[0]; // RRBotModularJoint has exactly one state and command interface on each joint if (joint.command_interfaces.size() != 1) @@ -85,27 +82,21 @@ hardware_interface::CallbackReturn RRBotModularJoint::on_init( return hardware_interface::CallbackReturn::ERROR; } - return hardware_interface::CallbackReturn::SUCCESS; -} - -std::vector RRBotModularJoint::export_state_interfaces() -{ - std::vector state_interfaces; - - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[0].name, hardware_interface::HW_IF_POSITION, &hw_joint_state_)); - - return state_interfaces; -} - -std::vector RRBotModularJoint::export_command_interfaces() -{ - std::vector command_interfaces; - - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[0].name, hardware_interface::HW_IF_POSITION, &hw_joint_command_)); + // assert that joint states and commands are the same so wie can later just iterate over one + for (const auto & [state_itf_name, state_itf_descr] : joint_state_interfaces_) + { + if (joint_command_interfaces_.find(state_itf_name) == joint_command_interfaces_.end()) + { + RCLCPP_FATAL( + rclcpp::get_logger("RRBotModularJoint"), + "Joint '%s' has state interface with name<%s> but not found in CommandInterfaces. Make " + "sure the configuration exports same State- and CommandInterfaces", + joint.name.c_str(), joint.state_itf_name.c_str()); + return hardware_interface::CallbackReturn::ERROR; + } + } - return command_interfaces; + return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn RRBotModularJoint::on_activate( @@ -122,12 +113,21 @@ hardware_interface::CallbackReturn RRBotModularJoint::on_activate( // END: This part here is for exemplary purposes - Please do not copy to your production code // set some default values for joints - if (std::isnan(hw_joint_state_)) + for (const auto & [cmd_itf_name, cmd_itf_descr] : joint_command_interfaces_) { - hw_joint_state_ = 0; - hw_joint_command_ = 0; + if (!valid_command(cmd_itf_name) || std::isnan(get_command(cmd_itf_name))) + { + set_command(cmd_itf_name, 0.0); + } } + for (const auto & [state_itf_name, state_itf_descr] : joint_state_interfaces_) + { + if (!valid_state(state_itf_name) || std::isnan(get_state(state_itf_name))) + { + set_state(state_itf_name, 0.0); + } + } RCLCPP_INFO(rclcpp::get_logger("RRBotModularJoint"), "Successfully activated!"); return hardware_interface::CallbackReturn::SUCCESS; @@ -158,10 +158,18 @@ hardware_interface::return_type RRBotModularJoint::read( RCLCPP_INFO(rclcpp::get_logger("RRBotModularJoint"), "Reading..."); // Simulate RRBot's movement - hw_joint_state_ = hw_joint_state_ + (hw_joint_command_ - hw_joint_state_) / hw_slowdown_; - RCLCPP_INFO( - rclcpp::get_logger("RRBotModularJoint"), "Got state %.5f for joint '%s'!", hw_joint_state_, - info_.joints[0].name.c_str()); + /* + * @pre joint_state_interfaces_ names and are subset of joint_command_interfaces_ + */ + for (const auto & [itf_name, itf_descr] : joint_state_interfaces_) + { + auto old_state = get_state(itf_name); + auto old_cmd = get_command(itf_name); + set_state(itf_name, old_state + (old_cmd - old_state) / hw_slowdown_); + RCLCPP_INFO( + rclcpp::get_logger("RRBotModularJoint"), "Got state %.5f for joint '%s'!", + get_state(itf_name), itf_name.c_str()); + } RCLCPP_INFO(rclcpp::get_logger("RRBotModularJoint"), "Joints successfully read!"); // END: This part here is for exemplary purposes - Please do not copy to your production code @@ -176,9 +184,12 @@ hardware_interface::return_type ros2_control_demo_example_6::RRBotModularJoint:: RCLCPP_INFO(rclcpp::get_logger("RRBotModularJoint"), "Writing...please wait..."); // Simulate sending commands to the hardware - RCLCPP_INFO( - rclcpp::get_logger("RRBotModularJoint"), "Got command %.5f for joint '%s'!", hw_joint_command_, - info_.joints[0].name.c_str()); + for (const auto & [itf_name, itf_descr] : joint_command_interfaces_) + { + RCLCPP_INFO( + rclcpp::get_logger("RRBotModularJoint"), "Got command %.5f for joint '%s'!", + get_command(itf_name), itf_name.c_str()); + } RCLCPP_INFO(rclcpp::get_logger("RRBotModularJoint"), "Joints successfully written!"); // END: This part here is for exemplary purposes - Please do not copy to your production code diff --git a/example_7/description/ros2_control/r6bot.ros2_control.xacro b/example_7/description/ros2_control/r6bot.ros2_control.xacro index 4c8d8eee9..0a8cf5a77 100644 --- a/example_7/description/ros2_control/r6bot.ros2_control.xacro +++ b/example_7/description/ros2_control/r6bot.ros2_control.xacro @@ -11,101 +11,143 @@ + double {-2*pi} {2*pi} + double -3.15 3.15 + double 0.0 - + + double + + double {-2*pi} {2*pi} + double -3.15 3.15 + double 0.0 - + + double + + double {-pi} {pi} + double -3.15 3.15 + double 0.0 - + + double + + double {-2*pi} {2*pi} + double -3.2 3.2 + double 0.0 - + + double + + double {-2*pi} {2*pi} + double -3.2 3.2 + double 0.0 - + + double + + double {-2*pi} {2*pi} + double -3.2 3.2 + double 0.0 - + + double + - - - - - - + + double + + + double + + + double + + + double + + + double + + + double + diff --git a/example_7/hardware/include/ros2_control_demo_example_7/r6bot_hardware.hpp b/example_7/hardware/include/ros2_control_demo_example_7/r6bot_hardware.hpp index 17688a80b..1c8aa7bdc 100644 --- a/example_7/hardware/include/ros2_control_demo_example_7/r6bot_hardware.hpp +++ b/example_7/hardware/include/ros2_control_demo_example_7/r6bot_hardware.hpp @@ -15,6 +15,7 @@ #ifndef ROS2_CONTROL_DEMO_EXAMPLE_7__R6BOT_HARDWARE_HPP_ #define ROS2_CONTROL_DEMO_EXAMPLE_7__R6BOT_HARDWARE_HPP_ +#include "memory" #include "string" #include "unordered_map" #include "vector" @@ -40,21 +41,27 @@ class RobotSystem : public hardware_interface::SystemInterface std::vector export_command_interfaces() override; + std::vector export_command_interface_descriptions() + override; + return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override; return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override; protected: - /// The size of this vector is (standard_interfaces_.size() x nr_joints) - std::vector joint_position_command_; - std::vector joint_velocities_command_; - std::vector joint_position_; - std::vector joint_velocities_; - std::vector ft_states_; - std::vector ft_command_; - - std::unordered_map> joint_interfaces = { - {"position", {}}, {"velocity", {}}}; + struct FTS_Sensor + { + explicit FTS_Sensor(const std::string & sensor_name) : name(sensor_name) {} + const std::string name; + const std::string force_x = "force.x"; + const std::string force_y = "force.y"; + const std::string force_z = "force.z"; + const std::string torque_x = "torque.x"; + const std::string torque_y = "torque.y"; + const std::string torque_z = "torque.z"; + }; + + std::unique_ptr fts_sensor_; }; } // namespace ros2_control_demo_example_7 diff --git a/example_7/hardware/r6bot_hardware.cpp b/example_7/hardware/r6bot_hardware.cpp index 7414b8fcf..2537163ba 100644 --- a/example_7/hardware/r6bot_hardware.cpp +++ b/example_7/hardware/r6bot_hardware.cpp @@ -26,93 +26,57 @@ CallbackReturn RobotSystem::on_init(const hardware_interface::HardwareInfo & inf } // robot has 6 joints and 2 interfaces - joint_position_.assign(6, 0); - joint_velocities_.assign(6, 0); - joint_position_command_.assign(6, 0); - joint_velocities_command_.assign(6, 0); - - // force sensor has 6 readings - ft_states_.assign(6, 0); - ft_command_.assign(6, 0); - - for (const auto & joint : info_.joints) + for (const auto & [state_itf_name, state_itf_descr] : joint_state_interfaces_) { - for (const auto & interface : joint.state_interfaces) - { - joint_interfaces[interface.name].push_back(joint.name); - } + set_state(state_itf_name, 0.0); } - - return CallbackReturn::SUCCESS; -} - -std::vector RobotSystem::export_state_interfaces() -{ - std::vector state_interfaces; - - int ind = 0; - for (const auto & joint_name : joint_interfaces["position"]) + for (const auto & [cmd_itf_name, cmd_itf_descr] : joint_command_interfaces_) { - state_interfaces.emplace_back(joint_name, "position", &joint_position_[ind++]); + set_state(cmd_itf_name, 0.0); } - ind = 0; - for (const auto & joint_name : joint_interfaces["velocity"]) + for (const auto & [sensor_itf_name, sensor_itf_descr] : sensor_state_interfaces_) { - state_interfaces.emplace_back(joint_name, "velocity", &joint_velocities_[ind++]); + set_state(sensor_itf_name, 0.0); } - state_interfaces.emplace_back("tcp_fts_sensor", "force.x", &ft_states_[0]); - state_interfaces.emplace_back("tcp_fts_sensor", "force.y", &ft_states_[1]); - state_interfaces.emplace_back("tcp_fts_sensor", "force.z", &ft_states_[2]); - state_interfaces.emplace_back("tcp_fts_sensor", "torque.x", &ft_states_[3]); - state_interfaces.emplace_back("tcp_fts_sensor", "torque.y", &ft_states_[4]); - state_interfaces.emplace_back("tcp_fts_sensor", "torque.z", &ft_states_[5]); + fts_sensor_ = std::make_unique(info_.sensors[0].name); - return state_interfaces; + return CallbackReturn::SUCCESS; } -std::vector RobotSystem::export_command_interfaces() +std::vector +RobotSystem::export_command_interface_descriptions() { - std::vector command_interfaces; - - int ind = 0; - for (const auto & joint_name : joint_interfaces["position"]) - { - command_interfaces.emplace_back(joint_name, "position", &joint_position_command_[ind++]); - } - - ind = 0; - for (const auto & joint_name : joint_interfaces["velocity"]) - { - command_interfaces.emplace_back(joint_name, "velocity", &joint_velocities_command_[ind++]); - } - - command_interfaces.emplace_back("tcp_fts_sensor", "force.x", &ft_command_[0]); - command_interfaces.emplace_back("tcp_fts_sensor", "force.y", &ft_command_[1]); - command_interfaces.emplace_back("tcp_fts_sensor", "force.z", &ft_command_[2]); - command_interfaces.emplace_back("tcp_fts_sensor", "torque.x", &ft_command_[3]); - command_interfaces.emplace_back("tcp_fts_sensor", "torque.y", &ft_command_[4]); - command_interfaces.emplace_back("tcp_fts_sensor", "torque.z", &ft_command_[5]); - - return command_interfaces; + std::vector additional_cmd_itfs; + additional_cmd_itfs.emplace_back( + fts_sensor_->name, hardware_interface::InterfaceInfo("force.x", "double")); + additional_cmd_itfs.emplace_back( + fts_sensor_->name, hardware_interface::InterfaceInfo("force.y", "double")); + additional_cmd_itfs.emplace_back( + fts_sensor_->name, hardware_interface::InterfaceInfo("force.z", "double")); + additional_cmd_itfs.emplace_back( + fts_sensor_->name, hardware_interface::InterfaceInfo("torque.x", "double")); + additional_cmd_itfs.emplace_back( + fts_sensor_->name, hardware_interface::InterfaceInfo("torque.y", "double")); + additional_cmd_itfs.emplace_back( + fts_sensor_->name, hardware_interface::InterfaceInfo("torque.z", "double")); + return additional_cmd_itfs; } return_type RobotSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { // TODO(pac48) set sensor_states_ values from subscriber - - for (auto i = 0ul; i < joint_velocities_command_.size(); i++) + for (const auto & component : info_.joints) { - joint_velocities_[i] = joint_velocities_command_[i]; - joint_position_[i] += joint_velocities_command_[i] * period.seconds(); - } + const auto & joint = component.name; + const auto joint_vel = joint + "/" + std::string(hardware_interface::HW_IF_VELOCITY); + const auto joint_pos = joint + "/" + std::string(hardware_interface::HW_IF_POSITION); - for (auto i = 0ul; i < joint_position_command_.size(); i++) - { - joint_position_[i] = joint_position_command_[i]; + set_state(joint_vel, get_command(joint_vel)); + set_state(joint_pos, get_state(joint_pos) + get_command(joint_vel) * period.seconds()); + set_state(joint_pos, get_command(joint_pos)); } - return return_type::OK; } diff --git a/example_8/description/ros2_control/rrbot_transmissions_system_position_only.ros2_control.xacro b/example_8/description/ros2_control/rrbot_transmissions_system_position_only.ros2_control.xacro index f7d13edf8..527fad71e 100644 --- a/example_8/description/ros2_control/rrbot_transmissions_system_position_only.ros2_control.xacro +++ b/example_8/description/ros2_control/rrbot_transmissions_system_position_only.ros2_control.xacro @@ -12,18 +12,24 @@ + double -1 1 - + + double + + double -1 1 - + + double + diff --git a/example_8/hardware/include/ros2_control_demo_example_8/rrbot_transmissions_system_position_only.hpp b/example_8/hardware/include/ros2_control_demo_example_8/rrbot_transmissions_system_position_only.hpp index c873a2815..2b17e4b3c 100644 --- a/example_8/hardware/include/ros2_control_demo_example_8/rrbot_transmissions_system_position_only.hpp +++ b/example_8/hardware/include/ros2_control_demo_example_8/rrbot_transmissions_system_position_only.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include "hardware_interface/handle.hpp" @@ -74,21 +75,19 @@ class RRBotTransmissionsSystemPositionOnlyHardware : public hardware_interface:: double actuator_slowdown_; // transmissions - std::vector> transmissions_; - - struct InterfaceData - { - explicit InterfaceData(const std::string & name); - - std::string name_; - double command_; - double state_; - - // this is the "sink" that will be part of the transmission Joint/Actuator handles - double transmission_passthrough_; - }; - std::vector joint_interfaces_; - std::vector actuator_interfaces_; + std::unordered_map> + transmissions_; + std::unordered_map joint_to_transmission_info_; + + std::unordered_map> + joint_handles_; + std::unordered_map> + actuator_handles_; + std::unordered_map> + actuator_states_; + + std::unordered_map actuator_to_joint_; + std::unordered_map joint_to_actuator_; }; } // namespace ros2_control_demo_example_8 diff --git a/example_8/hardware/rrbot_transmissions_system_position_only.cpp b/example_8/hardware/rrbot_transmissions_system_position_only.cpp index 6cf3b2428..3ad412257 100644 --- a/example_8/hardware/rrbot_transmissions_system_position_only.cpp +++ b/example_8/hardware/rrbot_transmissions_system_position_only.cpp @@ -54,18 +54,6 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: actuator_slowdown_ = hardware_interface::stod(info_.hardware_parameters["actuator_slowdown"]); - const auto num_joints = std::accumulate( - info_.transmissions.begin(), info_.transmissions.end(), 0ul, - [](const auto & acc, const auto & trans_info) { return acc + trans_info.joints.size(); }); - - const auto num_actuators = std::accumulate( - info_.transmissions.begin(), info_.transmissions.end(), 0ul, - [](const auto & acc, const auto & trans_info) { return acc + trans_info.actuators.size(); }); - - // reserve the space needed for joint and actuator data structures - joint_interfaces_.reserve(num_joints); - actuator_interfaces_.reserve(num_actuators); - // create transmissions, joint and actuator handles auto transmission_loader = transmission_interface::SimpleTransmissionLoader(); @@ -92,58 +80,124 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: return hardware_interface::CallbackReturn::ERROR; } - std::vector joint_handles; - for (const auto & joint_info : transmission_info.joints) + if (transmission_info.actuators.size() != transmission_info.joints.size()) + { + RCLCPP_FATAL( + *logger_, + "For transmission %s: the size of the actuators is %li but the size of the joints is %li. " + "Expected to be the same.", + transmission_info.name.c_str(), transmission_info.actuators.size(), + transmission_info.joints.size()); + return hardware_interface::CallbackReturn::ERROR; + } + std::vector> joint_handles_vec; + joint_handles_.reserve(transmission_info.actuators.size()); + std::vector> actuator_handles_vec; + actuator_handles_vec.reserve(transmission_info.actuators.size()); + + for (size_t i = 0; i < transmission_info.actuators.size(); ++i) { // this demo supports only one interface per joint - if (!(joint_info.state_interfaces.size() == 1 && - joint_info.state_interfaces[0] == hardware_interface::HW_IF_POSITION && - joint_info.command_interfaces.size() == 1 && - joint_info.command_interfaces[0] == hardware_interface::HW_IF_POSITION)) + if (!(transmission_info.joints.at(i).state_interfaces.size() == 1 && + transmission_info.joints.at(i).state_interfaces[0] == + hardware_interface::HW_IF_POSITION && + transmission_info.joints.at(i).command_interfaces.size() == 1 && + transmission_info.joints.at(i).command_interfaces[0] == + hardware_interface::HW_IF_POSITION)) { RCLCPP_FATAL( *logger_, "Invalid transmission joint '%s' configuration for this demo", - joint_info.name.c_str()); + transmission_info.joints.at(i).name.c_str()); return hardware_interface::CallbackReturn::ERROR; } - const auto joint_interface = - joint_interfaces_.insert(joint_interfaces_.end(), InterfaceData(joint_info.name)); - - transmission_interface::JointHandle joint_handle( - joint_info.name, hardware_interface::HW_IF_POSITION, - &joint_interface->transmission_passthrough_); - joint_handles.push_back(joint_handle); - } + const auto & joint_name = transmission_info.joints.at(i).name; + const auto & interface_type = std::string(hardware_interface::HW_IF_POSITION); + std::shared_ptr joint_handle = + std::make_shared( + hardware_interface::InterfaceDescription( + joint_name, hardware_interface::InterfaceInfo(interface_type, "double"))); - std::vector actuator_handles; - for (const auto & actuator_info : transmission_info.actuators) - { - // no check for actuators types + // check that transmission interface name is present in the StateInterfaces and + // CommandInterfaces. + if (joint_state_interfaces_.find(joint_handle->get_name()) == joint_state_interfaces_.end()) + { + if (joint_state_interfaces_.find(joint_handle->get_name()) == joint_state_interfaces_.end()) + RCLCPP_FATAL( + *logger_, + "A interface '%s' is expected to be present in joint_state_interfaces_ as its used " + "for JointHandle but was not found.", + joint_handle->get_name().c_str()); + return hardware_interface::CallbackReturn::ERROR; + } + else if ( + joint_command_interfaces_.find(joint_handle->get_name()) == joint_command_interfaces_.end()) + { + RCLCPP_FATAL( + *logger_, + "A interface '%s' is expected to be present in joint_command_interfaces_ as its used " + "for JointHandle but was not found.", + joint_handle->get_name().c_str()); + return hardware_interface::CallbackReturn::ERROR; + } - const auto actuator_interface = - actuator_interfaces_.insert(actuator_interfaces_.end(), InterfaceData(actuator_info.name)); - transmission_interface::ActuatorHandle actuator_handle( - actuator_info.name, hardware_interface::HW_IF_POSITION, - &actuator_interface->transmission_passthrough_); - actuator_handles.push_back(actuator_handle); - } + joint_handles_vec.push_back(joint_handle); + const auto & [jh_it, jh_succ] = + joint_handles_.insert({joint_handle->get_name(), joint_handle}); + if (!jh_succ) + { + RCLCPP_FATAL( + *logger_, + "Could not insert the JointHandle with name '%s' into map. Check for duplicate " + "names.", + joint_handle->get_name().c_str()); + return hardware_interface::CallbackReturn::ERROR; + } - /// @note no need to store the joint and actuator handles, the transmission - /// will keep whatever info it needs after is done with them + // no check for actuators types + const auto & actuator_name = transmission_info.actuators.at(i).name; + std::shared_ptr actuator_handle = + std::make_shared( + hardware_interface::InterfaceDescription( + actuator_name, hardware_interface::InterfaceInfo(interface_type, "double"))); + + actuator_handles_vec.push_back(actuator_handle); + const auto & [ah_it, ah_succ] = + actuator_handles_.insert({actuator_handle->get_name(), actuator_handle}); + if (!ah_succ) + { + RCLCPP_FATAL( + *logger_, + "Could not insert the ActuatorHandle with name '%s' into map. Check for duplicate " + "names.", + joint_handle->get_name().c_str()); + return hardware_interface::CallbackReturn::ERROR; + } + /// @note no need to store the joint and actuator handles, the transmission + /// will keep whatever info it needs after is done with them + try + { + transmission->configure(joint_handles_vec, actuator_handles_vec); + } + catch (const transmission_interface::TransmissionInterfaceException & exc) + { + RCLCPP_FATAL( + *logger_, "Error while configuring %s: %s", transmission_info.name.c_str(), exc.what()); + return hardware_interface::CallbackReturn::ERROR; + } - try - { - transmission->configure(joint_handles, actuator_handles); + // This is only ok because we know that only one joint per transmission is there and joint + // name and transmission joint name are the same. If there are multiple joints/actuators per + // transmission this has to be changed + auto actuator_state = std::make_shared( + hardware_interface::InterfaceDescription( + actuator_name, hardware_interface::InterfaceInfo(interface_type, "double"))); + actuator_states_[actuator_state->get_name()] = actuator_state; + transmissions_[joint_name] = transmission; + joint_to_transmission_info_[joint_name] = transmission_info; + joint_to_actuator_[joint_name] = actuator_name; + actuator_to_joint_[actuator_name] = joint_name; } - catch (const transmission_interface::TransmissionInterfaceException & exc) - { - RCLCPP_FATAL( - *logger_, "Error while configuring %s: %s", transmission_info.name.c_str(), exc.what()); - return hardware_interface::CallbackReturn::ERROR; - } - - transmissions_.push_back(transmission); } RCLCPP_INFO(*logger_, "Initialization successful"); @@ -156,56 +210,21 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: { RCLCPP_INFO(*logger_, "Configuring..."); - auto reset_interfaces = [](std::vector & interfaces) - { - for (auto & interface_data : interfaces) - { - interface_data.command_ = 0.0; - interface_data.state_ = 0.0; - interface_data.transmission_passthrough_ = kNaN; - } - }; - - reset_interfaces(joint_interfaces_); - reset_interfaces(actuator_interfaces_); - - RCLCPP_INFO(*logger_, "Configuration successful"); - - return hardware_interface::CallbackReturn::SUCCESS; -} - -std::vector -RRBotTransmissionsSystemPositionOnlyHardware::export_state_interfaces() -{ - std::vector state_interfaces; for (const auto & joint : info_.joints) { - /// @pre all joint interfaces exist, checked in on_init() - auto joint_interface = std::find_if( - joint_interfaces_.begin(), joint_interfaces_.end(), - [&](const InterfaceData & interface) { return interface.name_ == joint.name; }); - - state_interfaces.emplace_back(hardware_interface::StateInterface( - joint.name, hardware_interface::HW_IF_POSITION, &joint_interface->state_)); + const auto joint_pos = joint.name + "/" + hardware_interface::HW_IF_POSITION; + const auto actuator_name = joint_to_actuator_.at(joint.name); + const auto act_pos = actuator_name + "/" + hardware_interface::HW_IF_POSITION; + set_state(joint_pos, 0.0); + set_command(joint_pos, 0.0); + actuator_states_.at(act_pos)->set_value(0.0); + joint_handles_.at(joint_pos)->set_value(kNaN); + actuator_handles_.at(joint_pos)->set_value(kNaN); } - return state_interfaces; -} -std::vector -RRBotTransmissionsSystemPositionOnlyHardware::export_command_interfaces() -{ - std::vector command_interfaces; - for (const auto & joint : info_.joints) - { - /// @pre all joint interfaces exist, checked in on_init() - auto joint_interface = std::find_if( - joint_interfaces_.begin(), joint_interfaces_.end(), - [&](const InterfaceData & interface) { return interface.name_ == joint.name; }); + RCLCPP_INFO(*logger_, "Configuration successful"); - command_interfaces.emplace_back(hardware_interface::CommandInterface( - joint.name, hardware_interface::HW_IF_POSITION, &joint_interface->command_)); - } - return command_interfaces; + return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware::on_activate( @@ -229,44 +248,29 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: hardware_interface::return_type RRBotTransmissionsSystemPositionOnlyHardware::read( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - // actuator: state -> transmission - std::for_each( - actuator_interfaces_.begin(), actuator_interfaces_.end(), [](auto & actuator_interface) - { actuator_interface.transmission_passthrough_ = actuator_interface.state_; }); - - // transmission: actuator -> joint - std::for_each( - transmissions_.begin(), transmissions_.end(), - [](auto & transmission) { transmission->actuator_to_joint(); }); - - // joint: transmission -> state - std::for_each( - joint_interfaces_.begin(), joint_interfaces_.end(), [](auto & joint_interface) - { joint_interface.state_ = joint_interface.transmission_passthrough_; }); - - // log state data - std::stringstream ss; - ss << "State data:"; - for (const auto & transmission_info : info_.transmissions) + for (const auto & joint : info_.joints) { + const auto & actuator_name = joint_to_actuator_.at(joint.name); + const auto act_pos = actuator_name + "/" + hardware_interface::HW_IF_POSITION; + const auto joint_pos = joint.name + "/" + hardware_interface::HW_IF_POSITION; + // actuator: state -> transmission + actuator_handles_.at(act_pos)->set_value(actuator_states_.at(act_pos)->get_value()); + + // transmission: actuator -> joint + transmissions_.at(joint.name)->actuator_to_joint(); + + // joint: transmission -> state + set_state(joint_pos, joint_handles_.at(joint_pos)->get_value()); + // log state data // again, this only for simple transmissions, we know there is only one joint - const auto joint_interface = std::find_if( - joint_interfaces_.cbegin(), joint_interfaces_.cend(), [&](const auto & joint_interface) - { return joint_interface.name_ == transmission_info.joints[0].name; }); - - const auto actuator_interface = std::find_if( - actuator_interfaces_.cbegin(), actuator_interfaces_.cend(), - [&](const auto & actuator_interface) - { return actuator_interface.name_ == transmission_info.actuators[0].name; }); - - const auto & reduction = transmission_info.joints[0].mechanical_reduction; - - ss << std::endl - << "\t" << joint_interface->name_ << ": " << joint_interface->state_ << " <-- " - << transmission_info.name << "(R=" << reduction << ") <-- " << actuator_interface->name_ - << ": " << actuator_interface->state_; + const auto & transmission_info = joint_to_transmission_info_.at(joint.name); + std::stringstream ss; + ss << "State data:" << std::endl + << "\t" << joint_pos << ": " << get_state(joint_pos) << " <-- " << transmission_info.name + << "(R=" << transmission_info.joints[0].mechanical_reduction << ") <-- " << actuator_name + << ": " << actuator_states_.at(actuator_name)->get_value(); + RCLCPP_INFO_THROTTLE(*logger_, *clock_, 1000, "%s", ss.str().c_str()); } - RCLCPP_INFO_THROTTLE(*logger_, *clock_, 1000, "%s", ss.str().c_str()); return hardware_interface::return_type::OK; } @@ -274,63 +278,42 @@ hardware_interface::return_type RRBotTransmissionsSystemPositionOnlyHardware::re hardware_interface::return_type RRBotTransmissionsSystemPositionOnlyHardware::write( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - // joint: command -> transmission - std::for_each( - joint_interfaces_.begin(), joint_interfaces_.end(), [](auto & joint_interface) - { joint_interface.transmission_passthrough_ = joint_interface.command_; }); - - // transmission: joint -> actuator - std::for_each( - transmissions_.begin(), transmissions_.end(), - [](auto & transmission) { transmission->joint_to_actuator(); }); - - // actuator: transmission -> command - std::for_each( - actuator_interfaces_.begin(), actuator_interfaces_.end(), [](auto & actuator_interface) - { actuator_interface.command_ = actuator_interface.transmission_passthrough_; }); - - // simulate motor motion - std::for_each( - actuator_interfaces_.begin(), actuator_interfaces_.end(), - [&](auto & actuator_interface) - { - actuator_interface.state_ = - actuator_interface.state_ + - (actuator_interface.command_ - actuator_interface.state_) / actuator_slowdown_; - }); - - // log command data - std::stringstream ss; - ss << "Command data:"; - for (const auto & transmission_info : info_.transmissions) + for (const auto & joint : info_.joints) { - // again, this only for simple transmissions, we know there is only one joint - const auto joint_interface = std::find_if( - joint_interfaces_.cbegin(), joint_interfaces_.cend(), [&](const auto & joint_interface) - { return joint_interface.name_ == transmission_info.joints[0].name; }); - - const auto actuator_interface = std::find_if( - actuator_interfaces_.cbegin(), actuator_interfaces_.cend(), - [&](const auto & actuator_interface) - { return actuator_interface.name_ == transmission_info.actuators[0].name; }); - - const auto & reduction = transmission_info.joints[0].mechanical_reduction; - - ss << std::endl - << "\t" << joint_interface->name_ << ": " << joint_interface->command_ << " --> " - << transmission_info.name << "(R=" << reduction << ") --> " << actuator_interface->name_ - << ": " << actuator_interface->command_; + const auto & actuator_name = joint_to_actuator_.at(joint.name); + const auto act_pos = actuator_name + "/" + hardware_interface::HW_IF_POSITION; + const auto joint_pos = joint.name + "/" + hardware_interface::HW_IF_POSITION; + // joint: command -> transmission + joint_handles_.at(joint_pos)->set_value(get_command(joint_pos)); + + // transmission: joint -> actuator + transmissions_.at(joint.name)->joint_to_actuator(); + + // actuator: transmission -> command + actuator_states_.at(act_pos)->set_value(actuator_handles_.at(act_pos)->get_value()); + + // simulate motor motion + auto current_actuator_state = actuator_states_.at(act_pos)->get_value(); + double new_state = + current_actuator_state + + (actuator_handles_.at(act_pos)->get_value() - current_actuator_state) / + actuator_slowdown_; + actuator_states_.at(act_pos)->set_value(new_state); + + // log command data + const auto & transmission_info = joint_to_transmission_info_.at(joint.name); + std::stringstream ss; + ss << "Command data:" << std::endl + << "\t" << joint_pos << ": " << get_command(joint_pos) << " --> " << transmission_info.name + << "(R=" << transmission_info.joints[0].mechanical_reduction << ") --> " << actuator_name + << ": " << actuator_states_.at(actuator_name)->get_value(); + + RCLCPP_INFO_THROTTLE(*logger_, *clock_, 1000, "%s", ss.str().c_str()); } - RCLCPP_INFO_THROTTLE(*logger_, *clock_, 1000, "%s", ss.str().c_str()); return hardware_interface::return_type::OK; } -RRBotTransmissionsSystemPositionOnlyHardware::InterfaceData::InterfaceData(const std::string & name) -: name_(name), command_(kNaN), state_(kNaN), transmission_passthrough_(kNaN) -{ -} - } // namespace ros2_control_demo_example_8 #include "pluginlib/class_list_macros.hpp" From 1917d808b8e5de276375b74374a065c9fd6120ef Mon Sep 17 00:00:00 2001 From: Manuel M Date: Tue, 13 Aug 2024 17:11:38 +0200 Subject: [PATCH 2/2] update demos to full variant support --- .../ros2_control/rrbot.ros2_control.xacro | 31 +++- .../ros2_control_demo_example_10/rrbot.hpp | 30 +++- example_10/hardware/rrbot.cpp | 121 ++++++-------- ...s_for_position_feedback.ros2_control.xacro | 10 +- .../rrbot_actuator_without_feedback.hpp | 4 +- .../rrbot_sensor_for_position_feedback.hpp | 2 +- .../rrbot_actuator_without_feedback.cpp | 33 +--- .../rrbot_sensor_for_position_feedback.cpp | 24 +-- ..._system_multi_interface.ros2_control.xacro | 30 +++- .../rrbot_system_multi_interface.hpp | 29 ++-- .../hardware/rrbot_system_multi_interface.cpp | 148 ++++++++---------- ...rbot_system_with_sensor.ros2_control.xacro | 10 +- .../rrbot_system_with_sensor.hpp | 5 - .../hardware/rrbot_system_with_sensor.cpp | 103 ++++++------ .../ros2_control_demo_example_5/rrbot.hpp | 4 - example_5/hardware/rrbot.cpp | 78 +++++---- example_6/hardware/rrbot_actuator.cpp | 6 +- .../r6bot_hardware.hpp | 4 + 18 files changed, 330 insertions(+), 342 deletions(-) diff --git a/example_10/description/ros2_control/rrbot.ros2_control.xacro b/example_10/description/ros2_control/rrbot.ros2_control.xacro index 77611c089..1b3b2be5c 100644 --- a/example_10/description/ros2_control/rrbot.ros2_control.xacro +++ b/example_10/description/ros2_control/rrbot.ros2_control.xacro @@ -21,28 +21,45 @@ + double -1 1 - + + double + + double -1 1 - + + double + - - - - + + double + + + double + + + double + + + double + - + + double + + double 1.0 diff --git a/example_10/hardware/include/ros2_control_demo_example_10/rrbot.hpp b/example_10/hardware/include/ros2_control_demo_example_10/rrbot.hpp index 9f6b2c62f..9e5ff76df 100644 --- a/example_10/hardware/include/ros2_control_demo_example_10/rrbot.hpp +++ b/example_10/hardware/include/ros2_control_demo_example_10/rrbot.hpp @@ -68,11 +68,31 @@ class RRBotSystemWithGPIOHardware : public hardware_interface::SystemInterface private: // Parameters for the RRBot simulation - // Store the command and state interfaces for the simulated robot - std::vector hw_commands_; - std::vector hw_states_; - std::vector hw_gpio_in_; - std::vector hw_gpio_out_; + struct FlangeVacuum + { + explicit FlangeVacuum(const std::string & gpio_name) : name(gpio_name) {} + // delete move constructor since would throw because of const std::string members + // but we dont want to move this anyways so const for member is ok i guess + FlangeVacuum(FlangeVacuum && other) = delete; + + const std::string name; + const std::string vacuum = name + "/vacuum"; + }; + + struct FlangeIOs + { + explicit FlangeIOs(const std::string & gpio_name) : name(gpio_name) {} + // delete move constructor since would throw because of const std::string members + // but we dont want to move this anyways so const for member is ok i guess + FlangeIOs(FlangeIOs && other) = delete; + + const std::string name; + const std::string out = name + "/analog_output1"; + const std::string input_1 = name + "/analog_input1"; + const std::string input_2 = name + "/analog_input2"; + }; + std::unique_ptr flange_vacuum_; + std::unique_ptr flange_ios_; }; } // namespace ros2_control_demo_example_10 diff --git a/example_10/hardware/rrbot.cpp b/example_10/hardware/rrbot.cpp index d8e9bd84a..4d9708eda 100644 --- a/example_10/hardware/rrbot.cpp +++ b/example_10/hardware/rrbot.cpp @@ -35,9 +35,6 @@ hardware_interface::CallbackReturn RRBotSystemWithGPIOHardware::on_init( return hardware_interface::CallbackReturn::ERROR; } - hw_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - for (const hardware_interface::ComponentInfo & joint : info_.joints) { // RRBotSystemPositionOnly has exactly one state and command interface on each joint @@ -77,6 +74,27 @@ hardware_interface::CallbackReturn RRBotSystemWithGPIOHardware::on_init( return hardware_interface::CallbackReturn::ERROR; } } + // check if we have same command and state interfaces for joint this makes iterating easier + // first check if size is equal then we only need to iterate over one of them + if (joint_state_interfaces_.size() != joint_command_interfaces_.size()) + { + RCLCPP_FATAL( + rclcpp::get_logger("RRBotSystemWithSensorHardware"), + "Expect joint CommandInterface and joint StateInterfaces to be of equal size."); + return hardware_interface::CallbackReturn::ERROR; + } + for (const auto & [state_itf_name, state_itf_descr] : joint_state_interfaces_) + { + if (joint_command_interfaces_.find(state_itf_name) == joint_command_interfaces_.end()) + { + RCLCPP_FATAL( + rclcpp::get_logger("RRBotSystemWithSensorHardware"), + "Expect joint CommandInterface and joint StateInterfaces to be equal but StateInterface " + "includes<%s> which is not included in CommandInterfaces.", + state_itf_name.c_str()); + return hardware_interface::CallbackReturn::ERROR; + } + } // RRBotSystemWithGPIOHardware has exactly two GPIO components if (info_.gpios.size() != 2) @@ -116,6 +134,9 @@ hardware_interface::CallbackReturn RRBotSystemWithGPIOHardware::on_init( info_.gpios[0].state_interfaces.size(), 1); return hardware_interface::CallbackReturn::ERROR; } + // set gpios + flange_ios_ = std::make_unique(info_.gpios[0].name); + flange_vacuum_ = std::make_unique(info_.gpios[1].name); return hardware_interface::CallbackReturn::SUCCESS; } @@ -128,69 +149,23 @@ hardware_interface::CallbackReturn RRBotSystemWithGPIOHardware::on_configure( // END: This part here is for exemplary purposes - Please do not copy to your production code // reset values always when configuring hardware - std::fill(hw_states_.begin(), hw_states_.end(), 0); - std::fill(hw_commands_.begin(), hw_commands_.end(), 0); - std::fill(hw_gpio_in_.begin(), hw_gpio_in_.end(), 0); - std::fill(hw_gpio_out_.begin(), hw_gpio_out_.end(), 0); - - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Successfully configured!"); - - return hardware_interface::CallbackReturn::SUCCESS; -} - -std::vector -RRBotSystemWithGPIOHardware::export_state_interfaces() -{ - std::vector state_interfaces; - for (uint i = 0; i < info_.joints.size(); i++) - { - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_[i])); - } - - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "State interfaces:"); - hw_gpio_in_.resize(4); - size_t ct = 0; - for (size_t i = 0; i < info_.gpios.size(); i++) + for (const auto & [itf_name, itf_descr] : joint_state_interfaces_) { - for (auto state_if : info_.gpios.at(i).state_interfaces) - { - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.gpios.at(i).name, state_if.name, &hw_gpio_in_[ct++])); - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Added %s/%s", - info_.gpios.at(i).name.c_str(), state_if.name.c_str()); - } + set_state(itf_name, 0.0); + set_command(itf_name, 0.0); } - - return state_interfaces; -} - -std::vector -RRBotSystemWithGPIOHardware::export_command_interfaces() -{ - std::vector command_interfaces; - for (uint i = 0; i < info_.joints.size(); i++) + for (const auto & [gpio_state, gpio_state_desc] : gpio_state_interfaces_) { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i])); + set_state(gpio_state, 0.0); } - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Command interfaces:"); - hw_gpio_out_.resize(2); - size_t ct = 0; - for (size_t i = 0; i < info_.gpios.size(); i++) + for (const auto & [gpio_cmd, gpio_cmd_desc] : gpio_command_interfaces_) { - for (auto command_if : info_.gpios.at(i).command_interfaces) - { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.gpios.at(i).name, command_if.name, &hw_gpio_out_[ct++])); - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Added %s/%s", - info_.gpios.at(i).name.c_str(), command_if.name.c_str()); - } + set_command(gpio_cmd, 0.0); } - return command_interfaces; + RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Successfully configured!"); + + return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn RRBotSystemWithGPIOHardware::on_activate( @@ -201,9 +176,9 @@ hardware_interface::CallbackReturn RRBotSystemWithGPIOHardware::on_activate( // END: This part here is for exemplary purposes - Please do not copy to your production code // command and state should be equal when starting - for (uint i = 0; i < hw_states_.size(); i++) + for (const auto & [itf_name, itf_descr] : joint_state_interfaces_) { - hw_commands_[i] = hw_states_[i]; + set_command(itf_name, get_state(itf_name)); } RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Successfully activated!"); @@ -227,26 +202,26 @@ hardware_interface::return_type RRBotSystemWithGPIOHardware::read( // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Reading..."); - for (uint i = 0; i < hw_states_.size(); i++) + for (const auto & [itf_name, itf_descr] : joint_state_interfaces_) { // Simulate RRBot's movement - hw_states_[i] = hw_states_[i] + (hw_commands_[i] - hw_states_[i]); + set_state(itf_name, get_state(itf_name) + (get_command(itf_name) - get_state(itf_name))); } // mirror GPIOs back - hw_gpio_in_[0] = hw_gpio_out_[0]; - hw_gpio_in_[3] = hw_gpio_out_[1]; + set_state(flange_ios_->out, get_command(flange_ios_->out)); + set_state(flange_vacuum_->vacuum, get_command(flange_vacuum_->vacuum)); // random inputs unsigned int seed = time(NULL) + 1; - hw_gpio_in_[1] = static_cast(rand_r(&seed)); + set_state(flange_ios_->input_1, static_cast(rand_r(&seed))); seed = time(NULL) + 2; - hw_gpio_in_[2] = static_cast(rand_r(&seed)); + set_state(flange_ios_->input_2, static_cast(rand_r(&seed))); - for (uint i = 0; i < hw_gpio_in_.size(); i++) + for (const auto & [gpio_state, gpio_state_desc] : gpio_state_interfaces_) { RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Read %.1f from GP input %d!", - hw_gpio_in_[i], i); + rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Read %.1f from GP input %s!", + get_state(gpio_state), gpio_state.c_str()); } RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "GPIOs successfully read!"); // END: This part here is for exemplary purposes - Please do not copy to your production code @@ -260,11 +235,11 @@ hardware_interface::return_type RRBotSystemWithGPIOHardware::write( // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Writing..."); - for (uint i = 0; i < hw_gpio_out_.size(); i++) + for (const auto & [gpio_cmd, gpio_cmd_desc] : gpio_command_interfaces_) { RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Got command %.1f for GP output %d!", - hw_gpio_out_[i], i); + rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Got command %.1f for GP output %s!", + get_command(gpio_cmd), gpio_cmd.c_str()); } RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "GPIOs successfully written!"); // END: This part here is for exemplary purposes - Please do not copy to your production code diff --git a/example_14/description/ros2_control/rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.ros2_control.xacro b/example_14/description/ros2_control/rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.ros2_control.xacro index 739ffc8e5..b2b0d0f2d 100644 --- a/example_14/description/ros2_control/rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.ros2_control.xacro +++ b/example_14/description/ros2_control/rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.ros2_control.xacro @@ -17,7 +17,9 @@ 23286 - + + double + @@ -29,7 +31,9 @@ 23287 - + + double + @@ -42,6 +46,7 @@ + double -1 1 @@ -56,6 +61,7 @@ + double -1 1 diff --git a/example_14/hardware/include/ros2_control_demo_example_14/rrbot_actuator_without_feedback.hpp b/example_14/hardware/include/ros2_control_demo_example_14/rrbot_actuator_without_feedback.hpp index 775d4d592..fb37ed39a 100644 --- a/example_14/hardware/include/ros2_control_demo_example_14/rrbot_actuator_without_feedback.hpp +++ b/example_14/hardware/include/ros2_control_demo_example_14/rrbot_actuator_without_feedback.hpp @@ -74,9 +74,7 @@ class RRBotActuatorWithoutFeedback : public hardware_interface::ActuatorInterfac // Parameters for the RRBot simulation double hw_start_sec_; double hw_stop_sec_; - - // Store the command for the simulated robot - double hw_joint_command_; + std::string vel_itf_; // Fake "mechanical connection" between actuator and sensor using sockets struct sockaddr_in address_; diff --git a/example_14/hardware/include/ros2_control_demo_example_14/rrbot_sensor_for_position_feedback.hpp b/example_14/hardware/include/ros2_control_demo_example_14/rrbot_sensor_for_position_feedback.hpp index 20e9a51e9..fa8895f8d 100644 --- a/example_14/hardware/include/ros2_control_demo_example_14/rrbot_sensor_for_position_feedback.hpp +++ b/example_14/hardware/include/ros2_control_demo_example_14/rrbot_sensor_for_position_feedback.hpp @@ -80,7 +80,7 @@ class RRBotSensorPositionFeedback : public hardware_interface::SensorInterface // Store the command for the simulated robot double measured_velocity; // Local variable, but avoid initialization on each read double last_measured_velocity_; - double hw_joint_state_; + std::string pos_itf_; // Timestamps to calculate position for velocity rclcpp::Clock clock_; diff --git a/example_14/hardware/rrbot_actuator_without_feedback.cpp b/example_14/hardware/rrbot_actuator_without_feedback.cpp index 6feb8067c..4bd22e8c3 100644 --- a/example_14/hardware/rrbot_actuator_without_feedback.cpp +++ b/example_14/hardware/rrbot_actuator_without_feedback.cpp @@ -49,8 +49,6 @@ hardware_interface::CallbackReturn RRBotActuatorWithoutFeedback::on_init( socket_port_ = std::stoi(info_.hardware_parameters["example_param_socket_port"]); // END: This part here is for exemplary purposes - Please do not copy to your production code - hw_joint_command_ = std::numeric_limits::quiet_NaN(); - const hardware_interface::ComponentInfo & joint = info_.joints[0]; // RRBotActuatorWithoutFeedback has exactly one command interface and one joint if (joint.command_interfaces.size() != 1) @@ -71,6 +69,8 @@ hardware_interface::CallbackReturn RRBotActuatorWithoutFeedback::on_init( return hardware_interface::CallbackReturn::ERROR; } + vel_itf_ = joint.name + "/" + joint.command_interfaces[0].name; + // START: This part here is for exemplary purposes - Please do not copy to your production code // Initialize objects for fake mechanical connection sock_ = socket(AF_INET, SOCK_STREAM, 0); @@ -111,24 +111,6 @@ hardware_interface::CallbackReturn RRBotActuatorWithoutFeedback::on_shutdown( return hardware_interface::CallbackReturn::SUCCESS; } -std::vector -RRBotActuatorWithoutFeedback::export_state_interfaces() -{ - std::vector state_interfaces; - return state_interfaces; -} - -std::vector -RRBotActuatorWithoutFeedback::export_command_interfaces() -{ - std::vector command_interfaces; - - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[0].name, hardware_interface::HW_IF_VELOCITY, &hw_joint_command_)); - - return command_interfaces; -} - hardware_interface::CallbackReturn RRBotActuatorWithoutFeedback::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { @@ -145,9 +127,9 @@ hardware_interface::CallbackReturn RRBotActuatorWithoutFeedback::on_activate( // END: This part here is for exemplary purposes - Please do not copy to your production code // set some default values for joints - if (std::isnan(hw_joint_command_)) + if (!command_holds_value(vel_itf_) || std::isnan(get_command(vel_itf_))) { - hw_joint_command_ = 0; + set_command(vel_itf_, 0.0); } RCLCPP_INFO(rclcpp::get_logger("RRBotActuatorWithoutFeedback"), "Successfully activated!"); @@ -183,15 +165,16 @@ hardware_interface::return_type RRBotActuatorWithoutFeedback::read( hardware_interface::return_type ros2_control_demo_example_14::RRBotActuatorWithoutFeedback::write( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - if (std::isfinite(hw_joint_command_)) + if (std::isfinite(get_command(vel_itf_))) { // START: This part here is for exemplary purposes - Please do not copy to your production code RCLCPP_INFO( - rclcpp::get_logger("RRBotActuatorWithoutFeedback"), "Writing command: %f", hw_joint_command_); + rclcpp::get_logger("RRBotActuatorWithoutFeedback"), "Writing command: %f", + get_command(vel_itf_)); // Simulate sending commands to the hardware std::ostringstream data; - data << hw_joint_command_; + data << get_command(vel_itf_); RCLCPP_INFO( rclcpp::get_logger("RRBotActuatorWithoutFeedback"), "Sending data command: %s", data.str().c_str()); diff --git a/example_14/hardware/rrbot_sensor_for_position_feedback.cpp b/example_14/hardware/rrbot_sensor_for_position_feedback.cpp index 395c922c6..0984e6696 100644 --- a/example_14/hardware/rrbot_sensor_for_position_feedback.cpp +++ b/example_14/hardware/rrbot_sensor_for_position_feedback.cpp @@ -52,8 +52,6 @@ hardware_interface::CallbackReturn RRBotSensorPositionFeedback::on_init( socket_port_ = std::stoi(info_.hardware_parameters["example_param_socket_port"]); // END: This part here is for exemplary purposes - Please do not copy to your production code - hw_joint_state_ = std::numeric_limits::quiet_NaN(); - const hardware_interface::ComponentInfo & joint = info_.joints[0]; // RRBotSensorPositionFeedback has exactly one state interface and one joint if (joint.state_interfaces.size() != 1) @@ -73,6 +71,7 @@ hardware_interface::CallbackReturn RRBotSensorPositionFeedback::on_init( joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); return hardware_interface::CallbackReturn::ERROR; } + pos_itf_ = joint.name + "/" + joint.state_interfaces[0].name; clock_ = rclcpp::Clock(); @@ -184,25 +183,15 @@ hardware_interface::CallbackReturn RRBotSensorPositionFeedback::on_shutdown( return hardware_interface::CallbackReturn::SUCCESS; } -std::vector -RRBotSensorPositionFeedback::export_state_interfaces() -{ - std::vector state_interfaces; - - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[0].name, hardware_interface::HW_IF_POSITION, &hw_joint_state_)); - - return state_interfaces; -} - hardware_interface::CallbackReturn RRBotSensorPositionFeedback::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { // set some default values for joints - if (std::isnan(hw_joint_state_)) + if (!state_holds_value(pos_itf_) || std::isnan(get_state(pos_itf_))) { - hw_joint_state_ = 0; + set_state(pos_itf_, 0.0); } + last_measured_velocity_ = 0; // In general after a hardware is configured it can be read @@ -269,10 +258,11 @@ hardware_interface::return_type RRBotSensorPositionFeedback::read( RCLCPP_INFO( rclcpp::get_logger("RRBotSensorPositionFeedback"), "Got measured velocity %.5f", measured_velocity); - hw_joint_state_ += (last_measured_velocity_ * duration.seconds()) / hw_slowdown_; + set_state( + pos_itf_, get_state(pos_itf_) + (last_measured_velocity_ * duration.seconds()) / hw_slowdown_); RCLCPP_INFO( rclcpp::get_logger("RRBotSensorPositionFeedback"), "Got state %.5f for joint '%s'!", - hw_joint_state_, info_.joints[0].name.c_str()); + get_state(pos_itf_), info_.joints[0].name.c_str()); RCLCPP_INFO(rclcpp::get_logger("RRBotSensorPositionFeedback"), "Joints successfully read!"); // END: This part here is for exemplary purposes - Please do not copy to your production code diff --git a/example_3/description/ros2_control/rrbot_system_multi_interface.ros2_control.xacro b/example_3/description/ros2_control/rrbot_system_multi_interface.ros2_control.xacro index b0be21fac..a3028b6a2 100644 --- a/example_3/description/ros2_control/rrbot_system_multi_interface.ros2_control.xacro +++ b/example_3/description/ros2_control/rrbot_system_multi_interface.ros2_control.xacro @@ -21,37 +21,55 @@ + double -1 1 + double -1 1 + double -1 1 - - - + + double + + + double + + + double + + double -1 1 + double -1 1 + double -1 1 - - - + + double + + + double + + + double + diff --git a/example_3/hardware/include/ros2_control_demo_example_3/rrbot_system_multi_interface.hpp b/example_3/hardware/include/ros2_control_demo_example_3/rrbot_system_multi_interface.hpp index 578b07eab..55b3f07ea 100644 --- a/example_3/hardware/include/ros2_control_demo_example_3/rrbot_system_multi_interface.hpp +++ b/example_3/hardware/include/ros2_control_demo_example_3/rrbot_system_multi_interface.hpp @@ -77,14 +77,6 @@ class RRBotSystemMultiInterfaceHardware : public hardware_interface::SystemInter double hw_stop_sec_; double hw_slowdown_; - // Store the commands for the simulated robot - std::vector hw_commands_positions_; - std::vector hw_commands_velocities_; - std::vector hw_commands_accelerations_; - std::vector hw_states_positions_; - std::vector hw_states_velocities_; - std::vector hw_states_accelerations_; - // Enum defining at which control level we are // Dumb way of maintaining the command_interface type per joint. enum integration_level_t : std::uint8_t @@ -95,8 +87,25 @@ class RRBotSystemMultiInterfaceHardware : public hardware_interface::SystemInter ACCELERATION = 3 }; - // Active control mode for each actuator - std::vector control_level_; + struct Joint + { + explicit Joint(const std::string & joint_name) : name_(joint_name) {} + + const std::string & name() const { return name_; } + const std::string & position() const { return position_; } + const std::string & velocity() const { return velocity_; } + const std::string & acceleration() const { return acceleration_; } + + protected: + std::string name_; + std::string position_ = name_ + "/" + hardware_interface::HW_IF_POSITION; + std::string velocity_ = name_ + "/" + hardware_interface::HW_IF_VELOCITY; + std::string acceleration_ = name_ + "/" + hardware_interface::HW_IF_ACCELERATION; + }; + + std::vector joints_; + // All joints have same mode + integration_level_t control_level_; }; } // namespace ros2_control_demo_example_3 diff --git a/example_3/hardware/rrbot_system_multi_interface.cpp b/example_3/hardware/rrbot_system_multi_interface.cpp index ee6f4d37b..0f2bee8c1 100644 --- a/example_3/hardware/rrbot_system_multi_interface.cpp +++ b/example_3/hardware/rrbot_system_multi_interface.cpp @@ -41,13 +41,6 @@ hardware_interface::CallbackReturn RRBotSystemMultiInterfaceHardware::on_init( hw_stop_sec_ = stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]); hw_slowdown_ = stod(info_.hardware_parameters["example_param_hw_slowdown"]); // END: This part here is for exemplary purposes - Please do not copy to your production code - hw_states_positions_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_states_velocities_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_states_accelerations_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_commands_positions_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_commands_velocities_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_commands_accelerations_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - control_level_.resize(info_.joints.size(), integration_level_t::POSITION); for (const hardware_interface::ComponentInfo & joint : info_.joints) { @@ -96,42 +89,35 @@ hardware_interface::CallbackReturn RRBotSystemMultiInterfaceHardware::on_init( } } - return hardware_interface::CallbackReturn::SUCCESS; -} - -std::vector -RRBotSystemMultiInterfaceHardware::export_state_interfaces() -{ - std::vector state_interfaces; - for (std::size_t i = 0; i < info_.joints.size(); i++) + // check if we have same command and state interfaces for joint this makes iterating easier + // first check if size is equal then we only need to iterate over one of them + if (joint_state_interfaces_.size() != joint_command_interfaces_.size()) { - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_positions_[i])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_states_velocities_[i])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_ACCELERATION, &hw_states_accelerations_[i])); + RCLCPP_FATAL( + rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), + "Expect joint CommandInterface and joint StateInterfaces to be of equal size."); + return hardware_interface::CallbackReturn::ERROR; + } + for (const auto & [state_itf_name, state_itf_descr] : joint_state_interfaces_) + { + if (joint_command_interfaces_.find(state_itf_name) == joint_command_interfaces_.end()) + { + RCLCPP_FATAL( + rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), + "Expect joint CommandInterface and joint StateInterfaces to be equal but StateInterface " + "includes<%s> which is not included in CommandInterfaces.", + state_itf_name.c_str()); + return hardware_interface::CallbackReturn::ERROR; + } } - return state_interfaces; -} - -std::vector -RRBotSystemMultiInterfaceHardware::export_command_interfaces() -{ - std::vector command_interfaces; - for (std::size_t i = 0; i < info_.joints.size(); i++) + // fill joints vector + for (const auto & joint : info_.joints) { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_positions_[i])); - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_commands_velocities_[i])); - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_ACCELERATION, - &hw_commands_accelerations_[i])); + joints_.emplace_back(joint.name); } - return command_interfaces; + return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::return_type RRBotSystemMultiInterfaceHardware::prepare_command_mode_switch( @@ -178,21 +164,21 @@ hardware_interface::return_type RRBotSystemMultiInterfaceHardware::prepare_comma { if (key.find(info_.joints[i].name) != std::string::npos) { - hw_commands_velocities_[i] = 0; - hw_commands_accelerations_[i] = 0; - control_level_[i] = integration_level_t::UNDEFINED; // Revert to undefined + set_command(key + "/" + hardware_interface::HW_IF_VELOCITY, 0.0); + set_command(key + "/" + hardware_interface::HW_IF_ACCELERATION, 0.0); + control_level_ = integration_level_t::UNDEFINED; // Revert to undefined } } } // Set the new command modes for (std::size_t i = 0; i < info_.joints.size(); i++) { - if (control_level_[i] != integration_level_t::UNDEFINED) + if (control_level_ != integration_level_t::UNDEFINED) { // Something else is using the joint! Abort! return hardware_interface::return_type::ERROR; } - control_level_[i] = new_modes[i]; + control_level_ = new_modes[0]; } return hardware_interface::return_type::OK; } @@ -214,38 +200,24 @@ hardware_interface::CallbackReturn RRBotSystemMultiInterfaceHardware::on_activat // END: This part here is for exemplary purposes - Please do not copy to your production code // Set some default values - for (std::size_t i = 0; i < hw_states_positions_.size(); i++) + // we checked that joint_state_interfaces_ == joint_command_interfaces_ + for (const auto & [itf_name, itf_descr] : joint_state_interfaces_) { - if (std::isnan(hw_states_positions_[i])) + if (!state_holds_value(itf_name) || std::isnan(get_state(itf_name))) { - hw_states_positions_[i] = 0; + set_state(itf_name, 0.0); } - if (std::isnan(hw_states_velocities_[i])) + if (!command_holds_value(itf_name) || std::isnan(get_command(itf_name))) { - hw_states_velocities_[i] = 0; + set_command(itf_name, 0.0); } - if (std::isnan(hw_states_accelerations_[i])) - { - hw_states_accelerations_[i] = 0; - } - if (std::isnan(hw_commands_positions_[i])) - { - hw_commands_positions_[i] = 0; - } - if (std::isnan(hw_commands_velocities_[i])) - { - hw_commands_velocities_[i] = 0; - } - if (std::isnan(hw_commands_accelerations_[i])) - { - hw_commands_accelerations_[i] = 0; - } - control_level_[i] = integration_level_t::UNDEFINED; } + control_level_ = integration_level_t::UNDEFINED; + RCLCPP_INFO( rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"), "System successfully activated! %u", - control_level_[0]); + control_level_); return hardware_interface::CallbackReturn::SUCCESS; } @@ -273,9 +245,10 @@ hardware_interface::CallbackReturn RRBotSystemMultiInterfaceHardware::on_deactiv hardware_interface::return_type RRBotSystemMultiInterfaceHardware::read( const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { - for (std::size_t i = 0; i < hw_states_positions_.size(); i++) + // we checked that joint_state_interfaces_ == joint_command_interfaces_ + for (const auto & joint : joints_) { - switch (control_level_[i]) + switch (control_level_) { case integration_level_t::UNDEFINED: RCLCPP_INFO( @@ -284,27 +257,36 @@ hardware_interface::return_type RRBotSystemMultiInterfaceHardware::read( return hardware_interface::return_type::OK; break; case integration_level_t::POSITION: - hw_states_accelerations_[i] = 0; - hw_states_velocities_[i] = 0; - hw_states_positions_[i] += - (hw_commands_positions_[i] - hw_states_positions_[i]) / hw_slowdown_; + set_state(joint.acceleration(), 0.0); + set_state(joint.velocity(), 0.0); + set_state( + joint.position(), + get_state(joint.position()) + + (get_command(joint.position()) - get_state(joint.position()) / hw_slowdown_)); break; case integration_level_t::VELOCITY: - hw_states_accelerations_[i] = 0; - hw_states_velocities_[i] = hw_commands_velocities_[i]; - hw_states_positions_[i] += (hw_states_velocities_[i] * period.seconds()) / hw_slowdown_; + set_state(joint.acceleration(), 0.0); + set_state(joint.velocity(), get_command(joint.velocity())); + set_state( + joint.position(), get_state(joint.position()) + + (get_state(joint.velocity()) * period.seconds()) / hw_slowdown_); break; case integration_level_t::ACCELERATION: - hw_states_accelerations_[i] = hw_commands_accelerations_[i]; - hw_states_velocities_[i] += (hw_states_accelerations_[i] * period.seconds()) / hw_slowdown_; - hw_states_positions_[i] += (hw_states_velocities_[i] * period.seconds()) / hw_slowdown_; + set_state(joint.acceleration(), get_command(joint.acceleration())); + set_state( + joint.velocity(), + get_state(joint.velocity()) + + (get_command(joint.acceleration()) * period.seconds()) / hw_slowdown_); + set_state( + joint.position(), get_state(joint.position()) + + (get_state(joint.velocity()) * period.seconds()) / hw_slowdown_); break; } // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code RCLCPP_INFO( rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"), - "Got pos: %.5f, vel: %.5f, acc: %.5f for joint %lu!", hw_states_positions_[i], - hw_states_velocities_[i], hw_states_accelerations_[i], i); + "Got pos: %.5f, vel: %.5f, acc: %.5f for joint %s!", get_state(joint.position()), + get_state(joint.velocity()), get_state(joint.acceleration()), joint.name().c_str()); // END: This part here is for exemplary purposes - Please do not copy to your production code } return hardware_interface::return_type::OK; @@ -314,14 +296,14 @@ hardware_interface::return_type RRBotSystemMultiInterfaceHardware::write( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - for (std::size_t i = 0; i < hw_commands_positions_.size(); i++) + for (const auto & joint : joints_) { // Simulate sending commands to the hardware RCLCPP_INFO( rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"), - "Got the commands pos: %.5f, vel: %.5f, acc: %.5f for joint %lu, control_lvl:%u", - hw_commands_positions_[i], hw_commands_velocities_[i], hw_commands_accelerations_[i], i, - control_level_[i]); + "Got the commands pos: %.5f, vel: %.5f, acc: %.5f for joint %s, control_lvl:%u", + get_state(joint.position()), get_state(joint.velocity()), get_state(joint.acceleration()), + joint.name().c_str(), control_level_); } // END: This part here is for exemplary purposes - Please do not copy to your production code diff --git a/example_4/description/ros2_control/rrbot_system_with_sensor.ros2_control.xacro b/example_4/description/ros2_control/rrbot_system_with_sensor.ros2_control.xacro index 7b821e010..ec058545a 100644 --- a/example_4/description/ros2_control/rrbot_system_with_sensor.ros2_control.xacro +++ b/example_4/description/ros2_control/rrbot_system_with_sensor.ros2_control.xacro @@ -22,17 +22,23 @@ + double -1 1 - + + double + + double -1 1 - + + double + diff --git a/example_4/hardware/include/ros2_control_demo_example_4/rrbot_system_with_sensor.hpp b/example_4/hardware/include/ros2_control_demo_example_4/rrbot_system_with_sensor.hpp index de6b1f3d1..b6a79f54a 100644 --- a/example_4/hardware/include/ros2_control_demo_example_4/rrbot_system_with_sensor.hpp +++ b/example_4/hardware/include/ros2_control_demo_example_4/rrbot_system_with_sensor.hpp @@ -74,11 +74,6 @@ class RRBotSystemWithSensorHardware : public hardware_interface::SystemInterface double hw_stop_sec_; double hw_slowdown_; double hw_sensor_change_; - - // Store the command for the simulated robot - std::vector hw_joint_commands_; - std::vector hw_joint_states_; - std::vector hw_sensor_states_; }; } // namespace ros2_control_demo_example_4 diff --git a/example_4/hardware/rrbot_system_with_sensor.cpp b/example_4/hardware/rrbot_system_with_sensor.cpp index eea370e1f..8d796a36e 100644 --- a/example_4/hardware/rrbot_system_with_sensor.cpp +++ b/example_4/hardware/rrbot_system_with_sensor.cpp @@ -46,11 +46,6 @@ hardware_interface::CallbackReturn RRBotSystemWithSensorHardware::on_init( hw_sensor_change_ = stod(info_.hardware_parameters["example_param_max_sensor_change"]); // END: This part here is for exemplary purposes - Please do not copy to your production code - hw_joint_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_joint_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_sensor_states_.resize( - info_.sensors[0].state_interfaces.size(), std::numeric_limits::quiet_NaN()); - for (const hardware_interface::ComponentInfo & joint : info_.joints) { // RRBotSystemWithSensor has exactly one state and command interface on each joint @@ -91,6 +86,28 @@ hardware_interface::CallbackReturn RRBotSystemWithSensorHardware::on_init( } } + // check if we have same command and state interfaces for joint this makes iterating easier + // first check if size is equal then we only need to iterate over one of them + if (joint_state_interfaces_.size() != joint_command_interfaces_.size()) + { + RCLCPP_FATAL( + rclcpp::get_logger("RRBotSystemWithSensorHardware"), + "Expect joint CommandInterface and joint StateInterfaces to be of equal size."); + return hardware_interface::CallbackReturn::ERROR; + } + for (const auto & [state_itf_name, state_itf_descr] : joint_state_interfaces_) + { + if (joint_command_interfaces_.find(state_itf_name) == joint_command_interfaces_.end()) + { + RCLCPP_FATAL( + rclcpp::get_logger("RRBotSystemWithSensorHardware"), + "Expect joint CommandInterface and joint StateInterfaces to be equal but StateInterface " + "includes<%s> which is not included in CommandInterfaces.", + state_itf_name.c_str()); + return hardware_interface::CallbackReturn::ERROR; + } + } + return hardware_interface::CallbackReturn::SUCCESS; } @@ -110,10 +127,11 @@ hardware_interface::CallbackReturn RRBotSystemWithSensorHardware::on_configure( // END: This part here is for exemplary purposes - Please do not copy to your production code // reset values always when configuring hardware - for (uint i = 0; i < hw_joint_states_.size(); i++) + // we checked before that joint_state_itfs_ == joint_command_itfs_; + for (const auto & [itf_name, itf_desc] : joint_state_interfaces_) { - hw_joint_states_[i] = 0; - hw_joint_commands_[i] = 0; + set_state(itf_name, 0.0); + set_command(itf_name, 0.0); } RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Successfully configured!"); @@ -121,39 +139,6 @@ hardware_interface::CallbackReturn RRBotSystemWithSensorHardware::on_configure( return hardware_interface::CallbackReturn::SUCCESS; } -std::vector -RRBotSystemWithSensorHardware::export_state_interfaces() -{ - std::vector state_interfaces; - for (uint i = 0; i < info_.joints.size(); i++) - { - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_joint_states_[i])); - } - - // export sensor state interface - for (uint i = 0; i < info_.sensors[0].state_interfaces.size(); i++) - { - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.sensors[0].name, info_.sensors[0].state_interfaces[i].name, &hw_sensor_states_[i])); - } - - return state_interfaces; -} - -std::vector -RRBotSystemWithSensorHardware::export_command_interfaces() -{ - std::vector command_interfaces; - for (uint i = 0; i < info_.joints.size(); i++) - { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_joint_commands_[i])); - } - - return command_interfaces; -} - hardware_interface::CallbackReturn RRBotSystemWithSensorHardware::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { @@ -170,15 +155,18 @@ hardware_interface::CallbackReturn RRBotSystemWithSensorHardware::on_activate( // END: This part here is for exemplary purposes - Please do not copy to your production code // command and state should be equal when starting - for (uint i = 0; i < hw_joint_states_.size(); i++) + for (const auto & [itf_name, itf_desc] : joint_state_interfaces_) { - hw_joint_commands_[i] = hw_joint_states_[i]; + set_command(itf_name, get_state(itf_name)); } // set default value for sensor - if (std::isnan(hw_sensor_states_[0])) + for (const auto & [sensor_itf_name, sensor_itf_desc] : sensor_state_interfaces_) { - hw_sensor_states_[0] = 0; + if (!state_holds_value(sensor_itf_name) || std::isnan(get_state(sensor_itf_name))) + { + set_state(sensor_itf_name, 0.0); + } } RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Successfully activated!"); @@ -213,25 +201,30 @@ hardware_interface::return_type RRBotSystemWithSensorHardware::read( // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Reading...please wait..."); - for (uint i = 0; i < hw_joint_states_.size(); i++) + for (const auto & [itf_name, itf_desc] : joint_state_interfaces_) { // Simulate RRBot's movement - hw_joint_states_[i] += (hw_joint_commands_[i] - hw_joint_states_[i]) / hw_slowdown_; + auto old_state = get_state(itf_name); + auto new_state = old_state + (get_command(itf_name) - old_state) / hw_slowdown_; + set_state(itf_name, new_state); RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Got state %.5f for joint %u!", - hw_joint_states_[i], i); + rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Got state %.5f for joint %s!", + get_state(itf_name), itf_name.c_str()); } RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Joints successfully read!"); - for (uint i = 0; i < hw_sensor_states_.size(); i++) + int i = 0; + for (const auto & [sensor_itf_name, sensor_itf_desc] : sensor_state_interfaces_) { // Simulate RRBot's sensor data unsigned int seed = time(NULL) + i; - hw_sensor_states_[i] = - static_cast(rand_r(&seed)) / (static_cast(RAND_MAX / hw_sensor_change_)); + set_state( + sensor_itf_name, + static_cast(rand_r(&seed)) / (static_cast(RAND_MAX / hw_sensor_change_))); RCLCPP_INFO( rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Got value %e for interface %s!", - hw_sensor_states_[i], info_.sensors[0].state_interfaces[i].name.c_str()); + get_state(sensor_itf_name), sensor_itf_name.c_str()); + ++i; } RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Sensors successfully read!"); // END: This part here is for exemplary purposes - Please do not copy to your production code @@ -245,12 +238,12 @@ hardware_interface::return_type ros2_control_demo_example_4::RRBotSystemWithSens // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Writing...please wait..."); - for (uint i = 0; i < hw_joint_commands_.size(); i++) + for (const auto & [itf_name, itf_desc] : joint_command_interfaces_) { // Simulate sending commands to the hardware RCLCPP_INFO( rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Got command %.5f for joint %u!", - hw_joint_commands_[i], i); + get_command(itf_name), itf_name); } RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Joints successfully written!"); // END: This part here is for exemplary purposes - Please do not copy to your production code diff --git a/example_5/hardware/include/ros2_control_demo_example_5/rrbot.hpp b/example_5/hardware/include/ros2_control_demo_example_5/rrbot.hpp index c3d053464..448b61837 100644 --- a/example_5/hardware/include/ros2_control_demo_example_5/rrbot.hpp +++ b/example_5/hardware/include/ros2_control_demo_example_5/rrbot.hpp @@ -70,10 +70,6 @@ class RRBotSystemPositionOnlyHardware : public hardware_interface::SystemInterfa double hw_start_sec_; double hw_stop_sec_; double hw_slowdown_; - - // Store the command for the simulated robot - std::vector hw_commands_; - std::vector hw_states_; }; } // namespace ros2_control_demo_example_5 diff --git a/example_5/hardware/rrbot.cpp b/example_5/hardware/rrbot.cpp index 35fe1c0f2..4aeea73b8 100644 --- a/example_5/hardware/rrbot.cpp +++ b/example_5/hardware/rrbot.cpp @@ -40,9 +40,6 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_init( hw_stop_sec_ = stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]); hw_slowdown_ = stod(info_.hardware_parameters["example_param_hw_slowdown"]); // END: This part here is for exemplary purposes - Please do not copy to your production code - hw_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - for (const hardware_interface::ComponentInfo & joint : info_.joints) { // RRBotSystemPositionOnly has exactly one state and command interface on each joint @@ -83,6 +80,28 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_init( } } + // check if we have same command and state interfaces for joint this makes iterating easier + // first check if size is equal then we only need to iterate over one of them + if (joint_state_interfaces_.size() != joint_command_interfaces_.size()) + { + RCLCPP_FATAL( + rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), + "Expect joint CommandInterface and joint StateInterfaces to be of equal size."); + return hardware_interface::CallbackReturn::ERROR; + } + for (const auto & [state_itf_name, state_itf_descr] : joint_state_interfaces_) + { + if (joint_command_interfaces_.find(state_itf_name) == joint_command_interfaces_.end()) + { + RCLCPP_FATAL( + rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), + "Expect joint CommandInterface and joint StateInterfaces to be equal but StateInterface " + "includes<%s> which is not included in CommandInterfaces.", + state_itf_name.c_str()); + return hardware_interface::CallbackReturn::ERROR; + } + } + return hardware_interface::CallbackReturn::SUCCESS; } @@ -103,10 +122,11 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_configure // END: This part here is for exemplary purposes - Please do not copy to your production code // reset values always when configuring hardware - for (uint i = 0; i < hw_states_.size(); i++) + // we checked before that joint_state_itfs_ == joint_command_itfs_; + for (const auto & [itf_name, itf_desc] : joint_state_interfaces_) { - hw_states_[i] = 0; - hw_commands_[i] = 0; + set_state(itf_name, 0.0); + set_command(itf_name, 0.0); } RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Successfully configured!"); @@ -114,32 +134,6 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_configure return hardware_interface::CallbackReturn::SUCCESS; } -std::vector -RRBotSystemPositionOnlyHardware::export_state_interfaces() -{ - std::vector state_interfaces; - for (uint i = 0; i < info_.joints.size(); i++) - { - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_[i])); - } - - return state_interfaces; -} - -std::vector -RRBotSystemPositionOnlyHardware::export_command_interfaces() -{ - std::vector command_interfaces; - for (uint i = 0; i < info_.joints.size(); i++) - { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i])); - } - - return command_interfaces; -} - hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { @@ -157,9 +151,9 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_activate( // END: This part here is for exemplary purposes - Please do not copy to your production code // command and state should be equal when starting - for (uint i = 0; i < hw_states_.size(); i++) + for (const auto & [itf_name, itf_desc] : joint_state_interfaces_) { - hw_commands_[i] = hw_states_[i]; + set_command(itf_name, get_state(itf_name)); } RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Successfully activated!"); @@ -194,13 +188,15 @@ hardware_interface::return_type RRBotSystemPositionOnlyHardware::read( // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Reading..."); - for (uint i = 0; i < hw_states_.size(); i++) + for (const auto & [itf_name, itf_desc] : joint_state_interfaces_) { // Simulate RRBot's movement - hw_states_[i] = hw_states_[i] + (hw_commands_[i] - hw_states_[i]) / hw_slowdown_; + auto old_state = get_state(itf_name); + auto new_state = old_state + (get_command(itf_name) - old_state) / hw_slowdown_; + set_state(itf_name, new_state); RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Got state %.5f for joint %d!", - hw_states_[i], i); + rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Got state %.5f for joint %s!", + get_state(itf_name), itf_name.c_str()); } RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Joints successfully read!"); // END: This part here is for exemplary purposes - Please do not copy to your production code @@ -214,12 +210,12 @@ hardware_interface::return_type RRBotSystemPositionOnlyHardware::write( // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Writing..."); - for (uint i = 0; i < hw_commands_.size(); i++) + for (const auto & [itf_name, itf_desc] : joint_command_interfaces_) { // Simulate sending commands to the hardware RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Got command %.5f for joint %d!", - hw_commands_[i], i); + rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Got command %.5f for joint %u!", + get_command(itf_name), itf_name); } RCLCPP_INFO( rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Joints successfully written!"); diff --git a/example_6/hardware/rrbot_actuator.cpp b/example_6/hardware/rrbot_actuator.cpp index 2fe1fdf7c..2a02a618f 100644 --- a/example_6/hardware/rrbot_actuator.cpp +++ b/example_6/hardware/rrbot_actuator.cpp @@ -91,7 +91,7 @@ hardware_interface::CallbackReturn RRBotModularJoint::on_init( rclcpp::get_logger("RRBotModularJoint"), "Joint '%s' has state interface with name<%s> but not found in CommandInterfaces. Make " "sure the configuration exports same State- and CommandInterfaces", - joint.name.c_str(), joint.state_itf_name.c_str()); + joint.name.c_str(), state_itf_name.c_str()); return hardware_interface::CallbackReturn::ERROR; } } @@ -115,7 +115,7 @@ hardware_interface::CallbackReturn RRBotModularJoint::on_activate( // set some default values for joints for (const auto & [cmd_itf_name, cmd_itf_descr] : joint_command_interfaces_) { - if (!valid_command(cmd_itf_name) || std::isnan(get_command(cmd_itf_name))) + if (!command_holds_value(cmd_itf_name) || std::isnan(get_command(cmd_itf_name))) { set_command(cmd_itf_name, 0.0); } @@ -123,7 +123,7 @@ hardware_interface::CallbackReturn RRBotModularJoint::on_activate( for (const auto & [state_itf_name, state_itf_descr] : joint_state_interfaces_) { - if (!valid_state(state_itf_name) || std::isnan(get_state(state_itf_name))) + if (!command_holds_value(state_itf_name) || std::isnan(get_state(state_itf_name))) { set_state(state_itf_name, 0.0); } diff --git a/example_7/hardware/include/ros2_control_demo_example_7/r6bot_hardware.hpp b/example_7/hardware/include/ros2_control_demo_example_7/r6bot_hardware.hpp index 1c8aa7bdc..c609c419a 100644 --- a/example_7/hardware/include/ros2_control_demo_example_7/r6bot_hardware.hpp +++ b/example_7/hardware/include/ros2_control_demo_example_7/r6bot_hardware.hpp @@ -52,6 +52,10 @@ class RobotSystem : public hardware_interface::SystemInterface struct FTS_Sensor { explicit FTS_Sensor(const std::string & sensor_name) : name(sensor_name) {} + // delete move constructor since would throw because of const std::string members + // but we dont want to move this anyways so const for member is ok i guess + FTS_Sensor(FTS_Sensor && other) = delete; + const std::string name; const std::string force_x = "force.x"; const std::string force_y = "force.y";