Fix startup of MotionSequenceAction capability

This commit is contained in:
2023-02-23 15:49:03 +02:00
parent 63b17f95e3
commit b009b3974c
3 changed files with 16 additions and 3 deletions

View File

@@ -371,10 +371,14 @@ def launch_setup(context, *args, **kwargs):
'planning_plugin': 'pilz_industrial_motion_planner/CommandPlanner', 'planning_plugin': 'pilz_industrial_motion_planner/CommandPlanner',
'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""", '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""",
"default_planner_config": "PTP", "default_planner_config": "PTP",
"capabilities": "pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService",
} }
} }
move_group_capabilities = {
"capabilities": "pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService"
}
# Moveit controllers Configuration # Moveit controllers Configuration
moveit_controllers = { moveit_controllers = {
@@ -420,6 +424,7 @@ def launch_setup(context, *args, **kwargs):
robot_description_parameters, robot_description_parameters,
#ompl_planning_pipeline_config, #ompl_planning_pipeline_config,
pilz_planning_pipeline_config, pilz_planning_pipeline_config,
move_group_capabilities,
trajectory_execution, trajectory_execution,
plan_execution, plan_execution,
moveit_controllers, moveit_controllers,

View File

@@ -233,10 +233,13 @@ def launch_setup(context, *args, **kwargs):
'planning_plugin': 'pilz_industrial_motion_planner/CommandPlanner', 'planning_plugin': 'pilz_industrial_motion_planner/CommandPlanner',
'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""", '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""",
"default_planner_config": "PTP", "default_planner_config": "PTP",
"capabilities": "pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService",
} }
} }
move_group_capabilities = {
"capabilities": "pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService"
}
# Start the actual move_group node/action server # Start the actual move_group node/action server
move_group_node = Node( move_group_node = Node(
package='moveit_ros_move_group', package='moveit_ros_move_group',
@@ -246,6 +249,7 @@ def launch_setup(context, *args, **kwargs):
robot_description_parameters, robot_description_parameters,
#ompl_planning_pipeline_config, #ompl_planning_pipeline_config,
pilz_planning_pipeline_config, pilz_planning_pipeline_config,
move_group_capabilities,
trajectory_execution, trajectory_execution,
plan_execution, plan_execution,
moveit_controllers, moveit_controllers,

View File

@@ -198,10 +198,13 @@ def launch_setup(context, *args, **kwargs):
'planning_plugin': 'pilz_industrial_motion_planner/CommandPlanner', 'planning_plugin': 'pilz_industrial_motion_planner/CommandPlanner',
'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""", '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""",
"default_planner_config": "PTP", "default_planner_config": "PTP",
"capabilities": "pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService",
} }
} }
move_group_capabilities = {
"capabilities": "pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService"
}
# Moveit controllers Configuration # Moveit controllers Configuration
moveit_controllers = { moveit_controllers = {
moveit_controller_manager_key.perform(context): controllers_yaml, moveit_controller_manager_key.perform(context): controllers_yaml,
@@ -246,6 +249,7 @@ def launch_setup(context, *args, **kwargs):
robot_description_parameters, robot_description_parameters,
#ompl_planning_pipeline_config, #ompl_planning_pipeline_config,
pilz_planning_pipeline_config, pilz_planning_pipeline_config,
move_group_capabilities,
trajectory_execution, trajectory_execution,
plan_execution, plan_execution,
moveit_controllers, moveit_controllers,