diff --git a/moveit2.repos b/moveit2.repos index 903e175fdb..b0bf360af7 100644 --- a/moveit2.repos +++ b/moveit2.repos @@ -7,3 +7,11 @@ repositories: type: git url: https://github.com/ros-planning/moveit_msgs version: 2.2.0 + moveit_resources: + type: git + url: https://github.com/JafarAbdi/moveit_resources + version: pr-moveit_configs_utils + launch_param_builder: + type: git + url: https://github.com/PickNikRobotics/launch_param_builder + version: main diff --git a/moveit_configs_utils/moveit_configs_utils/__init__.py b/moveit_configs_utils/moveit_configs_utils/__init__.py new file mode 100644 index 0000000000..7a94606837 --- /dev/null +++ b/moveit_configs_utils/moveit_configs_utils/__init__.py @@ -0,0 +1 @@ +from .moveit_configs_builder import MoveItConfigsBuilder, MoveItConfigs diff --git a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py new file mode 100644 index 0000000000..d5304a79de --- /dev/null +++ b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py @@ -0,0 +1,445 @@ +"""Simplify loading moveit config parameters. + +This module provides builder-pattern based class to simplify loading moveit related parameters found in +robot_moveit_config package generated by moveit setup assistant. + +By default it expects the following structure for the moveit configs package + +robot_name_moveit_config/ + .setup_assistant -> Used to retrieve information about the SRDF file and + the URDF file used when generating the package + config/ + kinematics.yaml -> IK solver's parameters + joint_limits.yaml -> Overriding position/velocity/acceleration limits from the URDF file + moveit_cpp.yaml -> MoveItCpp related parameters + *_planning.yaml -> planning pipelines parameters + cartesian_limits.yaml -> Pilz planner parameters + # TODO(JafarAbdi): Check to see if this is a good default value + robot_name_controllers.yaml -> trajectory execution manager's parameters + ... + +Example: + moveit_configs = MoveItConfigsBuilder("robot_name").to_moveit_configs() + ... + moveit_configs.robot_description + moveit_configs.robot_description_semantic + moveit_configs.robot_description_kinematics + moveit_configs.planning_pipelines + moveit_configs.trajectory_execution + moveit_configs.planning_scene_monitor + moveit_configs.move_group_capabilities + moveit_configs.joint_limits + moveit_configs.moveit_cpp + moveit_configs.cartesian_limits + # Or to get all the parameters as a dictionary + moveit_configs.to_dict() + +Each function in MoveItConfigsBuilder have a file_path as an argument which is used to override the default +path for the file + +Example: + moveit_configs = MoveItConfigsBuilder("robot_name") + # Relative to robot_name_moveit_configs + .robot_description_semantic(Path("my_config") / "my_file.srdf") + .to_moveit_configs() + # Or + moveit_configs = MoveItConfigsBuilder("robot_name") + # Absolute path to robot_name_moveit_config + .robot_description_semantic(Path.home() / "my_config" / "new_file.srdf") + .to_moveit_configs() +""" + +from pathlib import Path +from typing import Optional, List +import logging +from ament_index_python.packages import get_package_share_directory + +from launch_param_builder import ParameterBuilder, load_yaml, load_xacro + + +class MoveItConfigs(object): + """Class containing MoveIt related parameters.""" + + __slots__ = [ + "__robot_description", + "__robot_description_semantic", + "__robot_description_kinematics", + "__planning_pipelines", + "__trajectory_execution", + "__planning_scene_monitor", + "__move_group_capabilities", + "__joint_limits", + "__moveit_cpp", + "__cartesian_limits", + ] + + def __init__(self): + # A dictionary that has the contents of the URDF file. + self.robot_description = {} + # A dictionary that has the contents of the SRDF file. + self.robot_description_semantic = {} + # A dictionary IK solver specific parameters. + self.robot_description_kinematics = {} + # A dictionary that contains the planning pipelines parameters. + self.planning_pipelines = {} + # A dictionary contains parameters for trajectory execution & moveit controller managers. + self.trajectory_execution = {} + # A dictionary that have the planning scene monitor's parameters. + self.planning_scene_monitor = {} + # A dictionary containing move_group's non-default capabilities. + self.move_group_capabilities = {} + # A dictionary containing the overridden position/velocity/acceleration limits. + self.joint_limits = {} + # A dictionary containing MoveItCpp related parameters. + self.moveit_cpp = {} + # A dictionary containing the cartesian limits for the Pilz planner. + self.cartesian_limits = {} + + @property + def robot_description(self): + return self.__robot_description + + @robot_description.setter + def robot_description(self, value): + self.__robot_description = value + + @property + def robot_description_semantic(self): + return self.__robot_description_semantic + + @robot_description_semantic.setter + def robot_description_semantic(self, value): + self.__robot_description_semantic = value + + @property + def robot_description_kinematics(self): + return self.__robot_description_kinematics + + @robot_description_kinematics.setter + def robot_description_kinematics(self, value): + self.__robot_description_kinematics = value + + @property + def planning_pipelines(self): + return self.__planning_pipelines + + @planning_pipelines.setter + def planning_pipelines(self, value): + self.__planning_pipelines = value + + @property + def trajectory_execution(self): + return self.__trajectory_execution + + @trajectory_execution.setter + def trajectory_execution(self, value): + self.__trajectory_execution = value + + @property + def planning_scene_monitor(self): + return self.__planning_scene_monitor + + @planning_scene_monitor.setter + def planning_scene_monitor(self, value): + self.__planning_scene_monitor = value + + @property + def move_group_capabilities(self): + return self.__move_group_capabilities + + @move_group_capabilities.setter + def move_group_capabilities(self, value): + self.__move_group_capabilities = value + + @property + def joint_limits(self): + return self.__joint_limits + + @joint_limits.setter + def joint_limits(self, value): + self.__joint_limits = value + + @property + def moveit_cpp(self): + return self.__moveit_cpp + + @moveit_cpp.setter + def moveit_cpp(self, value): + self.__moveit_cpp = value + + @property + def cartesian_limits(self): + return self.__cartesian_limits + + @cartesian_limits.setter + def cartesian_limits(self, value): + self.__cartesian_limits = value + + def to_dict(self): + parameters = {} + parameters.update(self.robot_description) + parameters.update(self.robot_description_semantic) + parameters.update(self.robot_description_kinematics) + parameters.update(self.planning_pipelines) + parameters.update(self.trajectory_execution) + parameters.update(self.planning_scene_monitor) + parameters.update(self.joint_limits) + parameters.update(self.moveit_cpp) + parameters.update(self.cartesian_limits) + return parameters + + +class MoveItConfigsBuilder(ParameterBuilder): + __moveit_configs = MoveItConfigs() + __robot_name: str + __urdf_package: Path + # Relative path of the URDF file w.r.t. __urdf_package + __urdf_file_path: Path + # Relative path of the SRDF file w.r.t. robot_name_moveit_config + __srdf_file_path: Path + # String specify the parameter name that the robot description will be loaded to, it will also be used as a prefix + # for "_planning", "_semantic", and "_kinematics" + __robot_description: str + __config_dir_path = Path("config") + + # 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.__robot_name = robot_name + setup_assistant_file = self._package_path / ".setup_assistant" + if not setup_assistant_file.exists(): + logging.warning( + f"\x1b[33;21mPackage `{self._package_path}` doesn't have `.setup_assistant` file " + f"-- using config/{robot_name}.urdf and config/{robot_name}.srdf\x1b[0m" + ) + self.__urdf_package = self._package_path + self.__urdf_file_path = self.__config_dir_path / ( + self.__robot_name + ".urdf" + ) + self.__srdf_filename = self.__config_dir_path / ( + self.__robot_name + ".srdf" + ) + else: + setup_assistant_yaml = load_yaml(setup_assistant_file) + self.__urdf_package = Path( + get_package_share_directory( + setup_assistant_yaml["moveit_setup_assistant_config"]["URDF"][ + "package" + ] + ) + ) + self.__urdf_file_path = Path( + setup_assistant_yaml["moveit_setup_assistant_config"]["URDF"][ + "relative_path" + ] + ) + self.__srdf_filename = Path( + setup_assistant_yaml["moveit_setup_assistant_config"]["SRDF"][ + "relative_path" + ] + ) + self.__robot_description = robot_description + + def robot_description(self, file_path: Optional[str] = None, mappings: dict = None): + """Load robot description. + + :param file_path: Absolute or relative path to the URDF file (w.r.t. robot_name_moveit_config). + :param mappings: mappings to be passed when loading the xacro file. + :return: Instance of MoveItConfigsBuilder with robot_description loaded. + """ + if file_path is None: + robot_description_file_path = self.__urdf_package / self.__urdf_file_path + else: + robot_description_file_path = self._package_path / file_path + self.__moveit_configs.robot_description = { + self.__robot_description: load_xacro( + robot_description_file_path, mappings=mappings + ) + } + return self + + def robot_description_semantic( + self, file_path: Optional[str] = None, mappings: dict = None + ): + """Load semantic robot description. + + :param file_path: Absolute or relative path to the SRDF file (w.r.t. robot_name_moveit_config). + :param mappings: mappings to be passed when loading the xacro file. + :return: Instance of MoveItConfigsBuilder with robot_description_semantic loaded. + """ + self.__moveit_configs.robot_description_semantic = { + self.__robot_description + + "_semantic": load_xacro( + self._package_path / (file_path or self.__srdf_filename), + mappings=mappings, + ) + } + return self + + def robot_description_kinematics(self, file_path: Optional[str] = None): + """Load IK solver parameters. + + :param file_path: Absolute or relative path to the kinematics yaml file (w.r.t. robot_name_moveit_config). + :return: Instance of MoveItConfigsBuilder with robot_description_kinematics loaded. + """ + self.__moveit_configs.robot_description_kinematics = { + self.__robot_description + + "_kinematics": load_yaml( + self._package_path + / (file_path or self.__config_dir_path / "kinematics.yaml") + ) + } + return self + + def joint_limits(self, file_path: Optional[str] = None): + """Load joint limits overrides. + + :param file_path: Absolute or relative path to the joint limits yaml file (w.r.t. robot_name_moveit_config). + :return: Instance of MoveItConfigsBuilder with robot_description_planning loaded. + """ + self.__moveit_configs.joint_limits = { + self.__robot_description + + "_planning": load_yaml( + self._package_path + / (file_path or self.__config_dir_path / "joint_limits.yaml") + ) + } + return self + + def moveit_cpp(self, file_path: Optional[str] = None): + """Load MoveItCpp parameters. + + :param file_path: Absolute or relative path to the MoveItCpp yaml file (w.r.t. robot_name_moveit_config). + :return: Instance of MoveItConfigsBuilder with moveit_cpp loaded. + """ + self.__moveit_configs.moveit_cpp = load_yaml( + self._package_path + / (file_path or self.__config_dir_path / "moveit_cpp.yaml") + ) + return self + + def trajectory_execution( + self, + file_path: Optional[str] = None, + moveit_manage_controllers: bool = True, + ): + """Load trajectory execution and moveit controller managers' parameters + + :param file_path: Absolute or relative path to the controllers yaml file (w.r.t. robot_name_moveit_config). + :param moveit_manage_controllers: Whether trajectory execution manager is allowed to switch controllers' states. + :return: Instance of MoveItConfigsBuilder with trajectory_execution loaded. + """ + self.__moveit_configs.trajectory_execution = { + "moveit_manage_controllers": moveit_manage_controllers, + } + self.__moveit_configs.trajectory_execution.update( + load_yaml( + self._package_path + / ( + file_path + or self.__config_dir_path / f"{self.__robot_name}_controllers.yaml" + ) + ) + ) + return self + + def planning_scene_monitor( + self, + publish_planning_scene: bool = True, + publish_geometry_updates: bool = True, + publish_state_updates: bool = True, + publish_transforms_updates: bool = True, + ): + self.__moveit_configs.planning_scene_monitor = { + # TODO: Fix parameter namespace upstream -- see planning_scene_monitor.cpp:262 + # "planning_scene_monitor": { + "publish_planning_scene": publish_planning_scene, + "publish_geometry_updates": publish_geometry_updates, + "publish_state_updates": publish_state_updates, + "publish_transforms_updates": publish_transforms_updates, + # } + } + return self + + def planning_pipelines( + self, default_planning_pipeline: str = None, pipelines: List[str] = None + ): + """Load planning pipelines parameters. + + :param default_planning_pipeline: Name of the default planning pipeline. + :param pipelines: List of the planning pipelines to be loaded. + :return: Instance of MoveItConfigsBuilder with planning_pipelines loaded. + """ + if pipelines is None: + pipelines = ["ompl"] + default_planning_pipeline = pipelines[0] + if default_planning_pipeline not in pipelines: + raise RuntimeError( + f"default_planning_pipeline: `{default_planning_pipeline}` doesn't name any of the input pipelines " + f"`{','.join(pipelines)}`" + ) + self.__moveit_configs.planning_pipelines = { + "planning_pipelines": pipelines, + "default_planning_pipeline": default_planning_pipeline, + } + for pipeline in pipelines: + self.__moveit_configs.planning_pipelines[pipeline] = load_yaml( + self._package_path + / self.__config_dir_path + / (pipeline + "_planning.yaml") + ) + return self + + def cartesian_limits(self, file_path: Optional[str] = None): + """Load cartesian limits. + + :param file_path: Absolute or relative path to the cartesian limits file (w.r.t. robot_name_moveit_config). + :return: Instance of MoveItConfigsBuilder with cartesian_limits loaded. + """ + self.__moveit_configs.cartesian_limits = { + self.__robot_description + + "_planning": load_yaml( + self._package_path + / (file_path or self.__config_dir_path / "cartesian_limits.yaml") + ) + } + return self + + def to_moveit_configs(self): + """Get MoveIt configs from robot_name_moveit_config. + + :return: An MoveItConfigs instance with all parameters loaded. + """ + if not self.__moveit_configs.robot_description: + self.robot_description() + if not self.__moveit_configs.robot_description_semantic: + self.robot_description_semantic() + if not self.__moveit_configs.robot_description_kinematics: + self.robot_description_kinematics() + if not self.__moveit_configs.planning_pipelines: + self.planning_pipelines() + # TODO(JafarAbdi): Not sure if the default value for file_path makes sense + # if not self.__moveit_configs.trajectory_execution: + # self.trajectory_execution() + if not self.__moveit_configs.planning_scene_monitor: + self.planning_scene_monitor() + if not self.__moveit_configs.joint_limits: + self.joint_limits() + # TODO(JafarAbdi): We should have a default moveit_cpp.yaml as port of a moveit config package + # if not self.__moveit_configs.moveit_cpp: + # self.moveit_cpp() + if not self.__moveit_configs.cartesian_limits: + self.cartesian_limits() + return self.__moveit_configs + + def to_dict(self, include_moveit_configs: bool = True): + """Get loaded parameters from robot_name_moveit_config as a dictionary. + + :param include_moveit_configs: Whether to include the MoveIt config parameters or + only the ones from ParameterBuilder + :return: Dictionary with all parameters loaded. + """ + parameters = self._parameters + if include_moveit_configs: + parameters.update(self.to_moveit_configs().to_dict()) + return parameters diff --git a/moveit_configs_utils/package.xml b/moveit_configs_utils/package.xml new file mode 100644 index 0000000000..b1b7075397 --- /dev/null +++ b/moveit_configs_utils/package.xml @@ -0,0 +1,20 @@ + + + + moveit_configs_utils + 2.3.0 + Python library for loading moveit config parameters in launch files + Jafar Abdi + MoveIt Release Team + BSD + + ament_lint_auto + ament_lint_common + + ament_index_python + launch_param_builder + + + ament_python + + diff --git a/moveit_configs_utils/resource/moveit_configs_utils b/moveit_configs_utils/resource/moveit_configs_utils new file mode 100644 index 0000000000..e69de29bb2 diff --git a/moveit_configs_utils/setup.py b/moveit_configs_utils/setup.py new file mode 100644 index 0000000000..d1222ab2ac --- /dev/null +++ b/moveit_configs_utils/setup.py @@ -0,0 +1,23 @@ +from setuptools import setup + +package_name = "moveit_configs_utils" + +setup( + name=package_name, + version="2.3.0", + packages=[package_name], + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="MoveIt Release Team", + maintainer_email="moveit_releasers@googlegroups.com", + description="Python library for loading MoveIt config parameters in launch files", + license="BSD", + tests_require=["pytest"], + entry_points={ + "console_scripts": [], + }, +) diff --git a/moveit_kinematics/package.xml b/moveit_kinematics/package.xml index 9becc2579a..dff7eb2fc3 100644 --- a/moveit_kinematics/package.xml +++ b/moveit_kinematics/package.xml @@ -47,6 +47,8 @@ ament_lint_auto ament_lint_common ros_testing + moveit_configs_utils + launch_param_builder ament_cmake diff --git a/moveit_kinematics/test/launch/fanuc-kdl-singular.test.py b/moveit_kinematics/test/launch/fanuc-kdl-singular.test.py index e6be9f6262..aeb6769fcc 100644 --- a/moveit_kinematics/test/launch/fanuc-kdl-singular.test.py +++ b/moveit_kinematics/test/launch/fanuc-kdl-singular.test.py @@ -1,64 +1,29 @@ import launch_testing -import os import pytest import unittest -import yaml -from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch_ros.actions import Node from launch_testing.util import KeepAliveProc - - -def load_file(package_name, file_path): - package_path = get_package_share_directory(package_name) - absolute_file_path = os.path.join(package_path, file_path) - - try: - with open(absolute_file_path, "r") as file: - return file.read() - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available - return None - - -def load_yaml(package_name, file_path): - package_path = get_package_share_directory(package_name) - absolute_file_path = os.path.join(package_path, file_path) - - try: - with open(absolute_file_path, "r") as file: - return yaml.full_load(file) - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available - return None +from moveit_configs_utils import MoveItConfigsBuilder +from launch_param_builder import ParameterBuilder @pytest.mark.rostest def generate_test_description(): - robot_description_config = load_file( - "moveit_resources_fanuc_description", "urdf/fanuc.urdf" - ) - robot_description = {"robot_description": robot_description_config} - - robot_description_semantic_config = load_file( - "moveit_resources_fanuc_moveit_config", "config/fanuc.srdf" - ) - robot_description_semantic = { - "robot_description_semantic": robot_description_semantic_config - } - kinematics_yaml = load_yaml( - "moveit_resources_fanuc_moveit_config", "config/kinematics.yaml" + moveit_configs = MoveItConfigsBuilder("moveit_resources_fanuc").to_dict() + test_param = ( + ParameterBuilder("moveit_kinematics") + .yaml("config/fanuc-kdl-singular-test.yaml") + .to_dict() ) - robot_description_kinematics = {"robot_description_kinematics": kinematics_yaml} - test_param = load_yaml("moveit_kinematics", "config/fanuc-kdl-singular-test.yaml") fanuc_kdl_singular = Node( package="moveit_kinematics", executable="test_kinematics_plugin", name="fanuc_kdl_singular", parameters=[ - robot_description, - robot_description_semantic, - robot_description_kinematics, + moveit_configs, test_param, ], output="screen", diff --git a/moveit_kinematics/test/launch/fanuc-kdl.test.py b/moveit_kinematics/test/launch/fanuc-kdl.test.py index e87512f4a9..0cac80990c 100644 --- a/moveit_kinematics/test/launch/fanuc-kdl.test.py +++ b/moveit_kinematics/test/launch/fanuc-kdl.test.py @@ -1,71 +1,29 @@ import launch_testing -import os import pytest import unittest -import yaml -from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch_ros.actions import Node from launch_testing.util import KeepAliveProc - - -def load_file(package_name, file_path): - package_path = get_package_share_directory(package_name) - absolute_file_path = os.path.join(package_path, file_path) - - try: - with open(absolute_file_path, "r") as file: - return file.read() - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available - return None - - -def load_yaml(package_name, file_path): - package_path = get_package_share_directory(package_name) - absolute_file_path = os.path.join(package_path, file_path) - - try: - with open(absolute_file_path, "r") as file: - return yaml.full_load(file) - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available - return None +from moveit_configs_utils import MoveItConfigsBuilder +from launch_param_builder import ParameterBuilder @pytest.mark.rostest def generate_test_description(): - # Component yaml files are grouped in separate namespaces - robot_description_config = load_file( - "moveit_resources_fanuc_description", "urdf/fanuc.urdf" - ) - robot_description = {"robot_description": robot_description_config} - - robot_description_semantic_config = load_file( - "moveit_resources_fanuc_moveit_config", "config/fanuc.srdf" - ) - robot_description_semantic = { - "robot_description_semantic": robot_description_semantic_config - } - kinematics_yaml = load_yaml( - "moveit_resources_fanuc_moveit_config", "config/kinematics.yaml" + moveit_configs = MoveItConfigsBuilder("moveit_resources_fanuc").to_dict() + test_param = ( + ParameterBuilder("moveit_kinematics") + .yaml("config/fanuc-kdl-test.yaml") + .to_dict() ) - robot_description_kinematics = {"robot_description_kinematics": kinematics_yaml} - joint_limits_yaml = { - "robot_description_planning": load_yaml( - "moveit_resources_fanuc_moveit_config", "config/joint_limits.yaml" - ) - } - test_param = load_yaml("moveit_kinematics", "config/fanuc-kdl-test.yaml") fanuc_kdl = Node( package="moveit_kinematics", executable="test_kinematics_plugin", name="fanuc_kdl", parameters=[ - robot_description, - robot_description_semantic, - robot_description_kinematics, - joint_limits_yaml, + moveit_configs, test_param, ], output="screen", diff --git a/moveit_kinematics/test/launch/fanuc-lma-singular.test.py b/moveit_kinematics/test/launch/fanuc-lma-singular.test.py index d3f54967a2..1a190ab2e1 100644 --- a/moveit_kinematics/test/launch/fanuc-lma-singular.test.py +++ b/moveit_kinematics/test/launch/fanuc-lma-singular.test.py @@ -1,70 +1,29 @@ import launch_testing -import os import pytest import unittest -import yaml -from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch_ros.actions import Node from launch_testing.util import KeepAliveProc - - -def load_file(package_name, file_path): - package_path = get_package_share_directory(package_name) - absolute_file_path = os.path.join(package_path, file_path) - - try: - with open(absolute_file_path, "r") as file: - return file.read() - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available - return None - - -def load_yaml(package_name, file_path): - package_path = get_package_share_directory(package_name) - absolute_file_path = os.path.join(package_path, file_path) - - try: - with open(absolute_file_path, "r") as file: - return yaml.full_load(file) - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available - return None +from moveit_configs_utils import MoveItConfigsBuilder +from launch_param_builder import ParameterBuilder @pytest.mark.rostest def generate_test_description(): - robot_description_config = load_file( - "moveit_resources_fanuc_description", "urdf/fanuc.urdf" - ) - robot_description = {"robot_description": robot_description_config} - - robot_description_semantic_config = load_file( - "moveit_resources_fanuc_moveit_config", "config/fanuc.srdf" - ) - robot_description_semantic = { - "robot_description_semantic": robot_description_semantic_config - } - kinematics_yaml = load_yaml( - "moveit_resources_fanuc_moveit_config", "config/kinematics.yaml" + moveit_configs = MoveItConfigsBuilder("moveit_resources_fanuc").to_dict() + test_param = ( + ParameterBuilder("moveit_kinematics") + .yaml("config/fanuc-lma-singular-test.yaml") + .to_dict() ) - robot_description_kinematics = {"robot_description_kinematics": kinematics_yaml} - joint_limits_yaml = { - "robot_description_planning": load_yaml( - "moveit_resources_fanuc_moveit_config", "config/joint_limits.yaml" - ) - } - test_param = load_yaml("moveit_kinematics", "config/fanuc-lma-singular-test.yaml") fanuc_lma_singular = Node( package="moveit_kinematics", executable="test_kinematics_plugin", name="fanuc_lma_singular", parameters=[ - robot_description, - robot_description_semantic, - robot_description_kinematics, - joint_limits_yaml, + moveit_configs, test_param, ], output="screen", diff --git a/moveit_kinematics/test/launch/fanuc-lma.test.py b/moveit_kinematics/test/launch/fanuc-lma.test.py index 96021e69df..82a6169a66 100644 --- a/moveit_kinematics/test/launch/fanuc-lma.test.py +++ b/moveit_kinematics/test/launch/fanuc-lma.test.py @@ -1,71 +1,29 @@ import launch_testing -import os import pytest import unittest -import yaml -from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch_ros.actions import Node from launch_testing.util import KeepAliveProc - - -def load_file(package_name, file_path): - package_path = get_package_share_directory(package_name) - absolute_file_path = os.path.join(package_path, file_path) - - try: - with open(absolute_file_path, "r") as file: - return file.read() - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available - return None - - -def load_yaml(package_name, file_path): - package_path = get_package_share_directory(package_name) - absolute_file_path = os.path.join(package_path, file_path) - - try: - with open(absolute_file_path, "r") as file: - return yaml.full_load(file) - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available - return None +from moveit_configs_utils import MoveItConfigsBuilder +from launch_param_builder import ParameterBuilder @pytest.mark.rostest def generate_test_description(): - # Component yaml files are grouped in separate namespaces - robot_description_config = load_file( - "moveit_resources_fanuc_description", "urdf/fanuc.urdf" - ) - robot_description = {"robot_description": robot_description_config} - - robot_description_semantic_config = load_file( - "moveit_resources_fanuc_moveit_config", "config/fanuc.srdf" - ) - robot_description_semantic = { - "robot_description_semantic": robot_description_semantic_config - } - kinematics_yaml = load_yaml( - "moveit_resources_fanuc_moveit_config", "config/kinematics.yaml" + moveit_configs = MoveItConfigsBuilder("moveit_resources_fanuc").to_dict() + test_param = ( + ParameterBuilder("moveit_kinematics") + .yaml("config/fanuc-lma-test.yaml") + .to_dict() ) - robot_description_kinematics = {"robot_description_kinematics": kinematics_yaml} - joint_limits_yaml = { - "robot_description_planning": load_yaml( - "moveit_resources_fanuc_moveit_config", "config/joint_limits.yaml" - ) - } - test_param = load_yaml("moveit_kinematics", "config/fanuc-lma-test.yaml") fanuc_lma = Node( package="moveit_kinematics", executable="test_kinematics_plugin", name="fanuc_lma", parameters=[ - robot_description, - robot_description_semantic, - robot_description_kinematics, - joint_limits_yaml, + moveit_configs, test_param, ], output="screen", diff --git a/moveit_kinematics/test/launch/panda-kdl-singular.test.py b/moveit_kinematics/test/launch/panda-kdl-singular.test.py index feedf386c5..0e3e41caf6 100644 --- a/moveit_kinematics/test/launch/panda-kdl-singular.test.py +++ b/moveit_kinematics/test/launch/panda-kdl-singular.test.py @@ -1,71 +1,30 @@ import launch_testing -import os import pytest import unittest -import yaml -from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch_ros.actions import Node from launch_testing.util import KeepAliveProc - - -def load_file(package_name, file_path): - package_path = get_package_share_directory(package_name) - absolute_file_path = os.path.join(package_path, file_path) - - try: - with open(absolute_file_path, "r") as file: - return file.read() - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available - return None - - -def load_yaml(package_name, file_path): - package_path = get_package_share_directory(package_name) - absolute_file_path = os.path.join(package_path, file_path) - - try: - with open(absolute_file_path, "r") as file: - return yaml.full_load(file) - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available - return None +from moveit_configs_utils import MoveItConfigsBuilder +from launch_param_builder import ParameterBuilder @pytest.mark.rostest def generate_test_description(): - # Component yaml files are grouped in separate namespaces - robot_description_config = load_file( - "moveit_resources_panda_description", "urdf/panda.urdf" - ) - robot_description = {"robot_description": robot_description_config} + moveit_configs = MoveItConfigsBuilder("moveit_resources_panda").to_dict() - robot_description_semantic_config = load_file( - "moveit_resources_panda_moveit_config", "config/panda.srdf" - ) - robot_description_semantic = { - "robot_description_semantic": robot_description_semantic_config - } - kinematics_yaml = load_yaml( - "moveit_resources_panda_moveit_config", "config/kinematics.yaml" + test_param = ( + ParameterBuilder("moveit_kinematics") + .yaml("config/panda-kdl-singular-test.yaml") + .to_dict() ) - robot_description_kinematics = {"robot_description_kinematics": kinematics_yaml} - joint_limits_yaml = { - "robot_description_planning": load_yaml( - "moveit_resources_panda_moveit_config", "config/joint_limits.yaml" - ) - } - test_param = load_yaml("moveit_kinematics", "config/panda-kdl-singular-test.yaml") panda_kdl_singular = Node( package="moveit_kinematics", executable="test_kinematics_plugin", name="panda_kdl_singular", parameters=[ - robot_description, - robot_description_semantic, - robot_description_kinematics, - joint_limits_yaml, + moveit_configs, test_param, ], output="screen", diff --git a/moveit_kinematics/test/launch/panda-kdl.test.py b/moveit_kinematics/test/launch/panda-kdl.test.py index f1d2cc33f7..2fea5a2680 100644 --- a/moveit_kinematics/test/launch/panda-kdl.test.py +++ b/moveit_kinematics/test/launch/panda-kdl.test.py @@ -1,71 +1,29 @@ import launch_testing -import os import pytest import unittest -import yaml -from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch_ros.actions import Node from launch_testing.util import KeepAliveProc - - -def load_file(package_name, file_path): - package_path = get_package_share_directory(package_name) - absolute_file_path = os.path.join(package_path, file_path) - - try: - with open(absolute_file_path, "r") as file: - return file.read() - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available - return None - - -def load_yaml(package_name, file_path): - package_path = get_package_share_directory(package_name) - absolute_file_path = os.path.join(package_path, file_path) - - try: - with open(absolute_file_path, "r") as file: - return yaml.full_load(file) - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available - return None +from moveit_configs_utils import MoveItConfigsBuilder +from launch_param_builder import ParameterBuilder @pytest.mark.rostest def generate_test_description(): - # Component yaml files are grouped in separate namespaces - robot_description_config = load_file( - "moveit_resources_panda_description", "urdf/panda.urdf" - ) - robot_description = {"robot_description": robot_description_config} - - robot_description_semantic_config = load_file( - "moveit_resources_panda_moveit_config", "config/panda.srdf" - ) - robot_description_semantic = { - "robot_description_semantic": robot_description_semantic_config - } - kinematics_yaml = load_yaml( - "moveit_resources_panda_moveit_config", "config/kinematics.yaml" + moveit_configs = MoveItConfigsBuilder("moveit_resources_panda").to_dict() + test_param = ( + ParameterBuilder("moveit_kinematics") + .yaml("config/panda-kdl-test.yaml") + .to_dict() ) - robot_description_kinematics = {"robot_description_kinematics": kinematics_yaml} - joint_limits_yaml = { - "robot_description_planning": load_yaml( - "moveit_resources_panda_moveit_config", "config/joint_limits.yaml" - ) - } - test_param = load_yaml("moveit_kinematics", "config/panda-kdl-test.yaml") panda_kdl = Node( package="moveit_kinematics", executable="test_kinematics_plugin", name="panda_kdl", parameters=[ - robot_description, - robot_description_semantic, - robot_description_kinematics, - joint_limits_yaml, + moveit_configs, test_param, ], output="screen", diff --git a/moveit_kinematics/test/launch/panda-lma-singular.test.py b/moveit_kinematics/test/launch/panda-lma-singular.test.py index 5dc711b4bb..1d69248260 100644 --- a/moveit_kinematics/test/launch/panda-lma-singular.test.py +++ b/moveit_kinematics/test/launch/panda-lma-singular.test.py @@ -1,70 +1,28 @@ import launch_testing -import os import pytest import unittest -import yaml -from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch_ros.actions import Node from launch_testing.util import KeepAliveProc - - -def load_file(package_name, file_path): - package_path = get_package_share_directory(package_name) - absolute_file_path = os.path.join(package_path, file_path) - - try: - with open(absolute_file_path, "r") as file: - return file.read() - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available - return None - - -def load_yaml(package_name, file_path): - package_path = get_package_share_directory(package_name) - absolute_file_path = os.path.join(package_path, file_path) - - try: - with open(absolute_file_path, "r") as file: - return yaml.full_load(file) - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available - return None +from moveit_configs_utils import MoveItConfigsBuilder +from launch_param_builder import ParameterBuilder @pytest.mark.rostest def generate_test_description(): - # Component yaml files are grouped in separate namespaces - robot_description_config = load_file( - "moveit_resources_panda_description", "urdf/panda.urdf" - ) - robot_description = {"robot_description": robot_description_config} - - robot_description_semantic_config = load_file( - "moveit_resources_panda_moveit_config", "config/panda.srdf" - ) - robot_description_semantic = { - "robot_description_semantic": robot_description_semantic_config - } - kinematics_yaml = load_yaml( - "moveit_resources_panda_moveit_config", "config/kinematics.yaml" + moveit_configs = MoveItConfigsBuilder("moveit_resources_panda").to_dict() + test_param = ( + ParameterBuilder("moveit_kinematics") + .yaml("config/panda-lma-singular-test.yaml") + .to_dict() ) - robot_description_kinematics = {"robot_description_kinematics": kinematics_yaml} - joint_limits_yaml = { - "robot_description_planning": load_yaml( - "moveit_resources_panda_moveit_config", "config/joint_limits.yaml" - ) - } - test_param = load_yaml("moveit_kinematics", "config/panda-lma-singular-test.yaml") panda_lma_singular = Node( package="moveit_kinematics", executable="test_kinematics_plugin", name="panda_lma_singular", parameters=[ - robot_description, - robot_description_semantic, - robot_description_kinematics, - joint_limits_yaml, + moveit_configs, test_param, ], output="screen", diff --git a/moveit_kinematics/test/launch/panda-lma.test.py b/moveit_kinematics/test/launch/panda-lma.test.py index f00698e340..325674f90f 100644 --- a/moveit_kinematics/test/launch/panda-lma.test.py +++ b/moveit_kinematics/test/launch/panda-lma.test.py @@ -1,71 +1,29 @@ import launch_testing -import os import pytest import unittest -import yaml -from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch_ros.actions import Node from launch_testing.util import KeepAliveProc - - -def load_file(package_name, file_path): - package_path = get_package_share_directory(package_name) - absolute_file_path = os.path.join(package_path, file_path) - - try: - with open(absolute_file_path, "r") as file: - return file.read() - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available - return None - - -def load_yaml(package_name, file_path): - package_path = get_package_share_directory(package_name) - absolute_file_path = os.path.join(package_path, file_path) - - try: - with open(absolute_file_path, "r") as file: - return yaml.full_load(file) - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available - return None +from moveit_configs_utils import MoveItConfigsBuilder +from launch_param_builder import ParameterBuilder @pytest.mark.rostest def generate_test_description(): - # Component yaml files are grouped in separate namespaces - robot_description_config = load_file( - "moveit_resources_panda_description", "urdf/panda.urdf" - ) - robot_description = {"robot_description": robot_description_config} - - robot_description_semantic_config = load_file( - "moveit_resources_panda_moveit_config", "config/panda.srdf" - ) - robot_description_semantic = { - "robot_description_semantic": robot_description_semantic_config - } - kinematics_yaml = load_yaml( - "moveit_resources_panda_moveit_config", "config/kinematics.yaml" + moveit_configs = MoveItConfigsBuilder("moveit_resources_panda").to_dict() + test_param = ( + ParameterBuilder("moveit_kinematics") + .yaml("config/panda-lma-test.yaml") + .to_dict() ) - robot_description_kinematics = {"robot_description_kinematics": kinematics_yaml} - joint_limits_yaml = { - "robot_description_planning": load_yaml( - "moveit_resources_panda_moveit_config", "config/joint_limits.yaml" - ) - } - test_param = load_yaml("moveit_kinematics", "config/panda-lma-test.yaml") panda_lma = Node( package="moveit_kinematics", executable="test_kinematics_plugin", name="panda_lma", parameters=[ - robot_description, - robot_description_semantic, - robot_description_kinematics, - joint_limits_yaml, + moveit_configs, test_param, ], output="screen", diff --git a/moveit_planners/pilz_industrial_motion_planner/package.xml b/moveit_planners/pilz_industrial_motion_planner/package.xml index a6c3d74cb3..01ea0730d3 100644 --- a/moveit_planners/pilz_industrial_motion_planner/package.xml +++ b/moveit_planners/pilz_industrial_motion_planner/package.xml @@ -45,7 +45,7 @@ moveit_resources_prbt_pg70_support boost diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/common_parameters.py b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/common_parameters.py index e983c856f1..638babdf39 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/common_parameters.py +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/common_parameters.py @@ -5,7 +5,7 @@ # TODO(henningkayser): Switch to ParameterBuilder once #591 is merged # from moveit_configs_utils import MoveItConfigsBuilder -# from parameter_builder import ParameterBuilder +# from launch_param_builder import ParameterBuilder def _load_file(package_name, file_path): diff --git a/moveit_ros/benchmarks/examples/demo_panda.launch.py b/moveit_ros/benchmarks/examples/demo_panda.launch.py index a3feae79f5..ceb13704c9 100644 --- a/moveit_ros/benchmarks/examples/demo_panda.launch.py +++ b/moveit_ros/benchmarks/examples/demo_panda.launch.py @@ -1,72 +1,21 @@ -import os -import yaml from launch import LaunchDescription from launch_ros.actions import Node -from ament_index_python.packages import get_package_share_directory - - -def load_file(package_name, file_path): - package_path = get_package_share_directory(package_name) - absolute_file_path = os.path.join(package_path, file_path) - - try: - with open(absolute_file_path, "r") as file: - return file.read() - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available - return None - - -def load_yaml(package_name, file_path): - package_path = get_package_share_directory(package_name) - absolute_file_path = os.path.join(package_path, file_path) - - try: - with open(absolute_file_path, "r") as file: - return yaml.safe_load(file) - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available - return None +from moveit_configs_utils import MoveItConfigsBuilder +from launch_param_builder import ParameterBuilder def generate_launch_description(): - moveit_ros_benchmarks_yaml = load_yaml("moveit_ros_benchmarks", "demo1.yaml") - moveit_ros_benchmarks_config = {"benchmark_config": moveit_ros_benchmarks_yaml} - - # Component yaml files are grouped in separate namespaces - robot_description_config = load_file( - "moveit_resources_panda_description", "urdf/panda.urdf" - ) - robot_description = {"robot_description": robot_description_config} - - robot_description_semantic_config = load_file( - "moveit_resources_panda_moveit_config", "config/panda.srdf" - ) - robot_description_semantic = { - "robot_description_semantic": robot_description_semantic_config - } - - kinematics_yaml = load_yaml( - "moveit_resources_panda_moveit_config", "config/kinematics.yaml" - ) - robot_description_kinematics = {"robot_description_kinematics": kinematics_yaml} - - ompl_planning_pipeline_config = { - "ompl": { - "planning_plugin": "ompl_interface/OMPLPlanner", - "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""", - "start_state_max_bounds_error": 0.1, - } - } - ompl_planning_yaml = load_yaml( - "moveit_resources_panda_moveit_config", "config/ompl_planning.yaml" + moveit_ros_benchmarks_config = ( + ParameterBuilder("moveit_ros_benchmarks") + .yaml( + parameter_namespace="benchmark_config", + file_path="demo1.yaml", + ) + .to_dict() ) - ompl_planning_pipeline_config["ompl"].update(ompl_planning_yaml) - joint_limits_yaml = { - "robot_description_planning": load_yaml( - "moveit_resources_panda_moveit_config", "config/joint_limits.yaml" - ) - } + moveit_configs = MoveItConfigsBuilder("moveit_resources_panda").to_dict() # moveit_ros_benchmark demo executable moveit_ros_benchmarks_node = Node( @@ -77,11 +26,7 @@ def generate_launch_description(): output="screen", parameters=[ moveit_ros_benchmarks_config, - robot_description, - robot_description_semantic, - robot_description_kinematics, - ompl_planning_pipeline_config, - joint_limits_yaml, + moveit_configs, ], ) diff --git a/moveit_ros/benchmarks/examples/demo_panda_predefined_poses.launch.py b/moveit_ros/benchmarks/examples/demo_panda_predefined_poses.launch.py index 0ed85e2134..32a18c7d20 100644 --- a/moveit_ros/benchmarks/examples/demo_panda_predefined_poses.launch.py +++ b/moveit_ros/benchmarks/examples/demo_panda_predefined_poses.launch.py @@ -1,74 +1,21 @@ -import os -import yaml from launch import LaunchDescription from launch_ros.actions import Node -from ament_index_python.packages import get_package_share_directory - - -def load_file(package_name, file_path): - package_path = get_package_share_directory(package_name) - absolute_file_path = os.path.join(package_path, file_path) - - try: - with open(absolute_file_path, "r") as file: - return file.read() - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available - return None - - -def load_yaml(package_name, file_path): - package_path = get_package_share_directory(package_name) - absolute_file_path = os.path.join(package_path, file_path) - - try: - with open(absolute_file_path, "r") as file: - return yaml.load(file) - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available - return None +from moveit_configs_utils import MoveItConfigsBuilder +from launch_param_builder import ParameterBuilder def generate_launch_description(): - moveit_ros_benchmarks_yaml = load_yaml( - "moveit_ros_benchmarks", "demo_panda_predefined_poses.yaml" - ) - moveit_ros_benchmarks_config = {"benchmark_config": moveit_ros_benchmarks_yaml} - - # Component yaml files are grouped in separate namespaces - robot_description_config = load_file( - "moveit_resources_panda_description", "urdf/panda.urdf" - ) - robot_description = {"robot_description": robot_description_config} - - robot_description_semantic_config = load_file( - "moveit_resources_panda_moveit_config", "config/panda.srdf" - ) - robot_description_semantic = { - "robot_description_semantic": robot_description_semantic_config - } - - kinematics_yaml = load_yaml( - "moveit_resources_panda_moveit_config", "config/kinematics.yaml" - ) - robot_description_kinematics = {"robot_description_kinematics": kinematics_yaml} - - ompl_planning_pipeline_config = { - "ompl": { - "planning_plugin": "ompl_interface/OMPLPlanner", - "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""", - "start_state_max_bounds_error": 0.1, - } - } - ompl_planning_yaml = load_yaml( - "moveit_resources_panda_moveit_config", "config/ompl_planning.yaml" + moveit_ros_benchmarks_config = ( + ParameterBuilder("moveit_ros_benchmarks") + .yaml( + parameter_namespace="benchmark_config", + file_path="demo_panda_predefined_poses.yaml", + ) + .to_dict() ) - ompl_planning_pipeline_config["ompl"].update(ompl_planning_yaml) - joint_limits_yaml = { - "robot_description_planning": load_yaml( - "moveit_resources_panda_moveit_config", "config/joint_limits.yaml" - ) - } + moveit_configs = MoveItConfigsBuilder("moveit_resources_panda").to_dict() # moveit_ros_benchmark demo executable moveit_ros_benchmarks_node = Node( @@ -79,11 +26,7 @@ def generate_launch_description(): output="screen", parameters=[ moveit_ros_benchmarks_config, - robot_description, - robot_description_semantic, - robot_description_kinematics, - ompl_planning_pipeline_config, - joint_limits_yaml, + moveit_configs, ], ) diff --git a/moveit_ros/benchmarks/package.xml b/moveit_ros/benchmarks/package.xml index b06822ba9f..cffcc2b7fa 100644 --- a/moveit_ros/benchmarks/package.xml +++ b/moveit_ros/benchmarks/package.xml @@ -31,6 +31,8 @@ libboost-date-time libboost-filesystem + moveit_configs_utils + launch_param_builder ament_lint_auto ament_lint_common diff --git a/moveit_ros/moveit_servo/launch/pose_tracking_example.launch.py b/moveit_ros/moveit_servo/launch/pose_tracking_example.launch.py index fc7c1676d9..735608c89f 100644 --- a/moveit_ros/moveit_servo/launch/pose_tracking_example.launch.py +++ b/moveit_ros/moveit_servo/launch/pose_tracking_example.launch.py @@ -1,69 +1,25 @@ import os -import yaml from launch import LaunchDescription from launch.actions import ExecuteProcess from launch_ros.actions import Node from ament_index_python.packages import get_package_share_directory -import xacro - - -def load_file(package_name, file_path): - package_path = get_package_share_directory(package_name) - absolute_file_path = os.path.join(package_path, file_path) - - try: - with open(absolute_file_path, "r") as file: - return file.read() - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available - return None - - -def load_yaml(package_name, file_path): - package_path = get_package_share_directory(package_name) - absolute_file_path = os.path.join(package_path, file_path) - - try: - with open(absolute_file_path, "r") as file: - return yaml.safe_load(file) - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available - return None +from moveit_configs_utils import MoveItConfigsBuilder +from launch_param_builder import ParameterBuilder def generate_launch_description(): - # Get URDF and SRDF - robot_description_config = xacro.process_file( - os.path.join( - get_package_share_directory("moveit_resources_panda_moveit_config"), - "config", - "panda.urdf.xacro", - ) + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .to_moveit_configs() ) - robot_description = {"robot_description": robot_description_config.toxml()} - - robot_description_semantic_config = load_file( - "moveit_resources_panda_moveit_config", "config/panda.srdf" - ) - robot_description_semantic = { - "robot_description_semantic": robot_description_semantic_config - } # Get parameters for the Pose Tracking node - pose_tracking_yaml = load_yaml("moveit_servo", "config/pose_tracking_settings.yaml") - pose_tracking_params = {"moveit_servo": pose_tracking_yaml} - - # Get parameters for the Servo node - servo_yaml = load_yaml( - "moveit_servo", "config/panda_simulated_config_pose_tracking.yaml" - ) - servo_params = {"moveit_servo": servo_yaml} - - kinematics_yaml = load_yaml( - "moveit_resources_panda_moveit_config", "config/kinematics.yaml" - ) - joint_limits_yaml = { - "robot_description_planning": load_yaml( - "moveit_resources_panda_moveit_config", "config/joint_limits.yaml" - ) + servo_params = { + "moveit_servo": ParameterBuilder("moveit_servo") + .yaml("config/pose_tracking_settings.yaml") + .yaml("config/panda_simulated_config_pose_tracking.yaml") + .to_dict() } # RViz @@ -78,12 +34,7 @@ def generate_launch_description(): # prefix=['xterm -e gdb -ex run --args'], output="log", arguments=["-d", rviz_config_file], - parameters=[ - robot_description, - robot_description_semantic, - kinematics_yaml, - joint_limits_yaml, - ], + parameters=[moveit_config.to_dict()], ) # Publishes tf's for the robot @@ -91,7 +42,7 @@ def generate_launch_description(): package="robot_state_publisher", executable="robot_state_publisher", output="screen", - parameters=[robot_description], + parameters=[moveit_config.robot_description], ) # A node to publish world -> panda_link0 transform @@ -109,12 +60,8 @@ def generate_launch_description(): # prefix=['xterm -e gdb -ex run --args'], output="screen", parameters=[ - robot_description, - robot_description_semantic, - kinematics_yaml, - pose_tracking_params, + moveit_config.to_dict(), servo_params, - joint_limits_yaml, ], ) @@ -127,7 +74,7 @@ def generate_launch_description(): ros2_control_node = Node( package="controller_manager", executable="ros2_control_node", - parameters=[robot_description, ros2_controllers_path], + parameters=[moveit_config.robot_description, ros2_controllers_path], output={ "stdout": "screen", "stderr": "screen", diff --git a/moveit_ros/moveit_servo/package.xml b/moveit_ros/moveit_servo/package.xml index 20a39e3c37..8207919666 100644 --- a/moveit_ros/moveit_servo/package.xml +++ b/moveit_ros/moveit_servo/package.xml @@ -42,6 +42,8 @@ joy robot_state_publisher tf2_ros + moveit_configs_utils + launch_param_builder ament_cmake_gtest ament_lint_auto diff --git a/moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py b/moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py index 28cb13baf0..a4a819ecdc 100644 --- a/moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py +++ b/moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py @@ -1,5 +1,4 @@ import os -import yaml import launch import launch_ros import launch_testing @@ -10,29 +9,8 @@ from launch_ros.descriptions import ComposableNode from ament_index_python.packages import get_package_share_directory from launch.actions import ExecuteProcess, TimerAction -import xacro - - -def load_file(package_name, file_path): - package_path = get_package_share_directory(package_name) - absolute_file_path = os.path.join(package_path, file_path) - - try: - with open(absolute_file_path, "r") as file: - return file.read() - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available - return None - - -def load_yaml(package_name, file_path): - package_path = get_package_share_directory(package_name) - absolute_file_path = os.path.join(package_path, file_path) - - try: - with open(absolute_file_path, "r") as file: - return yaml.safe_load(file) - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available - return None +from moveit_configs_utils import MoveItConfigsBuilder +from launch_param_builder import ParameterBuilder def generate_servo_test_description( @@ -42,44 +20,28 @@ def generate_servo_test_description( ): # Get parameters using the demo config file - servo_yaml = load_yaml("moveit_servo", "config/panda_simulated_config.yaml") - servo_params = {"moveit_servo": servo_yaml} + servo_params = { + "moveit_servo": ParameterBuilder("moveit_servo") + .yaml("config/panda_simulated_config.yaml") + .to_dict() + } # Get URDF and SRDF if start_position_path: initial_positions_file = os.path.join( os.path.dirname(__file__), start_position_path ) - robot_description_config = xacro.process_file( - os.path.join( - get_package_share_directory("moveit_resources_panda_moveit_config"), - "config", - "panda.urdf.xacro", - ), - mappings={"initial_positions_file": initial_positions_file}, - ) + robot_description_mappings = {"initial_positions_file": initial_positions_file} else: - robot_description_config = xacro.process_file( - os.path.join( - get_package_share_directory("moveit_resources_panda_moveit_config"), - "config", - "panda.urdf.xacro", - ) - ) - - robot_description = {"robot_description": robot_description_config.toxml()} + robot_description_mappings = None - robot_description_semantic_config = load_file( - "moveit_resources_panda_moveit_config", "config/panda.srdf" - ) - robot_description_semantic = { - "robot_description_semantic": robot_description_semantic_config - } - joint_limits_yaml = { - "robot_description_planning": load_yaml( - "moveit_resources_panda_moveit_config", "config/joint_limits.yaml" + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description( + file_path="config/panda.urdf.xacro", mappings=robot_description_mappings ) - } + .to_moveit_configs() + ) # ros2_control using FakeSystem as hardware ros2_controllers_path = os.path.join( @@ -90,7 +52,7 @@ def generate_servo_test_description( ros2_control_node = Node( package="controller_manager", executable="ros2_control_node", - parameters=[robot_description, ros2_controllers_path], + parameters=[moveit_config.robot_description, ros2_controllers_path], output={ "stdout": "screen", "stderr": "screen", @@ -119,7 +81,7 @@ def generate_servo_test_description( package="robot_state_publisher", plugin="robot_state_publisher::RobotStatePublisher", name="robot_state_publisher", - parameters=[robot_description], + parameters=[moveit_config.robot_description], ), ComposableNode( package="tf2_ros", @@ -137,9 +99,9 @@ def generate_servo_test_description( output="screen", parameters=[ servo_params, - robot_description, - robot_description_semantic, - joint_limits_yaml, + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.joint_limits, ], ) diff --git a/moveit_ros/moveit_servo/test/launch/test_servo_pose_tracking.test.py b/moveit_ros/moveit_servo/test/launch/test_servo_pose_tracking.test.py index a564a3038e..62c09b163c 100644 --- a/moveit_ros/moveit_servo/test/launch/test_servo_pose_tracking.test.py +++ b/moveit_ros/moveit_servo/test/launch/test_servo_pose_tracking.test.py @@ -5,77 +5,31 @@ import launch_testing.actions import launch_testing.asserts import unittest -import xacro -import yaml from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription from launch_ros.actions import Node, ComposableNodeContainer from launch_ros.descriptions import ComposableNode from launch.actions import ExecuteProcess, TimerAction from launch.some_substitutions_type import SomeSubstitutionsType from launch.substitutions import LaunchConfiguration, PathJoinSubstitution - - -def load_file(package_name, file_path): - package_path = get_package_share_directory(package_name) - absolute_file_path = os.path.join(package_path, file_path) - - try: - with open(absolute_file_path, "r") as file: - return file.read() - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available - return None - - -def load_yaml(package_name, file_path): - package_path = get_package_share_directory(package_name) - absolute_file_path = os.path.join(package_path, file_path) - - try: - with open(absolute_file_path, "r") as file: - return yaml.safe_load(file) - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available - return None +from moveit_configs_utils import MoveItConfigsBuilder +from launch_param_builder import ParameterBuilder def generate_servo_test_description(*args, gtest_name: SomeSubstitutionsType): - - # Get URDF and SRDF - robot_description_config = xacro.process_file( - os.path.join( - get_package_share_directory("moveit_resources_panda_moveit_config"), - "config", - "panda.urdf.xacro", - ) + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .to_moveit_configs() ) - robot_description = {"robot_description": robot_description_config.toxml()} - robot_description_semantic_config = load_file( - "moveit_resources_panda_moveit_config", "config/panda.srdf" - ) - robot_description_semantic = { - "robot_description_semantic": robot_description_semantic_config - } - joint_limits_yaml = { - "robot_description_planning": load_yaml( - "moveit_resources_panda_moveit_config", "config/joint_limits.yaml" - ) + # Get parameters for the Pose Tracking and Servo nodes + servo_params = { + "moveit_servo": ParameterBuilder("moveit_servo") + .yaml("config/pose_tracking_settings.yaml") + .yaml("config/panda_simulated_config_pose_tracking.yaml") + .to_dict() } - # Get parameters for the Pose Tracking node - pose_tracking_yaml = load_yaml("moveit_servo", "config/pose_tracking_settings.yaml") - pose_tracking_params = {"moveit_servo": pose_tracking_yaml} - - # Get parameters for the Servo node - servo_yaml = load_yaml( - "moveit_servo", "config/panda_simulated_config_pose_tracking.yaml" - ) - servo_params = {"moveit_servo": servo_yaml} - - kinematics_yaml = load_yaml( - "moveit_resources_panda_moveit_config", "config/kinematics.yaml" - ) - # ros2_control using FakeSystem as hardware ros2_controllers_path = os.path.join( get_package_share_directory("moveit_resources_panda_moveit_config"), @@ -85,7 +39,7 @@ def generate_servo_test_description(*args, gtest_name: SomeSubstitutionsType): ros2_control_node = Node( package="controller_manager", executable="ros2_control_node", - parameters=[robot_description, ros2_controllers_path], + parameters=[moveit_config.robot_description, ros2_controllers_path], output={ "stdout": "screen", "stderr": "screen", @@ -114,7 +68,7 @@ def generate_servo_test_description(*args, gtest_name: SomeSubstitutionsType): package="robot_state_publisher", plugin="robot_state_publisher::RobotStatePublisher", name="robot_state_publisher", - parameters=[robot_description], + parameters=[moveit_config.robot_description], ), ComposableNode( package="tf2_ros", @@ -131,12 +85,8 @@ def generate_servo_test_description(*args, gtest_name: SomeSubstitutionsType): [LaunchConfiguration("test_binary_dir"), gtest_name] ), parameters=[ - robot_description, - robot_description_semantic, - pose_tracking_params, + moveit_config.to_dict(), servo_params, - kinematics_yaml, - joint_limits_yaml, ], output="screen", ) diff --git a/moveit_ros/moveit_servo/test/launch/unit_test_servo_calcs.test.py b/moveit_ros/moveit_servo/test/launch/unit_test_servo_calcs.test.py index ca94473267..b2a2e5a394 100644 --- a/moveit_ros/moveit_servo/test/launch/unit_test_servo_calcs.test.py +++ b/moveit_ros/moveit_servo/test/launch/unit_test_servo_calcs.test.py @@ -4,15 +4,18 @@ import os import sys import unittest +from launch_param_builder import ParameterBuilder sys.path.append(os.path.dirname(__file__)) -from servo_launch_test_common import load_yaml def generate_test_description(): # Get parameters using the demo config file - servo_yaml = load_yaml("moveit_servo", "config/panda_simulated_config.yaml") - servo_params = {"moveit_servo": servo_yaml} + servo_params = ( + ParameterBuilder("moveit_servo") + .yaml("config/panda_simulated_config.yaml", parameter_namespace="moveit_servo") + .to_dict() + ) test_binary_dir_arg = launch.actions.DeclareLaunchArgument( name="test_binary_dir", diff --git a/moveit_ros/planning_interface/package.xml b/moveit_ros/planning_interface/package.xml index b7e1170762..ceec9ef1f0 100644 --- a/moveit_ros/planning_interface/package.xml +++ b/moveit_ros/planning_interface/package.xml @@ -49,6 +49,7 @@ ros_testing ament_cmake_gtest ament_lint_auto + moveit_configs_utils ament_cmake diff --git a/moveit_ros/planning_interface/test/launch/move_group_launch_test_common.py b/moveit_ros/planning_interface/test/launch/move_group_launch_test_common.py index 8613e2a3b8..2c08c686c2 100644 --- a/moveit_ros/planning_interface/test/launch/move_group_launch_test_common.py +++ b/moveit_ros/planning_interface/test/launch/move_group_launch_test_common.py @@ -1,134 +1,36 @@ import os -import yaml import launch import launch_ros import launch_testing -import xacro -from launch import LaunchDescription from launch.actions import ExecuteProcess, TimerAction from launch.some_substitutions_type import SomeSubstitutionsType from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch_ros.actions import ComposableNodeContainer, Node -from launch_ros.descriptions import ComposableNode +from launch_ros.actions import Node from ament_index_python.packages import get_package_share_directory - - -def load_file(package_name, file_path): - package_path = get_package_share_directory(package_name) - absolute_file_path = os.path.join(package_path, file_path) - - try: - with open(absolute_file_path, "r") as file: - return file.read() - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available - return None - - -def load_yaml(package_name, file_path): - package_path = get_package_share_directory(package_name) - absolute_file_path = os.path.join(package_path, file_path) - - try: - with open(absolute_file_path, "r") as file: - return yaml.safe_load(file) - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available - return None - - -def load_test_yaml(absolute_file_path): - try: - with open(absolute_file_path, "r") as file: - return yaml.safe_load(file) - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available - return None +from moveit_configs_utils import MoveItConfigsBuilder def generate_move_group_test_description(*args, gtest_name: SomeSubstitutionsType): - - # planning_context - robot_description_config = xacro.process_file( - os.path.join( - get_package_share_directory("moveit_resources_panda_moveit_config"), - "config", - "panda.urdf.xacro", - ) - ) - robot_description = {"robot_description": robot_description_config.toxml()} - - robot_description_semantic_config = load_file( - "moveit_resources_panda_moveit_config", "config/panda.srdf" + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .trajectory_execution(file_path="config/panda_gripper_controllers.yaml") + .to_moveit_configs() ) - robot_description_semantic = { - "robot_description_semantic": robot_description_semantic_config - } - - kinematics_yaml = load_yaml( - "moveit_resources_panda_moveit_config", "config/kinematics.yaml" - ) - robot_description_kinematics = {"robot_description_kinematics": kinematics_yaml} - - # Planning Functionality - ompl_planning_pipeline_config = { - "move_group": { - "planning_plugin": "ompl_interface/OMPLPlanner", - "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""", - "start_state_max_bounds_error": 0.1, - } - } - ompl_planning_yaml = load_yaml( - "moveit_resources_panda_moveit_config", "config/ompl_planning.yaml" - ) - ompl_planning_yaml["panda_arm"]["enforce_constrained_state_space"] = True - ompl_planning_yaml["panda_arm"][ + moveit_config.planning_pipelines["ompl"]["panda_arm"][ + "enforce_constrained_state_space" + ] = True + moveit_config.planning_pipelines["ompl"]["panda_arm"][ "projection_evaluator" ] = "joints(panda_joint1,panda_joint2)" - ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml) - - # Trajectory Execution Functionality - moveit_simple_controllers_yaml = load_yaml( - "moveit_resources_panda_moveit_config", "config/panda_controllers.yaml" - ) - - moveit_controllers = { - "moveit_simple_controller_manager": moveit_simple_controllers_yaml, - "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager", - } - - trajectory_execution = { - "moveit_manage_controllers": True, - "trajectory_execution.allowed_execution_duration_scaling": 1.5, - "trajectory_execution.allowed_goal_duration_margin": 1.0, - "trajectory_execution.allowed_start_tolerance": 0.01, - } - - planning_scene_monitor_parameters = { - "publish_planning_scene": True, - "publish_geometry_updates": True, - "publish_state_updates": True, - "publish_transforms_updates": True, - } - joint_limits_yaml = { - "robot_description_planning": load_yaml( - "moveit_resources_panda_moveit_config", "config/joint_limits.yaml" - ) - } # Start the actual move_group node/action server run_move_group_node = Node( package="moveit_ros_move_group", executable="move_group", output="screen", - parameters=[ - robot_description, - robot_description_semantic, - kinematics_yaml, - ompl_planning_pipeline_config, - trajectory_execution, - moveit_controllers, - planning_scene_monitor_parameters, - joint_limits_yaml, - ], + parameters=[moveit_config.to_dict()], ) # Static TF @@ -146,7 +48,7 @@ def generate_move_group_test_description(*args, gtest_name: SomeSubstitutionsTyp executable="robot_state_publisher", name="robot_state_publisher", output="both", - parameters=[robot_description], + parameters=[moveit_config.robot_description], ) # ros2_control using FakeSystem as hardware @@ -158,7 +60,7 @@ def generate_move_group_test_description(*args, gtest_name: SomeSubstitutionsTyp ros2_control_node = Node( package="controller_manager", executable="ros2_control_node", - parameters=[robot_description, ros2_controllers_path], + parameters=[moveit_config.robot_description, ros2_controllers_path], output={ "stdout": "screen", "stderr": "screen", @@ -167,7 +69,11 @@ def generate_move_group_test_description(*args, gtest_name: SomeSubstitutionsTyp # Load controllers load_controllers = [] - for controller in ["panda_arm_controller", "joint_state_broadcaster"]: + for controller in [ + "panda_arm_controller", + "panda_hand_controller", + "joint_state_broadcaster", + ]: load_controllers += [ ExecuteProcess( cmd=["ros2 run controller_manager spawner {}".format(controller)], @@ -181,7 +87,11 @@ def generate_move_group_test_description(*args, gtest_name: SomeSubstitutionsTyp executable=PathJoinSubstitution( [LaunchConfiguration("test_binary_dir"), gtest_name] ), - parameters=[robot_description, robot_description_semantic, kinematics_yaml], + parameters=[ + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + ], output="screen", )