New Launch Files using moveit_configs_utils (#1113)

* Add Package Path
* Launch Utils
* Some Basic Launch Files
* Remove circular dependencies

moveit_configs_utils/moveit_configs_utils/launch_utils.py

@@ -0,0 +1,46 @@
+from launch.actions import DeclareLaunchArgument
+from launch.conditions import IfCondition, UnlessCondition
+from launch.substitutions import LaunchConfiguration
+
+from launch_ros.actions import Node
+
+
+class DeclareBooleanLaunchArg(DeclareLaunchArgument):
+    """This launch action declares a launch argument with true and false as the only valid values"""
+
+    def __init__(self, name, *, default_value=False, description=None, **kwargs):
+        super().__init__(
+            name,
+            default_value=str(default_value),
+            description=description,
+            choices=["true", "false", "True", "False"],
+            **kwargs,
+        )
+
+
+def add_debuggable_node(
+    ld, package, executable, condition_name="debug", commands_file=None, **kwargs
+):
+    """Adds two versions of a Node to the launch description, one with gdb debugging, controlled by a launch config"""
+    standard_node = Node(
+        package=package,
+        executable=executable,
+        condition=UnlessCondition(LaunchConfiguration(condition_name)),
+        **kwargs,
+    )
+    ld.add_action(standard_node)
+
+    if commands_file:
+        dash_x_arg = f"-x {commands_file} "
+    else:
+        dash_x_arg = ""
+    prefix = [f"gdb {dash_x_arg}--ex run --args"]
+
+    debug_node = Node(
+        package=package,
+        executable=executable,
+        condition=IfCondition(LaunchConfiguration(condition_name)),
+        prefix=prefix,
+        **kwargs,
+    )
+    ld.add_action(debug_node)

moveit_configs_utils/moveit_configs_utils/launches.py

@@ -0,0 +1,134 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration
+
+from launch_ros.actions import Node
+
+from moveit_configs_utils.launch_utils import (
+    add_debuggable_node,
+    DeclareBooleanLaunchArg,
+)
+
+
+def generate_rsp_launch(moveit_config):
+    """Launch file for robot state publisher (rsp)"""
+
+    ld = LaunchDescription()
+    ld.add_action(DeclareLaunchArgument("publish_frequency", default_value="15.0"))
+
+    # Given the published joint states, publish tf for the robot links and the robot description
+    rsp_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        respawn=True,
+        output="screen",
+        parameters=[
+            moveit_config.robot_description,
+            {
+                "publish_frequency": LaunchConfiguration("publish_frequency"),
+            },
+        ],
+    )
+    ld.add_action(rsp_node)
+
+    return ld
+
+
+def generate_rviz_launch(moveit_config):
+    """Launch file for rviz"""
+    ld = LaunchDescription()
+
+    ld.add_action(DeclareBooleanLaunchArg("debug", default_value=False))
+    ld.add_action(
+        DeclareLaunchArgument(
+            "rviz_config",
+            default_value=str(moveit_config.package_path / "launch/moveit.rviz"),
+        )
+    )
+
+    rviz_parameters = [
+        moveit_config.planning_pipelines,
+        moveit_config.robot_description_kinematics,
+    ]
+
+    add_debuggable_node(
+        ld,
+        package="rviz2",
+        executable="rviz2",
+        output="log",
+        respawn=True,
+        arguments=["-d", LaunchConfiguration("rviz_config")],
+        parameters=rviz_parameters,
+    )
+
+    return ld
+
+
+def generate_assistant_launch(moveit_config):
+    """Launch file for MoveIt Setup Assistant"""
+    ld = LaunchDescription()
+
+    ld.add_action(DeclareBooleanLaunchArg("debug", default_value=False))
+    add_debuggable_node(
+        ld,
+        package="moveit_setup_assistant",
+        executable="moveit_setup_assistant",
+        arguments=[["--config_pkg=", str(moveit_config.package_path)]],
+    )
+
+    return ld
+
+
+def generate_warehouse_launch(moveit_config):
+    """Launch file for warehouse database"""
+    ld = LaunchDescription()
+    ld.add_action(
+        DeclareLaunchArgument(
+            "moveit_warehouse_database_path",
+            default_value=str(
+                moveit_config.package_path / "default_warehouse_mongo_db"
+            ),
+        )
+    )
+    ld.add_action(DeclareBooleanLaunchArg("reset", default_value=False))
+
+    # The default DB port for moveit (not default MongoDB port to avoid potential conflicts)
+    ld.add_action(DeclareLaunchArgument("moveit_warehouse_port", default_value="33829"))
+
+    # The default DB host for moveit
+    ld.add_action(
+        DeclareLaunchArgument("moveit_warehouse_host", default_value="localhost")
+    )
+
+    # Load warehouse parameters
+    db_parameters = [
+        {
+            "overwrite": False,
+            "database_path": LaunchConfiguration("moveit_warehouse_database_path"),
+            "warehouse_port": LaunchConfiguration("moveit_warehouse_port"),
+            "warehouse_host": LaunchConfiguration("moveit_warehouse_host"),
+            "warehouse_exec": "mongod",
+            "warehouse_plugin": "warehouse_ros_mongo::MongoDatabaseConnection",
+        },
+    ]
+    # Run the DB server
+    db_node = Node(
+        package="warehouse_ros_mongo",
+        executable="mongo_wrapper_ros.py",
+        # TODO(dlu): Figure out if this needs to be run in a specific directory
+        # (ROS 1 version set cwd="ROS_HOME")
+        parameters=db_parameters,
+    )
+    ld.add_action(db_node)
+
+    # If we want to reset the database, run this node
+    reset_node = Node(
+        package="moveit_ros_warehouse",
+        executable="moveit_init_demo_warehouse",
+        output="screen",
+        condition=IfCondition(LaunchConfiguration("reset")),
+    )
+    ld.add_action(reset_node)
+
+    return ld

moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py

@@ -21,6 +21,7 @@
 Example:
     moveit_configs = MoveItConfigsBuilder("robot_name").to_moveit_configs()
     ...
+    moveit_configs.package_path
     moveit_configs.robot_description
     moveit_configs.robot_description_semantic
     moveit_configs.robot_description_kinematics
@@ -61,6 +62,7 @@ class MoveItConfigs(object):
     """Class containing MoveIt related parameters."""
 
     __slots__ = [
+        "__package_path",
         "__robot_description",
         "__robot_description_semantic",
         "__robot_description_kinematics",
@@ -74,6 +76,8 @@ class MoveItConfigs(object):
     ]
 
     def __init__(self):
+        # A pathlib Path to the moveit config package
+        self.package_path = None
         # A dictionary that has the contents of the URDF file.
         self.robot_description = {}
         # A dictionary that has the contents of the SRDF file.
@@ -95,6 +99,14 @@ def __init__(self):
         # A dictionary containing the cartesian limits for the Pilz planner.
         self.cartesian_limits = {}
 
+    @property
+    def package_path(self):
+        return self.__package_path
+
+    @package_path.setter
+    def package_path(self, value):
+        self.__package_path = value
+
     @property
     def robot_description(self):
         return self.__robot_description
@@ -205,6 +217,7 @@ class MoveItConfigsBuilder(ParameterBuilder):
     # Look-up for robot_name_moveit_config package
     def __init__(self, robot_name: str, robot_description="robot_description"):
         super().__init__(robot_name + "_moveit_config")
+        self.__moveit_configs.package_path = self._package_path
         self.__robot_name = robot_name
         setup_assistant_file = self._package_path / ".setup_assistant"
         if not setup_assistant_file.exists():

moveit_configs_utils/package.xml

@@ -7,9 +7,12 @@
   <maintainer email="moveit_releasers@googlegroups.com">MoveIt Release Team</maintainer>
   <license>BSD</license>
   <author email="jafar@picknik.ai">Jafar Abdi</author>
+  <author email="davidvlu@gmail.com">David V. Lu!!</author>
 
   <exec_depend>ament_index_python</exec_depend>
   <exec_depend>launch_param_builder</exec_depend>
+  <exec_depend>launch</exec_depend>
+  <exec_depend>launch_ros</exec_depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>