[doc] Add more documentation regarding usage (#1055)

* Add note about installing ros2controlcli
* Add example_move script
* Add test for example_move

Author: Felix Exner (fexner)

ur_robot_driver/CMakeLists.txt

@@ -146,6 +146,7 @@ ament_python_install_package(${PROJECT_NAME})
 install(PROGRAMS
   scripts/tool_communication.py
   scripts/wait_for_robot_description
+  scripts/example_move.py
   scripts/start_ursim.sh
   DESTINATION lib/${PROJECT_NAME}
 )
@@ -171,6 +172,10 @@ if(BUILD_TESTING)
       TIMEOUT
         180
     )
+    add_launch_test(test/example_move.py
+      TIMEOUT
+        180
+    )
     add_launch_test(test/robot_driver.py
       TIMEOUT
         800

ur_robot_driver/doc/usage.rst

@@ -53,7 +53,8 @@ The most relevant arguments are the following:
 
   Note: *joint_state_broadcaster*\ , *speed_scaling_state_broadcaster*\ , *force_torque_sensor_broadcaster*\ , and *io_and_status_controller* will always start.
 
-  *HINT* : list all loaded controllers using ``ros2 control list_controllers`` command.
+  *HINT* : list all loaded controllers using ``ros2 control list_controllers`` command. For this,
+  the package ``ros2controlcli`` must be installed (``sudo apt-get install ros-${ROS_DISTRO}-ros2controlcli``).
 
 **NOTE**\ : The package can simulate hardware with the ros2_control ``MockSystem``. This emulator enables an environment for testing of "piping" of hardware and controllers, as well as testing robot's descriptions. For more details see `ros2_control documentation <https://ros-controls.github.io/control.ros.org/>`_ for more details.
 
@@ -139,7 +140,7 @@ Allowed UR - Type strings: ``ur3``\ , ``ur3e``\ , ``ur5``\ , ``ur5e``\ , ``ur10`
 2. Sending commands to controllers
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
-Before running any commands, first check the controllers' state using ``ros2 control list_controllers``.
+Before running any commands, first check the controllers' state using ``ros2 control list_controllers`` (Remember to install the ``ros2controlcli`` package as mentioned above).
 
 
 * Send some goal to the Joint Trajectory Controller by using a demo node from `ros2_controllers_test_nodes <https://github.com/ros-controls/ros2_controllers/blob/master/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_joint_trajectory_controller.py>`_ package by starting  the following command in another terminal:
@@ -164,6 +165,28 @@ Before running any commands, first check the controllers' state using ``ros2 con
 
   After a few seconds the robot should move(or jump when using emulation).
 
+In case you want to write your own ROS node to move the robot, there is an example python node included that you can use as a start.
+
+
+.. code-block:: console
+
+   $ ros2 run ur_robot_driver example_move.py
+   [INFO] [1720623611.547903428] [jtc_client]: Waiting for action server on scaled_joint_trajectory_controller/follow_joint_trajectory
+   [INFO] [1720623611.548368095] [jtc_client]: Executing trajectory traj0
+   [INFO] [1720623620.530203889] [jtc_client]: Done with result: SUCCESSFUL
+   [INFO] [1720623622.530668700] [jtc_client]: Executing trajectory traj1
+   [INFO] [1720623630.582108072] [jtc_client]: Done with result: SUCCESSFUL
+   [INFO] [1720623632.582576444] [jtc_client]: Done with all trajectories
+   [INFO] [1720623632.582957452] [jtc_client]: Done
+
+
+.. warning::
+
+   This is a very basic node that doesn't have the same safety checks as the test nodes above. Look
+   at the code and make sure that the robot is able to perform the motions safely before running
+   this on a real robot!
+
+
 3. Using only robot description
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 

ur_robot_driver/scripts/example_move.py

@@ -0,0 +1,190 @@
+#!/usr/bin/env python3
+# Copyright (c) 2024 FZI Forschungszentrum Informatik
+#
+# 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: Felix Exner
+
+# This is an example of how to interface the robot without any additional ROS components. For
+# real-life applications, we do recommend to use something like MoveIt!
+
+import time
+
+import rclpy
+from rclpy.action import ActionClient
+
+from builtin_interfaces.msg import Duration
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+from control_msgs.action import FollowJointTrajectory
+
+TRAJECTORIES = {
+    "traj0": [
+        {
+            "positions": [0.043128, -1.28824, 1.37179, -1.82208, -1.63632, -0.18],
+            "velocities": [0, 0, 0, 0, 0, 0],
+            "time_from_start": Duration(sec=4, nanosec=0),
+        },
+        {
+            "positions": [-0.195016, -1.70093, 0.902027, -0.944217, -1.52982, -0.195171],
+            "velocities": [0, 0, 0, 0, 0, 0],
+            "time_from_start": Duration(sec=8, nanosec=0),
+        },
+    ],
+    "traj1": [
+        {
+            "positions": [-0.195016, -1.70094, 0.902027, -0.944217, -1.52982, -0.195171],
+            "velocities": [0, 0, 0, 0, 0, 0],
+            "time_from_start": Duration(sec=0, nanosec=0),
+        },
+        {
+            "positions": [0.30493, -0.982258, 0.955637, -1.48215, -1.72737, 0.204445],
+            "velocities": [0, 0, 0, 0, 0, 0],
+            "time_from_start": Duration(sec=8, nanosec=0),
+        },
+    ],
+}
+
+
+class JTCClient(rclpy.node.Node):
+    """Small test client for the jtc."""
+
+    def __init__(self):
+        super().__init__("jtc_client")
+        self.declare_parameter("controller_name", "scaled_joint_trajectory_controller")
+        self.declare_parameter(
+            "joints",
+            [
+                "shoulder_pan_joint",
+                "shoulder_lift_joint",
+                "elbow_joint",
+                "wrist_1_joint",
+                "wrist_2_joint",
+                "wrist_3_joint",
+            ],
+        )
+
+        controller_name = self.get_parameter("controller_name").value + "/follow_joint_trajectory"
+        self.joints = self.get_parameter("joints").value
+
+        if self.joints is None or len(self.joints) == 0:
+            raise Exception('"joints" parameter is required')
+
+        self._action_client = ActionClient(self, FollowJointTrajectory, controller_name)
+        self.get_logger().info(f"Waiting for action server on {controller_name}")
+        self._action_client.wait_for_server()
+
+        self.parse_trajectories()
+        self.i = 0
+        self._send_goal_future = None
+        self._get_result_future = None
+        self.execute_next_trajectory()
+
+    def parse_trajectories(self):
+        self.goals = {}
+
+        for traj_name in TRAJECTORIES:
+            goal = JointTrajectory()
+            goal.joint_names = self.joints
+            for pt in TRAJECTORIES[traj_name]:
+                point = JointTrajectoryPoint()
+                point.positions = pt["positions"]
+                point.velocities = pt["velocities"]
+                point.time_from_start = pt["time_from_start"]
+                goal.points.append(point)
+
+            self.goals[traj_name] = goal
+
+    def execute_next_trajectory(self):
+        if self.i >= len(self.goals):
+            self.get_logger().info("Done with all trajectories")
+            raise SystemExit
+        traj_name = list(self.goals)[self.i]
+        self.i = self.i + 1
+        if traj_name:
+            self.execute_trajectory(traj_name)
+
+    def execute_trajectory(self, traj_name):
+        self.get_logger().info(f"Executing trajectory {traj_name}")
+        goal = FollowJointTrajectory.Goal()
+        goal.trajectory = self.goals[traj_name]
+
+        goal.goal_time_tolerance = Duration(sec=0, nanosec=1000)
+
+        self._send_goal_future = self._action_client.send_goal_async(goal)
+        self._send_goal_future.add_done_callback(self.goal_response_callback)
+
+    def goal_response_callback(self, future):
+        goal_handle = future.result()
+        if not goal_handle.accepted:
+            self.get_logger().error("Goal rejected :(")
+            raise RuntimeError("Goal rejected :(")
+
+        self.get_logger().debug("Goal accepted :)")
+
+        self._get_result_future = goal_handle.get_result_async()
+        self._get_result_future.add_done_callback(self.get_result_callback)
+
+    def get_result_callback(self, future):
+        result = future.result().result
+        self.get_logger().info(f"Done with result: {self.error_code_to_str(result.error_code)}")
+        if result.error_code == FollowJointTrajectory.Result.SUCCESSFUL:
+            time.sleep(2)
+            self.execute_next_trajectory()
+        else:
+            raise RuntimeError("Executing trajectory failed")
+
+    @staticmethod
+    def error_code_to_str(error_code):
+        if error_code == FollowJointTrajectory.Result.SUCCESSFUL:
+            return "SUCCESSFUL"
+        if error_code == FollowJointTrajectory.Result.INVALID_GOAL:
+            return "INVALID_GOAL"
+        if error_code == FollowJointTrajectory.Result.INVALID_JOINTS:
+            return "INVALID_JOINTS"
+        if error_code == FollowJointTrajectory.Result.OLD_HEADER_TIMESTAMP:
+            return "OLD_HEADER_TIMESTAMP"
+        if error_code == FollowJointTrajectory.Result.PATH_TOLERANCE_VIOLATED:
+            return "PATH_TOLERANCE_VIOLATED"
+        if error_code == FollowJointTrajectory.Result.GOAL_TOLERANCE_VIOLATED:
+            return "GOAL_TOLERANCE_VIOLATED"
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    jtc_client = JTCClient()
+    try:
+        rclpy.spin(jtc_client)
+    except SystemExit:
+        rclpy.logging.get_logger("jtc_client").info("Done")
+
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

ur_robot_driver/test/example_move.py

@@ -0,0 +1,78 @@
+#!/usr/bin/env python
+# Copyright 2019, FZI Forschungszentrum Informatik
+#
+# 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.
+
+import os
+import subprocess
+import sys
+import time
+import unittest
+
+import rclpy
+from rclpy.node import Node as ROSNode
+
+sys.path.append(os.path.dirname(__file__))
+from test_common import (  # noqa: E402
+    generate_driver_test_description,
+    DashboardInterface,
+    IoStatusInterface,
+)
+
+
+def generate_test_description():
+    ld = generate_driver_test_description()
+    return ld
+
+
+class ExampleMoveTest(unittest.TestCase):
+    @classmethod
+    def setUpClass(cls):
+        # Initialize the ROS context
+        rclpy.init()
+        cls.node = ROSNode("example_move_test")
+        time.sleep(1)
+        cls.init_robot(cls)
+
+    @classmethod
+    def tearDownClass(cls):
+        # Shutdown the ROS context
+        cls.node.destroy_node()
+        rclpy.shutdown()
+
+    def init_robot(self):
+        self._dashboard_interface = DashboardInterface(self.node)
+        self._io_status_controller_interface = IoStatusInterface(self.node)
+
+    def setUp(self):
+        self._dashboard_interface.start_robot()
+        time.sleep(1)
+        self.assertTrue(self._io_status_controller_interface.resend_robot_program().success)
+
+    def test_example_move(self):
+        cmd = ["ros2", "run", "ur_robot_driver", "example_move.py"]
+        subprocess.check_output(cmd)