From 6711436d3b22784e4c761c7d0e7f234001d2e60d Mon Sep 17 00:00:00 2001 From: AndyZe Date: Sun, 15 Jan 2023 14:23:56 -0600 Subject: [PATCH] Do not allow traj execution from PlanningComponent (#1835) * Do not allow traj execution from PlanningComponent * Deprecate, don't delete * Get the group_name from RobotTrajectory * Rebase --- .../include/moveit/moveit_cpp/moveit_cpp.h | 10 ++++++---- .../include/moveit/moveit_cpp/planning_component.h | 2 +- moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp | 13 +++++++++++-- .../planning/moveit_cpp/src/planning_component.cpp | 2 +- 4 files changed, 19 insertions(+), 8 deletions(-) diff --git a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h index 4cfecff6df..644bdb3faa 100644 --- a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h +++ b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h @@ -162,9 +162,6 @@ class MoveItCpp const trajectory_execution_manager::TrajectoryExecutionManagerPtr& getTrajectoryExecutionManager() const; trajectory_execution_manager::TrajectoryExecutionManagerPtr getTrajectoryExecutionManagerNonConst(); - /** \brief Execute a trajectory on the planning group specified by group_name using the trajectory execution manager. - * If blocking is set to false, the execution is run in background and the function returns immediately. */ - /** \brief Execute a trajectory on the planning group specified by group_name using the trajectory execution manager. * \param [in] group_name MoveIt group to execute for. * \param [in] robot_trajectory Contains trajectory info as well as metadata such as a RobotModel. @@ -173,10 +170,15 @@ class MoveItCpp * a controller. The exact behavior of finding a controller depends on which MoveItControllerManager plugin is active. * \return moveit_controller_manager::ExecutionStatus::SUCCEEDED if successful */ - moveit_controller_manager::ExecutionStatus + [[deprecated( + "MoveItCpp::execute() no longer requires a group_name parameter")]] moveit_controller_manager::ExecutionStatus execute(const std::string& group_name, const robot_trajectory::RobotTrajectoryPtr& robot_trajectory, bool blocking = true, const std::vector& controllers = std::vector()); + moveit_controller_manager::ExecutionStatus + execute(const robot_trajectory::RobotTrajectoryPtr& robot_trajectory, bool blocking = true, + const std::vector& controllers = std::vector()); + /** \brief Utility to terminate the given planning pipeline */ bool terminatePlanningPipeline(const std::string& pipeline_name); diff --git a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h index 147f77d85f..856d164ecb 100644 --- a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h +++ b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h @@ -202,7 +202,7 @@ class PlanningComponent /** \brief Execute the latest computed solution trajectory computed by plan(). By default this function terminates * after the execution is complete. The execution can be run in background by setting blocking to false. */ - bool execute(bool blocking = true); + [[deprecated("Use MoveItCpp::execute()")]] bool execute(bool blocking = true); /** \brief Return the last plan solution*/ const planning_interface::MotionPlanResponse& getLastMotionPlanResponse(); diff --git a/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp b/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp index 1418d71a25..60d1b86202 100644 --- a/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp +++ b/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp @@ -217,8 +217,15 @@ trajectory_execution_manager::TrajectoryExecutionManagerPtr MoveItCpp::getTrajec } moveit_controller_manager::ExecutionStatus -MoveItCpp::execute(const std::string& group_name, const robot_trajectory::RobotTrajectoryPtr& robot_trajectory, - bool blocking, const std::vector& controllers) +MoveItCpp::execute(const std::string& /* group_name */, const robot_trajectory::RobotTrajectoryPtr& robot_trajectory, + bool blocking, const std::vector& /* controllers */) +{ + return execute(robot_trajectory, blocking); +} + +moveit_controller_manager::ExecutionStatus +MoveItCpp::execute(const robot_trajectory::RobotTrajectoryPtr& robot_trajectory, bool blocking, + const std::vector& controllers) { if (!robot_trajectory) { @@ -226,6 +233,8 @@ MoveItCpp::execute(const std::string& group_name, const robot_trajectory::RobotT return moveit_controller_manager::ExecutionStatus::ABORTED; } + const std::string group_name = robot_trajectory->getGroupName(); + // Check if there are controllers that can handle the execution if (!trajectory_execution_manager_->ensureActiveControllersForGroup(group_name)) { diff --git a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp index 29765b896a..ed35b0a560 100644 --- a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp +++ b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp @@ -412,7 +412,7 @@ bool PlanningComponent::execute(bool blocking) // RCLCPP_ERROR("Failed to parameterize trajectory"); // return false; //} - return moveit_cpp_->execute(group_name_, last_plan_solution_.trajectory_, blocking); + return moveit_cpp_->execute(last_plan_solution_.trajectory_, blocking); } const planning_interface::MotionPlanResponse& PlanningComponent::getLastMotionPlanResponse()