diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index 95117915..923abfd0 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -34,216 +34,40 @@ from launch_ros.substitutions import FindPackageShare from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.conditions import IfCondition, UnlessCondition from launch.substitutions import ( - Command, - FindExecutable, LaunchConfiguration, PathJoinSubstitution, ) +from launch.launch_description_sources import AnyLaunchDescriptionSource -def launch_setup(context, *args, **kwargs): +def launch_setup(): # Initialize Arguments ur_type = LaunchConfiguration("ur_type") robot_ip = LaunchConfiguration("robot_ip") - safety_limits = LaunchConfiguration("safety_limits") - safety_pos_margin = LaunchConfiguration("safety_pos_margin") - safety_k_position = LaunchConfiguration("safety_k_position") # General arguments - runtime_config_package = LaunchConfiguration("runtime_config_package") controllers_file = LaunchConfiguration("controllers_file") - description_package = LaunchConfiguration("description_package") - description_file = LaunchConfiguration("description_file") - tf_prefix = LaunchConfiguration("tf_prefix") + description_launchfile = LaunchConfiguration("description_launchfile") use_mock_hardware = LaunchConfiguration("use_mock_hardware") - mock_sensor_commands = LaunchConfiguration("mock_sensor_commands") controller_spawner_timeout = LaunchConfiguration("controller_spawner_timeout") initial_joint_controller = LaunchConfiguration("initial_joint_controller") activate_joint_controller = LaunchConfiguration("activate_joint_controller") launch_rviz = LaunchConfiguration("launch_rviz") + rviz_config_file = LaunchConfiguration("rviz_config_file") headless_mode = LaunchConfiguration("headless_mode") launch_dashboard_client = LaunchConfiguration("launch_dashboard_client") use_tool_communication = LaunchConfiguration("use_tool_communication") - tool_parity = LaunchConfiguration("tool_parity") - tool_baud_rate = LaunchConfiguration("tool_baud_rate") - tool_stop_bits = LaunchConfiguration("tool_stop_bits") - tool_rx_idle_chars = LaunchConfiguration("tool_rx_idle_chars") - tool_tx_idle_chars = LaunchConfiguration("tool_tx_idle_chars") tool_device_name = LaunchConfiguration("tool_device_name") tool_tcp_port = LaunchConfiguration("tool_tcp_port") - tool_voltage = LaunchConfiguration("tool_voltage") - reverse_ip = LaunchConfiguration("reverse_ip") - script_command_port = LaunchConfiguration("script_command_port") - reverse_port = LaunchConfiguration("reverse_port") - script_sender_port = LaunchConfiguration("script_sender_port") - trajectory_port = LaunchConfiguration("trajectory_port") - - joint_limit_params = PathJoinSubstitution( - [FindPackageShare(description_package), "config", ur_type, "joint_limits.yaml"] - ) - kinematics_params = PathJoinSubstitution( - [ - FindPackageShare(description_package), - "config", - ur_type, - "default_kinematics.yaml", - ] - ) - physical_params = PathJoinSubstitution( - [ - FindPackageShare(description_package), - "config", - ur_type, - "physical_parameters.yaml", - ] - ) - visual_params = PathJoinSubstitution( - [ - FindPackageShare(description_package), - "config", - ur_type, - "visual_parameters.yaml", - ] - ) - script_filename = PathJoinSubstitution( - [ - FindPackageShare("ur_client_library"), - "resources", - "external_control.urscript", - ] - ) - input_recipe_filename = PathJoinSubstitution( - [FindPackageShare("ur_robot_driver"), "resources", "rtde_input_recipe.txt"] - ) - output_recipe_filename = PathJoinSubstitution( - [FindPackageShare("ur_robot_driver"), "resources", "rtde_output_recipe.txt"] - ) - - robot_description_content = Command( - [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution([FindPackageShare("ur_robot_driver"), "urdf", description_file]), - " ", - "robot_ip:=", - robot_ip, - " ", - "joint_limit_params:=", - joint_limit_params, - " ", - "kinematics_params:=", - kinematics_params, - " ", - "physical_params:=", - physical_params, - " ", - "visual_params:=", - visual_params, - " ", - "safety_limits:=", - safety_limits, - " ", - "safety_pos_margin:=", - safety_pos_margin, - " ", - "safety_k_position:=", - safety_k_position, - " ", - "name:=", - ur_type, - " ", - "script_filename:=", - script_filename, - " ", - "input_recipe_filename:=", - input_recipe_filename, - " ", - "output_recipe_filename:=", - output_recipe_filename, - " ", - "tf_prefix:=", - tf_prefix, - " ", - "use_mock_hardware:=", - use_mock_hardware, - " ", - "mock_sensor_commands:=", - mock_sensor_commands, - " ", - "headless_mode:=", - headless_mode, - " ", - "use_tool_communication:=", - use_tool_communication, - " ", - "tool_parity:=", - tool_parity, - " ", - "tool_baud_rate:=", - tool_baud_rate, - " ", - "tool_stop_bits:=", - tool_stop_bits, - " ", - "tool_rx_idle_chars:=", - tool_rx_idle_chars, - " ", - "tool_tx_idle_chars:=", - tool_tx_idle_chars, - " ", - "tool_device_name:=", - tool_device_name, - " ", - "tool_tcp_port:=", - tool_tcp_port, - " ", - "tool_voltage:=", - tool_voltage, - " ", - "reverse_ip:=", - reverse_ip, - " ", - "script_command_port:=", - script_command_port, - " ", - "reverse_port:=", - reverse_port, - " ", - "script_sender_port:=", - script_sender_port, - " ", - "trajectory_port:=", - trajectory_port, - " ", - ] - ) - robot_description = {"robot_description": robot_description_content} - - initial_joint_controllers = PathJoinSubstitution( - [FindPackageShare(runtime_config_package), "config", controllers_file] - ) - - rviz_config_file = PathJoinSubstitution( - [FindPackageShare(description_package), "rviz", "view_robot.rviz"] - ) - - # define update rate - update_rate_config_file = PathJoinSubstitution( - [ - FindPackageShare(runtime_config_package), - "config", - ur_type.perform(context) + "_update_rate.yaml", - ] - ) control_node = Node( package="controller_manager", executable="ros2_control_node", parameters=[ - update_rate_config_file, - ParameterFile(initial_joint_controllers, allow_substs=True), + LaunchConfiguration("update_rate_config_file"), + ParameterFile(controllers_file, allow_substs=True), ], output="screen", ) @@ -302,13 +126,6 @@ def launch_setup(context, *args, **kwargs): ], ) - robot_state_publisher_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - output="both", - parameters=[robot_description], - ) - rviz_node = Node( package="rviz2", condition=IfCondition(launch_rviz), @@ -373,13 +190,21 @@ def controller_spawner(controllers, active=True): condition=UnlessCondition(activate_joint_controller), ) + rsp = IncludeLaunchDescription( + AnyLaunchDescriptionSource(description_launchfile), + launch_arguments={ + "robot_ip": robot_ip, + "ur_type": ur_type, + }.items(), + ) + nodes_to_start = [ control_node, dashboard_client_node, tool_communication_node, controller_stopper_node, urscript_interface, - robot_state_publisher_node, + rsp, rviz_node, initial_joint_controller_spawner_stopped, initial_joint_controller_spawner_started, @@ -435,34 +260,24 @@ def generate_launch_description(): ) ) # General arguments - declared_arguments.append( - DeclareLaunchArgument( - "runtime_config_package", - default_value="ur_robot_driver", - description='Package with the controller\'s configuration in "config" folder. ' - "Usually the argument is not set, it enables use of a custom setup.", - ) - ) declared_arguments.append( DeclareLaunchArgument( "controllers_file", - default_value="ur_controllers.yaml", + default_value=PathJoinSubstitution( + [FindPackageShare("ur_robot_driver"), "config", "ur_controllers.yaml"] + ), description="YAML file with the controllers configuration.", ) ) declared_arguments.append( DeclareLaunchArgument( - "description_package", - default_value="ur_description", - description="Description package with robot URDF/XACRO files. Usually the argument " - "is not set, it enables use of a custom description.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "description_file", - default_value="ur.urdf.xacro", - description="URDF/XACRO description file with the robot.", + "description_launchfile", + default_value=PathJoinSubstitution( + [FindPackageShare("ur_robot_driver"), "launch", "ur_rsp.launch.py"] + ), + description="Launchfile (absolute path) providing the description. " + "The launchfile has to start a robot_state_publisher node that " + "publishes the description topic.", ) ) declared_arguments.append( @@ -520,6 +335,15 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument("launch_rviz", default_value="true", description="Launch RViz?") ) + declared_arguments.append( + DeclareLaunchArgument( + "rviz_config_file", + default_value=PathJoinSubstitution( + [FindPackageShare("ur_description"), "rviz", "view_robot.rviz"] + ), + description="RViz config file (absolute path) to use when launching rviz.", + ) + ) declared_arguments.append( DeclareLaunchArgument( "launch_dashboard_client", @@ -633,4 +457,20 @@ def generate_launch_description(): description="Port that will be opened for trajectory control.", ) ) - return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)]) + declared_arguments.append( + DeclareLaunchArgument( + name="update_rate_config_file", + default_value=[ + PathJoinSubstitution( + [ + FindPackageShare("ur_robot_driver"), + "config", + ] + ), + "/", + LaunchConfiguration("ur_type"), + "_update_rate.yaml", + ], + ) + ) + return LaunchDescription(declared_arguments + launch_setup()) diff --git a/ur_robot_driver/launch/ur_rsp.launch.py b/ur_robot_driver/launch/ur_rsp.launch.py new file mode 100644 index 00000000..aa232422 --- /dev/null +++ b/ur_robot_driver/launch/ur_rsp.launch.py @@ -0,0 +1,472 @@ +# 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 + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import ( + Command, + FindExecutable, + LaunchConfiguration, + PathJoinSubstitution, +) + + +def generate_launch_description(): + ur_type = LaunchConfiguration("ur_type") + robot_ip = LaunchConfiguration("robot_ip") + safety_limits = LaunchConfiguration("safety_limits") + safety_pos_margin = LaunchConfiguration("safety_pos_margin") + safety_k_position = LaunchConfiguration("safety_k_position") + # General arguments + kinematics_params_file = LaunchConfiguration("kinematics_params_file") + physical_params_file = LaunchConfiguration("physical_params_file") + visual_params_file = LaunchConfiguration("visual_params_file") + joint_limit_params_file = LaunchConfiguration("joint_limit_params_file") + description_file = LaunchConfiguration("description_file") + tf_prefix = LaunchConfiguration("tf_prefix") + use_mock_hardware = LaunchConfiguration("use_mock_hardware") + mock_sensor_commands = LaunchConfiguration("mock_sensor_commands") + headless_mode = LaunchConfiguration("headless_mode") + use_tool_communication = LaunchConfiguration("use_tool_communication") + tool_parity = LaunchConfiguration("tool_parity") + tool_baud_rate = LaunchConfiguration("tool_baud_rate") + tool_stop_bits = LaunchConfiguration("tool_stop_bits") + tool_rx_idle_chars = LaunchConfiguration("tool_rx_idle_chars") + tool_tx_idle_chars = LaunchConfiguration("tool_tx_idle_chars") + tool_device_name = LaunchConfiguration("tool_device_name") + tool_tcp_port = LaunchConfiguration("tool_tcp_port") + tool_voltage = LaunchConfiguration("tool_voltage") + reverse_ip = LaunchConfiguration("reverse_ip") + script_command_port = LaunchConfiguration("script_command_port") + reverse_port = LaunchConfiguration("reverse_port") + script_sender_port = LaunchConfiguration("script_sender_port") + trajectory_port = LaunchConfiguration("trajectory_port") + + script_filename = PathJoinSubstitution( + [ + FindPackageShare("ur_client_library"), + "resources", + "external_control.urscript", + ] + ) + input_recipe_filename = PathJoinSubstitution( + [FindPackageShare("ur_robot_driver"), "resources", "rtde_input_recipe.txt"] + ) + output_recipe_filename = PathJoinSubstitution( + [FindPackageShare("ur_robot_driver"), "resources", "rtde_output_recipe.txt"] + ) + + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + description_file, + " ", + "robot_ip:=", + robot_ip, + " ", + "joint_limit_params:=", + joint_limit_params_file, + " ", + "kinematics_params:=", + kinematics_params_file, + " ", + "physical_params:=", + physical_params_file, + " ", + "visual_params:=", + visual_params_file, + " ", + "safety_limits:=", + safety_limits, + " ", + "safety_pos_margin:=", + safety_pos_margin, + " ", + "safety_k_position:=", + safety_k_position, + " ", + "name:=", + ur_type, + " ", + "script_filename:=", + script_filename, + " ", + "input_recipe_filename:=", + input_recipe_filename, + " ", + "output_recipe_filename:=", + output_recipe_filename, + " ", + "tf_prefix:=", + tf_prefix, + " ", + "use_mock_hardware:=", + use_mock_hardware, + " ", + "mock_sensor_commands:=", + mock_sensor_commands, + " ", + "headless_mode:=", + headless_mode, + " ", + "use_tool_communication:=", + use_tool_communication, + " ", + "tool_parity:=", + tool_parity, + " ", + "tool_baud_rate:=", + tool_baud_rate, + " ", + "tool_stop_bits:=", + tool_stop_bits, + " ", + "tool_rx_idle_chars:=", + tool_rx_idle_chars, + " ", + "tool_tx_idle_chars:=", + tool_tx_idle_chars, + " ", + "tool_device_name:=", + tool_device_name, + " ", + "tool_tcp_port:=", + tool_tcp_port, + " ", + "tool_voltage:=", + tool_voltage, + " ", + "reverse_ip:=", + reverse_ip, + " ", + "script_command_port:=", + script_command_port, + " ", + "reverse_port:=", + reverse_port, + " ", + "script_sender_port:=", + script_sender_port, + " ", + "trajectory_port:=", + trajectory_port, + " ", + ] + ) + robot_description = {"robot_description": robot_description_content} + + declared_arguments = [] + # UR specific arguments + declared_arguments.append( + DeclareLaunchArgument( + "ur_type", + description="Typo/series of used UR robot.", + choices=[ + "ur3", + "ur3e", + "ur5", + "ur5e", + "ur10", + "ur10e", + "ur16e", + "ur20", + "ur30", + ], + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "robot_ip", description="IP address by which the robot can be reached." + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "safety_limits", + default_value="true", + description="Enables the safety limits controller if true.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "safety_pos_margin", + default_value="0.15", + description="The margin to lower and upper limits in the safety controller.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "safety_k_position", + default_value="20", + description="k-position factor in the safety controller.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "joint_limit_params_file", + default_value=PathJoinSubstitution( + [ + FindPackageShare("ur_description"), + "config", + ur_type, + "joint_limits.yaml", + ] + ), + description="Config file containing the joint limits (e.g. velocities, positions) of the robot.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "kinematics_params_file", + default_value=PathJoinSubstitution( + [ + FindPackageShare("ur_description"), + "config", + ur_type, + "default_kinematics.yaml", + ] + ), + description="The calibration configuration of the actual robot used.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "physical_params_file", + default_value=PathJoinSubstitution( + [ + FindPackageShare("ur_description"), + "config", + ur_type, + "physical_parameters.yaml", + ] + ), + description="Config file containing the physical parameters (e.g. masses, inertia) of the robot.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "visual_params_file", + default_value=PathJoinSubstitution( + [ + FindPackageShare("ur_description"), + "config", + ur_type, + "visual_parameters.yaml", + ] + ), + description="Config file containing the visual parameters (e.g. meshes) of the robot.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "description_file", + default_value=PathJoinSubstitution( + [FindPackageShare("ur_robot_driver"), "urdf", "ur.urdf.xacro"] + ), + description="URDF/XACRO description file with the robot.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "tf_prefix", + default_value="", + description="tf_prefix of the joint names, useful for " + "multi-robot setup. If changed, also joint names in the controllers' configuration " + "have to be updated.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "use_mock_hardware", + default_value="false", + description="Start robot with mock hardware mirroring command to its states.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "mock_sensor_commands", + default_value="false", + description="Enable mock command interfaces for sensors used for simple simulations. " + "Used only if 'use_mock_hardware' parameter is true.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "headless_mode", + default_value="false", + description="Enable headless mode for robot control", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "initial_joint_controller", + default_value="scaled_joint_trajectory_controller", + description="Initially loaded robot controller.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "activate_joint_controller", + default_value="true", + description="Activate loaded joint controller.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument("launch_rviz", default_value="true", description="Launch RViz?") + ) + declared_arguments.append( + DeclareLaunchArgument( + "launch_dashboard_client", + default_value="true", + description="Launch Dashboard Client?", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "use_tool_communication", + default_value="false", + description="Only available for e series!", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "tool_parity", + default_value="0", + description="Parity configuration for serial communication. Only effective, if " + "use_tool_communication is set to True.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "tool_baud_rate", + default_value="115200", + description="Baud rate configuration for serial communication. Only effective, if " + "use_tool_communication is set to True.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "tool_stop_bits", + default_value="1", + description="Stop bits configuration for serial communication. Only effective, if " + "use_tool_communication is set to True.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "tool_rx_idle_chars", + default_value="1.5", + description="RX idle chars configuration for serial communication. Only effective, " + "if use_tool_communication is set to True.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "tool_tx_idle_chars", + default_value="3.5", + description="TX idle chars configuration for serial communication. Only effective, " + "if use_tool_communication is set to True.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "tool_device_name", + default_value="/tmp/ttyUR", + description="File descriptor that will be generated for the tool communication device. " + "The user has be be allowed to write to this location. " + "Only effective, if use_tool_communication is set to True.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "tool_tcp_port", + default_value="54321", + description="Remote port that will be used for bridging the tool's serial device. " + "Only effective, if use_tool_communication is set to True.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "tool_voltage", + default_value="0", # 0 being a conservative value that won't destroy anything + description="Tool voltage that will be setup.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "reverse_ip", + default_value="0.0.0.0", + description="IP that will be used for the robot controller to communicate back to the driver.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "script_command_port", + default_value="50004", + description="Port that will be opened to forward URScript commands to the robot.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "reverse_port", + default_value="50001", + description="Port that will be opened to send cyclic instructions from the driver to the robot controller.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "script_sender_port", + default_value="50002", + description="The driver will offer an interface to query the external_control URScript on this port.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "trajectory_port", + default_value="50003", + description="Port that will be opened for trajectory control.", + ) + ) + + return LaunchDescription( + declared_arguments + + [ + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ), + ] + )