From 7d12dc16b92610ae05c99addc113096594c0dc34 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sat, 7 Sep 2024 10:19:51 +0000 Subject: [PATCH 1/6] Add tests for CLI commands --- example_1/CMakeLists.txt | 1 + example_1/test/test_rrbot_launch.py | 48 ++++++++ .../test/test_rrbot_launch_cli_direct.py | 115 ++++++++++++++++++ .../ros2_control_demo_testing/test_utils.py | 6 +- 4 files changed, 167 insertions(+), 3 deletions(-) create mode 100644 example_1/test/test_rrbot_launch_cli_direct.py diff --git a/example_1/CMakeLists.txt b/example_1/CMakeLists.txt index 507f8f70..0943b689 100644 --- a/example_1/CMakeLists.txt +++ b/example_1/CMakeLists.txt @@ -76,6 +76,7 @@ if(BUILD_TESTING) ament_add_pytest_test(example_1_urdf_xacro test/test_urdf_xacro.py) ament_add_pytest_test(view_example_1_launch test/test_view_robot_launch.py) ament_add_pytest_test(run_example_1_launch test/test_rrbot_launch.py) + ament_add_pytest_test(run_example_1_launch_cli_direct test/test_rrbot_launch_cli_direct.py) endif() diff --git a/example_1/test/test_rrbot_launch.py b/example_1/test/test_rrbot_launch.py index 2b25f51a..4c626647 100644 --- a/example_1/test/test_rrbot_launch.py +++ b/example_1/test/test_rrbot_launch.py @@ -31,6 +31,7 @@ import os import pytest import unittest +import subprocess from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription @@ -89,6 +90,53 @@ def test_check_if_msgs_published(self): check_if_js_published("/joint_states", ["joint1", "joint2"]) +class TestFixtureCLI(unittest.TestCase): + + def setUp(self): + rclpy.init() + self.node = Node("test_node") + + def tearDown(self): + self.node.destroy_node() + rclpy.shutdown() + + def test_main(self, proc_output): + + # Command to run the CLI + cname = "joint_trajectory_position_controller" + command = [ + "ros2", + "control", + "load_controller", + cname, + os.path.join( + get_package_share_directory("ros2_control_demo_example_1"), + "config/rrbot_controllers.yaml", + ), + ] + subprocess.run(command, check=True) + check_controllers_running(self.node, [cname], state="unconfigured") + check_controllers_running(self.node, ["forward_position_controller"], state="active") + + command = ["ros2", "control", "set_controller_state", cname, "inactive"] + subprocess.run(command, check=True) + check_controllers_running(self.node, [cname], state="inactive") + check_controllers_running(self.node, ["forward_position_controller"], state="active") + + command = [ + "ros2", + "control", + "set_controller_state", + "forward_position_controller", + "inactive", + ] + subprocess.run(command, check=True) + command = ["ros2", "control", "set_controller_state", cname, "active"] + subprocess.run(command, check=True) + check_controllers_running(self.node, ["forward_position_controller"], state="inactive") + check_controllers_running(self.node, [cname], state="active") + + # TODO(anyone): enable this if shutdown of ros2_control_node does not fail anymore # @launch_testing.post_shutdown_test() # # These tests are run after the processes in generate_test_description() have shutdown. diff --git a/example_1/test/test_rrbot_launch_cli_direct.py b/example_1/test/test_rrbot_launch_cli_direct.py new file mode 100644 index 00000000..79411dde --- /dev/null +++ b/example_1/test/test_rrbot_launch_cli_direct.py @@ -0,0 +1,115 @@ +# Copyright (c) 2024 AIT - Austrian Institute of Technology GmbH +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the {copyright_holder} nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# Author: Christoph Froehlich + +import os +import pytest +import unittest +import subprocess + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_testing.actions import ReadyToTest + +# import launch_testing.markers +import rclpy +from rclpy.node import Node +from ros2_control_demo_testing.test_utils import check_controllers_running + + +# Executes the given launch file and checks if all nodes can be started +@pytest.mark.rostest +def generate_test_description(): + launch_include = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory("ros2_control_demo_example_1"), + "launch/rrbot.launch.py", + ) + ), + launch_arguments={"gui": "False"}.items(), + ) + + return LaunchDescription([launch_include, ReadyToTest()]) + + +class TestFixtureCliDirect(unittest.TestCase): + + def setUp(self): + rclpy.init() + self.node = Node("test_node") + + def tearDown(self): + self.node.destroy_node() + rclpy.shutdown() + + def test_main(self, proc_output): + + # Command to run the CLI + cname = "joint_trajectory_position_controller" + command = [ + "ros2", + "control", + "load_controller", + "--set-state", + "inactive", + cname, + os.path.join( + get_package_share_directory("ros2_control_demo_example_1"), + "config/rrbot_controllers.yaml", + ), + ] + subprocess.run(command, check=True) + check_controllers_running(self.node, [cname], state="inactive") + check_controllers_running(self.node, ["forward_position_controller"], state="active") + + command = [ + "ros2", + "control", + "switch_controllers", + "--activate", + cname, + "--deactivate", + "forward_position_controller", + ] + subprocess.run(command, check=True) + check_controllers_running(self.node, ["forward_position_controller"], state="inactive") + check_controllers_running(self.node, [cname], state="active") + + +# TODO(anyone): enable this if shutdown of ros2_control_node does not fail anymore +# @launch_testing.post_shutdown_test() +# # These tests are run after the processes in generate_test_description() have shutdown. +# class TestDescriptionCraneShutdown(unittest.TestCase): + +# def test_exit_codes(self, proc_info): +# """Check if the processes exited normally.""" +# launch_testing.asserts.assertExitCodes(proc_info) diff --git a/ros2_control_demo_testing/ros2_control_demo_testing/test_utils.py b/ros2_control_demo_testing/ros2_control_demo_testing/test_utils.py index 5aeab91d..e23ca7bc 100644 --- a/ros2_control_demo_testing/ros2_control_demo_testing/test_utils.py +++ b/ros2_control_demo_testing/ros2_control_demo_testing/test_utils.py @@ -45,7 +45,7 @@ def check_node_running(node, node_name, timeout=5.0): assert found, f"{node_name} not found!" -def check_controllers_running(node, cnames, namespace=""): +def check_controllers_running(node, cnames, namespace="", state="active"): # wait for controller to be loaded before we call the CM services found = {cname: False for cname in cnames} # Define 'found' as a dictionary @@ -85,14 +85,14 @@ def check_controllers_running(node, cnames, namespace=""): assert controllers, "No controllers found!" for c in controllers: for cname in cnames: - if c.name == cname and c.state == "active": + if c.name == cname and c.state == state: found[cname] = True break time.sleep(0.1) assert all( found.values() - ), f"Controller(s) not found or not active: {', '.join([cname for cname, is_found in found.items() if not is_found])}" + ), f"Controller(s) not found or not {state}: {', '.join([cname for cname, is_found in found.items() if not is_found])}" def check_if_js_published(topic, joint_names): From f9586aceeb6b7c85b552240702929fd73154b801 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sat, 7 Sep 2024 10:22:09 +0000 Subject: [PATCH 2/6] Update docs --- example_1/doc/userdoc.rst | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/example_1/doc/userdoc.rst b/example_1/doc/userdoc.rst index 06fbe33d..3be561af 100644 --- a/example_1/doc/userdoc.rst +++ b/example_1/doc/userdoc.rst @@ -241,7 +241,7 @@ Tutorial steps .. code-block:: shell - ros2 control load_controller joint_trajectory_position_controller + ros2 control load_controller joint_trajectory_position_controller $(ros2 pkg prefix ros2_control_demo_example_1 --share)/config/rrbot_controllers.yaml .. group-tab:: Docker @@ -249,7 +249,7 @@ Tutorial steps .. code-block:: shell - ros2 control load_controller joint_trajectory_position_controller + ros2 control load_controller joint_trajectory_position_controller $(ros2 pkg prefix ros2_control_demo_example_1 --share)/config/rrbot_controllers.yaml what should return ``Successfully loaded controller joint_trajectory_position_controller``. Check the status with @@ -310,7 +310,7 @@ Tutorial steps .. code-block:: shell - ros2 control load_controller joint_trajectory_position_controller --set-state inactive + ros2 control load_controller --set-state inactive joint_trajectory_position_controller $(ros2 pkg prefix ros2_control_demo_example_1 --share)/config/rrbot_controllers.yaml .. group-tab:: Docker @@ -318,7 +318,7 @@ Tutorial steps .. code-block:: shell - ros2 control load_controller joint_trajectory_position_controller --set-state inactive + ros2 control load_controller --set-state inactive joint_trajectory_position_controller $(ros2 pkg prefix ros2_control_demo_example_1 --share)/config/rrbot_controllers.yaml You should get the result ``Successfully loaded controller joint_trajectory_position_controller into state inactive``. From 66829f48ad6498335710258697d6760e9452c882 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sat, 7 Sep 2024 20:41:56 +0000 Subject: [PATCH 3/6] Split controller yaml --- .../bringup/config/rrbot_controllers.yaml | 24 ------------------- example_1/bringup/config/rrbot_jtc.yaml | 22 +++++++++++++++++ example_1/doc/userdoc.rst | 19 +++++++-------- example_1/test/test_rrbot_launch.py | 2 +- .../test/test_rrbot_launch_cli_direct.py | 2 +- 5 files changed, 33 insertions(+), 36 deletions(-) create mode 100644 example_1/bringup/config/rrbot_jtc.yaml diff --git a/example_1/bringup/config/rrbot_controllers.yaml b/example_1/bringup/config/rrbot_controllers.yaml index 04ac6328..0a658759 100644 --- a/example_1/bringup/config/rrbot_controllers.yaml +++ b/example_1/bringup/config/rrbot_controllers.yaml @@ -13,27 +13,3 @@ forward_position_controller: - joint1 - joint2 interface_name: position - - -joint_trajectory_position_controller: - ros__parameters: - type: joint_trajectory_controller/JointTrajectoryController - - joints: - - joint1 - - joint2 - - command_interfaces: - - position - - state_interfaces: - - position - - action_monitor_rate: 20.0 # Defaults to 20 - - allow_partial_joints_goal: false # Defaults to false - open_loop_control: true - allow_integration_in_goal_trajectories: true - constraints: - stopped_velocity_tolerance: 0.01 # Defaults to 0.01 - goal_time: 0.0 # Defaults to 0.0 (start immediately) diff --git a/example_1/bringup/config/rrbot_jtc.yaml b/example_1/bringup/config/rrbot_jtc.yaml new file mode 100644 index 00000000..b3a96037 --- /dev/null +++ b/example_1/bringup/config/rrbot_jtc.yaml @@ -0,0 +1,22 @@ +joint_trajectory_position_controller: + ros__parameters: + type: joint_trajectory_controller/JointTrajectoryController + + joints: + - joint1 + - joint2 + + command_interfaces: + - position + + state_interfaces: + - position + + action_monitor_rate: 20.0 # Defaults to 20 + + allow_partial_joints_goal: false # Defaults to false + open_loop_control: true + allow_integration_in_goal_trajectories: true + constraints: + stopped_velocity_tolerance: 0.01 # Defaults to 0.01 + goal_time: 0.0 # Defaults to 0.0 (start immediately) diff --git a/example_1/doc/userdoc.rst b/example_1/doc/userdoc.rst index 3be561af..4bec4629 100644 --- a/example_1/doc/userdoc.rst +++ b/example_1/doc/userdoc.rst @@ -241,7 +241,7 @@ Tutorial steps .. code-block:: shell - ros2 control load_controller joint_trajectory_position_controller $(ros2 pkg prefix ros2_control_demo_example_1 --share)/config/rrbot_controllers.yaml + ros2 control load_controller joint_trajectory_position_controller $(ros2 pkg prefix ros2_control_demo_example_1 --share)/config/rrbot_jtc.yaml .. group-tab:: Docker @@ -249,7 +249,7 @@ Tutorial steps .. code-block:: shell - ros2 control load_controller joint_trajectory_position_controller $(ros2 pkg prefix ros2_control_demo_example_1 --share)/config/rrbot_controllers.yaml + ros2 control load_controller joint_trajectory_position_controller $(ros2 pkg prefix ros2_control_demo_example_1 --share)/config/rrbot_jtc.yaml what should return ``Successfully loaded controller joint_trajectory_position_controller``. Check the status with @@ -297,11 +297,6 @@ Tutorial steps what should give ``Successfully configured joint_trajectory_position_controller``. - .. note:: - - The parameters are already set in `rrbot_controllers.yaml `__ - but the controller was not loaded from the `launch file rrbot.launch.py `__ before. - As an alternative, you can load the controller directly in ``inactive``-state by means of the option for ``load_controller`` with .. tabs:: @@ -310,7 +305,7 @@ Tutorial steps .. code-block:: shell - ros2 control load_controller --set-state inactive joint_trajectory_position_controller $(ros2 pkg prefix ros2_control_demo_example_1 --share)/config/rrbot_controllers.yaml + ros2 control load_controller --set-state inactive joint_trajectory_position_controller $(ros2 pkg prefix ros2_control_demo_example_1 --share)/config/rrbot_jtc.yaml .. group-tab:: Docker @@ -318,7 +313,7 @@ Tutorial steps .. code-block:: shell - ros2 control load_controller --set-state inactive joint_trajectory_position_controller $(ros2 pkg prefix ros2_control_demo_example_1 --share)/config/rrbot_controllers.yaml + ros2 control load_controller --set-state inactive joint_trajectory_position_controller $(ros2 pkg prefix ros2_control_demo_example_1 --share)/config/rrbot_jtc.yaml You should get the result ``Successfully loaded controller joint_trajectory_position_controller into state inactive``. @@ -436,7 +431,11 @@ Files used for this demos ------------------------- * Launch file: `rrbot.launch.py `__ -* Controllers yaml: `rrbot_controllers.yaml `__ +* Controllers yaml: + + * `rrbot_controllers.yaml `__ + * `rrbot_jtc.yaml `__ + * URDF file: `rrbot.urdf.xacro `__ * Description: `rrbot_description.urdf.xacro `__ diff --git a/example_1/test/test_rrbot_launch.py b/example_1/test/test_rrbot_launch.py index 4c626647..ecc66ecf 100644 --- a/example_1/test/test_rrbot_launch.py +++ b/example_1/test/test_rrbot_launch.py @@ -111,7 +111,7 @@ def test_main(self, proc_output): cname, os.path.join( get_package_share_directory("ros2_control_demo_example_1"), - "config/rrbot_controllers.yaml", + "config/rrbot_jtc.yaml", ), ] subprocess.run(command, check=True) diff --git a/example_1/test/test_rrbot_launch_cli_direct.py b/example_1/test/test_rrbot_launch_cli_direct.py index 79411dde..cab04cfa 100644 --- a/example_1/test/test_rrbot_launch_cli_direct.py +++ b/example_1/test/test_rrbot_launch_cli_direct.py @@ -84,7 +84,7 @@ def test_main(self, proc_output): cname, os.path.join( get_package_share_directory("ros2_control_demo_example_1"), - "config/rrbot_controllers.yaml", + "config/rrbot_jtc.yaml", ), ] subprocess.run(command, check=True) From 0d0fa2703d65f6d7c7fe852cc80d0bcb6a8db300 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sat, 7 Sep 2024 20:46:16 +0000 Subject: [PATCH 4/6] Use fork branch --- ros2_control_demos-not-released.jazzy.repos | 4 ++++ ros2_control_demos-not-released.rolling.repos | 4 ++++ ros2_control_demos.jazzy.repos | 4 ++-- ros2_control_demos.rolling.repos | 4 ++-- 4 files changed, 12 insertions(+), 4 deletions(-) diff --git a/ros2_control_demos-not-released.jazzy.repos b/ros2_control_demos-not-released.jazzy.repos index 56f46b6f..ed8a8831 100644 --- a/ros2_control_demos-not-released.jazzy.repos +++ b/ros2_control_demos-not-released.jazzy.repos @@ -1 +1,5 @@ repositories: + ros2_control: + type: git + url: https://github.com/pal-robotics-forks/ros2_control.git + version: update/ros2controlcli diff --git a/ros2_control_demos-not-released.rolling.repos b/ros2_control_demos-not-released.rolling.repos index 56f46b6f..ed8a8831 100644 --- a/ros2_control_demos-not-released.rolling.repos +++ b/ros2_control_demos-not-released.rolling.repos @@ -1 +1,5 @@ repositories: + ros2_control: + type: git + url: https://github.com/pal-robotics-forks/ros2_control.git + version: update/ros2controlcli diff --git a/ros2_control_demos.jazzy.repos b/ros2_control_demos.jazzy.repos index 8ba0dc2c..7dc7f87a 100644 --- a/ros2_control_demos.jazzy.repos +++ b/ros2_control_demos.jazzy.repos @@ -9,8 +9,8 @@ repositories: version: master ros2_control: type: git - url: https://github.com/ros-controls/ros2_control.git - version: master + url: https://github.com/pal-robotics-forks/ros2_control.git + version: update/ros2controlcli ros2_controllers: type: git url: https://github.com/ros-controls/ros2_controllers.git diff --git a/ros2_control_demos.rolling.repos b/ros2_control_demos.rolling.repos index 7eba4e04..a9633510 100644 --- a/ros2_control_demos.rolling.repos +++ b/ros2_control_demos.rolling.repos @@ -9,8 +9,8 @@ repositories: version: master ros2_control: type: git - url: https://github.com/ros-controls/ros2_control.git - version: master + url: https://github.com/pal-robotics-forks/ros2_control.git + version: update/ros2controlcli ros2_controllers: type: git url: https://github.com/ros-controls/ros2_controllers.git From 7bd7f330879e9037629fff14ecbae0f0defcb5c5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sun, 8 Sep 2024 20:58:58 +0200 Subject: [PATCH 5/6] Revert binary build changes --- ros2_control_demos-not-released.jazzy.repos | 4 ---- ros2_control_demos-not-released.rolling.repos | 4 ---- 2 files changed, 8 deletions(-) diff --git a/ros2_control_demos-not-released.jazzy.repos b/ros2_control_demos-not-released.jazzy.repos index ed8a8831..56f46b6f 100644 --- a/ros2_control_demos-not-released.jazzy.repos +++ b/ros2_control_demos-not-released.jazzy.repos @@ -1,5 +1 @@ repositories: - ros2_control: - type: git - url: https://github.com/pal-robotics-forks/ros2_control.git - version: update/ros2controlcli diff --git a/ros2_control_demos-not-released.rolling.repos b/ros2_control_demos-not-released.rolling.repos index ed8a8831..56f46b6f 100644 --- a/ros2_control_demos-not-released.rolling.repos +++ b/ros2_control_demos-not-released.rolling.repos @@ -1,5 +1 @@ repositories: - ros2_control: - type: git - url: https://github.com/pal-robotics-forks/ros2_control.git - version: update/ros2controlcli From 17443a2fca8f4076485bb416d6ff4b7b5e0360f8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 12 Sep 2024 10:49:08 +0200 Subject: [PATCH 6/6] Switch to master branch of ros2_control --- ros2_control_demos.jazzy.repos | 4 ++-- ros2_control_demos.rolling.repos | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/ros2_control_demos.jazzy.repos b/ros2_control_demos.jazzy.repos index 7dc7f87a..8ba0dc2c 100644 --- a/ros2_control_demos.jazzy.repos +++ b/ros2_control_demos.jazzy.repos @@ -9,8 +9,8 @@ repositories: version: master ros2_control: type: git - url: https://github.com/pal-robotics-forks/ros2_control.git - version: update/ros2controlcli + url: https://github.com/ros-controls/ros2_control.git + version: master ros2_controllers: type: git url: https://github.com/ros-controls/ros2_controllers.git diff --git a/ros2_control_demos.rolling.repos b/ros2_control_demos.rolling.repos index a9633510..7eba4e04 100644 --- a/ros2_control_demos.rolling.repos +++ b/ros2_control_demos.rolling.repos @@ -9,8 +9,8 @@ repositories: version: master ros2_control: type: git - url: https://github.com/pal-robotics-forks/ros2_control.git - version: update/ros2controlcli + url: https://github.com/ros-controls/ros2_control.git + version: master ros2_controllers: type: git url: https://github.com/ros-controls/ros2_controllers.git