Fix CLI load_controller verb (ros-controls#585)

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()
 
 

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)

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)

example_1/doc/userdoc.rst

@@ -241,15 +241,15 @@ 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
 
         (from the docker terminal, see above)
 
         .. 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 <https://github.com/ros-controls/ros2_control_demos/tree/{REPOS_FILE_BRANCH}/example_1/bringup/config/rrbot_controllers.yaml>`__
-     but the controller was not loaded from the `launch file rrbot.launch.py <https://github.com/ros-controls/ros2_control_demos/tree/{REPOS_FILE_BRANCH}/example_1/bringup/launch/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,15 +305,15 @@ 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
 
         (from the docker terminal, see above)
 
         .. 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 <https://github.com/ros-controls/ros2_control_demos/tree/{REPOS_FILE_BRANCH}/example_1/bringup/launch/rrbot.launch.py>`__
-* Controllers yaml: `rrbot_controllers.yaml <https://github.com/ros-controls/ros2_control_demos/tree/{REPOS_FILE_BRANCH}/example_1/bringup/config/rrbot_controllers.yaml>`__
+* Controllers yaml:
+
+  * `rrbot_controllers.yaml <https://github.com/ros-controls/ros2_control_demos/tree/{REPOS_FILE_BRANCH}/example_1/bringup/config/rrbot_controllers.yaml>`__
+  * `rrbot_jtc.yaml <https://github.com/ros-controls/ros2_control_demos/tree/{REPOS_FILE_BRANCH}/example_1/bringup/config/rrbot_jtc.yaml>`__
+
 * URDF file: `rrbot.urdf.xacro <https://github.com/ros-controls/ros2_control_demos/tree/{REPOS_FILE_BRANCH}/example_1/description/urdf/rrbot.urdf.xacro>`__
 
   * Description: `rrbot_description.urdf.xacro <https://github.com/ros-controls/ros2_control_demos/tree/{REPOS_FILE_BRANCH}/ros2_control_demo_description/rrbot/urdf/rrbot_description.urdf.xacro>`__

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.

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)

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):