mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Use moveit_configs_utils for launch files (#365)
This commit is contained in:
parent
d1fb6ace93
commit
0128cd9250
5
.github/workflows/ci.yaml
vendored
5
.github/workflows/ci.yaml
vendored
@ -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:
|
||||||
|
|||||||
2
.github/workflows/lsan.suppressions
vendored
2
.github/workflows/lsan.suppressions
vendored
@ -1 +1,3 @@
|
|||||||
leak:class_loader
|
leak:class_loader
|
||||||
|
leak:rviz_default_plugins
|
||||||
|
leak:static_transform_broadcaster_node
|
||||||
|
|||||||
@ -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 -->
|
||||||
|
|||||||
@ -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,
|
||||||
],
|
],
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|||||||
@ -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,
|
||||||
],
|
],
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|||||||
@ -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])
|
||||||
|
|||||||
@ -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",
|
||||||
)
|
)
|
||||||
|
|||||||
@ -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])
|
||||||
|
|||||||
@ -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])
|
||||||
|
|||||||
@ -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,
|
||||||
],
|
],
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|||||||
@ -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>
|
||||||
|
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user