From 24dd3a124e577ef7373f62866653f7121175138f Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 3 Nov 2024 12:29:13 +0000 Subject: [PATCH] Ex9 --- .../ros2_control_demo_example_9/rrbot.hpp | 8 --- example_9/hardware/rrbot.cpp | 54 +++++-------------- 2 files changed, 14 insertions(+), 48 deletions(-) diff --git a/example_9/hardware/include/ros2_control_demo_example_9/rrbot.hpp b/example_9/hardware/include/ros2_control_demo_example_9/rrbot.hpp index f9b3ea82d..7643e7a87 100644 --- a/example_9/hardware/include/ros2_control_demo_example_9/rrbot.hpp +++ b/example_9/hardware/include/ros2_control_demo_example_9/rrbot.hpp @@ -40,10 +40,6 @@ class RRBotSystemPositionOnlyHardware : public hardware_interface::SystemInterfa hardware_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; - std::vector export_state_interfaces() override; - - std::vector export_command_interfaces() override; - hardware_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; @@ -61,10 +57,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_9 diff --git a/example_9/hardware/rrbot.cpp b/example_9/hardware/rrbot.cpp index ba1dc6cec..7e136dc91 100644 --- a/example_9/hardware/rrbot.cpp +++ b/example_9/hardware/rrbot.cpp @@ -43,8 +43,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) { @@ -100,41 +98,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 (const auto & [name, descr] : joint_state_interfaces_) { - hw_states_[i] = 0; - hw_commands_[i] = 0; + set_state(name, 0.0); } - - RCLCPP_INFO(get_logger(), "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++) - { - 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++) + for (const auto & [name, descr] : joint_command_interfaces_) { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i])); + set_command(name, 0.0); } + RCLCPP_INFO(get_logger(), "Successfully configured!"); - return command_interfaces; + return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_activate( @@ -151,9 +125,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 & [name, descr] : joint_state_interfaces_) { - hw_commands_[i] = hw_states_[i]; + set_command(name, get_state(name)); } RCLCPP_INFO(get_logger(), "Successfully activated!"); @@ -186,13 +160,13 @@ hardware_interface::return_type RRBotSystemPositionOnlyHardware::read( std::stringstream ss; ss << "Reading states:"; - for (uint i = 0; i < hw_states_.size(); i++) + for (const auto & [name, descr] : joint_state_interfaces_) { // Simulate RRBot's movement - hw_states_[i] = hw_states_[i] + (hw_commands_[i] - hw_states_[i]) / hw_slowdown_; - + auto new_value = get_state(name) + (get_command(name) - get_state(name)) / hw_slowdown_; + set_state(name, new_value); ss << std::fixed << std::setprecision(2) << std::endl - << "\t" << hw_states_[i] << " for joint '" << info_.joints[i].name.c_str() << "'"; + << "\t" << get_state(name) << " for joint '" << name << "'"; } RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); // END: This part here is for exemplary purposes - Please do not copy to your production code @@ -207,11 +181,11 @@ hardware_interface::return_type RRBotSystemPositionOnlyHardware::write( std::stringstream ss; ss << "Writing commands:"; - for (uint i = 0; i < hw_commands_.size(); i++) + for (const auto & [name, descr] : joint_command_interfaces_) { // Simulate sending commands to the hardware ss << std::fixed << std::setprecision(2) << std::endl - << "\t" << hw_commands_[i] << " for joint '" << info_.joints[i].name.c_str() << "'"; + << "\t" << get_command(name) << " for joint '" << name << "'"; } RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); // END: This part here is for exemplary purposes - Please do not copy to your production code