From a06554e72aeb19fd8d69d2b2033e3310b15e8fc1 Mon Sep 17 00:00:00 2001
From: "David V. Lu!!" <davidvlu@gmail.com>
Date: Thu, 21 Apr 2022 21:26:36 -0400
Subject: [PATCH] Demo launch for moveit_configs_utils (#1189)

* Demo launch for moveit_configs_utils

* Don't respawn rviz
---
 .../moveit_configs_utils/launches.py          | 108 +++++++++++++++++-
 1 file changed, 106 insertions(+), 2 deletions(-)

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