Do execution from moveit_cpp, add an explicit list of controllers (#563)

* Use MoveItCpp to execute trajectories, not PlanningComponents

* API update

* const

Author: AndyZe

doc/examples/moveit_cpp/src/moveit_cpp_tutorial.cpp

@@ -40,6 +40,8 @@ int main(int argc, char** argv)
   //
   static const std::string PLANNING_GROUP = "panda_arm";
   static const std::string LOGNAME = "moveit_cpp_tutorial";
+  // ros2_controllers
+  static const std::vector<std::string> CONTROLLERS(1, "panda_arm_controller");
 
   /* Otherwise robot with zeros joint_states */
   rclcpp::sleep_for(std::chrono::seconds(1));
@@ -95,7 +97,7 @@ int main(int argc, char** argv)
 
   // Now, we call the PlanningComponents to compute the plan and visualize it.
   // Note that we are just planning
-  auto plan_solution1 = planning_components->plan();
+  const planning_interface::MotionPlanResponse plan_solution1 = planning_components->plan();
 
   // Check if PlanningComponents succeeded in finding the plan
   if (plan_solution1)
@@ -110,7 +112,8 @@ int main(int argc, char** argv)
     visual_tools.trigger();
 
     /* Uncomment if you want to execute the plan */
-    /* planning_components->execute(); // Execute the plan */
+    /* bool blocking = true; */
+    /* moveit_controller_manager::ExecutionStatus result = moveit_cpp_ptr->execute(plan_solution1.trajectory_, blocking, CONTROLLERS); */
   }
 
   // Plan #1 visualization:
@@ -154,7 +157,8 @@ int main(int argc, char** argv)
     visual_tools.trigger();
 
     /* Uncomment if you want to execute the plan */
-    /* planning_components->execute(); // Execute the plan */
+    /* bool blocking = true; */
+    /* moveit_cpp_ptr->execute(plan_solution2.trajectory_, blocking, CONTROLLERS); */
   }
 
   // Plan #2 visualization:
@@ -198,7 +202,8 @@ int main(int argc, char** argv)
     visual_tools.trigger();
 
     /* Uncomment if you want to execute the plan */
-    /* planning_components->execute(); // Execute the plan */
+    /* bool blocking = true; */
+    /* moveit_cpp_ptr->execute(plan_solution3.trajectory_, blocking, CONTROLLERS); */
   }
 
   // Plan #3 visualization:
@@ -240,7 +245,8 @@ int main(int argc, char** argv)
     visual_tools.trigger();
 
     /* Uncomment if you want to execute the plan */
-    /* planning_components->execute(); // Execute the plan */
+    /* bool blocking = true; */
+    /* moveit_cpp_ptr->execute(plan_solution4.trajectory_, blocking, CONTROLLERS); */
   }
 
   // Plan #4 visualization:
@@ -293,7 +299,8 @@ int main(int argc, char** argv)
     visual_tools.trigger();
 
     /* Uncomment if you want to execute the plan */
-    /* planning_components->execute(); // Execute the plan */
+    /* bool blocking = true; */
+    /* moveit_cpp_ptr->execute(plan_solution5.trajectory_, blocking, CONTROLLERS); */
   }
 
   // Plan #5 visualization:

doc/how_to_guides/parallel_planning/src/parallel_planning_main.cpp

@@ -22,6 +22,7 @@ namespace
 {
 const rclcpp::Logger LOGGER = rclcpp::get_logger("parallel_planning_example");
 const std::string PLANNING_GROUP = "panda_arm";
+static const std::vector<std::string> CONTROLLERS(1, "panda_arm_controller");
 }  // namespace
 namespace parallel_planning_example
 {
@@ -241,7 +242,8 @@ class Demo
       visual_tools_.trigger();
     }
 
-    planning_component_->execute();
+    // Execute the trajectory and block until it's finished
+    moveit_cpp_->execute(plan_solution.trajectory_, true /* blocking*/, CONTROLLERS);
 
     // Start the next plan
     visual_tools_.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");