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",
)