From a8ca44b7f480d73ae9103ad96693ae97ab18201f Mon Sep 17 00:00:00 2001 From: Manuel M Date: Thu, 7 Dec 2023 09:42:11 +0100 Subject: [PATCH 1/5] adjust example 1 with new export of interfaces --- .../ros2_control_demo_example_1/rrbot.hpp | 10 ---- example_1/hardware/rrbot.cpp | 53 +++++-------------- 2 files changed, 14 insertions(+), 49 deletions(-) diff --git a/example_1/hardware/include/ros2_control_demo_example_1/rrbot.hpp b/example_1/hardware/include/ros2_control_demo_example_1/rrbot.hpp index 46881e85a..e4f34e883 100644 --- a/example_1/hardware/include/ros2_control_demo_example_1/rrbot.hpp +++ b/example_1/hardware/include/ros2_control_demo_example_1/rrbot.hpp @@ -43,12 +43,6 @@ class RRBotSystemPositionOnlyHardware : public hardware_interface::SystemInterfa hardware_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; - ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIC - std::vector export_state_interfaces() override; - - ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIC - std::vector export_command_interfaces() override; - ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIC hardware_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; @@ -70,10 +64,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_1 diff --git a/example_1/hardware/rrbot.cpp b/example_1/hardware/rrbot.cpp index 40b110be2..c83af26b3 100644 --- a/example_1/hardware/rrbot.cpp +++ b/example_1/hardware/rrbot.cpp @@ -40,8 +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) { @@ -103,41 +101,17 @@ 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++) + for (uint i = 0; i < joint_pos_states_.size(); i++) { - hw_states_[i] = 0; - hw_commands_[i] = 0; + joint_pos_states_[i] = 0.0; } - - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Successfully configured!"); - - return hardware_interface::CallbackReturn::SUCCESS; -} - -std::vector -RRBotSystemPositionOnlyHardware::export_state_interfaces() -{ - std::vector state_interfaces; - for (uint i = 0; i < info_.joints.size(); i++) + for (uint i = 0; i < joint_pos_commands_.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])); + joint_pos_commands_[i] = 0.0; } + RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Successfully configured!"); - return command_interfaces; + return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_activate( @@ -157,9 +131,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 (uint i = 0; i < joint_pos_states_.size(); i++) { - hw_commands_[i] = hw_states_[i]; + joint_pos_commands_[i] = joint_pos_states_[i]; } RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Successfully activated!"); @@ -194,13 +168,14 @@ 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 (uint i = 0; i < joint_pos_states_.size(); i++) { // Simulate RRBot's movement - hw_states_[i] = hw_states_[i] + (hw_commands_[i] - hw_states_[i]) / hw_slowdown_; + joint_pos_states_[i] = + joint_pos_states_[i] + (joint_pos_commands_[i] - joint_pos_states_[i]) / hw_slowdown_; RCLCPP_INFO( rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Got state %.5f for joint %d!", - hw_states_[i], i); + joint_pos_states_[i], i); } 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 +189,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 (uint i = 0; i < joint_pos_commands_.size(); i++) { // Simulate sending commands to the hardware RCLCPP_INFO( rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Got command %.5f for joint %d!", - hw_commands_[i], i); + joint_pos_commands_[i], i); } RCLCPP_INFO( rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Joints successfully written!"); From e81d85fc85ffb6abd9ea1779e54bd0ee78fbfdea Mon Sep 17 00:00:00 2001 From: Manuel M Date: Fri, 8 Dec 2023 10:47:50 +0100 Subject: [PATCH 2/5] adapt to changes --- example_1/hardware/rrbot.cpp | 33 +++++++++++++++++---------------- 1 file changed, 17 insertions(+), 16 deletions(-) diff --git a/example_1/hardware/rrbot.cpp b/example_1/hardware/rrbot.cpp index c83af26b3..6081a5a1e 100644 --- a/example_1/hardware/rrbot.cpp +++ b/example_1/hardware/rrbot.cpp @@ -101,13 +101,13 @@ 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 < joint_pos_states_.size(); i++) + for (const auto & descr : joint_states_descr_) { - joint_pos_states_[i] = 0.0; + joint_state_set_value(descr, 0.0); } - for (uint i = 0; i < joint_pos_commands_.size(); i++) + for (const auto & descr : joint_commands_descr_) { - joint_pos_commands_[i] = 0.0; + joint_command_set_value(descr, 0.0); } RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Successfully configured!"); @@ -131,9 +131,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 < joint_pos_states_.size(); i++) + for (const auto & descr : joint_states_descr_) { - joint_pos_commands_[i] = joint_pos_states_[i]; + joint_command_set_value(descr, joint_state_get_value(descr)); } RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Successfully activated!"); @@ -168,14 +168,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 < joint_pos_states_.size(); i++) + for (const auto & descr : joint_states_descr_) { + auto new_value = joint_state_get_value(descr) + + (joint_command_get_value(descr) - joint_state_get_value(descr)) / hw_slowdown_; + joint_state_set_value(descr, new_value); // Simulate RRBot's movement - joint_pos_states_[i] = - joint_pos_states_[i] + (joint_pos_commands_[i] - joint_pos_states_[i]) / hw_slowdown_; - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Got state %.5f for joint %d!", - joint_pos_states_[i], i); + RCLCPP_INFO_STREAM( + rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), + "Got state " << joint_state_get_value(descr) << " for joint: " << descr.get_name() << "!"); } 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 @@ -189,12 +190,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 < joint_pos_commands_.size(); i++) + for (const auto & descr : joint_commands_descr_) { // Simulate sending commands to the hardware - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Got command %.5f for joint %d!", - joint_pos_commands_[i], i); + RCLCPP_INFO_STREAM( + rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), + "Got command" << joint_command_get_value(descr) << " for joint: " << descr.get_name() << "!"); } RCLCPP_INFO( rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Joints successfully written!"); From e66b7a8c45f4f6e4581c3edf9014a05f4363d217 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Tue, 12 Dec 2023 13:40:13 +0100 Subject: [PATCH 3/5] adjust to descriptions --- example_1/hardware/rrbot.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/example_1/hardware/rrbot.cpp b/example_1/hardware/rrbot.cpp index 6081a5a1e..4a2cc823f 100644 --- a/example_1/hardware/rrbot.cpp +++ b/example_1/hardware/rrbot.cpp @@ -101,11 +101,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 (const auto & descr : joint_states_descr_) + for (const auto & descr : joint_states_descriptions_) { joint_state_set_value(descr, 0.0); } - for (const auto & descr : joint_commands_descr_) + for (const auto & descr : joint_commands_descriptions_) { joint_command_set_value(descr, 0.0); } @@ -131,7 +131,7 @@ 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 (const auto & descr : joint_states_descr_) + for (const auto & descr : joint_states_descriptions_) { joint_command_set_value(descr, joint_state_get_value(descr)); } @@ -168,7 +168,7 @@ 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 (const auto & descr : joint_states_descr_) + for (const auto & descr : joint_states_descriptions_) { auto new_value = joint_state_get_value(descr) + (joint_command_get_value(descr) - joint_state_get_value(descr)) / hw_slowdown_; @@ -190,7 +190,7 @@ 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 (const auto & descr : joint_commands_descr_) + for (const auto & descr : joint_commands_descriptions_) { // Simulate sending commands to the hardware RCLCPP_INFO_STREAM( From 714f0f00e616f0615fd6d9cfef974e7bb9a19314 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Tue, 19 Dec 2023 08:38:20 +0100 Subject: [PATCH 4/5] adapt example 1 to stored handle-names/descriptions in map --- example_1/hardware/rrbot.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/example_1/hardware/rrbot.cpp b/example_1/hardware/rrbot.cpp index 4a2cc823f..2d7de0504 100644 --- a/example_1/hardware/rrbot.cpp +++ b/example_1/hardware/rrbot.cpp @@ -101,11 +101,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 (const auto & descr : joint_states_descriptions_) + for (const auto & [name, descr] : joint_state_interfaces_) { joint_state_set_value(descr, 0.0); } - for (const auto & descr : joint_commands_descriptions_) + for (const auto & [name, descr] : joint_command_interfaces_) { joint_command_set_value(descr, 0.0); } @@ -131,7 +131,7 @@ 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 (const auto & descr : joint_states_descriptions_) + for (const auto & [name, descr] : joint_state_interfaces_) { joint_command_set_value(descr, joint_state_get_value(descr)); } @@ -168,7 +168,7 @@ 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 (const auto & descr : joint_states_descriptions_) + for (const auto & [name, descr] : joint_state_interfaces_) { auto new_value = joint_state_get_value(descr) + (joint_command_get_value(descr) - joint_state_get_value(descr)) / hw_slowdown_; @@ -190,7 +190,7 @@ 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 (const auto & descr : joint_commands_descriptions_) + for (const auto & [name, descr] : joint_command_interfaces_) { // Simulate sending commands to the hardware RCLCPP_INFO_STREAM( From abfc30063211bb5979f16da8cbb633b03b8af898 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 20 Dec 2023 14:19:21 +0100 Subject: [PATCH 5/5] adjust to usage of get_/set_state or command functions --- example_1/hardware/rrbot.cpp | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/example_1/hardware/rrbot.cpp b/example_1/hardware/rrbot.cpp index 2d7de0504..4e7664649 100644 --- a/example_1/hardware/rrbot.cpp +++ b/example_1/hardware/rrbot.cpp @@ -103,11 +103,11 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_configure // reset values always when configuring hardware for (const auto & [name, descr] : joint_state_interfaces_) { - joint_state_set_value(descr, 0.0); + set_state(name, 0.0); } for (const auto & [name, descr] : joint_command_interfaces_) { - joint_command_set_value(descr, 0.0); + set_command(name, 0.0); } RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Successfully configured!"); @@ -133,7 +133,7 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_activate( // command and state should be equal when starting for (const auto & [name, descr] : joint_state_interfaces_) { - joint_command_set_value(descr, joint_state_get_value(descr)); + set_command(name, get_state(name)); } RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Successfully activated!"); @@ -170,13 +170,12 @@ hardware_interface::return_type RRBotSystemPositionOnlyHardware::read( for (const auto & [name, descr] : joint_state_interfaces_) { - auto new_value = joint_state_get_value(descr) + - (joint_command_get_value(descr) - joint_state_get_value(descr)) / hw_slowdown_; - joint_state_set_value(descr, new_value); + auto new_value = get_state(name) + (get_command(name) - get_state(name)) / hw_slowdown_; + set_state(name, new_value); // Simulate RRBot's movement RCLCPP_INFO_STREAM( rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), - "Got state " << joint_state_get_value(descr) << " for joint: " << descr.get_name() << "!"); + "Got state " << get_state(name) << " for joint: " << name << "!"); } 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 @@ -195,7 +194,7 @@ hardware_interface::return_type RRBotSystemPositionOnlyHardware::write( // Simulate sending commands to the hardware RCLCPP_INFO_STREAM( rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), - "Got command" << joint_command_get_value(descr) << " for joint: " << descr.get_name() << "!"); + "Got command" << get_command(name) << " for joint: " << name << "!"); } RCLCPP_INFO( rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Joints successfully written!");