Use moveit_configs_utils for launch files (#365)

This commit is contained in:
Jafar 2022-05-26 19:29:08 +03:00 committed by GitHub
parent d1fb6ace93
commit 0128cd9250
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
11 changed files with 95 additions and 398 deletions

View File

@ -15,6 +15,10 @@ jobs:
matrix: matrix:
env: env:
# TODO: We have to use -Wno-redundant-decls since rosidl_generator_c is generating broken headers # 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 - IMAGE: rolling-source
NAME: ccov NAME: ccov
TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Debug -DCMAKE_CXX_FLAGS="--coverage" TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Debug -DCMAKE_CXX_FLAGS="--coverage"
@ -34,6 +38,7 @@ jobs:
DOCKER_RUN_OPTS: >- DOCKER_RUN_OPTS: >-
-e PRELOAD=libasan.so.5 -e PRELOAD=libasan.so.5
-e LSAN_OPTIONS="suppressions=$PWD/.github/workflows/lsan.suppressions,fast_unwind_on_malloc=0" -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" TARGET_CMAKE_ARGS: -DCMAKE_CXX_FLAGS="-fsanitize=address -fno-omit-frame-pointer -O1"
env: env:

View File

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

View File

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

View File

@ -1,73 +1,23 @@
import unittest import unittest
import os
import yaml
import launch_testing
import pytest
from launch import LaunchDescription from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration 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 from launch_ros.actions import Node
from launch_testing.actions import ReadyToTest
import pytest from launch_testing.util import KeepAliveProc
from moveit_configs_utils import MoveItConfigsBuilder
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
@pytest.mark.launch_test @pytest.mark.launch_test
def generate_test_description(): def generate_test_description():
# planning_context moveit_config = (
robot_description_config = xacro.process_file( MoveItConfigsBuilder("moveit_resources_panda")
os.path.join( .robot_description(file_path="config/panda.urdf.xacro")
get_package_share_directory("moveit_resources_panda_moveit_config"), .to_moveit_configs()
"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"
)
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( test_exec = Node(
executable=[ executable=[
@ -75,10 +25,10 @@ def generate_test_description():
], ],
output="screen", output="screen",
parameters=[ parameters=[
robot_description, moveit_config.robot_description,
robot_description_semantic, moveit_config.robot_description_semantic,
robot_description_kinematics, moveit_config.robot_description_kinematics,
robot_description_planning, moveit_config.joint_limits,
], ],
) )

View File

@ -1,72 +1,25 @@
import os
import yaml
from launch import LaunchDescription from launch import LaunchDescription
from launch_ros.actions import Node from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory from moveit_configs_utils import MoveItConfigsBuilder
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 generate_launch_description(): def generate_launch_description():
# Get URDF and SRDF moveit_config = (
robot_description_config = load_file( MoveItConfigsBuilder("moveit_resources_panda")
"moveit_resources_panda_description", "urdf/panda.urdf" .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( cartesian_task = Node(
package="moveit_task_constructor_demo", package="moveit_task_constructor_demo",
executable="alternative_path_costs", executable="alternative_path_costs",
output="screen", output="screen",
parameters=[ parameters=[
robot_description, moveit_config.robot_description,
robot_description_semantic, moveit_config.robot_description_semantic,
kinematics_yaml, moveit_config.robot_description_kinematics,
ompl_planning_pipeline_config, moveit_config.planning_pipelines,
], ],
) )

View File

@ -1,55 +1,24 @@
import os
import yaml
from launch import LaunchDescription from launch import LaunchDescription
from launch_ros.actions import Node from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory from moveit_configs_utils import MoveItConfigsBuilder
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 generate_launch_description(): def generate_launch_description():
# Get URDF and SRDF moveit_config = (
robot_description_config = load_file( MoveItConfigsBuilder("moveit_resources_panda")
"moveit_resources_panda_description", "urdf/panda.urdf" .robot_description(file_path="config/panda.urdf.xacro")
) .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"
) )
cartesian_task = Node( cartesian_task = Node(
package="moveit_task_constructor_demo", package="moveit_task_constructor_demo",
executable="cartesian", executable="cartesian",
output="screen", 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]) return LaunchDescription([cartesian_task])

View File

@ -1,91 +1,20 @@
import os import os
import yaml
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription from launch import LaunchDescription
from launch.actions import ExecuteProcess from launch.actions import ExecuteProcess
from launch_ros.actions import Node from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory from moveit_configs_utils import MoveItConfigsBuilder
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
def generate_launch_description(): def generate_launch_description():
# planning_context moveit_config = (
robot_description_config = xacro.process_file( MoveItConfigsBuilder("moveit_resources_panda")
os.path.join( .planning_pipelines(pipelines=["ompl"])
get_package_share_directory("moveit_resources_panda_moveit_config"), .robot_description(file_path="config/panda.urdf.xacro")
"config", .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml")
"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"
)
# 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 # Load ExecuteTaskSolutionCapability so we can execute found solutions in simulation
move_group_capabilities = { move_group_capabilities = {
@ -98,13 +27,7 @@ def generate_launch_description():
executable="move_group", executable="move_group",
output="screen", output="screen",
parameters=[ parameters=[
robot_description, moveit_config.to_dict(),
robot_description_semantic,
kinematics_yaml,
ompl_planning_pipeline_config,
trajectory_execution,
moveit_controllers,
planning_scene_monitor_parameters,
move_group_capabilities, move_group_capabilities,
], ],
) )
@ -120,10 +43,10 @@ def generate_launch_description():
output="log", output="log",
arguments=["-d", rviz_config_file], arguments=["-d", rviz_config_file],
parameters=[ parameters=[
robot_description, moveit_config.robot_description,
robot_description_semantic, moveit_config.robot_description_semantic,
ompl_planning_pipeline_config, moveit_config.robot_description_kinematics,
kinematics_yaml, moveit_config.planning_pipelines,
], ],
) )
@ -142,23 +65,20 @@ def generate_launch_description():
executable="robot_state_publisher", executable="robot_state_publisher",
name="robot_state_publisher", name="robot_state_publisher",
output="both", output="both",
parameters=[robot_description], parameters=[moveit_config.robot_description],
) )
# ros2_control using FakeSystem as hardware # ros2_control using FakeSystem as hardware
ros2_controllers_path = os.path.join( ros2_controllers_path = os.path.join(
get_package_share_directory("moveit_resources_panda_moveit_config"), get_package_share_directory("moveit_resources_panda_moveit_config"),
"config", "config",
"panda_ros_controllers.yaml", "ros2_controllers.yaml",
) )
ros2_control_node = Node( ros2_control_node = Node(
package="controller_manager", package="controller_manager",
executable="ros2_control_node", executable="ros2_control_node",
parameters=[robot_description, ros2_controllers_path], parameters=[moveit_config.robot_description, ros2_controllers_path],
output={ output="both",
"stdout": "screen",
"stderr": "screen",
},
) )
# Load controllers # Load controllers
@ -170,7 +90,7 @@ def generate_launch_description():
]: ]:
load_controllers += [ load_controllers += [
ExecuteProcess( ExecuteProcess(
cmd=["ros2 run controller_manager spawner.py {}".format(controller)], cmd=["ros2 run controller_manager spawner {}".format(controller)],
shell=True, shell=True,
output="screen", output="screen",
) )

View File

@ -1,55 +1,24 @@
import os
import yaml
from launch import LaunchDescription from launch import LaunchDescription
from launch_ros.actions import Node from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory from moveit_configs_utils import MoveItConfigsBuilder
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 generate_launch_description(): def generate_launch_description():
# Get URDF and SRDF moveit_config = (
robot_description_config = load_file( MoveItConfigsBuilder("moveit_resources_panda")
"moveit_resources_panda_description", "urdf/panda.urdf" .robot_description(file_path="config/panda.urdf.xacro")
) .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"
) )
cartesian_task = Node( cartesian_task = Node(
package="moveit_task_constructor_demo", package="moveit_task_constructor_demo",
executable="ik_clearance_cost", executable="ik_clearance_cost",
output="screen", 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]) return LaunchDescription([cartesian_task])

View File

@ -1,55 +1,24 @@
import os
import yaml
from launch import LaunchDescription from launch import LaunchDescription
from launch_ros.actions import Node from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory from moveit_configs_utils import MoveItConfigsBuilder
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 generate_launch_description(): def generate_launch_description():
# Get URDF and SRDF moveit_config = (
robot_description_config = load_file( MoveItConfigsBuilder("moveit_resources_panda")
"moveit_resources_panda_description", "urdf/panda.urdf" .robot_description(file_path="config/panda.urdf.xacro")
) .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"
) )
modular_task = Node( modular_task = Node(
package="moveit_task_constructor_demo", package="moveit_task_constructor_demo",
executable="modular", executable="modular",
output="screen", 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]) return LaunchDescription([modular_task])

View File

@ -1,62 +1,19 @@
import os import os
import yaml
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription from launch import LaunchDescription
from launch_ros.actions import Node from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory from moveit_configs_utils import MoveItConfigsBuilder
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 generate_launch_description(): def generate_launch_description():
# Get URDF and SRDF moveit_config = (
robot_description_config = load_file( MoveItConfigsBuilder("moveit_resources_panda")
"moveit_resources_panda_description", "urdf/panda.urdf" .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( pick_place_demo = Node(
package="moveit_task_constructor_demo", package="moveit_task_constructor_demo",
@ -68,10 +25,11 @@ def generate_launch_description():
"config", "config",
"panda_config.yaml", "panda_config.yaml",
), ),
robot_description, moveit_config.robot_description,
robot_description_semantic, moveit_config.robot_description_semantic,
kinematics_yaml, moveit_config.robot_description_kinematics,
ompl_planning_pipeline_config, moveit_config.planning_pipelines,
moveit_config.joint_limits,
], ],
) )

View File

@ -15,6 +15,7 @@
<depend>moveit_ros_planning_interface</depend> <depend>moveit_ros_planning_interface</depend>
<depend>moveit_task_constructor_core</depend> <depend>moveit_task_constructor_core</depend>
<depend>rosparam_shortcuts</depend> <depend>rosparam_shortcuts</depend>
<exec_depend>moveit_configs_utils</exec_depend>
<exec_depend>moveit_resources_panda_moveit_config</exec_depend> <exec_depend>moveit_resources_panda_moveit_config</exec_depend>
<exec_depend>moveit_task_constructor_capabilities</exec_depend> <exec_depend>moveit_task_constructor_capabilities</exec_depend>