robossembler/cnc-graver-assist
7
0
Fork 0
Code Issues 5 Pull requests 1 Projects Releases Packages Wiki Activity Actions

refactor(moveit launch): migrate to moveit_config_builder

Browse source
This commit is contained in:
Ilya Uraev 2025-03-10 16:39:19 +03:00
parent 193e2ef3c9
commit 4555d2b12e
1 changed files with 5 additions and 108 deletions
Download patch file Download diff file Expand all files Collapse all files

113
rbs_mill_assist/launch/moveit.launch.py
View file

@ -1,21 +1,9 @@
from launch_ros.actions import Node
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, OpaqueFunction
from ament_index_python.packages import get_package_share_directory
from launch_ros.substitutions import FindPackage, FindPackageShare
from launch.substitutions import (
Command,
FindExecutable,
LaunchConfiguration,
PathJoinSubstitution,
)
import yaml
from rbs_launch_utils.launch_common import (
get_package_share_directory,
load_yaml,
load_yaml_abs,
)
import os
from moveit_configs_utils import MoveItConfigsBuilder
@ -92,114 +80,34 @@ def launch_setup(context, *args, **kwargs):
robot_description_semantic_content = LaunchConfiguration(
"robot_description_semantic"
).perform(context)
# robot_description_kinematics_filepath = LaunchConfiguration(
# "robot_description_kinematics"
# ).perform(context)
namespace = LaunchConfiguration("namespace").perform(context)
moveit_config_package = LaunchConfiguration("moveit_config_package").perform(
context
)
robot_description = {"robot_description": robot_description_content}
robot_description_semantic = {
"robot_description_semantic": robot_description_semantic_content
}
# robot_description_kinematics = {
# "robot_description_kinematics": load_yaml_abs(
# robot_description_kinematics_filepath
# )
# }
use_sim_time = {"use_sim_time": use_sim_time}
# Planning Configuration
ompl_planning_pipeline_config = {
"default_planning_pipeline": "ompl",
"planning_pipelines": ["ompl"],
"ompl": {
"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_config_package, "moveit/ompl_planning.yaml")
ompl_planning_pipeline_config["ompl"].update(ompl_planning_yaml)
moveit_controllers = load_yaml(
moveit_config_package, "moveit/moveit_controllers.yaml"
)
# moveit_controllers = {
# "moveit_simple_controller_manager":
# controllers_yaml,
# "moveit_controller_manager":
# "moveit_simple_controller_manager/MoveItSimpleControllerManager",
# }
xacro_mappings = yaml.safe_load(os.path.join(get_package_share_directory("rbs_mill_assist"), "urdf", "xacro_args.yaml"))
moveit_config = (
MoveItConfigsBuilder(robot_name="rbs_arm", package_name="rbs_mill_assist")
.robot_description(file_path="urdf/rbs_arm.xacro")
.trajectory_execution(file_path="moveit/moveit_controllers.yaml")
.joint_limits("moveit/joint_limits.yaml")
.pilz_cartesian_limits("moveit/pilz_cartesian_limits.yaml")
.robot_description_kinematics("moveit/kinematics.yaml")
.robot_description_semantic("srdf/rbs_arm.srdf")
.moveit_cpp("moveit/moveit_cpp.yaml")
.planning_pipelines(pipelines=["ompl", "chomp"], default_planning_pipeline="chomp")
.planning_scene_monitor(
publish_geometry_updates=True,
publish_planning_scene=True,
publish_robot_description=True,
publish_robot_description_semantic=True,
)
.robot_description_semantic("srdf/rbs_arm.srdf", mappings={})
.planning_pipelines(pipelines=["ompl", "chomp", "stomp"], default_planning_pipeline="ompl")
.to_moveit_configs()
)
# trajectory_execution = {
# "moveit_manage_controllers": False,
# "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,
}
robot_description_planning = {
"robot_description_planning": load_yaml(
moveit_config_package,
os.path.join("moveit", "joint_limits.yaml"),
)
}
move_group_node = Node(
package="moveit_ros_move_group",
executable="move_group",
namespace=namespace,
parameters=[
moveit_config.to_dict(),
robot_description,
robot_description_semantic,
# robot_description_kinematics,
ompl_planning_pipeline_config,
# trajectory_execution,
moveit_controllers,
planning_scene_monitor_parameters,
use_sim_time,
robot_description_planning,
# moveit_config.to_dict()
# moveit_config.robot_description,
# moveit_config.robot_description_semantic,
# moveit_config.planning_pipelines,
# moveit_config.planning_scene_monitor,
moveit_config.robot_description_kinematics,
# moveit_config.joint_limits,
# moveit_config.trajectory_execution,
use_sim_time
],
)
planning_scene_publisher = Node(
@ -215,18 +123,7 @@ def launch_setup(context, *args, **kwargs):
output="log",
# arguments=["-d", rviz_full_config],
parameters=[
# robot_description,
# robot_description_semantic,
# robot_description_kinematics,
# robot_description_planning,
# ompl_planning_pipeline_config,
# # trajectory_execution,
# moveit_controllers,
# planning_scene_monitor_parameters,
{"use_sim_time": True},
# moveit_config.robot_description,
# moveit_config.robot_description_semantic,
# moveit_config.robot_description_kinematics,
use_sim_time,
moveit_config.robot_description,
moveit_config.robot_description_semantic,
moveit_config.planning_pipelines,