Wait for robot_description in moveit startup

Since we built our moveit config around receiving the description via topic, we also add a script that waits for the robot description to be published.
This helps when components support subscribing to the description, but crash if it is not present.

ur_moveit_config/launch/ur_moveit.launch.py

@@ -33,8 +33,9 @@
 from pathlib import Path
 
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument
+from launch.actions import DeclareLaunchArgument, RegisterEventHandler
 from launch.conditions import IfCondition
+from launch.event_handlers import OnProcessExit
 from launch.substitutions import (
     LaunchConfiguration,
     PathJoinSubstitution,
@@ -93,38 +94,49 @@ def generate_launch_description():
     ld = LaunchDescription()
     ld.add_entity(declare_arguments())
 
-    ld.add_action(
-        Node(
-            package="moveit_ros_move_group",
-            executable="move_group",
-            output="screen",
-            parameters=[
-                moveit_config.to_dict(),
-                warehouse_ros_config,
-            ],
-        )
+    wait_robot_description = Node(
+        package="ur_robot_driver",
+        executable="wait_for_robot_description",
+        output="screen",
+    )
+    ld.add_action(wait_robot_description)
+
+    move_group_node = Node(
+        package="moveit_ros_move_group",
+        executable="move_group",
+        output="screen",
+        parameters=[
+            moveit_config.to_dict(),
+            warehouse_ros_config,
+        ],
     )
 
     rviz_config_file = PathJoinSubstitution(
         [FindPackageShare("ur_moveit_config"), "config", "moveit.rviz"]
     )
+    rviz_node = Node(
+        package="rviz2",
+        condition=IfCondition(launch_rviz),
+        executable="rviz2",
+        name="rviz2_moveit",
+        output="log",
+        arguments=["-d", rviz_config_file],
+        parameters=[
+            moveit_config.robot_description,
+            moveit_config.robot_description_semantic,
+            moveit_config.robot_description_kinematics,
+            moveit_config.planning_pipelines,
+            moveit_config.joint_limits,
+            warehouse_ros_config,
+        ],
+    )
+
     ld.add_action(
-        Node(
-            package="rviz2",
-            condition=IfCondition(launch_rviz),
-            executable="rviz2",
-            name="rviz2_moveit",
-            output="log",
-            arguments=["-d", rviz_config_file],
-            parameters=[
-                moveit_config.robot_description,
-                moveit_config.robot_description_semantic,
-                moveit_config.robot_description_kinematics,
-                moveit_config.planning_pipelines,
-                moveit_config.joint_limits,
-                warehouse_ros_config,
-            ],
-        )
+        RegisterEventHandler(
+            OnProcessExit(
+                target_action=wait_robot_description, on_exit=[move_group_node, rviz_node]
+            )
+        ),
     )
 
     return ld

ur_robot_driver/CMakeLists.txt

@@ -142,20 +142,18 @@ ament_export_dependencies(
 # Install Python execs
 ament_python_install_package(${PROJECT_NAME})
 
-# Install Python executables
+# Install executables
 install(PROGRAMS
   scripts/tool_communication.py
+  scripts/wait_for_robot_description
+  scripts/start_ursim.sh
   DESTINATION lib/${PROJECT_NAME}
 )
 
 install(PROGRAMS scripts/wait_dashboard_server.sh
   DESTINATION bin
 )
 
-install(PROGRAMS scripts/start_ursim.sh
-  DESTINATION lib/${PROJECT_NAME}
-)
-
 install(DIRECTORY config launch urdf
   DESTINATION share/${PROJECT_NAME}
 )

ur_robot_driver/scripts/wait_for_robot_description

@@ -0,0 +1,74 @@
+#!/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.
+
+import rclpy
+from rclpy.node import Node
+
+from std_msgs.msg import String
+
+from rclpy.task import Future
+from rclpy.qos import QoSProfile, QoSDurabilityPolicy
+
+
+class DescriptionSubscriber(Node):
+
+    def __init__(self):
+        super().__init__("wait_for_robot_description")
+        self.topic = "robot_description"
+        qos_profile = QoSProfile(
+            depth=1,
+            durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
+        )
+        self.subscription = self.create_subscription(
+            String, self.topic, self.desc_callback, qos_profile=qos_profile
+        )
+        self.future = Future()
+
+        self.get_logger().info(f"Waiting for message on {self.resolve_topic_name(self.topic)}.")
+
+    def desc_callback(self, msg):
+        self.get_logger().info(
+            f"Received message on {self.resolve_topic_name(self.topic)}. Shutting down."
+        )
+        self.future.set_result(True)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    sub = DescriptionSubscriber()
+
+    rclpy.spin_until_future_complete(sub, sub.future)
+
+    sub.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()