Specify controller name in MGI execution

moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.cpp

@@ -108,7 +108,7 @@ void MoveGroupExecuteTrajectoryAction::executePath(const std::shared_ptr<ExecTra
   RCLCPP_INFO(LOGGER, "Execution request received");
 
   context_->trajectory_execution_manager_->clear();
-  if (context_->trajectory_execution_manager_->push(goal->get_goal()->trajectory))
+  if (context_->trajectory_execution_manager_->push(goal->get_goal()->trajectory, goal->get_goal()->controller_names))
   {
     setExecuteTrajectoryState(MONITOR, goal);
     context_->trajectory_execution_manager_->execute();

moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h

@@ -715,17 +715,41 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface
       target. No execution is performed. The resulting plan is stored in \e plan*/
   moveit::core::MoveItErrorCode plan(Plan& plan);
 
-  /** \brief Given a \e plan, execute it without waiting for completion. */
-  moveit::core::MoveItErrorCode asyncExecute(const Plan& plan);
-
-  /** \brief Given a \e robot trajectory, execute it without waiting for completion. */
-  moveit::core::MoveItErrorCode asyncExecute(const moveit_msgs::msg::RobotTrajectory& trajectory);
-
-  /** \brief Given a \e plan, execute it while waiting for completion. */
-  moveit::core::MoveItErrorCode execute(const Plan& plan);
-
-  /** \brief Given a \e robot trajectory, execute it while waiting for completion. */
-  moveit::core::MoveItErrorCode execute(const moveit_msgs::msg::RobotTrajectory& trajectory);
+  /** \brief Given a \e plan, execute it without waiting for completion.
+   *  \param [in] plan The motion plan for which to execute
+   *  \param [in] controllers An optional list of ros2_controllers to execute with. If none, MoveIt will attempt to find
+   * a controller. The exact behavior of finding a controller depends on which MoveItControllerManager plugin is active.
+   *  \return moveit::core::MoveItErrorCode::SUCCESS if successful
+   */
+  moveit::core::MoveItErrorCode asyncExecute(const Plan& plan,
+                                             const std::vector<std::string>& controllers = std::vector<std::string>());
+
+  /** \brief Given a \e robot trajectory, execute it without waiting for completion.
+   *  \param [in] trajectory The trajectory to execute
+   *  \param [in] controllers An optional list of ros2_controllers to execute with. If none, MoveIt will attempt to find
+   * a controller. The exact behavior of finding a controller depends on which MoveItControllerManager plugin is active.
+   *  \return moveit::core::MoveItErrorCode::SUCCESS if successful
+   */
+  moveit::core::MoveItErrorCode asyncExecute(const moveit_msgs::msg::RobotTrajectory& trajectory,
+                                             const std::vector<std::string>& controllers = std::vector<std::string>());
+
+  /** \brief Given a \e plan, execute it while waiting for completion.
+   *  \param [in] plan Contains trajectory info as well as metadata such as a RobotModel.
+   *  \param [in] controllers An optional list of ros2_controllers to execute with. If none, MoveIt will attempt to find
+   * a controller. The exact behavior of finding a controller depends on which MoveItControllerManager plugin is active.
+   *  \return moveit::core::MoveItErrorCode::SUCCESS if successful
+   */
+  moveit::core::MoveItErrorCode execute(const Plan& plan,
+                                        const std::vector<std::string>& controllers = std::vector<std::string>());
+
+  /** \brief Given a \e robot trajectory, execute it while waiting for completion.
+   *  \param [in] trajectory The trajectory to execute
+   *  \param [in] controllers An optional list of ros2_controllers to execute with. If none, MoveIt will attempt to find
+   * a controller. The exact behavior of finding a controller depends on which MoveItControllerManager plugin is active.
+   *  \return moveit::core::MoveItErrorCode::SUCCESS if successful
+   */
+  moveit::core::MoveItErrorCode execute(const moveit_msgs::msg::RobotTrajectory& trajectory,
+                                        const std::vector<std::string>& controllers = std::vector<std::string>());
 
   /** \brief Compute a Cartesian path that follows specified waypoints with a step size of at most \e eef_step meters
       between end effector configurations of consecutive points in the result \e trajectory. The reference frame for the

moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp

@@ -783,7 +783,8 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
     return res->error_code;
   }
 
-  moveit::core::MoveItErrorCode execute(const moveit_msgs::msg::RobotTrajectory& trajectory, bool wait)
+  moveit::core::MoveItErrorCode execute(const moveit_msgs::msg::RobotTrajectory& trajectory, bool wait,
+                                        const std::vector<std::string>& controllers = std::vector<std::string>())
   {
     if (!execute_action_client_ || !execute_action_client_->action_server_is_ready())
     {
@@ -831,6 +832,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
 
     moveit_msgs::action::ExecuteTrajectory::Goal goal;
     goal.trajectory = trajectory;
+    goal.controller_names = controllers;
 
     auto goal_handle_future = execute_action_client_->async_send_goal(goal, send_goal_opts);
     if (!wait)
@@ -1455,24 +1457,27 @@ moveit::core::MoveItErrorCode MoveGroupInterface::move()
   return impl_->move(true);
 }
 
-moveit::core::MoveItErrorCode MoveGroupInterface::asyncExecute(const Plan& plan)
+moveit::core::MoveItErrorCode MoveGroupInterface::asyncExecute(const Plan& plan,
+                                                               const std::vector<std::string>& controllers)
 {
-  return impl_->execute(plan.trajectory, false);
+  return impl_->execute(plan.trajectory, false, controllers);
 }
 
-moveit::core::MoveItErrorCode MoveGroupInterface::asyncExecute(const moveit_msgs::msg::RobotTrajectory& trajectory)
+moveit::core::MoveItErrorCode MoveGroupInterface::asyncExecute(const moveit_msgs::msg::RobotTrajectory& trajectory,
+                                                               const std::vector<std::string>& controllers)
 {
-  return impl_->execute(trajectory, false);
+  return impl_->execute(trajectory, false, controllers);
 }
 
-moveit::core::MoveItErrorCode MoveGroupInterface::execute(const Plan& plan)
+moveit::core::MoveItErrorCode MoveGroupInterface::execute(const Plan& plan, const std::vector<std::string>& controllers)
 {
-  return impl_->execute(plan.trajectory, true);
+  return impl_->execute(plan.trajectory, true, controllers);
 }
 
-moveit::core::MoveItErrorCode MoveGroupInterface::execute(const moveit_msgs::msg::RobotTrajectory& trajectory)
+moveit::core::MoveItErrorCode MoveGroupInterface::execute(const moveit_msgs::msg::RobotTrajectory& trajectory,
+                                                          const std::vector<std::string>& controllers)
 {
-  return impl_->execute(trajectory, true);
+  return impl_->execute(trajectory, true, controllers);
 }
 
 moveit::core::MoveItErrorCode MoveGroupInterface::plan(Plan& plan)