From 81ec447873f43268117389ccc3e911d96feff447 Mon Sep 17 00:00:00 2001 From: "David V. Lu!!" Date: Wed, 16 Mar 2022 15:25:13 -0400 Subject: [PATCH] New Launch Files using moveit_configs_utils (#1113) * Add Package Path * Launch Utils * Some Basic Launch Files * Remove circular dependencies --- .../moveit_configs_utils/launch_utils.py | 46 ++++++ .../moveit_configs_utils/launches.py | 134 ++++++++++++++++++ .../moveit_configs_builder.py | 13 ++ moveit_configs_utils/package.xml | 3 + 4 files changed, 196 insertions(+) create mode 100644 moveit_configs_utils/moveit_configs_utils/launch_utils.py create mode 100644 moveit_configs_utils/moveit_configs_utils/launches.py diff --git a/moveit_configs_utils/moveit_configs_utils/launch_utils.py b/moveit_configs_utils/moveit_configs_utils/launch_utils.py new file mode 100644 index 0000000000..0d44927787 --- /dev/null +++ b/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) diff --git a/moveit_configs_utils/moveit_configs_utils/launches.py b/moveit_configs_utils/moveit_configs_utils/launches.py new file mode 100644 index 0000000000..fd7854bde8 --- /dev/null +++ b/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 diff --git a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py index 96a2040618..a605bb35e2 100644 --- a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py +++ b/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(): diff --git a/moveit_configs_utils/package.xml b/moveit_configs_utils/package.xml index c26caf88a9..b2c8782bc9 100644 --- a/moveit_configs_utils/package.xml +++ b/moveit_configs_utils/package.xml @@ -7,9 +7,12 @@ MoveIt Release Team BSD Jafar Abdi + David V. Lu!! ament_index_python launch_param_builder + launch + launch_ros ament_lint_auto ament_lint_common