From 790da575aad1228380ba0aa782083014d3128d69 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 13 Oct 2025 20:20:44 +0000 Subject: [PATCH 01/29] Update spawner syntax --- .../test/control.launch.py | 3 ++- .../test/ros2_controllers.yaml | 9 +++++---- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/joint_state_topic_hardware_interface/test/control.launch.py b/joint_state_topic_hardware_interface/test/control.launch.py index 4dd127c..5d315f4 100644 --- a/joint_state_topic_hardware_interface/test/control.launch.py +++ b/joint_state_topic_hardware_interface/test/control.launch.py @@ -47,7 +47,8 @@ def generate_launch_description(): Node( package="controller_manager", executable="spawner", - arguments=[controller], + parameters=[ros2_controllers_file], + arguments=[controller, "--param-file", ros2_controllers_file], ) for controller in controllers ], diff --git a/joint_state_topic_hardware_interface/test/ros2_controllers.yaml b/joint_state_topic_hardware_interface/test/ros2_controllers.yaml index acdf09c..1bfb07d 100644 --- a/joint_state_topic_hardware_interface/test/ros2_controllers.yaml +++ b/joint_state_topic_hardware_interface/test/ros2_controllers.yaml @@ -1,13 +1,14 @@ controller_manager: ros__parameters: update_rate: 50 - joint_state_broadcaster: - type: joint_state_broadcaster/JointStateBroadcaster - joint_trajectory_controller: - type: joint_trajectory_controller/JointTrajectoryController + +joint_state_broadcaster: + ros__parameters: + type: joint_state_broadcaster/JointStateBroadcaster joint_trajectory_controller: ros__parameters: + type: joint_trajectory_controller/JointTrajectoryController joints: - joint_1 - joint_2 From cda89fcddccfb435d854db010692b69995472a5a Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 13 Oct 2025 20:20:58 +0000 Subject: [PATCH 02/29] Use proper integration test --- .../CMakeLists.txt | 33 ++++---- .../test/joint_state_topic_based_robot.py | 12 +-- .../test/ros2_control.test.py | 79 ++++++++++++++----- .../test/test_topic_based_robot.py | 35 -------- 4 files changed, 84 insertions(+), 75 deletions(-) delete mode 100755 joint_state_topic_hardware_interface/test/test_topic_based_robot.py diff --git a/joint_state_topic_hardware_interface/CMakeLists.txt b/joint_state_topic_hardware_interface/CMakeLists.txt index 993fcee..a088413 100644 --- a/joint_state_topic_hardware_interface/CMakeLists.txt +++ b/joint_state_topic_hardware_interface/CMakeLists.txt @@ -59,27 +59,26 @@ install( if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) - find_package(ament_cmake_pytest REQUIRED) find_package(ros2_control_test_assets REQUIRED) - find_package(ros_testing REQUIRED) - ament_add_gmock(joint_state_topic_hardware_interface_test test/joint_state_topic_hardware_interface_test.cpp) - target_link_libraries(joint_state_topic_hardware_interface_test - ${PROJECT_NAME} - angles::angles - rclcpp::rclcpp - hardware_interface::hardware_interface - ${sensor_msgs_TARGETS} - ros2_control_test_assets::ros2_control_test_assets) + # ament_add_gmock(joint_state_topic_hardware_interface_test test/joint_state_topic_hardware_interface_test.cpp) + # target_link_libraries(joint_state_topic_hardware_interface_test + # ${PROJECT_NAME} + # angles::angles + # rclcpp::rclcpp + # hardware_interface::hardware_interface + # ${sensor_msgs_TARGETS} + # ros2_control_test_assets::ros2_control_test_assets) # Integration tests - # TODO(anyone): reactivate if https://github.com/ros-controls/topic_based_hardware_interfaces/issues/21 is fixed - # add_ros_test( - # test/ros2_control.test.py - # TIMEOUT - # 120 - # ARGS - # test_file:=${CMAKE_CURRENT_SOURCE_DIR}/test/test_topic_based_robot.py) + find_package(ament_cmake_ros REQUIRED) + find_package(launch_testing_ament_cmake REQUIRED) + function(add_ros_isolated_launch_test path) + set(RUNNER "${ament_cmake_ros_DIR}/run_test_isolated.py") + add_launch_test("${path}" RUNNER "${RUNNER}" ${ARGN}) + endfunction() + add_ros_isolated_launch_test( + test/ros2_control.test.py) endif() pluginlib_export_plugin_description_file(hardware_interface joint_state_topic_hardware_interface_plugin_description.xml) diff --git a/joint_state_topic_hardware_interface/test/joint_state_topic_based_robot.py b/joint_state_topic_hardware_interface/test/joint_state_topic_based_robot.py index af417c1..45b32b7 100644 --- a/joint_state_topic_hardware_interface/test/joint_state_topic_based_robot.py +++ b/joint_state_topic_hardware_interface/test/joint_state_topic_based_robot.py @@ -39,7 +39,7 @@ def __init__(self, joint_names: list[str]) -> None: qos_profile=qos_profile_sensor_data, callback_group=self.callback_group, ) - # Subscriber for the desired joint state from the controller + # Subscriber for the joint state commands from ros2_control self.desired_joint_state_subscriber = self.create_subscription( JointState, "topic_based_joint_commands", @@ -47,7 +47,7 @@ def __init__(self, joint_names: list[str]) -> None: QoSProfile(depth=1), callback_group=self.callback_group, ) - # Reported joint state from ros2_control + # Reported joint state from joint_state_broadcaster self.current_joint_state_subscriber = self.create_subscription( JointState, "joint_states", @@ -82,7 +82,7 @@ def get_current_joint_command(self) -> OrderedDict[str, float]: throttle_duration_sec=2.0, skip_first=True, ) - rclpy.spin_once(self, timeout_sec=0.0) + rclpy.spin_once(self, timeout_sec=1.0) return self.last_joint_command def get_current_joint_state(self) -> OrderedDict[str, float]: @@ -94,7 +94,7 @@ def get_current_joint_state(self) -> OrderedDict[str, float]: throttle_duration_sec=2.0, skip_first=True, ) - rclpy.spin_once(self, timeout_sec=0.0) + rclpy.spin_once(self, timeout_sec=1.0) return self.current_joint_state def set_joint_positions( @@ -113,4 +113,6 @@ def set_joint_positions( joint_positions, atol=1e-3, ): - rclpy.spin_once(self, timeout_sec=0.0) + self.get_logger().warning( + f"Waiting for current joint states from topic '{self.current_joint_state_subscriber.topic_name}': {self.get_current_joint_state()} to be equal to the commanded positions: {joint_positions}...", + ) diff --git a/joint_state_topic_hardware_interface/test/ros2_control.test.py b/joint_state_topic_hardware_interface/test/ros2_control.test.py index ce78780..75be396 100644 --- a/joint_state_topic_hardware_interface/test/ros2_control.test.py +++ b/joint_state_topic_hardware_interface/test/ros2_control.test.py @@ -24,20 +24,34 @@ IncludeLaunchDescription, TimerAction, ) + from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import ( LaunchConfiguration, PathJoinSubstitution, ) -from launch_ros.actions import Node - +from launch_testing.actions import ReadyToTest +from launch_testing.util import KeepAliveProc +import launch_testing.markers +import pytest +import rclpy +import sys +# JointStateTopicBasedRobot is in the same folder as this test +sys.path.insert(0, str(Path(__file__).parent)) +from joint_state_topic_based_robot import JointStateTopicBasedRobot SCRIPT_PATH = Path(os.path.realpath(__file__)).parent +from controller_manager.test_utils import ( + check_controllers_running, + check_if_js_published, +) +# This function specifies the processes to be run for our test +@pytest.mark.rostest def generate_test_description(): - test_node = Node( - executable=LaunchConfiguration("test_file"), - ) + # This is necessary to get unbuffered output from the process under test + proc_env = os.environ.copy() + proc_env['PYTHONUNBUFFERED'] = '1' return LaunchDescription( [ @@ -53,24 +67,53 @@ def generate_test_description(): ], ), ), - DeclareLaunchArgument("test_file"), - TimerAction(period=2.0, actions=[test_node]), - launch_testing.util.KeepAliveProc(), - launch_testing.actions.ReadyToTest(), + KeepAliveProc(), + ReadyToTest(), ], - ), { - "test_node": test_node, - } + ) + + +class TestFixture(unittest.TestCase): + @classmethod + def setUpClass(cls): + rclpy.init() + + @classmethod + def tearDownClass(cls): + rclpy.shutdown() + + def setUp(self): + self.robot = JointStateTopicBasedRobot(["joint_1", "joint_2", "joint_3"]) + + def tearDown(self): + self.robot.destroy_node() + + def test_controller_running(self, proc_output): + cnames = ["joint_trajectory_controller", "joint_state_broadcaster"] + check_controllers_running(self.robot, cnames) + def test_check_if_msgs_published(self): + check_if_js_published("/joint_states", ["joint_1", "joint_2", "joint_3"]) -class TestWaitForCompletion(unittest.TestCase): - # Waits for test to complete, then waits a bit to make sure result files are generated - def test_run_complete(self, test_node): - self.proc_info.assertWaitForShutdown(test_node, timeout=4000.0) + def test_main(self, proc_output): + # By default the joint_states should have the initial_value from rrr.urdf.xacro + self.robot.get_logger().info("Checking initial joint states...") + current_joint_state = self.robot.get_current_joint_state() + urdf_initial_values = [0.2, 0.3, 0.1] + assert current_joint_state == urdf_initial_values, ( + f"{current_joint_state=} != {urdf_initial_values=}" + ) + # Test setting the robot joint states + self.robot.get_logger().info("Set joint positions...") + joint_state = [0.1, 0.2, 0.3] + self.robot.set_joint_positions(joint_state) + self.robot.get_logger().info("Checking current joint states...") + current_joint_state = self.robot.get_current_joint_state() + assert current_joint_state == joint_state, f"{current_joint_state=} != {joint_state=}" @launch_testing.post_shutdown_test() class TestProcessPostShutdown(unittest.TestCase): # Checks if the test has been completed with acceptable exit codes (successful codes) - def test_pass(self, proc_info, test_node): - launch_testing.asserts.assertExitCodes(proc_info, process=test_node) + def test_pass(self, proc_info): + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/joint_state_topic_hardware_interface/test/test_topic_based_robot.py b/joint_state_topic_hardware_interface/test/test_topic_based_robot.py deleted file mode 100755 index 4329730..0000000 --- a/joint_state_topic_hardware_interface/test/test_topic_based_robot.py +++ /dev/null @@ -1,35 +0,0 @@ -#!/usr/bin/env python3 - -# Copyright 2025 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. - - -import rclpy -from joint_state_topic_based_robot import JointStateTopicBasedRobot - -rclpy.init() - -robot = JointStateTopicBasedRobot(["joint_1", "joint_2", "joint_3"]) -# By default the joint_states should have the initial_value from rrr.urdf.xacro -current_joint_state = robot.get_current_joint_state() -urdf_initial_values = [0.2, 0.3, 0.1] -assert current_joint_state == urdf_initial_values, ( - f"{current_joint_state=} != {urdf_initial_values=}" -) - -# Test setting the robot joint states -joint_state = [0.1, 0.2, 0.3] -robot.set_joint_positions(joint_state) -current_joint_state = robot.get_current_joint_state() -assert current_joint_state == joint_state, f"{current_joint_state=} != {joint_state=}" From ec9e86e57441f5b61ac97e89ca9bf7dd56632709 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 13 Oct 2025 20:28:51 +0000 Subject: [PATCH 03/29] Let the actual_joint_state be published from a timer --- .../test/joint_state_topic_based_robot.py | 34 +++++++++++-------- 1 file changed, 20 insertions(+), 14 deletions(-) diff --git a/joint_state_topic_hardware_interface/test/joint_state_topic_based_robot.py b/joint_state_topic_hardware_interface/test/joint_state_topic_based_robot.py index 45b32b7..086f47c 100644 --- a/joint_state_topic_hardware_interface/test/joint_state_topic_based_robot.py +++ b/joint_state_topic_hardware_interface/test/joint_state_topic_based_robot.py @@ -29,16 +29,10 @@ class JointStateTopicBasedRobot(Node): def __init__(self, joint_names: list[str]) -> None: super().__init__("topic_based_robot") self.joint_names = joint_names.copy() + self.joint_positions = [0.0] * len(joint_names) self.last_joint_command = [] self.current_joint_state = [] self.callback_group = ReentrantCallbackGroup() - # Publisher for the robot internal joint state (Ground truth) - self.actual_joint_state_publisher = self.create_publisher( - JointState, - "topic_based_joint_states", - qos_profile=qos_profile_sensor_data, - callback_group=self.callback_group, - ) # Subscriber for the joint state commands from ros2_control self.desired_joint_state_subscriber = self.create_subscription( JointState, @@ -55,6 +49,13 @@ def __init__(self, joint_names: list[str]) -> None: qos_profile_system_default, callback_group=self.callback_group, ) + # Publisher for the robot internal joint state (Ground truth) + self.actual_joint_state_publisher = self.create_publisher( + JointState, + "topic_based_joint_states", + qos_profile=qos_profile_sensor_data, + callback_group=self.callback_group, + ) def filter_joint_state_msg(self, msg: JointState): joint_states = [] @@ -102,12 +103,8 @@ def set_joint_positions( joint_positions: list[float] | np.ndarray, ) -> None: """Set the joint positions of the robot.""" - self.actual_joint_state_publisher.publish( - JointState( - name=list(self.joint_names), - position=joint_positions, - ), - ) + self.joint_positions = joint_positions + self.timer = self.create_timer(0.1, self.timer_callback) while not np.allclose( self.get_current_joint_state(), joint_positions, @@ -115,4 +112,13 @@ def set_joint_positions( ): self.get_logger().warning( f"Waiting for current joint states from topic '{self.current_joint_state_subscriber.topic_name}': {self.get_current_joint_state()} to be equal to the commanded positions: {joint_positions}...", - ) + ) + + def timer_callback(self): + self.actual_joint_state_publisher.publish( + JointState( + name=list(self.joint_names), + position=self.joint_positions, + ), + ) + From 1f847ebd60fc3b1bd729429b6edacbddd37ccb7a Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 13 Oct 2025 20:31:38 +0000 Subject: [PATCH 04/29] Update package.xml --- joint_state_topic_hardware_interface/package.xml | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/joint_state_topic_hardware_interface/package.xml b/joint_state_topic_hardware_interface/package.xml index 3e9a485..7def38b 100644 --- a/joint_state_topic_hardware_interface/package.xml +++ b/joint_state_topic_hardware_interface/package.xml @@ -27,16 +27,17 @@ sensor_msgs ament_cmake_gmock + ament_cmake_ros controller_manager joint_state_broadcaster joint_trajectory_controller - launch launch_ros + launch_testing_ament_cmake launch_testing + launch rclpy robot_state_publisher ros2_control_test_assets - ros_testing xacro From 8d4f79612d13f14f0585d2e5d505495efa1e3d22 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 13 Oct 2025 20:32:40 +0000 Subject: [PATCH 05/29] Pre-commit --- .../test/joint_state_topic_based_robot.py | 3 +- .../test/ros2_control.test.py | 28 +++++++++---------- 2 files changed, 15 insertions(+), 16 deletions(-) diff --git a/joint_state_topic_hardware_interface/test/joint_state_topic_based_robot.py b/joint_state_topic_hardware_interface/test/joint_state_topic_based_robot.py index 086f47c..43e5df6 100644 --- a/joint_state_topic_hardware_interface/test/joint_state_topic_based_robot.py +++ b/joint_state_topic_hardware_interface/test/joint_state_topic_based_robot.py @@ -112,7 +112,7 @@ def set_joint_positions( ): self.get_logger().warning( f"Waiting for current joint states from topic '{self.current_joint_state_subscriber.topic_name}': {self.get_current_joint_state()} to be equal to the commanded positions: {joint_positions}...", - ) + ) def timer_callback(self): self.actual_joint_state_publisher.publish( @@ -121,4 +121,3 @@ def timer_callback(self): position=self.joint_positions, ), ) - diff --git a/joint_state_topic_hardware_interface/test/ros2_control.test.py b/joint_state_topic_hardware_interface/test/ros2_control.test.py index 75be396..d26968c 100644 --- a/joint_state_topic_hardware_interface/test/ros2_control.test.py +++ b/joint_state_topic_hardware_interface/test/ros2_control.test.py @@ -14,44 +14,41 @@ import os +import sys import unittest from pathlib import Path import launch_testing +import launch_testing.markers +import pytest +import rclpy from launch import LaunchDescription from launch.actions import ( - DeclareLaunchArgument, IncludeLaunchDescription, - TimerAction, ) - from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import ( - LaunchConfiguration, PathJoinSubstitution, ) from launch_testing.actions import ReadyToTest from launch_testing.util import KeepAliveProc -import launch_testing.markers -import pytest -import rclpy -import sys + # JointStateTopicBasedRobot is in the same folder as this test sys.path.insert(0, str(Path(__file__).parent)) -from joint_state_topic_based_robot import JointStateTopicBasedRobot -SCRIPT_PATH = Path(os.path.realpath(__file__)).parent from controller_manager.test_utils import ( check_controllers_running, check_if_js_published, ) +from joint_state_topic_based_robot import JointStateTopicBasedRobot + # This function specifies the processes to be run for our test @pytest.mark.rostest def generate_test_description(): # This is necessary to get unbuffered output from the process under test proc_env = os.environ.copy() - proc_env['PYTHONUNBUFFERED'] = '1' + proc_env["PYTHONUNBUFFERED"] = "1" return LaunchDescription( [ @@ -60,7 +57,7 @@ def generate_test_description(): [ PathJoinSubstitution( [ - str(SCRIPT_PATH), + str(Path(os.path.realpath(__file__)).parent), "control.launch.py", ], ), @@ -95,7 +92,7 @@ def test_controller_running(self, proc_output): def test_check_if_msgs_published(self): check_if_js_published("/joint_states", ["joint_1", "joint_2", "joint_3"]) - def test_main(self, proc_output): + def test_main(self, proc_output): # By default the joint_states should have the initial_value from rrr.urdf.xacro self.robot.get_logger().info("Checking initial joint states...") current_joint_state = self.robot.get_current_joint_state() @@ -110,7 +107,10 @@ def test_main(self, proc_output): self.robot.set_joint_positions(joint_state) self.robot.get_logger().info("Checking current joint states...") current_joint_state = self.robot.get_current_joint_state() - assert current_joint_state == joint_state, f"{current_joint_state=} != {joint_state=}" + assert current_joint_state == joint_state, ( + f"{current_joint_state=} != {joint_state=}" + ) + @launch_testing.post_shutdown_test() class TestProcessPostShutdown(unittest.TestCase): From 3006911a5f11a9da141f83a7b87f8eebc70b864c Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 13 Oct 2025 20:34:33 +0000 Subject: [PATCH 06/29] Reactivate gmock test --- .../CMakeLists.txt | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/joint_state_topic_hardware_interface/CMakeLists.txt b/joint_state_topic_hardware_interface/CMakeLists.txt index a088413..5e91d42 100644 --- a/joint_state_topic_hardware_interface/CMakeLists.txt +++ b/joint_state_topic_hardware_interface/CMakeLists.txt @@ -61,14 +61,14 @@ if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(ros2_control_test_assets REQUIRED) - # ament_add_gmock(joint_state_topic_hardware_interface_test test/joint_state_topic_hardware_interface_test.cpp) - # target_link_libraries(joint_state_topic_hardware_interface_test - # ${PROJECT_NAME} - # angles::angles - # rclcpp::rclcpp - # hardware_interface::hardware_interface - # ${sensor_msgs_TARGETS} - # ros2_control_test_assets::ros2_control_test_assets) + ament_add_gmock(joint_state_topic_hardware_interface_test test/joint_state_topic_hardware_interface_test.cpp) + target_link_libraries(joint_state_topic_hardware_interface_test + ${PROJECT_NAME} + angles::angles + rclcpp::rclcpp + hardware_interface::hardware_interface + ${sensor_msgs_TARGETS} + ros2_control_test_assets::ros2_control_test_assets) # Integration tests find_package(ament_cmake_ros REQUIRED) From dce942783241373b7fb55b05e5d50b86eeafb6bb Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 13 Oct 2025 20:36:33 +0000 Subject: [PATCH 07/29] Remove wrong node parameters --- joint_state_topic_hardware_interface/test/control.launch.py | 1 - 1 file changed, 1 deletion(-) diff --git a/joint_state_topic_hardware_interface/test/control.launch.py b/joint_state_topic_hardware_interface/test/control.launch.py index 5d315f4..1cde396 100644 --- a/joint_state_topic_hardware_interface/test/control.launch.py +++ b/joint_state_topic_hardware_interface/test/control.launch.py @@ -47,7 +47,6 @@ def generate_launch_description(): Node( package="controller_manager", executable="spawner", - parameters=[ros2_controllers_file], arguments=[controller, "--param-file", ros2_controllers_file], ) for controller in controllers From 438c64a228e75fe1591608d76eee544eb1851e9b Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 13 Oct 2025 20:39:27 +0000 Subject: [PATCH 08/29] Reorder includes --- .../test/ros2_control.test.py | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/joint_state_topic_hardware_interface/test/ros2_control.test.py b/joint_state_topic_hardware_interface/test/ros2_control.test.py index d26968c..f753b77 100644 --- a/joint_state_topic_hardware_interface/test/ros2_control.test.py +++ b/joint_state_topic_hardware_interface/test/ros2_control.test.py @@ -12,7 +12,6 @@ # See the License for the specific language governing permissions and # limitations under the License. - import os import sys import unittest @@ -22,6 +21,10 @@ import launch_testing.markers import pytest import rclpy +from controller_manager.test_utils import ( + check_controllers_running, + check_if_js_published, +) from launch import LaunchDescription from launch.actions import ( IncludeLaunchDescription, @@ -35,11 +38,6 @@ # JointStateTopicBasedRobot is in the same folder as this test sys.path.insert(0, str(Path(__file__).parent)) - -from controller_manager.test_utils import ( - check_controllers_running, - check_if_js_published, -) from joint_state_topic_based_robot import JointStateTopicBasedRobot From 5e6cf88ebd30ea280eebe2b00cd2af4cd5be3eaa Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 13 Oct 2025 20:43:00 +0000 Subject: [PATCH 09/29] Don't wrap it in a list --- .../test/ros2_control.test.py | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/joint_state_topic_hardware_interface/test/ros2_control.test.py b/joint_state_topic_hardware_interface/test/ros2_control.test.py index f753b77..f24d9a5 100644 --- a/joint_state_topic_hardware_interface/test/ros2_control.test.py +++ b/joint_state_topic_hardware_interface/test/ros2_control.test.py @@ -52,14 +52,12 @@ def generate_test_description(): [ IncludeLaunchDescription( PythonLaunchDescriptionSource( - [ - PathJoinSubstitution( - [ - str(Path(os.path.realpath(__file__)).parent), - "control.launch.py", - ], - ), - ], + PathJoinSubstitution( + [ + str(Path(os.path.realpath(__file__)).parent), + "control.launch.py", + ], + ), ), ), KeepAliveProc(), From 0700746a788cbc8318aa581f822b0f443278812a Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 13 Oct 2025 20:59:44 +0000 Subject: [PATCH 10/29] Wrap another Path obj with str() --- joint_state_topic_hardware_interface/test/control.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/joint_state_topic_hardware_interface/test/control.launch.py b/joint_state_topic_hardware_interface/test/control.launch.py index 1cde396..9147059 100644 --- a/joint_state_topic_hardware_interface/test/control.launch.py +++ b/joint_state_topic_hardware_interface/test/control.launch.py @@ -23,7 +23,7 @@ def generate_launch_description(): - ros2_controllers_file = Path(SCRIPT_PATH / "ros2_controllers.yaml") + ros2_controllers_file = str(Path(SCRIPT_PATH / "ros2_controllers.yaml")) robot_description = { "robot_description": xacro.process_file(SCRIPT_PATH / "rrr.urdf.xacro").toxml(), } From 2a6f1a886b04cdaf672c49612970a29935756337 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 13 Oct 2025 21:09:51 +0000 Subject: [PATCH 11/29] Make code compatible with python 3.9 --- .../test/joint_state_topic_based_robot.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/joint_state_topic_hardware_interface/test/joint_state_topic_based_robot.py b/joint_state_topic_hardware_interface/test/joint_state_topic_based_robot.py index 43e5df6..0b764a5 100644 --- a/joint_state_topic_hardware_interface/test/joint_state_topic_based_robot.py +++ b/joint_state_topic_hardware_interface/test/joint_state_topic_based_robot.py @@ -14,6 +14,7 @@ from collections import OrderedDict +from typing import Union import numpy as np import rclpy @@ -100,7 +101,7 @@ def get_current_joint_state(self) -> OrderedDict[str, float]: def set_joint_positions( self, - joint_positions: list[float] | np.ndarray, + joint_positions: Union[list[float], np.ndarray], ) -> None: """Set the joint positions of the robot.""" self.joint_positions = joint_positions From a335ed7f01b295777767ab7cf713c541fa105cd5 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 14 Oct 2025 20:25:03 +0000 Subject: [PATCH 12/29] Use c++ executable and topic_tools --- .../CMakeLists.txt | 8 + .../package.xml | 2 + .../test/example_position.cpp | 211 ++++++++++++++++++ .../test/joint_state_topic_based_robot.py | 124 ---------- .../test/ros2_control.test.py | 100 +++++++-- .../test/ros2_controllers.yaml | 2 +- 6 files changed, 302 insertions(+), 145 deletions(-) create mode 100644 joint_state_topic_hardware_interface/test/example_position.cpp delete mode 100644 joint_state_topic_hardware_interface/test/joint_state_topic_based_robot.py diff --git a/joint_state_topic_hardware_interface/CMakeLists.txt b/joint_state_topic_hardware_interface/CMakeLists.txt index 5e91d42..6c5e40a 100644 --- a/joint_state_topic_hardware_interface/CMakeLists.txt +++ b/joint_state_topic_hardware_interface/CMakeLists.txt @@ -73,6 +73,14 @@ if(BUILD_TESTING) # Integration tests find_package(ament_cmake_ros REQUIRED) find_package(launch_testing_ament_cmake REQUIRED) + find_package(rclcpp_action REQUIRED) + find_package(control_msgs REQUIRED) + add_executable(example_position test/example_position.cpp) + target_link_libraries(example_position PUBLIC + ${control_msgs_TARGETS} + rclcpp::rclcpp + rclcpp_action::rclcpp_action + ) function(add_ros_isolated_launch_test path) set(RUNNER "${ament_cmake_ros_DIR}/run_test_isolated.py") add_launch_test("${path}" RUNNER "${RUNNER}" ${ARGN}) diff --git a/joint_state_topic_hardware_interface/package.xml b/joint_state_topic_hardware_interface/package.xml index 7def38b..212d542 100644 --- a/joint_state_topic_hardware_interface/package.xml +++ b/joint_state_topic_hardware_interface/package.xml @@ -28,6 +28,7 @@ ament_cmake_gmock ament_cmake_ros + control_msgs controller_manager joint_state_broadcaster joint_trajectory_controller @@ -38,6 +39,7 @@ rclpy robot_state_publisher ros2_control_test_assets + topic_tools xacro diff --git a/joint_state_topic_hardware_interface/test/example_position.cpp b/joint_state_topic_hardware_interface/test/example_position.cpp new file mode 100644 index 0000000..e7f99ff --- /dev/null +++ b/joint_state_topic_hardware_interface/test/example_position.cpp @@ -0,0 +1,211 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// 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 +#include +#include +#include + +#include +#include + +#include + +std::shared_ptr node; +bool common_goal_accepted = false; +rclcpp_action::ResultCode common_resultcode = rclcpp_action::ResultCode::UNKNOWN; +int common_action_result_code = control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL; + +std::vector desired_goals = { 0, -0.5, 0.5, 0.0 }; +unsigned int ct_goals_reached = 0; + +void common_goal_response(rclcpp_action::ClientGoalHandle::SharedPtr future) +{ + RCLCPP_DEBUG(node->get_logger(), "common_goal_response time: %f", rclcpp::Clock().now().seconds()); + auto goal_handle = future.get(); + if (!goal_handle) + { + common_goal_accepted = false; + printf("Goal rejected\n"); + } + else + { + common_goal_accepted = true; + printf("Goal accepted\n"); + } +} + +void common_result_response( + const rclcpp_action::ClientGoalHandle::WrappedResult& result) +{ + printf("common_result_response time: %f\n", rclcpp::Clock(RCL_ROS_TIME).now().seconds()); + common_resultcode = result.code; + common_action_result_code = result.result->error_code; + switch (result.code) + { + case rclcpp_action::ResultCode::SUCCEEDED: + printf("Action goal succeeded\n"); + break; + case rclcpp_action::ResultCode::ABORTED: + printf("Action goal was aborted\n"); + return; + case rclcpp_action::ResultCode::CANCELED: + printf("Action goal was canceled\n"); + return; + default: + printf("Action goal: Unknown result code\n"); + return; + } +} + +void common_feedback(rclcpp_action::ClientGoalHandle::SharedPtr, + const std::shared_ptr feedback) +{ + std::cout << "feedback->desired: positions: " << feedback->desired.positions.at(0); + std::cout << ", velocities: " << feedback->desired.velocities.at(0) << std::endl; + + std::cout << "feedback->actual: positions: " << feedback->actual.positions.at(0); + std::cout << std::endl; + + if (ct_goals_reached < desired_goals.size()) + { + if (fabs(feedback->actual.positions[0] - desired_goals.at(ct_goals_reached)) < 0.1) + { + std::cout << "Goal # " << ct_goals_reached << ": " << desired_goals.at(ct_goals_reached) << " reached" + << std::endl; + ct_goals_reached++; + if (ct_goals_reached < desired_goals.size()) + { + std::cout << "next goal # " << ct_goals_reached << ": " << desired_goals.at(ct_goals_reached) << std::endl; + } + } + } +} + +int main(int argc, char* argv[]) +{ + rclcpp::init(argc, argv); + node = std::make_shared("trajectory_test_node"); + + RCLCPP_DEBUG(node->get_logger(), "node created"); + + rclcpp_action::Client::SharedPtr action_client; + action_client = rclcpp_action::create_client( + node->get_node_base_interface(), node->get_node_graph_interface(), node->get_node_logging_interface(), + node->get_node_waitables_interface(), "/joint_trajectory_controller/follow_joint_trajectory"); + + while (true) + { + bool response = action_client->wait_for_action_server(std::chrono::seconds(1)); + if (!response) + { + using namespace std::chrono_literals; + std::this_thread::sleep_for(2000ms); + RCLCPP_WARN(node->get_logger(), "Trying to connect to the server again"); + continue; + } + else + { + break; + } + } + + RCLCPP_DEBUG(node->get_logger(), "Created action server"); + + std::vector joint_names = { "joint_1" }; + + std::vector points; + trajectory_msgs::msg::JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(1.0); + point.positions.resize(joint_names.size()); + point.positions[0] = desired_goals[0]; + + trajectory_msgs::msg::JointTrajectoryPoint point2; + point2.time_from_start = rclcpp::Duration::from_seconds(2.0); + point2.positions.resize(joint_names.size()); + point2.positions[0] = desired_goals[1]; + + trajectory_msgs::msg::JointTrajectoryPoint point3; + point3.time_from_start = rclcpp::Duration::from_seconds(3.0); + point3.positions.resize(joint_names.size()); + point3.positions[0] = desired_goals[2]; + + trajectory_msgs::msg::JointTrajectoryPoint point4; + point4.time_from_start = rclcpp::Duration::from_seconds(4.0); + point4.positions.resize(joint_names.size()); + point4.positions[0] = desired_goals[3]; + + points.push_back(point); + points.push_back(point2); + points.push_back(point3); + points.push_back(point4); + + rclcpp_action::Client::SendGoalOptions opt; + opt.goal_response_callback = std::bind(common_goal_response, std::placeholders::_1); + opt.result_callback = std::bind(common_result_response, std::placeholders::_1); + opt.feedback_callback = std::bind(common_feedback, std::placeholders::_1, std::placeholders::_2); + + control_msgs::action::FollowJointTrajectory_Goal goal_msg; + goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(1.0); + goal_msg.trajectory.joint_names = joint_names; + goal_msg.trajectory.points = points; + + RCLCPP_DEBUG(node->get_logger(), "async_send_goal"); + auto goal_handle_future = action_client->async_send_goal(goal_msg, opt); + + if (rclcpp::spin_until_future_complete(node, goal_handle_future) != rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR(node->get_logger(), "send goal call failed :("); + action_client.reset(); + node.reset(); + return 1; + } + RCLCPP_INFO(node->get_logger(), "send goal call ok :)"); + + rclcpp_action::ClientGoalHandle::SharedPtr goal_handle = + goal_handle_future.get(); + if (!goal_handle) + { + RCLCPP_ERROR(node->get_logger(), "Goal was rejected by server"); + action_client.reset(); + node.reset(); + return 1; + } + RCLCPP_INFO(node->get_logger(), "Goal was accepted by server"); + + // Wait for the server to be done with the goal + auto result_future = action_client->async_get_result(goal_handle); + RCLCPP_INFO(node->get_logger(), "Waiting for result"); + if (rclcpp::spin_until_future_complete(node, result_future) != rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR(node->get_logger(), "get result call failed :("); + action_client.reset(); + node.reset(); + return 1; + } + action_client.reset(); + + if (desired_goals.size() != ct_goals_reached) + { + RCLCPP_ERROR(node->get_logger(), "Not all the goals were reached"); + rclcpp::shutdown(); + return -1; + } + + node.reset(); + + rclcpp::shutdown(); + + return 0; +} diff --git a/joint_state_topic_hardware_interface/test/joint_state_topic_based_robot.py b/joint_state_topic_hardware_interface/test/joint_state_topic_based_robot.py deleted file mode 100644 index 0b764a5..0000000 --- a/joint_state_topic_hardware_interface/test/joint_state_topic_based_robot.py +++ /dev/null @@ -1,124 +0,0 @@ -# Copyright 2025 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. - - -from collections import OrderedDict -from typing import Union - -import numpy as np -import rclpy -from rclpy.callback_groups import ReentrantCallbackGroup -from rclpy.node import Node -from rclpy.qos import QoSProfile, qos_profile_sensor_data, qos_profile_system_default -from sensor_msgs.msg import JointState - - -class JointStateTopicBasedRobot(Node): - """JointState Topic Based Robot to test joint_state_topic_hardware_interface""" - - def __init__(self, joint_names: list[str]) -> None: - super().__init__("topic_based_robot") - self.joint_names = joint_names.copy() - self.joint_positions = [0.0] * len(joint_names) - self.last_joint_command = [] - self.current_joint_state = [] - self.callback_group = ReentrantCallbackGroup() - # Subscriber for the joint state commands from ros2_control - self.desired_joint_state_subscriber = self.create_subscription( - JointState, - "topic_based_joint_commands", - self.command_callback, - QoSProfile(depth=1), - callback_group=self.callback_group, - ) - # Reported joint state from joint_state_broadcaster - self.current_joint_state_subscriber = self.create_subscription( - JointState, - "joint_states", - self.joint_states_callback, - qos_profile_system_default, - callback_group=self.callback_group, - ) - # Publisher for the robot internal joint state (Ground truth) - self.actual_joint_state_publisher = self.create_publisher( - JointState, - "topic_based_joint_states", - qos_profile=qos_profile_sensor_data, - callback_group=self.callback_group, - ) - - def filter_joint_state_msg(self, msg: JointState): - joint_states = [] - for joint_name in self.joint_names: - try: - index = msg.name.index(joint_name) - except ValueError: - msg = f"Joint name '{joint_name}' not in input keys {msg.name}" - raise ValueError(msg) from None - joint_states.append(msg.position[index]) - return joint_states - - def command_callback(self, msg: JointState): - self.last_joint_command = self.filter_joint_state_msg(msg) - - def joint_states_callback(self, msg: JointState): - self.current_joint_state = self.filter_joint_state_msg(msg) - - def get_current_joint_command(self) -> OrderedDict[str, float]: - """Get the last joint command sent to the robot.""" - self.last_joint_command = [] - while len(self.last_joint_command) == 0: - self.get_logger().warning( - f"Waiting for joint command '{self.desired_joint_state_subscriber.topic_name}'...", - throttle_duration_sec=2.0, - skip_first=True, - ) - rclpy.spin_once(self, timeout_sec=1.0) - return self.last_joint_command - - def get_current_joint_state(self) -> OrderedDict[str, float]: - """Get the current joint state reported by ros2_control on joint_states topic.""" - self.current_joint_state = [] - while len(self.current_joint_state) == 0: - self.get_logger().warning( - f"Waiting for current joint states from topic '{self.current_joint_state_subscriber.topic_name}'...", - throttle_duration_sec=2.0, - skip_first=True, - ) - rclpy.spin_once(self, timeout_sec=1.0) - return self.current_joint_state - - def set_joint_positions( - self, - joint_positions: Union[list[float], np.ndarray], - ) -> None: - """Set the joint positions of the robot.""" - self.joint_positions = joint_positions - self.timer = self.create_timer(0.1, self.timer_callback) - while not np.allclose( - self.get_current_joint_state(), - joint_positions, - atol=1e-3, - ): - self.get_logger().warning( - f"Waiting for current joint states from topic '{self.current_joint_state_subscriber.topic_name}': {self.get_current_joint_state()} to be equal to the commanded positions: {joint_positions}...", - ) - - def timer_callback(self): - self.actual_joint_state_publisher.publish( - JointState( - name=list(self.joint_names), - position=self.joint_positions, - ), - ) diff --git a/joint_state_topic_hardware_interface/test/ros2_control.test.py b/joint_state_topic_hardware_interface/test/ros2_control.test.py index f24d9a5..e1529b1 100644 --- a/joint_state_topic_hardware_interface/test/ros2_control.test.py +++ b/joint_state_topic_hardware_interface/test/ros2_control.test.py @@ -13,17 +13,19 @@ # limitations under the License. import os -import sys import unittest +from collections import OrderedDict from pathlib import Path import launch_testing import launch_testing.markers import pytest import rclpy +from ament_index_python.packages import get_package_prefix from controller_manager.test_utils import ( check_controllers_running, check_if_js_published, + check_node_running, ) from launch import LaunchDescription from launch.actions import ( @@ -33,12 +35,11 @@ from launch.substitutions import ( PathJoinSubstitution, ) +from launch_ros.actions import Node from launch_testing.actions import ReadyToTest from launch_testing.util import KeepAliveProc - -# JointStateTopicBasedRobot is in the same folder as this test -sys.path.insert(0, str(Path(__file__).parent)) -from joint_state_topic_based_robot import JointStateTopicBasedRobot +from rclpy.qos import qos_profile_system_default +from sensor_msgs.msg import JointState # This function specifies the processes to be run for our test @@ -60,6 +61,12 @@ def generate_test_description(): ), ), ), + Node( + package="topic_tools", + executable="relay", + output="screen", + arguments=["/topic_based_joint_commands", "/topic_based_joint_states"], + ), KeepAliveProc(), ReadyToTest(), ], @@ -76,37 +83,90 @@ def tearDownClass(cls): rclpy.shutdown() def setUp(self): - self.robot = JointStateTopicBasedRobot(["joint_1", "joint_2", "joint_3"]) + self.node = rclpy.create_node("test_node") + + # Reported joint state from joint_state_broadcaster + self.current_joint_state_subscriber = self.node.create_subscription( + JointState, + "joint_states", + self.joint_states_callback, + qos_profile_system_default, + ) + + self.joint_names = ["joint_1", "joint_2", "joint_3"] def tearDown(self): - self.robot.destroy_node() + self.node.destroy_node() def test_controller_running(self, proc_output): cnames = ["joint_trajectory_controller", "joint_state_broadcaster"] - check_controllers_running(self.robot, cnames) + check_node_running(self.node, "relay") + check_controllers_running(self.node, cnames) def test_check_if_msgs_published(self): - check_if_js_published("/joint_states", ["joint_1", "joint_2", "joint_3"]) + check_if_js_published("/joint_states", self.joint_names) - def test_main(self, proc_output): + def test_main(self, launch_service, proc_info, proc_output): # By default the joint_states should have the initial_value from rrr.urdf.xacro - self.robot.get_logger().info("Checking initial joint states...") - current_joint_state = self.robot.get_current_joint_state() + self.node.get_logger().info("Checking initial joint states...") + current_joint_state = self.get_current_joint_state() urdf_initial_values = [0.2, 0.3, 0.1] assert current_joint_state == urdf_initial_values, ( f"{current_joint_state=} != {urdf_initial_values=}" ) - # Test setting the robot joint states - self.robot.get_logger().info("Set joint positions...") - joint_state = [0.1, 0.2, 0.3] - self.robot.set_joint_positions(joint_state) - self.robot.get_logger().info("Checking current joint states...") - current_joint_state = self.robot.get_current_joint_state() - assert current_joint_state == joint_state, ( - f"{current_joint_state=} != {joint_state=}" + # Test setting the robot joint states via controller + # example_position is run directly here as it was not installed + pkg_name = "joint_state_topic_hardware_interface" + proc_action = Node( + executable=os.path.join( + get_package_prefix(pkg_name).replace("install", "build"), + "example_position", + ), + output="screen", + ) + + with launch_testing.tools.launch_process( + launch_service, proc_action, proc_info, proc_output + ): + proc_info.assertWaitForShutdown(process=proc_action, timeout=300) + launch_testing.asserts.assertExitCodes( + proc_info, process=proc_action, allowable_exit_codes=[0] + ) + + self.node.get_logger().info("Checking final joint states...") + current_joint_state = self.get_current_joint_state() + final_values = [0.0, 0.3, 0.1] # joint_2 and joint_3 should remain unchanged + assert current_joint_state == final_values, ( + f"{current_joint_state=} != {final_values=}" ) + def joint_states_callback(self, msg: JointState): + self.current_joint_state = self.filter_joint_state_msg(msg) + + def get_current_joint_state(self) -> OrderedDict[str, float]: + """Get the current joint state reported by ros2_control on joint_states topic.""" + self.current_joint_state = [] + while len(self.current_joint_state) == 0: + self.node.get_logger().warning( + f"Waiting for current joint states from topic '{self.current_joint_state_subscriber.topic_name}'...", + throttle_duration_sec=2.0, + skip_first=True, + ) + rclpy.spin_once(self.node, timeout_sec=1.0) + return self.current_joint_state + + def filter_joint_state_msg(self, msg: JointState): + joint_states = [] + for joint_name in self.joint_names: + try: + index = msg.name.index(joint_name) + except ValueError: + msg = f"Joint name '{joint_name}' not in input keys {msg.name}" + raise ValueError(msg) from None + joint_states.append(msg.position[index]) + return joint_states + @launch_testing.post_shutdown_test() class TestProcessPostShutdown(unittest.TestCase): diff --git a/joint_state_topic_hardware_interface/test/ros2_controllers.yaml b/joint_state_topic_hardware_interface/test/ros2_controllers.yaml index 1bfb07d..8337043 100644 --- a/joint_state_topic_hardware_interface/test/ros2_controllers.yaml +++ b/joint_state_topic_hardware_interface/test/ros2_controllers.yaml @@ -20,7 +20,7 @@ joint_trajectory_controller: - velocity state_publish_rate: 100.0 action_monitor_rate: 20.0 - allow_partial_joints_goal: false + allow_partial_joints_goal: true constraints: stopped_velocity_tolerance: 0.0 goal_time: 0.0 From 6e8986311a243fcf3fd5739b9e753f75fb302c84 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 14 Oct 2025 20:30:40 +0000 Subject: [PATCH 13/29] Improve include order --- .../test/ros2_control.test.py | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/joint_state_topic_hardware_interface/test/ros2_control.test.py b/joint_state_topic_hardware_interface/test/ros2_control.test.py index e1529b1..b8ca120 100644 --- a/joint_state_topic_hardware_interface/test/ros2_control.test.py +++ b/joint_state_topic_hardware_interface/test/ros2_control.test.py @@ -28,13 +28,9 @@ check_node_running, ) from launch import LaunchDescription -from launch.actions import ( - IncludeLaunchDescription, -) +from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import ( - PathJoinSubstitution, -) +from launch.substitutions import PathJoinSubstitution from launch_ros.actions import Node from launch_testing.actions import ReadyToTest from launch_testing.util import KeepAliveProc From c7cfec32c2db6e7e966d1496d73f20ae74addd5f Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 14 Oct 2025 20:37:23 +0000 Subject: [PATCH 14/29] Make assertion more robust --- .../test/ros2_control.test.py | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/joint_state_topic_hardware_interface/test/ros2_control.test.py b/joint_state_topic_hardware_interface/test/ros2_control.test.py index b8ca120..7f969e4 100644 --- a/joint_state_topic_hardware_interface/test/ros2_control.test.py +++ b/joint_state_topic_hardware_interface/test/ros2_control.test.py @@ -19,6 +19,7 @@ import launch_testing import launch_testing.markers +import numpy as np import pytest import rclpy from ament_index_python.packages import get_package_prefix @@ -133,9 +134,11 @@ def test_main(self, launch_service, proc_info, proc_output): self.node.get_logger().info("Checking final joint states...") current_joint_state = self.get_current_joint_state() final_values = [0.0, 0.3, 0.1] # joint_2 and joint_3 should remain unchanged - assert current_joint_state == final_values, ( - f"{current_joint_state=} != {final_values=}" - ) + assert np.allclose( + current_joint_state, + final_values, + atol=1e-3, + ), f"{current_joint_state=} != {final_values=}" def joint_states_callback(self, msg: JointState): self.current_joint_state = self.filter_joint_state_msg(msg) From b9acb9f949181583a4d60db0c5ca29c2aa9a1a39 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 15 Oct 2025 18:37:38 +0000 Subject: [PATCH 15/29] Rename executable and update goal tolerances --- joint_state_topic_hardware_interface/CMakeLists.txt | 4 ++-- .../test/{example_position.cpp => jtc_client_position.cpp} | 2 +- .../test/ros2_control.test.py | 4 ++-- .../test/ros2_controllers.yaml | 3 +++ 4 files changed, 8 insertions(+), 5 deletions(-) rename joint_state_topic_hardware_interface/test/{example_position.cpp => jtc_client_position.cpp} (99%) diff --git a/joint_state_topic_hardware_interface/CMakeLists.txt b/joint_state_topic_hardware_interface/CMakeLists.txt index 6c5e40a..a5037a7 100644 --- a/joint_state_topic_hardware_interface/CMakeLists.txt +++ b/joint_state_topic_hardware_interface/CMakeLists.txt @@ -75,8 +75,8 @@ if(BUILD_TESTING) find_package(launch_testing_ament_cmake REQUIRED) find_package(rclcpp_action REQUIRED) find_package(control_msgs REQUIRED) - add_executable(example_position test/example_position.cpp) - target_link_libraries(example_position PUBLIC + add_executable(jtc_client_position test/jtc_client_position.cpp) + target_link_libraries(jtc_client_position PUBLIC ${control_msgs_TARGETS} rclcpp::rclcpp rclcpp_action::rclcpp_action diff --git a/joint_state_topic_hardware_interface/test/example_position.cpp b/joint_state_topic_hardware_interface/test/jtc_client_position.cpp similarity index 99% rename from joint_state_topic_hardware_interface/test/example_position.cpp rename to joint_state_topic_hardware_interface/test/jtc_client_position.cpp index e7f99ff..69a627a 100644 --- a/joint_state_topic_hardware_interface/test/example_position.cpp +++ b/joint_state_topic_hardware_interface/test/jtc_client_position.cpp @@ -80,7 +80,7 @@ void common_feedback(rclcpp_action::ClientGoalHandleactual.positions[0] - desired_goals.at(ct_goals_reached)) < 0.1) + if (fabs(feedback->actual.positions[0] - desired_goals.at(ct_goals_reached)) <= 0.05) { std::cout << "Goal # " << ct_goals_reached << ": " << desired_goals.at(ct_goals_reached) << " reached" << std::endl; diff --git a/joint_state_topic_hardware_interface/test/ros2_control.test.py b/joint_state_topic_hardware_interface/test/ros2_control.test.py index 7f969e4..4331a5d 100644 --- a/joint_state_topic_hardware_interface/test/ros2_control.test.py +++ b/joint_state_topic_hardware_interface/test/ros2_control.test.py @@ -113,12 +113,12 @@ def test_main(self, launch_service, proc_info, proc_output): ) # Test setting the robot joint states via controller - # example_position is run directly here as it was not installed + # jtc_client_position was not installed, call it directly from build space pkg_name = "joint_state_topic_hardware_interface" proc_action = Node( executable=os.path.join( get_package_prefix(pkg_name).replace("install", "build"), - "example_position", + "jtc_client_position", ), output="screen", ) diff --git a/joint_state_topic_hardware_interface/test/ros2_controllers.yaml b/joint_state_topic_hardware_interface/test/ros2_controllers.yaml index 8337043..ed61f3a 100644 --- a/joint_state_topic_hardware_interface/test/ros2_controllers.yaml +++ b/joint_state_topic_hardware_interface/test/ros2_controllers.yaml @@ -24,3 +24,6 @@ joint_trajectory_controller: constraints: stopped_velocity_tolerance: 0.0 goal_time: 0.0 + joint_1: + trajectory: 0.1 + goal: 0.01 From 4e3407d854146b95c9fc43288643dae96b98aea2 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 15 Oct 2025 18:50:16 +0000 Subject: [PATCH 16/29] Rename files --- joint_state_topic_hardware_interface/CMakeLists.txt | 2 +- joint_state_topic_hardware_interface/test/rrr.urdf.xacro | 2 +- .../test/{ros2_control.test.py => rrr_position.test.py} | 0 3 files changed, 2 insertions(+), 2 deletions(-) rename joint_state_topic_hardware_interface/test/{ros2_control.test.py => rrr_position.test.py} (100%) diff --git a/joint_state_topic_hardware_interface/CMakeLists.txt b/joint_state_topic_hardware_interface/CMakeLists.txt index a5037a7..331b329 100644 --- a/joint_state_topic_hardware_interface/CMakeLists.txt +++ b/joint_state_topic_hardware_interface/CMakeLists.txt @@ -86,7 +86,7 @@ if(BUILD_TESTING) add_launch_test("${path}" RUNNER "${RUNNER}" ${ARGN}) endfunction() add_ros_isolated_launch_test( - test/ros2_control.test.py) + test/position.test.py) endif() pluginlib_export_plugin_description_file(hardware_interface joint_state_topic_hardware_interface_plugin_description.xml) diff --git a/joint_state_topic_hardware_interface/test/rrr.urdf.xacro b/joint_state_topic_hardware_interface/test/rrr.urdf.xacro index 3288c87..dd497f9 100644 --- a/joint_state_topic_hardware_interface/test/rrr.urdf.xacro +++ b/joint_state_topic_hardware_interface/test/rrr.urdf.xacro @@ -109,7 +109,7 @@ - + joint_state_topic_hardware_interface/JointStateTopicSystem /topic_based_joint_commands diff --git a/joint_state_topic_hardware_interface/test/ros2_control.test.py b/joint_state_topic_hardware_interface/test/rrr_position.test.py similarity index 100% rename from joint_state_topic_hardware_interface/test/ros2_control.test.py rename to joint_state_topic_hardware_interface/test/rrr_position.test.py From 3703699fa84cc225b55410de31f982a22820b8c5 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 15 Oct 2025 18:53:09 +0000 Subject: [PATCH 17/29] Fix cmake --- joint_state_topic_hardware_interface/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/joint_state_topic_hardware_interface/CMakeLists.txt b/joint_state_topic_hardware_interface/CMakeLists.txt index 331b329..76acc18 100644 --- a/joint_state_topic_hardware_interface/CMakeLists.txt +++ b/joint_state_topic_hardware_interface/CMakeLists.txt @@ -86,7 +86,7 @@ if(BUILD_TESTING) add_launch_test("${path}" RUNNER "${RUNNER}" ${ARGN}) endfunction() add_ros_isolated_launch_test( - test/position.test.py) + test/rrr_position.test.py) endif() pluginlib_export_plugin_description_file(hardware_interface joint_state_topic_hardware_interface_plugin_description.xml) From a904bd771a5f9f9c60656c4daedea6910d64b6fd Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 15 Oct 2025 19:00:01 +0000 Subject: [PATCH 18/29] Move to subfolder --- joint_state_topic_hardware_interface/CMakeLists.txt | 4 ++-- .../test/{ => rrr}/jtc_client_position.cpp | 0 .../test/{rrr_position.test.py => rrr/position.test.py} | 2 +- .../test/{ => rrr}/ros2_controllers.yaml | 0 .../test/{control.launch.py => rrr/rrr.launch.py} | 0 .../test/{ => rrr}/rrr.urdf.xacro | 0 6 files changed, 3 insertions(+), 3 deletions(-) rename joint_state_topic_hardware_interface/test/{ => rrr}/jtc_client_position.cpp (100%) rename joint_state_topic_hardware_interface/test/{rrr_position.test.py => rrr/position.test.py} (99%) rename joint_state_topic_hardware_interface/test/{ => rrr}/ros2_controllers.yaml (100%) rename joint_state_topic_hardware_interface/test/{control.launch.py => rrr/rrr.launch.py} (100%) rename joint_state_topic_hardware_interface/test/{ => rrr}/rrr.urdf.xacro (100%) diff --git a/joint_state_topic_hardware_interface/CMakeLists.txt b/joint_state_topic_hardware_interface/CMakeLists.txt index 76acc18..69d3148 100644 --- a/joint_state_topic_hardware_interface/CMakeLists.txt +++ b/joint_state_topic_hardware_interface/CMakeLists.txt @@ -75,7 +75,7 @@ if(BUILD_TESTING) find_package(launch_testing_ament_cmake REQUIRED) find_package(rclcpp_action REQUIRED) find_package(control_msgs REQUIRED) - add_executable(jtc_client_position test/jtc_client_position.cpp) + add_executable(jtc_client_position test/rrr/jtc_client_position.cpp) target_link_libraries(jtc_client_position PUBLIC ${control_msgs_TARGETS} rclcpp::rclcpp @@ -86,7 +86,7 @@ if(BUILD_TESTING) add_launch_test("${path}" RUNNER "${RUNNER}" ${ARGN}) endfunction() add_ros_isolated_launch_test( - test/rrr_position.test.py) + test/rrr/position.test.py) endif() pluginlib_export_plugin_description_file(hardware_interface joint_state_topic_hardware_interface_plugin_description.xml) diff --git a/joint_state_topic_hardware_interface/test/jtc_client_position.cpp b/joint_state_topic_hardware_interface/test/rrr/jtc_client_position.cpp similarity index 100% rename from joint_state_topic_hardware_interface/test/jtc_client_position.cpp rename to joint_state_topic_hardware_interface/test/rrr/jtc_client_position.cpp diff --git a/joint_state_topic_hardware_interface/test/rrr_position.test.py b/joint_state_topic_hardware_interface/test/rrr/position.test.py similarity index 99% rename from joint_state_topic_hardware_interface/test/rrr_position.test.py rename to joint_state_topic_hardware_interface/test/rrr/position.test.py index 4331a5d..f5d3bcd 100644 --- a/joint_state_topic_hardware_interface/test/rrr_position.test.py +++ b/joint_state_topic_hardware_interface/test/rrr/position.test.py @@ -53,7 +53,7 @@ def generate_test_description(): PathJoinSubstitution( [ str(Path(os.path.realpath(__file__)).parent), - "control.launch.py", + "rrr.launch.py", ], ), ), diff --git a/joint_state_topic_hardware_interface/test/ros2_controllers.yaml b/joint_state_topic_hardware_interface/test/rrr/ros2_controllers.yaml similarity index 100% rename from joint_state_topic_hardware_interface/test/ros2_controllers.yaml rename to joint_state_topic_hardware_interface/test/rrr/ros2_controllers.yaml diff --git a/joint_state_topic_hardware_interface/test/control.launch.py b/joint_state_topic_hardware_interface/test/rrr/rrr.launch.py similarity index 100% rename from joint_state_topic_hardware_interface/test/control.launch.py rename to joint_state_topic_hardware_interface/test/rrr/rrr.launch.py diff --git a/joint_state_topic_hardware_interface/test/rrr.urdf.xacro b/joint_state_topic_hardware_interface/test/rrr/rrr.urdf.xacro similarity index 100% rename from joint_state_topic_hardware_interface/test/rrr.urdf.xacro rename to joint_state_topic_hardware_interface/test/rrr/rrr.urdf.xacro From c650a565867dd9179e9c7035f034c5e27df67cbc Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 15 Oct 2025 19:57:19 +0000 Subject: [PATCH 19/29] Initial version --- .../joint_state_topic_hardware_interface.hpp | 26 -- .../joint_state_topic_hardware_interface.cpp | 241 ++++++------------ 2 files changed, 80 insertions(+), 187 deletions(-) diff --git a/joint_state_topic_hardware_interface/include/joint_state_topic_hardware_interface/joint_state_topic_hardware_interface.hpp b/joint_state_topic_hardware_interface/include/joint_state_topic_hardware_interface/joint_state_topic_hardware_interface.hpp index b8a8062..51482a8 100644 --- a/joint_state_topic_hardware_interface/include/joint_state_topic_hardware_interface/joint_state_topic_hardware_interface.hpp +++ b/joint_state_topic_hardware_interface/include/joint_state_topic_hardware_interface/joint_state_topic_hardware_interface.hpp @@ -38,10 +38,6 @@ class JointStateTopicSystem : public hardware_interface::SystemInterface public: CallbackReturn on_init(const hardware_interface::HardwareComponentInterfaceParams& params) override; - std::vector export_state_interfaces() override; - - std::vector export_command_interfaces() override; - hardware_interface::return_type read(const rclcpp::Time& time, const rclcpp::Duration& period) override; hardware_interface::return_type write(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) override; @@ -52,31 +48,9 @@ class JointStateTopicSystem : public hardware_interface::SystemInterface sensor_msgs::msg::JointState latest_joint_state_; bool sum_wrapped_joint_states_{ false }; - /// Use standard interfaces for joints because they are relevant for dynamic behavior - std::array standard_interfaces_ = { - { hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_ACCELERATION, - hardware_interface::HW_IF_EFFORT } - }; - - struct MimicJoint - { - std::size_t joint_index; - std::size_t mimicked_joint_index; - double multiplier = 1.0; - }; - std::vector mimic_joints_; - - /// The size of this vector is (standard_interfaces_.size() x nr_joints) - std::vector> joint_command_values_; - std::vector> joint_state_values_; - // If the difference between the current joint state and joint command is less than this value, // the joint command will not be published. double trigger_joint_command_threshold_ = 1e-5; - - template - bool getInterface(const std::string& name, const std::string& interface_name, const size_t vector_index, - std::vector>& values, std::vector& interfaces); }; } // namespace joint_state_topic_hardware_interface diff --git a/joint_state_topic_hardware_interface/src/joint_state_topic_hardware_interface.cpp b/joint_state_topic_hardware_interface/src/joint_state_topic_hardware_interface.cpp index 6d09455..0f22ba9 100644 --- a/joint_state_topic_hardware_interface/src/joint_state_topic_hardware_interface.cpp +++ b/joint_state_topic_hardware_interface/src/joint_state_topic_hardware_interface.cpp @@ -29,13 +29,14 @@ namespace { /** @brief Sums the total rotation for joint states that wrap from 2*pi to -2*pi when rotating in the positive direction */ -void sumRotationFromMinus2PiTo2Pi(const double current_wrapped_rad, double& total_rotation) +double sumRotationFromMinus2PiTo2Pi(const double current_wrapped_rad, double total_rotation_in) { double delta = 0; - angles::shortest_angular_distance_with_large_limits(total_rotation, current_wrapped_rad, 2 * M_PI, -2 * M_PI, delta); + angles::shortest_angular_distance_with_large_limits(total_rotation_in, current_wrapped_rad, 2 * M_PI, -2 * M_PI, + delta); // Add the corrected delta to the total rotation - total_rotation += delta; + return total_rotation_in + delta; } } // namespace @@ -54,61 +55,31 @@ CallbackReturn JointStateTopicSystem::on_init(const hardware_interface::Hardware return CallbackReturn::ERROR; } - // Initialize storage for all joints' standard interfaces, regardless of their existence and set all values to nan - joint_command_values_.resize(standard_interfaces_.size()); - joint_state_values_.resize(standard_interfaces_.size()); - for (auto i = 0u; i < standard_interfaces_.size(); i++) + // Update mimic joints + // TODO(christophfroehlich): move that to the handles + const auto& joints = get_hardware_info().joints; + for (const auto& mimic_joint : get_hardware_info().mimic_joints) { - joint_command_values_[i].resize(get_hardware_info().joints.size(), 0.0); - joint_state_values_[i].resize(get_hardware_info().joints.size(), 0.0); - } - - // Initial command values - for (auto i = 0u; i < get_hardware_info().joints.size(); i++) - { - const auto& component = get_hardware_info().joints[i]; - for (const auto& interface : component.state_interfaces) + const auto& mimic_joint_name = joints.at(mimic_joint.joint_index).name; + const auto& mimicked_joint_name = joints.at(mimic_joint.mimicked_joint_index).name; + if (joint_state_interfaces_.find(mimic_joint_name + "/" + hardware_interface::HW_IF_POSITION) != + joint_state_interfaces_.end()) { - auto it = std::find(standard_interfaces_.begin(), standard_interfaces_.end(), interface.name); - // If interface name is found in the interfaces list - if (it != standard_interfaces_.end()) - { - auto index = static_cast(std::distance(standard_interfaces_.begin(), it)); - // Check the initial_value param is used - if (!interface.initial_value.empty()) - { - joint_state_values_[index][i] = std::stod(interface.initial_value); - joint_command_values_[index][i] = std::stod(interface.initial_value); - } - } + set_state(mimic_joint_name + "/" + hardware_interface::HW_IF_POSITION, + mimic_joint.offset + + mimic_joint.multiplier * get_state(mimicked_joint_name + "/" + hardware_interface::HW_IF_POSITION)); } - } - - // Search for mimic joints - for (auto i = 0u; i < get_hardware_info().joints.size(); ++i) - { - const auto& joint = get_hardware_info().joints.at(i); - if (joint.parameters.find("mimic") != joint.parameters.cend()) + if (joint_state_interfaces_.find(mimic_joint_name + "/" + hardware_interface::HW_IF_VELOCITY) != + joint_state_interfaces_.end()) { - const auto mimicked_joint_it = std::find_if( - get_hardware_info().joints.begin(), get_hardware_info().joints.end(), - [&mimicked_joint = joint.parameters.at("mimic")](const hardware_interface::ComponentInfo& joint_info) { - return joint_info.name == mimicked_joint; - }); - if (mimicked_joint_it == get_hardware_info().joints.cend()) - { - throw std::runtime_error(std::string("Mimicked joint '") + joint.parameters.at("mimic") + "' not found"); - } - MimicJoint mimic_joint; - mimic_joint.joint_index = i; - mimic_joint.mimicked_joint_index = - static_cast(std::distance(get_hardware_info().joints.begin(), mimicked_joint_it)); - auto param_it = joint.parameters.find("multiplier"); - if (param_it != joint.parameters.end()) - { - mimic_joint.multiplier = std::stod(joint.parameters.at("multiplier")); - } - mimic_joints_.push_back(mimic_joint); + set_state(mimic_joint_name + "/" + hardware_interface::HW_IF_VELOCITY, + mimic_joint.multiplier * get_state(mimicked_joint_name + "/" + hardware_interface::HW_IF_VELOCITY)); + } + if (joint_state_interfaces_.find(mimic_joint_name + "/" + hardware_interface::HW_IF_ACCELERATION) != + joint_state_interfaces_.end()) + { + set_state(mimic_joint_name + "/" + hardware_interface::HW_IF_ACCELERATION, + mimic_joint.multiplier * get_state(mimicked_joint_name + "/" + hardware_interface::HW_IF_ACCELERATION)); } } @@ -143,47 +114,6 @@ CallbackReturn JointStateTopicSystem::on_init(const hardware_interface::Hardware return CallbackReturn::SUCCESS; } -std::vector JointStateTopicSystem::export_state_interfaces() -{ - std::vector state_interfaces; - - // Joints' state interfaces - for (auto i = 0u; i < get_hardware_info().joints.size(); i++) - { - const auto& joint = get_hardware_info().joints[i]; - for (const auto& interface : joint.state_interfaces) - { - // Add interface: if not in the standard list then use "other" interface list - if (!getInterface(joint.name, interface.name, i, joint_state_values_, state_interfaces)) - { - throw std::runtime_error("Interface is not found in the standard list."); - } - } - } - - return state_interfaces; -} - -std::vector JointStateTopicSystem::export_command_interfaces() -{ - std::vector command_interfaces; - - // Joints' state interfaces - for (auto i = 0u; i < get_hardware_info().joints.size(); i++) - { - const auto& joint = get_hardware_info().joints[i]; - for (const auto& interface : joint.command_interfaces) - { - if (!getInterface(joint.name, interface.name, i, joint_command_values_, command_interfaces)) - { - throw std::runtime_error("Interface is not found in the standard list."); - } - } - } - - return command_interfaces; -} - hardware_interface::return_type JointStateTopicSystem::read(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { @@ -195,113 +125,102 @@ hardware_interface::return_type JointStateTopicSystem::read(const rclcpp::Time& const hardware_interface::ComponentInfo& info) { return joint_name == info.name; }); if (it != joints.end()) { - auto j = static_cast(std::distance(joints.begin(), it)); - if (sum_wrapped_joint_states_) - { - sumRotationFromMinus2PiTo2Pi(latest_joint_state_.position[i], joint_state_values_[POSITION_INTERFACE_INDEX][j]); - } - else + if (std::isfinite(latest_joint_state_.position.at(i))) { - joint_state_values_[POSITION_INTERFACE_INDEX][j] = latest_joint_state_.position[i]; + if (sum_wrapped_joint_states_) + { + auto name = latest_joint_state_.name[i] + "/" + hardware_interface::HW_IF_POSITION; + + set_state(name, sumRotationFromMinus2PiTo2Pi(latest_joint_state_.position.at(i), get_state(name))); + } + else + { + set_state(latest_joint_state_.name[i] + "/" + hardware_interface::HW_IF_POSITION, + latest_joint_state_.position.at(i)); + } } - if (!latest_joint_state_.velocity.empty()) + if (!latest_joint_state_.velocity.empty() && std::isfinite(latest_joint_state_.velocity.at(i))) { - joint_state_values_[VELOCITY_INTERFACE_INDEX][j] = latest_joint_state_.velocity[i]; + set_state(latest_joint_state_.name[i] + "/" + hardware_interface::HW_IF_VELOCITY, + latest_joint_state_.velocity.at(i)); } - if (!latest_joint_state_.effort.empty()) + if (!latest_joint_state_.effort.empty() && std::isfinite(latest_joint_state_.effort.at(i))) { - joint_state_values_[EFFORT_INTERFACE_INDEX][j] = latest_joint_state_.effort[i]; + set_state(latest_joint_state_.name[i] + "/" + hardware_interface::HW_IF_EFFORT, + latest_joint_state_.effort.at(i)); } } } - for (const auto& mimic_joint : mimic_joints_) - { - for (auto& joint_state : joint_state_values_) - { - joint_state[mimic_joint.joint_index] = mimic_joint.multiplier * joint_state[mimic_joint.mimicked_joint_index]; - } - } - return hardware_interface::return_type::OK; } -template -bool JointStateTopicSystem::getInterface(const std::string& name, const std::string& interface_name, - const size_t vector_index, std::vector>& values, - std::vector& interfaces) -{ - auto it = std::find(standard_interfaces_.begin(), standard_interfaces_.end(), interface_name); - if (it != standard_interfaces_.end()) - { - auto j = static_cast(std::distance(standard_interfaces_.begin(), it)); - interfaces.emplace_back(name, *it, &values[j][vector_index]); - return true; - } - return false; -} - hardware_interface::return_type JointStateTopicSystem::write(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { // To avoid spamming TopicBased's joint command topic we check the difference between the joint states and // the current joint commands, if it's smaller than a threshold we don't publish it. - const auto diff = std::transform_reduce( - joint_state_values_[POSITION_INTERFACE_INDEX].cbegin(), joint_state_values_[POSITION_INTERFACE_INDEX].cend(), - joint_command_values_[POSITION_INTERFACE_INDEX].cbegin(), 0.0, - [](const auto d1, const auto d2) { return std::abs(d1) + std::abs(d2); }, std::minus{}); - if (diff <= trigger_joint_command_threshold_) - { - return hardware_interface::return_type::OK; - } + // const auto diff = std::transform_reduce( + // joint_state_values_[POSITION_INTERFACE_INDEX].cbegin(), joint_state_values_[POSITION_INTERFACE_INDEX].cend(), + // joint_command_values_[POSITION_INTERFACE_INDEX].cbegin(), 0.0, + // [](const auto d1, const auto d2) { return std::abs(d1) + std::abs(d2); }, std::minus{}); + // if (diff <= trigger_joint_command_threshold_) + // { + // return hardware_interface::return_type::OK; + // } sensor_msgs::msg::JointState joint_state; - for (std::size_t i = 0; i < get_hardware_info().joints.size(); ++i) + const auto& joints = get_hardware_info().joints; + for (std::size_t i = 0; i < joints.size(); ++i) { - joint_state.name.push_back(get_hardware_info().joints[i].name); + joint_state.name.push_back(joints[i].name); joint_state.header.stamp = get_node()->now(); // only send commands to the interfaces that are defined for this joint - for (const auto& interface : get_hardware_info().joints[i].command_interfaces) + for (const auto& interface : joints[i].command_interfaces) { if (interface.name == hardware_interface::HW_IF_POSITION) { - joint_state.position.push_back(joint_command_values_[POSITION_INTERFACE_INDEX][i]); + joint_state.position.push_back(get_command(joints[i].name + "/" + interface.name)); } else if (interface.name == hardware_interface::HW_IF_VELOCITY) { - joint_state.velocity.push_back(joint_command_values_[VELOCITY_INTERFACE_INDEX][i]); + joint_state.velocity.push_back(get_command(joints[i].name + "/" + interface.name)); } else if (interface.name == hardware_interface::HW_IF_EFFORT) { - joint_state.effort.push_back(joint_command_values_[EFFORT_INTERFACE_INDEX][i]); + joint_state.effort.push_back(get_command(joints[i].name + "/" + interface.name)); } else { RCLCPP_WARN_ONCE(get_node()->get_logger(), "Joint '%s' has unsupported command interfaces found: %s.", - get_hardware_info().joints[i].name.c_str(), interface.name.c_str()); + joints[i].name.c_str(), interface.name.c_str()); } } } - for (const auto& mimic_joint : mimic_joints_) + // Update mimic joints + for (const auto& mimic_joint : get_hardware_info().mimic_joints) { - for (const auto& interface : get_hardware_info().joints[mimic_joint.mimicked_joint_index].command_interfaces) + const auto& mimic_joint_name = joints.at(mimic_joint.joint_index).name; + const auto& mimicked_joint_name = joints.at(mimic_joint.mimicked_joint_index).name; + if (joint_state_interfaces_.find(mimic_joint_name + "/" + hardware_interface::HW_IF_POSITION) != + joint_state_interfaces_.end()) { - if (interface.name == hardware_interface::HW_IF_POSITION) - { - joint_state.position[mimic_joint.joint_index] = - mimic_joint.multiplier * joint_state.position[mimic_joint.mimicked_joint_index]; - } - else if (interface.name == hardware_interface::HW_IF_VELOCITY) - { - joint_state.velocity[mimic_joint.joint_index] = - mimic_joint.multiplier * joint_state.velocity[mimic_joint.mimicked_joint_index]; - } - else if (interface.name == hardware_interface::HW_IF_EFFORT) - { - joint_state.effort[mimic_joint.joint_index] = - mimic_joint.multiplier * joint_state.effort[mimic_joint.mimicked_joint_index]; - } + set_state(mimic_joint_name + "/" + hardware_interface::HW_IF_POSITION, + mimic_joint.offset + + mimic_joint.multiplier * get_state(mimicked_joint_name + "/" + hardware_interface::HW_IF_POSITION)); + } + if (joint_state_interfaces_.find(mimic_joint_name + "/" + hardware_interface::HW_IF_VELOCITY) != + joint_state_interfaces_.end()) + { + set_state(mimic_joint_name + "/" + hardware_interface::HW_IF_VELOCITY, + mimic_joint.multiplier * get_state(mimicked_joint_name + "/" + hardware_interface::HW_IF_VELOCITY)); + } + if (joint_state_interfaces_.find(mimic_joint_name + "/" + hardware_interface::HW_IF_ACCELERATION) != + joint_state_interfaces_.end()) + { + set_state(mimic_joint_name + "/" + hardware_interface::HW_IF_ACCELERATION, + mimic_joint.multiplier * get_state(mimicked_joint_name + "/" + hardware_interface::HW_IF_ACCELERATION)); } } From 5cf0fccf70df6ce28f6eec9a1cfa95e386b5e69a Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 15 Oct 2025 20:20:11 +0000 Subject: [PATCH 20/29] Fix mimic and add gripper test --- .../CMakeLists.txt | 7 + .../joint_state_topic_hardware_interface.hpp | 2 + .../package.xml | 1 + .../joint_state_topic_hardware_interface.cpp | 126 +++++++------ .../test/gripper/gripper.launch.py | 56 ++++++ .../test/gripper/gripper.urdf.xacro | 98 ++++++++++ .../gripper/gripper_controller_position.yaml | 13 ++ .../test/gripper/position.test.py | 174 ++++++++++++++++++ .../test/gripper/test_gripper.cpp | 57 ++++++ 9 files changed, 479 insertions(+), 55 deletions(-) create mode 100644 joint_state_topic_hardware_interface/test/gripper/gripper.launch.py create mode 100644 joint_state_topic_hardware_interface/test/gripper/gripper.urdf.xacro create mode 100644 joint_state_topic_hardware_interface/test/gripper/gripper_controller_position.yaml create mode 100644 joint_state_topic_hardware_interface/test/gripper/position.test.py create mode 100644 joint_state_topic_hardware_interface/test/gripper/test_gripper.cpp diff --git a/joint_state_topic_hardware_interface/CMakeLists.txt b/joint_state_topic_hardware_interface/CMakeLists.txt index 69d3148..6c161a9 100644 --- a/joint_state_topic_hardware_interface/CMakeLists.txt +++ b/joint_state_topic_hardware_interface/CMakeLists.txt @@ -81,12 +81,19 @@ if(BUILD_TESTING) rclcpp::rclcpp rclcpp_action::rclcpp_action ) + add_executable(test_gripper test/gripper/test_gripper.cpp) + target_link_libraries(test_gripper PUBLIC + ${std_msgs_TARGETS} + rclcpp::rclcpp + ) function(add_ros_isolated_launch_test path) set(RUNNER "${ament_cmake_ros_DIR}/run_test_isolated.py") add_launch_test("${path}" RUNNER "${RUNNER}" ${ARGN}) endfunction() add_ros_isolated_launch_test( test/rrr/position.test.py) + add_ros_isolated_launch_test( + test/gripper/position.test.py) endif() pluginlib_export_plugin_description_file(hardware_interface joint_state_topic_hardware_interface_plugin_description.xml) diff --git a/joint_state_topic_hardware_interface/include/joint_state_topic_hardware_interface/joint_state_topic_hardware_interface.hpp b/joint_state_topic_hardware_interface/include/joint_state_topic_hardware_interface/joint_state_topic_hardware_interface.hpp index 51482a8..063bdf3 100644 --- a/joint_state_topic_hardware_interface/include/joint_state_topic_hardware_interface/joint_state_topic_hardware_interface.hpp +++ b/joint_state_topic_hardware_interface/include/joint_state_topic_hardware_interface/joint_state_topic_hardware_interface.hpp @@ -38,6 +38,8 @@ class JointStateTopicSystem : public hardware_interface::SystemInterface public: CallbackReturn on_init(const hardware_interface::HardwareComponentInterfaceParams& params) override; + CallbackReturn on_activate(const rclcpp_lifecycle::State& /*previous_state*/) override; + hardware_interface::return_type read(const rclcpp::Time& time, const rclcpp::Duration& period) override; hardware_interface::return_type write(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) override; diff --git a/joint_state_topic_hardware_interface/package.xml b/joint_state_topic_hardware_interface/package.xml index 212d542..b6e5cd8 100644 --- a/joint_state_topic_hardware_interface/package.xml +++ b/joint_state_topic_hardware_interface/package.xml @@ -30,6 +30,7 @@ ament_cmake_ros control_msgs controller_manager + forward_command_controller joint_state_broadcaster joint_trajectory_controller launch_ros diff --git a/joint_state_topic_hardware_interface/src/joint_state_topic_hardware_interface.cpp b/joint_state_topic_hardware_interface/src/joint_state_topic_hardware_interface.cpp index 0f22ba9..f8ca058 100644 --- a/joint_state_topic_hardware_interface/src/joint_state_topic_hardware_interface.cpp +++ b/joint_state_topic_hardware_interface/src/joint_state_topic_hardware_interface.cpp @@ -55,34 +55,6 @@ CallbackReturn JointStateTopicSystem::on_init(const hardware_interface::Hardware return CallbackReturn::ERROR; } - // Update mimic joints - // TODO(christophfroehlich): move that to the handles - const auto& joints = get_hardware_info().joints; - for (const auto& mimic_joint : get_hardware_info().mimic_joints) - { - const auto& mimic_joint_name = joints.at(mimic_joint.joint_index).name; - const auto& mimicked_joint_name = joints.at(mimic_joint.mimicked_joint_index).name; - if (joint_state_interfaces_.find(mimic_joint_name + "/" + hardware_interface::HW_IF_POSITION) != - joint_state_interfaces_.end()) - { - set_state(mimic_joint_name + "/" + hardware_interface::HW_IF_POSITION, - mimic_joint.offset + - mimic_joint.multiplier * get_state(mimicked_joint_name + "/" + hardware_interface::HW_IF_POSITION)); - } - if (joint_state_interfaces_.find(mimic_joint_name + "/" + hardware_interface::HW_IF_VELOCITY) != - joint_state_interfaces_.end()) - { - set_state(mimic_joint_name + "/" + hardware_interface::HW_IF_VELOCITY, - mimic_joint.multiplier * get_state(mimicked_joint_name + "/" + hardware_interface::HW_IF_VELOCITY)); - } - if (joint_state_interfaces_.find(mimic_joint_name + "/" + hardware_interface::HW_IF_ACCELERATION) != - joint_state_interfaces_.end()) - { - set_state(mimic_joint_name + "/" + hardware_interface::HW_IF_ACCELERATION, - mimic_joint.multiplier * get_state(mimicked_joint_name + "/" + hardware_interface::HW_IF_ACCELERATION)); - } - } - const auto get_hardware_parameter = [this](const std::string& parameter_name, const std::string& default_value) { if (auto it = get_hardware_info().hardware_parameters.find(parameter_name); it != get_hardware_info().hardware_parameters.end()) @@ -114,17 +86,61 @@ CallbackReturn JointStateTopicSystem::on_init(const hardware_interface::Hardware return CallbackReturn::SUCCESS; } +hardware_interface::CallbackReturn JointStateTopicSystem::on_activate(const rclcpp_lifecycle::State& /*previous_state*/) +{ + // initialize mimic joints + // TODO(christophfroehlich): move that to the handles + const auto& joints = get_hardware_info().joints; + for (const auto& mimic_joint : get_hardware_info().mimic_joints) + { + const auto& mimic_joint_name = joints.at(mimic_joint.joint_index).name; + const auto& mimicked_joint_name = joints.at(mimic_joint.mimicked_joint_index).name; + if (joint_state_interfaces_.find(mimic_joint_name + "/" + hardware_interface::HW_IF_POSITION) != + joint_state_interfaces_.end()) + { + set_state(mimic_joint_name + "/" + hardware_interface::HW_IF_POSITION, + mimic_joint.offset + + mimic_joint.multiplier * get_state(mimicked_joint_name + "/" + hardware_interface::HW_IF_POSITION)); + } + if (joint_state_interfaces_.find(mimic_joint_name + "/" + hardware_interface::HW_IF_VELOCITY) != + joint_state_interfaces_.end()) + { + set_state(mimic_joint_name + "/" + hardware_interface::HW_IF_VELOCITY, + mimic_joint.multiplier * get_state(mimicked_joint_name + "/" + hardware_interface::HW_IF_VELOCITY)); + } + if (joint_state_interfaces_.find(mimic_joint_name + "/" + hardware_interface::HW_IF_ACCELERATION) != + joint_state_interfaces_.end()) + { + set_state(mimic_joint_name + "/" + hardware_interface::HW_IF_ACCELERATION, + mimic_joint.multiplier * get_state(mimicked_joint_name + "/" + hardware_interface::HW_IF_ACCELERATION)); + } + } + + return CallbackReturn::SUCCESS; +} + hardware_interface::return_type JointStateTopicSystem::read(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { + const auto& joints = get_hardware_info().joints; for (std::size_t i = 0; i < latest_joint_state_.name.size(); ++i) { - const auto& joints = get_hardware_info().joints; auto it = std::find_if(joints.begin(), joints.end(), [&joint_name = std::as_const(latest_joint_state_.name[i])]( const hardware_interface::ComponentInfo& info) { return joint_name == info.name; }); if (it != joints.end()) { + auto it2 = std::find_if(get_hardware_info().mimic_joints.begin(), get_hardware_info().mimic_joints.end(), + [idx = static_cast(std::distance(joints.begin(), it))]( + const hardware_interface::MimicJoint& mimic_joint) { + return idx == mimic_joint.joint_index; + }); + if (it2 != get_hardware_info().mimic_joints.end()) + { + // mimic joints are updated at the end of this function + continue; + } + if (std::isfinite(latest_joint_state_.position.at(i))) { if (sum_wrapped_joint_states_) @@ -152,6 +168,32 @@ hardware_interface::return_type JointStateTopicSystem::read(const rclcpp::Time& } } + // Update mimic joints + for (const auto& mimic_joint : get_hardware_info().mimic_joints) + { + const auto& mimic_joint_name = joints.at(mimic_joint.joint_index).name; + const auto& mimicked_joint_name = joints.at(mimic_joint.mimicked_joint_index).name; + if (joint_state_interfaces_.find(mimic_joint_name + "/" + hardware_interface::HW_IF_POSITION) != + joint_state_interfaces_.end()) + { + set_state(mimic_joint_name + "/" + hardware_interface::HW_IF_POSITION, + mimic_joint.offset + + mimic_joint.multiplier * get_state(mimicked_joint_name + "/" + hardware_interface::HW_IF_POSITION)); + } + if (joint_state_interfaces_.find(mimic_joint_name + "/" + hardware_interface::HW_IF_VELOCITY) != + joint_state_interfaces_.end()) + { + set_state(mimic_joint_name + "/" + hardware_interface::HW_IF_VELOCITY, + mimic_joint.multiplier * get_state(mimicked_joint_name + "/" + hardware_interface::HW_IF_VELOCITY)); + } + if (joint_state_interfaces_.find(mimic_joint_name + "/" + hardware_interface::HW_IF_ACCELERATION) != + joint_state_interfaces_.end()) + { + set_state(mimic_joint_name + "/" + hardware_interface::HW_IF_ACCELERATION, + mimic_joint.multiplier * get_state(mimicked_joint_name + "/" + hardware_interface::HW_IF_ACCELERATION)); + } + } + return hardware_interface::return_type::OK; } @@ -198,32 +240,6 @@ hardware_interface::return_type JointStateTopicSystem::write(const rclcpp::Time& } } - // Update mimic joints - for (const auto& mimic_joint : get_hardware_info().mimic_joints) - { - const auto& mimic_joint_name = joints.at(mimic_joint.joint_index).name; - const auto& mimicked_joint_name = joints.at(mimic_joint.mimicked_joint_index).name; - if (joint_state_interfaces_.find(mimic_joint_name + "/" + hardware_interface::HW_IF_POSITION) != - joint_state_interfaces_.end()) - { - set_state(mimic_joint_name + "/" + hardware_interface::HW_IF_POSITION, - mimic_joint.offset + - mimic_joint.multiplier * get_state(mimicked_joint_name + "/" + hardware_interface::HW_IF_POSITION)); - } - if (joint_state_interfaces_.find(mimic_joint_name + "/" + hardware_interface::HW_IF_VELOCITY) != - joint_state_interfaces_.end()) - { - set_state(mimic_joint_name + "/" + hardware_interface::HW_IF_VELOCITY, - mimic_joint.multiplier * get_state(mimicked_joint_name + "/" + hardware_interface::HW_IF_VELOCITY)); - } - if (joint_state_interfaces_.find(mimic_joint_name + "/" + hardware_interface::HW_IF_ACCELERATION) != - joint_state_interfaces_.end()) - { - set_state(mimic_joint_name + "/" + hardware_interface::HW_IF_ACCELERATION, - mimic_joint.multiplier * get_state(mimicked_joint_name + "/" + hardware_interface::HW_IF_ACCELERATION)); - } - } - if (rclcpp::ok()) { topic_based_joint_commands_publisher_->publish(joint_state); diff --git a/joint_state_topic_hardware_interface/test/gripper/gripper.launch.py b/joint_state_topic_hardware_interface/test/gripper/gripper.launch.py new file mode 100644 index 0000000..3f1f7e4 --- /dev/null +++ b/joint_state_topic_hardware_interface/test/gripper/gripper.launch.py @@ -0,0 +1,56 @@ +# Copyright 2025 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. + +import os +from pathlib import Path + +import xacro +from launch import LaunchDescription +from launch_ros.actions import Node + +SCRIPT_PATH = Path(os.path.realpath(__file__)).parent + + +def generate_launch_description(): + ros2_controllers_file = str(Path(SCRIPT_PATH / "gripper_controller_position.yaml")) + robot_description = { + "robot_description": xacro.process_file( + SCRIPT_PATH / "gripper.urdf.xacro" + ).toxml(), + } + controllers = ["joint_state_broadcaster", "gripper_controller"] + return LaunchDescription( + [ + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + parameters=[robot_description], + ), + Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_description, ros2_controllers_file], + output="screen", + ), + ] + + [ + Node( + package="controller_manager", + executable="spawner", + arguments=[controller, "--param-file", ros2_controllers_file], + ) + for controller in controllers + ], + ) diff --git a/joint_state_topic_hardware_interface/test/gripper/gripper.urdf.xacro b/joint_state_topic_hardware_interface/test/gripper/gripper.urdf.xacro new file mode 100644 index 0000000..b00412d --- /dev/null +++ b/joint_state_topic_hardware_interface/test/gripper/gripper.urdf.xacro @@ -0,0 +1,98 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + joint_state_topic_hardware_interface/JointStateTopicSystem + /topic_based_joint_commands + /topic_based_joint_states + + + + + 0.15 + + + + + + + + + + + diff --git a/joint_state_topic_hardware_interface/test/gripper/gripper_controller_position.yaml b/joint_state_topic_hardware_interface/test/gripper/gripper_controller_position.yaml new file mode 100644 index 0000000..e8bedb3 --- /dev/null +++ b/joint_state_topic_hardware_interface/test/gripper/gripper_controller_position.yaml @@ -0,0 +1,13 @@ +controller_manager: + ros__parameters: + update_rate: 100 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + +gripper_controller: + ros__parameters: + type: forward_command_controller/ForwardCommandController + joints: + - right_finger_joint + interface_name: position diff --git a/joint_state_topic_hardware_interface/test/gripper/position.test.py b/joint_state_topic_hardware_interface/test/gripper/position.test.py new file mode 100644 index 0000000..779caa0 --- /dev/null +++ b/joint_state_topic_hardware_interface/test/gripper/position.test.py @@ -0,0 +1,174 @@ +# Copyright 2025 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. + +import os +import unittest +from collections import OrderedDict +from pathlib import Path + +import launch_testing +import launch_testing.markers +import numpy as np +import pytest +import rclpy +from ament_index_python.packages import get_package_prefix +from controller_manager.test_utils import ( + check_controllers_running, + check_if_js_published, + check_node_running, +) +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import PathJoinSubstitution +from launch_ros.actions import Node +from launch_testing.actions import ReadyToTest +from launch_testing.util import KeepAliveProc +from rclpy.qos import qos_profile_system_default +from sensor_msgs.msg import JointState + + +# This function specifies the processes to be run for our test +@pytest.mark.rostest +def generate_test_description(): + # This is necessary to get unbuffered output from the process under test + proc_env = os.environ.copy() + proc_env["PYTHONUNBUFFERED"] = "1" + + return LaunchDescription( + [ + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + str(Path(os.path.realpath(__file__)).parent), + "gripper.launch.py", + ], + ), + ), + ), + Node( + package="topic_tools", + executable="relay", + output="screen", + arguments=["/topic_based_joint_commands", "/topic_based_joint_states"], + ), + KeepAliveProc(), + ReadyToTest(), + ], + ) + + +class TestFixture(unittest.TestCase): + @classmethod + def setUpClass(cls): + rclpy.init() + + @classmethod + def tearDownClass(cls): + rclpy.shutdown() + + def setUp(self): + self.node = rclpy.create_node("test_node") + + # Reported joint state from joint_state_broadcaster + self.current_joint_state_subscriber = self.node.create_subscription( + JointState, + "joint_states", + self.joint_states_callback, + qos_profile_system_default, + ) + + self.joint_names = ["right_finger_joint", "left_finger_joint"] + + def tearDown(self): + self.node.destroy_node() + + def test_controller_running(self, proc_output): + cnames = ["gripper_controller", "joint_state_broadcaster"] + check_node_running(self.node, "relay") + check_controllers_running(self.node, cnames) + + def test_check_if_msgs_published(self): + check_if_js_published("/joint_states", self.joint_names) + + def test_main(self, launch_service, proc_info, proc_output): + # By default the joint_states should have the initial_value from rrr.urdf.xacro + self.node.get_logger().info("Checking initial joint states...") + current_joint_state = self.get_current_joint_state() + urdf_initial_values = [0.15, 0.15] + assert current_joint_state == urdf_initial_values, ( + f"{current_joint_state=} != {urdf_initial_values=}" + ) + + # Test setting the robot joint states via controller + # test_gripper was not installed, call it directly from build space + pkg_name = "joint_state_topic_hardware_interface" + proc_action = Node( + executable=os.path.join( + get_package_prefix(pkg_name).replace("install", "build"), + "test_gripper", + ), + output="screen", + ) + + with launch_testing.tools.launch_process( + launch_service, proc_action, proc_info, proc_output + ): + proc_info.assertWaitForShutdown(process=proc_action, timeout=300) + launch_testing.asserts.assertExitCodes( + proc_info, process=proc_action, allowable_exit_codes=[0] + ) + + self.node.get_logger().info("Checking final joint states...") + current_joint_state = self.get_current_joint_state() + final_values = [0.0, 0.0] + assert np.allclose( + current_joint_state, + final_values, + atol=1e-3, + ), f"{current_joint_state=} != {final_values=}" + + def joint_states_callback(self, msg: JointState): + self.current_joint_state = self.filter_joint_state_msg(msg) + + def get_current_joint_state(self) -> OrderedDict[str, float]: + """Get the current joint state reported by ros2_control on joint_states topic.""" + self.current_joint_state = [] + while len(self.current_joint_state) == 0: + self.node.get_logger().warning( + f"Waiting for current joint states from topic '{self.current_joint_state_subscriber.topic_name}'...", + throttle_duration_sec=2.0, + skip_first=True, + ) + rclpy.spin_once(self.node, timeout_sec=1.0) + return self.current_joint_state + + def filter_joint_state_msg(self, msg: JointState): + joint_states = [] + for joint_name in self.joint_names: + try: + index = msg.name.index(joint_name) + except ValueError: + msg = f"Joint name '{joint_name}' not in input keys {msg.name}" + raise ValueError(msg) from None + joint_states.append(msg.position[index]) + return joint_states + + +@launch_testing.post_shutdown_test() +class TestProcessPostShutdown(unittest.TestCase): + # Checks if the test has been completed with acceptable exit codes (successful codes) + def test_pass(self, proc_info): + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/joint_state_topic_hardware_interface/test/gripper/test_gripper.cpp b/joint_state_topic_hardware_interface/test/gripper/test_gripper.cpp new file mode 100644 index 0000000..569318a --- /dev/null +++ b/joint_state_topic_hardware_interface/test/gripper/test_gripper.cpp @@ -0,0 +1,57 @@ +// Copyright 2022 Open Source Robotics Foundation, Inc. +// +// 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. +// +// Author: Denis Štogl (Stogl Robotics Consulting) +// + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/float64_multi_array.hpp" + +int main(int argc, char* argv[]) +{ + rclcpp::init(argc, argv); + + std::shared_ptr node = std::make_shared("gripper_test_node"); + + auto publisher = node->create_publisher("/gripper_controller/commands", 10); + + RCLCPP_INFO(node->get_logger(), "node created"); + + std_msgs::msg::Float64MultiArray commands; + + using namespace std::chrono_literals; + + commands.data.push_back(0); + publisher->publish(commands); + std::this_thread::sleep_for(1s); + + commands.data[0] = 0.38; + publisher->publish(commands); + std::this_thread::sleep_for(1s); + + commands.data[0] = 0.19; + publisher->publish(commands); + std::this_thread::sleep_for(1s); + + commands.data[0] = 0; + publisher->publish(commands); + std::this_thread::sleep_for(1s); + rclcpp::shutdown(); + + return 0; +} From 311e3a68caf616aa6a196a9f1a97b49d22bd3e5d Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 15 Oct 2025 20:23:02 +0000 Subject: [PATCH 21/29] No need to set initial value for mimic joints --- .../joint_state_topic_hardware_interface.hpp | 2 -- .../joint_state_topic_hardware_interface.cpp | 33 ------------------- .../test/gripper/position.test.py | 2 +- .../test/gripper/test_gripper.cpp | 2 +- 4 files changed, 2 insertions(+), 37 deletions(-) diff --git a/joint_state_topic_hardware_interface/include/joint_state_topic_hardware_interface/joint_state_topic_hardware_interface.hpp b/joint_state_topic_hardware_interface/include/joint_state_topic_hardware_interface/joint_state_topic_hardware_interface.hpp index 063bdf3..51482a8 100644 --- a/joint_state_topic_hardware_interface/include/joint_state_topic_hardware_interface/joint_state_topic_hardware_interface.hpp +++ b/joint_state_topic_hardware_interface/include/joint_state_topic_hardware_interface/joint_state_topic_hardware_interface.hpp @@ -38,8 +38,6 @@ class JointStateTopicSystem : public hardware_interface::SystemInterface public: CallbackReturn on_init(const hardware_interface::HardwareComponentInterfaceParams& params) override; - CallbackReturn on_activate(const rclcpp_lifecycle::State& /*previous_state*/) override; - hardware_interface::return_type read(const rclcpp::Time& time, const rclcpp::Duration& period) override; hardware_interface::return_type write(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) override; diff --git a/joint_state_topic_hardware_interface/src/joint_state_topic_hardware_interface.cpp b/joint_state_topic_hardware_interface/src/joint_state_topic_hardware_interface.cpp index f8ca058..f585d6b 100644 --- a/joint_state_topic_hardware_interface/src/joint_state_topic_hardware_interface.cpp +++ b/joint_state_topic_hardware_interface/src/joint_state_topic_hardware_interface.cpp @@ -86,39 +86,6 @@ CallbackReturn JointStateTopicSystem::on_init(const hardware_interface::Hardware return CallbackReturn::SUCCESS; } -hardware_interface::CallbackReturn JointStateTopicSystem::on_activate(const rclcpp_lifecycle::State& /*previous_state*/) -{ - // initialize mimic joints - // TODO(christophfroehlich): move that to the handles - const auto& joints = get_hardware_info().joints; - for (const auto& mimic_joint : get_hardware_info().mimic_joints) - { - const auto& mimic_joint_name = joints.at(mimic_joint.joint_index).name; - const auto& mimicked_joint_name = joints.at(mimic_joint.mimicked_joint_index).name; - if (joint_state_interfaces_.find(mimic_joint_name + "/" + hardware_interface::HW_IF_POSITION) != - joint_state_interfaces_.end()) - { - set_state(mimic_joint_name + "/" + hardware_interface::HW_IF_POSITION, - mimic_joint.offset + - mimic_joint.multiplier * get_state(mimicked_joint_name + "/" + hardware_interface::HW_IF_POSITION)); - } - if (joint_state_interfaces_.find(mimic_joint_name + "/" + hardware_interface::HW_IF_VELOCITY) != - joint_state_interfaces_.end()) - { - set_state(mimic_joint_name + "/" + hardware_interface::HW_IF_VELOCITY, - mimic_joint.multiplier * get_state(mimicked_joint_name + "/" + hardware_interface::HW_IF_VELOCITY)); - } - if (joint_state_interfaces_.find(mimic_joint_name + "/" + hardware_interface::HW_IF_ACCELERATION) != - joint_state_interfaces_.end()) - { - set_state(mimic_joint_name + "/" + hardware_interface::HW_IF_ACCELERATION, - mimic_joint.multiplier * get_state(mimicked_joint_name + "/" + hardware_interface::HW_IF_ACCELERATION)); - } - } - - return CallbackReturn::SUCCESS; -} - hardware_interface::return_type JointStateTopicSystem::read(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { diff --git a/joint_state_topic_hardware_interface/test/gripper/position.test.py b/joint_state_topic_hardware_interface/test/gripper/position.test.py index 779caa0..45c5e3c 100644 --- a/joint_state_topic_hardware_interface/test/gripper/position.test.py +++ b/joint_state_topic_hardware_interface/test/gripper/position.test.py @@ -133,7 +133,7 @@ def test_main(self, launch_service, proc_info, proc_output): self.node.get_logger().info("Checking final joint states...") current_joint_state = self.get_current_joint_state() - final_values = [0.0, 0.0] + final_values = [0.09, 0.09] assert np.allclose( current_joint_state, final_values, diff --git a/joint_state_topic_hardware_interface/test/gripper/test_gripper.cpp b/joint_state_topic_hardware_interface/test/gripper/test_gripper.cpp index 569318a..e149639 100644 --- a/joint_state_topic_hardware_interface/test/gripper/test_gripper.cpp +++ b/joint_state_topic_hardware_interface/test/gripper/test_gripper.cpp @@ -48,7 +48,7 @@ int main(int argc, char* argv[]) publisher->publish(commands); std::this_thread::sleep_for(1s); - commands.data[0] = 0; + commands.data[0] = 0.09; publisher->publish(commands); std::this_thread::sleep_for(1s); rclcpp::shutdown(); From 4b1abd5c0df183047d6bf93b4fc8d62a4fdce063 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 15 Oct 2025 20:35:27 +0000 Subject: [PATCH 22/29] Reactivate trigger_joint_command_threshold_ --- .../joint_state_topic_hardware_interface.cpp | 46 +++++++++++-------- 1 file changed, 28 insertions(+), 18 deletions(-) diff --git a/joint_state_topic_hardware_interface/src/joint_state_topic_hardware_interface.cpp b/joint_state_topic_hardware_interface/src/joint_state_topic_hardware_interface.cpp index f585d6b..fc8d2e6 100644 --- a/joint_state_topic_hardware_interface/src/joint_state_topic_hardware_interface.cpp +++ b/joint_state_topic_hardware_interface/src/joint_state_topic_hardware_interface.cpp @@ -92,17 +92,17 @@ hardware_interface::return_type JointStateTopicSystem::read(const rclcpp::Time& const auto& joints = get_hardware_info().joints; for (std::size_t i = 0; i < latest_joint_state_.name.size(); ++i) { - auto it = std::find_if(joints.begin(), joints.end(), - [&joint_name = std::as_const(latest_joint_state_.name[i])]( - const hardware_interface::ComponentInfo& info) { return joint_name == info.name; }); + const auto it = std::find_if(joints.begin(), joints.end(), + [name = latest_joint_state_.name[i]](const hardware_interface::ComponentInfo& joint) { + return joint.name == name; + }); if (it != joints.end()) { - auto it2 = std::find_if(get_hardware_info().mimic_joints.begin(), get_hardware_info().mimic_joints.end(), - [idx = static_cast(std::distance(joints.begin(), it))]( - const hardware_interface::MimicJoint& mimic_joint) { - return idx == mimic_joint.joint_index; - }); - if (it2 != get_hardware_info().mimic_joints.end()) + if (std::find_if(get_hardware_info().mimic_joints.begin(), get_hardware_info().mimic_joints.end(), + [idx = static_cast(std::distance(joints.begin(), it))]( + const hardware_interface::MimicJoint& mimic_joint) { + return idx == mimic_joint.joint_index; + }) != get_hardware_info().mimic_joints.end()) { // mimic joints are updated at the end of this function continue; @@ -167,19 +167,29 @@ hardware_interface::return_type JointStateTopicSystem::read(const rclcpp::Time& hardware_interface::return_type JointStateTopicSystem::write(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { + const auto& joints = get_hardware_info().joints; // To avoid spamming TopicBased's joint command topic we check the difference between the joint states and // the current joint commands, if it's smaller than a threshold we don't publish it. - // const auto diff = std::transform_reduce( - // joint_state_values_[POSITION_INTERFACE_INDEX].cbegin(), joint_state_values_[POSITION_INTERFACE_INDEX].cend(), - // joint_command_values_[POSITION_INTERFACE_INDEX].cbegin(), 0.0, - // [](const auto d1, const auto d2) { return std::abs(d1) + std::abs(d2); }, std::minus{}); - // if (diff <= trigger_joint_command_threshold_) - // { - // return hardware_interface::return_type::OK; - // } + auto diff = 0.0; + for (std::size_t i = 0; i < joints.size(); ++i) + { + for (const auto& interface : joints[i].command_interfaces) + { + if (interface.name != hardware_interface::HW_IF_POSITION) + { + continue; + } + // sum the absolute difference for all joints + diff += std::abs(get_state(joints[i].name + "/" + interface.name) - + get_command(joints[i].name + "/" + interface.name)); + } + } + if (diff <= trigger_joint_command_threshold_) + { + return hardware_interface::return_type::OK; + } sensor_msgs::msg::JointState joint_state; - const auto& joints = get_hardware_info().joints; for (std::size_t i = 0; i < joints.size(); ++i) { joint_state.name.push_back(joints[i].name); From b3bb2b7977e470e1b24ea455e2f17db9e4374c04 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 17 Oct 2025 12:08:20 +0000 Subject: [PATCH 23/29] Update gmock test --- ...nt_state_topic_hardware_interface_test.cpp | 417 ++++++++++++++++-- 1 file changed, 390 insertions(+), 27 deletions(-) diff --git a/joint_state_topic_hardware_interface/test/joint_state_topic_hardware_interface_test.cpp b/joint_state_topic_hardware_interface/test/joint_state_topic_hardware_interface_test.cpp index 5816585..927e2c4 100644 --- a/joint_state_topic_hardware_interface/test/joint_state_topic_hardware_interface_test.cpp +++ b/joint_state_topic_hardware_interface/test/joint_state_topic_hardware_interface_test.cpp @@ -29,7 +29,121 @@ #include #include -TEST(TestTopicBasedSystem, load_topic_based_system_2dof) +namespace +{ +const auto TIME = rclcpp::Time(0); +const auto PERIOD = rclcpp::Duration::from_seconds(0.1); // 0.1 seconds for easier math +const auto COMPARE_DELTA = 0.0001; +} // namespace + +class TestTopicBasedSystem : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() override + { + node_ = std::make_shared(::testing::UnitTest::GetInstance()->current_test_info()->name()); + + INITIALIZE_ROS2_CONTROL_INTROSPECTION_REGISTRY(node_, hardware_interface::DEFAULT_INTROSPECTION_TOPIC, + hardware_interface::DEFAULT_REGISTRY_KEY); + } + + void TearDown() override + { + node_.reset(); + } + + rclcpp::Node::SharedPtr node_; +}; +class TestableResourceManager : public hardware_interface::ResourceManager +{ +public: + friend TestTopicBasedSystem; + + FRIEND_TEST(TestTopicBasedSystem, load_topic_based_system_2dof); + FRIEND_TEST(TestTopicBasedSystem, load_topic_based_system_2dof_missing_position); + FRIEND_TEST(TestTopicBasedSystem, load_topic_based_system_with_mimic_joint); + FRIEND_TEST(TestTopicBasedSystem, TestTopicBasedSystem_load_topic_based_system_with_mimic_joint_missing_position); + + // The API of the ResourceManager has changed in hardware_interface 4.13.0 +#if HARDWARE_INTERFACE_VERSION_GTE(4, 13, 0) + explicit TestableResourceManager(const hardware_interface::ResourceManagerParams& params, bool load) + : hardware_interface::ResourceManager(params, load) + { + } +#else + explicit TestableResourceManager(const std::string& urdf, bool validate_interfaces = true, bool activate_all = false) + : hardware_interface::ResourceManager(urdf, validate_interfaces, activate_all) + { + } +#endif + + explicit TestableResourceManager(rclcpp::Node::SharedPtr node) + : hardware_interface::ResourceManager(node->get_node_clock_interface(), node->get_node_logging_interface()) + { + } + + explicit TestableResourceManager(rclcpp::Node::SharedPtr node, const std::string& urdf, bool activate_all = false, + unsigned int cm_update_rate = 100) + : hardware_interface::ResourceManager(urdf, node->get_node_clock_interface(), node->get_node_logging_interface(), + activate_all, cm_update_rate) + { + } +}; + +void set_components_state(TestableResourceManager& rm, const std::vector& components, + const uint8_t state_id, const std::string& state_name) +{ + for (const auto& component : components) + { + rclcpp_lifecycle::State state(state_id, state_name); + rm.set_component_state(component, state); + } +} + +auto configure_components = [](TestableResourceManager& rm, + const std::vector& components = { "GenericSystem2dof" }) { + set_components_state(rm, components, lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + hardware_interface::lifecycle_state_names::INACTIVE); +}; + +auto activate_components = [](TestableResourceManager& rm, + const std::vector& components = { "GenericSystem2dof" }) { + set_components_state(rm, components, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + hardware_interface::lifecycle_state_names::ACTIVE); +}; + +auto deactivate_components = [](TestableResourceManager& rm, + const std::vector& components = { "GenericSystem2dof" }) { + set_components_state(rm, components, lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + hardware_interface::lifecycle_state_names::INACTIVE); +}; + +TestableResourceManager init_rm(rclcpp::Node::SharedPtr node, const std::string& urdf) +{ + // The API of the ResourceManager has changed in hardware_interface 4.13.0 +#if HARDWARE_INTERFACE_VERSION_GTE(4, 13, 0) + hardware_interface::ResourceManagerParams params; + params.robot_description = urdf; + params.clock = node->get_clock(); + params.logger = node->get_logger(); + params.executor = std::make_shared(); + return TestableResourceManager(params, true); +#else + return TestableResourceManager(urdf, true, false); +#endif +} + +TEST_F(TestTopicBasedSystem, load_topic_based_system_2dof) { const std::string hardware_system_2dof_topic_based = R"( @@ -42,46 +156,295 @@ TEST(TestTopicBasedSystem, load_topic_based_system_2dof) - + + 1.57 + - + + -1.57 + )"; auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_topic_based + ros2_control_test_assets::urdf_tail; - auto node = std::make_shared("test_topic_based_system"); -// The API of the ResourceManager has changed in hardware_interface 4.13.0 -#if HARDWARE_INTERFACE_VERSION_GTE(4, 13, 0) - hardware_interface::ResourceManagerParams params; - params.robot_description = urdf; - params.clock = node->get_clock(); - params.logger = node->get_logger(); - params.executor = std::make_shared(); - try - { - hardware_interface::ResourceManager rm(params, true); - } - catch (const std::exception& e) - { - std::cerr << "Exception caught: " << e.what() << std::endl; - FAIL() << "Exception thrown during ResourceManager construction: " << e.what(); - } -#else - ASSERT_NO_THROW(hardware_interface::ResourceManager rm(urdf, true, false)); -#endif + TestableResourceManager rm = init_rm(node_, urdf); + + // Activate components to get all interfaces available + activate_components(rm, { "JointStateTopicBasedSystem2dof" }); + + // Check interfaces + EXPECT_EQ(1u, rm.system_components_size()); + ASSERT_EQ(4u, rm.state_interface_keys().size()); + EXPECT_TRUE(rm.state_interface_exists("joint1/position")); + EXPECT_TRUE(rm.state_interface_exists("joint1/velocity")); + EXPECT_TRUE(rm.state_interface_exists("joint2/position")); + EXPECT_TRUE(rm.state_interface_exists("joint2/velocity")); + + ASSERT_EQ(4u, rm.command_interface_keys().size()); + EXPECT_TRUE(rm.state_interface_exists("joint1/position")); + EXPECT_TRUE(rm.state_interface_exists("joint1/velocity")); + EXPECT_TRUE(rm.state_interface_exists("joint2/position")); + EXPECT_TRUE(rm.state_interface_exists("joint2/velocity")); + + // Check initial values + hardware_interface::LoanedStateInterface j1_p_s = rm.claim_state_interface("joint1/position"); + hardware_interface::LoanedStateInterface j1_v_s = rm.claim_state_interface("joint1/velocity"); + hardware_interface::LoanedStateInterface j2_p_s = rm.claim_state_interface("joint2/position"); + hardware_interface::LoanedStateInterface j2_v_s = rm.claim_state_interface("joint2/velocity"); + hardware_interface::LoanedCommandInterface j1_p_c = rm.claim_command_interface("joint1/position"); + hardware_interface::LoanedCommandInterface j1_v_c = rm.claim_command_interface("joint1/velocity"); + hardware_interface::LoanedCommandInterface j2_p_c = rm.claim_command_interface("joint2/position"); + hardware_interface::LoanedCommandInterface j2_v_c = rm.claim_command_interface("joint2/velocity"); + + EXPECT_EQ(j1_p_s.get_optional().value(), 1.57); + EXPECT_TRUE(std::isnan(j1_v_s.get_optional().value())); + EXPECT_EQ(j2_p_s.get_optional().value(), -1.57); + EXPECT_TRUE(std::isnan(j2_v_s.get_optional().value())); + EXPECT_TRUE(std::isnan(j1_p_c.get_optional().value())); + EXPECT_TRUE(std::isnan(j1_v_c.get_optional().value())); + EXPECT_TRUE(std::isnan(j2_p_c.get_optional().value())); + EXPECT_TRUE(std::isnan(j2_v_c.get_optional().value())); + + // set some new values in commands + ASSERT_TRUE(j1_p_c.set_value(0.11)); + ASSERT_TRUE(j1_v_c.set_value(0.12)); + ASSERT_TRUE(j2_p_c.set_value(0.13)); + ASSERT_TRUE(j2_v_c.set_value(0.14)); + + hardware_interface::return_type ret; + ASSERT_NO_THROW(ret = rm.read(TIME, PERIOD).result); + ASSERT_EQ(ret, hardware_interface::return_type::OK); + ASSERT_NO_THROW(ret = rm.write(TIME, PERIOD).result); + ASSERT_EQ(ret, hardware_interface::return_type::OK); + + // command is not propagated to state until topic from robot is received + // same holds for velocity + EXPECT_EQ(j1_p_s.get_optional().value(), 1.57); + EXPECT_TRUE(std::isnan(j1_v_s.get_optional().value())); + EXPECT_EQ(j2_p_s.get_optional().value(), -1.57); + EXPECT_TRUE(std::isnan(j2_v_s.get_optional().value())); + + EXPECT_EQ(j1_p_c.get_optional().value(), 0.11); + EXPECT_EQ(j1_v_c.get_optional().value(), 0.12); + EXPECT_EQ(j2_p_c.get_optional().value(), 0.13); + EXPECT_EQ(j2_v_c.get_optional().value(), 0.14); } -int main(int argc, char** argv) +TEST_F(TestTopicBasedSystem, load_topic_based_system_2dof_missing_position) { - testing::InitGoogleMock(&argc, argv); - rclcpp::init(argc, argv); + const std::string hardware_system_2dof_topic_based = + R"( + + + joint_state_topic_hardware_interface/JointStateTopicSystem + /topic_based_joint_commands + /topic_based_custom_joint_states + + + + + + + + + + +)"; + auto urdf = + ros2_control_test_assets::urdf_head + hardware_system_2dof_topic_based + ros2_control_test_assets::urdf_tail; + + TestableResourceManager rm = init_rm(node_, urdf); + + // Activate components to get all interfaces available + activate_components(rm, { "JointStateTopicBasedSystem2dof" }); + + // Check interfaces + EXPECT_EQ(1u, rm.system_components_size()); + ASSERT_EQ(2u, rm.state_interface_keys().size()); + EXPECT_TRUE(rm.state_interface_exists("joint1/velocity")); + EXPECT_TRUE(rm.state_interface_exists("joint2/velocity")); + + ASSERT_EQ(2u, rm.command_interface_keys().size()); + EXPECT_TRUE(rm.state_interface_exists("joint1/velocity")); + EXPECT_TRUE(rm.state_interface_exists("joint2/velocity")); + + // Check initial values + hardware_interface::LoanedStateInterface j1_v_s = rm.claim_state_interface("joint1/velocity"); + hardware_interface::LoanedStateInterface j2_v_s = rm.claim_state_interface("joint2/velocity"); + hardware_interface::LoanedCommandInterface j1_v_c = rm.claim_command_interface("joint1/velocity"); + hardware_interface::LoanedCommandInterface j2_v_c = rm.claim_command_interface("joint2/velocity"); + + EXPECT_TRUE(std::isnan(j1_v_s.get_optional().value())); + EXPECT_TRUE(std::isnan(j2_v_s.get_optional().value())); + EXPECT_TRUE(std::isnan(j1_v_c.get_optional().value())); + EXPECT_TRUE(std::isnan(j2_v_c.get_optional().value())); + + // set some new values in commands + ASSERT_TRUE(j1_v_c.set_value(0.12)); + ASSERT_TRUE(j2_v_c.set_value(0.14)); + + hardware_interface::return_type ret; + ASSERT_NO_THROW(ret = rm.read(TIME, PERIOD).result); + ASSERT_EQ(ret, hardware_interface::return_type::OK); + ASSERT_NO_THROW(ret = rm.write(TIME, PERIOD).result); + ASSERT_EQ(ret, hardware_interface::return_type::OK); + + // command is not propagated to state until topic from robot is received + // same holds for velocity + EXPECT_TRUE(std::isnan(j1_v_s.get_optional().value())); + EXPECT_TRUE(std::isnan(j2_v_s.get_optional().value())); + + EXPECT_EQ(j1_v_c.get_optional().value(), 0.12); + EXPECT_EQ(j2_v_c.get_optional().value(), 0.14); +} + +TEST_F(TestTopicBasedSystem, load_topic_based_system_with_mimic_joint) +{ + const std::string hardware_system_2dof_with_mimic_joint = + R"( + + + joint_state_topic_hardware_interface/JointStateTopicSystem + /topic_based_joint_commands + /topic_based_custom_joint_states + + + + + 1.57 + + + + + + + + +)"; + auto urdf = ros2_control_test_assets::urdf_head_mimic + hardware_system_2dof_with_mimic_joint + + ros2_control_test_assets::urdf_tail; + + TestableResourceManager rm = init_rm(node_, urdf); + + // Activate components to get all interfaces available + activate_components(rm, { "JointStateTopicBasedSystem2dofMimic" }); + + // Check interfaces + EXPECT_EQ(1u, rm.system_components_size()); + ASSERT_EQ(4u, rm.state_interface_keys().size()); + EXPECT_TRUE(rm.state_interface_exists("joint1/position")); + EXPECT_TRUE(rm.state_interface_exists("joint1/velocity")); + EXPECT_TRUE(rm.state_interface_exists("joint2/position")); + EXPECT_TRUE(rm.state_interface_exists("joint2/velocity")); + + ASSERT_EQ(1u, rm.command_interface_keys().size()); + EXPECT_TRUE(rm.state_interface_exists("joint1/position")); + + // Check initial values + hardware_interface::LoanedStateInterface j1_p_s = rm.claim_state_interface("joint1/position"); + hardware_interface::LoanedStateInterface j1_v_s = rm.claim_state_interface("joint1/velocity"); + hardware_interface::LoanedStateInterface j2_p_s = rm.claim_state_interface("joint2/position"); + hardware_interface::LoanedStateInterface j2_v_s = rm.claim_state_interface("joint2/velocity"); + hardware_interface::LoanedCommandInterface j1_p_c = rm.claim_command_interface("joint1/position"); + + EXPECT_EQ(j1_p_s.get_optional().value(), 1.57); + EXPECT_TRUE(std::isnan(j1_v_s.get_optional().value())); + EXPECT_TRUE(std::isnan(j2_p_s.get_optional().value())); + EXPECT_TRUE(std::isnan(j2_v_s.get_optional().value())); + EXPECT_TRUE(std::isnan(j1_p_c.get_optional().value())); + + // set some new values in commands + ASSERT_TRUE(j1_p_c.set_value(0.11)); + + hardware_interface::return_type ret; + ASSERT_NO_THROW(ret = rm.read(TIME, PERIOD).result); + ASSERT_EQ(ret, hardware_interface::return_type::OK); + ASSERT_NO_THROW(ret = rm.write(TIME, PERIOD).result); + ASSERT_EQ(ret, hardware_interface::return_type::OK); + + // command is not propagated to state until topic from robot is received + // same holds for velocity + // but mimic joint should update its state based on the first joint state + EXPECT_EQ(j1_p_s.get_optional().value(), 1.57); + EXPECT_TRUE(std::isnan(j1_v_s.get_optional().value())); + EXPECT_EQ(j2_p_s.get_optional().value(), -2 * 1.57); + EXPECT_TRUE(std::isnan(j2_v_s.get_optional().value())); + EXPECT_EQ(j1_p_c.get_optional().value(), 0.11); +} + +TEST_F(TestTopicBasedSystem, load_topic_based_system_with_mimic_joint_missing_position) +{ + const std::string hardware_system_2dof_with_mimic_joint = + R"( + + + joint_state_topic_hardware_interface/JointStateTopicSystem + /topic_based_joint_commands + /topic_based_custom_joint_states + + + + + 1.57 + + + + + + + +)"; + auto urdf = ros2_control_test_assets::urdf_head_mimic + hardware_system_2dof_with_mimic_joint + + ros2_control_test_assets::urdf_tail; + + TestableResourceManager rm = init_rm(node_, urdf); + + // Activate components to get all interfaces available + activate_components(rm, { "JointStateTopicBasedSystem2dofMimic" }); + + // Check interfaces + EXPECT_EQ(1u, rm.system_components_size()); + ASSERT_EQ(3u, rm.state_interface_keys().size()); + EXPECT_TRUE(rm.state_interface_exists("joint1/position")); + EXPECT_TRUE(rm.state_interface_exists("joint1/velocity")); + EXPECT_FALSE(rm.state_interface_exists("joint2/position")); + EXPECT_TRUE(rm.state_interface_exists("joint2/velocity")); + + ASSERT_EQ(1u, rm.command_interface_keys().size()); + EXPECT_TRUE(rm.state_interface_exists("joint1/position")); + + // Check initial values + hardware_interface::LoanedStateInterface j1_p_s = rm.claim_state_interface("joint1/position"); + hardware_interface::LoanedStateInterface j1_v_s = rm.claim_state_interface("joint1/velocity"); + hardware_interface::LoanedStateInterface j2_v_s = rm.claim_state_interface("joint2/velocity"); + hardware_interface::LoanedCommandInterface j1_p_c = rm.claim_command_interface("joint1/position"); + + EXPECT_EQ(j1_p_s.get_optional().value(), 1.57); + EXPECT_TRUE(std::isnan(j1_v_s.get_optional().value())); + EXPECT_TRUE(std::isnan(j2_v_s.get_optional().value())); + EXPECT_TRUE(std::isnan(j1_p_c.get_optional().value())); + + // set some new values in commands + ASSERT_TRUE(j1_p_c.set_value(0.11)); + + // should not throw even if mimic joint is missing position interface + hardware_interface::return_type ret; + ASSERT_NO_THROW(ret = rm.read(TIME, PERIOD).result); + ASSERT_EQ(ret, hardware_interface::return_type::OK); + ASSERT_NO_THROW(ret = rm.write(TIME, PERIOD).result); + ASSERT_EQ(ret, hardware_interface::return_type::OK); - return RUN_ALL_TESTS(); + // command is not propagated to state until topic from robot is received + // same holds for velocity + EXPECT_EQ(j1_p_s.get_optional().value(), 1.57); + EXPECT_TRUE(std::isnan(j1_v_s.get_optional().value())); + EXPECT_TRUE(std::isnan(j2_v_s.get_optional().value())); + EXPECT_EQ(j1_p_c.get_optional().value(), 0.11); } From 0a6fd32393c4129822e3bff64c6703215107cbce Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 17 Oct 2025 12:43:10 +0000 Subject: [PATCH 24/29] Test with received messages --- ...nt_state_topic_hardware_interface_test.cpp | 299 ++++++++++++------ 1 file changed, 211 insertions(+), 88 deletions(-) diff --git a/joint_state_topic_hardware_interface/test/joint_state_topic_hardware_interface_test.cpp b/joint_state_topic_hardware_interface/test/joint_state_topic_hardware_interface_test.cpp index 927e2c4..1f657ea 100644 --- a/joint_state_topic_hardware_interface/test/joint_state_topic_hardware_interface_test.cpp +++ b/joint_state_topic_hardware_interface/test/joint_state_topic_hardware_interface_test.cpp @@ -29,6 +29,8 @@ #include #include +#include "sensor_msgs/msg/joint_state.hpp" + namespace { const auto TIME = rclcpp::Time(0); @@ -36,6 +38,9 @@ const auto PERIOD = rclcpp::Duration::from_seconds(0.1); // 0.1 seconds for eas const auto COMPARE_DELTA = 0.0001; } // namespace +// Forward declaration +class TestableResourceManager; + class TestTopicBasedSystem : public ::testing::Test { protected: @@ -51,8 +56,12 @@ class TestTopicBasedSystem : public ::testing::Test void SetUp() override { + executor_ = std::make_shared(); node_ = std::make_shared(::testing::UnitTest::GetInstance()->current_test_info()->name()); + js_publisher_ = node_->create_publisher("/topic_based_custom_joint_states", + rclcpp::SystemDefaultsQoS()); + INITIALIZE_ROS2_CONTROL_INTROSPECTION_REGISTRY(node_, hardware_interface::DEFAULT_INTROSPECTION_TOPIC, hardware_interface::DEFAULT_REGISTRY_KEY); } @@ -63,7 +72,72 @@ class TestTopicBasedSystem : public ::testing::Test } rclcpp::Node::SharedPtr node_; + rclcpp::Executor::SharedPtr executor_; + std::unique_ptr rm_; + rclcpp::Publisher::SharedPtr js_publisher_; + + /// Publish joint_state_message + /** + * names - names of joints + * points - vector of trajectory-velocities. One point per controlled joint + */ + void publish(const std::vector& joint_names = {}, const std::vector& points_positions = {}, + const std::vector& points_velocities = {}, const std::vector& points_effort = {}) + { + int wait_count = 0; + const auto topic = js_publisher_->get_topic_name(); + while (node_->count_subscribers(topic) == 0) + { + if (wait_count >= 5) + { + auto error_msg = std::string("publishing to ") + topic + " but no node subscribes to it"; + throw std::runtime_error(error_msg); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + ++wait_count; + } + + sensor_msgs::msg::JointState state_msg; + state_msg.name = joint_names; + + state_msg.position = points_positions; + state_msg.velocity = points_velocities; + state_msg.effort = points_effort; + + js_publisher_->publish(state_msg); + } + + /** + * @brief wait_for_msg block until a new JointState is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function + */ + void wait_for_msg(const std::chrono::milliseconds& timeout = std::chrono::milliseconds{ 10 }) + { + auto until = node_->get_clock()->now() + timeout; + while (node_->get_clock()->now() < until) + { + executor_->spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + } + + void init_rm(const std::string& urdf) + { + // The API of the ResourceManager has changed in hardware_interface 4.13.0 +#if HARDWARE_INTERFACE_VERSION_GTE(4, 13, 0) + hardware_interface::ResourceManagerParams params; + params.robot_description = urdf; + params.clock = node_->get_clock(); + params.logger = node_->get_logger(); + params.executor = executor_; + rm_ = std::make_unique(params, true); +#else + rm_ = std::make_unique(urdf, true, false); +#endif + } }; + class TestableResourceManager : public hardware_interface::ResourceManager { public: @@ -128,21 +202,6 @@ auto deactivate_components = [](TestableResourceManager& rm, hardware_interface::lifecycle_state_names::INACTIVE); }; -TestableResourceManager init_rm(rclcpp::Node::SharedPtr node, const std::string& urdf) -{ - // The API of the ResourceManager has changed in hardware_interface 4.13.0 -#if HARDWARE_INTERFACE_VERSION_GTE(4, 13, 0) - hardware_interface::ResourceManagerParams params; - params.robot_description = urdf; - params.clock = node->get_clock(); - params.logger = node->get_logger(); - params.executor = std::make_shared(); - return TestableResourceManager(params, true); -#else - return TestableResourceManager(urdf, true, false); -#endif -} - TEST_F(TestTopicBasedSystem, load_topic_based_system_2dof) { const std::string hardware_system_2dof_topic_based = @@ -174,34 +233,34 @@ TEST_F(TestTopicBasedSystem, load_topic_based_system_2dof) auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_topic_based + ros2_control_test_assets::urdf_tail; - TestableResourceManager rm = init_rm(node_, urdf); + init_rm(urdf); // Activate components to get all interfaces available - activate_components(rm, { "JointStateTopicBasedSystem2dof" }); + activate_components(*rm_, { "JointStateTopicBasedSystem2dof" }); // Check interfaces - EXPECT_EQ(1u, rm.system_components_size()); - ASSERT_EQ(4u, rm.state_interface_keys().size()); - EXPECT_TRUE(rm.state_interface_exists("joint1/position")); - EXPECT_TRUE(rm.state_interface_exists("joint1/velocity")); - EXPECT_TRUE(rm.state_interface_exists("joint2/position")); - EXPECT_TRUE(rm.state_interface_exists("joint2/velocity")); - - ASSERT_EQ(4u, rm.command_interface_keys().size()); - EXPECT_TRUE(rm.state_interface_exists("joint1/position")); - EXPECT_TRUE(rm.state_interface_exists("joint1/velocity")); - EXPECT_TRUE(rm.state_interface_exists("joint2/position")); - EXPECT_TRUE(rm.state_interface_exists("joint2/velocity")); + EXPECT_EQ(1u, rm_->system_components_size()); + ASSERT_EQ(4u, rm_->state_interface_keys().size()); + EXPECT_TRUE(rm_->state_interface_exists("joint1/position")); + EXPECT_TRUE(rm_->state_interface_exists("joint1/velocity")); + EXPECT_TRUE(rm_->state_interface_exists("joint2/position")); + EXPECT_TRUE(rm_->state_interface_exists("joint2/velocity")); + + ASSERT_EQ(4u, rm_->command_interface_keys().size()); + EXPECT_TRUE(rm_->state_interface_exists("joint1/position")); + EXPECT_TRUE(rm_->state_interface_exists("joint1/velocity")); + EXPECT_TRUE(rm_->state_interface_exists("joint2/position")); + EXPECT_TRUE(rm_->state_interface_exists("joint2/velocity")); // Check initial values - hardware_interface::LoanedStateInterface j1_p_s = rm.claim_state_interface("joint1/position"); - hardware_interface::LoanedStateInterface j1_v_s = rm.claim_state_interface("joint1/velocity"); - hardware_interface::LoanedStateInterface j2_p_s = rm.claim_state_interface("joint2/position"); - hardware_interface::LoanedStateInterface j2_v_s = rm.claim_state_interface("joint2/velocity"); - hardware_interface::LoanedCommandInterface j1_p_c = rm.claim_command_interface("joint1/position"); - hardware_interface::LoanedCommandInterface j1_v_c = rm.claim_command_interface("joint1/velocity"); - hardware_interface::LoanedCommandInterface j2_p_c = rm.claim_command_interface("joint2/position"); - hardware_interface::LoanedCommandInterface j2_v_c = rm.claim_command_interface("joint2/velocity"); + hardware_interface::LoanedStateInterface j1_p_s = rm_->claim_state_interface("joint1/position"); + hardware_interface::LoanedStateInterface j1_v_s = rm_->claim_state_interface("joint1/velocity"); + hardware_interface::LoanedStateInterface j2_p_s = rm_->claim_state_interface("joint2/position"); + hardware_interface::LoanedStateInterface j2_v_s = rm_->claim_state_interface("joint2/velocity"); + hardware_interface::LoanedCommandInterface j1_p_c = rm_->claim_command_interface("joint1/position"); + hardware_interface::LoanedCommandInterface j1_v_c = rm_->claim_command_interface("joint1/velocity"); + hardware_interface::LoanedCommandInterface j2_p_c = rm_->claim_command_interface("joint2/position"); + hardware_interface::LoanedCommandInterface j2_v_c = rm_->claim_command_interface("joint2/velocity"); EXPECT_EQ(j1_p_s.get_optional().value(), 1.57); EXPECT_TRUE(std::isnan(j1_v_s.get_optional().value())); @@ -219,9 +278,9 @@ TEST_F(TestTopicBasedSystem, load_topic_based_system_2dof) ASSERT_TRUE(j2_v_c.set_value(0.14)); hardware_interface::return_type ret; - ASSERT_NO_THROW(ret = rm.read(TIME, PERIOD).result); + ASSERT_NO_THROW(ret = rm_->read(TIME, PERIOD).result); ASSERT_EQ(ret, hardware_interface::return_type::OK); - ASSERT_NO_THROW(ret = rm.write(TIME, PERIOD).result); + ASSERT_NO_THROW(ret = rm_->write(TIME, PERIOD).result); ASSERT_EQ(ret, hardware_interface::return_type::OK); // command is not propagated to state until topic from robot is received @@ -235,9 +294,30 @@ TEST_F(TestTopicBasedSystem, load_topic_based_system_2dof) EXPECT_EQ(j1_v_c.get_optional().value(), 0.12); EXPECT_EQ(j2_p_c.get_optional().value(), 0.13); EXPECT_EQ(j2_v_c.get_optional().value(), 0.14); + + publish({ "joint1", "joint2" }, { 0.21, 0.23 }, { 0.22, 0.24 }); + + wait_for_msg(); + + ASSERT_NO_THROW(ret = rm_->read(TIME, PERIOD).result); + ASSERT_EQ(ret, hardware_interface::return_type::OK); + ASSERT_NO_THROW(ret = rm_->write(TIME, PERIOD).result); + ASSERT_EQ(ret, hardware_interface::return_type::OK); + + // new states should have been updated from topic + EXPECT_EQ(j1_p_s.get_optional().value(), 0.21); + EXPECT_EQ(j1_v_s.get_optional().value(), 0.22); + EXPECT_EQ(j2_p_s.get_optional().value(), 0.23); + EXPECT_EQ(j2_v_s.get_optional().value(), 0.24); + + // commands should remain unchanged + EXPECT_EQ(j1_p_c.get_optional().value(), 0.11); + EXPECT_EQ(j1_v_c.get_optional().value(), 0.12); + EXPECT_EQ(j2_p_c.get_optional().value(), 0.13); + EXPECT_EQ(j2_v_c.get_optional().value(), 0.14); } -TEST_F(TestTopicBasedSystem, load_topic_based_system_2dof_missing_position) +TEST_F(TestTopicBasedSystem, load_topic_based_system_2dof_velocity_only) { const std::string hardware_system_2dof_topic_based = R"( @@ -260,26 +340,26 @@ TEST_F(TestTopicBasedSystem, load_topic_based_system_2dof_missing_position) auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_topic_based + ros2_control_test_assets::urdf_tail; - TestableResourceManager rm = init_rm(node_, urdf); + init_rm(urdf); // Activate components to get all interfaces available - activate_components(rm, { "JointStateTopicBasedSystem2dof" }); + activate_components(*rm_, { "JointStateTopicBasedSystem2dof" }); // Check interfaces - EXPECT_EQ(1u, rm.system_components_size()); - ASSERT_EQ(2u, rm.state_interface_keys().size()); - EXPECT_TRUE(rm.state_interface_exists("joint1/velocity")); - EXPECT_TRUE(rm.state_interface_exists("joint2/velocity")); + EXPECT_EQ(1u, rm_->system_components_size()); + ASSERT_EQ(2u, rm_->state_interface_keys().size()); + EXPECT_TRUE(rm_->state_interface_exists("joint1/velocity")); + EXPECT_TRUE(rm_->state_interface_exists("joint2/velocity")); - ASSERT_EQ(2u, rm.command_interface_keys().size()); - EXPECT_TRUE(rm.state_interface_exists("joint1/velocity")); - EXPECT_TRUE(rm.state_interface_exists("joint2/velocity")); + ASSERT_EQ(2u, rm_->command_interface_keys().size()); + EXPECT_TRUE(rm_->state_interface_exists("joint1/velocity")); + EXPECT_TRUE(rm_->state_interface_exists("joint2/velocity")); // Check initial values - hardware_interface::LoanedStateInterface j1_v_s = rm.claim_state_interface("joint1/velocity"); - hardware_interface::LoanedStateInterface j2_v_s = rm.claim_state_interface("joint2/velocity"); - hardware_interface::LoanedCommandInterface j1_v_c = rm.claim_command_interface("joint1/velocity"); - hardware_interface::LoanedCommandInterface j2_v_c = rm.claim_command_interface("joint2/velocity"); + hardware_interface::LoanedStateInterface j1_v_s = rm_->claim_state_interface("joint1/velocity"); + hardware_interface::LoanedStateInterface j2_v_s = rm_->claim_state_interface("joint2/velocity"); + hardware_interface::LoanedCommandInterface j1_v_c = rm_->claim_command_interface("joint1/velocity"); + hardware_interface::LoanedCommandInterface j2_v_c = rm_->claim_command_interface("joint2/velocity"); EXPECT_TRUE(std::isnan(j1_v_s.get_optional().value())); EXPECT_TRUE(std::isnan(j2_v_s.get_optional().value())); @@ -291,9 +371,9 @@ TEST_F(TestTopicBasedSystem, load_topic_based_system_2dof_missing_position) ASSERT_TRUE(j2_v_c.set_value(0.14)); hardware_interface::return_type ret; - ASSERT_NO_THROW(ret = rm.read(TIME, PERIOD).result); + ASSERT_NO_THROW(ret = rm_->read(TIME, PERIOD).result); ASSERT_EQ(ret, hardware_interface::return_type::OK); - ASSERT_NO_THROW(ret = rm.write(TIME, PERIOD).result); + ASSERT_NO_THROW(ret = rm_->write(TIME, PERIOD).result); ASSERT_EQ(ret, hardware_interface::return_type::OK); // command is not propagated to state until topic from robot is received @@ -303,6 +383,14 @@ TEST_F(TestTopicBasedSystem, load_topic_based_system_2dof_missing_position) EXPECT_EQ(j1_v_c.get_optional().value(), 0.12); EXPECT_EQ(j2_v_c.get_optional().value(), 0.14); + + publish({ "joint1", "joint2" }, { 0.21, 0.23 }, { 0.22, 0.24 }); + + wait_for_msg(); + + // Reading should fail as position interface is missing + ASSERT_NO_THROW(ret = rm_->read(TIME, PERIOD).result); + ASSERT_EQ(ret, hardware_interface::return_type::ERROR); } TEST_F(TestTopicBasedSystem, load_topic_based_system_with_mimic_joint) @@ -331,28 +419,28 @@ TEST_F(TestTopicBasedSystem, load_topic_based_system_with_mimic_joint) auto urdf = ros2_control_test_assets::urdf_head_mimic + hardware_system_2dof_with_mimic_joint + ros2_control_test_assets::urdf_tail; - TestableResourceManager rm = init_rm(node_, urdf); + init_rm(urdf); // Activate components to get all interfaces available - activate_components(rm, { "JointStateTopicBasedSystem2dofMimic" }); + activate_components(*rm_, { "JointStateTopicBasedSystem2dofMimic" }); // Check interfaces - EXPECT_EQ(1u, rm.system_components_size()); - ASSERT_EQ(4u, rm.state_interface_keys().size()); - EXPECT_TRUE(rm.state_interface_exists("joint1/position")); - EXPECT_TRUE(rm.state_interface_exists("joint1/velocity")); - EXPECT_TRUE(rm.state_interface_exists("joint2/position")); - EXPECT_TRUE(rm.state_interface_exists("joint2/velocity")); + EXPECT_EQ(1u, rm_->system_components_size()); + ASSERT_EQ(4u, rm_->state_interface_keys().size()); + EXPECT_TRUE(rm_->state_interface_exists("joint1/position")); + EXPECT_TRUE(rm_->state_interface_exists("joint1/velocity")); + EXPECT_TRUE(rm_->state_interface_exists("joint2/position")); + EXPECT_TRUE(rm_->state_interface_exists("joint2/velocity")); - ASSERT_EQ(1u, rm.command_interface_keys().size()); - EXPECT_TRUE(rm.state_interface_exists("joint1/position")); + ASSERT_EQ(1u, rm_->command_interface_keys().size()); + EXPECT_TRUE(rm_->state_interface_exists("joint1/position")); // Check initial values - hardware_interface::LoanedStateInterface j1_p_s = rm.claim_state_interface("joint1/position"); - hardware_interface::LoanedStateInterface j1_v_s = rm.claim_state_interface("joint1/velocity"); - hardware_interface::LoanedStateInterface j2_p_s = rm.claim_state_interface("joint2/position"); - hardware_interface::LoanedStateInterface j2_v_s = rm.claim_state_interface("joint2/velocity"); - hardware_interface::LoanedCommandInterface j1_p_c = rm.claim_command_interface("joint1/position"); + hardware_interface::LoanedStateInterface j1_p_s = rm_->claim_state_interface("joint1/position"); + hardware_interface::LoanedStateInterface j1_v_s = rm_->claim_state_interface("joint1/velocity"); + hardware_interface::LoanedStateInterface j2_p_s = rm_->claim_state_interface("joint2/position"); + hardware_interface::LoanedStateInterface j2_v_s = rm_->claim_state_interface("joint2/velocity"); + hardware_interface::LoanedCommandInterface j1_p_c = rm_->claim_command_interface("joint1/position"); EXPECT_EQ(j1_p_s.get_optional().value(), 1.57); EXPECT_TRUE(std::isnan(j1_v_s.get_optional().value())); @@ -364,9 +452,9 @@ TEST_F(TestTopicBasedSystem, load_topic_based_system_with_mimic_joint) ASSERT_TRUE(j1_p_c.set_value(0.11)); hardware_interface::return_type ret; - ASSERT_NO_THROW(ret = rm.read(TIME, PERIOD).result); + ASSERT_NO_THROW(ret = rm_->read(TIME, PERIOD).result); ASSERT_EQ(ret, hardware_interface::return_type::OK); - ASSERT_NO_THROW(ret = rm.write(TIME, PERIOD).result); + ASSERT_NO_THROW(ret = rm_->write(TIME, PERIOD).result); ASSERT_EQ(ret, hardware_interface::return_type::OK); // command is not propagated to state until topic from robot is received @@ -377,6 +465,24 @@ TEST_F(TestTopicBasedSystem, load_topic_based_system_with_mimic_joint) EXPECT_EQ(j2_p_s.get_optional().value(), -2 * 1.57); EXPECT_TRUE(std::isnan(j2_v_s.get_optional().value())); EXPECT_EQ(j1_p_c.get_optional().value(), 0.11); + + publish({ "joint1", "joint2" }, { 0.21, 0.23 }, { 0.22, 0.24 }); + + wait_for_msg(); + + ASSERT_NO_THROW(ret = rm_->read(TIME, PERIOD).result); + ASSERT_EQ(ret, hardware_interface::return_type::OK); + ASSERT_NO_THROW(ret = rm_->write(TIME, PERIOD).result); + ASSERT_EQ(ret, hardware_interface::return_type::OK); + + // new states should have been updated from topic + EXPECT_EQ(j1_p_s.get_optional().value(), 0.21); + EXPECT_EQ(j1_v_s.get_optional().value(), 0.22); + EXPECT_EQ(j2_p_s.get_optional().value(), -2 * 0.21); // received value ignored due to mimic + EXPECT_EQ(j2_v_s.get_optional().value(), -2 * 0.22); // received value ignored due to mimic + + // commands should remain unchanged + EXPECT_EQ(j1_p_c.get_optional().value(), 0.11); } TEST_F(TestTopicBasedSystem, load_topic_based_system_with_mimic_joint_missing_position) @@ -404,27 +510,27 @@ TEST_F(TestTopicBasedSystem, load_topic_based_system_with_mimic_joint_missing_po auto urdf = ros2_control_test_assets::urdf_head_mimic + hardware_system_2dof_with_mimic_joint + ros2_control_test_assets::urdf_tail; - TestableResourceManager rm = init_rm(node_, urdf); + init_rm(urdf); // Activate components to get all interfaces available - activate_components(rm, { "JointStateTopicBasedSystem2dofMimic" }); + activate_components(*rm_, { "JointStateTopicBasedSystem2dofMimic" }); // Check interfaces - EXPECT_EQ(1u, rm.system_components_size()); - ASSERT_EQ(3u, rm.state_interface_keys().size()); - EXPECT_TRUE(rm.state_interface_exists("joint1/position")); - EXPECT_TRUE(rm.state_interface_exists("joint1/velocity")); - EXPECT_FALSE(rm.state_interface_exists("joint2/position")); - EXPECT_TRUE(rm.state_interface_exists("joint2/velocity")); + EXPECT_EQ(1u, rm_->system_components_size()); + ASSERT_EQ(3u, rm_->state_interface_keys().size()); + EXPECT_TRUE(rm_->state_interface_exists("joint1/position")); + EXPECT_TRUE(rm_->state_interface_exists("joint1/velocity")); + EXPECT_FALSE(rm_->state_interface_exists("joint2/position")); + EXPECT_TRUE(rm_->state_interface_exists("joint2/velocity")); - ASSERT_EQ(1u, rm.command_interface_keys().size()); - EXPECT_TRUE(rm.state_interface_exists("joint1/position")); + ASSERT_EQ(1u, rm_->command_interface_keys().size()); + EXPECT_TRUE(rm_->state_interface_exists("joint1/position")); // Check initial values - hardware_interface::LoanedStateInterface j1_p_s = rm.claim_state_interface("joint1/position"); - hardware_interface::LoanedStateInterface j1_v_s = rm.claim_state_interface("joint1/velocity"); - hardware_interface::LoanedStateInterface j2_v_s = rm.claim_state_interface("joint2/velocity"); - hardware_interface::LoanedCommandInterface j1_p_c = rm.claim_command_interface("joint1/position"); + hardware_interface::LoanedStateInterface j1_p_s = rm_->claim_state_interface("joint1/position"); + hardware_interface::LoanedStateInterface j1_v_s = rm_->claim_state_interface("joint1/velocity"); + hardware_interface::LoanedStateInterface j2_v_s = rm_->claim_state_interface("joint2/velocity"); + hardware_interface::LoanedCommandInterface j1_p_c = rm_->claim_command_interface("joint1/position"); EXPECT_EQ(j1_p_s.get_optional().value(), 1.57); EXPECT_TRUE(std::isnan(j1_v_s.get_optional().value())); @@ -436,9 +542,9 @@ TEST_F(TestTopicBasedSystem, load_topic_based_system_with_mimic_joint_missing_po // should not throw even if mimic joint is missing position interface hardware_interface::return_type ret; - ASSERT_NO_THROW(ret = rm.read(TIME, PERIOD).result); + ASSERT_NO_THROW(ret = rm_->read(TIME, PERIOD).result); ASSERT_EQ(ret, hardware_interface::return_type::OK); - ASSERT_NO_THROW(ret = rm.write(TIME, PERIOD).result); + ASSERT_NO_THROW(ret = rm_->write(TIME, PERIOD).result); ASSERT_EQ(ret, hardware_interface::return_type::OK); // command is not propagated to state until topic from robot is received @@ -447,4 +553,21 @@ TEST_F(TestTopicBasedSystem, load_topic_based_system_with_mimic_joint_missing_po EXPECT_TRUE(std::isnan(j1_v_s.get_optional().value())); EXPECT_TRUE(std::isnan(j2_v_s.get_optional().value())); EXPECT_EQ(j1_p_c.get_optional().value(), 0.11); + + publish({ "joint1", "joint2" }, { 0.21, 0.23 }, { 0.22, 0.24 }); + + wait_for_msg(); + + ASSERT_NO_THROW(ret = rm_->read(TIME, PERIOD).result); + ASSERT_EQ(ret, hardware_interface::return_type::OK); + ASSERT_NO_THROW(ret = rm_->write(TIME, PERIOD).result); + ASSERT_EQ(ret, hardware_interface::return_type::OK); + + // new states should have been updated from topic + EXPECT_EQ(j1_p_s.get_optional().value(), 0.21); + EXPECT_EQ(j1_v_s.get_optional().value(), 0.22); + EXPECT_EQ(j2_v_s.get_optional().value(), -2 * 0.22); // received value ignored due to mimic + + // commands should remain unchanged + EXPECT_EQ(j1_p_c.get_optional().value(), 0.11); } From ba83b225cefba4acb8af6b16ca4f1746b51a65f2 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 17 Oct 2025 12:59:00 +0000 Subject: [PATCH 25/29] Accept velocity only setups as well --- .../joint_state_topic_hardware_interface.cpp | 2 +- ...nt_state_topic_hardware_interface_test.cpp | 32 ++++++++++++------- 2 files changed, 22 insertions(+), 12 deletions(-) diff --git a/joint_state_topic_hardware_interface/src/joint_state_topic_hardware_interface.cpp b/joint_state_topic_hardware_interface/src/joint_state_topic_hardware_interface.cpp index fc8d2e6..fc5ba18 100644 --- a/joint_state_topic_hardware_interface/src/joint_state_topic_hardware_interface.cpp +++ b/joint_state_topic_hardware_interface/src/joint_state_topic_hardware_interface.cpp @@ -108,7 +108,7 @@ hardware_interface::return_type JointStateTopicSystem::read(const rclcpp::Time& continue; } - if (std::isfinite(latest_joint_state_.position.at(i))) + if (!latest_joint_state_.position.empty() && std::isfinite(latest_joint_state_.position.at(i))) { if (sum_wrapped_joint_states_) { diff --git a/joint_state_topic_hardware_interface/test/joint_state_topic_hardware_interface_test.cpp b/joint_state_topic_hardware_interface/test/joint_state_topic_hardware_interface_test.cpp index 1f657ea..a15f275 100644 --- a/joint_state_topic_hardware_interface/test/joint_state_topic_hardware_interface_test.cpp +++ b/joint_state_topic_hardware_interface/test/joint_state_topic_hardware_interface_test.cpp @@ -143,11 +143,6 @@ class TestableResourceManager : public hardware_interface::ResourceManager public: friend TestTopicBasedSystem; - FRIEND_TEST(TestTopicBasedSystem, load_topic_based_system_2dof); - FRIEND_TEST(TestTopicBasedSystem, load_topic_based_system_2dof_missing_position); - FRIEND_TEST(TestTopicBasedSystem, load_topic_based_system_with_mimic_joint); - FRIEND_TEST(TestTopicBasedSystem, TestTopicBasedSystem_load_topic_based_system_with_mimic_joint_missing_position); - // The API of the ResourceManager has changed in hardware_interface 4.13.0 #if HARDWARE_INTERFACE_VERSION_GTE(4, 13, 0) explicit TestableResourceManager(const hardware_interface::ResourceManagerParams& params, bool load) @@ -317,7 +312,7 @@ TEST_F(TestTopicBasedSystem, load_topic_based_system_2dof) EXPECT_EQ(j2_v_c.get_optional().value(), 0.14); } -TEST_F(TestTopicBasedSystem, load_topic_based_system_2dof_velocity_only) +TEST_F(TestTopicBasedSystem, topic_based_system_2dof_velocity_only) { const std::string hardware_system_2dof_topic_based = R"( @@ -384,16 +379,31 @@ TEST_F(TestTopicBasedSystem, load_topic_based_system_2dof_velocity_only) EXPECT_EQ(j1_v_c.get_optional().value(), 0.12); EXPECT_EQ(j2_v_c.get_optional().value(), 0.14); + // Reading should fail as position interface is missing publish({ "joint1", "joint2" }, { 0.21, 0.23 }, { 0.22, 0.24 }); - wait_for_msg(); - - // Reading should fail as position interface is missing ASSERT_NO_THROW(ret = rm_->read(TIME, PERIOD).result); ASSERT_EQ(ret, hardware_interface::return_type::ERROR); + + // Reading should succeed now because position field is empty + activate_components(*rm_, { "JointStateTopicBasedSystem2dof" }); + publish({ "joint1", "joint2" }, {}, { 0.22, 0.24 }); + wait_for_msg(std::chrono::milliseconds{ 100 }); + ASSERT_NO_THROW(ret = rm_->read(TIME, PERIOD).result); + ASSERT_EQ(ret, hardware_interface::return_type::OK); + ASSERT_NO_THROW(ret = rm_->write(TIME, PERIOD).result); + ASSERT_EQ(ret, hardware_interface::return_type::OK); + + // new states should have been updated from topic + EXPECT_EQ(j1_v_s.get_optional().value(), 0.22); + EXPECT_EQ(j2_v_s.get_optional().value(), 0.24); + + // commands should remain unchanged + EXPECT_EQ(j1_v_c.get_optional().value(), 0.12); + EXPECT_EQ(j2_v_c.get_optional().value(), 0.14); } -TEST_F(TestTopicBasedSystem, load_topic_based_system_with_mimic_joint) +TEST_F(TestTopicBasedSystem, topic_based_system_with_mimic_joint) { const std::string hardware_system_2dof_with_mimic_joint = R"( @@ -485,7 +495,7 @@ TEST_F(TestTopicBasedSystem, load_topic_based_system_with_mimic_joint) EXPECT_EQ(j1_p_c.get_optional().value(), 0.11); } -TEST_F(TestTopicBasedSystem, load_topic_based_system_with_mimic_joint_missing_position) +TEST_F(TestTopicBasedSystem, topic_based_system_with_mimic_joint_missing_position) { const std::string hardware_system_2dof_with_mimic_joint = R"( From f9cedf8afb76622b77f98a02d2ff39a09b6d923c Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 17 Oct 2025 13:05:54 +0000 Subject: [PATCH 26/29] Split tests --- ...nt_state_topic_hardware_interface_test.cpp | 83 +++++++++++++++++-- 1 file changed, 75 insertions(+), 8 deletions(-) diff --git a/joint_state_topic_hardware_interface/test/joint_state_topic_hardware_interface_test.cpp b/joint_state_topic_hardware_interface/test/joint_state_topic_hardware_interface_test.cpp index a15f275..80451af 100644 --- a/joint_state_topic_hardware_interface/test/joint_state_topic_hardware_interface_test.cpp +++ b/joint_state_topic_hardware_interface/test/joint_state_topic_hardware_interface_test.cpp @@ -379,16 +379,9 @@ TEST_F(TestTopicBasedSystem, topic_based_system_2dof_velocity_only) EXPECT_EQ(j1_v_c.get_optional().value(), 0.12); EXPECT_EQ(j2_v_c.get_optional().value(), 0.14); - // Reading should fail as position interface is missing - publish({ "joint1", "joint2" }, { 0.21, 0.23 }, { 0.22, 0.24 }); - wait_for_msg(); - ASSERT_NO_THROW(ret = rm_->read(TIME, PERIOD).result); - ASSERT_EQ(ret, hardware_interface::return_type::ERROR); - - // Reading should succeed now because position field is empty - activate_components(*rm_, { "JointStateTopicBasedSystem2dof" }); publish({ "joint1", "joint2" }, {}, { 0.22, 0.24 }); wait_for_msg(std::chrono::milliseconds{ 100 }); + ASSERT_NO_THROW(ret = rm_->read(TIME, PERIOD).result); ASSERT_EQ(ret, hardware_interface::return_type::OK); ASSERT_NO_THROW(ret = rm_->write(TIME, PERIOD).result); @@ -403,6 +396,80 @@ TEST_F(TestTopicBasedSystem, topic_based_system_2dof_velocity_only) EXPECT_EQ(j2_v_c.get_optional().value(), 0.14); } +TEST_F(TestTopicBasedSystem, topic_based_system_2dof_velocity_only_inconsistent_topic) +{ + const std::string hardware_system_2dof_topic_based = + R"( + + + joint_state_topic_hardware_interface/JointStateTopicSystem + /topic_based_joint_commands + /topic_based_custom_joint_states + + + + + + + + + + +)"; + auto urdf = + ros2_control_test_assets::urdf_head + hardware_system_2dof_topic_based + ros2_control_test_assets::urdf_tail; + + init_rm(urdf); + + // Activate components to get all interfaces available + activate_components(*rm_, { "JointStateTopicBasedSystem2dof" }); + + // Check interfaces + EXPECT_EQ(1u, rm_->system_components_size()); + ASSERT_EQ(2u, rm_->state_interface_keys().size()); + EXPECT_TRUE(rm_->state_interface_exists("joint1/velocity")); + EXPECT_TRUE(rm_->state_interface_exists("joint2/velocity")); + + ASSERT_EQ(2u, rm_->command_interface_keys().size()); + EXPECT_TRUE(rm_->state_interface_exists("joint1/velocity")); + EXPECT_TRUE(rm_->state_interface_exists("joint2/velocity")); + + // Check initial values + hardware_interface::LoanedStateInterface j1_v_s = rm_->claim_state_interface("joint1/velocity"); + hardware_interface::LoanedStateInterface j2_v_s = rm_->claim_state_interface("joint2/velocity"); + hardware_interface::LoanedCommandInterface j1_v_c = rm_->claim_command_interface("joint1/velocity"); + hardware_interface::LoanedCommandInterface j2_v_c = rm_->claim_command_interface("joint2/velocity"); + + EXPECT_TRUE(std::isnan(j1_v_s.get_optional().value())); + EXPECT_TRUE(std::isnan(j2_v_s.get_optional().value())); + EXPECT_TRUE(std::isnan(j1_v_c.get_optional().value())); + EXPECT_TRUE(std::isnan(j2_v_c.get_optional().value())); + + // set some new values in commands + ASSERT_TRUE(j1_v_c.set_value(0.12)); + ASSERT_TRUE(j2_v_c.set_value(0.14)); + + hardware_interface::return_type ret; + ASSERT_NO_THROW(ret = rm_->read(TIME, PERIOD).result); + ASSERT_EQ(ret, hardware_interface::return_type::OK); + ASSERT_NO_THROW(ret = rm_->write(TIME, PERIOD).result); + ASSERT_EQ(ret, hardware_interface::return_type::OK); + + // command is not propagated to state until topic from robot is received + // same holds for velocity + EXPECT_TRUE(std::isnan(j1_v_s.get_optional().value())); + EXPECT_TRUE(std::isnan(j2_v_s.get_optional().value())); + + EXPECT_EQ(j1_v_c.get_optional().value(), 0.12); + EXPECT_EQ(j2_v_c.get_optional().value(), 0.14); + + // Reading should fail as position interface is missing + publish({ "joint1", "joint2" }, { 0.21, 0.23 }, { 0.22, 0.24 }); + wait_for_msg(); + ASSERT_NO_THROW(ret = rm_->read(TIME, PERIOD).result); + ASSERT_EQ(ret, hardware_interface::return_type::ERROR); +} + TEST_F(TestTopicBasedSystem, topic_based_system_with_mimic_joint) { const std::string hardware_system_2dof_with_mimic_joint = From f1fd7201937f9218baeac36d24473447cc3e7f2b Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 17 Oct 2025 13:18:49 +0000 Subject: [PATCH 27/29] Update includes --- ...nt_state_topic_hardware_interface_test.cpp | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/joint_state_topic_hardware_interface/test/joint_state_topic_hardware_interface_test.cpp b/joint_state_topic_hardware_interface/test/joint_state_topic_hardware_interface_test.cpp index 80451af..336b291 100644 --- a/joint_state_topic_hardware_interface/test/joint_state_topic_hardware_interface_test.cpp +++ b/joint_state_topic_hardware_interface/test/joint_state_topic_hardware_interface_test.cpp @@ -13,29 +13,29 @@ // limitations under the License. #include +#include #include -#include #include #include -#if __has_include() -#include + +#if __has_include("hardware_interface/hardware_interface/version.h") +#include "hardware_interface/hardware_interface/version.h" #else -#include +#include "hardware_interface/version.h" #endif -#include -#include -#include -#include -#include - +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "ros2_control_test_assets/descriptions.hpp" #include "sensor_msgs/msg/joint_state.hpp" namespace { const auto TIME = rclcpp::Time(0); const auto PERIOD = rclcpp::Duration::from_seconds(0.1); // 0.1 seconds for easier math -const auto COMPARE_DELTA = 0.0001; } // namespace // Forward declaration From a223bf577e664d8a6f387ebe3ca153d040fd0d71 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 17 Oct 2025 13:44:58 +0000 Subject: [PATCH 28/29] Convert unused lambda to unused methods --- .../joint_state_topic_hardware_interface_test.cpp | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/joint_state_topic_hardware_interface/test/joint_state_topic_hardware_interface_test.cpp b/joint_state_topic_hardware_interface/test/joint_state_topic_hardware_interface_test.cpp index 336b291..51755e0 100644 --- a/joint_state_topic_hardware_interface/test/joint_state_topic_hardware_interface_test.cpp +++ b/joint_state_topic_hardware_interface/test/joint_state_topic_hardware_interface_test.cpp @@ -179,20 +179,23 @@ void set_components_state(TestableResourceManager& rm, const std::vector& components = { "GenericSystem2dof" }) { +void configure_components(TestableResourceManager& rm, + const std::vector& components = { "GenericSystem2dof" }) +{ set_components_state(rm, components, lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, hardware_interface::lifecycle_state_names::INACTIVE); }; -auto activate_components = [](TestableResourceManager& rm, - const std::vector& components = { "GenericSystem2dof" }) { +void activate_components(TestableResourceManager& rm, + const std::vector& components = { "GenericSystem2dof" }) +{ set_components_state(rm, components, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE); }; -auto deactivate_components = [](TestableResourceManager& rm, - const std::vector& components = { "GenericSystem2dof" }) { +void deactivate_components(TestableResourceManager& rm, + const std::vector& components = { "GenericSystem2dof" }) +{ set_components_state(rm, components, lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, hardware_interface::lifecycle_state_names::INACTIVE); }; From 68f39dc81d3a73bdafda410fed11519eca1394a4 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 19 Oct 2025 20:54:59 +0000 Subject: [PATCH 29/29] Use has_state instead --- .../src/joint_state_topic_hardware_interface.cpp | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/joint_state_topic_hardware_interface/src/joint_state_topic_hardware_interface.cpp b/joint_state_topic_hardware_interface/src/joint_state_topic_hardware_interface.cpp index fc5ba18..b2b9bf5 100644 --- a/joint_state_topic_hardware_interface/src/joint_state_topic_hardware_interface.cpp +++ b/joint_state_topic_hardware_interface/src/joint_state_topic_hardware_interface.cpp @@ -140,21 +140,18 @@ hardware_interface::return_type JointStateTopicSystem::read(const rclcpp::Time& { const auto& mimic_joint_name = joints.at(mimic_joint.joint_index).name; const auto& mimicked_joint_name = joints.at(mimic_joint.mimicked_joint_index).name; - if (joint_state_interfaces_.find(mimic_joint_name + "/" + hardware_interface::HW_IF_POSITION) != - joint_state_interfaces_.end()) + if (has_state(mimic_joint_name + "/" + hardware_interface::HW_IF_POSITION)) { set_state(mimic_joint_name + "/" + hardware_interface::HW_IF_POSITION, mimic_joint.offset + mimic_joint.multiplier * get_state(mimicked_joint_name + "/" + hardware_interface::HW_IF_POSITION)); } - if (joint_state_interfaces_.find(mimic_joint_name + "/" + hardware_interface::HW_IF_VELOCITY) != - joint_state_interfaces_.end()) + if (has_state(mimic_joint_name + "/" + hardware_interface::HW_IF_VELOCITY)) { set_state(mimic_joint_name + "/" + hardware_interface::HW_IF_VELOCITY, mimic_joint.multiplier * get_state(mimicked_joint_name + "/" + hardware_interface::HW_IF_VELOCITY)); } - if (joint_state_interfaces_.find(mimic_joint_name + "/" + hardware_interface::HW_IF_ACCELERATION) != - joint_state_interfaces_.end()) + if (has_state(mimic_joint_name + "/" + hardware_interface::HW_IF_ACCELERATION)) { set_state(mimic_joint_name + "/" + hardware_interface::HW_IF_ACCELERATION, mimic_joint.multiplier * get_state(mimicked_joint_name + "/" + hardware_interface::HW_IF_ACCELERATION));