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 11bc5e1eb..f3e67f79f 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 @@ -40,10 +40,6 @@ class RRBotSystemWithGPIOHardware : public hardware_interface::SystemInterface 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; @@ -58,12 +54,9 @@ 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_; + double hw_start_sec_; + double hw_stop_sec_; + double hw_slowdown_; }; } // namespace ros2_control_demo_example_10 diff --git a/example_10/hardware/rrbot.cpp b/example_10/hardware/rrbot.cpp index 9c0d2836b..4e76f1378 100644 --- a/example_10/hardware/rrbot.cpp +++ b/example_10/hardware/rrbot.cpp @@ -37,9 +37,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 @@ -123,67 +120,25 @@ 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(get_logger(), "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++) + for (const auto & [name, descr] : joint_state_interfaces_) { - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_[i])); + set_state(name, 0.0); } - - RCLCPP_INFO(get_logger(), "State interfaces:"); - hw_gpio_in_.resize(4); - size_t ct = 0; - for (size_t i = 0; i < info_.gpios.size(); i++) + for (const auto & [name, descr] : joint_command_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( - get_logger(), "Added %s/%s", info_.gpios.at(i).name.c_str(), state_if.name.c_str()); - } + set_command(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 & [name, descr] : gpio_state_interfaces_) { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i])); + set_state(name, 0.0); } - RCLCPP_INFO(get_logger(), "Command interfaces:"); - hw_gpio_out_.resize(2); - size_t ct = 0; - for (size_t i = 0; i < info_.gpios.size(); i++) + for (const auto & [name, descr] : 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( - get_logger(), "Added %s/%s", info_.gpios.at(i).name.c_str(), command_if.name.c_str()); - } + set_command(name, 0.0); } + RCLCPP_INFO(get_logger(), "Successfully configured!"); - return command_interfaces; + return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn RRBotSystemWithGPIOHardware::on_activate( @@ -194,9 +149,13 @@ 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 & [name, descr] : joint_state_interfaces_) + { + set_command(name, get_state(name)); + } + for (const auto & [name, descr] : gpio_command_interfaces_) { - hw_commands_[i] = hw_states_[i]; + set_command(name, get_state(name)); } RCLCPP_INFO(get_logger(), "Successfully activated!"); @@ -221,25 +180,33 @@ hardware_interface::return_type RRBotSystemWithGPIOHardware::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]); + auto new_value = get_state(name) + (get_command(name) - get_state(name)) / hw_slowdown_; + set_state(name, new_value); + } + + for (const auto & [name, descr] : gpio_command_interfaces_) + { + // mirror GPIOs back + set_state(name, get_command(name)); } - // mirror GPIOs back - hw_gpio_in_[0] = hw_gpio_out_[0]; - hw_gpio_in_[3] = hw_gpio_out_[1]; - // random inputs + // random inputs analog_input1 and analog_input2 unsigned int seed = time(NULL) + 1; - hw_gpio_in_[1] = static_cast(rand_r(&seed)); + set_state( + info_.gpios[0].name + "/" + info_.gpios[0].state_interfaces[1].name, + static_cast(rand_r(&seed))); seed = time(NULL) + 2; - hw_gpio_in_[2] = static_cast(rand_r(&seed)); + set_state( + info_.gpios[0].name + "/" + info_.gpios[0].state_interfaces[2].name, + static_cast(rand_r(&seed))); - for (uint i = 0; i < hw_gpio_in_.size(); i++) + for (const auto & [name, descr] : gpio_state_interfaces_) { ss << std::fixed << std::setprecision(2) << std::endl - << "\t" << hw_gpio_in_[i] << " from GPIO input '" << static_cast(i) << "'"; + << "\t" << get_state(name) << " from GPIO input '" << 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 @@ -254,10 +221,11 @@ hardware_interface::return_type RRBotSystemWithGPIOHardware::write( std::stringstream ss; ss << "Writing commands:"; - for (uint i = 0; i < hw_gpio_out_.size(); i++) + for (const auto & [name, descr] : gpio_command_interfaces_) { + // Simulate sending commands to the hardware ss << std::fixed << std::setprecision(2) << std::endl - << "\t" << hw_gpio_out_[i] << " for GPIO output '" << static_cast(i) << "'"; + << "\t" << get_command(name) << " for GPIO output '" << 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 diff --git a/example_11/hardware/carlikebot_system.cpp b/example_11/hardware/carlikebot_system.cpp index b8deca75c..a1100c0d0 100644 --- a/example_11/hardware/carlikebot_system.cpp +++ b/example_11/hardware/carlikebot_system.cpp @@ -56,6 +56,7 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init( // Steering joints have a position command interface and a position state interface if (joint_is_steering) { + steering_joint_ = joint.name; RCLCPP_INFO(get_logger(), "Joint '%s' is a steering joint.", joint.name.c_str()); if (joint.command_interfaces.size() != 1) @@ -93,6 +94,7 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init( else { RCLCPP_INFO(get_logger(), "Joint '%s' is a drive joint.", joint.name.c_str()); + traction_joint_ = joint.name; // Drive joints have a velocity command interface and a velocity state interface if (joint.command_interfaces.size() != 1) @@ -143,69 +145,33 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init( hw_stop_sec_ = std::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_interfaces_["steering"] = Joint("virtual_front_wheel_joint"); - - hw_interfaces_["traction"] = Joint("virtual_rear_wheel_joint"); - return hardware_interface::CallbackReturn::SUCCESS; } -std::vector CarlikeBotSystemHardware::export_state_interfaces() +hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) { - std::vector state_interfaces; + RCLCPP_INFO(get_logger(), "Configuring ...please wait..."); - for (auto & joint : hw_interfaces_) + for (auto i = 0; i < hw_start_sec_; i++) { - state_interfaces.emplace_back(hardware_interface::StateInterface( - joint.second.joint_name, hardware_interface::HW_IF_POSITION, &joint.second.state.position)); - - if (joint.first == "traction") - { - state_interfaces.emplace_back(hardware_interface::StateInterface( - joint.second.joint_name, hardware_interface::HW_IF_VELOCITY, &joint.second.state.velocity)); - } + rclcpp::sleep_for(std::chrono::seconds(1)); + RCLCPP_INFO(get_logger(), "%.1f seconds left...", hw_start_sec_ - i); } - RCLCPP_INFO(get_logger(), "Exported %zu state interfaces.", state_interfaces.size()); - - for (auto s : state_interfaces) + // reset values always when configuring hardware + for (const auto & [name, descr] : joint_state_interfaces_) { - RCLCPP_INFO(get_logger(), "Exported state interface '%s'.", s.get_name().c_str()); + set_state(name, 0.0); } - - return state_interfaces; -} - -std::vector -CarlikeBotSystemHardware::export_command_interfaces() -{ - std::vector command_interfaces; - - for (auto & joint : hw_interfaces_) + for (const auto & [name, descr] : joint_command_interfaces_) { - if (joint.first == "steering") - { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - joint.second.joint_name, hardware_interface::HW_IF_POSITION, - &joint.second.command.position)); - } - else if (joint.first == "traction") - { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - joint.second.joint_name, hardware_interface::HW_IF_VELOCITY, - &joint.second.command.velocity)); - } + set_command(name, 0.0); } - RCLCPP_INFO(get_logger(), "Exported %zu command interfaces.", command_interfaces.size()); - - for (auto i = 0u; i < command_interfaces.size(); i++) - { - RCLCPP_INFO( - get_logger(), "Exported command interface '%s'.", command_interfaces[i].get_name().c_str()); - } + RCLCPP_INFO(get_logger(), "Successfully configured!"); - return command_interfaces; + return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_activate( @@ -219,20 +185,10 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_activate( RCLCPP_INFO(get_logger(), "%.1f seconds left...", hw_start_sec_ - i); } - for (auto & joint : hw_interfaces_) + // command and state should be equal when starting + for (const auto & [name, descr] : joint_command_interfaces_) { - joint.second.state.position = 0.0; - - if (joint.first == "traction") - { - joint.second.state.velocity = 0.0; - joint.second.command.velocity = 0.0; - } - - else if (joint.first == "steering") - { - joint.second.command.position = 0.0; - } + set_command(name, get_state(name)); } RCLCPP_INFO(get_logger(), "Successfully activated!"); @@ -261,26 +217,32 @@ hardware_interface::return_type CarlikeBotSystemHardware::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 - - hw_interfaces_["steering"].state.position = hw_interfaces_["steering"].command.position; - - hw_interfaces_["traction"].state.velocity = hw_interfaces_["traction"].command.velocity; - hw_interfaces_["traction"].state.position += - hw_interfaces_["traction"].state.velocity * period.seconds(); + // update states from commands and integrate velocity to position + set_state( + steering_joint_ + "/" + hardware_interface::HW_IF_POSITION, + get_command(steering_joint_ + "/" + hardware_interface::HW_IF_POSITION)); + + set_state( + traction_joint_ + "/" + hardware_interface::HW_IF_VELOCITY, + get_command(traction_joint_ + "/" + hardware_interface::HW_IF_VELOCITY)); + set_state( + traction_joint_ + "/" + hardware_interface::HW_IF_POSITION, + get_state(traction_joint_ + "/" + hardware_interface::HW_IF_POSITION) + + get_command(traction_joint_ + "/" + hardware_interface::HW_IF_VELOCITY) * period.seconds()); std::stringstream ss; ss << "Reading states:"; ss << std::fixed << std::setprecision(2) << std::endl << "\t" - << "position: " << hw_interfaces_["steering"].state.position << " for joint '" - << hw_interfaces_["steering"].joint_name.c_str() << "'" << std::endl + << "position: " << get_state(steering_joint_ + "/" + hardware_interface::HW_IF_POSITION) + << " for joint '" << steering_joint_ << "'" << std::endl << "\t" - << "position: " << hw_interfaces_["traction"].state.position << " for joint '" - << hw_interfaces_["traction"].joint_name.c_str() << "'" << std::endl + << "position: " << get_state(traction_joint_ + "/" + hardware_interface::HW_IF_POSITION) + << " for joint '" << traction_joint_ << "'" << std::endl << "\t" - << "velocity: " << hw_interfaces_["traction"].state.velocity << " for joint '" - << hw_interfaces_["traction"].joint_name.c_str() << "'"; + << "velocity: " << get_state(traction_joint_ + "/" + hardware_interface::HW_IF_VELOCITY) + << " for joint '" << traction_joint_ << "'"; RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); @@ -298,11 +260,11 @@ hardware_interface::return_type ros2_control_demo_example_11 ::CarlikeBotSystemH ss << std::fixed << std::setprecision(2) << std::endl << "\t" - << "position: " << hw_interfaces_["steering"].command.position << " for joint '" - << hw_interfaces_["steering"].joint_name.c_str() << "'" << std::endl + << "position: " << get_command(steering_joint_ + "/" + hardware_interface::HW_IF_POSITION) + << " for joint '" << steering_joint_ << "'" << std::endl << "\t" - << "velocity: " << hw_interfaces_["traction"].command.velocity << " for joint '" - << hw_interfaces_["traction"].joint_name.c_str() << "'"; + << "velocity: " << get_command(traction_joint_ + "/" + hardware_interface::HW_IF_VELOCITY) + << " for joint '" << traction_joint_ << "'"; 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 diff --git a/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp b/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp index 44ab8a1f5..e92c40c40 100644 --- a/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp +++ b/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp @@ -63,9 +63,8 @@ class CarlikeBotSystemHardware : public hardware_interface::SystemInterface hardware_interface::CallbackReturn on_init( const hardware_interface::HardwareInfo & info) override; - std::vector export_state_interfaces() override; - - std::vector export_command_interfaces() override; + hardware_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; hardware_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; @@ -84,9 +83,9 @@ class CarlikeBotSystemHardware : public hardware_interface::SystemInterface double hw_start_sec_; double hw_stop_sec_; - // std::vector> - // hw_interfaces_; // name of joint, state, command - std::map hw_interfaces_; + // joint names + std::string steering_joint_; + std::string traction_joint_; }; } // namespace ros2_control_demo_example_11 diff --git a/example_12/hardware/include/ros2_control_demo_example_12/rrbot.hpp b/example_12/hardware/include/ros2_control_demo_example_12/rrbot.hpp index ac116044e..47064b8a7 100644 --- a/example_12/hardware/include/ros2_control_demo_example_12/rrbot.hpp +++ b/example_12/hardware/include/ros2_control_demo_example_12/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_12 diff --git a/example_12/hardware/rrbot.cpp b/example_12/hardware/rrbot.cpp index 53e20ab7c..a6cc4cf77 100644 --- a/example_12/hardware/rrbot.cpp +++ b/example_12/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 diff --git a/example_2/hardware/diffbot_system.cpp b/example_2/hardware/diffbot_system.cpp index 87a21f53a..28574f1f2 100644 --- a/example_2/hardware/diffbot_system.cpp +++ b/example_2/hardware/diffbot_system.cpp @@ -45,9 +45,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,30 +96,31 @@ hardware_interface::CallbackReturn DiffBotSystemHardware::on_init( return hardware_interface::CallbackReturn::SUCCESS; } -std::vector DiffBotSystemHardware::export_state_interfaces() +hardware_interface::CallbackReturn DiffBotSystemHardware::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) { - std::vector state_interfaces; - for (auto i = 0u; i < info_.joints.size(); i++) + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + RCLCPP_INFO(get_logger(), "Configuring ...please wait..."); + + for (int i = 0; i < hw_start_sec_; 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])); + rclcpp::sleep_for(std::chrono::seconds(1)); + RCLCPP_INFO(get_logger(), "%.1f seconds left...", hw_start_sec_ - i); } + // END: This part here is for exemplary purposes - Please do not copy to your production code - return state_interfaces; -} - -std::vector DiffBotSystemHardware::export_command_interfaces() -{ - std::vector command_interfaces; - for (auto i = 0u; i < info_.joints.size(); i++) + // reset values always when configuring hardware + for (const auto & [name, descr] : joint_state_interfaces_) { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_commands_[i])); + set_state(name, 0.0); } + for (const auto & [name, descr] : joint_command_interfaces_) + { + set_command(name, 0.0); + } + RCLCPP_INFO(get_logger(), "Successfully configured!"); - return command_interfaces; + return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn DiffBotSystemHardware::on_activate( @@ -138,15 +136,10 @@ 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++) + // command and state should be equal when starting + for (const auto & [name, descr] : joint_command_interfaces_) { - if (std::isnan(hw_positions_[i])) - { - hw_positions_[i] = 0; - hw_velocities_[i] = 0; - hw_commands_[i] = 0; - } + set_command(name, get_state(name)); } RCLCPP_INFO(get_logger(), "Successfully activated!"); @@ -178,18 +171,21 @@ hardware_interface::return_type DiffBotSystemHardware::read( // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code std::stringstream ss; ss << "Reading states:"; - for (std::size_t i = 0; i < hw_velocities_.size(); i++) + ss << std::fixed << std::setprecision(2); + for (const auto & [name, descr] : joint_state_interfaces_) { - // 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]; - - ss << std::fixed << std::setprecision(2) << std::endl - << "\t" - "position " - << hw_positions_[i] << " and velocity " << hw_velocities_[i] << " for '" - << info_.joints[i].name.c_str() << "'!"; + if (descr.get_interface_name() == hardware_interface::HW_IF_POSITION) + { + // Simulate DiffBot wheels's movement as a first-order system + // Update the joint status: this is a revolute joint without any limit. + // Simply integrates + auto velo = get_command(descr.get_prefix_name() + "/" + hardware_interface::HW_IF_VELOCITY); + set_state(name, get_state(name) + period.seconds() * velo); + + ss << std::endl + << "\t position " << get_state(name) << " and velocity " << velo << " for '" << 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 @@ -203,13 +199,13 @@ 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 std::stringstream ss; ss << "Writing commands:"; - for (auto i = 0u; i < hw_commands_.size(); i++) + for (const auto & [name, descr] : joint_command_interfaces_) { // Simulate sending commands to the hardware - hw_velocities_[i] = hw_commands_[i]; + set_state(name, get_command(name)); ss << std::fixed << std::setprecision(2) << std::endl - << "\t" << "command " << hw_commands_[i] << " for '" << info_.joints[i].name.c_str() << "'!"; + << "\t" << "command " << get_command(name) << " for '" << 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 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 1e383abd9..bf5bd9689 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 @@ -40,9 +40,8 @@ class DiffBotSystemHardware : public hardware_interface::SystemInterface hardware_interface::CallbackReturn on_init( const hardware_interface::HardwareInfo & info) override; - std::vector export_state_interfaces() override; - - std::vector export_command_interfaces() override; + hardware_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; hardware_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; @@ -60,11 +59,6 @@ class DiffBotSystemHardware : public hardware_interface::SystemInterface // Parameters for the DiffBot simulation 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_; }; } // namespace ros2_control_demo_example_2 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 7469de7c0..a952e077a 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 @@ -42,9 +42,8 @@ class RRBotSystemMultiInterfaceHardware : public hardware_interface::SystemInter hardware_interface::CallbackReturn on_init( const hardware_interface::HardwareInfo & info) override; - std::vector export_state_interfaces() override; - - std::vector export_command_interfaces() override; + hardware_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; hardware_interface::return_type prepare_command_mode_switch( const std::vector & start_interfaces, @@ -68,14 +67,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 diff --git a/example_3/hardware/rrbot_system_multi_interface.cpp b/example_3/hardware/rrbot_system_multi_interface.cpp index 0818056a4..213c17b30 100644 --- a/example_3/hardware/rrbot_system_multi_interface.cpp +++ b/example_3/hardware/rrbot_system_multi_interface.cpp @@ -43,12 +43,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) @@ -99,39 +93,31 @@ hardware_interface::CallbackReturn RRBotSystemMultiInterfaceHardware::on_init( return hardware_interface::CallbackReturn::SUCCESS; } -std::vector -RRBotSystemMultiInterfaceHardware::export_state_interfaces() +hardware_interface::CallbackReturn RRBotSystemMultiInterfaceHardware::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) { - std::vector state_interfaces; - for (std::size_t i = 0; i < info_.joints.size(); i++) + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + RCLCPP_INFO(get_logger(), "Configuring ...please wait..."); + + for (int i = 0; i < hw_start_sec_; i++) { - 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::sleep_for(std::chrono::seconds(1)); + RCLCPP_INFO(get_logger(), "%.1f seconds left...", hw_start_sec_ - i); } + // END: This part here is for exemplary purposes - Please do not copy to your production code - return state_interfaces; -} - -std::vector -RRBotSystemMultiInterfaceHardware::export_command_interfaces() -{ - std::vector command_interfaces; - for (std::size_t i = 0; i < info_.joints.size(); i++) + // reset values always when configuring hardware + for (const auto & [name, descr] : joint_state_interfaces_) { - 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])); + set_state(name, 0.0); } + for (const auto & [name, descr] : joint_command_interfaces_) + { + set_command(name, 0.0); + } + RCLCPP_INFO(get_logger(), "Successfully configured!"); - return command_interfaces; + return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::return_type RRBotSystemMultiInterfaceHardware::prepare_command_mode_switch( @@ -178,8 +164,11 @@ 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; + set_command( + info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION, + get_state(info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION)); + set_command(info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY, 0.0); + set_command(info_.joints[i].name + "/" + hardware_interface::HW_IF_ACCELERATION, 0.0); control_level_[i] = integration_level_t::UNDEFINED; // Revert to undefined } } @@ -211,32 +200,8 @@ 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++) + for (std::size_t i = 0; i < info_.joints.size(); i++) { - if (std::isnan(hw_states_positions_[i])) - { - hw_states_positions_[i] = 0; - } - if (std::isnan(hw_states_velocities_[i])) - { - hw_states_velocities_[i] = 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; } @@ -268,8 +233,11 @@ hardware_interface::return_type RRBotSystemMultiInterfaceHardware::read( // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code std::stringstream ss; ss << "Reading states:"; - for (std::size_t i = 0; i < hw_states_positions_.size(); i++) + for (std::size_t i = 0; i < info_.joints.size(); i++) { + const auto name_acc = info_.joints[i].name + "/" + hardware_interface::HW_IF_ACCELERATION; + const auto name_vel = info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY; + const auto name_pos = info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION; switch (control_level_[i]) { case integration_level_t::UNDEFINED: @@ -277,26 +245,30 @@ 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(name_acc, 0.); + set_state(name_vel, 0.); + set_state( + name_pos, + get_state(name_pos) + (get_command(name_pos) - get_state(name_pos)) / 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(name_acc, 0.); + set_state(name_vel, get_command(name_vel)); + set_state( + name_pos, get_state(name_pos) + get_state(name_vel) * 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(name_acc, get_command(name_acc)); + set_state( + name_vel, get_state(name_vel) + get_state(name_acc) * period.seconds() / hw_slowdown_); + set_state( + name_pos, get_state(name_pos) + get_state(name_vel) * period.seconds() / hw_slowdown_); break; } ss << std::fixed << std::setprecision(2) << std::endl << "\t" - << "pos: " << hw_states_positions_[i] << ", vel: " << hw_states_velocities_[i] - << ", acc: " << hw_states_accelerations_[i] << " for joint " << i; + << "pos: " << get_state(name_pos) << ", vel: " << get_state(name_vel) + << ", acc: " << get_state(name_acc) << " for joint " << i; } 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 @@ -309,13 +281,16 @@ hardware_interface::return_type RRBotSystemMultiInterfaceHardware::write( // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code std::stringstream ss; ss << "Writing commands:"; - for (std::size_t i = 0; i < hw_commands_positions_.size(); i++) + for (std::size_t i = 0; i < info_.joints.size(); i++) { // Simulate sending commands to the hardware + const auto name_acc = info_.joints[i].name + "/" + hardware_interface::HW_IF_ACCELERATION; + const auto name_vel = info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY; + const auto name_pos = info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION; ss << std::fixed << std::setprecision(2) << std::endl << "\t" - << "command pos: " << hw_commands_positions_[i] << ", vel: " << hw_commands_velocities_[i] - << ", acc: " << hw_commands_accelerations_[i] << " for joint " << i + << "command pos: " << get_command(name_pos) << ", vel: " << get_command(name_vel) + << ", acc: " << get_command(name_acc) << " for joint " << i << ", control lvl: " << static_cast(control_level_[i]); } RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); 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 e166c9906..f09fcf3c8 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 @@ -43,10 +43,6 @@ class RRBotSystemWithSensorHardware : public hardware_interface::SystemInterface 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; @@ -65,11 +61,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 751cbc512..f7df7eb83 100644 --- a/example_4/hardware/rrbot_system_with_sensor.cpp +++ b/example_4/hardware/rrbot_system_with_sensor.cpp @@ -48,11 +48,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 @@ -107,48 +102,21 @@ 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++) + for (const auto & [name, descr] : joint_state_interfaces_) { - hw_joint_states_[i] = 0; - hw_joint_commands_[i] = 0; + set_state(name, 0.0); } - - RCLCPP_INFO(get_logger(), "Successfully configured!"); - - return hardware_interface::CallbackReturn::SUCCESS; -} - -std::vector -RRBotSystemWithSensorHardware::export_state_interfaces() -{ - std::vector state_interfaces; - for (uint i = 0; i < info_.joints.size(); i++) + for (const auto & [name, descr] : sensor_state_interfaces_) { - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_joint_states_[i])); + set_state(name, 0.0); } - - // export sensor state interface - for (uint i = 0; i < info_.sensors[0].state_interfaces.size(); i++) + for (const auto & [name, descr] : joint_command_interfaces_) { - 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])); + set_command(name, 0.0); } + RCLCPP_INFO(get_logger(), "Successfully configured!"); - return command_interfaces; + return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn RRBotSystemWithSensorHardware::on_activate( @@ -165,17 +133,10 @@ 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++) - { - hw_joint_commands_[i] = hw_joint_states_[i]; - } - - // set default value for sensor - if (std::isnan(hw_sensor_states_[0])) + for (const auto & [name, descr] : joint_command_interfaces_) { - hw_sensor_states_[0] = 0; + set_command(name, get_state(name)); } - RCLCPP_INFO(get_logger(), "Successfully activated!"); return hardware_interface::CallbackReturn::SUCCESS; @@ -204,30 +165,28 @@ hardware_interface::return_type RRBotSystemWithSensorHardware::read( { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code std::stringstream ss; - ss << "Reading states from joints:"; + ss << "Reading states from joints:" << std::fixed << std::setprecision(2); - for (uint i = 0; i < hw_joint_states_.size(); i++) + for (const auto & [name, descr] : joint_state_interfaces_) { // Simulate RRBot's movement - hw_joint_states_[i] += (hw_joint_commands_[i] - hw_joint_states_[i]) / hw_slowdown_; - - ss << std::fixed << std::setprecision(2) << std::endl - << "\t" << hw_joint_states_[i] << " for joint '" << info_.joints[i].name.c_str() << "'"; + auto new_value = get_state(name) + (get_command(name) - get_state(name)) / hw_slowdown_; + set_state(name, new_value); + ss << std::endl << "\t" << get_state(name) << " for joint '" << name << "'"; } RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); ss.str(""); - ss << "Reading states from sensors:"; - for (uint i = 0; i < hw_sensor_states_.size(); i++) + ss << "Reading states from sensors:" << std::fixed << std::setprecision(2); + size_t i = 0; + for (const auto & [name, 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_)); + unsigned int seed = time(NULL) + i++; + set_state( + name, static_cast(rand_r(&seed)) / (static_cast(RAND_MAX / hw_sensor_change_))); - ss << std::fixed << std::setprecision(2) << std::endl - << "\t" << hw_sensor_states_[i] << " for sensor '" - << info_.sensors[0].state_interfaces[i].name.c_str() << "'"; + ss << std::endl << "\t" << get_state(name) << " for sensor '" << 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 @@ -241,12 +200,11 @@ 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 std::stringstream ss; ss << "Writing commands:"; - - for (uint i = 0; i < hw_joint_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_joint_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 diff --git a/example_5/hardware/external_rrbot_force_torque_sensor.cpp b/example_5/hardware/external_rrbot_force_torque_sensor.cpp index aadc299cd..11c76ad3f 100644 --- a/example_5/hardware/external_rrbot_force_torque_sensor.cpp +++ b/example_5/hardware/external_rrbot_force_torque_sensor.cpp @@ -47,27 +47,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*/) { @@ -109,18 +91,16 @@ hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::read( { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code std::stringstream ss; - ss << "Reading states:"; - - for (uint i = 0; i < hw_sensor_states_.size(); i++) + ss << "Reading states from sensors:" << std::fixed << std::setprecision(2); + size_t i = 0; + for (const auto & [name, 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_)); + unsigned int seed = time(NULL) + i++; + set_state( + name, static_cast(rand_r(&seed)) / (static_cast(RAND_MAX / hw_sensor_change_))); - ss << std::fixed << std::setprecision(2) << std::endl - << "\t" << hw_sensor_states_[i] << " for sensor '" - << info_.sensors[0].state_interfaces[i].name.c_str() << "'"; + ss << std::endl << "\t" << get_state(name) << " for sensor '" << 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 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 3f0a3a941..d64265445 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 @@ -39,8 +39,6 @@ class ExternalRRBotForceTorqueSensorHardware : public hardware_interface::Sensor hardware_interface::CallbackReturn on_init( const hardware_interface::HardwareInfo & info) override; - std::vector export_state_interfaces() override; - hardware_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; @@ -55,9 +53,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_5/hardware/include/ros2_control_demo_example_5/rrbot.hpp b/example_5/hardware/include/ros2_control_demo_example_5/rrbot.hpp index 3fdac8de1..b1961a1ff 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 @@ -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_5 diff --git a/example_5/hardware/rrbot.cpp b/example_5/hardware/rrbot.cpp index 6c395451b..eb2d645c3 100644 --- a/example_5/hardware/rrbot.cpp +++ b/example_5/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 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 4874bd47d..f650989d7 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 @@ -40,9 +40,8 @@ class RRBotModularJoint : public hardware_interface::ActuatorInterface hardware_interface::CallbackReturn on_init( const hardware_interface::HardwareInfo & info) override; - std::vector export_state_interfaces() override; - - std::vector export_command_interfaces() override; + hardware_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; hardware_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; @@ -61,10 +60,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 a1528b623..0df4c7781 100644 --- a/example_6/hardware/rrbot_actuator.cpp +++ b/example_6/hardware/rrbot_actuator.cpp @@ -47,9 +47,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) @@ -88,24 +85,31 @@ hardware_interface::CallbackReturn RRBotModularJoint::on_init( return hardware_interface::CallbackReturn::SUCCESS; } -std::vector RRBotModularJoint::export_state_interfaces() +hardware_interface::CallbackReturn RRBotModularJoint::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) { - 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; -} + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + RCLCPP_INFO(get_logger(), "Configuring ...please wait..."); -std::vector RRBotModularJoint::export_command_interfaces() -{ - std::vector command_interfaces; + for (int i = 0; i < hw_start_sec_; i++) + { + rclcpp::sleep_for(std::chrono::seconds(1)); + RCLCPP_INFO(get_logger(), "%.1f seconds left...", hw_start_sec_ - i); + } + // END: This part here is for exemplary purposes - Please do not copy to your production code - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[0].name, hardware_interface::HW_IF_POSITION, &hw_joint_command_)); + // reset values always when configuring hardware + for (const auto & [name, descr] : joint_state_interfaces_) + { + set_state(name, 0.0); + } + for (const auto & [name, descr] : joint_command_interfaces_) + { + set_command(name, 0.0); + } + RCLCPP_INFO(get_logger(), "Successfully configured!"); - return command_interfaces; + return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn RRBotModularJoint::on_activate( @@ -121,11 +125,10 @@ 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_)) + // command and state should be equal when starting + for (const auto & [name, descr] : joint_state_interfaces_) { - hw_joint_state_ = 0; - hw_joint_command_ = 0; + set_command(name, get_state(name)); } RCLCPP_INFO(get_logger(), "Successfully activated!"); @@ -159,10 +162,16 @@ hardware_interface::return_type RRBotModularJoint::read( ss << "Reading states:"; // Simulate RRBot's movement - hw_joint_state_ = hw_joint_state_ + (hw_joint_command_ - hw_joint_state_) / hw_slowdown_; + ss << "Reading states:"; - ss << std::fixed << std::setprecision(2) << std::endl - << "\t" << hw_joint_state_ << " for joint '" << info_.joints[0].name.c_str() << "'"; + for (const auto & [name, descr] : joint_state_interfaces_) + { + // Simulate RRBot's movement + 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" << get_state(name) << " for joint '" << name << "'"; + } RCLCPP_INFO(get_logger(), "%s", ss.str().c_str()); // END: This part here is for exemplary purposes - Please do not copy to your production code @@ -177,10 +186,12 @@ hardware_interface::return_type ros2_control_demo_example_6::RRBotModularJoint:: std::stringstream ss; ss << "Writing commands:"; - // Simulate sending commands to the hardware - ss << std::fixed << std::setprecision(2) << std::endl - << "\t" << hw_joint_command_ << " for joint '" << info_.joints[0].name.c_str() << "'"; - + for (const auto & [name, descr] : joint_command_interfaces_) + { + // Simulate sending commands to the hardware + ss << std::fixed << std::setprecision(2) << std::endl + << "\t" << get_command(name) << " for joint '" << name << "'"; + } RCLCPP_INFO(get_logger(), "%s", ss.str().c_str()); // END: This part here is for exemplary purposes - Please do not copy to your production code diff --git a/example_7/doc/userdoc.rst b/example_7/doc/userdoc.rst index 50402591d..c2a4e6b58 100644 --- a/example_7/doc/userdoc.rst +++ b/example_7/doc/userdoc.rst @@ -193,13 +193,14 @@ In ros2_control, hardware system components are integrated via user defined driv The following code blocks will explain the requirements for writing a new hardware interface. -The hardware plugin for the tutorial robot is a class called ``RobotSystem`` that inherits from ``hardware_interface::SystemInterface``. The ``SystemInterface`` is one of the offered hardware interfaces designed for a complete robot system. For example, The UR5 uses this interface. The ``RobotSystem`` must implement five public methods. +The hardware plugin for the tutorial robot is a class called ``RobotSystem`` that inherits from ``hardware_interface::SystemInterface``. The ``SystemInterface`` is one of the offered hardware interfaces designed for a complete robot system. For example, The UR5 uses this interface. The ``RobotSystem`` must implement the following public methods. 1. ``on_init`` -2. ``export_state_interfaces`` -3. ``export_command_interfaces`` -4. ``read`` -5. ``write`` +2. ``on_configure`` +3. ``read`` +4. ``write`` + +There are more methods that can be implemented for lifecycle changes, but they are not required for the tutorial robot. .. code-block:: c++ @@ -209,15 +210,14 @@ The hardware plugin for the tutorial robot is a class called ``RobotSystem`` tha class HARDWARE_INTERFACE_PUBLIC RobotSystem : public hardware_interface::SystemInterface { public: CallbackReturn on_init(const hardware_interface::HardwareInfo &info) override; - std::vector export_state_interfaces() override; - std::vector export_command_interfaces() override; + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) 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; // private members // ... } -The ``on_init`` method is called once during ros2_control initialization if the ``RobotSystem`` was specified in the URDF. In this method, communication between the robot hardware needs to be setup and memory dynamic should be allocated. Since the tutorial robot is simulated, explicit communication will not be established. Instead, vectors will be initialized that represent the state all the hardware, e.g. a vector of doubles describing joint angles, etc. +The ``on_init`` method is called once during ros2_control initialization if the ``RobotSystem`` was specified in the URDF. In this method, communication between the robot hardware needs to be setup and memory dynamic should be allocated. Since the tutorial robot is simulated, explicit communication will not be established. .. code-block:: c++ @@ -232,26 +232,15 @@ The ``on_init`` method is called once during ros2_control initialization if the Notably, the behavior of ``on_init`` is expected to vary depending on the URDF file. The ``SystemInterface::on_init(info)`` call fills out the ``info`` object with specifics from the URDF. For example, the ``info`` object has fields for joints, sensors, gpios, and more. Suppose the sensor field has a name value of ``tcp_force_torque_sensor``. Then the ``on_init`` must try to establish communication with that sensor. If it fails, then an error value is returned. -Next, ``export_state_interfaces`` and ``export_command_interfaces`` methods are called in succession. The ``export_state_interfaces`` method returns a vector of ``StateInterface``, describing the ``state_interfaces`` for each joint. The ``StateInterface`` objects are read only data handles. Their constructors require an interface name, interface type, and a pointer to a double data value. For the ``RobotSystem``, the data pointers reference class member variables. This way, the data can be access from every method. - -.. code-block:: c++ - - std::vector RobotSystem::export_state_interfaces() { - std::vector state_interfaces; - // add state interfaces to ``state_interfaces`` for each joint, e.g. `info_.joints[0].state_interfaces_`, `info_.joints[1].state_interfaces_`, `info_.joints[2].state_interfaces_` ... - // ... - return state_interfaces; - } - -The ``export_command_interfaces`` method is nearly identical to the previous one. The difference is that a vector of ``CommandInterface`` is returned. The vector contains objects describing the ``command_interfaces`` for each joint. +The ``on_configure`` method is called once during ros2_control lifecycle of the ``RobotSystem``. Initial values are set for state and command interfaces that represent the state all the hardware, e.g. doubles describing joint angles, etc. .. code-block:: c++ - std::vector RobotSystem::export_command_interfaces() { - std::vector command_interfaces; - // add command interfaces to ``command_interfaces`` for each joint, e.g. `info_.joints[0].command_interfaces_`, `info_.joints[1].command_interfaces_`, `info_.joints[2].command_interfaces_` ... + CallbackReturn RobotSystem::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) + // setup communication with robot hardware // ... - return command_interfaces; + return CallbackReturn::SUCCESS; } The ``read`` method is core method in the ros2_control loop. During the main loop, ros2_control loops over all hardware components and calls the ``read`` method. It is executed on the realtime thread, hence the method must obey by realtime constraints. The ``read`` method is responsible for updating the data values of the ``state_interfaces``. Since the data value point to class member variables, those values can be filled with their corresponding sensor values, which will in turn update the values of each exported ``StateInterface`` object. 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..69da3d74f 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 @@ -36,25 +36,13 @@ class RobotSystem : public hardware_interface::SystemInterface public: CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; - std::vector export_state_interfaces() override; - - std::vector export_command_interfaces() override; + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) 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", {}}}; }; } // namespace ros2_control_demo_example_7 diff --git a/example_7/hardware/r6bot_hardware.cpp b/example_7/hardware/r6bot_hardware.cpp index 7414b8fcf..4f710eb64 100644 --- a/example_7/hardware/r6bot_hardware.cpp +++ b/example_7/hardware/r6bot_hardware.cpp @@ -16,6 +16,8 @@ #include #include +#include + namespace ros2_control_demo_example_7 { CallbackReturn RobotSystem::on_init(const hardware_interface::HardwareInfo & info) @@ -24,95 +26,39 @@ CallbackReturn RobotSystem::on_init(const hardware_interface::HardwareInfo & inf { return CallbackReturn::ERROR; } - - // 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 & interface : joint.state_interfaces) - { - joint_interfaces[interface.name].push_back(joint.name); - } - } - - return CallbackReturn::SUCCESS; + return hardware_interface::CallbackReturn::SUCCESS; } -std::vector RobotSystem::export_state_interfaces() +CallbackReturn RobotSystem::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { - std::vector state_interfaces; - - int ind = 0; - for (const auto & joint_name : joint_interfaces["position"]) + // reset values always when configuring hardware + for (const auto & [name, descr] : joint_state_interfaces_) { - state_interfaces.emplace_back(joint_name, "position", &joint_position_[ind++]); + set_state(name, 0.0); } - - ind = 0; - for (const auto & joint_name : joint_interfaces["velocity"]) + for (const auto & [name, descr] : joint_command_interfaces_) { - state_interfaces.emplace_back(joint_name, "velocity", &joint_velocities_[ind++]); + set_command(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]); - - return state_interfaces; -} - -std::vector RobotSystem::export_command_interfaces() -{ - 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"]) + for (const auto & [name, descr] : sensor_state_interfaces_) { - command_interfaces.emplace_back(joint_name, "velocity", &joint_velocities_command_[ind++]); + set_state(name, 0.0); } - 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; + return CallbackReturn::SUCCESS; } 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++) - { - joint_velocities_[i] = joint_velocities_command_[i]; - joint_position_[i] += joint_velocities_command_[i] * period.seconds(); - } - - for (auto i = 0ul; i < joint_position_command_.size(); i++) + for (std::size_t i = 0; i < info_.joints.size(); i++) { - joint_position_[i] = joint_position_command_[i]; + const auto name_vel = info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY; + const auto name_pos = info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION; + set_state(name_vel, get_command(name_vel)); + set_state(name_pos, get_state(name_pos) + get_state(name_vel) * period.seconds()); } - return return_type::OK; } diff --git a/example_7/reference_generator/send_trajectory.cpp b/example_7/reference_generator/send_trajectory.cpp index e6d3fd787..3de586d1a 100644 --- a/example_7/reference_generator/send_trajectory.cpp +++ b/example_7/reference_generator/send_trajectory.cpp @@ -96,6 +96,15 @@ int main(int argc, char ** argv) trajectory_msg.points.push_back(trajectory_point_msg); } + // send zero velocities in the end + std::fill(trajectory_point_msg.velocities.begin(), trajectory_point_msg.velocities.end(), 0.0); + trajectory_point_msg.time_from_start.sec = trajectory_len / loop_rate; + trajectory_point_msg.time_from_start.nanosec = static_cast( + 1E9 / loop_rate * + static_cast( + trajectory_len - loop_rate * (trajectory_len / loop_rate))); // implicit integer division + trajectory_msg.points.push_back(trajectory_point_msg); + pub->publish(trajectory_msg); while (rclcpp::ok()) { 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 fc01cb2b8..a5a8d2d8e 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 @@ -41,10 +41,6 @@ class RRBotTransmissionsSystemPositionOnlyHardware : public hardware_interface:: 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; diff --git a/example_8/hardware/rrbot_transmissions_system_position_only.cpp b/example_8/hardware/rrbot_transmissions_system_position_only.cpp index 594ec13f8..37ca0084e 100644 --- a/example_8/hardware/rrbot_transmissions_system_position_only.cpp +++ b/example_8/hardware/rrbot_transmissions_system_position_only.cpp @@ -164,49 +164,32 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: reset_interfaces(joint_interfaces_); reset_interfaces(actuator_interfaces_); - RCLCPP_INFO(get_logger(), "Configuration successful"); - - return hardware_interface::CallbackReturn::SUCCESS; -} - -std::vector -RRBotTransmissionsSystemPositionOnlyHardware::export_state_interfaces() -{ - std::vector state_interfaces; - for (const auto & joint : info_.joints) + // reset values always when configuring hardware + for (const auto & [name, descr] : joint_state_interfaces_) { - /// @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_)); + set_state(name, 0.0); } - return state_interfaces; -} - -std::vector -RRBotTransmissionsSystemPositionOnlyHardware::export_command_interfaces() -{ - std::vector command_interfaces; - for (const auto & joint : info_.joints) + for (const auto & [name, descr] : joint_command_interfaces_) { - /// @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; }); - - command_interfaces.emplace_back(hardware_interface::CommandInterface( - joint.name, hardware_interface::HW_IF_POSITION, &joint_interface->command_)); + set_command(name, 0.0); } - return command_interfaces; + + RCLCPP_INFO(get_logger(), "Configuration successful"); + + return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { RCLCPP_INFO(get_logger(), "Activating..."); + + // command and state should be equal when starting + for (const auto & [name, descr] : joint_state_interfaces_) + { + set_command(name, get_state(name)); + } + RCLCPP_INFO(get_logger(), "Activation successful"); return hardware_interface::CallbackReturn::SUCCESS; @@ -263,12 +246,30 @@ hardware_interface::return_type RRBotTransmissionsSystemPositionOnlyHardware::re } RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); + // update internal storage from resource_manager + std::for_each( + joint_interfaces_.begin(), joint_interfaces_.end(), + [this](auto & joint_interface) + { + set_state( + joint_interface.name_ + "/" + hardware_interface::HW_IF_POSITION, joint_interface.state_); + }); + return hardware_interface::return_type::OK; } hardware_interface::return_type RRBotTransmissionsSystemPositionOnlyHardware::write( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { + // update internal storage from resource_manager + std::for_each( + joint_interfaces_.begin(), joint_interfaces_.end(), + [this](auto & joint_interface) + { + joint_interface.command_ = + get_command(joint_interface.name_ + "/" + hardware_interface::HW_IF_POSITION); + }); + // joint: command -> transmission std::for_each( joint_interfaces_.begin(), joint_interfaces_.end(), [](auto & joint_interface) 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