Use moveit_configs_utils for launch files (#365)

.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:

.github/workflows/lsan.suppressions

@@ -1 +1,3 @@
 leak:class_loader
+leak:rviz_default_plugins
+leak:static_transform_broadcaster_node

core/package.xml

@@ -21,6 +21,7 @@
 
 	<test_depend>ament_cmake_gmock</test_depend>
 	<test_depend>ament_cmake_gtest</test_depend>
+	<test_depend>moveit_configs_utils</test_depend>
 	<!-- TODO(JafarAbdi): Enable after porting integration tests-->
 	<!-- test_depend>launch</test_depend -->
 	<!-- test_depend>launch_testing</test_depend -->

core/test/move_to.launch.py

@@ -1,84 +1,34 @@
 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=[
             LaunchConfiguration("test_binary"),
         ],
         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,
         ],
     )
 

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"
-    )
-    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"
+    moveit_config = (
+        MoveItConfigsBuilder("moveit_resources_panda")
+        .robot_description(file_path="config/panda.urdf.xacro")
+        .planning_pipelines(pipelines=["ompl"])
+        .to_moveit_configs()
     )
-    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,
         ],
     )
 

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])