From 6dbccd6d7912cd3bbc18d100a2ecd96429443941 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 21 May 2024 12:59:01 +0200 Subject: [PATCH] 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 | 66 ++++++++++------- ur_robot_driver/CMakeLists.txt | 8 +- .../scripts/wait_for_robot_description | 74 +++++++++++++++++++ 3 files changed, 116 insertions(+), 32 deletions(-) create mode 100755 ur_robot_driver/scripts/wait_for_robot_description diff --git a/ur_moveit_config/launch/ur_moveit.launch.py b/ur_moveit_config/launch/ur_moveit.launch.py index 2ce2c5a8b..5ee7b048a 100644 --- a/ur_moveit_config/launch/ur_moveit.launch.py +++ b/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 diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt index 640667234..d341b911f 100644 --- a/ur_robot_driver/CMakeLists.txt +++ b/ur_robot_driver/CMakeLists.txt @@ -142,9 +142,11 @@ 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} ) @@ -152,10 +154,6 @@ 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} ) diff --git a/ur_robot_driver/scripts/wait_for_robot_description b/ur_robot_driver/scripts/wait_for_robot_description new file mode 100755 index 000000000..6975cd8ec --- /dev/null +++ b/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()