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/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 06fbe33d..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 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 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 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_jtc.yaml .. group-tab:: Docker @@ -318,7 +313,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_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 2b25f51a..ecc66ecf 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_jtc.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..cab04cfa --- /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_jtc.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):