Simulation launch fileΒΆ
The following is the complete launch file described in more detail in the simulation setup tutorial.
1# Imports
2import xacro, os
3from ament_index_python.packages import get_package_share_directory
4from launch import LaunchDescription
5from launch.substitutions import PathJoinSubstitution
6from launch_ros.actions import Node
7from launch_ros.substitutions import FindPackageShare
8
9
10
11# ROS2 launch description
12def generate_launch_description():
13
14 # Declare arguments
15 share_pkg_dir = get_package_share_directory("ur_coppeliasim")
16 xacro_path = os.path.join(share_pkg_dir, "urdf", "ur.urdf.xacro")
17 available_joint_controllers = os.path.join(
18 share_pkg_dir, "config", "ur_controllers_coppelia.yaml"
19 )
20 declared_arguments = []
21 use_sim_time = True
22 ur_type = "ur3e"
23
24 robot_description_content = xacro.process_file(
25 xacro_path,
26 mappings={
27 "safety_limits": "true",
28 "safety_pos_margin": "0.15",
29 "safety_k_position": "20",
30 "name": "ur",
31 "ur_type": ur_type,
32 "prefix": '',
33 "sim_ignition": "false",
34 "sim_gazebo": "false",
35 "simulation_controllers": available_joint_controllers
36 }
37 ).toprettyxml(indent=' ')
38 robot_description = {"robot_description": robot_description_content}
39 robot_controllers = available_joint_controllers
40
41 # Control node
42 control_node = Node(
43 package="controller_manager",
44 executable="ros2_control_node",
45 parameters=[
46 robot_description, robot_controllers, {
47 "use_sim_time": use_sim_time
48 }
49 ],
50 output="both",
51 remappings=[
52 ('motion_control_handle/target_frame', 'target_frame'),
53 ('cartesian_motion_controller/target_frame', 'target_frame'),
54 ]
55 )
56
57 # Joint broadcaster
58 joint_state_broadcaster_spawner = Node(
59 package="controller_manager",
60 executable="spawner",
61 arguments=["joint_state_broadcaster", "-c", "/controller_manager"],
62 parameters=[{
63 "use_sim_time": use_sim_time
64 }]
65 )
66
67 # Cartesian motion controller
68 cartesian_motion_controller_spawner = Node(
69 package="controller_manager",
70 executable="spawner",
71 arguments=["cartesian_motion_controller", "-c", "/controller_manager"],
72 parameters=[{
73 "use_sim_time": use_sim_time
74 }]
75 )
76
77 # Motion handle
78 motion_control_handle_spawner = Node(
79 package="controller_manager",
80 executable="spawner",
81 arguments=["motion_control_handle", "-c", "/controller_manager"],
82 parameters=[{
83 "use_sim_time": use_sim_time
84 }]
85 )
86
87 # State publisher
88 robot_state_publisher = Node(
89 package="robot_state_publisher",
90 executable="robot_state_publisher",
91 output="both",
92 parameters=[robot_description, {
93 "use_sim_time": use_sim_time
94 }],
95 )
96
97 # RViz
98 rviz_config = PathJoinSubstitution([
99 FindPackageShare("cartesian_controller_simulation"), "etc", "robot.rviz"
100 ])
101 rviz = Node(
102 package="rviz2",
103 executable="rviz2",
104 name="rviz2",
105 output="log",
106 arguments=["-d", rviz_config],
107 parameters=[{
108 "use_sim_time": use_sim_time
109 }]
110 )
111
112 # Final launch description
113 nodes = [
114 control_node,
115 joint_state_broadcaster_spawner,
116 cartesian_motion_controller_spawner,
117 motion_control_handle_spawner,
118 robot_state_publisher,
119 rviz,
120 ]
121
122 return LaunchDescription(declared_arguments + nodes)