Skip to content

Commit

Permalink
Move starting the robot_state_publisher to an own launch file (#977)
Browse files Browse the repository at this point in the history 
* Factor loading the description and launch the rsp into own launchfile
* Remove all "_package" configurations and directly use file paths instead
* Specify all description config files directly

With the `description_package/config/<ur_type>/...` approach one had
to actually replace all files in order to provide an own description.
This should make it easier to adapt.

Co-authored-by: Vincenzo Di Pentima <[email protected]>
  • Loading branch information
@fmauch @VinDp
fmauch and VinDp committed May 6, 2024
1 parent 2d5e315 commit 0dd8770
Show file tree
Hide file tree
Showing 2 changed files with 524 additions and 212 deletions.
264 changes: 52 additions & 212 deletions ur_robot_driver/launch/ur_control.launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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",
)
Expand Down Expand Up @@ -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),
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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(
Expand Down Expand Up @@ -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",
Expand Down Expand Up @@ -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())
Loading

0 comments on commit 0dd8770

Please sign in to comment.