3 from launch
import LaunchDescription
4 from launch.actions
import (
6 IncludeLaunchDescription,
8 from launch.conditions
import IfCondition
9 from launch.launch_description_sources
import PythonLaunchDescriptionSource
10 from launch.substitutions
import LaunchConfiguration
12 from launch_ros.actions
import Node
13 from launch_ros.parameter_descriptions
import ParameterValue
15 from srdfdom.srdf
import SRDF
19 DeclareBooleanLaunchArg,
24 """Launch file for robot state publisher (rsp)"""
26 ld = LaunchDescription()
27 ld.add_action(DeclareLaunchArgument(
"publish_frequency", default_value=
"15.0"))
31 package=
"robot_state_publisher",
32 executable=
"robot_state_publisher",
36 moveit_config.robot_description,
38 "publish_frequency": LaunchConfiguration(
"publish_frequency"),
42 ld.add_action(rsp_node)
48 """Launch file for rviz"""
49 ld = LaunchDescription()
53 DeclareLaunchArgument(
55 default_value=str(moveit_config.package_path /
"config/moveit.rviz"),
60 moveit_config.planning_pipelines,
61 moveit_config.robot_description_kinematics,
70 arguments=[
"-d", LaunchConfiguration(
"rviz_config")],
71 parameters=rviz_parameters,
78 """Launch file for MoveIt Setup Assistant"""
79 ld = LaunchDescription()
84 package=
"moveit_setup_assistant",
85 executable=
"moveit_setup_assistant",
86 arguments=[[
"--config_pkg=", str(moveit_config.package_path)]],
93 ld = LaunchDescription()
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:
103 executable=
"static_transform_publisher",
104 name=f
"static_transform_publisher{name_counter}",
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"]:
126 package=
"controller_manager",
127 executable=
"spawner",
128 arguments=[controller],
136 """Launch file for warehouse database"""
137 ld = LaunchDescription()
139 DeclareLaunchArgument(
140 "moveit_warehouse_database_path",
142 moveit_config.package_path /
"default_warehouse_mongo_db"
149 ld.add_action(DeclareLaunchArgument(
"moveit_warehouse_port", default_value=
"33829"))
153 DeclareLaunchArgument(
"moveit_warehouse_host", default_value=
"localhost")
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",
169 package=
"warehouse_ros_mongo",
170 executable=
"mongo_wrapper_ros.py",
173 parameters=db_parameters,
175 ld.add_action(db_node)
179 package=
"moveit_ros_warehouse",
180 executable=
"moveit_init_demo_warehouse",
182 condition=IfCondition(LaunchConfiguration(
"reset")),
184 ld.add_action(reset_node)
190 ld = LaunchDescription()
201 DeclareLaunchArgument(
203 default_value=moveit_config.move_group_capabilities[
"capabilities"],
208 DeclareLaunchArgument(
209 "disable_capabilities",
210 default_value=moveit_config.move_group_capabilities[
"disable_capabilities"],
218 should_publish = LaunchConfiguration(
"publish_monitored_planning_scene")
220 move_group_configuration = {
221 "publish_robot_description_semantic":
True,
222 "allow_trajectory_execution": LaunchConfiguration(
"allow_trajectory_execution"),
224 "capabilities": ParameterValue(
225 LaunchConfiguration(
"capabilities"), value_type=str
227 "disable_capabilities": ParameterValue(
228 LaunchConfiguration(
"disable_capabilities"), value_type=str
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,
238 move_group_params = [
239 moveit_config.to_dict(),
240 move_group_configuration,
245 package=
"moveit_ros_move_group",
246 executable=
"move_group",
247 commands_file=str(moveit_config.package_path /
"launch" /
"gdb_settings.gdb"),
249 parameters=move_group_params,
250 extra_debug_args=[
"--debug"],
252 additional_env={
"DISPLAY": os.environ[
"DISPLAY"]},
259 Launches a self contained demo
261 launch_package_path is optional to use different launch and config packages
264 * static_virtual_joint_tfs
265 * robot_state_publisher
268 * warehouse_db (optional)
269 * ros2_control_node + controller spawners
271 if launch_package_path ==
None:
272 launch_package_path = moveit_config.package_path
274 ld = LaunchDescription()
279 description=
"By default, we do not start a database (it can be large)",
286 description=
"By default, we are not in debug mode",
291 virtual_joints_launch = (
292 launch_package_path /
"launch/static_virtual_joint_tfs.launch.py"
295 if virtual_joints_launch.exists():
297 IncludeLaunchDescription(
298 PythonLaunchDescriptionSource(str(virtual_joints_launch)),
304 IncludeLaunchDescription(
305 PythonLaunchDescriptionSource(
306 str(launch_package_path /
"launch/rsp.launch.py")
312 IncludeLaunchDescription(
313 PythonLaunchDescriptionSource(
314 str(launch_package_path /
"launch/move_group.launch.py")
321 IncludeLaunchDescription(
322 PythonLaunchDescriptionSource(
323 str(launch_package_path /
"launch/moveit_rviz.launch.py")
325 condition=IfCondition(LaunchConfiguration(
"use_rviz")),
331 IncludeLaunchDescription(
332 PythonLaunchDescriptionSource(
333 str(launch_package_path /
"launch/warehouse_db.launch.py")
335 condition=IfCondition(LaunchConfiguration(
"db")),
342 package=
"controller_manager",
343 executable=
"ros2_control_node",
345 moveit_config.robot_description,
346 str(moveit_config.package_path /
"config/ros2_controllers.yaml"),
352 IncludeLaunchDescription(
353 PythonLaunchDescriptionSource(
354 str(launch_package_path /
"launch/spawn_controllers.launch.py")
def add_debuggable_node(ld, package, executable, condition_name="debug", commands_file=None, extra_debug_args=None, **kwargs)
def generate_setup_assistant_launch(moveit_config)
def generate_moveit_rviz_launch(moveit_config)
def generate_demo_launch(moveit_config, launch_package_path=None)
def generate_rsp_launch(moveit_config)
def generate_spawn_controllers_launch(moveit_config)
def generate_static_virtual_joint_tfs_launch(moveit_config)
def generate_warehouse_db_launch(moveit_config)
def generate_move_group_launch(moveit_config)