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 b55a821a6..c5fa52217 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 @@ -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_1 diff --git a/example_1/hardware/rrbot.cpp b/example_1/hardware/rrbot.cpp index 40b110be2..4e7664649 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 (const auto & [name, descr] : joint_state_interfaces_) { - hw_states_[i] = 0; - hw_commands_[i] = 0; + set_state(name, 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 (const auto & [name, descr] : joint_command_interfaces_) { - 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])); + set_command(name, 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 (const auto & [name, descr] : joint_state_interfaces_) { - hw_commands_[i] = hw_states_[i]; + set_command(name, get_state(name)); } 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 (const auto & [name, descr] : joint_state_interfaces_) { + auto new_value = get_state(name) + (get_command(name) - get_state(name)) / hw_slowdown_; + set_state(name, new_value); // Simulate RRBot's movement - hw_states_[i] = hw_states_[i] + (hw_commands_[i] - hw_states_[i]) / hw_slowdown_; - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Got state %.5f for joint %d!", - hw_states_[i], i); + RCLCPP_INFO_STREAM( + rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), + "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 @@ -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 (const auto & [name, descr] : 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_INFO_STREAM( + rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), + "Got command" << get_command(name) << " for joint: " << name << "!"); } RCLCPP_INFO( rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Joints successfully written!");