From d45d38f5fe60b756282ebd65a61eb86399131483 Mon Sep 17 00:00:00 2001
From: Joachim Schleicher <J.Schleicher@pilz.de>
Date: Fri, 16 Oct 2020 11:58:06 +0200
Subject: [PATCH 1/2] move capabilities arguments into planning pipeline

so that the pilz sequence capability can be appended there.
---
 prbt_moveit_config/launch/move_group.launch       | 13 ++-----------
 .../launch/ompl_planning_pipeline.launch.xml      |  8 ++++++++
 ...z_command_planner_planning_pipeline.launch.xml | 14 ++++++++++++++
 ...al_motion_planner_planning_pipeline.launch.xml | 14 ++++++++++++++
 .../launch/planning_pipeline.launch.xml           | 15 ++++++++++++---
 5 files changed, 50 insertions(+), 14 deletions(-)

diff --git a/prbt_moveit_config/launch/move_group.launch b/prbt_moveit_config/launch/move_group.launch
index 472efaadd..a3e2c21f2 100644
--- a/prbt_moveit_config/launch/move_group.launch
+++ b/prbt_moveit_config/launch/move_group.launch
@@ -27,15 +27,6 @@
   <!-- Enable/Disable gripper -->
   <arg name="gripper" default="" />
 
-  <!-- add sequence capabilities for pilz_command_planner pipeline -->
-  <arg name="_capabilities" value="$(arg capabilities)" unless="$(eval arg('pipeline')=='pilz_command_planner' or arg('pipeline')=='pilz_industrial_motion_planner')" />
-  <arg name="_capabilities"
-       value="$(arg capabilities) pilz_trajectory_generation/MoveGroupSequenceAction pilz_trajectory_generation/MoveGroupSequenceService"
-       if="$(eval arg('pipeline')=='pilz_command_planner')" />
-  <arg name="_capabilities"
-       value="$(arg capabilities) pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService"
-       if="$(eval arg('pipeline')=='pilz_industrial_motion_planner')" />
-
   <!-- load robot models -->
   <include file="$(find prbt_moveit_config)/launch/planning_context.launch" >
     <arg name="gripper" value="$(arg gripper)" />
@@ -45,6 +36,8 @@
   <!-- Planning Functionality -->
   <include ns="move_group" file="$(find prbt_moveit_config)/launch/planning_pipeline.launch.xml">
     <arg name="pipeline" value="$(arg pipeline)" />
+    <arg name="capabilities" value="$(arg capabilities)"/>
+    <arg name="disable_capabilities" value="$(arg disable_capabilities)"/>
   </include>
 
   <!-- Trajectory Execution Functionality -->
@@ -68,8 +61,6 @@
     <param name="allow_trajectory_execution" value="$(arg allow_trajectory_execution)"/>
     <param name="max_safe_path_cost" value="$(arg max_safe_path_cost)"/>
     <param name="jiggle_fraction" value="$(arg jiggle_fraction)" />
-    <param name="capabilities" value="$(arg _capabilities)"/>
-    <param name="disable_capabilities" value="$(arg disable_capabilities)"/>
 
     <!-- Publish the planning scene of the physical robot so that rviz plugin can know actual robot -->
     <param name="planning_scene_monitor/publish_planning_scene" value="$(arg publish_monitored_planning_scene)" />
diff --git a/prbt_moveit_config/launch/ompl_planning_pipeline.launch.xml b/prbt_moveit_config/launch/ompl_planning_pipeline.launch.xml
index 4562767ae..558a19470 100644
--- a/prbt_moveit_config/launch/ompl_planning_pipeline.launch.xml
+++ b/prbt_moveit_config/launch/ompl_planning_pipeline.launch.xml
@@ -3,6 +3,12 @@
   <!-- OMPL Plugin for MoveIt! -->
   <arg name="planning_plugin" value="ompl_interface/OMPLPlanner" />
 
+  <!-- define capabilites that are loaded on start (space seperated) -->
+  <arg name="capabilities" default=""/>
+
+  <!-- inhibit capabilites (space seperated) -->
+  <arg name="disable_capabilities" default=""/>
+
   <!-- The request adapters (plugins) used when planning with OMPL. 
        ORDER MATTERS -->
   <arg name="planning_adapters" value="default_planner_request_adapters/AddTimeParameterization
@@ -16,6 +22,8 @@
   <param name="planning_plugin" value="$(arg planning_plugin)" />
   <param name="request_adapters" value="$(arg planning_adapters)" />
   <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
+  <param name="capabilities" value="$(arg capabilities)" />
+  <param name="disable_capabilities" value="$(arg disable_capabilities)" />
 
   <rosparam command="load" file="$(find prbt_moveit_config)/config/ompl_planning.yaml"/>
 
diff --git a/prbt_moveit_config/launch/pilz_command_planner_planning_pipeline.launch.xml b/prbt_moveit_config/launch/pilz_command_planner_planning_pipeline.launch.xml
index 2a8aa8730..53a016857 100644
--- a/prbt_moveit_config/launch/pilz_command_planner_planning_pipeline.launch.xml
+++ b/prbt_moveit_config/launch/pilz_command_planner_planning_pipeline.launch.xml
@@ -3,12 +3,26 @@
   <!-- Pilz Command Planner Plugin for MoveIt! -->
   <arg name="planning_plugin" value="pilz::CommandPlanner" />
 
+  <!-- The request adapters (plugins) used when planning.
+       ORDER MATTERS -->
   <arg name="planning_adapters" value="" />
 
+  <!-- define capabilites that are loaded on start (space seperated) -->
+  <arg name="capabilities" default=""/>
+
+  <!-- inhibit capabilites (space seperated) -->
+  <arg name="disable_capabilities" default=""/>
+
   <arg name="start_state_max_bounds_error" value="0.1" />
 
   <param name="planning_plugin" value="$(arg planning_plugin)" />
   <param name="request_adapters" value="$(arg planning_adapters)" />
   <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
 
+  <!-- MoveGroup capabilities to load, append sequence capability -->
+  <param name="capabilities" value="$(arg capabilities)
+                                    pilz_industrial_motion_planner/MoveGroupSequenceAction
+                                    pilz_industrial_motion_planner/MoveGroupSequenceService
+                                   " />
+  <param name="disable_capabilities" value="$(arg disable_capabilities)" />
 </launch>
diff --git a/prbt_moveit_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml b/prbt_moveit_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml
index 6f3c1cd41..31d5f8302 100644
--- a/prbt_moveit_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml
+++ b/prbt_moveit_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml
@@ -3,12 +3,26 @@
   <!-- Pilz Command Planner Plugin for MoveIt! -->
   <arg name="planning_plugin" value="pilz_industrial_motion_planner::CommandPlanner" />
 
+  <!-- The request adapters (plugins) used when planning.
+       ORDER MATTERS -->
   <arg name="planning_adapters" value="" />
 
+  <!-- define capabilites that are loaded on start (space seperated) -->
+  <arg name="capabilities" default=""/>
+
+  <!-- inhibit capabilites (space seperated) -->
+  <arg name="disable_capabilities" default=""/>
+
   <arg name="start_state_max_bounds_error" value="0.1" />
 
   <param name="planning_plugin" value="$(arg planning_plugin)" />
   <param name="request_adapters" value="$(arg planning_adapters)" />
   <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
 
+  <!-- MoveGroup capabilities to load, append sequence capability -->
+  <param name="capabilities" value="$(arg capabilities)
+                                    pilz_industrial_motion_planner/MoveGroupSequenceAction
+                                    pilz_industrial_motion_planner/MoveGroupSequenceService
+                                   " />
+  <param name="disable_capabilities" value="$(arg disable_capabilities)" />
 </launch>
diff --git a/prbt_moveit_config/launch/planning_pipeline.launch.xml b/prbt_moveit_config/launch/planning_pipeline.launch.xml
index f427358aa..863a1b1b9 100644
--- a/prbt_moveit_config/launch/planning_pipeline.launch.xml
+++ b/prbt_moveit_config/launch/planning_pipeline.launch.xml
@@ -1,10 +1,19 @@
 <launch>
 
-  <!-- This file makes it easy to include different planning pipelines; 
-       It is assumed that all planning pipelines are named XXX_planning_pipeline.launch  -->  
+  <!-- This file makes it easy to include different planning pipelines;
+       It is assumed that all planning pipelines are named XXX_planning_pipeline.launch  -->
 
   <arg name="pipeline" default="ompl" />
 
-  <include file="$(find prbt_moveit_config)/launch/$(arg pipeline)_planning_pipeline.launch.xml" />
+  <!-- define capabilites that are loaded on start (space seperated) -->
+  <arg name="capabilities" default=""/>
+
+  <!-- inhibit capabilites (space seperated) -->
+  <arg name="disable_capabilities" default=""/>
+
+  <include file="$(find prbt_moveit_config)/launch/$(arg pipeline)_planning_pipeline.launch.xml">
+    <arg name="capabilities" value="$(arg capabilities)"/>
+    <arg name="disable_capabilities" value="$(arg disable_capabilities)"/>
+  </include>
 
 </launch>

From aeeeb7ef5e177562ae38f188f95d283064a3230e Mon Sep 17 00:00:00 2001
From: Joachim Schleicher <J.Schleicher@pilz.de>
Date: Fri, 16 Oct 2020 12:12:14 +0200
Subject: [PATCH 2/2] fixup

thanks to @rfeistenauer for the catch :-)
---
 .../launch/pilz_command_planner_planning_pipeline.launch.xml  | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/prbt_moveit_config/launch/pilz_command_planner_planning_pipeline.launch.xml b/prbt_moveit_config/launch/pilz_command_planner_planning_pipeline.launch.xml
index 53a016857..e9e426d77 100644
--- a/prbt_moveit_config/launch/pilz_command_planner_planning_pipeline.launch.xml
+++ b/prbt_moveit_config/launch/pilz_command_planner_planning_pipeline.launch.xml
@@ -21,8 +21,8 @@
 
   <!-- MoveGroup capabilities to load, append sequence capability -->
   <param name="capabilities" value="$(arg capabilities)
-                                    pilz_industrial_motion_planner/MoveGroupSequenceAction
-                                    pilz_industrial_motion_planner/MoveGroupSequenceService
+                                    pilz_trajectory_generation/MoveGroupSequenceAction
+                                    pilz_trajectory_generation/MoveGroupSequenceService
                                    " />
   <param name="disable_capabilities" value="$(arg disable_capabilities)" />
 </launch>