From ad7928cdc5270fe6ac810411b4345bb93f0d43e0 Mon Sep 17 00:00:00 2001 From: Noel Jimenez Date: Wed, 18 Jan 2023 16:51:25 +0000 Subject: [PATCH 01/27] add copy for transmission demo and set in the urdf --- ...sions_rrbot_system_position_only.launch.py | 89 +++++++++++++++++++ ...ot_system_position_only.ros2_control.xacro | 71 +++++++++++++++ ...ions_rrbot_system_position_only.urdf.xacro | 46 ++++++++++ 3 files changed, 206 insertions(+) create mode 100644 ros2_control_demo_bringup/launch/transmissions_rrbot_system_position_only.launch.py create mode 100644 ros2_control_demo_description/rrbot_description/ros2_control/transmissions_rrbot_system_position_only.ros2_control.xacro create mode 100644 ros2_control_demo_description/rrbot_description/urdf/transmissions_rrbot_system_position_only.urdf.xacro diff --git a/ros2_control_demo_bringup/launch/transmissions_rrbot_system_position_only.launch.py b/ros2_control_demo_bringup/launch/transmissions_rrbot_system_position_only.launch.py new file mode 100644 index 000000000..6ab0ad8ee --- /dev/null +++ b/ros2_control_demo_bringup/launch/transmissions_rrbot_system_position_only.launch.py @@ -0,0 +1,89 @@ +# Copyright 2021 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir + + +def generate_launch_description(): + # Declare arguments + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "prefix", + default_value='""', + description="Prefix of the joint names, useful for \ + multi-robot setup. If changed than also joint names in the controllers' configuration \ + have to be updated.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "use_fake_hardware", + default_value="false", + description="Start robot with fake hardware mirroring command to its states.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "mock_sensor_commands", + default_value="false", + description="Enable fake command interfaces for sensors used for simple simulations. \ + Used only if 'use_fake_hardware' parameter is true.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "slowdown", default_value="50.0", description="Slowdown factor of the RRbot." + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "robot_controller", + default_value="forward_position_controller", + description="Robot controller to start.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "start_rviz", + default_value="true", + description="Start RViz2 automatically with this launch file.", + ) + ) + + # Initialize Arguments + prefix = LaunchConfiguration("prefix") + use_fake_hardware = LaunchConfiguration("use_fake_hardware") + mock_sensor_commands = LaunchConfiguration("mock_sensor_commands") + slowdown = LaunchConfiguration("slowdown") + robot_controller = LaunchConfiguration("robot_controller") + start_rviz = LaunchConfiguration("start_rviz") + + base_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ThisLaunchFileDir(), "/rrbot_base.launch.py"]), + launch_arguments={ + "description_file": "transmissions_rrbot_system_position_only.urdf.xacro", + "prefix": prefix, + "use_fake_hardware": use_fake_hardware, + "mock_sensor_commands": mock_sensor_commands, + "slowdown": slowdown, + "robot_controller": robot_controller, + "start_rviz": start_rviz, + }.items(), + ) + + return LaunchDescription(declared_arguments + [base_launch]) diff --git a/ros2_control_demo_description/rrbot_description/ros2_control/transmissions_rrbot_system_position_only.ros2_control.xacro b/ros2_control_demo_description/rrbot_description/ros2_control/transmissions_rrbot_system_position_only.ros2_control.xacro new file mode 100644 index 000000000..9351ed91b --- /dev/null +++ b/ros2_control_demo_description/rrbot_description/ros2_control/transmissions_rrbot_system_position_only.ros2_control.xacro @@ -0,0 +1,71 @@ + + + + + + + + + + gazebo_ros2_control/GazeboSystem + + + + + + mock_components/GenericSystem + ${mock_sensor_commands} + 0.0 + + + ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware + 0.0 + 3.0 + ${slowdown} + + + + + + + -1 + 1 + + + + + + -1 + 1 + + + + + + transmission_interface/SimpleTransmission + + 2.0 + 0.0 + + + 2.0 + 0.0 + + + + + transmission_interface/SimpleTransmission + + 2.0 + 0.0 + + + 2.0 + 0.0 + + + + + + + diff --git a/ros2_control_demo_description/rrbot_description/urdf/transmissions_rrbot_system_position_only.urdf.xacro b/ros2_control_demo_description/rrbot_description/urdf/transmissions_rrbot_system_position_only.urdf.xacro new file mode 100644 index 000000000..8ee198885 --- /dev/null +++ b/ros2_control_demo_description/rrbot_description/urdf/transmissions_rrbot_system_position_only.urdf.xacro @@ -0,0 +1,46 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + + + + + + + + + + From 75b7d9b8565957068a5b53795db071269c78a006 Mon Sep 17 00:00:00 2001 From: Jordan Palacios Date: Thu, 19 Jan 2023 12:08:59 +0100 Subject: [PATCH 02/27] Renamed to rrbot_transmissions --- ...ch.py => rrbot_transmissions_system_position_only.launch.py} | 2 +- ...rrbot_transmissions_system_position_only.ros2_control.xacro} | 0 ...acro => rrbot_transmissions_system_position_only.urdf.xacro} | 2 +- 3 files changed, 2 insertions(+), 2 deletions(-) rename ros2_control_demo_bringup/launch/{transmissions_rrbot_system_position_only.launch.py => rrbot_transmissions_system_position_only.launch.py} (98%) rename ros2_control_demo_description/rrbot_description/ros2_control/{transmissions_rrbot_system_position_only.ros2_control.xacro => rrbot_transmissions_system_position_only.ros2_control.xacro} (100%) rename ros2_control_demo_description/rrbot_description/urdf/{transmissions_rrbot_system_position_only.urdf.xacro => rrbot_transmissions_system_position_only.urdf.xacro} (96%) diff --git a/ros2_control_demo_bringup/launch/transmissions_rrbot_system_position_only.launch.py b/ros2_control_demo_bringup/launch/rrbot_transmissions_system_position_only.launch.py similarity index 98% rename from ros2_control_demo_bringup/launch/transmissions_rrbot_system_position_only.launch.py rename to ros2_control_demo_bringup/launch/rrbot_transmissions_system_position_only.launch.py index 6ab0ad8ee..3ca29b4c7 100644 --- a/ros2_control_demo_bringup/launch/transmissions_rrbot_system_position_only.launch.py +++ b/ros2_control_demo_bringup/launch/rrbot_transmissions_system_position_only.launch.py @@ -76,7 +76,7 @@ def generate_launch_description(): base_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource([ThisLaunchFileDir(), "/rrbot_base.launch.py"]), launch_arguments={ - "description_file": "transmissions_rrbot_system_position_only.urdf.xacro", + "description_file": "rrbot_transmissions_system_position_only.urdf.xacro", "prefix": prefix, "use_fake_hardware": use_fake_hardware, "mock_sensor_commands": mock_sensor_commands, diff --git a/ros2_control_demo_description/rrbot_description/ros2_control/transmissions_rrbot_system_position_only.ros2_control.xacro b/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_transmissions_system_position_only.ros2_control.xacro similarity index 100% rename from ros2_control_demo_description/rrbot_description/ros2_control/transmissions_rrbot_system_position_only.ros2_control.xacro rename to ros2_control_demo_description/rrbot_description/ros2_control/rrbot_transmissions_system_position_only.ros2_control.xacro diff --git a/ros2_control_demo_description/rrbot_description/urdf/transmissions_rrbot_system_position_only.urdf.xacro b/ros2_control_demo_description/rrbot_description/urdf/rrbot_transmissions_system_position_only.urdf.xacro similarity index 96% rename from ros2_control_demo_description/rrbot_description/urdf/transmissions_rrbot_system_position_only.urdf.xacro rename to ros2_control_demo_description/rrbot_description/urdf/rrbot_transmissions_system_position_only.urdf.xacro index 8ee198885..f3d8ab2a1 100644 --- a/ros2_control_demo_description/rrbot_description/urdf/transmissions_rrbot_system_position_only.urdf.xacro +++ b/ros2_control_demo_description/rrbot_description/urdf/rrbot_transmissions_system_position_only.urdf.xacro @@ -23,7 +23,7 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc - + From d92c9a0d199306aaae1c21ce771095eb9921270f Mon Sep 17 00:00:00 2001 From: Jordan Palacios Date: Thu, 19 Jan 2023 12:09:42 +0100 Subject: [PATCH 03/27] Changed reduction value of transmission2 --- ...rbot_transmissions_system_position_only.ros2_control.xacro | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_transmissions_system_position_only.ros2_control.xacro b/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_transmissions_system_position_only.ros2_control.xacro index 9351ed91b..344de324f 100644 --- a/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_transmissions_system_position_only.ros2_control.xacro +++ b/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_transmissions_system_position_only.ros2_control.xacro @@ -56,11 +56,11 @@ transmission_interface/SimpleTransmission - 2.0 + 4.0 0.0 - 2.0 + 4.0 0.0 From 305f287dc2ef4c4e7e4b217ec92fdf4ce7db14ed Mon Sep 17 00:00:00 2001 From: Jordan Palacios Date: Thu, 19 Jan 2023 12:28:16 +0100 Subject: [PATCH 04/27] Added the rrbot_transmissions_system_position_only Exact copy of rrbot_system_position_only for now. Using it from the transmission demo --- ...ns_system_position_only.ros2_control.xacro | 2 +- ...nsmissions_system_position_only.urdf.xacro | 2 +- ros2_control_demo_hardware/CMakeLists.txt | 3 +- ...bot_transmissions_system_position_only.hpp | 81 ++++++ .../ros2_control_demo_hardware.xml | 7 + ...bot_transmissions_system_position_only.cpp | 236 ++++++++++++++++++ 6 files changed, 328 insertions(+), 3 deletions(-) create mode 100644 ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_transmissions_system_position_only.hpp create mode 100644 ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp diff --git a/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_transmissions_system_position_only.ros2_control.xacro b/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_transmissions_system_position_only.ros2_control.xacro index 344de324f..ffd79dbe4 100644 --- a/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_transmissions_system_position_only.ros2_control.xacro +++ b/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_transmissions_system_position_only.ros2_control.xacro @@ -18,7 +18,7 @@ 0.0 - ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware + ros2_control_demo_hardware/RRBotTransmissionsSystemPositionOnlyHardware 0.0 3.0 ${slowdown} diff --git a/ros2_control_demo_description/rrbot_description/urdf/rrbot_transmissions_system_position_only.urdf.xacro b/ros2_control_demo_description/rrbot_description/urdf/rrbot_transmissions_system_position_only.urdf.xacro index f3d8ab2a1..a04bc1256 100644 --- a/ros2_control_demo_description/rrbot_description/urdf/rrbot_transmissions_system_position_only.urdf.xacro +++ b/ros2_control_demo_description/rrbot_description/urdf/rrbot_transmissions_system_position_only.urdf.xacro @@ -38,7 +38,7 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc diff --git a/ros2_control_demo_hardware/CMakeLists.txt b/ros2_control_demo_hardware/CMakeLists.txt index e27bb5658..cdf4e225f 100644 --- a/ros2_control_demo_hardware/CMakeLists.txt +++ b/ros2_control_demo_hardware/CMakeLists.txt @@ -26,6 +26,7 @@ add_library( src/rrbot_system_position_only.cpp src/rrbot_system_multi_interface.cpp src/rrbot_system_with_sensor.cpp + src/rrbot_transmissions_system_position_only.cpp src/rrbot_actuator.cpp src/external_rrbot_force_torque_sensor.cpp ) @@ -46,7 +47,7 @@ ament_target_dependencies( # which is appropriate when building the dll but not consuming it. target_compile_definitions(${PROJECT_NAME} PRIVATE "ROS2_CONTROL_DEMO_HARDWARE_BUILDING_DLL") -# Export hardware pligins +# Export hardware plugins pluginlib_export_plugin_description_file(hardware_interface ros2_control_demo_hardware.xml) # INSTALL diff --git a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_transmissions_system_position_only.hpp b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_transmissions_system_position_only.hpp new file mode 100644 index 000000000..0d36be6a7 --- /dev/null +++ b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_transmissions_system_position_only.hpp @@ -0,0 +1,81 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ROS2_CONTROL_DEMO_HARDWARE__RRBOT_TRANSMISSIONS_SYSTEM_POSITION_ONLY_HPP_ +#define ROS2_CONTROL_DEMO_HARDWARE__RRBOT_TRANSMISSIONS_SYSTEM_POSITION_ONLY_HPP_ + +#include +#include +#include + +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "ros2_control_demo_hardware/visibility_control.h" + +namespace ros2_control_demo_hardware +{ +class RRBotTransmissionsSystemPositionOnlyHardware : public hardware_interface::SystemInterface +{ +public: + RCLCPP_SHARED_PTR_DEFINITIONS(RRBotTransmissionsSystemPositionOnlyHardware); + + ROS2_CONTROL_DEMO_HARDWARE_PUBLIC + hardware_interface::CallbackReturn on_init( + const hardware_interface::HardwareInfo & info) override; + + ROS2_CONTROL_DEMO_HARDWARE_PUBLIC + hardware_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + ROS2_CONTROL_DEMO_HARDWARE_PUBLIC + std::vector export_state_interfaces() override; + + ROS2_CONTROL_DEMO_HARDWARE_PUBLIC + std::vector export_command_interfaces() override; + + ROS2_CONTROL_DEMO_HARDWARE_PUBLIC + hardware_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + ROS2_CONTROL_DEMO_HARDWARE_PUBLIC + hardware_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + ROS2_CONTROL_DEMO_HARDWARE_PUBLIC + hardware_interface::return_type read( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + ROS2_CONTROL_DEMO_HARDWARE_PUBLIC + hardware_interface::return_type write( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + +private: + // Parameters for the RRBot simulation + 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_hardware + +#endif // ROS2_CONTROL_DEMO_HARDWARE__RRBOT_TRANSMISSIONS_SYSTEM_POSITION_ONLY_HPP_ diff --git a/ros2_control_demo_hardware/ros2_control_demo_hardware.xml b/ros2_control_demo_hardware/ros2_control_demo_hardware.xml index 519d54456..6962cef61 100644 --- a/ros2_control_demo_hardware/ros2_control_demo_hardware.xml +++ b/ros2_control_demo_hardware/ros2_control_demo_hardware.xml @@ -41,4 +41,11 @@ The ros2_control RRBot example using external Force Torque Sensor where it is separate from the RRBot. + + + The ROS2 Control minimal robot protocol with trasmissions. + + diff --git a/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp b/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp new file mode 100644 index 000000000..113885eff --- /dev/null +++ b/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp @@ -0,0 +1,236 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "ros2_control_demo_hardware/rrbot_transmissions_system_position_only.hpp" + +#include +#include +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace ros2_control_demo_hardware +{ +hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware::on_init( + const hardware_interface::HardwareInfo & info) +{ + if ( + hardware_interface::SystemInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } + + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + hw_start_sec_ = stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]); + hw_stop_sec_ = stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]); + hw_slowdown_ = stod(info_.hardware_parameters["example_param_hw_slowdown"]); + // END: This part here is for exemplary purposes - Please do not copy to your production code + hw_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + hw_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + + for (const hardware_interface::ComponentInfo & joint : info_.joints) + { + // RRBotSystemPositionOnly has exactly one state and command interface on each joint + if (joint.command_interfaces.size() != 1) + { + RCLCPP_FATAL( + rclcpp::get_logger("RRBotTransmissionsSystemPositionOnlyHardware"), + "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(), + joint.command_interfaces.size()); + return hardware_interface::CallbackReturn::ERROR; + } + + if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) + { + RCLCPP_FATAL( + rclcpp::get_logger("RRBotTransmissionsSystemPositionOnlyHardware"), + "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(), + joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); + return hardware_interface::CallbackReturn::ERROR; + } + + if (joint.state_interfaces.size() != 1) + { + RCLCPP_FATAL( + rclcpp::get_logger("RRBotTransmissionsSystemPositionOnlyHardware"), + "Joint '%s' has %zu state interface. 1 expected.", joint.name.c_str(), + joint.state_interfaces.size()); + return hardware_interface::CallbackReturn::ERROR; + } + + if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) + { + RCLCPP_FATAL( + rclcpp::get_logger("RRBotTransmissionsSystemPositionOnlyHardware"), + "Joint '%s' have %s state interface. '%s' expected.", joint.name.c_str(), + joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); + return hardware_interface::CallbackReturn::ERROR; + } + } + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + RCLCPP_INFO( + rclcpp::get_logger("RRBotTransmissionsSystemPositionOnlyHardware"), "Configuring ...please wait..."); + + for (int i = 0; i < hw_start_sec_; i++) + { + rclcpp::sleep_for(std::chrono::seconds(1)); + RCLCPP_INFO( + rclcpp::get_logger("RRBotTransmissionsSystemPositionOnlyHardware"), "%.1f seconds left...", + hw_start_sec_ - i); + } + // 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++) + { + hw_states_[i] = 0; + hw_commands_[i] = 0; + } + + RCLCPP_INFO(rclcpp::get_logger("RRBotTransmissionsSystemPositionOnlyHardware"), "Successfully configured!"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +std::vector +RRBotTransmissionsSystemPositionOnlyHardware::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 +RRBotTransmissionsSystemPositionOnlyHardware::export_command_interfaces() +{ + std::vector command_interfaces; + for (uint i = 0; i < info_.joints.size(); i++) + { + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i])); + } + + return command_interfaces; +} + +hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + RCLCPP_INFO( + rclcpp::get_logger("RRBotTransmissionsSystemPositionOnlyHardware"), "Activating ...please wait..."); + + for (int i = 0; i < hw_start_sec_; i++) + { + rclcpp::sleep_for(std::chrono::seconds(1)); + RCLCPP_INFO( + rclcpp::get_logger("RRBotTransmissionsSystemPositionOnlyHardware"), "%.1f seconds left...", + hw_start_sec_ - i); + } + // 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++) + { + hw_commands_[i] = hw_states_[i]; + } + + RCLCPP_INFO(rclcpp::get_logger("RRBotTransmissionsSystemPositionOnlyHardware"), "Successfully activated!"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + RCLCPP_INFO( + rclcpp::get_logger("RRBotTransmissionsSystemPositionOnlyHardware"), "Deactivating ...please wait..."); + + for (int i = 0; i < hw_stop_sec_; i++) + { + rclcpp::sleep_for(std::chrono::seconds(1)); + RCLCPP_INFO( + rclcpp::get_logger("RRBotTransmissionsSystemPositionOnlyHardware"), "%.1f seconds left...", + hw_stop_sec_ - i); + } + + RCLCPP_INFO(rclcpp::get_logger("RRBotTransmissionsSystemPositionOnlyHardware"), "Successfully deactivated!"); + // END: This part here is for exemplary purposes - Please do not copy to your production code + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::return_type RRBotTransmissionsSystemPositionOnlyHardware::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 + RCLCPP_INFO(rclcpp::get_logger("RRBotTransmissionsSystemPositionOnlyHardware"), "Reading..."); + + for (uint i = 0; i < hw_states_.size(); i++) + { + // Simulate RRBot's movement + hw_states_[i] = hw_states_[i] + (hw_commands_[i] - hw_states_[i]) / hw_slowdown_; + RCLCPP_INFO( + rclcpp::get_logger("RRBotTransmissionsSystemPositionOnlyHardware"), "Got state %.5f for joint %d!", + hw_states_[i], i); + } + RCLCPP_INFO(rclcpp::get_logger("RRBotTransmissionsSystemPositionOnlyHardware"), "Joints successfully read!"); + // END: This part here is for exemplary purposes - Please do not copy to your production code + + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type RRBotTransmissionsSystemPositionOnlyHardware::write( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + RCLCPP_INFO(rclcpp::get_logger("RRBotTransmissionsSystemPositionOnlyHardware"), "Writing..."); + + for (uint i = 0; i < hw_commands_.size(); i++) + { + // Simulate sending commands to the hardware + RCLCPP_INFO( + rclcpp::get_logger("RRBotTransmissionsSystemPositionOnlyHardware"), "Got command %.5f for joint %d!", + hw_commands_[i], i); + } + RCLCPP_INFO( + rclcpp::get_logger("RRBotTransmissionsSystemPositionOnlyHardware"), "Joints successfully written!"); + // END: This part here is for exemplary purposes - Please do not copy to your production code + + return hardware_interface::return_type::OK; +} + +} // namespace ros2_control_demo_hardware + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + ros2_control_demo_hardware::RRBotTransmissionsSystemPositionOnlyHardware, hardware_interface::SystemInterface) From e6a795e9aeb763da3fbb64e0286437724bfaa288 Mon Sep 17 00:00:00 2001 From: Jordan Palacios Date: Thu, 19 Jan 2023 13:08:23 +0100 Subject: [PATCH 05/27] Moved logger into its own variable --- ...bot_transmissions_system_position_only.hpp | 4 +- ...bot_transmissions_system_position_only.cpp | 40 ++++++++++--------- 2 files changed, 24 insertions(+), 20 deletions(-) diff --git a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_transmissions_system_position_only.hpp b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_transmissions_system_position_only.hpp index 0d36be6a7..773a7eb3d 100644 --- a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_transmissions_system_position_only.hpp +++ b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_transmissions_system_position_only.hpp @@ -23,8 +23,8 @@ #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/logger.hpp" #include "rclcpp/macros.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "ros2_control_demo_hardware/visibility_control.h" @@ -66,6 +66,8 @@ class RRBotTransmissionsSystemPositionOnlyHardware : public hardware_interface:: const rclcpp::Time & time, const rclcpp::Duration & period) override; private: + std::unique_ptr logger_; + // Parameters for the RRBot simulation double hw_start_sec_; double hw_stop_sec_; diff --git a/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp b/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp index 113885eff..ae9696f89 100644 --- a/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp +++ b/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp @@ -28,6 +28,8 @@ namespace ros2_control_demo_hardware hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware::on_init( const hardware_interface::HardwareInfo & info) { + logger_ = std::make_unique(rclcpp::get_logger("RRBotTransmissionsSystemPositionOnlyHardware")); + if ( hardware_interface::SystemInterface::on_init(info) != hardware_interface::CallbackReturn::SUCCESS) @@ -49,7 +51,7 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: if (joint.command_interfaces.size() != 1) { RCLCPP_FATAL( - rclcpp::get_logger("RRBotTransmissionsSystemPositionOnlyHardware"), + *logger_, "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(), joint.command_interfaces.size()); return hardware_interface::CallbackReturn::ERROR; @@ -58,7 +60,7 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) { RCLCPP_FATAL( - rclcpp::get_logger("RRBotTransmissionsSystemPositionOnlyHardware"), + *logger_, "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(), joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); return hardware_interface::CallbackReturn::ERROR; @@ -67,7 +69,7 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: if (joint.state_interfaces.size() != 1) { RCLCPP_FATAL( - rclcpp::get_logger("RRBotTransmissionsSystemPositionOnlyHardware"), + *logger_, "Joint '%s' has %zu state interface. 1 expected.", joint.name.c_str(), joint.state_interfaces.size()); return hardware_interface::CallbackReturn::ERROR; @@ -76,7 +78,7 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) { RCLCPP_FATAL( - rclcpp::get_logger("RRBotTransmissionsSystemPositionOnlyHardware"), + *logger_, "Joint '%s' have %s state interface. '%s' expected.", joint.name.c_str(), joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); return hardware_interface::CallbackReturn::ERROR; @@ -91,13 +93,13 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code RCLCPP_INFO( - rclcpp::get_logger("RRBotTransmissionsSystemPositionOnlyHardware"), "Configuring ...please wait..."); + *logger_, "Configuring ...please wait..."); for (int i = 0; i < hw_start_sec_; i++) { rclcpp::sleep_for(std::chrono::seconds(1)); RCLCPP_INFO( - rclcpp::get_logger("RRBotTransmissionsSystemPositionOnlyHardware"), "%.1f seconds left...", + *logger_, "%.1f seconds left...", hw_start_sec_ - i); } // END: This part here is for exemplary purposes - Please do not copy to your production code @@ -109,7 +111,7 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: hw_commands_[i] = 0; } - RCLCPP_INFO(rclcpp::get_logger("RRBotTransmissionsSystemPositionOnlyHardware"), "Successfully configured!"); + RCLCPP_INFO(*logger_, "Successfully configured!"); return hardware_interface::CallbackReturn::SUCCESS; } @@ -145,13 +147,13 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code RCLCPP_INFO( - rclcpp::get_logger("RRBotTransmissionsSystemPositionOnlyHardware"), "Activating ...please wait..."); + *logger_, "Activating ...please wait..."); for (int i = 0; i < hw_start_sec_; i++) { rclcpp::sleep_for(std::chrono::seconds(1)); RCLCPP_INFO( - rclcpp::get_logger("RRBotTransmissionsSystemPositionOnlyHardware"), "%.1f seconds left...", + *logger_, "%.1f seconds left...", hw_start_sec_ - i); } // END: This part here is for exemplary purposes - Please do not copy to your production code @@ -162,7 +164,7 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: hw_commands_[i] = hw_states_[i]; } - RCLCPP_INFO(rclcpp::get_logger("RRBotTransmissionsSystemPositionOnlyHardware"), "Successfully activated!"); + RCLCPP_INFO(*logger_, "Successfully activated!"); return hardware_interface::CallbackReturn::SUCCESS; } @@ -172,17 +174,17 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code RCLCPP_INFO( - rclcpp::get_logger("RRBotTransmissionsSystemPositionOnlyHardware"), "Deactivating ...please wait..."); + *logger_, "Deactivating ...please wait..."); for (int i = 0; i < hw_stop_sec_; i++) { rclcpp::sleep_for(std::chrono::seconds(1)); RCLCPP_INFO( - rclcpp::get_logger("RRBotTransmissionsSystemPositionOnlyHardware"), "%.1f seconds left...", + *logger_, "%.1f seconds left...", hw_stop_sec_ - i); } - RCLCPP_INFO(rclcpp::get_logger("RRBotTransmissionsSystemPositionOnlyHardware"), "Successfully deactivated!"); + RCLCPP_INFO(*logger_, "Successfully deactivated!"); // END: This part here is for exemplary purposes - Please do not copy to your production code return hardware_interface::CallbackReturn::SUCCESS; @@ -192,17 +194,17 @@ hardware_interface::return_type RRBotTransmissionsSystemPositionOnlyHardware::re const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO(rclcpp::get_logger("RRBotTransmissionsSystemPositionOnlyHardware"), "Reading..."); + RCLCPP_INFO(*logger_, "Reading..."); for (uint i = 0; i < hw_states_.size(); i++) { // Simulate RRBot's movement hw_states_[i] = hw_states_[i] + (hw_commands_[i] - hw_states_[i]) / hw_slowdown_; RCLCPP_INFO( - rclcpp::get_logger("RRBotTransmissionsSystemPositionOnlyHardware"), "Got state %.5f for joint %d!", + *logger_, "Got state %.5f for joint %d!", hw_states_[i], i); } - RCLCPP_INFO(rclcpp::get_logger("RRBotTransmissionsSystemPositionOnlyHardware"), "Joints successfully read!"); + RCLCPP_INFO(*logger_, "Joints successfully read!"); // END: This part here is for exemplary purposes - Please do not copy to your production code return hardware_interface::return_type::OK; @@ -212,17 +214,17 @@ hardware_interface::return_type RRBotTransmissionsSystemPositionOnlyHardware::wr const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO(rclcpp::get_logger("RRBotTransmissionsSystemPositionOnlyHardware"), "Writing..."); + RCLCPP_INFO(*logger_, "Writing..."); for (uint i = 0; i < hw_commands_.size(); i++) { // Simulate sending commands to the hardware RCLCPP_INFO( - rclcpp::get_logger("RRBotTransmissionsSystemPositionOnlyHardware"), "Got command %.5f for joint %d!", + *logger_, "Got command %.5f for joint %d!", hw_commands_[i], i); } RCLCPP_INFO( - rclcpp::get_logger("RRBotTransmissionsSystemPositionOnlyHardware"), "Joints successfully written!"); + *logger_, "Joints successfully written!"); // END: This part here is for exemplary purposes - Please do not copy to your production code return hardware_interface::return_type::OK; From e0b3c4d320ef37158925577b9dc483c64437ec8a Mon Sep 17 00:00:00 2001 From: Jordan Palacios Date: Thu, 19 Jan 2023 14:25:56 +0100 Subject: [PATCH 06/27] Uncrustify --- ...bot_transmissions_system_position_only.cpp | 60 ++++++++----------- 1 file changed, 26 insertions(+), 34 deletions(-) diff --git a/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp b/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp index ae9696f89..00e1b3d86 100644 --- a/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp +++ b/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp @@ -28,7 +28,10 @@ namespace ros2_control_demo_hardware hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware::on_init( const hardware_interface::HardwareInfo & info) { - logger_ = std::make_unique(rclcpp::get_logger("RRBotTransmissionsSystemPositionOnlyHardware")); + logger_ = + std::make_unique( + rclcpp::get_logger( + "RRBotTransmissionsSystemPositionOnlyHardware")); if ( hardware_interface::SystemInterface::on_init(info) != @@ -45,11 +48,9 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: 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) - { + for (const hardware_interface::ComponentInfo & joint : info_.joints) { // RRBotSystemPositionOnly has exactly one state and command interface on each joint - if (joint.command_interfaces.size() != 1) - { + if (joint.command_interfaces.size() != 1) { RCLCPP_FATAL( *logger_, "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(), @@ -57,8 +58,7 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: return hardware_interface::CallbackReturn::ERROR; } - if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) - { + if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) { RCLCPP_FATAL( *logger_, "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(), @@ -66,8 +66,7 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: return hardware_interface::CallbackReturn::ERROR; } - if (joint.state_interfaces.size() != 1) - { + if (joint.state_interfaces.size() != 1) { RCLCPP_FATAL( *logger_, "Joint '%s' has %zu state interface. 1 expected.", joint.name.c_str(), @@ -75,8 +74,7 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: return hardware_interface::CallbackReturn::ERROR; } - if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) - { + if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) { RCLCPP_FATAL( *logger_, "Joint '%s' have %s state interface. '%s' expected.", joint.name.c_str(), @@ -95,8 +93,7 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: RCLCPP_INFO( *logger_, "Configuring ...please wait..."); - for (int i = 0; i < hw_start_sec_; i++) - { + for (int i = 0; i < hw_start_sec_; i++) { rclcpp::sleep_for(std::chrono::seconds(1)); RCLCPP_INFO( *logger_, "%.1f seconds left...", @@ -105,8 +102,7 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: // END: This part here is for exemplary purposes - Please do not copy to your production code // reset values always when configuring hardware - for (uint i = 0; i < hw_states_.size(); i++) - { + for (uint i = 0; i < hw_states_.size(); i++) { hw_states_[i] = 0; hw_commands_[i] = 0; } @@ -120,10 +116,10 @@ std::vector RRBotTransmissionsSystemPositionOnlyHardware::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])); + 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; @@ -133,10 +129,10 @@ std::vector RRBotTransmissionsSystemPositionOnlyHardware::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])); + for (uint i = 0; i < info_.joints.size(); i++) { + command_interfaces.emplace_back( + hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i])); } return command_interfaces; @@ -149,8 +145,7 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: RCLCPP_INFO( *logger_, "Activating ...please wait..."); - for (int i = 0; i < hw_start_sec_; i++) - { + for (int i = 0; i < hw_start_sec_; i++) { rclcpp::sleep_for(std::chrono::seconds(1)); RCLCPP_INFO( *logger_, "%.1f seconds left...", @@ -159,8 +154,7 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: // END: This part here is for exemplary purposes - Please do not copy to your production code // command and state should be equal when starting - for (uint i = 0; i < hw_states_.size(); i++) - { + for (uint i = 0; i < hw_states_.size(); i++) { hw_commands_[i] = hw_states_[i]; } @@ -176,8 +170,7 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: RCLCPP_INFO( *logger_, "Deactivating ...please wait..."); - for (int i = 0; i < hw_stop_sec_; i++) - { + for (int i = 0; i < hw_stop_sec_; i++) { rclcpp::sleep_for(std::chrono::seconds(1)); RCLCPP_INFO( *logger_, "%.1f seconds left...", @@ -196,8 +189,7 @@ hardware_interface::return_type RRBotTransmissionsSystemPositionOnlyHardware::re // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code RCLCPP_INFO(*logger_, "Reading..."); - for (uint i = 0; i < hw_states_.size(); i++) - { + for (uint i = 0; i < hw_states_.size(); i++) { // Simulate RRBot's movement hw_states_[i] = hw_states_[i] + (hw_commands_[i] - hw_states_[i]) / hw_slowdown_; RCLCPP_INFO( @@ -216,8 +208,7 @@ hardware_interface::return_type RRBotTransmissionsSystemPositionOnlyHardware::wr // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code RCLCPP_INFO(*logger_, "Writing..."); - for (uint i = 0; i < hw_commands_.size(); i++) - { + for (uint i = 0; i < hw_commands_.size(); i++) { // Simulate sending commands to the hardware RCLCPP_INFO( *logger_, "Got command %.5f for joint %d!", @@ -235,4 +226,5 @@ hardware_interface::return_type RRBotTransmissionsSystemPositionOnlyHardware::wr #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( - ros2_control_demo_hardware::RRBotTransmissionsSystemPositionOnlyHardware, hardware_interface::SystemInterface) + ros2_control_demo_hardware::RRBotTransmissionsSystemPositionOnlyHardware, + hardware_interface::SystemInterface) From 73ed52efefc7b9c4ceffec4f6065c9f5aabaa0d6 Mon Sep 17 00:00:00 2001 From: Jordan Palacios Date: Thu, 19 Jan 2023 14:52:46 +0100 Subject: [PATCH 07/27] Renamed hw interface to joint. Added actuator interfaces --- ...bot_transmissions_system_position_only.hpp | 10 +++++-- ...bot_transmissions_system_position_only.cpp | 28 +++++++++---------- 2 files changed, 21 insertions(+), 17 deletions(-) diff --git a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_transmissions_system_position_only.hpp b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_transmissions_system_position_only.hpp index 773a7eb3d..6c21d0be1 100644 --- a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_transmissions_system_position_only.hpp +++ b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_transmissions_system_position_only.hpp @@ -73,9 +73,13 @@ class RRBotTransmissionsSystemPositionOnlyHardware : public hardware_interface:: double hw_stop_sec_; double hw_slowdown_; - // Store the command for the simulated robot - std::vector hw_commands_; - std::vector hw_states_; + // interfaces for the simulated robot joints + std::vector joint_commands_; + std::vector joint_states_; + + // interfaces for the simulated robot actuators + std::vector actuator_commands_; + std::vector actuator_states_; }; } // namespace ros2_control_demo_hardware diff --git a/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp b/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp index 00e1b3d86..ebb497ce9 100644 --- a/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp +++ b/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp @@ -45,8 +45,8 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: 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()); + joint_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + joint_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 @@ -102,9 +102,9 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: // 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++) { - hw_states_[i] = 0; - hw_commands_[i] = 0; + for (uint i = 0; i < joint_states_.size(); i++) { + joint_states_[i] = 0; + joint_commands_[i] = 0; } RCLCPP_INFO(*logger_, "Successfully configured!"); @@ -119,7 +119,7 @@ RRBotTransmissionsSystemPositionOnlyHardware::export_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])); + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &joint_states_[i])); } return state_interfaces; @@ -132,7 +132,7 @@ RRBotTransmissionsSystemPositionOnlyHardware::export_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])); + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &joint_commands_[i])); } return command_interfaces; @@ -154,8 +154,8 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: // 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++) { - hw_commands_[i] = hw_states_[i]; + for (uint i = 0; i < joint_states_.size(); i++) { + joint_commands_[i] = joint_states_[i]; } RCLCPP_INFO(*logger_, "Successfully activated!"); @@ -189,12 +189,12 @@ hardware_interface::return_type RRBotTransmissionsSystemPositionOnlyHardware::re // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code RCLCPP_INFO(*logger_, "Reading..."); - for (uint i = 0; i < hw_states_.size(); i++) { + for (uint i = 0; i < joint_states_.size(); i++) { // Simulate RRBot's movement - hw_states_[i] = hw_states_[i] + (hw_commands_[i] - hw_states_[i]) / hw_slowdown_; + joint_states_[i] = joint_states_[i] + (joint_commands_[i] - joint_states_[i]) / hw_slowdown_; RCLCPP_INFO( *logger_, "Got state %.5f for joint %d!", - hw_states_[i], i); + joint_states_[i], i); } RCLCPP_INFO(*logger_, "Joints successfully read!"); // END: This part here is for exemplary purposes - Please do not copy to your production code @@ -208,11 +208,11 @@ hardware_interface::return_type RRBotTransmissionsSystemPositionOnlyHardware::wr // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code RCLCPP_INFO(*logger_, "Writing..."); - for (uint i = 0; i < hw_commands_.size(); i++) { + for (uint i = 0; i < joint_commands_.size(); i++) { // Simulate sending commands to the hardware RCLCPP_INFO( *logger_, "Got command %.5f for joint %d!", - hw_commands_[i], i); + joint_commands_[i], i); } RCLCPP_INFO( *logger_, "Joints successfully written!"); From d2f00483a2682e35cd953a2133d1baa665221d70 Mon Sep 17 00:00:00 2001 From: Jordan Palacios Date: Thu, 19 Jan 2023 14:56:54 +0100 Subject: [PATCH 08/27] Formatting. Remove comments --- ...bot_transmissions_system_position_only.cpp | 47 +++++-------------- 1 file changed, 11 insertions(+), 36 deletions(-) diff --git a/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp b/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp index ebb497ce9..87353df60 100644 --- a/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp +++ b/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp @@ -21,7 +21,7 @@ #include #include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "rclcpp/rclcpp.hpp" +#include "rclcpp/logging.hpp" namespace ros2_control_demo_hardware { @@ -40,11 +40,10 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: return hardware_interface::CallbackReturn::ERROR; } - // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code hw_start_sec_ = stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]); 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 + joint_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); joint_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); @@ -89,17 +88,12 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { - // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO( - *logger_, "Configuring ...please wait..."); + RCLCPP_INFO(*logger_, "Configuring ...please wait..."); for (int i = 0; i < hw_start_sec_; i++) { rclcpp::sleep_for(std::chrono::seconds(1)); - RCLCPP_INFO( - *logger_, "%.1f seconds left...", - hw_start_sec_ - i); + RCLCPP_INFO(*logger_, "%.1f seconds left...", hw_start_sec_ - i); } - // END: This part here is for exemplary purposes - Please do not copy to your production code // reset values always when configuring hardware for (uint i = 0; i < joint_states_.size(); i++) { @@ -141,17 +135,12 @@ RRBotTransmissionsSystemPositionOnlyHardware::export_command_interfaces() hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { - // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO( - *logger_, "Activating ...please wait..."); + RCLCPP_INFO(*logger_, "Activating ...please wait..."); for (int i = 0; i < hw_start_sec_; i++) { rclcpp::sleep_for(std::chrono::seconds(1)); - RCLCPP_INFO( - *logger_, "%.1f seconds left...", - hw_start_sec_ - i); + RCLCPP_INFO(*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 and state should be equal when starting for (uint i = 0; i < joint_states_.size(); i++) { @@ -166,19 +155,14 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { - // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO( - *logger_, "Deactivating ...please wait..."); + RCLCPP_INFO(*logger_, "Deactivating ...please wait..."); for (int i = 0; i < hw_stop_sec_; i++) { rclcpp::sleep_for(std::chrono::seconds(1)); - RCLCPP_INFO( - *logger_, "%.1f seconds left...", - hw_stop_sec_ - i); + RCLCPP_INFO(*logger_, "%.1f seconds left...", hw_stop_sec_ - i); } RCLCPP_INFO(*logger_, "Successfully deactivated!"); - // END: This part here is for exemplary purposes - Please do not copy to your production code return hardware_interface::CallbackReturn::SUCCESS; } @@ -186,18 +170,14 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: hardware_interface::return_type RRBotTransmissionsSystemPositionOnlyHardware::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 RCLCPP_INFO(*logger_, "Reading..."); for (uint i = 0; i < joint_states_.size(); i++) { // Simulate RRBot's movement joint_states_[i] = joint_states_[i] + (joint_commands_[i] - joint_states_[i]) / hw_slowdown_; - RCLCPP_INFO( - *logger_, "Got state %.5f for joint %d!", - joint_states_[i], i); + RCLCPP_INFO(*logger_, "Got state %.5f for joint %d!", joint_states_[i], i); } RCLCPP_INFO(*logger_, "Joints successfully read!"); - // END: This part here is for exemplary purposes - Please do not copy to your production code return hardware_interface::return_type::OK; } @@ -205,18 +185,13 @@ hardware_interface::return_type RRBotTransmissionsSystemPositionOnlyHardware::re hardware_interface::return_type RRBotTransmissionsSystemPositionOnlyHardware::write( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code RCLCPP_INFO(*logger_, "Writing..."); for (uint i = 0; i < joint_commands_.size(); i++) { // Simulate sending commands to the hardware - RCLCPP_INFO( - *logger_, "Got command %.5f for joint %d!", - joint_commands_[i], i); + RCLCPP_INFO(*logger_, "Got command %.5f for joint %d!", joint_commands_[i], i); } - RCLCPP_INFO( - *logger_, "Joints successfully written!"); - // END: This part here is for exemplary purposes - Please do not copy to your production code + RCLCPP_INFO(*logger_, "Joints successfully written!"); return hardware_interface::return_type::OK; } From 1296d2c73b203f040bd692eb365bf9786810c753 Mon Sep 17 00:00:00 2001 From: Jordan Palacios Date: Fri, 20 Jan 2023 10:17:25 +0100 Subject: [PATCH 09/27] Using transmissions for joint and actuator interfaces Added a helper object to manage the joint to transmissions to actuator data handling --- ros2_control_demo_hardware/CMakeLists.txt | 3 + ...bot_transmissions_system_position_only.hpp | 37 +++- ros2_control_demo_hardware/package.xml | 1 + ...bot_transmissions_system_position_only.cpp | 207 ++++++++++++++---- 4 files changed, 197 insertions(+), 51 deletions(-) diff --git a/ros2_control_demo_hardware/CMakeLists.txt b/ros2_control_demo_hardware/CMakeLists.txt index cdf4e225f..4cbe811ea 100644 --- a/ros2_control_demo_hardware/CMakeLists.txt +++ b/ros2_control_demo_hardware/CMakeLists.txt @@ -16,6 +16,7 @@ find_package(hardware_interface REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) +find_package(transmission_interface REQUIRED) ## COMPILE @@ -41,6 +42,7 @@ ament_target_dependencies( pluginlib rclcpp rclcpp_lifecycle + transmission_interface ) # Causes the visibility macros to use dllexport rather than dllimport, @@ -76,5 +78,6 @@ ament_export_dependencies( pluginlib rclcpp rclcpp_lifecycle + transmission_interface ) ament_package() diff --git a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_transmissions_system_position_only.hpp b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_transmissions_system_position_only.hpp index 6c21d0be1..9a0f165e5 100644 --- a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_transmissions_system_position_only.hpp +++ b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_transmissions_system_position_only.hpp @@ -15,6 +15,7 @@ #ifndef ROS2_CONTROL_DEMO_HARDWARE__RRBOT_TRANSMISSIONS_SYSTEM_POSITION_ONLY_HPP_ #define ROS2_CONTROL_DEMO_HARDWARE__RRBOT_TRANSMISSIONS_SYSTEM_POSITION_ONLY_HPP_ +#include #include #include #include @@ -27,6 +28,7 @@ #include "rclcpp/macros.hpp" #include "rclcpp_lifecycle/state.hpp" #include "ros2_control_demo_hardware/visibility_control.h" +#include "transmission_interface/transmission.hpp" namespace ros2_control_demo_hardware { @@ -68,18 +70,43 @@ class RRBotTransmissionsSystemPositionOnlyHardware : public hardware_interface:: private: std::unique_ptr logger_; - // Parameters for the RRBot simulation + // parameters for the RRBot simulation double hw_start_sec_; double hw_stop_sec_; double hw_slowdown_; + /// @todo delete this // interfaces for the simulated robot joints - std::vector joint_commands_; - std::vector joint_states_; +// std::vector joint_commands_; +// std::vector joint_states_; + /// @todo delete this // interfaces for the simulated robot actuators - std::vector actuator_commands_; - std::vector actuator_states_; +// std::vector actuator_commands_; +// std::vector actuator_states_; + + // interfaces for the transmission joint and actuator handles +// std::vector joint_handle_interfaces_; +// std::vector actuator_handle_interfaces_; + + /// @note no need to keep them? they sort off exist in the transmission + // joint handles + // actuator handles + + // transmissions + std::vector> transmissions_; + + struct InterfaceData + { + InterfaceData(const std::string & name); + + std::string name_; + double command_; + double state_; + double transmission_; + }; + std::vector joint_interfaces_; + std::vector actuator_interfaces_; }; } // namespace ros2_control_demo_hardware diff --git a/ros2_control_demo_hardware/package.xml b/ros2_control_demo_hardware/package.xml index 866c80bab..4e996853f 100644 --- a/ros2_control_demo_hardware/package.xml +++ b/ros2_control_demo_hardware/package.xml @@ -15,6 +15,7 @@ pluginlib rclcpp rclcpp_lifecycle + transmission_interface ament_cmake_gtest diff --git a/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp b/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp index 87353df60..1f2b66eab 100644 --- a/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp +++ b/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp @@ -18,13 +18,18 @@ #include #include #include +#include #include #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rclcpp/logging.hpp" +#include "transmission_interface/simple_transmission_loader.hpp" namespace ros2_control_demo_hardware { + +constexpr double kNaN = std::numeric_limits::quiet_NaN(); + hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware::on_init( const hardware_interface::HardwareInfo & info) { @@ -33,6 +38,8 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: rclcpp::get_logger( "RRBotTransmissionsSystemPositionOnlyHardware")); + RCLCPP_INFO(*logger_, "Initializing..."); + if ( hardware_interface::SystemInterface::on_init(info) != hardware_interface::CallbackReturn::SUCCESS) @@ -44,6 +51,7 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: hw_stop_sec_ = stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]); hw_slowdown_ = stod(info_.hardware_parameters["example_param_hw_slowdown"]); +#ifdef UNDEF joint_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); joint_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); @@ -81,6 +89,83 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: return hardware_interface::CallbackReturn::ERROR; } } +#endif + + /// @todo check joint data info from the core interfaces is consistent + /// with the one in the transmissions, i.e., number and name of joints + + const auto num_joints = std::accumulate( + info_.transmissions.begin(), info_.transmissions.end(), 0, + [](const auto & acc, const auto & trans_info) + { + return acc + trans_info.joints.size(); + }); + + const auto num_actuators = std::accumulate( + info_.transmissions.begin(), info_.transmissions.end(), 0, + [](const auto & acc, const auto & trans_info) + { + return acc + trans_info.actuators.size(); + }); + + // prereserve the space needed for joint and actuator interfaces + joint_interfaces_.reserve(num_joints); + actuator_interfaces_.reserve(num_actuators); + + // create transmissions, joint and actuator handles + auto transmission_loader = transmission_interface::SimpleTransmissionLoader(); + + for (const auto & transmission_info : info_.transmissions) { + // only simple transmissions are supported in this demo + if (transmission_info.type != "transmission_interface/SimpleTransmission") { + RCLCPP_FATAL( + *logger_, "Transmission '%s' of type '%s' not supported in this demo", + transmission_info.name.c_str(), transmission_info.type.c_str()); + return hardware_interface::CallbackReturn::ERROR; + } + + /// @todo add try/catch pair for this + auto transmission = transmission_loader.load(transmission_info); + + std::vector joint_handles; + for (const auto & joint_info : transmission_info.joints) { + if (!(joint_info.interfaces.size() == 1 && + joint_info.interfaces[0] == hardware_interface::HW_IF_POSITION)) + { + RCLCPP_FATAL( + *logger_, "Invalid transmission joint '%s' configuration", joint_info.name.c_str()); + return hardware_interface::CallbackReturn::ERROR; + } + + auto joint_interface = + joint_interfaces_.insert(joint_interfaces_.end(), InterfaceData(joint_info.name)); + + transmission_interface::JointHandle joint_handle(joint_info.name, + hardware_interface::HW_IF_POSITION, &joint_interface->transmission_); + joint_handles.push_back(joint_handle); + } + + std::vector actuator_handles; + for (const auto & actuator_info : transmission_info.actuators) { + // no check for actuators types? + + auto actuator_interface = + actuator_interfaces_.insert(actuator_interfaces_.end(), InterfaceData(actuator_info.name)); + transmission_interface::ActuatorHandle actuator_handle(actuator_info.name, + hardware_interface::HW_IF_POSITION, &actuator_interface->transmission_); + actuator_handles.push_back(actuator_handle); + } + + /// @note no need to store the joint and actuator handles, the + /// transmission will keep whatever info it needs + + /// @todo add try/catch pair for this + transmission->configure(joint_handles, actuator_handles); + + transmissions_.push_back(transmission); + } + + RCLCPP_INFO(*logger_, "Initialization successful"); return hardware_interface::CallbackReturn::SUCCESS; } @@ -88,20 +173,33 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { - RCLCPP_INFO(*logger_, "Configuring ...please wait..."); + RCLCPP_INFO(*logger_, "Configuring..."); - for (int i = 0; i < hw_start_sec_; i++) { - rclcpp::sleep_for(std::chrono::seconds(1)); - RCLCPP_INFO(*logger_, "%.1f seconds left...", hw_start_sec_ - i); - } +// for (int i = 0; i < hw_start_sec_; i++) { +// rclcpp::sleep_for(std::chrono::seconds(1)); +// RCLCPP_INFO(*logger_, "%.1f seconds left...", hw_start_sec_ - i); +// } - // reset values always when configuring hardware - for (uint i = 0; i < joint_states_.size(); i++) { - joint_states_[i] = 0; - joint_commands_[i] = 0; - } +// // reset values always when configuring hardware +// for (uint i = 0; i < joint_states_.size(); i++) { +// joint_states_[i] = 0; +// joint_commands_[i] = 0; +// } + + + auto reset_interfaces = [](std::vector & interfaces) + { + for (auto & interface_data : interfaces) { + interface_data.command_ = 0.0; + interface_data.state_ = 0.0; + interface_data.transmission_ = kNaN; + } + }; + + reset_interfaces(joint_interfaces_); + reset_interfaces(actuator_interfaces_); - RCLCPP_INFO(*logger_, "Successfully configured!"); + RCLCPP_INFO(*logger_, "Configuration successful"); return hardware_interface::CallbackReturn::SUCCESS; } @@ -110,12 +208,18 @@ std::vector RRBotTransmissionsSystemPositionOnlyHardware::export_state_interfaces() { std::vector state_interfaces; - for (uint i = 0; i < info_.joints.size(); i++) { + for (const auto & joint : info_.joints) { + /// @pre all joint interfaces exist, checked in on_init() + auto joint_interface = + std::find_if( + joint_interfaces_.begin(), joint_interfaces_.end(), + [&](const InterfaceData & interface) {return interface.name_ == joint.name;}); + state_interfaces.emplace_back( hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &joint_states_[i])); + joint.name, + hardware_interface::HW_IF_POSITION, &joint_interface->state_)); } - return state_interfaces; } @@ -123,31 +227,37 @@ std::vector RRBotTransmissionsSystemPositionOnlyHardware::export_command_interfaces() { std::vector command_interfaces; - for (uint i = 0; i < info_.joints.size(); i++) { + for (const auto & joint : info_.joints) { + /// @pre all joint interfaces exist, checked in on_init() + auto joint_interface = + std::find_if( + joint_interfaces_.begin(), joint_interfaces_.end(), + [&](const InterfaceData & interface) {return interface.name_ == joint.name;}); + command_interfaces.emplace_back( hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &joint_commands_[i])); + joint.name, + hardware_interface::HW_IF_POSITION, &joint_interface->command_)); } - return command_interfaces; } hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { - RCLCPP_INFO(*logger_, "Activating ...please wait..."); + RCLCPP_INFO(*logger_, "Activating..."); - for (int i = 0; i < hw_start_sec_; i++) { - rclcpp::sleep_for(std::chrono::seconds(1)); - RCLCPP_INFO(*logger_, "%.1f seconds left...", hw_start_sec_ - i); - } +// for (int i = 0; i < hw_start_sec_; i++) { +// rclcpp::sleep_for(std::chrono::seconds(1)); +// RCLCPP_INFO(*logger_, "%.1f seconds left...", hw_start_sec_ - i); +// } - // command and state should be equal when starting - for (uint i = 0; i < joint_states_.size(); i++) { - joint_commands_[i] = joint_states_[i]; - } +// // command and state should be equal when starting +// for (uint i = 0; i < joint_states_.size(); i++) { +// joint_commands_[i] = joint_states_[i]; +// } - RCLCPP_INFO(*logger_, "Successfully activated!"); + RCLCPP_INFO(*logger_, "Activation successful"); return hardware_interface::CallbackReturn::SUCCESS; } @@ -155,14 +265,14 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { - RCLCPP_INFO(*logger_, "Deactivating ...please wait..."); + RCLCPP_INFO(*logger_, "Deactivating..."); - for (int i = 0; i < hw_stop_sec_; i++) { - rclcpp::sleep_for(std::chrono::seconds(1)); - RCLCPP_INFO(*logger_, "%.1f seconds left...", hw_stop_sec_ - i); - } +// for (int i = 0; i < hw_stop_sec_; i++) { +// rclcpp::sleep_for(std::chrono::seconds(1)); +// RCLCPP_INFO(*logger_, "%.1f seconds left...", hw_stop_sec_ - i); +// } - RCLCPP_INFO(*logger_, "Successfully deactivated!"); + RCLCPP_INFO(*logger_, "Deactivation successful"); return hardware_interface::CallbackReturn::SUCCESS; } @@ -170,14 +280,14 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: hardware_interface::return_type RRBotTransmissionsSystemPositionOnlyHardware::read( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - RCLCPP_INFO(*logger_, "Reading..."); +// RCLCPP_INFO(*logger_, "Reading..."); - for (uint i = 0; i < joint_states_.size(); i++) { - // Simulate RRBot's movement - joint_states_[i] = joint_states_[i] + (joint_commands_[i] - joint_states_[i]) / hw_slowdown_; - RCLCPP_INFO(*logger_, "Got state %.5f for joint %d!", joint_states_[i], i); - } - RCLCPP_INFO(*logger_, "Joints successfully read!"); +// for (uint i = 0; i < joint_states_.size(); i++) { +// // Simulate RRBot's movement +// joint_states_[i] = joint_states_[i] + (joint_commands_[i] - joint_states_[i]) / hw_slowdown_; +// RCLCPP_INFO(*logger_, "Got state %.5f for joint %d!", joint_states_[i], i); +// } +// RCLCPP_INFO(*logger_, "Joints successfully read!"); return hardware_interface::return_type::OK; } @@ -185,17 +295,22 @@ hardware_interface::return_type RRBotTransmissionsSystemPositionOnlyHardware::re hardware_interface::return_type RRBotTransmissionsSystemPositionOnlyHardware::write( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - RCLCPP_INFO(*logger_, "Writing..."); +// RCLCPP_INFO(*logger_, "Writing..."); - for (uint i = 0; i < joint_commands_.size(); i++) { - // Simulate sending commands to the hardware - RCLCPP_INFO(*logger_, "Got command %.5f for joint %d!", joint_commands_[i], i); - } - RCLCPP_INFO(*logger_, "Joints successfully written!"); +// for (uint i = 0; i < joint_commands_.size(); i++) { +// // Simulate sending commands to the hardware +// RCLCPP_INFO(*logger_, "Got command %.5f for joint %d!", joint_commands_[i], i); +// } +// RCLCPP_INFO(*logger_, "Joints successfully written!"); return hardware_interface::return_type::OK; } +RRBotTransmissionsSystemPositionOnlyHardware::InterfaceData::InterfaceData(const std::string & name) +: name_(name), command_(kNaN), state_(kNaN), transmission_(kNaN) +{ +} + } // namespace ros2_control_demo_hardware #include "pluginlib/class_list_macros.hpp" From 6b30b7ed5a61dc969d1600f3cfb64deec1e49011 Mon Sep 17 00:00:00 2001 From: Jordan Palacios Date: Fri, 20 Jan 2023 11:26:08 +0100 Subject: [PATCH 10/27] Propagating data between all the interfaces --- ...bot_transmissions_system_position_only.cpp | 45 +++++++++++++++++++ 1 file changed, 45 insertions(+) diff --git a/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp b/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp index 1f2b66eab..fc17a0553 100644 --- a/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp +++ b/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp @@ -289,6 +289,25 @@ hardware_interface::return_type RRBotTransmissionsSystemPositionOnlyHardware::re // } // RCLCPP_INFO(*logger_, "Joints successfully read!"); + // actuator: state -> transmission + std::for_each( + actuator_interfaces_.begin(), actuator_interfaces_.end(), + [](auto & actuator_interface) { + actuator_interface.transmission_ = actuator_interface.state_; + }); + + // transmission: actuator -> joint + std::for_each( + transmissions_.begin(), transmissions_.end(), [](auto & transmission) { + transmission->actuator_to_joint(); + }); + + // joint: transmission -> state + std::for_each( + joint_interfaces_.begin(), joint_interfaces_.end(), [](auto & joint_interface) { + joint_interface.state_ = joint_interface.transmission_; + }); + return hardware_interface::return_type::OK; } @@ -303,6 +322,32 @@ hardware_interface::return_type RRBotTransmissionsSystemPositionOnlyHardware::wr // } // RCLCPP_INFO(*logger_, "Joints successfully written!"); + // joint: command -> transmission + std::for_each( + joint_interfaces_.begin(), joint_interfaces_.end(), [](auto & joint_interface) { + joint_interface.transmission_ = joint_interface.command_; + }); + + // transmission: joint -> actuator + std::for_each( + transmissions_.begin(), transmissions_.end(), [](auto & transmission) { + transmission->joint_to_actuator(); + }); + + // actuator: transmission -> command + std::for_each( + actuator_interfaces_.begin(), actuator_interfaces_.end(), + [](auto & actuator_interface) { + actuator_interface.command_ = actuator_interface.transmission_; + }); + + // simulate motor motion + /// @todo for now suppose a perfect actuator with infinite velocity, + /// need to add ramping + std::for_each( + actuator_interfaces_.begin(), actuator_interfaces_.end(), + [](auto & actuator_interface) {actuator_interface.state_ = actuator_interface.command_;}); + return hardware_interface::return_type::OK; } From 2d7fe62ac6b5ad33a2a0c8f0c01ba271058da3da Mon Sep 17 00:00:00 2001 From: Jordan Palacios Date: Fri, 20 Jan 2023 11:38:27 +0100 Subject: [PATCH 11/27] Cleanup. Added some consts --- ...bot_transmissions_system_position_only.hpp | 18 ------ ...bot_transmissions_system_position_only.cpp | 60 +++---------------- 2 files changed, 8 insertions(+), 70 deletions(-) diff --git a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_transmissions_system_position_only.hpp b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_transmissions_system_position_only.hpp index 9a0f165e5..03c09e050 100644 --- a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_transmissions_system_position_only.hpp +++ b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_transmissions_system_position_only.hpp @@ -75,24 +75,6 @@ class RRBotTransmissionsSystemPositionOnlyHardware : public hardware_interface:: double hw_stop_sec_; double hw_slowdown_; - /// @todo delete this - // interfaces for the simulated robot joints -// std::vector joint_commands_; -// std::vector joint_states_; - - /// @todo delete this - // interfaces for the simulated robot actuators -// std::vector actuator_commands_; -// std::vector actuator_states_; - - // interfaces for the transmission joint and actuator handles -// std::vector joint_handle_interfaces_; -// std::vector actuator_handle_interfaces_; - - /// @note no need to keep them? they sort off exist in the transmission - // joint handles - // actuator handles - // transmissions std::vector> transmissions_; diff --git a/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp b/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp index fc17a0553..0c2d519d2 100644 --- a/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp +++ b/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp @@ -129,15 +129,17 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: std::vector joint_handles; for (const auto & joint_info : transmission_info.joints) { + // this demo supports only one interface per joint if (!(joint_info.interfaces.size() == 1 && joint_info.interfaces[0] == hardware_interface::HW_IF_POSITION)) { RCLCPP_FATAL( - *logger_, "Invalid transmission joint '%s' configuration", joint_info.name.c_str()); + *logger_, "Invalid transmission joint '%s' configuration for this demo", + joint_info.name.c_str()); return hardware_interface::CallbackReturn::ERROR; } - auto joint_interface = + const auto joint_interface = joint_interfaces_.insert(joint_interfaces_.end(), InterfaceData(joint_info.name)); transmission_interface::JointHandle joint_handle(joint_info.name, @@ -147,17 +149,17 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: std::vector actuator_handles; for (const auto & actuator_info : transmission_info.actuators) { - // no check for actuators types? + // no check for actuators types - auto actuator_interface = + const auto actuator_interface = actuator_interfaces_.insert(actuator_interfaces_.end(), InterfaceData(actuator_info.name)); transmission_interface::ActuatorHandle actuator_handle(actuator_info.name, hardware_interface::HW_IF_POSITION, &actuator_interface->transmission_); actuator_handles.push_back(actuator_handle); } - /// @note no need to store the joint and actuator handles, the - /// transmission will keep whatever info it needs + /// @note no need to store the joint and actuator handles, the transmission + /// will keep whatever info it needs after is done with them /// @todo add try/catch pair for this transmission->configure(joint_handles, actuator_handles); @@ -175,18 +177,6 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: { RCLCPP_INFO(*logger_, "Configuring..."); -// for (int i = 0; i < hw_start_sec_; i++) { -// rclcpp::sleep_for(std::chrono::seconds(1)); -// RCLCPP_INFO(*logger_, "%.1f seconds left...", hw_start_sec_ - i); -// } - -// // reset values always when configuring hardware -// for (uint i = 0; i < joint_states_.size(); i++) { -// joint_states_[i] = 0; -// joint_commands_[i] = 0; -// } - - auto reset_interfaces = [](std::vector & interfaces) { for (auto & interface_data : interfaces) { @@ -246,17 +236,6 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: const rclcpp_lifecycle::State & /*previous_state*/) { RCLCPP_INFO(*logger_, "Activating..."); - -// for (int i = 0; i < hw_start_sec_; i++) { -// rclcpp::sleep_for(std::chrono::seconds(1)); -// RCLCPP_INFO(*logger_, "%.1f seconds left...", hw_start_sec_ - i); -// } - -// // command and state should be equal when starting -// for (uint i = 0; i < joint_states_.size(); i++) { -// joint_commands_[i] = joint_states_[i]; -// } - RCLCPP_INFO(*logger_, "Activation successful"); return hardware_interface::CallbackReturn::SUCCESS; @@ -266,12 +245,6 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: const rclcpp_lifecycle::State & /*previous_state*/) { RCLCPP_INFO(*logger_, "Deactivating..."); - -// for (int i = 0; i < hw_stop_sec_; i++) { -// rclcpp::sleep_for(std::chrono::seconds(1)); -// RCLCPP_INFO(*logger_, "%.1f seconds left...", hw_stop_sec_ - i); -// } - RCLCPP_INFO(*logger_, "Deactivation successful"); return hardware_interface::CallbackReturn::SUCCESS; @@ -280,15 +253,6 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: hardware_interface::return_type RRBotTransmissionsSystemPositionOnlyHardware::read( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { -// RCLCPP_INFO(*logger_, "Reading..."); - -// for (uint i = 0; i < joint_states_.size(); i++) { -// // Simulate RRBot's movement -// joint_states_[i] = joint_states_[i] + (joint_commands_[i] - joint_states_[i]) / hw_slowdown_; -// RCLCPP_INFO(*logger_, "Got state %.5f for joint %d!", joint_states_[i], i); -// } -// RCLCPP_INFO(*logger_, "Joints successfully read!"); - // actuator: state -> transmission std::for_each( actuator_interfaces_.begin(), actuator_interfaces_.end(), @@ -314,14 +278,6 @@ hardware_interface::return_type RRBotTransmissionsSystemPositionOnlyHardware::re hardware_interface::return_type RRBotTransmissionsSystemPositionOnlyHardware::write( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { -// RCLCPP_INFO(*logger_, "Writing..."); - -// for (uint i = 0; i < joint_commands_.size(); i++) { -// // Simulate sending commands to the hardware -// RCLCPP_INFO(*logger_, "Got command %.5f for joint %d!", joint_commands_[i], i); -// } -// RCLCPP_INFO(*logger_, "Joints successfully written!"); - // joint: command -> transmission std::for_each( joint_interfaces_.begin(), joint_interfaces_.end(), [](auto & joint_interface) { From 311f60d6f84224bbc1252c27c96ff9cad7513649 Mon Sep 17 00:00:00 2001 From: Jordan Palacios Date: Mon, 23 Jan 2023 11:53:49 +0100 Subject: [PATCH 12/27] Simulate motor motion --- .../src/rrbot_transmissions_system_position_only.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp b/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp index 0c2d519d2..8ac7f5b24 100644 --- a/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp +++ b/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp @@ -298,11 +298,12 @@ hardware_interface::return_type RRBotTransmissionsSystemPositionOnlyHardware::wr }); // simulate motor motion - /// @todo for now suppose a perfect actuator with infinite velocity, - /// need to add ramping std::for_each( actuator_interfaces_.begin(), actuator_interfaces_.end(), - [](auto & actuator_interface) {actuator_interface.state_ = actuator_interface.command_;}); + [&](auto & actuator_interface) { + actuator_interface.state_ = actuator_interface.state_ + + (actuator_interface.command_ - actuator_interface.state_) / hw_slowdown_; + }); return hardware_interface::return_type::OK; } From 70d74d8cc315be439e37d880cff61604e060cf78 Mon Sep 17 00:00:00 2001 From: Jordan Palacios Date: Tue, 24 Jan 2023 10:08:56 +0100 Subject: [PATCH 13/27] Added logging to periodically show interface data --- ...ns_system_position_only.ros2_control.xacro | 4 +- ...bot_transmissions_system_position_only.hpp | 2 + ...bot_transmissions_system_position_only.cpp | 54 +++++++++++++++++++ 3 files changed, 58 insertions(+), 2 deletions(-) diff --git a/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_transmissions_system_position_only.ros2_control.xacro b/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_transmissions_system_position_only.ros2_control.xacro index ffd79dbe4..d31c5abc6 100644 --- a/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_transmissions_system_position_only.ros2_control.xacro +++ b/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_transmissions_system_position_only.ros2_control.xacro @@ -44,7 +44,7 @@ transmission_interface/SimpleTransmission - 2.0 + 0.0 0.0 @@ -56,7 +56,7 @@ transmission_interface/SimpleTransmission - 4.0 + 0.0 0.0 diff --git a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_transmissions_system_position_only.hpp b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_transmissions_system_position_only.hpp index 03c09e050..7284db227 100644 --- a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_transmissions_system_position_only.hpp +++ b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_transmissions_system_position_only.hpp @@ -24,6 +24,7 @@ #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/clock.hpp" #include "rclcpp/logger.hpp" #include "rclcpp/macros.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -69,6 +70,7 @@ class RRBotTransmissionsSystemPositionOnlyHardware : public hardware_interface:: private: std::unique_ptr logger_; + std::unique_ptr clock_; // parameters for the RRBot simulation double hw_start_sec_; diff --git a/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp b/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp index 8ac7f5b24..49837abef 100644 --- a/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp +++ b/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp @@ -19,9 +19,11 @@ #include #include #include +#include #include #include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/clock.hpp" #include "rclcpp/logging.hpp" #include "transmission_interface/simple_transmission_loader.hpp" @@ -38,6 +40,8 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: rclcpp::get_logger( "RRBotTransmissionsSystemPositionOnlyHardware")); + clock_ = std::make_unique(); + RCLCPP_INFO(*logger_, "Initializing..."); if ( @@ -272,6 +276,31 @@ hardware_interface::return_type RRBotTransmissionsSystemPositionOnlyHardware::re joint_interface.state_ = joint_interface.transmission_; }); + // log state data + std::stringstream ss; + ss << "State data:"; + for (const auto & transmission_info : info_.transmissions) { + // again, this only for simple transmissions, we know there is only one joint + const auto joint_interface = std::find_if( + joint_interfaces_.cbegin(), joint_interfaces_.cend(), + [&](const auto & joint_interface) { + return joint_interface.name_ == transmission_info.joints[0].name; + }); + + const auto actuator_interface = std::find_if( + actuator_interfaces_.cbegin(), actuator_interfaces_.cend(), + [&](const auto & actuator_interface) { + return actuator_interface.name_ == transmission_info.actuators[0].name; + }); + + const auto & reduction = transmission_info.joints[0].mechanical_reduction; + + ss << std::endl << "\t" << joint_interface->name_ << ": " << joint_interface->state_ << + " <-- " << transmission_info.name << "(R=" << reduction << ") <-- " << + actuator_interface->name_ << ": " << actuator_interface->state_; + } + RCLCPP_INFO_THROTTLE(*logger_, *clock_, 1000, "%s", ss.str().c_str()); + return hardware_interface::return_type::OK; } @@ -305,6 +334,31 @@ hardware_interface::return_type RRBotTransmissionsSystemPositionOnlyHardware::wr (actuator_interface.command_ - actuator_interface.state_) / hw_slowdown_; }); + // log command data + std::stringstream ss; + ss << "Command data:"; + for (const auto & transmission_info : info_.transmissions) { + // again, this only for simple transmissions, we know there is only one joint + const auto joint_interface = std::find_if( + joint_interfaces_.cbegin(), joint_interfaces_.cend(), + [&](const auto & joint_interface) { + return joint_interface.name_ == transmission_info.joints[0].name; + }); + + const auto actuator_interface = std::find_if( + actuator_interfaces_.cbegin(), actuator_interfaces_.cend(), + [&](const auto & actuator_interface) { + return actuator_interface.name_ == transmission_info.actuators[0].name; + }); + + const auto & reduction = transmission_info.joints[0].mechanical_reduction; + + ss << std::endl << "\t" << joint_interface->name_ << ": " << joint_interface->command_ << + " --> " << transmission_info.name << "(R=" << reduction << ") --> " << + actuator_interface->name_ << ": " << actuator_interface->command_; + } + RCLCPP_INFO_THROTTLE(*logger_, *clock_, 1000, "%s", ss.str().c_str()); + return hardware_interface::return_type::OK; } From 2f095cbc49ba59e877a433558b544e0748191e7d Mon Sep 17 00:00:00 2001 From: Jordan Palacios Date: Tue, 24 Jan 2023 10:14:01 +0100 Subject: [PATCH 14/27] Refactored the parameters a bit. Removed unused ones --- ...issions_system_position_only.ros2_control.xacro | 4 +--- .../rrbot_transmissions_system_position_only.hpp | 4 +--- .../rrbot_transmissions_system_position_only.cpp | 14 ++++++-------- 3 files changed, 8 insertions(+), 14 deletions(-) diff --git a/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_transmissions_system_position_only.ros2_control.xacro b/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_transmissions_system_position_only.ros2_control.xacro index d31c5abc6..ec87f384b 100644 --- a/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_transmissions_system_position_only.ros2_control.xacro +++ b/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_transmissions_system_position_only.ros2_control.xacro @@ -19,9 +19,7 @@ ros2_control_demo_hardware/RRBotTransmissionsSystemPositionOnlyHardware - 0.0 - 3.0 - ${slowdown} + ${slowdown} diff --git a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_transmissions_system_position_only.hpp b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_transmissions_system_position_only.hpp index 7284db227..45b90f5a5 100644 --- a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_transmissions_system_position_only.hpp +++ b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_transmissions_system_position_only.hpp @@ -73,9 +73,7 @@ class RRBotTransmissionsSystemPositionOnlyHardware : public hardware_interface:: std::unique_ptr clock_; // parameters for the RRBot simulation - double hw_start_sec_; - double hw_stop_sec_; - double hw_slowdown_; + double actuator_slowdown_; // transmissions std::vector> transmissions_; diff --git a/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp b/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp index 49837abef..4394125a6 100644 --- a/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp +++ b/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp @@ -51,9 +51,7 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: return hardware_interface::CallbackReturn::ERROR; } - hw_start_sec_ = stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]); - hw_stop_sec_ = stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]); - hw_slowdown_ = stod(info_.hardware_parameters["example_param_hw_slowdown"]); + actuator_slowdown_ = std::stod(info_.hardware_parameters["actuator_slowdown"]); #ifdef UNDEF joint_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); @@ -296,8 +294,8 @@ hardware_interface::return_type RRBotTransmissionsSystemPositionOnlyHardware::re const auto & reduction = transmission_info.joints[0].mechanical_reduction; ss << std::endl << "\t" << joint_interface->name_ << ": " << joint_interface->state_ << - " <-- " << transmission_info.name << "(R=" << reduction << ") <-- " << - actuator_interface->name_ << ": " << actuator_interface->state_; + " <-- " << transmission_info.name << "(R=" << reduction << ") <-- " << + actuator_interface->name_ << ": " << actuator_interface->state_; } RCLCPP_INFO_THROTTLE(*logger_, *clock_, 1000, "%s", ss.str().c_str()); @@ -331,7 +329,7 @@ hardware_interface::return_type RRBotTransmissionsSystemPositionOnlyHardware::wr actuator_interfaces_.begin(), actuator_interfaces_.end(), [&](auto & actuator_interface) { actuator_interface.state_ = actuator_interface.state_ + - (actuator_interface.command_ - actuator_interface.state_) / hw_slowdown_; + (actuator_interface.command_ - actuator_interface.state_) / actuator_slowdown_; }); // log command data @@ -354,8 +352,8 @@ hardware_interface::return_type RRBotTransmissionsSystemPositionOnlyHardware::wr const auto & reduction = transmission_info.joints[0].mechanical_reduction; ss << std::endl << "\t" << joint_interface->name_ << ": " << joint_interface->command_ << - " --> " << transmission_info.name << "(R=" << reduction << ") --> " << - actuator_interface->name_ << ": " << actuator_interface->command_; + " --> " << transmission_info.name << "(R=" << reduction << ") --> " << + actuator_interface->name_ << ": " << actuator_interface->command_; } RCLCPP_INFO_THROTTLE(*logger_, *clock_, 1000, "%s", ss.str().c_str()); From 42cfca8d499167463b0310412b564033623834f8 Mon Sep 17 00:00:00 2001 From: Jordan Palacios Date: Tue, 24 Jan 2023 10:53:34 +0100 Subject: [PATCH 15/27] Added try/catches where needed Checking joint consistency in the description not required. Already done by the parser --- ...bot_transmissions_system_position_only.cpp | 70 ++++++------------- 1 file changed, 21 insertions(+), 49 deletions(-) diff --git a/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp b/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp index 4394125a6..226ada330 100644 --- a/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp +++ b/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp @@ -26,6 +26,8 @@ #include "rclcpp/clock.hpp" #include "rclcpp/logging.hpp" #include "transmission_interface/simple_transmission_loader.hpp" +#include "transmission_interface/transmission_interface_exception.hpp" +#include "transmission_interface/transmission.hpp" namespace ros2_control_demo_hardware { @@ -53,58 +55,15 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: actuator_slowdown_ = std::stod(info_.hardware_parameters["actuator_slowdown"]); -#ifdef UNDEF - joint_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - joint_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 - if (joint.command_interfaces.size() != 1) { - RCLCPP_FATAL( - *logger_, - "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(), - joint.command_interfaces.size()); - return hardware_interface::CallbackReturn::ERROR; - } - - if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) { - RCLCPP_FATAL( - *logger_, - "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(), - joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); - return hardware_interface::CallbackReturn::ERROR; - } - - if (joint.state_interfaces.size() != 1) { - RCLCPP_FATAL( - *logger_, - "Joint '%s' has %zu state interface. 1 expected.", joint.name.c_str(), - joint.state_interfaces.size()); - return hardware_interface::CallbackReturn::ERROR; - } - - if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) { - RCLCPP_FATAL( - *logger_, - "Joint '%s' have %s state interface. '%s' expected.", joint.name.c_str(), - joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); - return hardware_interface::CallbackReturn::ERROR; - } - } -#endif - - /// @todo check joint data info from the core interfaces is consistent - /// with the one in the transmissions, i.e., number and name of joints - const auto num_joints = std::accumulate( - info_.transmissions.begin(), info_.transmissions.end(), 0, + info_.transmissions.begin(), info_.transmissions.end(), 0ul, [](const auto & acc, const auto & trans_info) { return acc + trans_info.joints.size(); }); const auto num_actuators = std::accumulate( - info_.transmissions.begin(), info_.transmissions.end(), 0, + info_.transmissions.begin(), info_.transmissions.end(), 0ul, [](const auto & acc, const auto & trans_info) { return acc + trans_info.actuators.size(); @@ -126,8 +85,15 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: return hardware_interface::CallbackReturn::ERROR; } - /// @todo add try/catch pair for this - auto transmission = transmission_loader.load(transmission_info); + std::shared_ptr transmission; + try { + transmission = transmission_loader.load(transmission_info); + } catch (const transmission_interface::TransmissionInterfaceException & exc) { + RCLCPP_FATAL( + *logger_, "Error while loading %s: %s", + transmission_info.name.c_str(), exc.what()); + return hardware_interface::CallbackReturn::ERROR; + } std::vector joint_handles; for (const auto & joint_info : transmission_info.joints) { @@ -163,8 +129,14 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: /// @note no need to store the joint and actuator handles, the transmission /// will keep whatever info it needs after is done with them - /// @todo add try/catch pair for this - transmission->configure(joint_handles, actuator_handles); + try { + transmission->configure(joint_handles, actuator_handles); + } catch (const transmission_interface::TransmissionInterfaceException & exc) { + RCLCPP_FATAL( + *logger_, "Error while configuring %s: %s", + transmission_info.name.c_str(), exc.what()); + return hardware_interface::CallbackReturn::ERROR; + } transmissions_.push_back(transmission); } From 1cd2fb01bc7b7912baece31ec7d9d26de9cf66bd Mon Sep 17 00:00:00 2001 From: Jordan Palacios Date: Tue, 24 Jan 2023 18:18:59 +0100 Subject: [PATCH 16/27] Added transmission demo to README --- README.md | 29 +++++++++++++++++++++++++++++ 1 file changed, 29 insertions(+) diff --git a/README.md b/README.md index 88ff1c509..4b108eebb 100644 --- a/README.md +++ b/README.md @@ -530,6 +530,35 @@ Available controllers: Commanding the robot: see the commands below. +### Example 6: "Industrial Robots with an exposed transmission interface" + +Files: + - Launch file: [rrbot_transmissions_system_position_only.launch.py](ros2_control_demo_bringup/launch/rrbot_transmissions_system_position_only.launch.py) + - Controllers yaml: [rrbot_controllers.yaml](ros2_control_demo_bringup/config/rrbot_controllers.yaml) + - URDF: [rrbot_transmissions_system_position_only.urdf.xacro](ros2_control_demo_description/rrbot_description/urdf/rrbot_transmissions_system_position_only.urdf.xacro) + - `ros2_control` URDF tag: [rrbot_transmissions_system_position_only.ros2_control.xacro](ros2_control_demo_description/rrbot_description/ros2_control/rrbot_transmissions_system_position_only.ros2_control.xacro) + +Interfaces: + - Command interfaces: + - joint1/position + - joint2/position + - State interfaces: + - joint1/position + - joint2/position + +Available controllers: + - `joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster]` + - `forward_position_controller[forward_command_controller/ForwardCommandController]` (position) + +Moving the robot: + - see below description of `forward_position_controller` + +Available launch file options: + - `use_fake_hardware:=true` - start `FakeSystem` instead of hardware. + This is a simple simulation that mimics joint command to their states. + This is useful to test *ros2_control* integration and controllers without physical hardware. + + ## Controllers and moving hardware To move the robot you should load and start controllers. From 3c469c7c8735428c52b8785f1d8dea4030531a07 Mon Sep 17 00:00:00 2001 From: Jordan Palacios Date: Wed, 25 Jan 2023 12:20:20 +0000 Subject: [PATCH 17/27] Use clang-format instead --- ...bot_transmissions_system_position_only.cpp | 179 +++++++++--------- 1 file changed, 86 insertions(+), 93 deletions(-) diff --git a/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp b/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp index 226ada330..953df9e90 100644 --- a/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp +++ b/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp @@ -26,8 +26,8 @@ #include "rclcpp/clock.hpp" #include "rclcpp/logging.hpp" #include "transmission_interface/simple_transmission_loader.hpp" -#include "transmission_interface/transmission_interface_exception.hpp" #include "transmission_interface/transmission.hpp" +#include "transmission_interface/transmission_interface_exception.hpp" namespace ros2_control_demo_hardware { @@ -37,10 +37,8 @@ constexpr double kNaN = std::numeric_limits::quiet_NaN(); hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware::on_init( const hardware_interface::HardwareInfo & info) { - logger_ = - std::make_unique( - rclcpp::get_logger( - "RRBotTransmissionsSystemPositionOnlyHardware")); + logger_ = std::make_unique( + rclcpp::get_logger("RRBotTransmissionsSystemPositionOnlyHardware")); clock_ = std::make_unique(); @@ -57,17 +55,11 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: const auto num_joints = std::accumulate( info_.transmissions.begin(), info_.transmissions.end(), 0ul, - [](const auto & acc, const auto & trans_info) - { - return acc + trans_info.joints.size(); - }); + [](const auto & acc, const auto & trans_info) { return acc + trans_info.joints.size(); }); const auto num_actuators = std::accumulate( info_.transmissions.begin(), info_.transmissions.end(), 0ul, - [](const auto & acc, const auto & trans_info) - { - return acc + trans_info.actuators.size(); - }); + [](const auto & acc, const auto & trans_info) { return acc + trans_info.actuators.size(); }); // prereserve the space needed for joint and actuator interfaces joint_interfaces_.reserve(num_joints); @@ -76,9 +68,11 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: // create transmissions, joint and actuator handles auto transmission_loader = transmission_interface::SimpleTransmissionLoader(); - for (const auto & transmission_info : info_.transmissions) { + for (const auto & transmission_info : info_.transmissions) + { // only simple transmissions are supported in this demo - if (transmission_info.type != "transmission_interface/SimpleTransmission") { + if (transmission_info.type != "transmission_interface/SimpleTransmission") + { RCLCPP_FATAL( *logger_, "Transmission '%s' of type '%s' not supported in this demo", transmission_info.name.c_str(), transmission_info.type.c_str()); @@ -86,20 +80,23 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: } std::shared_ptr transmission; - try { + try + { transmission = transmission_loader.load(transmission_info); - } catch (const transmission_interface::TransmissionInterfaceException & exc) { + } + catch (const transmission_interface::TransmissionInterfaceException & exc) + { RCLCPP_FATAL( - *logger_, "Error while loading %s: %s", - transmission_info.name.c_str(), exc.what()); + *logger_, "Error while loading %s: %s", transmission_info.name.c_str(), exc.what()); return hardware_interface::CallbackReturn::ERROR; } std::vector joint_handles; - for (const auto & joint_info : transmission_info.joints) { + for (const auto & joint_info : transmission_info.joints) + { // this demo supports only one interface per joint if (!(joint_info.interfaces.size() == 1 && - joint_info.interfaces[0] == hardware_interface::HW_IF_POSITION)) + joint_info.interfaces[0] == hardware_interface::HW_IF_POSITION)) { RCLCPP_FATAL( *logger_, "Invalid transmission joint '%s' configuration for this demo", @@ -110,31 +107,34 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: const auto joint_interface = joint_interfaces_.insert(joint_interfaces_.end(), InterfaceData(joint_info.name)); - transmission_interface::JointHandle joint_handle(joint_info.name, - hardware_interface::HW_IF_POSITION, &joint_interface->transmission_); + transmission_interface::JointHandle joint_handle( + joint_info.name, hardware_interface::HW_IF_POSITION, &joint_interface->transmission_); joint_handles.push_back(joint_handle); } std::vector actuator_handles; - for (const auto & actuator_info : transmission_info.actuators) { + for (const auto & actuator_info : transmission_info.actuators) + { // no check for actuators types const auto actuator_interface = actuator_interfaces_.insert(actuator_interfaces_.end(), InterfaceData(actuator_info.name)); - transmission_interface::ActuatorHandle actuator_handle(actuator_info.name, - hardware_interface::HW_IF_POSITION, &actuator_interface->transmission_); + transmission_interface::ActuatorHandle actuator_handle( + actuator_info.name, hardware_interface::HW_IF_POSITION, &actuator_interface->transmission_); actuator_handles.push_back(actuator_handle); } /// @note no need to store the joint and actuator handles, the transmission /// will keep whatever info it needs after is done with them - try { + try + { transmission->configure(joint_handles, actuator_handles); - } catch (const transmission_interface::TransmissionInterfaceException & exc) { + } + catch (const transmission_interface::TransmissionInterfaceException & exc) + { RCLCPP_FATAL( - *logger_, "Error while configuring %s: %s", - transmission_info.name.c_str(), exc.what()); + *logger_, "Error while configuring %s: %s", transmission_info.name.c_str(), exc.what()); return hardware_interface::CallbackReturn::ERROR; } @@ -152,13 +152,14 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: RCLCPP_INFO(*logger_, "Configuring..."); auto reset_interfaces = [](std::vector & interfaces) + { + for (auto & interface_data : interfaces) { - for (auto & interface_data : interfaces) { - interface_data.command_ = 0.0; - interface_data.state_ = 0.0; - interface_data.transmission_ = kNaN; - } - }; + interface_data.command_ = 0.0; + interface_data.state_ = 0.0; + interface_data.transmission_ = kNaN; + } + }; reset_interfaces(joint_interfaces_); reset_interfaces(actuator_interfaces_); @@ -172,17 +173,15 @@ std::vector RRBotTransmissionsSystemPositionOnlyHardware::export_state_interfaces() { std::vector state_interfaces; - for (const auto & joint : info_.joints) { + for (const auto & joint : info_.joints) + { /// @pre all joint interfaces exist, checked in on_init() - auto joint_interface = - std::find_if( + auto joint_interface = std::find_if( joint_interfaces_.begin(), joint_interfaces_.end(), - [&](const InterfaceData & interface) {return interface.name_ == joint.name;}); + [&](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_)); + state_interfaces.emplace_back(hardware_interface::StateInterface( + joint.name, hardware_interface::HW_IF_POSITION, &joint_interface->state_)); } return state_interfaces; } @@ -191,17 +190,15 @@ std::vector RRBotTransmissionsSystemPositionOnlyHardware::export_command_interfaces() { std::vector command_interfaces; - for (const auto & joint : info_.joints) { + for (const auto & joint : info_.joints) + { /// @pre all joint interfaces exist, checked in on_init() - auto joint_interface = - std::find_if( + auto joint_interface = std::find_if( joint_interfaces_.begin(), joint_interfaces_.end(), - [&](const InterfaceData & interface) {return interface.name_ == joint.name;}); + [&](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_)); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + joint.name, hardware_interface::HW_IF_POSITION, &joint_interface->command_)); } return command_interfaces; } @@ -230,44 +227,41 @@ hardware_interface::return_type RRBotTransmissionsSystemPositionOnlyHardware::re // actuator: state -> transmission std::for_each( actuator_interfaces_.begin(), actuator_interfaces_.end(), - [](auto & actuator_interface) { - actuator_interface.transmission_ = actuator_interface.state_; - }); + [](auto & actuator_interface) + { actuator_interface.transmission_ = actuator_interface.state_; }); // transmission: actuator -> joint std::for_each( - transmissions_.begin(), transmissions_.end(), [](auto & transmission) { - transmission->actuator_to_joint(); - }); + transmissions_.begin(), transmissions_.end(), + [](auto & transmission) { transmission->actuator_to_joint(); }); // joint: transmission -> state std::for_each( - joint_interfaces_.begin(), joint_interfaces_.end(), [](auto & joint_interface) { - joint_interface.state_ = joint_interface.transmission_; - }); + joint_interfaces_.begin(), joint_interfaces_.end(), + [](auto & joint_interface) { joint_interface.state_ = joint_interface.transmission_; }); // log state data std::stringstream ss; ss << "State data:"; - for (const auto & transmission_info : info_.transmissions) { + for (const auto & transmission_info : info_.transmissions) + { // again, this only for simple transmissions, we know there is only one joint const auto joint_interface = std::find_if( joint_interfaces_.cbegin(), joint_interfaces_.cend(), - [&](const auto & joint_interface) { - return joint_interface.name_ == transmission_info.joints[0].name; - }); + [&](const auto & joint_interface) + { return joint_interface.name_ == transmission_info.joints[0].name; }); const auto actuator_interface = std::find_if( actuator_interfaces_.cbegin(), actuator_interfaces_.cend(), - [&](const auto & actuator_interface) { - return actuator_interface.name_ == transmission_info.actuators[0].name; - }); + [&](const auto & actuator_interface) + { return actuator_interface.name_ == transmission_info.actuators[0].name; }); const auto & reduction = transmission_info.joints[0].mechanical_reduction; - ss << std::endl << "\t" << joint_interface->name_ << ": " << joint_interface->state_ << - " <-- " << transmission_info.name << "(R=" << reduction << ") <-- " << - actuator_interface->name_ << ": " << actuator_interface->state_; + ss << std::endl + << "\t" << joint_interface->name_ << ": " << joint_interface->state_ << " <-- " + << transmission_info.name << "(R=" << reduction << ") <-- " << actuator_interface->name_ + << ": " << actuator_interface->state_; } RCLCPP_INFO_THROTTLE(*logger_, *clock_, 1000, "%s", ss.str().c_str()); @@ -279,53 +273,52 @@ hardware_interface::return_type RRBotTransmissionsSystemPositionOnlyHardware::wr { // joint: command -> transmission std::for_each( - joint_interfaces_.begin(), joint_interfaces_.end(), [](auto & joint_interface) { - joint_interface.transmission_ = joint_interface.command_; - }); + joint_interfaces_.begin(), joint_interfaces_.end(), + [](auto & joint_interface) { joint_interface.transmission_ = joint_interface.command_; }); // transmission: joint -> actuator std::for_each( - transmissions_.begin(), transmissions_.end(), [](auto & transmission) { - transmission->joint_to_actuator(); - }); + transmissions_.begin(), transmissions_.end(), + [](auto & transmission) { transmission->joint_to_actuator(); }); // actuator: transmission -> command std::for_each( actuator_interfaces_.begin(), actuator_interfaces_.end(), - [](auto & actuator_interface) { - actuator_interface.command_ = actuator_interface.transmission_; - }); + [](auto & actuator_interface) + { actuator_interface.command_ = actuator_interface.transmission_; }); // simulate motor motion std::for_each( actuator_interfaces_.begin(), actuator_interfaces_.end(), - [&](auto & actuator_interface) { - actuator_interface.state_ = actuator_interface.state_ + - (actuator_interface.command_ - actuator_interface.state_) / actuator_slowdown_; + [&](auto & actuator_interface) + { + actuator_interface.state_ = + actuator_interface.state_ + + (actuator_interface.command_ - actuator_interface.state_) / actuator_slowdown_; }); // log command data std::stringstream ss; ss << "Command data:"; - for (const auto & transmission_info : info_.transmissions) { + for (const auto & transmission_info : info_.transmissions) + { // again, this only for simple transmissions, we know there is only one joint const auto joint_interface = std::find_if( joint_interfaces_.cbegin(), joint_interfaces_.cend(), - [&](const auto & joint_interface) { - return joint_interface.name_ == transmission_info.joints[0].name; - }); + [&](const auto & joint_interface) + { return joint_interface.name_ == transmission_info.joints[0].name; }); const auto actuator_interface = std::find_if( actuator_interfaces_.cbegin(), actuator_interfaces_.cend(), - [&](const auto & actuator_interface) { - return actuator_interface.name_ == transmission_info.actuators[0].name; - }); + [&](const auto & actuator_interface) + { return actuator_interface.name_ == transmission_info.actuators[0].name; }); const auto & reduction = transmission_info.joints[0].mechanical_reduction; - ss << std::endl << "\t" << joint_interface->name_ << ": " << joint_interface->command_ << - " --> " << transmission_info.name << "(R=" << reduction << ") --> " << - actuator_interface->name_ << ": " << actuator_interface->command_; + ss << std::endl + << "\t" << joint_interface->name_ << ": " << joint_interface->command_ << " --> " + << transmission_info.name << "(R=" << reduction << ") --> " << actuator_interface->name_ + << ": " << actuator_interface->command_; } RCLCPP_INFO_THROTTLE(*logger_, *clock_, 1000, "%s", ss.str().c_str()); From 648fd0983745f32e32f58da02a809f9863fd0e88 Mon Sep 17 00:00:00 2001 From: Noel Jimenez Date: Wed, 25 Jan 2023 14:27:17 +0000 Subject: [PATCH 18/27] rename transmissions ros2_control macro --- .../rrbot_transmissions_system_position_only.ros2_control.xacro | 2 +- .../urdf/rrbot_transmissions_system_position_only.urdf.xacro | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_transmissions_system_position_only.ros2_control.xacro b/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_transmissions_system_position_only.ros2_control.xacro index ec87f384b..0ad532598 100644 --- a/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_transmissions_system_position_only.ros2_control.xacro +++ b/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_transmissions_system_position_only.ros2_control.xacro @@ -1,7 +1,7 @@ - + diff --git a/ros2_control_demo_description/rrbot_description/urdf/rrbot_transmissions_system_position_only.urdf.xacro b/ros2_control_demo_description/rrbot_description/urdf/rrbot_transmissions_system_position_only.urdf.xacro index a04bc1256..260d9c706 100644 --- a/ros2_control_demo_description/rrbot_description/urdf/rrbot_transmissions_system_position_only.urdf.xacro +++ b/ros2_control_demo_description/rrbot_description/urdf/rrbot_transmissions_system_position_only.urdf.xacro @@ -37,7 +37,7 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc - Date: Thu, 26 Jan 2023 10:36:28 +0100 Subject: [PATCH 19/27] Remove unnecessary actuator parameters --- ...ansmissions_system_position_only.ros2_control.xacro | 10 ++-------- 1 file changed, 2 insertions(+), 8 deletions(-) diff --git a/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_transmissions_system_position_only.ros2_control.xacro b/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_transmissions_system_position_only.ros2_control.xacro index 0ad532598..b7c25f015 100644 --- a/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_transmissions_system_position_only.ros2_control.xacro +++ b/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_transmissions_system_position_only.ros2_control.xacro @@ -41,10 +41,7 @@ transmission_interface/SimpleTransmission - - 0.0 - 0.0 - + 2.0 0.0 @@ -53,10 +50,7 @@ transmission_interface/SimpleTransmission - - 0.0 - 0.0 - + 4.0 0.0 From 8afa0767245d7d9cffa0e4c60e45c91d4fed5979 Mon Sep 17 00:00:00 2001 From: Jordan Palacios Date: Thu, 2 Feb 2023 14:36:35 +0100 Subject: [PATCH 20/27] Typo --- ros2_control_demo_hardware/ros2_control_demo_hardware.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros2_control_demo_hardware/ros2_control_demo_hardware.xml b/ros2_control_demo_hardware/ros2_control_demo_hardware.xml index 6962cef61..d03be0eec 100644 --- a/ros2_control_demo_hardware/ros2_control_demo_hardware.xml +++ b/ros2_control_demo_hardware/ros2_control_demo_hardware.xml @@ -45,7 +45,7 @@ type="ros2_control_demo_hardware::RRBotTransmissionsSystemPositionOnlyHardware" base_class_type="hardware_interface::SystemInterface"> - The ROS2 Control minimal robot protocol with trasmissions. + The ROS2 Control minimal robot protocol with transmissions. From 90fec586e3a18687815a4da0036e6cddce3c9d40 Mon Sep 17 00:00:00 2001 From: Jordan Palacios Date: Thu, 2 Feb 2023 15:09:36 +0100 Subject: [PATCH 21/27] Removed gazebo, mock hardware and fake sensors from the transmission demo --- README.md | 5 ---- ...ansmissions_system_position_only.launch.py | 19 --------------- ...ns_system_position_only.ros2_control.xacro | 23 ++++--------------- 3 files changed, 5 insertions(+), 42 deletions(-) diff --git a/README.md b/README.md index 4b108eebb..5767d246e 100644 --- a/README.md +++ b/README.md @@ -553,11 +553,6 @@ Available controllers: Moving the robot: - see below description of `forward_position_controller` -Available launch file options: - - `use_fake_hardware:=true` - start `FakeSystem` instead of hardware. - This is a simple simulation that mimics joint command to their states. - This is useful to test *ros2_control* integration and controllers without physical hardware. - ## Controllers and moving hardware diff --git a/ros2_control_demo_bringup/launch/rrbot_transmissions_system_position_only.launch.py b/ros2_control_demo_bringup/launch/rrbot_transmissions_system_position_only.launch.py index 3ca29b4c7..6d3d0dca3 100644 --- a/ros2_control_demo_bringup/launch/rrbot_transmissions_system_position_only.launch.py +++ b/ros2_control_demo_bringup/launch/rrbot_transmissions_system_position_only.launch.py @@ -30,21 +30,6 @@ def generate_launch_description(): have to be updated.", ) ) - declared_arguments.append( - DeclareLaunchArgument( - "use_fake_hardware", - default_value="false", - description="Start robot with fake hardware mirroring command to its states.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "mock_sensor_commands", - default_value="false", - description="Enable fake command interfaces for sensors used for simple simulations. \ - Used only if 'use_fake_hardware' parameter is true.", - ) - ) declared_arguments.append( DeclareLaunchArgument( "slowdown", default_value="50.0", description="Slowdown factor of the RRbot." @@ -67,8 +52,6 @@ def generate_launch_description(): # Initialize Arguments prefix = LaunchConfiguration("prefix") - use_fake_hardware = LaunchConfiguration("use_fake_hardware") - mock_sensor_commands = LaunchConfiguration("mock_sensor_commands") slowdown = LaunchConfiguration("slowdown") robot_controller = LaunchConfiguration("robot_controller") start_rviz = LaunchConfiguration("start_rviz") @@ -78,8 +61,6 @@ def generate_launch_description(): launch_arguments={ "description_file": "rrbot_transmissions_system_position_only.urdf.xacro", "prefix": prefix, - "use_fake_hardware": use_fake_hardware, - "mock_sensor_commands": mock_sensor_commands, "slowdown": slowdown, "robot_controller": robot_controller, "start_rviz": start_rviz, diff --git a/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_transmissions_system_position_only.ros2_control.xacro b/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_transmissions_system_position_only.ros2_control.xacro index b7c25f015..590b21c9f 100644 --- a/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_transmissions_system_position_only.ros2_control.xacro +++ b/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_transmissions_system_position_only.ros2_control.xacro @@ -5,24 +5,10 @@ - - - gazebo_ros2_control/GazeboSystem - - - - - - mock_components/GenericSystem - ${mock_sensor_commands} - 0.0 - - - ros2_control_demo_hardware/RRBotTransmissionsSystemPositionOnlyHardware - ${slowdown} - - - + + ros2_control_demo_hardware/RRBotTransmissionsSystemPositionOnlyHardware + ${slowdown} + @@ -31,6 +17,7 @@ + -1 From af5d9619f3e535ed2a84587ae866c8ffda719b74 Mon Sep 17 00:00:00 2001 From: Jordan Palacios Date: Thu, 2 Feb 2023 15:16:45 +0100 Subject: [PATCH 22/27] Missing 'explicit' from constructor --- .../rrbot_transmissions_system_position_only.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_transmissions_system_position_only.hpp b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_transmissions_system_position_only.hpp index 45b90f5a5..e2ee13384 100644 --- a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_transmissions_system_position_only.hpp +++ b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_transmissions_system_position_only.hpp @@ -80,7 +80,7 @@ class RRBotTransmissionsSystemPositionOnlyHardware : public hardware_interface:: struct InterfaceData { - InterfaceData(const std::string & name); + explicit InterfaceData(const std::string & name); std::string name_; double command_; From 727b0799c35fe77e61b7ac03d74e606c43bac3bd Mon Sep 17 00:00:00 2001 From: Jordan Palacios Date: Fri, 3 Feb 2023 08:40:53 +0000 Subject: [PATCH 23/27] Further cleanup of gazebo, mock hardware and fake sensors from the transmission demo --- .../rrbot_transmissions_system_position_only.urdf.xacro | 8 -------- 1 file changed, 8 deletions(-) diff --git a/ros2_control_demo_description/rrbot_description/urdf/rrbot_transmissions_system_position_only.urdf.xacro b/ros2_control_demo_description/rrbot_description/urdf/rrbot_transmissions_system_position_only.urdf.xacro index 260d9c706..f9fd6347c 100644 --- a/ros2_control_demo_description/rrbot_description/urdf/rrbot_transmissions_system_position_only.urdf.xacro +++ b/ros2_control_demo_description/rrbot_description/urdf/rrbot_transmissions_system_position_only.urdf.xacro @@ -5,20 +5,14 @@ Copied and modified from ROS1 example - https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_description/urdf/rrbot.xacro --> - - - - - - @@ -35,8 +29,6 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc - - Date: Wed, 15 Feb 2023 07:11:03 +0000 Subject: [PATCH 24/27] branding fix --- ros2_control_demo_hardware/ros2_control_demo_hardware.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros2_control_demo_hardware/ros2_control_demo_hardware.xml b/ros2_control_demo_hardware/ros2_control_demo_hardware.xml index d03be0eec..1456dc50a 100644 --- a/ros2_control_demo_hardware/ros2_control_demo_hardware.xml +++ b/ros2_control_demo_hardware/ros2_control_demo_hardware.xml @@ -45,7 +45,7 @@ type="ros2_control_demo_hardware::RRBotTransmissionsSystemPositionOnlyHardware" base_class_type="hardware_interface::SystemInterface"> - The ROS2 Control minimal robot protocol with transmissions. + The ros2_control minimal robot protocol with transmissions. From 79b82727155609af22c02aa60481d61f31fc7524 Mon Sep 17 00:00:00 2001 From: Jordan Palacios Date: Wed, 15 Feb 2023 09:28:40 +0100 Subject: [PATCH 25/27] Remove pointer definitions Co-authored-by: Bence Magyar --- .../rrbot_transmissions_system_position_only.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_transmissions_system_position_only.hpp b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_transmissions_system_position_only.hpp index e2ee13384..734a79391 100644 --- a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_transmissions_system_position_only.hpp +++ b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_transmissions_system_position_only.hpp @@ -36,7 +36,6 @@ namespace ros2_control_demo_hardware class RRBotTransmissionsSystemPositionOnlyHardware : public hardware_interface::SystemInterface { public: - RCLCPP_SHARED_PTR_DEFINITIONS(RRBotTransmissionsSystemPositionOnlyHardware); ROS2_CONTROL_DEMO_HARDWARE_PUBLIC hardware_interface::CallbackReturn on_init( From 0b00f3577ebdc870efd3a1f2330530b8282755d1 Mon Sep 17 00:00:00 2001 From: Jordan Palacios Date: Wed, 15 Feb 2023 09:40:02 +0100 Subject: [PATCH 26/27] Fixed comment about reserve Co-authored-by: Bence Magyar --- .../src/rrbot_transmissions_system_position_only.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp b/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp index 953df9e90..c2f981593 100644 --- a/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp +++ b/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp @@ -61,7 +61,7 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: info_.transmissions.begin(), info_.transmissions.end(), 0ul, [](const auto & acc, const auto & trans_info) { return acc + trans_info.actuators.size(); }); - // prereserve the space needed for joint and actuator interfaces + // reserve the space needed for joint and actuator data structures joint_interfaces_.reserve(num_joints); actuator_interfaces_.reserve(num_actuators); From 4e98b53031a98fa3b6891502b3c84e9c930c9b0f Mon Sep 17 00:00:00 2001 From: Jordan Palacios Date: Wed, 15 Feb 2023 14:27:38 +0100 Subject: [PATCH 27/27] Renamed member variable and added description --- ...bot_transmissions_system_position_only.hpp | 5 +++-- ...bot_transmissions_system_position_only.cpp | 20 +++++++++++-------- 2 files changed, 15 insertions(+), 10 deletions(-) diff --git a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_transmissions_system_position_only.hpp b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_transmissions_system_position_only.hpp index 734a79391..80e3692f5 100644 --- a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_transmissions_system_position_only.hpp +++ b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_transmissions_system_position_only.hpp @@ -36,7 +36,6 @@ namespace ros2_control_demo_hardware class RRBotTransmissionsSystemPositionOnlyHardware : public hardware_interface::SystemInterface { public: - ROS2_CONTROL_DEMO_HARDWARE_PUBLIC hardware_interface::CallbackReturn on_init( const hardware_interface::HardwareInfo & info) override; @@ -84,7 +83,9 @@ class RRBotTransmissionsSystemPositionOnlyHardware : public hardware_interface:: std::string name_; double command_; double state_; - double transmission_; + + // this is the "sink" that will be part of the transmission Joint/Actuator handles + double transmission_passthrough_; }; std::vector joint_interfaces_; std::vector actuator_interfaces_; diff --git a/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp b/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp index c2f981593..929a39618 100644 --- a/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp +++ b/ros2_control_demo_hardware/src/rrbot_transmissions_system_position_only.cpp @@ -108,7 +108,8 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: joint_interfaces_.insert(joint_interfaces_.end(), InterfaceData(joint_info.name)); transmission_interface::JointHandle joint_handle( - joint_info.name, hardware_interface::HW_IF_POSITION, &joint_interface->transmission_); + joint_info.name, hardware_interface::HW_IF_POSITION, + &joint_interface->transmission_passthrough_); joint_handles.push_back(joint_handle); } @@ -120,7 +121,8 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: const auto actuator_interface = actuator_interfaces_.insert(actuator_interfaces_.end(), InterfaceData(actuator_info.name)); transmission_interface::ActuatorHandle actuator_handle( - actuator_info.name, hardware_interface::HW_IF_POSITION, &actuator_interface->transmission_); + actuator_info.name, hardware_interface::HW_IF_POSITION, + &actuator_interface->transmission_passthrough_); actuator_handles.push_back(actuator_handle); } @@ -157,7 +159,7 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: { interface_data.command_ = 0.0; interface_data.state_ = 0.0; - interface_data.transmission_ = kNaN; + interface_data.transmission_passthrough_ = kNaN; } }; @@ -228,7 +230,7 @@ hardware_interface::return_type RRBotTransmissionsSystemPositionOnlyHardware::re std::for_each( actuator_interfaces_.begin(), actuator_interfaces_.end(), [](auto & actuator_interface) - { actuator_interface.transmission_ = actuator_interface.state_; }); + { actuator_interface.transmission_passthrough_ = actuator_interface.state_; }); // transmission: actuator -> joint std::for_each( @@ -238,7 +240,8 @@ hardware_interface::return_type RRBotTransmissionsSystemPositionOnlyHardware::re // joint: transmission -> state std::for_each( joint_interfaces_.begin(), joint_interfaces_.end(), - [](auto & joint_interface) { joint_interface.state_ = joint_interface.transmission_; }); + [](auto & joint_interface) + { joint_interface.state_ = joint_interface.transmission_passthrough_; }); // log state data std::stringstream ss; @@ -274,7 +277,8 @@ hardware_interface::return_type RRBotTransmissionsSystemPositionOnlyHardware::wr // joint: command -> transmission std::for_each( joint_interfaces_.begin(), joint_interfaces_.end(), - [](auto & joint_interface) { joint_interface.transmission_ = joint_interface.command_; }); + [](auto & joint_interface) + { joint_interface.transmission_passthrough_ = joint_interface.command_; }); // transmission: joint -> actuator std::for_each( @@ -285,7 +289,7 @@ hardware_interface::return_type RRBotTransmissionsSystemPositionOnlyHardware::wr std::for_each( actuator_interfaces_.begin(), actuator_interfaces_.end(), [](auto & actuator_interface) - { actuator_interface.command_ = actuator_interface.transmission_; }); + { actuator_interface.command_ = actuator_interface.transmission_passthrough_; }); // simulate motor motion std::for_each( @@ -326,7 +330,7 @@ hardware_interface::return_type RRBotTransmissionsSystemPositionOnlyHardware::wr } RRBotTransmissionsSystemPositionOnlyHardware::InterfaceData::InterfaceData(const std::string & name) -: name_(name), command_(kNaN), state_(kNaN), transmission_(kNaN) +: name_(name), command_(kNaN), state_(kNaN), transmission_passthrough_(kNaN) { }