From cadcfefdab642a22b684705e62b591f01117eb05 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Fri, 13 Aug 2021 14:44:09 +0300 Subject: [PATCH 01/39] Add package to moveit2 --- moveit_configs_utils/package.xml | 21 ++ .../resource/moveit_configs_utils | 0 moveit_configs_utils/setup.cfg | 4 + moveit_configs_utils/setup.py | 30 +++ .../src/moveit_configs_utils/__init__.py | 1 + .../moveit_configs_builder.py | 196 ++++++++++++++++++ 6 files changed, 252 insertions(+) create mode 100644 moveit_configs_utils/package.xml create mode 100644 moveit_configs_utils/resource/moveit_configs_utils create mode 100644 moveit_configs_utils/setup.cfg create mode 100644 moveit_configs_utils/setup.py create mode 100644 moveit_configs_utils/src/moveit_configs_utils/__init__.py create mode 100644 moveit_configs_utils/src/moveit_configs_utils/moveit_configs_builder.py diff --git a/moveit_configs_utils/package.xml b/moveit_configs_utils/package.xml new file mode 100644 index 0000000000..71682012d7 --- /dev/null +++ b/moveit_configs_utils/package.xml @@ -0,0 +1,21 @@ + + + + moveit_configs_utils + 0.0.0 + TODO: Package description + jafar_abdi + TODO: License declaration + + ament_python + + ament_lint_auto + ament_lint_common + + ament_index_python + parameter_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.cfg b/moveit_configs_utils/setup.cfg new file mode 100644 index 0000000000..94c3ab679c --- /dev/null +++ b/moveit_configs_utils/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/moveit_configs_utils +[install] +install_scripts=$base/lib/moveit_configs_utils diff --git a/moveit_configs_utils/setup.py b/moveit_configs_utils/setup.py new file mode 100644 index 0000000000..7d3241b772 --- /dev/null +++ b/moveit_configs_utils/setup.py @@ -0,0 +1,30 @@ +from setuptools import setup + +package_name = 'moveit_configs_utils' + +setup( + name=package_name, + version='0.0.0', + package_dir={'': 'src'}, + 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, + author='TODO', + author_email='TODO', + maintainer='TODO', + maintainer_email='TODO', + keywords=['ROS2'], + classifiers=[ + 'Intended Audience :: Developers', + 'License :: OSI Approved :: BSD', + 'Programming Language :: Python', + 'Topic :: Software Development', + ], + description='TODO', + license='BSD', +) \ No newline at end of file diff --git a/moveit_configs_utils/src/moveit_configs_utils/__init__.py b/moveit_configs_utils/src/moveit_configs_utils/__init__.py new file mode 100644 index 0000000000..7a94606837 --- /dev/null +++ b/moveit_configs_utils/src/moveit_configs_utils/__init__.py @@ -0,0 +1 @@ +from .moveit_configs_builder import MoveItConfigsBuilder, MoveItConfigs diff --git a/moveit_configs_utils/src/moveit_configs_utils/moveit_configs_builder.py b/moveit_configs_utils/src/moveit_configs_utils/moveit_configs_builder.py new file mode 100644 index 0000000000..70788484b4 --- /dev/null +++ b/moveit_configs_utils/src/moveit_configs_utils/moveit_configs_builder.py @@ -0,0 +1,196 @@ +from pathlib import Path +from typing import Optional, List +import logging +import xacro +from ament_index_python.packages import get_package_share_directory + +from parameter_builder import ParameterBuilder, load_yaml + + +class MoveItConfigs(object): + def __setattr__(self, name, value): + if hasattr(self, name): + object.__setattr__(self, name, value) + else: + raise AttributeError( + f"Cannot set name {name} on object of type {self.__class__.__name__}" + ) + + robot_description = None + robot_description_semantic = None + robot_description_planning = None + robot_description_kinematics = None + planning_pipelines = None + trajectory_execution = None + planning_scene_monitor = None + move_group_capabilities = None + move_group = None + joint_limits = None + + +class MoveItConfigsBuilder(ParameterBuilder): + __moveit_configs = MoveItConfigs() + __robot_name: str + __urdf_package: Path + __urdf_filename: str + __srdf_filename: str + __robot_description: str + + # 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(): + # TODO: Should this throw an exception .? + 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_filename = "config/" + self.__robot_name + ".urdf" + self.__srdf_filename = "config/" + 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_filename = setup_assistant_yaml[ + "moveit_setup_assistant_config" + ]["URDF"]["relative_path"] + self.__srdf_filename = setup_assistant_yaml[ + "moveit_setup_assistant_config" + ]["SRDF"]["relative_path"] + self.__robot_description = robot_description + + def robot_description(self, file_name: Optional[str] = None, mappings: dict = None): + if file_name is None: + robot_description_file = xacro.process_file( + self.__urdf_package / self.__urdf_filename, mappings=mappings + ) + else: + robot_description_file = xacro.process_file( + self._package_path / "config" / file_name, mappings=mappings + ) + self.__moveit_configs.robot_description = { + self.__robot_description: robot_description_file.toxml() + } + return self + + def robot_description_semantic( + self, file_name: Optional[str] = None, mappings: dict = None + ): + robot_description_semantic = xacro.process_file( + self._package_path + / (("config/" + file_name) if file_name else self.__srdf_filename), + mappings=mappings, + ) + self.__moveit_configs.robot_description_semantic = { + self.__robot_description + "_semantic": robot_description_semantic.toxml() + } + return self + + def robot_description_kinematics(self, file_name: Optional[str] = None): + self.__moveit_configs.robot_description_kinematics = { + self.__robot_description + + "_kinematics": load_yaml( + self._package_path / "config" / (file_name or "kinematics.yaml") + ) + } + return self + + def joint_limits(self, file_name: Optional[str] = None): + self.__moveit_configs.joint_limits = { + self.__robot_description + + "_planning": load_yaml( + self._package_path / "config" / (file_name or "joint_limits.yaml") + ) + } + return self + + def trajectory_execution( + self, + file_name: Optional[str] = None, + moveit_manage_controllers: bool = True, + ): + self.__moveit_configs.trajectory_execution = { + "moveit_manage_controllers": moveit_manage_controllers, + } + self.__moveit_configs.trajectory_execution.update( + load_yaml( + self._package_path + / "config" + / (file_name or 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 + ): + if pipelines is None: + pipelines = ["ompl"] + default_planning_pipeline = pipelines[0] + elif default_planning_pipeline not in pipelines: + raise RuntimeError( + f"default_planning_pipeline: `{default_planning_pipeline}` doesn't name any of the input pipelines `{','.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 / "config" / (pipeline + "_planning.yaml") + ) + return self + + def moveit_configs(self): + return self.__moveit_configs + + def to_dict(self): + parameters = self._parameters + parameters.update(self.__moveit_configs.robot_description) + parameters.update(self.__moveit_configs.robot_description_semantic) + parameters.update(self.__moveit_configs.robot_description_kinematics) + parameters.update(self.__moveit_configs.planning_pipelines) + parameters.update(self.__moveit_configs.trajectory_execution) + parameters.update(self.__moveit_configs.planning_scene_monitor) + parameters.update(self.__moveit_configs.joint_limits) + return parameters + + +# USAGE +# moveit_config = ( +# MoveItConfigsBuilder("ROBOT_NAME") +# .robot_description() +# .robot_description_semantic() +# .robot_description_kinematics() +# .joint_limits() +# .trajectory_execution(file_name="panda_gripper_controllers.yaml") +# .planning_scene_monitor() +# .planning_pipelines() +# .moveit_configs() +# ) From b6ace6d8f111c721d5ed6e611a59bddc9db768bd Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Fri, 13 Aug 2021 17:47:16 +0300 Subject: [PATCH 02/39] Refactor filepath to be w.r.t. package root path --- .../moveit_configs_builder.py | 32 +++++++++---------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/moveit_configs_utils/src/moveit_configs_utils/moveit_configs_builder.py b/moveit_configs_utils/src/moveit_configs_utils/moveit_configs_builder.py index 70788484b4..e4cb7c70fb 100644 --- a/moveit_configs_utils/src/moveit_configs_utils/moveit_configs_builder.py +++ b/moveit_configs_utils/src/moveit_configs_utils/moveit_configs_builder.py @@ -74,7 +74,7 @@ def robot_description(self, file_name: Optional[str] = None, mappings: dict = No ) else: robot_description_file = xacro.process_file( - self._package_path / "config" / file_name, mappings=mappings + self._package_path / file_name, mappings=mappings ) self.__moveit_configs.robot_description = { self.__robot_description: robot_description_file.toxml() @@ -85,8 +85,7 @@ def robot_description_semantic( self, file_name: Optional[str] = None, mappings: dict = None ): robot_description_semantic = xacro.process_file( - self._package_path - / (("config/" + file_name) if file_name else self.__srdf_filename), + self._package_path / (file_name or self.__srdf_filename), mappings=mappings, ) self.__moveit_configs.robot_description_semantic = { @@ -98,7 +97,7 @@ def robot_description_kinematics(self, file_name: Optional[str] = None): self.__moveit_configs.robot_description_kinematics = { self.__robot_description + "_kinematics": load_yaml( - self._package_path / "config" / (file_name or "kinematics.yaml") + self._package_path / (file_name or "config/kinematics.yaml") ) } return self @@ -107,7 +106,7 @@ def joint_limits(self, file_name: Optional[str] = None): self.__moveit_configs.joint_limits = { self.__robot_description + "_planning": load_yaml( - self._package_path / "config" / (file_name or "joint_limits.yaml") + self._package_path / (file_name or "config/joint_limits.yaml") ) } return self @@ -123,8 +122,7 @@ def trajectory_execution( self.__moveit_configs.trajectory_execution.update( load_yaml( self._package_path - / "config" - / (file_name or self.__robot_name + "_controllers.yaml") + / (file_name or f"config/{self.__robot_name}_controllers.yaml") ) ) return self @@ -155,7 +153,8 @@ def planning_pipelines( default_planning_pipeline = pipelines[0] elif default_planning_pipeline not in pipelines: raise RuntimeError( - f"default_planning_pipeline: `{default_planning_pipeline}` doesn't name any of the input pipelines `{','.join(pipelines)}`" + 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, @@ -170,15 +169,16 @@ def planning_pipelines( def moveit_configs(self): return self.__moveit_configs - def to_dict(self): + def to_dict(self, include_moveit_configs: bool = True): parameters = self._parameters - parameters.update(self.__moveit_configs.robot_description) - parameters.update(self.__moveit_configs.robot_description_semantic) - parameters.update(self.__moveit_configs.robot_description_kinematics) - parameters.update(self.__moveit_configs.planning_pipelines) - parameters.update(self.__moveit_configs.trajectory_execution) - parameters.update(self.__moveit_configs.planning_scene_monitor) - parameters.update(self.__moveit_configs.joint_limits) + if include_moveit_configs: + parameters.update(self.__moveit_configs.robot_description) + parameters.update(self.__moveit_configs.robot_description_semantic) + parameters.update(self.__moveit_configs.robot_description_kinematics) + parameters.update(self.__moveit_configs.planning_pipelines) + parameters.update(self.__moveit_configs.trajectory_execution) + parameters.update(self.__moveit_configs.planning_scene_monitor) + parameters.update(self.__moveit_configs.joint_limits) return parameters From b1e4c64e24f0052c53f746e7cdb78a5a3de82e12 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Fri, 13 Aug 2021 18:12:10 +0300 Subject: [PATCH 03/39] Add new parameter to moveit configs utils to support moveit cpp --- .../moveit_configs_builder.py | 35 +++++++++++-------- 1 file changed, 20 insertions(+), 15 deletions(-) diff --git a/moveit_configs_utils/src/moveit_configs_utils/moveit_configs_builder.py b/moveit_configs_utils/src/moveit_configs_utils/moveit_configs_builder.py index e4cb7c70fb..09233ef80f 100644 --- a/moveit_configs_utils/src/moveit_configs_utils/moveit_configs_builder.py +++ b/moveit_configs_utils/src/moveit_configs_utils/moveit_configs_builder.py @@ -1,10 +1,9 @@ from pathlib import Path from typing import Optional, List import logging -import xacro from ament_index_python.packages import get_package_share_directory -from parameter_builder import ParameterBuilder, load_yaml +from parameter_builder import ParameterBuilder, load_yaml, load_xacro class MoveItConfigs(object): @@ -26,6 +25,7 @@ def __setattr__(self, name, value): move_group_capabilities = None move_group = None joint_limits = None + moveit_cpp = None class MoveItConfigsBuilder(ParameterBuilder): @@ -69,27 +69,25 @@ def __init__(self, robot_name: str, robot_description="robot_description"): def robot_description(self, file_name: Optional[str] = None, mappings: dict = None): if file_name is None: - robot_description_file = xacro.process_file( - self.__urdf_package / self.__urdf_filename, mappings=mappings - ) + robot_description_file_path = self.__urdf_package / self.__urdf_filename else: - robot_description_file = xacro.process_file( - self._package_path / file_name, mappings=mappings - ) + robot_description_file_path = self._package_path / file_name self.__moveit_configs.robot_description = { - self.__robot_description: robot_description_file.toxml() + self.__robot_description: load_xacro( + robot_description_file_path, mappings=mappings + ) } return self def robot_description_semantic( self, file_name: Optional[str] = None, mappings: dict = None ): - robot_description_semantic = xacro.process_file( - self._package_path / (file_name or self.__srdf_filename), - mappings=mappings, - ) self.__moveit_configs.robot_description_semantic = { - self.__robot_description + "_semantic": robot_description_semantic.toxml() + self.__robot_description + + "_semantic": load_xacro( + self._package_path / (file_name or self.__srdf_filename), + mappings=mappings, + ) } return self @@ -111,6 +109,12 @@ def joint_limits(self, file_name: Optional[str] = None): } return self + def moveit_cpp(self, file_name: Optional[str] = None): + self.__moveit_configs.moveit_cpp = load_yaml( + self._package_path / (file_name or "config/moveit_cpp.yaml") + ) + return self + def trajectory_execution( self, file_name: Optional[str] = None, @@ -151,7 +155,7 @@ def planning_pipelines( if pipelines is None: pipelines = ["ompl"] default_planning_pipeline = pipelines[0] - elif default_planning_pipeline not in pipelines: + 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)}`" @@ -179,6 +183,7 @@ def to_dict(self, include_moveit_configs: bool = True): parameters.update(self.__moveit_configs.trajectory_execution) parameters.update(self.__moveit_configs.planning_scene_monitor) parameters.update(self.__moveit_configs.joint_limits) + parameters.update(self.__moveit_configs.moveit_cpp) return parameters From 10899784dceac287f632e44664f044bb8b64ab1d Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Fri, 13 Aug 2021 18:40:38 +0300 Subject: [PATCH 04/39] Refactor moveit_servo --- .../launch/pose_tracking_example.launch.py | 90 +++++-------------- moveit_ros/moveit_servo/package.xml | 2 + .../test/launch/servo_launch_test_common.py | 81 +++++------------ .../launch/test_servo_pose_tracking.test.py | 86 +++++------------- .../test/launch/unit_test_servo_calcs.test.py | 9 +- 5 files changed, 76 insertions(+), 192 deletions(-) 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..44c7b62354 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,28 @@ 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 parameter_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_name="config/panda.urdf.xacro") + .robot_description_semantic() + .robot_description_kinematics() + .joint_limits() + .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 @@ -79,10 +38,10 @@ def generate_launch_description(): output="log", arguments=["-d", rviz_config_file], parameters=[ - robot_description, - robot_description_semantic, - kinematics_yaml, - joint_limits_yaml, + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + moveit_config.joint_limits, ], ) @@ -91,7 +50,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 +68,11 @@ 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.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, servo_params, - joint_limits_yaml, + moveit_config.joint_limits, ], ) @@ -127,7 +85,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 ebcfdceee4..3436ce033e 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 + parameter_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..91ddba6a20 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 parameter_builder import ParameterBuilder def generate_servo_test_description( @@ -42,44 +20,31 @@ 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_name="config/panda.urdf.xacro", mappings=robot_description_mappings ) - } + .robot_description_semantic() + .robot_description_kinematics() + .joint_limits() + .moveit_configs() + ) # ros2_control using FakeSystem as hardware ros2_controllers_path = os.path.join( @@ -90,7 +55,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 +84,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 +102,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..4c7d527a09 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,34 @@ 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 parameter_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_name="config/panda.urdf.xacro") + .robot_description_semantic() + .robot_description_kinematics() + .joint_limits() + .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 +42,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 +71,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 +88,11 @@ 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.robot_description, + moveit_config.robot_description_semantic, servo_params, - kinematics_yaml, - joint_limits_yaml, + moveit_config.robot_description_kinematics, + moveit_config.joint_limits, ], 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..5662ebc8f5 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 parameter_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 = { + "moveit_servo": ParameterBuilder("moveit_servo") + .yaml("config/panda_simulated_config.yaml") + .to_dict() + } test_binary_dir_arg = launch.actions.DeclareLaunchArgument( name="test_binary_dir", From db0eb9f74df3d8517eb5962e5cef81b3f6debe30 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Sat, 14 Aug 2021 00:51:00 +0300 Subject: [PATCH 05/39] Refactor benchmarks --- .../benchmarks/examples/demo_panda.launch.py | 87 +++++------------- .../demo_panda_predefined_poses.launch.py | 89 +++++-------------- moveit_ros/benchmarks/package.xml | 2 + 3 files changed, 46 insertions(+), 132 deletions(-) diff --git a/moveit_ros/benchmarks/examples/demo_panda.launch.py b/moveit_ros/benchmarks/examples/demo_panda.launch.py index a3feae79f5..b659151bb4 100644 --- a/moveit_ros/benchmarks/examples/demo_panda.launch.py +++ b/moveit_ros/benchmarks/examples/demo_panda.launch.py @@ -1,72 +1,29 @@ -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 parameter_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" + moveit_ros_benchmarks_config = ( + ParameterBuilder("moveit_ros_benchmarks") + .yaml( + parameter_namespace="benchmark_config", + file_path="demo1.yaml", + ) + .to_dict() ) - 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_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description() + .robot_description_semantic() + .robot_description_kinematics() + .joint_limits() + .planning_pipelines() + .moveit_configs() ) - 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_ros_benchmark demo executable moveit_ros_benchmarks_node = Node( @@ -77,11 +34,11 @@ 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_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + moveit_config.planning_pipelines, + moveit_config.joint_limits, ], ) 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..862d6d7ebb 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,29 @@ -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 parameter_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" + moveit_ros_benchmarks_config = ( + ParameterBuilder("moveit_ros_benchmarks") + .yaml( + parameter_namespace="benchmark_config", + file_path="demo_panda_predefined_poses.yaml", + ) + .to_dict() ) - 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_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description() + .robot_description_semantic() + .robot_description_kinematics() + .joint_limits() + .planning_pipelines() + .moveit_configs() ) - 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_ros_benchmark demo executable moveit_ros_benchmarks_node = Node( @@ -79,11 +34,11 @@ 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_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + moveit_config.planning_pipelines, + moveit_config.joint_limits, ], ) diff --git a/moveit_ros/benchmarks/package.xml b/moveit_ros/benchmarks/package.xml index 52f162aec6..55e8d30ab8 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 + parameter_builder ament_lint_auto ament_lint_common From a2ef6c6520ce8ffba250cd1351b8b557c2a80234 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Sat, 14 Aug 2021 01:36:22 +0300 Subject: [PATCH 06/39] Rename file_name to file_path --- .../moveit_configs_builder.py | 28 +++++++++---------- .../launch/pose_tracking_example.launch.py | 2 +- .../test/launch/servo_launch_test_common.py | 2 +- .../launch/test_servo_pose_tracking.test.py | 2 +- 4 files changed, 17 insertions(+), 17 deletions(-) diff --git a/moveit_configs_utils/src/moveit_configs_utils/moveit_configs_builder.py b/moveit_configs_utils/src/moveit_configs_utils/moveit_configs_builder.py index 09233ef80f..8b640c76e0 100644 --- a/moveit_configs_utils/src/moveit_configs_utils/moveit_configs_builder.py +++ b/moveit_configs_utils/src/moveit_configs_utils/moveit_configs_builder.py @@ -67,11 +67,11 @@ def __init__(self, robot_name: str, robot_description="robot_description"): ]["SRDF"]["relative_path"] self.__robot_description = robot_description - def robot_description(self, file_name: Optional[str] = None, mappings: dict = None): - if file_name is None: + def robot_description(self, file_path: Optional[str] = None, mappings: dict = None): + if file_path is None: robot_description_file_path = self.__urdf_package / self.__urdf_filename else: - robot_description_file_path = self._package_path / file_name + 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 @@ -80,44 +80,44 @@ def robot_description(self, file_name: Optional[str] = None, mappings: dict = No return self def robot_description_semantic( - self, file_name: Optional[str] = None, mappings: dict = None + self, file_path: Optional[str] = None, mappings: dict = None ): self.__moveit_configs.robot_description_semantic = { self.__robot_description + "_semantic": load_xacro( - self._package_path / (file_name or self.__srdf_filename), + self._package_path / (file_path or self.__srdf_filename), mappings=mappings, ) } return self - def robot_description_kinematics(self, file_name: Optional[str] = None): + def robot_description_kinematics(self, file_path: Optional[str] = None): self.__moveit_configs.robot_description_kinematics = { self.__robot_description + "_kinematics": load_yaml( - self._package_path / (file_name or "config/kinematics.yaml") + self._package_path / (file_path or "config/kinematics.yaml") ) } return self - def joint_limits(self, file_name: Optional[str] = None): + def joint_limits(self, file_path: Optional[str] = None): self.__moveit_configs.joint_limits = { self.__robot_description + "_planning": load_yaml( - self._package_path / (file_name or "config/joint_limits.yaml") + self._package_path / (file_path or "config/joint_limits.yaml") ) } return self - def moveit_cpp(self, file_name: Optional[str] = None): + def moveit_cpp(self, file_path: Optional[str] = None): self.__moveit_configs.moveit_cpp = load_yaml( - self._package_path / (file_name or "config/moveit_cpp.yaml") + self._package_path / (file_path or "config/moveit_cpp.yaml") ) return self def trajectory_execution( self, - file_name: Optional[str] = None, + file_path: Optional[str] = None, moveit_manage_controllers: bool = True, ): self.__moveit_configs.trajectory_execution = { @@ -126,7 +126,7 @@ def trajectory_execution( self.__moveit_configs.trajectory_execution.update( load_yaml( self._package_path - / (file_name or f"config/{self.__robot_name}_controllers.yaml") + / (file_path or f"config/{self.__robot_name}_controllers.yaml") ) ) return self @@ -194,7 +194,7 @@ def to_dict(self, include_moveit_configs: bool = True): # .robot_description_semantic() # .robot_description_kinematics() # .joint_limits() -# .trajectory_execution(file_name="panda_gripper_controllers.yaml") +# .trajectory_execution(file_path="panda_gripper_controllers.yaml") # .planning_scene_monitor() # .planning_pipelines() # .moveit_configs() 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 44c7b62354..2d91b766a1 100644 --- a/moveit_ros/moveit_servo/launch/pose_tracking_example.launch.py +++ b/moveit_ros/moveit_servo/launch/pose_tracking_example.launch.py @@ -10,7 +10,7 @@ def generate_launch_description(): moveit_config = ( MoveItConfigsBuilder("moveit_resources_panda") - .robot_description(file_name="config/panda.urdf.xacro") + .robot_description(file_path="config/panda.urdf.xacro") .robot_description_semantic() .robot_description_kinematics() .joint_limits() 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 91ddba6a20..7e343cf4ac 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 @@ -38,7 +38,7 @@ def generate_servo_test_description( moveit_config = ( MoveItConfigsBuilder("moveit_resources_panda") .robot_description( - file_name="config/panda.urdf.xacro", mappings=robot_description_mappings + file_path="config/panda.urdf.xacro", mappings=robot_description_mappings ) .robot_description_semantic() .robot_description_kinematics() 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 4c7d527a09..2b794e9678 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 @@ -18,7 +18,7 @@ def generate_servo_test_description(*args, gtest_name: SomeSubstitutionsType): moveit_config = ( MoveItConfigsBuilder("moveit_resources_panda") - .robot_description(file_name="config/panda.urdf.xacro") + .robot_description(file_path="config/panda.urdf.xacro") .robot_description_semantic() .robot_description_kinematics() .joint_limits() From f7c585318cf3f128ff2fdd7275dd0a9d644aca7a Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Sat, 14 Aug 2021 01:37:02 +0300 Subject: [PATCH 07/39] Refactor planning interface --- moveit_ros/planning_interface/package.xml | 1 + .../launch/move_group_launch_test_common.py | 147 +++++------------- 2 files changed, 36 insertions(+), 112 deletions(-) diff --git a/moveit_ros/planning_interface/package.xml b/moveit_ros/planning_interface/package.xml index 1b63951247..c28cf58217 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..9545a403cd 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,118 +1,34 @@ 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" - ) - robot_description_semantic = { - "robot_description_semantic": robot_description_semantic_config - } - - kinematics_yaml = load_yaml( - "moveit_resources_panda_moveit_config", "config/kinematics.yaml" + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .robot_description_semantic() + .robot_description_kinematics() + .joint_limits() + .trajectory_execution(file_path="config/panda_gripper_controllers.yaml") + .planning_scene_monitor() + .planning_pipelines() + .moveit_configs() ) - 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( @@ -120,14 +36,13 @@ def generate_move_group_test_description(*args, gtest_name: SomeSubstitutionsTyp 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, + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + moveit_config.planning_pipelines, + moveit_config.trajectory_execution, + moveit_config.planning_scene_monitor, + moveit_config.joint_limits, ], ) @@ -146,7 +61,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 +73,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 +82,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 +100,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", ) From e26c29037a0a9276b586f307f3a971b989c962d7 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Sat, 14 Aug 2021 01:50:51 +0300 Subject: [PATCH 08/39] Refactor moveit kinematics --- moveit_kinematics/package.xml | 2 + .../test/launch/fanuc-kdl-singular.test.py | 59 +++++------------ .../test/launch/fanuc-kdl.test.py | 66 +++++-------------- .../test/launch/fanuc-lma-singular.test.py | 65 +++++------------- .../test/launch/fanuc-lma.test.py | 66 +++++-------------- .../test/launch/panda-kdl-singular.test.py | 65 +++++------------- .../test/launch/panda-kdl.test.py | 66 +++++-------------- .../test/launch/panda-lma-singular.test.py | 66 +++++-------------- .../test/launch/panda-lma.test.py | 66 +++++-------------- 9 files changed, 138 insertions(+), 383 deletions(-) diff --git a/moveit_kinematics/package.xml b/moveit_kinematics/package.xml index 6f31e395a8..aab93c6cb8 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 + parameter_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..e83ae9b7b1 100644 --- a/moveit_kinematics/test/launch/fanuc-kdl-singular.test.py +++ b/moveit_kinematics/test/launch/fanuc-kdl-singular.test.py @@ -1,64 +1,39 @@ 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 parameter_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" + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_fanuc") + .robot_description() + .robot_description_semantic() + .robot_description_kinematics() + .joint_limits() + .moveit_configs() ) - robot_description_semantic = { - "robot_description_semantic": robot_description_semantic_config - } - kinematics_yaml = load_yaml( - "moveit_resources_fanuc_moveit_config", "config/kinematics.yaml" + 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_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + moveit_config.joint_limits, 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..33c830bc2b 100644 --- a/moveit_kinematics/test/launch/fanuc-kdl.test.py +++ b/moveit_kinematics/test/launch/fanuc-kdl.test.py @@ -1,71 +1,39 @@ 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 parameter_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" + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_fanuc") + .robot_description() + .robot_description_semantic() + .robot_description_kinematics() + .joint_limits() + .moveit_configs() ) - robot_description_semantic = { - "robot_description_semantic": robot_description_semantic_config - } - kinematics_yaml = load_yaml( - "moveit_resources_fanuc_moveit_config", "config/kinematics.yaml" + 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_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + moveit_config.joint_limits, 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..27907faac5 100644 --- a/moveit_kinematics/test/launch/fanuc-lma-singular.test.py +++ b/moveit_kinematics/test/launch/fanuc-lma-singular.test.py @@ -1,70 +1,39 @@ 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 parameter_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" + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_fanuc") + .robot_description() + .robot_description_semantic() + .robot_description_kinematics() + .joint_limits() + .moveit_configs() ) - robot_description_semantic = { - "robot_description_semantic": robot_description_semantic_config - } - kinematics_yaml = load_yaml( - "moveit_resources_fanuc_moveit_config", "config/kinematics.yaml" + 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_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + moveit_config.joint_limits, 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..8963cee888 100644 --- a/moveit_kinematics/test/launch/fanuc-lma.test.py +++ b/moveit_kinematics/test/launch/fanuc-lma.test.py @@ -1,71 +1,39 @@ 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 parameter_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" + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_fanuc") + .robot_description() + .robot_description_semantic() + .robot_description_kinematics() + .joint_limits() + .moveit_configs() ) - robot_description_semantic = { - "robot_description_semantic": robot_description_semantic_config - } - kinematics_yaml = load_yaml( - "moveit_resources_fanuc_moveit_config", "config/kinematics.yaml" + 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_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + moveit_config.joint_limits, 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..7229e2d703 100644 --- a/moveit_kinematics/test/launch/panda-kdl-singular.test.py +++ b/moveit_kinematics/test/launch/panda-kdl-singular.test.py @@ -1,71 +1,40 @@ 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 parameter_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" + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description() + .robot_description_semantic() + .robot_description_kinematics() + .joint_limits() + .moveit_configs() ) - 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" + 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_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + moveit_config.joint_limits, 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..1d93c49410 100644 --- a/moveit_kinematics/test/launch/panda-kdl.test.py +++ b/moveit_kinematics/test/launch/panda-kdl.test.py @@ -1,71 +1,39 @@ 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 parameter_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" + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description() + .robot_description_semantic() + .robot_description_kinematics() + .joint_limits() + .moveit_configs() ) - 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-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_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + moveit_config.joint_limits, 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..8a1471854b 100644 --- a/moveit_kinematics/test/launch/panda-lma-singular.test.py +++ b/moveit_kinematics/test/launch/panda-lma-singular.test.py @@ -1,70 +1,38 @@ 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 parameter_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" + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description() + .robot_description_semantic() + .robot_description_kinematics() + .joint_limits() + .moveit_configs() ) - 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-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_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + moveit_config.joint_limits, 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..c58679f5d1 100644 --- a/moveit_kinematics/test/launch/panda-lma.test.py +++ b/moveit_kinematics/test/launch/panda-lma.test.py @@ -1,71 +1,39 @@ 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 parameter_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" + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description() + .robot_description_semantic() + .robot_description_kinematics() + .joint_limits() + .moveit_configs() ) - 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-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_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + moveit_config.joint_limits, test_param, ], output="screen", From 4fb985ea0dfc82f9c9d12a210ecde1b90752ac1a Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Sat, 14 Aug 2021 02:00:08 +0300 Subject: [PATCH 09/39] Add source for parameter tools --- moveit2.repos | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/moveit2.repos b/moveit2.repos index 3cc9fb6e3f..4c55bfdf57 100644 --- a/moveit2.repos +++ b/moveit2.repos @@ -20,3 +20,7 @@ repositories: type: git url: https://github.com/ros-planning/warehouse_ros_mongo version: ros2 + parameter_tools: + type: git + url: https://github.com/JafarAbdi/parameter_tools + version: pr-add_parameter_builder From 21a78d6c3ee430e301a8900557f56d13722cb74b Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Sat, 14 Aug 2021 02:05:10 +0300 Subject: [PATCH 10/39] pre-commit --- moveit_configs_utils/setup.py | 37 +++++++++---------- .../test/launch/panda-lma.test.py | 14 +++---- 2 files changed, 25 insertions(+), 26 deletions(-) diff --git a/moveit_configs_utils/setup.py b/moveit_configs_utils/setup.py index 7d3241b772..96168cf5b4 100644 --- a/moveit_configs_utils/setup.py +++ b/moveit_configs_utils/setup.py @@ -1,30 +1,29 @@ from setuptools import setup -package_name = 'moveit_configs_utils' +package_name = "moveit_configs_utils" setup( name=package_name, - version='0.0.0', - package_dir={'': 'src'}, + version="0.0.0", + package_dir={"": "src"}, packages=[package_name], data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), ], - install_requires=['setuptools'], + install_requires=["setuptools"], zip_safe=True, - author='TODO', - author_email='TODO', - maintainer='TODO', - maintainer_email='TODO', - keywords=['ROS2'], + author="TODO", + author_email="TODO", + maintainer="TODO", + maintainer_email="TODO", + keywords=["ROS2"], classifiers=[ - 'Intended Audience :: Developers', - 'License :: OSI Approved :: BSD', - 'Programming Language :: Python', - 'Topic :: Software Development', + "Intended Audience :: Developers", + "License :: OSI Approved :: BSD", + "Programming Language :: Python", + "Topic :: Software Development", ], - description='TODO', - license='BSD', -) \ No newline at end of file + description="TODO", + license="BSD", +) diff --git a/moveit_kinematics/test/launch/panda-lma.test.py b/moveit_kinematics/test/launch/panda-lma.test.py index c58679f5d1..320505599c 100644 --- a/moveit_kinematics/test/launch/panda-lma.test.py +++ b/moveit_kinematics/test/launch/panda-lma.test.py @@ -13,16 +13,16 @@ def generate_test_description(): moveit_config = ( MoveItConfigsBuilder("moveit_resources_panda") - .robot_description() - .robot_description_semantic() - .robot_description_kinematics() - .joint_limits() - .moveit_configs() + .robot_description() + .robot_description_semantic() + .robot_description_kinematics() + .joint_limits() + .moveit_configs() ) test_param = ( ParameterBuilder("moveit_kinematics") - .yaml("config/panda-lma-test.yaml") - .to_dict() + .yaml("config/panda-lma-test.yaml") + .to_dict() ) panda_lma = Node( From c08574c4592a6c8722f3d2bfde51a1aed59fa4b4 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Sat, 14 Aug 2021 02:23:02 +0300 Subject: [PATCH 11/39] Fixes --- moveit_configs_utils/package.xml | 8 +++----- moveit_configs_utils/setup.py | 21 ++++++++------------- 2 files changed, 11 insertions(+), 18 deletions(-) diff --git a/moveit_configs_utils/package.xml b/moveit_configs_utils/package.xml index 71682012d7..d05ed5a8de 100644 --- a/moveit_configs_utils/package.xml +++ b/moveit_configs_utils/package.xml @@ -3,11 +3,9 @@ moveit_configs_utils 0.0.0 - TODO: Package description - jafar_abdi - TODO: License declaration - - ament_python + Python library for loading moveit config parameters in launch files + MoveIt Release Team + BSD ament_lint_auto ament_lint_common diff --git a/moveit_configs_utils/setup.py b/moveit_configs_utils/setup.py index 96168cf5b4..57e7bfb7ae 100644 --- a/moveit_configs_utils/setup.py +++ b/moveit_configs_utils/setup.py @@ -5,7 +5,6 @@ setup( name=package_name, version="0.0.0", - package_dir={"": "src"}, packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), @@ -13,17 +12,13 @@ ], install_requires=["setuptools"], zip_safe=True, - author="TODO", - author_email="TODO", - maintainer="TODO", - maintainer_email="TODO", - keywords=["ROS2"], - classifiers=[ - "Intended Audience :: Developers", - "License :: OSI Approved :: BSD", - "Programming Language :: Python", - "Topic :: Software Development", - ], - description="TODO", + 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': [ + ], + }, ) From 6294cc830762230ada1ae620f284db7aeefe4dc1 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Sat, 14 Aug 2021 02:25:45 +0300 Subject: [PATCH 12/39] Update moveit_resources tag --- moveit2.repos | 9 ++++----- moveit_configs_utils/setup.py | 5 ++--- 2 files changed, 6 insertions(+), 8 deletions(-) diff --git a/moveit2.repos b/moveit2.repos index 4c55bfdf57..2ee6c68fcf 100644 --- a/moveit2.repos +++ b/moveit2.repos @@ -7,11 +7,10 @@ repositories: type: git url: https://github.com/ros-planning/moveit_msgs version: ros2 - # TODO(#885): Re-enable when circular dependency is fixed - # moveit_resources: - # type: git - # url: https://github.com/ros-planning/moveit_resources - # version: ros2 + moveit_resources: + type: git + url: https://github.com/JafarAbdi/moveit_resources + version: pr-moveit_configs_utils warehouse_ros: type: git url: https://github.com/ros-planning/warehouse_ros diff --git a/moveit_configs_utils/setup.py b/moveit_configs_utils/setup.py index 57e7bfb7ae..6de9df6a1e 100644 --- a/moveit_configs_utils/setup.py +++ b/moveit_configs_utils/setup.py @@ -16,9 +16,8 @@ maintainer_email="moveit_releasers@googlegroups.com", description="Python library for loading moveit config parameters in launch files", license="BSD", - tests_require=['pytest'], + tests_require=["pytest"], entry_points={ - 'console_scripts': [ - ], + "console_scripts": [], }, ) From f1f3f212dc4b3060204fa04edd30bfec2ca3c162 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Thu, 9 Sep 2021 20:16:48 +0300 Subject: [PATCH 13/39] Fix bug --- moveit_configs_utils/setup.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit_configs_utils/setup.py b/moveit_configs_utils/setup.py index 6de9df6a1e..07bec708c6 100644 --- a/moveit_configs_utils/setup.py +++ b/moveit_configs_utils/setup.py @@ -1,11 +1,11 @@ -from setuptools import setup +from setuptools import setup, find_packages package_name = "moveit_configs_utils" setup( name=package_name, version="0.0.0", - packages=[package_name], + packages=find_packages(), data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), ("share/" + package_name, ["package.xml"]), From 1a312e98d359ad7d2dc7c1fe2f3ac4f5ea8992b2 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Thu, 9 Sep 2021 20:24:30 +0300 Subject: [PATCH 14/39] Update parameter tools tag --- moveit2.repos | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit2.repos b/moveit2.repos index 2ee6c68fcf..9f0dd0a1d4 100644 --- a/moveit2.repos +++ b/moveit2.repos @@ -21,5 +21,5 @@ repositories: version: ros2 parameter_tools: type: git - url: https://github.com/JafarAbdi/parameter_tools - version: pr-add_parameter_builder + url: https://github.com/PickNikRobotics/parameter_tools + version: main From f1fe62dc25ad88f39398a59d925c966f0ebd521d Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Thu, 9 Sep 2021 22:16:03 +0300 Subject: [PATCH 15/39] Fixes --- .../{src => }/moveit_configs_utils/__init__.py | 0 .../{src => }/moveit_configs_utils/moveit_configs_builder.py | 0 moveit_configs_utils/setup.cfg | 4 ---- moveit_configs_utils/setup.py | 4 ++-- 4 files changed, 2 insertions(+), 6 deletions(-) rename moveit_configs_utils/{src => }/moveit_configs_utils/__init__.py (100%) rename moveit_configs_utils/{src => }/moveit_configs_utils/moveit_configs_builder.py (100%) delete mode 100644 moveit_configs_utils/setup.cfg diff --git a/moveit_configs_utils/src/moveit_configs_utils/__init__.py b/moveit_configs_utils/moveit_configs_utils/__init__.py similarity index 100% rename from moveit_configs_utils/src/moveit_configs_utils/__init__.py rename to moveit_configs_utils/moveit_configs_utils/__init__.py diff --git a/moveit_configs_utils/src/moveit_configs_utils/moveit_configs_builder.py b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py similarity index 100% rename from moveit_configs_utils/src/moveit_configs_utils/moveit_configs_builder.py rename to moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py diff --git a/moveit_configs_utils/setup.cfg b/moveit_configs_utils/setup.cfg deleted file mode 100644 index 94c3ab679c..0000000000 --- a/moveit_configs_utils/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/moveit_configs_utils -[install] -install_scripts=$base/lib/moveit_configs_utils diff --git a/moveit_configs_utils/setup.py b/moveit_configs_utils/setup.py index 07bec708c6..6de9df6a1e 100644 --- a/moveit_configs_utils/setup.py +++ b/moveit_configs_utils/setup.py @@ -1,11 +1,11 @@ -from setuptools import setup, find_packages +from setuptools import setup package_name = "moveit_configs_utils" setup( name=package_name, version="0.0.0", - packages=find_packages(), + packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), ("share/" + package_name, ["package.xml"]), From 44655109d9282c9cef5ab96f9e0d0b6319b4c648 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Fri, 10 Sep 2021 17:44:49 +0300 Subject: [PATCH 16/39] Use property/setters for MoveItConfigs --- .../moveit_configs_builder.py | 142 +++++++++++++++--- 1 file changed, 124 insertions(+), 18 deletions(-) diff --git a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py index 8b640c76e0..40974d8648 100644 --- a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py +++ b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py @@ -7,25 +7,131 @@ class MoveItConfigs(object): - def __setattr__(self, name, value): - if hasattr(self, name): - object.__setattr__(self, name, value) - else: - raise AttributeError( - f"Cannot set name {name} on object of type {self.__class__.__name__}" - ) + __slots__ = [ + "__robot_description", + "__robot_description_semantic", + "__robot_description_planning", + "__robot_description_kinematics", + "__planning_pipelines", + "__trajectory_execution", + "__planning_scene_monitor", + "__move_group_capabilities", + "__move_group", + "__joint_limits", + "__moveit_cpp", + ] + + def __init__(self, **kwargs): + assert all( + "__" + key in self.__slots__ for key in kwargs.keys() + ), "Invalid arguments passed to constructor: %s" % ", ".join( + sorted(k for k in kwargs.keys() if "__" + k not in self.__slots__) + ) + self.__robot_description = kwargs.get("robot_description", None) + self.__robot_description_semantic = kwargs.get( + "robot_description_semantic", None + ) + self.__robot_description_planning = kwargs.get( + "robot_description_planning", None + ) + self.__robot_description_kinematics = kwargs.get( + "robot_description_kinematics", None + ) + self.__planning_pipelines = kwargs.get("planning_pipelines", None) + self.__trajectory_execution = kwargs.get("trajectory_execution", None) + self.__planning_scene_monitor = kwargs.get("planning_scene_monitor", None) + self.__move_group_capabilities = kwargs.get("move_group_capabilities", None) + self.__move_group = kwargs.get("move_group", None) + self.__joint_limits = kwargs.get("joint_limits", None) + self.__moveit_cpp = kwargs.get("moveit_cpp", None) + + @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_planning(self): + return self.__robot_description_planning + + @robot_description_planning.setter + def robot_description_planning(self, value): + self.__robot_description_planning = 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 move_group(self): + return self.__move_group + + @move_group.setter + def move_group(self, value): + self.__move_group = 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 - robot_description = None - robot_description_semantic = None - robot_description_planning = None - robot_description_kinematics = None - planning_pipelines = None - trajectory_execution = None - planning_scene_monitor = None - move_group_capabilities = None - move_group = None - joint_limits = None - moveit_cpp = None + @moveit_cpp.setter + def moveit_cpp(self, value): + self.__moveit_cpp = value class MoveItConfigsBuilder(ParameterBuilder): From 2a74434c3bf8d52d4134fa222183b7adaa92cdec Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Fri, 10 Sep 2021 17:45:12 +0300 Subject: [PATCH 17/39] Rename .moveit_configs() to .to_moveit_configs() --- .../moveit_configs_utils/moveit_configs_builder.py | 4 ++-- moveit_kinematics/test/launch/fanuc-kdl-singular.test.py | 2 +- moveit_kinematics/test/launch/fanuc-kdl.test.py | 2 +- moveit_kinematics/test/launch/fanuc-lma-singular.test.py | 2 +- moveit_kinematics/test/launch/fanuc-lma.test.py | 2 +- moveit_kinematics/test/launch/panda-kdl-singular.test.py | 2 +- moveit_kinematics/test/launch/panda-kdl.test.py | 2 +- moveit_kinematics/test/launch/panda-lma-singular.test.py | 2 +- moveit_kinematics/test/launch/panda-lma.test.py | 2 +- moveit_ros/benchmarks/examples/demo_panda.launch.py | 2 +- .../benchmarks/examples/demo_panda_predefined_poses.launch.py | 2 +- .../moveit_servo/launch/pose_tracking_example.launch.py | 2 +- .../moveit_servo/test/launch/servo_launch_test_common.py | 2 +- .../moveit_servo/test/launch/test_servo_pose_tracking.test.py | 2 +- .../test/launch/move_group_launch_test_common.py | 2 +- 15 files changed, 16 insertions(+), 16 deletions(-) diff --git a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py index 40974d8648..bcf47de43e 100644 --- a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py +++ b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py @@ -276,7 +276,7 @@ def planning_pipelines( ) return self - def moveit_configs(self): + def to_moveit_configs(self): return self.__moveit_configs def to_dict(self, include_moveit_configs: bool = True): @@ -303,5 +303,5 @@ def to_dict(self, include_moveit_configs: bool = True): # .trajectory_execution(file_path="panda_gripper_controllers.yaml") # .planning_scene_monitor() # .planning_pipelines() -# .moveit_configs() +# .to_moveit_configs() # ) diff --git a/moveit_kinematics/test/launch/fanuc-kdl-singular.test.py b/moveit_kinematics/test/launch/fanuc-kdl-singular.test.py index e83ae9b7b1..64475c3dc5 100644 --- a/moveit_kinematics/test/launch/fanuc-kdl-singular.test.py +++ b/moveit_kinematics/test/launch/fanuc-kdl-singular.test.py @@ -17,7 +17,7 @@ def generate_test_description(): .robot_description_semantic() .robot_description_kinematics() .joint_limits() - .moveit_configs() + .to_moveit_configs() ) test_param = ( ParameterBuilder("moveit_kinematics") diff --git a/moveit_kinematics/test/launch/fanuc-kdl.test.py b/moveit_kinematics/test/launch/fanuc-kdl.test.py index 33c830bc2b..f45ade929f 100644 --- a/moveit_kinematics/test/launch/fanuc-kdl.test.py +++ b/moveit_kinematics/test/launch/fanuc-kdl.test.py @@ -17,7 +17,7 @@ def generate_test_description(): .robot_description_semantic() .robot_description_kinematics() .joint_limits() - .moveit_configs() + .to_moveit_configs() ) test_param = ( ParameterBuilder("moveit_kinematics") diff --git a/moveit_kinematics/test/launch/fanuc-lma-singular.test.py b/moveit_kinematics/test/launch/fanuc-lma-singular.test.py index 27907faac5..b3db5c8157 100644 --- a/moveit_kinematics/test/launch/fanuc-lma-singular.test.py +++ b/moveit_kinematics/test/launch/fanuc-lma-singular.test.py @@ -17,7 +17,7 @@ def generate_test_description(): .robot_description_semantic() .robot_description_kinematics() .joint_limits() - .moveit_configs() + .to_moveit_configs() ) test_param = ( ParameterBuilder("moveit_kinematics") diff --git a/moveit_kinematics/test/launch/fanuc-lma.test.py b/moveit_kinematics/test/launch/fanuc-lma.test.py index 8963cee888..a198b569aa 100644 --- a/moveit_kinematics/test/launch/fanuc-lma.test.py +++ b/moveit_kinematics/test/launch/fanuc-lma.test.py @@ -17,7 +17,7 @@ def generate_test_description(): .robot_description_semantic() .robot_description_kinematics() .joint_limits() - .moveit_configs() + .to_moveit_configs() ) test_param = ( ParameterBuilder("moveit_kinematics") diff --git a/moveit_kinematics/test/launch/panda-kdl-singular.test.py b/moveit_kinematics/test/launch/panda-kdl-singular.test.py index 7229e2d703..294ac98f8c 100644 --- a/moveit_kinematics/test/launch/panda-kdl-singular.test.py +++ b/moveit_kinematics/test/launch/panda-kdl-singular.test.py @@ -17,7 +17,7 @@ def generate_test_description(): .robot_description_semantic() .robot_description_kinematics() .joint_limits() - .moveit_configs() + .to_moveit_configs() ) test_param = ( diff --git a/moveit_kinematics/test/launch/panda-kdl.test.py b/moveit_kinematics/test/launch/panda-kdl.test.py index 1d93c49410..fd73b33827 100644 --- a/moveit_kinematics/test/launch/panda-kdl.test.py +++ b/moveit_kinematics/test/launch/panda-kdl.test.py @@ -17,7 +17,7 @@ def generate_test_description(): .robot_description_semantic() .robot_description_kinematics() .joint_limits() - .moveit_configs() + .to_moveit_configs() ) test_param = ( ParameterBuilder("moveit_kinematics") diff --git a/moveit_kinematics/test/launch/panda-lma-singular.test.py b/moveit_kinematics/test/launch/panda-lma-singular.test.py index 8a1471854b..33411292ca 100644 --- a/moveit_kinematics/test/launch/panda-lma-singular.test.py +++ b/moveit_kinematics/test/launch/panda-lma-singular.test.py @@ -16,7 +16,7 @@ def generate_test_description(): .robot_description_semantic() .robot_description_kinematics() .joint_limits() - .moveit_configs() + .to_moveit_configs() ) test_param = ( ParameterBuilder("moveit_kinematics") diff --git a/moveit_kinematics/test/launch/panda-lma.test.py b/moveit_kinematics/test/launch/panda-lma.test.py index 320505599c..e5ad744468 100644 --- a/moveit_kinematics/test/launch/panda-lma.test.py +++ b/moveit_kinematics/test/launch/panda-lma.test.py @@ -17,7 +17,7 @@ def generate_test_description(): .robot_description_semantic() .robot_description_kinematics() .joint_limits() - .moveit_configs() + .to_moveit_configs() ) test_param = ( ParameterBuilder("moveit_kinematics") diff --git a/moveit_ros/benchmarks/examples/demo_panda.launch.py b/moveit_ros/benchmarks/examples/demo_panda.launch.py index b659151bb4..ea2ae16485 100644 --- a/moveit_ros/benchmarks/examples/demo_panda.launch.py +++ b/moveit_ros/benchmarks/examples/demo_panda.launch.py @@ -22,7 +22,7 @@ def generate_launch_description(): .robot_description_kinematics() .joint_limits() .planning_pipelines() - .moveit_configs() + .to_moveit_configs() ) # moveit_ros_benchmark demo executable 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 862d6d7ebb..f581e909b8 100644 --- a/moveit_ros/benchmarks/examples/demo_panda_predefined_poses.launch.py +++ b/moveit_ros/benchmarks/examples/demo_panda_predefined_poses.launch.py @@ -22,7 +22,7 @@ def generate_launch_description(): .robot_description_kinematics() .joint_limits() .planning_pipelines() - .moveit_configs() + .to_moveit_configs() ) # moveit_ros_benchmark demo executable 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 2d91b766a1..86c2b6f336 100644 --- a/moveit_ros/moveit_servo/launch/pose_tracking_example.launch.py +++ b/moveit_ros/moveit_servo/launch/pose_tracking_example.launch.py @@ -14,7 +14,7 @@ def generate_launch_description(): .robot_description_semantic() .robot_description_kinematics() .joint_limits() - .moveit_configs() + .to_moveit_configs() ) # Get parameters for the Pose Tracking node 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 7e343cf4ac..68c30e7b54 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 @@ -43,7 +43,7 @@ def generate_servo_test_description( .robot_description_semantic() .robot_description_kinematics() .joint_limits() - .moveit_configs() + .to_moveit_configs() ) # ros2_control using FakeSystem as hardware 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 2b794e9678..c03d11ab9c 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 @@ -22,7 +22,7 @@ def generate_servo_test_description(*args, gtest_name: SomeSubstitutionsType): .robot_description_semantic() .robot_description_kinematics() .joint_limits() - .moveit_configs() + .to_moveit_configs() ) # Get parameters for the Pose Tracking and Servo nodes 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 9545a403cd..bb1462d534 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 @@ -20,7 +20,7 @@ def generate_move_group_test_description(*args, gtest_name: SomeSubstitutionsTyp .trajectory_execution(file_path="config/panda_gripper_controllers.yaml") .planning_scene_monitor() .planning_pipelines() - .moveit_configs() + .to_moveit_configs() ) moveit_config.planning_pipelines["ompl"]["panda_arm"][ From 9c0da67aaa8c19445ee98bab87fec88a4d466096 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Fri, 10 Sep 2021 17:51:58 +0300 Subject: [PATCH 18/39] Remove todo --- .../moveit_configs_utils/moveit_configs_builder.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py index bcf47de43e..6b6ddafd75 100644 --- a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py +++ b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py @@ -141,14 +141,12 @@ class MoveItConfigsBuilder(ParameterBuilder): __urdf_filename: str __srdf_filename: str __robot_description: str - # 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(): - # TODO: Should this throw an exception .? 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" From 7c46f0e7cecfc2230fd5b60cd4354205f39c041e Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Fri, 10 Sep 2021 17:52:19 +0300 Subject: [PATCH 19/39] to_dict: Only add the loaded parameters --- .../moveit_configs_builder.py | 24 ++++++++++++------- 1 file changed, 16 insertions(+), 8 deletions(-) diff --git a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py index 6b6ddafd75..2d60fc192e 100644 --- a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py +++ b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py @@ -280,14 +280,22 @@ def to_moveit_configs(self): def to_dict(self, include_moveit_configs: bool = True): parameters = self._parameters if include_moveit_configs: - parameters.update(self.__moveit_configs.robot_description) - parameters.update(self.__moveit_configs.robot_description_semantic) - parameters.update(self.__moveit_configs.robot_description_kinematics) - parameters.update(self.__moveit_configs.planning_pipelines) - parameters.update(self.__moveit_configs.trajectory_execution) - parameters.update(self.__moveit_configs.planning_scene_monitor) - parameters.update(self.__moveit_configs.joint_limits) - parameters.update(self.__moveit_configs.moveit_cpp) + if self.__moveit_configs.robot_description: + parameters.update(self.__moveit_configs.robot_description) + if self.__moveit_configs.robot_description_semantic: + parameters.update(self.__moveit_configs.robot_description_semantic) + if self.__moveit_configs.robot_description_kinematics: + parameters.update(self.__moveit_configs.robot_description_kinematics) + if self.__moveit_configs.planning_pipelines: + parameters.update(self.__moveit_configs.planning_pipelines) + if self.__moveit_configs.trajectory_execution: + parameters.update(self.__moveit_configs.trajectory_execution) + if self.__moveit_configs.planning_scene_monitor: + parameters.update(self.__moveit_configs.planning_scene_monitor) + if self.__moveit_configs.joint_limits: + parameters.update(self.__moveit_configs.joint_limits) + if self.__moveit_configs.moveit_cpp: + parameters.update(self.__moveit_configs.moveit_cpp) return parameters From 1aeedfe738cb0eaeed029c7e5ec1b0c9a26e899d Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Fri, 10 Sep 2021 17:53:10 +0300 Subject: [PATCH 20/39] clean-up --- .../moveit_configs_utils/moveit_configs_builder.py | 14 -------------- 1 file changed, 14 deletions(-) diff --git a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py index 2d60fc192e..6b5e7c2350 100644 --- a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py +++ b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py @@ -297,17 +297,3 @@ def to_dict(self, include_moveit_configs: bool = True): if self.__moveit_configs.moveit_cpp: parameters.update(self.__moveit_configs.moveit_cpp) return parameters - - -# USAGE -# moveit_config = ( -# MoveItConfigsBuilder("ROBOT_NAME") -# .robot_description() -# .robot_description_semantic() -# .robot_description_kinematics() -# .joint_limits() -# .trajectory_execution(file_path="panda_gripper_controllers.yaml") -# .planning_scene_monitor() -# .planning_pipelines() -# .to_moveit_configs() -# ) From 5e48ccddfd25619bb8585acc406023219d01976d Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Fri, 10 Sep 2021 17:55:54 +0300 Subject: [PATCH 21/39] Use parameter namespace for servo params --- .../test/launch/unit_test_servo_calcs.test.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) 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 5662ebc8f5..5b42c42553 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 @@ -11,11 +11,11 @@ def generate_test_description(): # Get parameters using the demo config file - servo_params = { - "moveit_servo": ParameterBuilder("moveit_servo") - .yaml("config/panda_simulated_config.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", From 8cb9d6c16aad1084441af479a03da7271e70015b Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Fri, 10 Sep 2021 17:57:01 +0300 Subject: [PATCH 22/39] Run black --- .../moveit_configs_utils/moveit_configs_builder.py | 1 + 1 file changed, 1 insertion(+) diff --git a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py index 6b5e7c2350..85f9b0abb3 100644 --- a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py +++ b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py @@ -141,6 +141,7 @@ class MoveItConfigsBuilder(ParameterBuilder): __urdf_filename: str __srdf_filename: str __robot_description: str + # Look-up for robot_name_moveit_config package def __init__(self, robot_name: str, robot_description="robot_description"): super().__init__(robot_name + "_moveit_config") From 4509ec6656a0157f9a87f3558fe8c9e79307b09e Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Fri, 10 Sep 2021 18:29:13 +0300 Subject: [PATCH 23/39] Add ability to load cartesian_limits parameters --- .../moveit_configs_builder.py | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py index 85f9b0abb3..9e1f2303f6 100644 --- a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py +++ b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py @@ -19,6 +19,7 @@ class MoveItConfigs(object): "__move_group", "__joint_limits", "__moveit_cpp", + "__cartesian_limits", ] def __init__(self, **kwargs): @@ -44,6 +45,7 @@ def __init__(self, **kwargs): self.__move_group = kwargs.get("move_group", None) self.__joint_limits = kwargs.get("joint_limits", None) self.__moveit_cpp = kwargs.get("moveit_cpp", None) + self.__cartesian_limits = kwargs.get("cartesian_limits", None) @property def robot_description(self): @@ -133,6 +135,14 @@ def moveit_cpp(self): 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 + class MoveItConfigsBuilder(ParameterBuilder): __moveit_configs = MoveItConfigs() @@ -275,6 +285,12 @@ def planning_pipelines( ) return self + def cartesian_limits(self, file_path: Optional[str] = None): + self.__moveit_configs.cartesian_limits = load_yaml( + self._package_path / (file_path or "config/cartesian_limits.yaml") + ) + return self + def to_moveit_configs(self): return self.__moveit_configs @@ -297,4 +313,6 @@ def to_dict(self, include_moveit_configs: bool = True): parameters.update(self.__moveit_configs.joint_limits) if self.__moveit_configs.moveit_cpp: parameters.update(self.__moveit_configs.moveit_cpp) + if self.__moveit_configs.cartesian_limits: + parameters.update(self.__moveit_configs.cartesian_limits) return parameters From 7fc45543d06eff1c180dc0450ea5f4f41230a651 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Mon, 13 Sep 2021 16:04:16 +0300 Subject: [PATCH 24/39] Simplification --- .../moveit_configs_builder.py | 69 ++++++++----------- 1 file changed, 27 insertions(+), 42 deletions(-) diff --git a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py index 9e1f2303f6..9e006c257e 100644 --- a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py +++ b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py @@ -22,30 +22,19 @@ class MoveItConfigs(object): "__cartesian_limits", ] - def __init__(self, **kwargs): - assert all( - "__" + key in self.__slots__ for key in kwargs.keys() - ), "Invalid arguments passed to constructor: %s" % ", ".join( - sorted(k for k in kwargs.keys() if "__" + k not in self.__slots__) - ) - self.__robot_description = kwargs.get("robot_description", None) - self.__robot_description_semantic = kwargs.get( - "robot_description_semantic", None - ) - self.__robot_description_planning = kwargs.get( - "robot_description_planning", None - ) - self.__robot_description_kinematics = kwargs.get( - "robot_description_kinematics", None - ) - self.__planning_pipelines = kwargs.get("planning_pipelines", None) - self.__trajectory_execution = kwargs.get("trajectory_execution", None) - self.__planning_scene_monitor = kwargs.get("planning_scene_monitor", None) - self.__move_group_capabilities = kwargs.get("move_group_capabilities", None) - self.__move_group = kwargs.get("move_group", None) - self.__joint_limits = kwargs.get("joint_limits", None) - self.__moveit_cpp = kwargs.get("moveit_cpp", None) - self.__cartesian_limits = kwargs.get("cartesian_limits", None) + def __init__(self): + self.robot_description = {} + self.robot_description_semantic = {} + self.robot_description_planning = {} + self.robot_description_kinematics = {} + self.planning_pipelines = {} + self.trajectory_execution = {} + self.planning_scene_monitor = {} + self.move_group_capabilities = {} + self.move_group = {} + self.joint_limits = {} + self.moveit_cpp = {} + self.cartesian_limits = {} @property def robot_description(self): @@ -143,6 +132,19 @@ def cartesian_limits(self): 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() @@ -297,22 +299,5 @@ def to_moveit_configs(self): def to_dict(self, include_moveit_configs: bool = True): parameters = self._parameters if include_moveit_configs: - if self.__moveit_configs.robot_description: - parameters.update(self.__moveit_configs.robot_description) - if self.__moveit_configs.robot_description_semantic: - parameters.update(self.__moveit_configs.robot_description_semantic) - if self.__moveit_configs.robot_description_kinematics: - parameters.update(self.__moveit_configs.robot_description_kinematics) - if self.__moveit_configs.planning_pipelines: - parameters.update(self.__moveit_configs.planning_pipelines) - if self.__moveit_configs.trajectory_execution: - parameters.update(self.__moveit_configs.trajectory_execution) - if self.__moveit_configs.planning_scene_monitor: - parameters.update(self.__moveit_configs.planning_scene_monitor) - if self.__moveit_configs.joint_limits: - parameters.update(self.__moveit_configs.joint_limits) - if self.__moveit_configs.moveit_cpp: - parameters.update(self.__moveit_configs.moveit_cpp) - if self.__moveit_configs.cartesian_limits: - parameters.update(self.__moveit_configs.cartesian_limits) + parameters.update(self.__moveit_configs.to_dict()) return parameters From da658d833858f32755af6aa7be923abe3264e5da Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Mon, 13 Sep 2021 16:28:14 +0300 Subject: [PATCH 25/39] Use to_dict --- .../test/launch/fanuc-kdl-singular.test.py | 9 +++------ moveit_kinematics/test/launch/fanuc-kdl.test.py | 9 +++------ .../test/launch/fanuc-lma-singular.test.py | 9 +++------ moveit_kinematics/test/launch/fanuc-lma.test.py | 9 +++------ .../test/launch/panda-kdl-singular.test.py | 9 +++------ moveit_kinematics/test/launch/panda-kdl.test.py | 9 +++------ .../test/launch/panda-lma-singular.test.py | 9 +++------ moveit_kinematics/test/launch/panda-lma.test.py | 9 +++------ moveit_ros/benchmarks/examples/demo_panda.launch.py | 10 +++------- .../examples/demo_panda_predefined_poses.launch.py | 10 +++------- .../launch/pose_tracking_example.launch.py | 12 ++---------- .../test/launch/test_servo_pose_tracking.test.py | 5 +---- .../test/launch/move_group_launch_test_common.py | 10 +--------- 13 files changed, 34 insertions(+), 85 deletions(-) diff --git a/moveit_kinematics/test/launch/fanuc-kdl-singular.test.py b/moveit_kinematics/test/launch/fanuc-kdl-singular.test.py index 64475c3dc5..6a88c156fb 100644 --- a/moveit_kinematics/test/launch/fanuc-kdl-singular.test.py +++ b/moveit_kinematics/test/launch/fanuc-kdl-singular.test.py @@ -11,13 +11,13 @@ @pytest.mark.rostest def generate_test_description(): - moveit_config = ( + moveit_configs = ( MoveItConfigsBuilder("moveit_resources_fanuc") .robot_description() .robot_description_semantic() .robot_description_kinematics() .joint_limits() - .to_moveit_configs() + .to_dict() ) test_param = ( ParameterBuilder("moveit_kinematics") @@ -30,10 +30,7 @@ def generate_test_description(): executable="test_kinematics_plugin", name="fanuc_kdl_singular", parameters=[ - moveit_config.robot_description, - moveit_config.robot_description_semantic, - moveit_config.robot_description_kinematics, - moveit_config.joint_limits, + 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 f45ade929f..05a059181a 100644 --- a/moveit_kinematics/test/launch/fanuc-kdl.test.py +++ b/moveit_kinematics/test/launch/fanuc-kdl.test.py @@ -11,13 +11,13 @@ @pytest.mark.rostest def generate_test_description(): - moveit_config = ( + moveit_configs = ( MoveItConfigsBuilder("moveit_resources_fanuc") .robot_description() .robot_description_semantic() .robot_description_kinematics() .joint_limits() - .to_moveit_configs() + .to_dict() ) test_param = ( ParameterBuilder("moveit_kinematics") @@ -30,10 +30,7 @@ def generate_test_description(): executable="test_kinematics_plugin", name="fanuc_kdl", parameters=[ - moveit_config.robot_description, - moveit_config.robot_description_semantic, - moveit_config.robot_description_kinematics, - moveit_config.joint_limits, + 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 b3db5c8157..ecaa150f1c 100644 --- a/moveit_kinematics/test/launch/fanuc-lma-singular.test.py +++ b/moveit_kinematics/test/launch/fanuc-lma-singular.test.py @@ -11,13 +11,13 @@ @pytest.mark.rostest def generate_test_description(): - moveit_config = ( + moveit_configs = ( MoveItConfigsBuilder("moveit_resources_fanuc") .robot_description() .robot_description_semantic() .robot_description_kinematics() .joint_limits() - .to_moveit_configs() + .to_dict() ) test_param = ( ParameterBuilder("moveit_kinematics") @@ -30,10 +30,7 @@ def generate_test_description(): executable="test_kinematics_plugin", name="fanuc_lma_singular", parameters=[ - moveit_config.robot_description, - moveit_config.robot_description_semantic, - moveit_config.robot_description_kinematics, - moveit_config.joint_limits, + 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 a198b569aa..b176ea0653 100644 --- a/moveit_kinematics/test/launch/fanuc-lma.test.py +++ b/moveit_kinematics/test/launch/fanuc-lma.test.py @@ -11,13 +11,13 @@ @pytest.mark.rostest def generate_test_description(): - moveit_config = ( + moveit_configs = ( MoveItConfigsBuilder("moveit_resources_fanuc") .robot_description() .robot_description_semantic() .robot_description_kinematics() .joint_limits() - .to_moveit_configs() + .to_dict() ) test_param = ( ParameterBuilder("moveit_kinematics") @@ -30,10 +30,7 @@ def generate_test_description(): executable="test_kinematics_plugin", name="fanuc_lma", parameters=[ - moveit_config.robot_description, - moveit_config.robot_description_semantic, - moveit_config.robot_description_kinematics, - moveit_config.joint_limits, + 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 294ac98f8c..d41d830955 100644 --- a/moveit_kinematics/test/launch/panda-kdl-singular.test.py +++ b/moveit_kinematics/test/launch/panda-kdl-singular.test.py @@ -11,13 +11,13 @@ @pytest.mark.rostest def generate_test_description(): - moveit_config = ( + moveit_configs = ( MoveItConfigsBuilder("moveit_resources_panda") .robot_description() .robot_description_semantic() .robot_description_kinematics() .joint_limits() - .to_moveit_configs() + .to_dict() ) test_param = ( @@ -31,10 +31,7 @@ def generate_test_description(): executable="test_kinematics_plugin", name="panda_kdl_singular", parameters=[ - moveit_config.robot_description, - moveit_config.robot_description_semantic, - moveit_config.robot_description_kinematics, - moveit_config.joint_limits, + 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 fd73b33827..7fcb10490f 100644 --- a/moveit_kinematics/test/launch/panda-kdl.test.py +++ b/moveit_kinematics/test/launch/panda-kdl.test.py @@ -11,13 +11,13 @@ @pytest.mark.rostest def generate_test_description(): - moveit_config = ( + moveit_configs = ( MoveItConfigsBuilder("moveit_resources_panda") .robot_description() .robot_description_semantic() .robot_description_kinematics() .joint_limits() - .to_moveit_configs() + .to_dict() ) test_param = ( ParameterBuilder("moveit_kinematics") @@ -30,10 +30,7 @@ def generate_test_description(): executable="test_kinematics_plugin", name="panda_kdl", parameters=[ - moveit_config.robot_description, - moveit_config.robot_description_semantic, - moveit_config.robot_description_kinematics, - moveit_config.joint_limits, + 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 33411292ca..5dd2d919a7 100644 --- a/moveit_kinematics/test/launch/panda-lma-singular.test.py +++ b/moveit_kinematics/test/launch/panda-lma-singular.test.py @@ -10,13 +10,13 @@ @pytest.mark.rostest def generate_test_description(): - moveit_config = ( + moveit_configs = ( MoveItConfigsBuilder("moveit_resources_panda") .robot_description() .robot_description_semantic() .robot_description_kinematics() .joint_limits() - .to_moveit_configs() + .to_dict() ) test_param = ( ParameterBuilder("moveit_kinematics") @@ -29,10 +29,7 @@ def generate_test_description(): executable="test_kinematics_plugin", name="panda_lma_singular", parameters=[ - moveit_config.robot_description, - moveit_config.robot_description_semantic, - moveit_config.robot_description_kinematics, - moveit_config.joint_limits, + 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 e5ad744468..98343c8b24 100644 --- a/moveit_kinematics/test/launch/panda-lma.test.py +++ b/moveit_kinematics/test/launch/panda-lma.test.py @@ -11,13 +11,13 @@ @pytest.mark.rostest def generate_test_description(): - moveit_config = ( + moveit_configs = ( MoveItConfigsBuilder("moveit_resources_panda") .robot_description() .robot_description_semantic() .robot_description_kinematics() .joint_limits() - .to_moveit_configs() + .to_dict() ) test_param = ( ParameterBuilder("moveit_kinematics") @@ -30,10 +30,7 @@ def generate_test_description(): executable="test_kinematics_plugin", name="panda_lma", parameters=[ - moveit_config.robot_description, - moveit_config.robot_description_semantic, - moveit_config.robot_description_kinematics, - moveit_config.joint_limits, + moveit_configs, test_param, ], output="screen", diff --git a/moveit_ros/benchmarks/examples/demo_panda.launch.py b/moveit_ros/benchmarks/examples/demo_panda.launch.py index ea2ae16485..44433931b9 100644 --- a/moveit_ros/benchmarks/examples/demo_panda.launch.py +++ b/moveit_ros/benchmarks/examples/demo_panda.launch.py @@ -15,14 +15,14 @@ def generate_launch_description(): .to_dict() ) - moveit_config = ( + moveit_configs = ( MoveItConfigsBuilder("moveit_resources_panda") .robot_description() .robot_description_semantic() .robot_description_kinematics() .joint_limits() .planning_pipelines() - .to_moveit_configs() + .to_dict() ) # moveit_ros_benchmark demo executable @@ -34,11 +34,7 @@ def generate_launch_description(): output="screen", parameters=[ moveit_ros_benchmarks_config, - moveit_config.robot_description, - moveit_config.robot_description_semantic, - moveit_config.robot_description_kinematics, - moveit_config.planning_pipelines, - moveit_config.joint_limits, + 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 f581e909b8..31ba2105bc 100644 --- a/moveit_ros/benchmarks/examples/demo_panda_predefined_poses.launch.py +++ b/moveit_ros/benchmarks/examples/demo_panda_predefined_poses.launch.py @@ -15,14 +15,14 @@ def generate_launch_description(): .to_dict() ) - moveit_config = ( + moveit_configs = ( MoveItConfigsBuilder("moveit_resources_panda") .robot_description() .robot_description_semantic() .robot_description_kinematics() .joint_limits() .planning_pipelines() - .to_moveit_configs() + .to_dict() ) # moveit_ros_benchmark demo executable @@ -34,11 +34,7 @@ def generate_launch_description(): output="screen", parameters=[ moveit_ros_benchmarks_config, - moveit_config.robot_description, - moveit_config.robot_description_semantic, - moveit_config.robot_description_kinematics, - moveit_config.planning_pipelines, - moveit_config.joint_limits, + moveit_configs, ], ) 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 86c2b6f336..afc606f5af 100644 --- a/moveit_ros/moveit_servo/launch/pose_tracking_example.launch.py +++ b/moveit_ros/moveit_servo/launch/pose_tracking_example.launch.py @@ -37,12 +37,7 @@ def generate_launch_description(): # prefix=['xterm -e gdb -ex run --args'], output="log", arguments=["-d", rviz_config_file], - parameters=[ - moveit_config.robot_description, - moveit_config.robot_description_semantic, - moveit_config.robot_description_kinematics, - moveit_config.joint_limits, - ], + parameters=[moveit_config.to_dict()], ) # Publishes tf's for the robot @@ -68,11 +63,8 @@ def generate_launch_description(): # prefix=['xterm -e gdb -ex run --args'], output="screen", parameters=[ - moveit_config.robot_description, - moveit_config.robot_description_semantic, - moveit_config.robot_description_kinematics, + moveit_config.to_dict(), servo_params, - 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 c03d11ab9c..2cc89aced8 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 @@ -88,11 +88,8 @@ def generate_servo_test_description(*args, gtest_name: SomeSubstitutionsType): [LaunchConfiguration("test_binary_dir"), gtest_name] ), parameters=[ - moveit_config.robot_description, - moveit_config.robot_description_semantic, + moveit_config.to_dict(), servo_params, - moveit_config.robot_description_kinematics, - moveit_config.joint_limits, ], output="screen", ) 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 bb1462d534..13915dde1b 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 @@ -35,15 +35,7 @@ def generate_move_group_test_description(*args, gtest_name: SomeSubstitutionsTyp package="moveit_ros_move_group", executable="move_group", output="screen", - parameters=[ - moveit_config.robot_description, - moveit_config.robot_description_semantic, - moveit_config.robot_description_kinematics, - moveit_config.planning_pipelines, - moveit_config.trajectory_execution, - moveit_config.planning_scene_monitor, - moveit_config.joint_limits, - ], + parameters=[moveit_config.to_dict()], ) # Static TF From ae3a1ef679a7a3ad1ad2a404e9f3c601725dbc65 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Mon, 20 Dec 2021 10:31:35 +0300 Subject: [PATCH 26/39] Fix cartesian limits' namespace --- .../moveit_configs_utils/moveit_configs_builder.py | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py index 9e006c257e..713ff22cb1 100644 --- a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py +++ b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py @@ -288,9 +288,12 @@ def planning_pipelines( return self def cartesian_limits(self, file_path: Optional[str] = None): - self.__moveit_configs.cartesian_limits = load_yaml( - self._package_path / (file_path or "config/cartesian_limits.yaml") - ) + self.__moveit_configs.cartesian_limits = { + self.__robot_description + + "_planning": load_yaml( + self._package_path / (file_path or "config/cartesian_limits.yaml") + ) + } return self def to_moveit_configs(self): From e295d7481a935285bc323a37b7b9bb791d2273c4 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Mon, 20 Dec 2021 10:36:30 +0300 Subject: [PATCH 27/39] Apply Dave suggestions --- moveit_configs_utils/package.xml | 2 +- moveit_configs_utils/setup.py | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/moveit_configs_utils/package.xml b/moveit_configs_utils/package.xml index d05ed5a8de..3f08e130ec 100644 --- a/moveit_configs_utils/package.xml +++ b/moveit_configs_utils/package.xml @@ -2,7 +2,7 @@ moveit_configs_utils - 0.0.0 + 2.3.0 Python library for loading moveit config parameters in launch files MoveIt Release Team BSD diff --git a/moveit_configs_utils/setup.py b/moveit_configs_utils/setup.py index 6de9df6a1e..d1222ab2ac 100644 --- a/moveit_configs_utils/setup.py +++ b/moveit_configs_utils/setup.py @@ -4,7 +4,7 @@ setup( name=package_name, - version="0.0.0", + version="2.3.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), @@ -14,7 +14,7 @@ zip_safe=True, maintainer="MoveIt Release Team", maintainer_email="moveit_releasers@googlegroups.com", - description="Python library for loading moveit config parameters in launch files", + description="Python library for loading MoveIt config parameters in launch files", license="BSD", tests_require=["pytest"], entry_points={ From 1eae514384d583272058c12fcc4306e97f83d2c7 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Mon, 20 Dec 2021 11:50:09 +0300 Subject: [PATCH 28/39] Update --- .../moveit_configs_builder.py | 22 ++++++++++++++++++- 1 file changed, 21 insertions(+), 1 deletion(-) diff --git a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py index 713ff22cb1..50165939b0 100644 --- a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py +++ b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py @@ -297,10 +297,30 @@ def cartesian_limits(self, file_path: Optional[str] = None): return self def to_moveit_configs(self): + 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): parameters = self._parameters if include_moveit_configs: - parameters.update(self.__moveit_configs.to_dict()) + parameters.update(self.to_moveit_configs().to_dict()) return parameters From 7a5d000314b2a96ed3c2fc3e91ba511db1a452b9 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Mon, 20 Dec 2021 15:43:59 +0300 Subject: [PATCH 29/39] Remove explicit file seperator --- .../moveit_configs_builder.py | 54 +++++++++++++------ 1 file changed, 37 insertions(+), 17 deletions(-) diff --git a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py index 50165939b0..6adacd2950 100644 --- a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py +++ b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py @@ -150,9 +150,12 @@ class MoveItConfigsBuilder(ParameterBuilder): __moveit_configs = MoveItConfigs() __robot_name: str __urdf_package: Path - __urdf_filename: str - __srdf_filename: str + # Relative path of the URDF file + __urdf_file_path: Path + # Relative path of the SRDF file + __srdf_file_path: Path __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"): @@ -165,8 +168,12 @@ def __init__(self, robot_name: str, robot_description="robot_description"): f"-- using config/{robot_name}.urdf and config/{robot_name}.srdf\x1b[0m" ) self.__urdf_package = self._package_path - self.__urdf_filename = "config/" + self.__robot_name + ".urdf" - self.__srdf_filename = "config/" + self.__robot_name + ".srdf" + 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( @@ -176,17 +183,21 @@ def __init__(self, robot_name: str, robot_description="robot_description"): ] ) ) - self.__urdf_filename = setup_assistant_yaml[ - "moveit_setup_assistant_config" - ]["URDF"]["relative_path"] - self.__srdf_filename = setup_assistant_yaml[ - "moveit_setup_assistant_config" - ]["SRDF"]["relative_path"] + 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): if file_path is None: - robot_description_file_path = self.__urdf_package / self.__urdf_filename + 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 = { @@ -212,7 +223,8 @@ def robot_description_kinematics(self, file_path: Optional[str] = None): self.__moveit_configs.robot_description_kinematics = { self.__robot_description + "_kinematics": load_yaml( - self._package_path / (file_path or "config/kinematics.yaml") + self._package_path + / (file_path or self.__config_dir_path / "kinematics.yaml") ) } return self @@ -221,14 +233,16 @@ def joint_limits(self, file_path: Optional[str] = None): self.__moveit_configs.joint_limits = { self.__robot_description + "_planning": load_yaml( - self._package_path / (file_path or "config/joint_limits.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): self.__moveit_configs.moveit_cpp = load_yaml( - self._package_path / (file_path or "config/moveit_cpp.yaml") + self._package_path + / (file_path or self.__config_dir_path / "moveit_cpp.yaml") ) return self @@ -243,7 +257,10 @@ def trajectory_execution( self.__moveit_configs.trajectory_execution.update( load_yaml( self._package_path - / (file_path or f"config/{self.__robot_name}_controllers.yaml") + / ( + file_path + or self.__config_dir_path / f"{self.__robot_name}_controllers.yaml" + ) ) ) return self @@ -283,7 +300,9 @@ def planning_pipelines( } for pipeline in pipelines: self.__moveit_configs.planning_pipelines[pipeline] = load_yaml( - self._package_path / "config" / (pipeline + "_planning.yaml") + self._package_path + / self.__config_dir_path + / (pipeline + "_planning.yaml") ) return self @@ -291,7 +310,8 @@ def cartesian_limits(self, file_path: Optional[str] = None): self.__moveit_configs.cartesian_limits = { self.__robot_description + "_planning": load_yaml( - self._package_path / (file_path or "config/cartesian_limits.yaml") + self._package_path + / (file_path or self.__config_dir_path / "cartesian_limits.yaml") ) } return self From 476187a978f178e1ca1846d4cb28caefc95f86c3 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Mon, 20 Dec 2021 20:32:23 +0300 Subject: [PATCH 30/39] Remove unused attribute --- .../moveit_configs_utils/moveit_configs_builder.py | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py index 6adacd2950..48cf3dd60c 100644 --- a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py +++ b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py @@ -10,7 +10,6 @@ class MoveItConfigs(object): __slots__ = [ "__robot_description", "__robot_description_semantic", - "__robot_description_planning", "__robot_description_kinematics", "__planning_pipelines", "__trajectory_execution", @@ -25,7 +24,6 @@ class MoveItConfigs(object): def __init__(self): self.robot_description = {} self.robot_description_semantic = {} - self.robot_description_planning = {} self.robot_description_kinematics = {} self.planning_pipelines = {} self.trajectory_execution = {} @@ -52,14 +50,6 @@ def robot_description_semantic(self): def robot_description_semantic(self, value): self.__robot_description_semantic = value - @property - def robot_description_planning(self): - return self.__robot_description_planning - - @robot_description_planning.setter - def robot_description_planning(self, value): - self.__robot_description_planning = value - @property def robot_description_kinematics(self): return self.__robot_description_kinematics From f0f3dd4fb97f7d3e50b8f4e5b1998ab0efc96a36 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Mon, 20 Dec 2021 21:44:25 +0300 Subject: [PATCH 31/39] Remove unused move_group attribute --- .../moveit_configs_utils/moveit_configs_builder.py | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py index 48cf3dd60c..8de1569e21 100644 --- a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py +++ b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py @@ -15,7 +15,6 @@ class MoveItConfigs(object): "__trajectory_execution", "__planning_scene_monitor", "__move_group_capabilities", - "__move_group", "__joint_limits", "__moveit_cpp", "__cartesian_limits", @@ -29,7 +28,6 @@ def __init__(self): self.trajectory_execution = {} self.planning_scene_monitor = {} self.move_group_capabilities = {} - self.move_group = {} self.joint_limits = {} self.moveit_cpp = {} self.cartesian_limits = {} @@ -90,14 +88,6 @@ def move_group_capabilities(self): def move_group_capabilities(self, value): self.__move_group_capabilities = value - @property - def move_group(self): - return self.__move_group - - @move_group.setter - def move_group(self, value): - self.__move_group = value - @property def joint_limits(self): return self.__joint_limits From 2d5b37c4092b1d3ae947ab9a8f48e0106c96bf57 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Mon, 20 Dec 2021 22:52:30 +0300 Subject: [PATCH 32/39] Add comments --- .../moveit_configs_builder.py | 123 +++++++++++++++++- 1 file changed, 121 insertions(+), 2 deletions(-) diff --git a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py index 8de1569e21..657c86a4c8 100644 --- a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py +++ b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py @@ -1,3 +1,54 @@ +"""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_configs/ + .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_configs + .robot_description_semantic(Path.home() / "my_config" / "new_file.srdf") + .to_moveit_configs() +""" + from pathlib import Path from typing import Optional, List import logging @@ -7,6 +58,8 @@ class MoveItConfigs(object): + """Class containing MoveIt related parameters.""" + __slots__ = [ "__robot_description", "__robot_description_semantic", @@ -21,15 +74,25 @@ class MoveItConfigs(object): ] def __init__(self): + # A dictionary that have the content of the URDF file. self.robot_description = {} + # A dictionary that have the content 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 MoveItCpp related parameters. self.moveit_cpp = {} + # A dictionary containing the cartesian limits for the Pilz planner. self.cartesian_limits = {} @property @@ -130,10 +193,12 @@ class MoveItConfigsBuilder(ParameterBuilder): __moveit_configs = MoveItConfigs() __robot_name: str __urdf_package: Path - # Relative path of the URDF file + # Relative path of the URDF file w.r.t. __urdf_package __urdf_file_path: Path - # Relative path of the SRDF file + # Relative path of the SRDF file w.r.t. robot_name_moveit_configs __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") @@ -176,6 +241,12 @@ def __init__(self, robot_name: str, robot_description="robot_description"): 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_configs). + :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: @@ -190,6 +261,12 @@ def robot_description(self, file_path: Optional[str] = None, mappings: dict = No 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_configs). + :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( @@ -200,6 +277,11 @@ def robot_description_semantic( 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_configs). + :return: Instance of MoveItConfigsBuilder with robot_description_kinematics loaded. + """ self.__moveit_configs.robot_description_kinematics = { self.__robot_description + "_kinematics": load_yaml( @@ -210,6 +292,11 @@ def robot_description_kinematics(self, file_path: Optional[str] = None): 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_configs). + :return: Instance of MoveItConfigsBuilder with robot_description_planning loaded. + """ self.__moveit_configs.joint_limits = { self.__robot_description + "_planning": load_yaml( @@ -220,6 +307,11 @@ def joint_limits(self, file_path: Optional[str] = None): 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_configs). + :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") @@ -231,6 +323,12 @@ def trajectory_execution( 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_configs). + :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, } @@ -266,6 +364,12 @@ def planning_scene_monitor( 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] @@ -287,6 +391,11 @@ def planning_pipelines( 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_configs). + :return: Instance of MoveItConfigsBuilder with cartesian_limits loaded. + """ self.__moveit_configs.cartesian_limits = { self.__robot_description + "_planning": load_yaml( @@ -297,6 +406,10 @@ def cartesian_limits(self, file_path: Optional[str] = None): return self def to_moveit_configs(self): + """Get MoveIt configs from robot_name_moveit_configs. + + :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: @@ -320,6 +433,12 @@ def to_moveit_configs(self): return self.__moveit_configs def to_dict(self, include_moveit_configs: bool = True): + """Get loaded parameters from robot_name_moveit_configs 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()) From ff079fd62ff09026720034d492b6111a2069bab5 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Mon, 20 Dec 2021 22:55:13 +0300 Subject: [PATCH 33/39] Add author per Dave request --- moveit_configs_utils/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/moveit_configs_utils/package.xml b/moveit_configs_utils/package.xml index 3f08e130ec..427ea3b45a 100644 --- a/moveit_configs_utils/package.xml +++ b/moveit_configs_utils/package.xml @@ -4,6 +4,7 @@ moveit_configs_utils 2.3.0 Python library for loading moveit config parameters in launch files + Jafar Abdi MoveIt Release Team BSD From 04360fa4642068fca252fb4818654b22ef317484 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Tue, 21 Dec 2021 00:14:43 +0300 Subject: [PATCH 34/39] Simplify --- .../test/launch/fanuc-kdl-singular.test.py | 9 +-------- moveit_kinematics/test/launch/fanuc-kdl.test.py | 9 +-------- .../test/launch/fanuc-lma-singular.test.py | 9 +-------- moveit_kinematics/test/launch/fanuc-lma.test.py | 9 +-------- .../test/launch/panda-kdl-singular.test.py | 9 +-------- moveit_kinematics/test/launch/panda-kdl.test.py | 9 +-------- .../test/launch/panda-lma-singular.test.py | 9 +-------- moveit_kinematics/test/launch/panda-lma.test.py | 9 +-------- moveit_ros/benchmarks/examples/demo_panda.launch.py | 10 +--------- .../examples/demo_panda_predefined_poses.launch.py | 10 +--------- .../launch/pose_tracking_example.launch.py | 3 --- .../test/launch/servo_launch_test_common.py | 3 --- .../test/launch/test_servo_pose_tracking.test.py | 3 --- .../test/launch/move_group_launch_test_common.py | 5 ----- 14 files changed, 10 insertions(+), 96 deletions(-) diff --git a/moveit_kinematics/test/launch/fanuc-kdl-singular.test.py b/moveit_kinematics/test/launch/fanuc-kdl-singular.test.py index 6a88c156fb..628fc26cc0 100644 --- a/moveit_kinematics/test/launch/fanuc-kdl-singular.test.py +++ b/moveit_kinematics/test/launch/fanuc-kdl-singular.test.py @@ -11,14 +11,7 @@ @pytest.mark.rostest def generate_test_description(): - moveit_configs = ( - MoveItConfigsBuilder("moveit_resources_fanuc") - .robot_description() - .robot_description_semantic() - .robot_description_kinematics() - .joint_limits() - .to_dict() - ) + moveit_configs = MoveItConfigsBuilder("moveit_resources_fanuc").to_dict() test_param = ( ParameterBuilder("moveit_kinematics") .yaml("config/fanuc-kdl-singular-test.yaml") diff --git a/moveit_kinematics/test/launch/fanuc-kdl.test.py b/moveit_kinematics/test/launch/fanuc-kdl.test.py index 05a059181a..84dbea11ba 100644 --- a/moveit_kinematics/test/launch/fanuc-kdl.test.py +++ b/moveit_kinematics/test/launch/fanuc-kdl.test.py @@ -11,14 +11,7 @@ @pytest.mark.rostest def generate_test_description(): - moveit_configs = ( - MoveItConfigsBuilder("moveit_resources_fanuc") - .robot_description() - .robot_description_semantic() - .robot_description_kinematics() - .joint_limits() - .to_dict() - ) + moveit_configs = MoveItConfigsBuilder("moveit_resources_fanuc").to_dict() test_param = ( ParameterBuilder("moveit_kinematics") .yaml("config/fanuc-kdl-test.yaml") diff --git a/moveit_kinematics/test/launch/fanuc-lma-singular.test.py b/moveit_kinematics/test/launch/fanuc-lma-singular.test.py index ecaa150f1c..133cb6d36e 100644 --- a/moveit_kinematics/test/launch/fanuc-lma-singular.test.py +++ b/moveit_kinematics/test/launch/fanuc-lma-singular.test.py @@ -11,14 +11,7 @@ @pytest.mark.rostest def generate_test_description(): - moveit_configs = ( - MoveItConfigsBuilder("moveit_resources_fanuc") - .robot_description() - .robot_description_semantic() - .robot_description_kinematics() - .joint_limits() - .to_dict() - ) + moveit_configs = MoveItConfigsBuilder("moveit_resources_fanuc").to_dict() test_param = ( ParameterBuilder("moveit_kinematics") .yaml("config/fanuc-lma-singular-test.yaml") diff --git a/moveit_kinematics/test/launch/fanuc-lma.test.py b/moveit_kinematics/test/launch/fanuc-lma.test.py index b176ea0653..a87f28cd92 100644 --- a/moveit_kinematics/test/launch/fanuc-lma.test.py +++ b/moveit_kinematics/test/launch/fanuc-lma.test.py @@ -11,14 +11,7 @@ @pytest.mark.rostest def generate_test_description(): - moveit_configs = ( - MoveItConfigsBuilder("moveit_resources_fanuc") - .robot_description() - .robot_description_semantic() - .robot_description_kinematics() - .joint_limits() - .to_dict() - ) + moveit_configs = MoveItConfigsBuilder("moveit_resources_fanuc").to_dict() test_param = ( ParameterBuilder("moveit_kinematics") .yaml("config/fanuc-lma-test.yaml") diff --git a/moveit_kinematics/test/launch/panda-kdl-singular.test.py b/moveit_kinematics/test/launch/panda-kdl-singular.test.py index d41d830955..5b4fb3a7f7 100644 --- a/moveit_kinematics/test/launch/panda-kdl-singular.test.py +++ b/moveit_kinematics/test/launch/panda-kdl-singular.test.py @@ -11,14 +11,7 @@ @pytest.mark.rostest def generate_test_description(): - moveit_configs = ( - MoveItConfigsBuilder("moveit_resources_panda") - .robot_description() - .robot_description_semantic() - .robot_description_kinematics() - .joint_limits() - .to_dict() - ) + moveit_configs = MoveItConfigsBuilder("moveit_resources_panda").to_dict() test_param = ( ParameterBuilder("moveit_kinematics") diff --git a/moveit_kinematics/test/launch/panda-kdl.test.py b/moveit_kinematics/test/launch/panda-kdl.test.py index 7fcb10490f..14d7c0cd3e 100644 --- a/moveit_kinematics/test/launch/panda-kdl.test.py +++ b/moveit_kinematics/test/launch/panda-kdl.test.py @@ -11,14 +11,7 @@ @pytest.mark.rostest def generate_test_description(): - moveit_configs = ( - MoveItConfigsBuilder("moveit_resources_panda") - .robot_description() - .robot_description_semantic() - .robot_description_kinematics() - .joint_limits() - .to_dict() - ) + moveit_configs = MoveItConfigsBuilder("moveit_resources_panda").to_dict() test_param = ( ParameterBuilder("moveit_kinematics") .yaml("config/panda-kdl-test.yaml") diff --git a/moveit_kinematics/test/launch/panda-lma-singular.test.py b/moveit_kinematics/test/launch/panda-lma-singular.test.py index 5dd2d919a7..f702e80cc9 100644 --- a/moveit_kinematics/test/launch/panda-lma-singular.test.py +++ b/moveit_kinematics/test/launch/panda-lma-singular.test.py @@ -10,14 +10,7 @@ @pytest.mark.rostest def generate_test_description(): - moveit_configs = ( - MoveItConfigsBuilder("moveit_resources_panda") - .robot_description() - .robot_description_semantic() - .robot_description_kinematics() - .joint_limits() - .to_dict() - ) + moveit_configs = MoveItConfigsBuilder("moveit_resources_panda").to_dict() test_param = ( ParameterBuilder("moveit_kinematics") .yaml("config/panda-lma-singular-test.yaml") diff --git a/moveit_kinematics/test/launch/panda-lma.test.py b/moveit_kinematics/test/launch/panda-lma.test.py index 98343c8b24..3d60adbc4c 100644 --- a/moveit_kinematics/test/launch/panda-lma.test.py +++ b/moveit_kinematics/test/launch/panda-lma.test.py @@ -11,14 +11,7 @@ @pytest.mark.rostest def generate_test_description(): - moveit_configs = ( - MoveItConfigsBuilder("moveit_resources_panda") - .robot_description() - .robot_description_semantic() - .robot_description_kinematics() - .joint_limits() - .to_dict() - ) + moveit_configs = MoveItConfigsBuilder("moveit_resources_panda").to_dict() test_param = ( ParameterBuilder("moveit_kinematics") .yaml("config/panda-lma-test.yaml") diff --git a/moveit_ros/benchmarks/examples/demo_panda.launch.py b/moveit_ros/benchmarks/examples/demo_panda.launch.py index 44433931b9..d6f7582899 100644 --- a/moveit_ros/benchmarks/examples/demo_panda.launch.py +++ b/moveit_ros/benchmarks/examples/demo_panda.launch.py @@ -15,15 +15,7 @@ def generate_launch_description(): .to_dict() ) - moveit_configs = ( - MoveItConfigsBuilder("moveit_resources_panda") - .robot_description() - .robot_description_semantic() - .robot_description_kinematics() - .joint_limits() - .planning_pipelines() - .to_dict() - ) + moveit_configs = MoveItConfigsBuilder("moveit_resources_panda").to_dict() # moveit_ros_benchmark demo executable moveit_ros_benchmarks_node = Node( 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 31ba2105bc..3d284f323b 100644 --- a/moveit_ros/benchmarks/examples/demo_panda_predefined_poses.launch.py +++ b/moveit_ros/benchmarks/examples/demo_panda_predefined_poses.launch.py @@ -15,15 +15,7 @@ def generate_launch_description(): .to_dict() ) - moveit_configs = ( - MoveItConfigsBuilder("moveit_resources_panda") - .robot_description() - .robot_description_semantic() - .robot_description_kinematics() - .joint_limits() - .planning_pipelines() - .to_dict() - ) + moveit_configs = MoveItConfigsBuilder("moveit_resources_panda").to_dict() # moveit_ros_benchmark demo executable moveit_ros_benchmarks_node = Node( 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 afc606f5af..b085798b6c 100644 --- a/moveit_ros/moveit_servo/launch/pose_tracking_example.launch.py +++ b/moveit_ros/moveit_servo/launch/pose_tracking_example.launch.py @@ -11,9 +11,6 @@ def generate_launch_description(): moveit_config = ( MoveItConfigsBuilder("moveit_resources_panda") .robot_description(file_path="config/panda.urdf.xacro") - .robot_description_semantic() - .robot_description_kinematics() - .joint_limits() .to_moveit_configs() ) 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 68c30e7b54..2481b232c6 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 @@ -40,9 +40,6 @@ def generate_servo_test_description( .robot_description( file_path="config/panda.urdf.xacro", mappings=robot_description_mappings ) - .robot_description_semantic() - .robot_description_kinematics() - .joint_limits() .to_moveit_configs() ) 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 2cc89aced8..3215969e49 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 @@ -19,9 +19,6 @@ def generate_servo_test_description(*args, gtest_name: SomeSubstitutionsType): moveit_config = ( MoveItConfigsBuilder("moveit_resources_panda") .robot_description(file_path="config/panda.urdf.xacro") - .robot_description_semantic() - .robot_description_kinematics() - .joint_limits() .to_moveit_configs() ) 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 13915dde1b..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 @@ -14,12 +14,7 @@ def generate_move_group_test_description(*args, gtest_name: SomeSubstitutionsTyp moveit_config = ( MoveItConfigsBuilder("moveit_resources_panda") .robot_description(file_path="config/panda.urdf.xacro") - .robot_description_semantic() - .robot_description_kinematics() - .joint_limits() .trajectory_execution(file_path="config/panda_gripper_controllers.yaml") - .planning_scene_monitor() - .planning_pipelines() .to_moveit_configs() ) From 4df29302e4a8bd3928ef83a9b6ae808b7d5acee1 Mon Sep 17 00:00:00 2001 From: Jafar Abdi Date: Fri, 31 Dec 2021 09:46:42 +0300 Subject: [PATCH 35/39] Update moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py Co-authored-by: AndyZe --- .../moveit_configs_utils/moveit_configs_builder.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py index 657c86a4c8..779ac93801 100644 --- a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py +++ b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py @@ -90,7 +90,7 @@ def __init__(self): self.move_group_capabilities = {} # A dictionary containing the overridden position/velocity/acceleration limits. self.joint_limits = {} - # A dictionary MoveItCpp related parameters. + # A dictionary containing MoveItCpp related parameters. self.moveit_cpp = {} # A dictionary containing the cartesian limits for the Pilz planner. self.cartesian_limits = {} From 2d29ebbe81834f260340de2c3f09d4bb4a38f458 Mon Sep 17 00:00:00 2001 From: Jafar Abdi Date: Fri, 31 Dec 2021 09:46:48 +0300 Subject: [PATCH 36/39] Update moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py Co-authored-by: AndyZe --- .../moveit_configs_utils/moveit_configs_builder.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py index 779ac93801..8fd0b9b949 100644 --- a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py +++ b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py @@ -74,9 +74,9 @@ class MoveItConfigs(object): ] def __init__(self): - # A dictionary that have the content of the URDF file. + # A dictionary that has the contents of the URDF file. self.robot_description = {} - # A dictionary that have the content of the SRDF file. + # A dictionary that has the contents of the SRDF file. self.robot_description_semantic = {} # A dictionary IK solver specific parameters. self.robot_description_kinematics = {} From 0159e48fc07d6b4b1760ebdfa463bec60285a74e Mon Sep 17 00:00:00 2001 From: AndyZe Date: Fri, 14 Jan 2022 11:11:42 -0600 Subject: [PATCH 37/39] Rename robot_name_moveit_configs Co-authored-by: Stephanie Eng --- .../moveit_configs_builder.py | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py index 8fd0b9b949..0b9987d402 100644 --- a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py +++ b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py @@ -5,7 +5,7 @@ By default it expects the following structure for the moveit configs package -robot_name_moveit_configs/ +robot_name_moveit_config/ .setup_assistant -> Used to retrieve information about the SRDF file and the URDF file used when generating the package config/ @@ -44,7 +44,7 @@ .to_moveit_configs() # Or moveit_configs = MoveItConfigsBuilder("robot_name") - # Absolute path to robot_name_moveit_configs + # Absolute path to robot_name_moveit_config .robot_description_semantic(Path.home() / "my_config" / "new_file.srdf") .to_moveit_configs() """ @@ -195,7 +195,7 @@ class MoveItConfigsBuilder(ParameterBuilder): __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_configs + # 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" @@ -243,7 +243,7 @@ def __init__(self, robot_name: str, 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_configs). + :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. """ @@ -263,7 +263,7 @@ def robot_description_semantic( ): """Load semantic robot description. - :param file_path: Absolute or relative path to the SRDF file (w.r.t. robot_name_moveit_configs). + :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. """ @@ -279,7 +279,7 @@ def robot_description_semantic( 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_configs). + :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 = { @@ -294,7 +294,7 @@ def robot_description_kinematics(self, file_path: Optional[str] = None): 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_configs). + :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 = { @@ -309,7 +309,7 @@ def joint_limits(self, file_path: Optional[str] = None): 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_configs). + :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( @@ -325,7 +325,7 @@ def trajectory_execution( ): """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_configs). + :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. """ @@ -393,7 +393,7 @@ def planning_pipelines( 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_configs). + :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 = { @@ -406,7 +406,7 @@ def cartesian_limits(self, file_path: Optional[str] = None): return self def to_moveit_configs(self): - """Get MoveIt configs from robot_name_moveit_configs. + """Get MoveIt configs from robot_name_moveit_config. :return: An MoveItConfigs instance with all parameters loaded. """ @@ -433,7 +433,7 @@ def to_moveit_configs(self): return self.__moveit_configs def to_dict(self, include_moveit_configs: bool = True): - """Get loaded parameters from robot_name_moveit_configs as a dictionary. + """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 From 51445645da8e9515e18739a141243d098668fce2 Mon Sep 17 00:00:00 2001 From: Jafar Abdi Date: Thu, 20 Jan 2022 16:40:33 -0700 Subject: [PATCH 38/39] Update moveit2.repos --- moveit2.repos | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit2.repos b/moveit2.repos index 4fa1ba3af0..b0bf360af7 100644 --- a/moveit2.repos +++ b/moveit2.repos @@ -11,7 +11,7 @@ repositories: type: git url: https://github.com/JafarAbdi/moveit_resources version: pr-moveit_configs_utils - parameter_tools: + launch_param_builder: type: git - url: https://github.com/PickNikRobotics/parameter_tools + url: https://github.com/PickNikRobotics/launch_param_builder version: main From 28b6849a61d897e9e77133faa1b7c7f608da9a4e Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Fri, 21 Jan 2022 08:48:11 -0700 Subject: [PATCH 39/39] Fix package name --- .../moveit_configs_utils/moveit_configs_builder.py | 2 +- moveit_configs_utils/package.xml | 2 +- moveit_kinematics/package.xml | 2 +- moveit_kinematics/test/launch/fanuc-kdl-singular.test.py | 2 +- moveit_kinematics/test/launch/fanuc-kdl.test.py | 2 +- moveit_kinematics/test/launch/fanuc-lma-singular.test.py | 2 +- moveit_kinematics/test/launch/fanuc-lma.test.py | 2 +- moveit_kinematics/test/launch/panda-kdl-singular.test.py | 2 +- moveit_kinematics/test/launch/panda-kdl.test.py | 2 +- moveit_kinematics/test/launch/panda-lma-singular.test.py | 2 +- moveit_kinematics/test/launch/panda-lma.test.py | 2 +- moveit_planners/pilz_industrial_motion_planner/package.xml | 2 +- .../test/unit_tests/launch/common_parameters.py | 2 +- moveit_ros/benchmarks/examples/demo_panda.launch.py | 2 +- .../benchmarks/examples/demo_panda_predefined_poses.launch.py | 2 +- moveit_ros/benchmarks/package.xml | 2 +- moveit_ros/moveit_servo/launch/pose_tracking_example.launch.py | 2 +- moveit_ros/moveit_servo/package.xml | 2 +- moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py | 2 +- .../moveit_servo/test/launch/test_servo_pose_tracking.test.py | 2 +- .../moveit_servo/test/launch/unit_test_servo_calcs.test.py | 2 +- 21 files changed, 21 insertions(+), 21 deletions(-) diff --git a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py index 0b9987d402..d5304a79de 100644 --- a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py +++ b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py @@ -54,7 +54,7 @@ import logging from ament_index_python.packages import get_package_share_directory -from parameter_builder import ParameterBuilder, load_yaml, load_xacro +from launch_param_builder import ParameterBuilder, load_yaml, load_xacro class MoveItConfigs(object): diff --git a/moveit_configs_utils/package.xml b/moveit_configs_utils/package.xml index 427ea3b45a..b1b7075397 100644 --- a/moveit_configs_utils/package.xml +++ b/moveit_configs_utils/package.xml @@ -12,7 +12,7 @@ ament_lint_common ament_index_python - parameter_builder + launch_param_builder ament_python diff --git a/moveit_kinematics/package.xml b/moveit_kinematics/package.xml index f525a67785..dff7eb2fc3 100644 --- a/moveit_kinematics/package.xml +++ b/moveit_kinematics/package.xml @@ -48,7 +48,7 @@ ament_lint_common ros_testing moveit_configs_utils - parameter_builder + 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 628fc26cc0..aeb6769fcc 100644 --- a/moveit_kinematics/test/launch/fanuc-kdl-singular.test.py +++ b/moveit_kinematics/test/launch/fanuc-kdl-singular.test.py @@ -5,7 +5,7 @@ from launch_ros.actions import Node from launch_testing.util import KeepAliveProc from moveit_configs_utils import MoveItConfigsBuilder -from parameter_builder import ParameterBuilder +from launch_param_builder import ParameterBuilder @pytest.mark.rostest diff --git a/moveit_kinematics/test/launch/fanuc-kdl.test.py b/moveit_kinematics/test/launch/fanuc-kdl.test.py index 84dbea11ba..0cac80990c 100644 --- a/moveit_kinematics/test/launch/fanuc-kdl.test.py +++ b/moveit_kinematics/test/launch/fanuc-kdl.test.py @@ -5,7 +5,7 @@ from launch_ros.actions import Node from launch_testing.util import KeepAliveProc from moveit_configs_utils import MoveItConfigsBuilder -from parameter_builder import ParameterBuilder +from launch_param_builder import ParameterBuilder @pytest.mark.rostest diff --git a/moveit_kinematics/test/launch/fanuc-lma-singular.test.py b/moveit_kinematics/test/launch/fanuc-lma-singular.test.py index 133cb6d36e..1a190ab2e1 100644 --- a/moveit_kinematics/test/launch/fanuc-lma-singular.test.py +++ b/moveit_kinematics/test/launch/fanuc-lma-singular.test.py @@ -5,7 +5,7 @@ from launch_ros.actions import Node from launch_testing.util import KeepAliveProc from moveit_configs_utils import MoveItConfigsBuilder -from parameter_builder import ParameterBuilder +from launch_param_builder import ParameterBuilder @pytest.mark.rostest diff --git a/moveit_kinematics/test/launch/fanuc-lma.test.py b/moveit_kinematics/test/launch/fanuc-lma.test.py index a87f28cd92..82a6169a66 100644 --- a/moveit_kinematics/test/launch/fanuc-lma.test.py +++ b/moveit_kinematics/test/launch/fanuc-lma.test.py @@ -5,7 +5,7 @@ from launch_ros.actions import Node from launch_testing.util import KeepAliveProc from moveit_configs_utils import MoveItConfigsBuilder -from parameter_builder import ParameterBuilder +from launch_param_builder import ParameterBuilder @pytest.mark.rostest diff --git a/moveit_kinematics/test/launch/panda-kdl-singular.test.py b/moveit_kinematics/test/launch/panda-kdl-singular.test.py index 5b4fb3a7f7..0e3e41caf6 100644 --- a/moveit_kinematics/test/launch/panda-kdl-singular.test.py +++ b/moveit_kinematics/test/launch/panda-kdl-singular.test.py @@ -5,7 +5,7 @@ from launch_ros.actions import Node from launch_testing.util import KeepAliveProc from moveit_configs_utils import MoveItConfigsBuilder -from parameter_builder import ParameterBuilder +from launch_param_builder import ParameterBuilder @pytest.mark.rostest diff --git a/moveit_kinematics/test/launch/panda-kdl.test.py b/moveit_kinematics/test/launch/panda-kdl.test.py index 14d7c0cd3e..2fea5a2680 100644 --- a/moveit_kinematics/test/launch/panda-kdl.test.py +++ b/moveit_kinematics/test/launch/panda-kdl.test.py @@ -5,7 +5,7 @@ from launch_ros.actions import Node from launch_testing.util import KeepAliveProc from moveit_configs_utils import MoveItConfigsBuilder -from parameter_builder import ParameterBuilder +from launch_param_builder import ParameterBuilder @pytest.mark.rostest diff --git a/moveit_kinematics/test/launch/panda-lma-singular.test.py b/moveit_kinematics/test/launch/panda-lma-singular.test.py index f702e80cc9..1d69248260 100644 --- a/moveit_kinematics/test/launch/panda-lma-singular.test.py +++ b/moveit_kinematics/test/launch/panda-lma-singular.test.py @@ -5,7 +5,7 @@ from launch_ros.actions import Node from launch_testing.util import KeepAliveProc from moveit_configs_utils import MoveItConfigsBuilder -from parameter_builder import ParameterBuilder +from launch_param_builder import ParameterBuilder @pytest.mark.rostest diff --git a/moveit_kinematics/test/launch/panda-lma.test.py b/moveit_kinematics/test/launch/panda-lma.test.py index 3d60adbc4c..325674f90f 100644 --- a/moveit_kinematics/test/launch/panda-lma.test.py +++ b/moveit_kinematics/test/launch/panda-lma.test.py @@ -5,7 +5,7 @@ from launch_ros.actions import Node from launch_testing.util import KeepAliveProc from moveit_configs_utils import MoveItConfigsBuilder -from parameter_builder import ParameterBuilder +from launch_param_builder import ParameterBuilder @pytest.mark.rostest 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 d6f7582899..ceb13704c9 100644 --- a/moveit_ros/benchmarks/examples/demo_panda.launch.py +++ b/moveit_ros/benchmarks/examples/demo_panda.launch.py @@ -1,7 +1,7 @@ from launch import LaunchDescription from launch_ros.actions import Node from moveit_configs_utils import MoveItConfigsBuilder -from parameter_builder import ParameterBuilder +from launch_param_builder import ParameterBuilder def generate_launch_description(): 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 3d284f323b..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,7 +1,7 @@ from launch import LaunchDescription from launch_ros.actions import Node from moveit_configs_utils import MoveItConfigsBuilder -from parameter_builder import ParameterBuilder +from launch_param_builder import ParameterBuilder def generate_launch_description(): diff --git a/moveit_ros/benchmarks/package.xml b/moveit_ros/benchmarks/package.xml index e5f87edaad..cffcc2b7fa 100644 --- a/moveit_ros/benchmarks/package.xml +++ b/moveit_ros/benchmarks/package.xml @@ -32,7 +32,7 @@ libboost-date-time libboost-filesystem moveit_configs_utils - parameter_builder + 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 b085798b6c..735608c89f 100644 --- a/moveit_ros/moveit_servo/launch/pose_tracking_example.launch.py +++ b/moveit_ros/moveit_servo/launch/pose_tracking_example.launch.py @@ -4,7 +4,7 @@ from launch_ros.actions import Node from ament_index_python.packages import get_package_share_directory from moveit_configs_utils import MoveItConfigsBuilder -from parameter_builder import ParameterBuilder +from launch_param_builder import ParameterBuilder def generate_launch_description(): diff --git a/moveit_ros/moveit_servo/package.xml b/moveit_ros/moveit_servo/package.xml index 8a93d0db64..8207919666 100644 --- a/moveit_ros/moveit_servo/package.xml +++ b/moveit_ros/moveit_servo/package.xml @@ -43,7 +43,7 @@ robot_state_publisher tf2_ros moveit_configs_utils - parameter_builder + 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 2481b232c6..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 @@ -10,7 +10,7 @@ from ament_index_python.packages import get_package_share_directory from launch.actions import ExecuteProcess, TimerAction from moveit_configs_utils import MoveItConfigsBuilder -from parameter_builder import ParameterBuilder +from launch_param_builder import ParameterBuilder def generate_servo_test_description( 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 3215969e49..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 @@ -12,7 +12,7 @@ from launch.some_substitutions_type import SomeSubstitutionsType from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from moveit_configs_utils import MoveItConfigsBuilder -from parameter_builder import ParameterBuilder +from launch_param_builder import ParameterBuilder def generate_servo_test_description(*args, gtest_name: SomeSubstitutionsType): 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 5b42c42553..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,7 +4,7 @@ import os import sys import unittest -from parameter_builder import ParameterBuilder +from launch_param_builder import ParameterBuilder sys.path.append(os.path.dirname(__file__))