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