diff --git a/moveit_configs_utils/moveit_configs_utils/launches.py b/moveit_configs_utils/moveit_configs_utils/launches.py index f362d3ab53..056b0110b1 100644 --- a/moveit_configs_utils/moveit_configs_utils/launches.py +++ b/moveit_configs_utils/moveit_configs_utils/launches.py @@ -1,6 +1,10 @@ from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument +from launch.actions import ( + DeclareLaunchArgument, + IncludeLaunchDescription, +) from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node @@ -60,7 +64,7 @@ def generate_moveit_rviz_launch(moveit_config): package="rviz2", executable="rviz2", output="log", - respawn=True, + respawn=False, arguments=["-d", LaunchConfiguration("rviz_config")], parameters=rviz_parameters, ) @@ -231,3 +235,103 @@ def generate_move_group_launch(moveit_config): additional_env={"DISPLAY": ":0"}, ) return ld + + +def generate_demo_launch(moveit_config): + """ + Launches a self contained demo + + Includes + * static_virtual_joint_tfs + * robot_state_publisher + * move_group + * moveit_rviz + * warehouse_db (optional) + * ros2_control_node + controller spawners + """ + ld = LaunchDescription() + ld.add_action( + DeclareBooleanLaunchArg( + "db", + default_value=False, + description="By default, we do not start a database (it can be large)", + ) + ) + ld.add_action( + DeclareBooleanLaunchArg( + "debug", + default_value=False, + description="By default, we are not in debug mode", + ) + ) + ld.add_action(DeclareBooleanLaunchArg("use_rviz", default_value=True)) + + # If there are virtual joints, broadcast static tf by including virtual_joints launch + virtual_joints_launch = ( + moveit_config.package_path / "launch/static_virtual_joint_tfs.launch.py" + ) + if virtual_joints_launch.exists(): + ld.add_action( + IncludeLaunchDescription( + PythonLaunchDescriptionSource(str(virtual_joints_launch)), + ) + ) + + # Given the published joint states, publish tf for the robot links + ld.add_action( + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + str(moveit_config.package_path / "launch/rsp.launch.py") + ), + ) + ) + + ld.add_action( + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + str(moveit_config.package_path / "launch/move_group.launch.py") + ), + ) + ) + + # Run Rviz and load the default config to see the state of the move_group node + ld.add_action( + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + str(moveit_config.package_path / "launch/moveit_rviz.launch.py") + ), + condition=IfCondition(LaunchConfiguration("use_rviz")), + ) + ) + + # If database loading was enabled, start mongodb as well + ld.add_action( + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + str(moveit_config.package_path / "launch/warehouse_db.launch.py") + ), + condition=IfCondition(LaunchConfiguration("db")), + ) + ) + + # Fake joint driver + ld.add_action( + Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[ + moveit_config.robot_description, + str(moveit_config.package_path / "config/ros2_controllers.yaml"), + ], + ) + ) + + ld.add_action( + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + str(moveit_config.package_path / "launch/spawn_controllers.launch.py") + ), + ) + ) + + return ld