diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 49b3e4f0..c37d6caf 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -15,6 +15,10 @@ jobs: matrix: env: # TODO: We have to use -Wno-redundant-decls since rosidl_generator_c is generating broken headers + - IMAGE: humble-source + CXXFLAGS: >- + -Werror -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wno-redundant-decls + -Wno-unused-parameter -Wno-unused-function -Wno-deprecated-copy -Wno-unused-but-set-parameter - IMAGE: rolling-source NAME: ccov TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Debug -DCMAKE_CXX_FLAGS="--coverage" @@ -34,6 +38,7 @@ jobs: DOCKER_RUN_OPTS: >- -e PRELOAD=libasan.so.5 -e LSAN_OPTIONS="suppressions=$PWD/.github/workflows/lsan.suppressions,fast_unwind_on_malloc=0" + -e ASAN_OPTIONS="new_delete_type_mismatch=0,alloc_dealloc_mismatch=0" TARGET_CMAKE_ARGS: -DCMAKE_CXX_FLAGS="-fsanitize=address -fno-omit-frame-pointer -O1" env: diff --git a/.github/workflows/lsan.suppressions b/.github/workflows/lsan.suppressions index 79bf7e06..3164a5a4 100644 --- a/.github/workflows/lsan.suppressions +++ b/.github/workflows/lsan.suppressions @@ -1 +1,3 @@ leak:class_loader +leak:rviz_default_plugins +leak:static_transform_broadcaster_node diff --git a/core/package.xml b/core/package.xml index ff9c20bd..cb73f80d 100644 --- a/core/package.xml +++ b/core/package.xml @@ -21,6 +21,7 @@ ament_cmake_gmock ament_cmake_gtest + moveit_configs_utils diff --git a/core/test/move_to.launch.py b/core/test/move_to.launch.py index 103a20cf..023283be 100644 --- a/core/test/move_to.launch.py +++ b/core/test/move_to.launch.py @@ -1,73 +1,23 @@ import unittest -import os -import yaml +import launch_testing +import pytest from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration -import launch_testing -from launch_testing.asserts import assertExitCodes -from launch_testing.util import KeepAliveProc -from launch_testing.actions import ReadyToTest -from ament_index_python.packages import get_package_share_directory from launch_ros.actions import Node - -import pytest - -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 launch_testing.actions import ReadyToTest +from launch_testing.util import KeepAliveProc +from moveit_configs_utils import MoveItConfigsBuilder @pytest.mark.launch_test def generate_test_description(): - # planning_context - robot_description_config = xacro.process_file( - os.path.join( - get_package_share_directory("moveit_resources_panda_moveit_config"), - "config", - "panda.urdf.xacro", - ) + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .to_moveit_configs() ) - robot_description = {"robot_description": robot_description_config.toxml()} - - robot_description_semantic_config = load_file( - "moveit_resources_panda_moveit_config", "config/panda.srdf" - ) - robot_description_semantic = { - "robot_description_semantic": robot_description_semantic_config - } - - kinematics_yaml = load_yaml( - "moveit_resources_panda_moveit_config", "config/kinematics.yaml" - ) - robot_description_kinematics = {"robot_description_kinematics": kinematics_yaml} - - robot_description_planning = { - "robot_description_planning": load_yaml( - "moveit_resources_panda_moveit_config", "config/joint_limits.yaml" - ) - } test_exec = Node( executable=[ @@ -75,10 +25,10 @@ def generate_test_description(): ], output="screen", parameters=[ - robot_description, - robot_description_semantic, - robot_description_kinematics, - robot_description_planning, + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + moveit_config.joint_limits, ], ) diff --git a/demo/launch/alternative_path_costs.launch.py b/demo/launch/alternative_path_costs.launch.py index e72e8ceb..4e465dd4 100644 --- a/demo/launch/alternative_path_costs.launch.py +++ b/demo/launch/alternative_path_costs.launch.py @@ -1,72 +1,25 @@ -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 def generate_launch_description(): - # Get URDF and SRDF - robot_description_config = load_file( - "moveit_resources_panda_description", "urdf/panda.urdf" + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .planning_pipelines(pipelines=["ompl"]) + .to_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" - ) - - # 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_pipeline_config["move_group"].update(ompl_planning_yaml) cartesian_task = Node( package="moveit_task_constructor_demo", executable="alternative_path_costs", output="screen", parameters=[ - robot_description, - robot_description_semantic, - kinematics_yaml, - ompl_planning_pipeline_config, + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + moveit_config.planning_pipelines, ], ) diff --git a/demo/launch/cartesian.launch.py b/demo/launch/cartesian.launch.py index 8773ece6..fe2db9ac 100644 --- a/demo/launch/cartesian.launch.py +++ b/demo/launch/cartesian.launch.py @@ -1,55 +1,24 @@ -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 def generate_launch_description(): - # Get URDF and SRDF - 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_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .to_moveit_configs() ) cartesian_task = Node( package="moveit_task_constructor_demo", executable="cartesian", output="screen", - parameters=[robot_description, robot_description_semantic, kinematics_yaml], + parameters=[ + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + ], ) return LaunchDescription([cartesian_task]) diff --git a/demo/launch/demo.launch.py b/demo/launch/demo.launch.py index 2756703a..a757a7d6 100644 --- a/demo/launch/demo.launch.py +++ b/demo/launch/demo.launch.py @@ -1,91 +1,20 @@ import os -import yaml + +from ament_index_python.packages import get_package_share_directory 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 def generate_launch_description(): - # planning_context - 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") + .planning_pipelines(pipelines=["ompl"]) + .robot_description(file_path="config/panda.urdf.xacro") + .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml") + .to_moveit_configs() ) - robot_description = {"robot_description": robot_description_config.toxml()} - - robot_description_semantic_config = load_file( - "moveit_resources_panda_moveit_config", "config/panda.srdf" - ) - robot_description_semantic = { - "robot_description_semantic": robot_description_semantic_config - } - - kinematics_yaml = load_yaml( - "moveit_resources_panda_moveit_config", "config/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_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.2, - "trajectory_execution.allowed_goal_duration_margin": 0.5, - "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, - } # Load ExecuteTaskSolutionCapability so we can execute found solutions in simulation move_group_capabilities = { @@ -98,13 +27,7 @@ def generate_launch_description(): 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, + moveit_config.to_dict(), move_group_capabilities, ], ) @@ -120,10 +43,10 @@ def generate_launch_description(): output="log", arguments=["-d", rviz_config_file], parameters=[ - robot_description, - robot_description_semantic, - ompl_planning_pipeline_config, - kinematics_yaml, + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + moveit_config.planning_pipelines, ], ) @@ -142,23 +65,20 @@ def generate_launch_description(): executable="robot_state_publisher", name="robot_state_publisher", output="both", - parameters=[robot_description], + parameters=[moveit_config.robot_description], ) # ros2_control using FakeSystem as hardware ros2_controllers_path = os.path.join( get_package_share_directory("moveit_resources_panda_moveit_config"), "config", - "panda_ros_controllers.yaml", + "ros2_controllers.yaml", ) ros2_control_node = Node( package="controller_manager", executable="ros2_control_node", - parameters=[robot_description, ros2_controllers_path], - output={ - "stdout": "screen", - "stderr": "screen", - }, + parameters=[moveit_config.robot_description, ros2_controllers_path], + output="both", ) # Load controllers @@ -170,7 +90,7 @@ def generate_launch_description(): ]: load_controllers += [ ExecuteProcess( - cmd=["ros2 run controller_manager spawner.py {}".format(controller)], + cmd=["ros2 run controller_manager spawner {}".format(controller)], shell=True, output="screen", ) diff --git a/demo/launch/ik_clearance_cost.launch.py b/demo/launch/ik_clearance_cost.launch.py index e97f2a8b..9d2b826d 100644 --- a/demo/launch/ik_clearance_cost.launch.py +++ b/demo/launch/ik_clearance_cost.launch.py @@ -1,55 +1,24 @@ -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 def generate_launch_description(): - # Get URDF and SRDF - 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_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .to_moveit_configs() ) cartesian_task = Node( package="moveit_task_constructor_demo", executable="ik_clearance_cost", output="screen", - parameters=[robot_description, robot_description_semantic, kinematics_yaml], + parameters=[ + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + ], ) return LaunchDescription([cartesian_task]) diff --git a/demo/launch/modular.launch.py b/demo/launch/modular.launch.py index 673dfe97..a4bd7bb2 100644 --- a/demo/launch/modular.launch.py +++ b/demo/launch/modular.launch.py @@ -1,55 +1,24 @@ -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 def generate_launch_description(): - # Get URDF and SRDF - 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_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .to_moveit_configs() ) modular_task = Node( package="moveit_task_constructor_demo", executable="modular", output="screen", - parameters=[robot_description, robot_description_semantic, kinematics_yaml], + parameters=[ + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + ], ) return LaunchDescription([modular_task]) diff --git a/demo/launch/pickplace.launch.py b/demo/launch/pickplace.launch.py index ebf16e91..d332dbdb 100644 --- a/demo/launch/pickplace.launch.py +++ b/demo/launch/pickplace.launch.py @@ -1,62 +1,19 @@ import os -import yaml + +from ament_index_python.packages import get_package_share_directory 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 def generate_launch_description(): - # Get URDF and SRDF - robot_description_config = load_file( - "moveit_resources_panda_description", "urdf/panda.urdf" + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .planning_pipelines(pipelines=["ompl"]) + .robot_description(file_path="config/panda.urdf.xacro") + .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml") + .to_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" - ) - - # 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_pipeline_config["move_group"].update(ompl_planning_yaml) pick_place_demo = Node( package="moveit_task_constructor_demo", @@ -68,10 +25,11 @@ def generate_launch_description(): "config", "panda_config.yaml", ), - robot_description, - robot_description_semantic, - kinematics_yaml, - ompl_planning_pipeline_config, + 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/demo/package.xml b/demo/package.xml index b6530c09..6bc46923 100644 --- a/demo/package.xml +++ b/demo/package.xml @@ -15,6 +15,7 @@ moveit_ros_planning_interface moveit_task_constructor_core rosparam_shortcuts + moveit_configs_utils moveit_resources_panda_moveit_config moveit_task_constructor_capabilities