From 1dd221a456699ebbdeea2ce48f836410dbef0328 Mon Sep 17 00:00:00 2001 From: Sanjeev <62834697+kumar-sanjeeev@users.noreply.github.com> Date: Fri, 20 Dec 2024 21:45:15 +0100 Subject: [PATCH 1/4] Update header files of realtime_tools pkg (#676) --- .../include/passthrough_controller/passthrough_controller.hpp | 2 +- .../rrbot_sensor_for_position_feedback.hpp | 2 +- .../include/ros2_control_demo_example_7/r6bot_controller.hpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/example_12/controllers/include/passthrough_controller/passthrough_controller.hpp b/example_12/controllers/include/passthrough_controller/passthrough_controller.hpp index 3fdb3ffa3..cea7233b0 100644 --- a/example_12/controllers/include/passthrough_controller/passthrough_controller.hpp +++ b/example_12/controllers/include/passthrough_controller/passthrough_controller.hpp @@ -22,7 +22,7 @@ #include #include "controller_interface/chainable_controller_interface.hpp" -#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_buffer.hpp" #include "std_msgs/msg/float64_multi_array.hpp" // auto-generated by generate_parameter_library #include "passthrough_controller_parameters.hpp" diff --git a/example_14/hardware/include/ros2_control_demo_example_14/rrbot_sensor_for_position_feedback.hpp b/example_14/hardware/include/ros2_control_demo_example_14/rrbot_sensor_for_position_feedback.hpp index 5644d714d..50aafa93c 100644 --- a/example_14/hardware/include/ros2_control_demo_example_14/rrbot_sensor_for_position_feedback.hpp +++ b/example_14/hardware/include/ros2_control_demo_example_14/rrbot_sensor_for_position_feedback.hpp @@ -33,7 +33,7 @@ #include "rclcpp/clock.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/time.hpp" -#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_buffer.hpp" namespace ros2_control_demo_example_14 { diff --git a/example_7/controller/include/ros2_control_demo_example_7/r6bot_controller.hpp b/example_7/controller/include/ros2_control_demo_example_7/r6bot_controller.hpp index b01cd9592..ee8c24f96 100644 --- a/example_7/controller/include/ros2_control_demo_example_7/r6bot_controller.hpp +++ b/example_7/controller/include/ros2_control_demo_example_7/r6bot_controller.hpp @@ -33,7 +33,7 @@ #include "rclcpp/timer.hpp" #include "rclcpp_lifecycle/lifecycle_publisher.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" -#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_buffer.hpp" #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" From 1b1c2e5817c02dc3adf8a6c769714096c2a6f0ac Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sat, 21 Dec 2024 10:45:26 +0100 Subject: [PATCH 2/4] Remove unused robot_description parameter (#668) --- example_13/bringup/launch/three_robots.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/example_13/bringup/launch/three_robots.launch.py b/example_13/bringup/launch/three_robots.launch.py index e9a1679cc..39a3cfee3 100644 --- a/example_13/bringup/launch/three_robots.launch.py +++ b/example_13/bringup/launch/three_robots.launch.py @@ -88,7 +88,7 @@ def generate_launch_description(): control_node = Node( package="controller_manager", executable="ros2_control_node", - parameters=[robot_description, robot_controllers], + parameters=[robot_controllers], ) robot_state_pub_node = Node( package="robot_state_publisher", From e23a12a68914604dc98ec029304e90d98d229983 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sat, 21 Dec 2024 21:03:00 +0100 Subject: [PATCH 3/4] Use new spawner option for ros args (#577) Co-authored-by: Sai Kishor Kothakota --- example_11/bringup/launch/carlikebot.launch.py | 11 +++++++---- example_11/test/test_carlikebot_launch.py | 10 ++++++++++ .../bringup/launch/rrbot_namespace.launch.py | 16 ++++++++-------- example_2/bringup/launch/diffbot.launch.py | 11 +++++++---- 4 files changed, 32 insertions(+), 16 deletions(-) diff --git a/example_11/bringup/launch/carlikebot.launch.py b/example_11/bringup/launch/carlikebot.launch.py index 47c3c3327..13054a298 100644 --- a/example_11/bringup/launch/carlikebot.launch.py +++ b/example_11/bringup/launch/carlikebot.launch.py @@ -77,9 +77,6 @@ def generate_launch_description(): executable="ros2_control_node", parameters=[robot_controllers], output="both", - remappings=[ - ("/bicycle_steering_controller/tf_odometry", "/tf"), - ], condition=IfCondition(remap_odometry_tf), ) control_node = Node( @@ -114,7 +111,13 @@ def generate_launch_description(): robot_bicycle_controller_spawner = Node( package="controller_manager", executable="spawner", - arguments=["bicycle_steering_controller", "--param-file", robot_controllers], + arguments=[ + "bicycle_steering_controller", + "--param-file", + robot_controllers, + "--controller-ros-args", + "-r /bicycle_steering_controller/tf_odometry:=/tf", + ], ) # Delay rviz start after `joint_state_broadcaster` diff --git a/example_11/test/test_carlikebot_launch.py b/example_11/test/test_carlikebot_launch.py index 1afb4ff33..1b45ec0d8 100644 --- a/example_11/test/test_carlikebot_launch.py +++ b/example_11/test/test_carlikebot_launch.py @@ -37,6 +37,7 @@ from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_testing.actions import ReadyToTest +from launch_testing_ros import WaitForTopics # import launch_testing.markers import rclpy @@ -46,6 +47,8 @@ check_node_running, ) +from tf2_msgs.msg import TFMessage + # Executes the given launch file and checks if all nodes can be started @pytest.mark.rostest @@ -98,6 +101,13 @@ def test_check_if_msgs_published(self): ], ) + def test_remapped_topic(self): + # we don't want to implement a tf lookup here + # so just check if the unmapped topic is not published + old_topic = "/bicycle_steering_controller/tf_odometry" + wait_for_topics = WaitForTopics([(old_topic, TFMessage)]) + assert not wait_for_topics.wait(), f"Topic '{old_topic}' found, but should be remapped!" + # TODO(anyone): enable this if shutdown of ros2_control_node does not fail anymore # @launch_testing.post_shutdown_test() diff --git a/example_15/bringup/launch/rrbot_namespace.launch.py b/example_15/bringup/launch/rrbot_namespace.launch.py index 537bdb208..1544f3657 100644 --- a/example_15/bringup/launch/rrbot_namespace.launch.py +++ b/example_15/bringup/launch/rrbot_namespace.launch.py @@ -69,13 +69,6 @@ def generate_launch_description(): executable="ros2_control_node", namespace="/rrbot", parameters=[robot_description, robot_controllers], - remappings=[ - ( - # we use the remapping from a relative name to FQN /position_commands - "forward_position_controller/commands", - "/position_commands", - ), - ], output={ "stdout": "screen", "stderr": "screen", @@ -108,7 +101,14 @@ def generate_launch_description(): package="controller_manager", executable="spawner", namespace="rrbot", - arguments=["forward_position_controller", "--param-file", robot_controllers], + arguments=[ + "forward_position_controller", + "--param-file", + robot_controllers, + # we use the remapping from a relative name to FQN /position_commands + "--controller-ros-args", + "-r forward_position_controller/commands:=/position_commands", + ], ) robot_position_trajectory_controller_spawner = Node( diff --git a/example_2/bringup/launch/diffbot.launch.py b/example_2/bringup/launch/diffbot.launch.py index 1c345464f..797ea5fd8 100644 --- a/example_2/bringup/launch/diffbot.launch.py +++ b/example_2/bringup/launch/diffbot.launch.py @@ -75,9 +75,6 @@ def generate_launch_description(): executable="ros2_control_node", parameters=[robot_controllers], output="both", - remappings=[ - ("/diffbot_base_controller/cmd_vel", "/cmd_vel"), - ], ) robot_state_pub_node = Node( package="robot_state_publisher", @@ -103,7 +100,13 @@ def generate_launch_description(): robot_controller_spawner = Node( package="controller_manager", executable="spawner", - arguments=["diffbot_base_controller", "--param-file", robot_controllers], + arguments=[ + "diffbot_base_controller", + "--param-file", + robot_controllers, + "--controller-ros-args", + "-r /diffbot_base_controller/cmd_vel:=/cmd_vel", + ], ) # Delay rviz start after `joint_state_broadcaster` From 27f529136053d2e65fffd3c1b6373ca901771613 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 25 Dec 2024 20:42:51 +0000 Subject: [PATCH 4/4] Remove the use of visibility macros (#679) --- .../ros2_control_demo_example_7/r6bot_controller.hpp | 11 ----------- example_7/doc/userdoc.rst | 2 +- 2 files changed, 1 insertion(+), 12 deletions(-) diff --git a/example_7/controller/include/ros2_control_demo_example_7/r6bot_controller.hpp b/example_7/controller/include/ros2_control_demo_example_7/r6bot_controller.hpp index ee8c24f96..9c02c6c1d 100644 --- a/example_7/controller/include/ros2_control_demo_example_7/r6bot_controller.hpp +++ b/example_7/controller/include/ros2_control_demo_example_7/r6bot_controller.hpp @@ -42,43 +42,32 @@ namespace ros2_control_demo_example_7 class RobotController : public controller_interface::ControllerInterface { public: - CONTROLLER_INTERFACE_PUBLIC RobotController(); - CONTROLLER_INTERFACE_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override; - CONTROLLER_INTERFACE_PUBLIC controller_interface::InterfaceConfiguration state_interface_configuration() const override; - CONTROLLER_INTERFACE_PUBLIC controller_interface::return_type update( const rclcpp::Time & time, const rclcpp::Duration & period) override; - CONTROLLER_INTERFACE_PUBLIC controller_interface::CallbackReturn on_init() override; - CONTROLLER_INTERFACE_PUBLIC controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; - CONTROLLER_INTERFACE_PUBLIC controller_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; - CONTROLLER_INTERFACE_PUBLIC controller_interface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & previous_state) override; - CONTROLLER_INTERFACE_PUBLIC controller_interface::CallbackReturn on_cleanup( const rclcpp_lifecycle::State & previous_state) override; - CONTROLLER_INTERFACE_PUBLIC controller_interface::CallbackReturn on_error( const rclcpp_lifecycle::State & previous_state) override; - CONTROLLER_INTERFACE_PUBLIC controller_interface::CallbackReturn on_shutdown( const rclcpp_lifecycle::State & previous_state) override; diff --git a/example_7/doc/userdoc.rst b/example_7/doc/userdoc.rst index c2a4e6b58..580bcfeee 100644 --- a/example_7/doc/userdoc.rst +++ b/example_7/doc/userdoc.rst @@ -207,7 +207,7 @@ There are more methods that can be implemented for lifecycle changes, but they a using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; #include "hardware_interface/types/hardware_interface_return_values.hpp" - class HARDWARE_INTERFACE_PUBLIC RobotSystem : public hardware_interface::SystemInterface { + class RobotSystem : public hardware_interface::SystemInterface { public: CallbackReturn on_init(const hardware_interface::HardwareInfo &info) override; CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override;