moveit2
The MoveIt Motion Planning Framework for ROS 2.
launches.py
Go to the documentation of this file.
1 import os
2 
3 from launch import LaunchDescription
4 from launch.actions import (
5  DeclareLaunchArgument,
6  IncludeLaunchDescription,
7 )
8 from launch.conditions import IfCondition
9 from launch.launch_description_sources import PythonLaunchDescriptionSource
10 from launch.substitutions import LaunchConfiguration
11 
12 from launch_ros.actions import Node
13 from launch_ros.parameter_descriptions import ParameterValue
14 
15 from srdfdom.srdf import SRDF
16 
18  add_debuggable_node,
19  DeclareBooleanLaunchArg,
20 )
21 
22 
23 def generate_rsp_launch(moveit_config):
24  """Launch file for robot state publisher (rsp)"""
25 
26  ld = LaunchDescription()
27  ld.add_action(DeclareLaunchArgument("publish_frequency", default_value="15.0"))
28 
29  # Given the published joint states, publish tf for the robot links and the robot description
30  rsp_node = Node(
31  package="robot_state_publisher",
32  executable="robot_state_publisher",
33  respawn=True,
34  output="screen",
35  parameters=[
36  moveit_config.robot_description,
37  {
38  "publish_frequency": LaunchConfiguration("publish_frequency"),
39  },
40  ],
41  )
42  ld.add_action(rsp_node)
43 
44  return ld
45 
46 
47 def generate_moveit_rviz_launch(moveit_config):
48  """Launch file for rviz"""
49  ld = LaunchDescription()
50 
51  ld.add_action(DeclareBooleanLaunchArg("debug", default_value=False))
52  ld.add_action(
53  DeclareLaunchArgument(
54  "rviz_config",
55  default_value=str(moveit_config.package_path / "config/moveit.rviz"),
56  )
57  )
58 
59  rviz_parameters = [
60  moveit_config.planning_pipelines,
61  moveit_config.robot_description_kinematics,
62  ]
63 
65  ld,
66  package="rviz2",
67  executable="rviz2",
68  output="log",
69  respawn=False,
70  arguments=["-d", LaunchConfiguration("rviz_config")],
71  parameters=rviz_parameters,
72  )
73 
74  return ld
75 
76 
78  """Launch file for MoveIt Setup Assistant"""
79  ld = LaunchDescription()
80 
81  ld.add_action(DeclareBooleanLaunchArg("debug", default_value=False))
83  ld,
84  package="moveit_setup_assistant",
85  executable="moveit_setup_assistant",
86  arguments=[["--config_pkg=", str(moveit_config.package_path)]],
87  )
88 
89  return ld
90 
91 
93  ld = LaunchDescription()
94 
95  name_counter = 0
96 
97  for key, xml_contents in moveit_config.robot_description_semantic.items():
98  srdf = SRDF.from_xml_string(xml_contents)
99  for vj in srdf.virtual_joints:
100  ld.add_action(
101  Node(
102  package="tf2_ros",
103  executable="static_transform_publisher",
104  name=f"static_transform_publisher{name_counter}",
105  output="log",
106  arguments=[
107  "--frame-id",
108  vj.parent_frame,
109  "--child-frame-id",
110  vj.child_link,
111  ],
112  )
113  )
114  name_counter += 1
115  return ld
116 
117 
119  controller_names = moveit_config.trajectory_execution.get(
120  "moveit_simple_controller_manager", {}
121  ).get("controller_names", [])
122  ld = LaunchDescription()
123  for controller in controller_names + ["joint_state_broadcaster"]:
124  ld.add_action(
125  Node(
126  package="controller_manager",
127  executable="spawner",
128  arguments=[controller],
129  output="screen",
130  )
131  )
132  return ld
133 
134 
135 def generate_warehouse_db_launch(moveit_config):
136  """Launch file for warehouse database"""
137  ld = LaunchDescription()
138  ld.add_action(
139  DeclareLaunchArgument(
140  "moveit_warehouse_database_path",
141  default_value=str(
142  moveit_config.package_path / "default_warehouse_mongo_db"
143  ),
144  )
145  )
146  ld.add_action(DeclareBooleanLaunchArg("reset", default_value=False))
147 
148  # The default DB port for moveit (not default MongoDB port to avoid potential conflicts)
149  ld.add_action(DeclareLaunchArgument("moveit_warehouse_port", default_value="33829"))
150 
151  # The default DB host for moveit
152  ld.add_action(
153  DeclareLaunchArgument("moveit_warehouse_host", default_value="localhost")
154  )
155 
156  # Load warehouse parameters
157  db_parameters = [
158  {
159  "overwrite": False,
160  "database_path": LaunchConfiguration("moveit_warehouse_database_path"),
161  "warehouse_port": LaunchConfiguration("moveit_warehouse_port"),
162  "warehouse_host": LaunchConfiguration("moveit_warehouse_host"),
163  "warehouse_exec": "mongod",
164  "warehouse_plugin": "warehouse_ros_mongo::MongoDatabaseConnection",
165  },
166  ]
167  # Run the DB server
168  db_node = Node(
169  package="warehouse_ros_mongo",
170  executable="mongo_wrapper_ros.py",
171  # TODO(dlu): Figure out if this needs to be run in a specific directory
172  # (ROS 1 version set cwd="ROS_HOME")
173  parameters=db_parameters,
174  )
175  ld.add_action(db_node)
176 
177  # If we want to reset the database, run this node
178  reset_node = Node(
179  package="moveit_ros_warehouse",
180  executable="moveit_init_demo_warehouse",
181  output="screen",
182  condition=IfCondition(LaunchConfiguration("reset")),
183  )
184  ld.add_action(reset_node)
185 
186  return ld
187 
188 
189 def generate_move_group_launch(moveit_config):
190  ld = LaunchDescription()
191 
192  ld.add_action(DeclareBooleanLaunchArg("debug", default_value=False))
193  ld.add_action(
194  DeclareBooleanLaunchArg("allow_trajectory_execution", default_value=True)
195  )
196  ld.add_action(
197  DeclareBooleanLaunchArg("publish_monitored_planning_scene", default_value=True)
198  )
199  # load non-default MoveGroup capabilities (space separated)
200  ld.add_action(
201  DeclareLaunchArgument(
202  "capabilities",
203  default_value=moveit_config.move_group_capabilities["capabilities"],
204  )
205  )
206  # inhibit these default MoveGroup capabilities (space separated)
207  ld.add_action(
208  DeclareLaunchArgument(
209  "disable_capabilities",
210  default_value=moveit_config.move_group_capabilities["disable_capabilities"],
211  )
212  )
213 
214  # do not copy dynamics information from /joint_states to internal robot monitoring
215  # default to false, because almost nothing in move_group relies on this information
216  ld.add_action(DeclareBooleanLaunchArg("monitor_dynamics", default_value=False))
217 
218  should_publish = LaunchConfiguration("publish_monitored_planning_scene")
219 
220  move_group_configuration = {
221  "publish_robot_description_semantic": True,
222  "allow_trajectory_execution": LaunchConfiguration("allow_trajectory_execution"),
223  # Note: Wrapping the following values is necessary so that the parameter value can be the empty string
224  "capabilities": ParameterValue(
225  LaunchConfiguration("capabilities"), value_type=str
226  ),
227  "disable_capabilities": ParameterValue(
228  LaunchConfiguration("disable_capabilities"), value_type=str
229  ),
230  # Publish the planning scene of the physical robot so that rviz plugin can know actual robot
231  "publish_planning_scene": should_publish,
232  "publish_geometry_updates": should_publish,
233  "publish_state_updates": should_publish,
234  "publish_transforms_updates": should_publish,
235  "monitor_dynamics": False,
236  }
237 
238  move_group_params = [
239  moveit_config.to_dict(),
240  move_group_configuration,
241  ]
242 
244  ld,
245  package="moveit_ros_move_group",
246  executable="move_group",
247  commands_file=str(moveit_config.package_path / "launch" / "gdb_settings.gdb"),
248  output="screen",
249  parameters=move_group_params,
250  extra_debug_args=["--debug"],
251  # Set the display variable, in case OpenGL code is used internally
252  additional_env={"DISPLAY": os.environ["DISPLAY"]},
253  )
254  return ld
255 
256 
257 def generate_demo_launch(moveit_config, launch_package_path=None):
258  """
259  Launches a self contained demo
260 
261  launch_package_path is optional to use different launch and config packages
262 
263  Includes
264  * static_virtual_joint_tfs
265  * robot_state_publisher
266  * move_group
267  * moveit_rviz
268  * warehouse_db (optional)
269  * ros2_control_node + controller spawners
270  """
271  if launch_package_path == None:
272  launch_package_path = moveit_config.package_path
273 
274  ld = LaunchDescription()
275  ld.add_action(
277  "db",
278  default_value=False,
279  description="By default, we do not start a database (it can be large)",
280  )
281  )
282  ld.add_action(
284  "debug",
285  default_value=False,
286  description="By default, we are not in debug mode",
287  )
288  )
289  ld.add_action(DeclareBooleanLaunchArg("use_rviz", default_value=True))
290  # If there are virtual joints, broadcast static tf by including virtual_joints launch
291  virtual_joints_launch = (
292  launch_package_path / "launch/static_virtual_joint_tfs.launch.py"
293  )
294 
295  if virtual_joints_launch.exists():
296  ld.add_action(
297  IncludeLaunchDescription(
298  PythonLaunchDescriptionSource(str(virtual_joints_launch)),
299  )
300  )
301 
302  # Given the published joint states, publish tf for the robot links
303  ld.add_action(
304  IncludeLaunchDescription(
305  PythonLaunchDescriptionSource(
306  str(launch_package_path / "launch/rsp.launch.py")
307  ),
308  )
309  )
310 
311  ld.add_action(
312  IncludeLaunchDescription(
313  PythonLaunchDescriptionSource(
314  str(launch_package_path / "launch/move_group.launch.py")
315  ),
316  )
317  )
318 
319  # Run Rviz and load the default config to see the state of the move_group node
320  ld.add_action(
321  IncludeLaunchDescription(
322  PythonLaunchDescriptionSource(
323  str(launch_package_path / "launch/moveit_rviz.launch.py")
324  ),
325  condition=IfCondition(LaunchConfiguration("use_rviz")),
326  )
327  )
328 
329  # If database loading was enabled, start mongodb as well
330  ld.add_action(
331  IncludeLaunchDescription(
332  PythonLaunchDescriptionSource(
333  str(launch_package_path / "launch/warehouse_db.launch.py")
334  ),
335  condition=IfCondition(LaunchConfiguration("db")),
336  )
337  )
338 
339  # Fake joint driver
340  ld.add_action(
341  Node(
342  package="controller_manager",
343  executable="ros2_control_node",
344  parameters=[
345  moveit_config.robot_description,
346  str(moveit_config.package_path / "config/ros2_controllers.yaml"),
347  ],
348  )
349  )
350 
351  ld.add_action(
352  IncludeLaunchDescription(
353  PythonLaunchDescriptionSource(
354  str(launch_package_path / "launch/spawn_controllers.launch.py")
355  ),
356  )
357  )
358 
359  return ld
def add_debuggable_node(ld, package, executable, condition_name="debug", commands_file=None, extra_debug_args=None, **kwargs)
Definition: launch_utils.py:29
def generate_setup_assistant_launch(moveit_config)
Definition: launches.py:77
def generate_moveit_rviz_launch(moveit_config)
Definition: launches.py:47
def generate_demo_launch(moveit_config, launch_package_path=None)
Definition: launches.py:257
def generate_rsp_launch(moveit_config)
Definition: launches.py:23
def generate_spawn_controllers_launch(moveit_config)
Definition: launches.py:118
def generate_static_virtual_joint_tfs_launch(moveit_config)
Definition: launches.py:92
def generate_warehouse_db_launch(moveit_config)
Definition: launches.py:135
def generate_move_group_launch(moveit_config)
Definition: launches.py:189