From 3ec75aba17d29ed815ab5946787e2e17350b608b Mon Sep 17 00:00:00 2001 From: Dongya Jiang <1983534+patrickKXMD@users.noreply.github.com> Date: Mon, 13 Jan 2025 01:39:36 +0800 Subject: [PATCH] move TrajectoryExecutionManager::clear() to private (#3226) * move TrajectoryExecutionManager::clear() to private Signed-off-by: Dongya Jiang <1983534+patrickKXMD@users.noreply.github.com> * Apply suggestions from code review Co-authored-by: Robert Haschke --------- Signed-off-by: Dongya Jiang <1983534+patrickKXMD@users.noreply.github.com> Co-authored-by: Robert Haschke --- .../trajectory_execution_manager.cpp | 5 ----- .../execute_trajectory_action_capability.cpp | 1 - moveit_ros/planning/plan_execution/src/plan_execution.cpp | 1 - .../trajectory_execution_manager.hpp | 6 +++--- 4 files changed, 3 insertions(+), 10 deletions(-) diff --git a/moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp b/moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp index 1c0693e638..e74260e6a0 100644 --- a/moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp +++ b/moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp @@ -200,11 +200,6 @@ void initTrajectoryExecutionManager(py::module& m) Stop whatever executions are active, if any. )") - .def("clear", &trajectory_execution_manager::TrajectoryExecutionManager::clear, - R"( - Clear the trajectories to execute. - )") - .def("enable_execution_duration_monitoring", &trajectory_execution_manager::TrajectoryExecutionManager::enableExecutionDurationMonitoring, py::arg("flag"), diff --git a/moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.cpp b/moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.cpp index ff7959e439..602b16a6ee 100644 --- a/moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.cpp @@ -106,7 +106,6 @@ void MoveGroupExecuteTrajectoryAction::executePath(const std::shared_ptrtrajectory_execution_manager_->clear(); if (context_->trajectory_execution_manager_->push(goal->get_goal()->trajectory, goal->get_goal()->controller_names)) { setExecuteTrajectoryState(MONITOR, goal); diff --git a/moveit_ros/planning/plan_execution/src/plan_execution.cpp b/moveit_ros/planning/plan_execution/src/plan_execution.cpp index bfbc2c37ac..173bceccad 100644 --- a/moveit_ros/planning/plan_execution/src/plan_execution.cpp +++ b/moveit_ros/planning/plan_execution/src/plan_execution.cpp @@ -401,7 +401,6 @@ moveit_msgs::msg::MoveItErrorCodes plan_execution::PlanExecution::executeAndMoni plan.plan_components[component_idx].trajectory->getRobotTrajectoryMsg(msg); if (!trajectory_execution_manager_->push(msg, plan.plan_components[component_idx].controller_name)) { - trajectory_execution_manager_->clear(); RCLCPP_ERROR(logger_, "Apparently trajectory initialization failed"); execution_complete_ = true; result.val = moveit_msgs::msg::MoveItErrorCodes::CONTROL_FAILED; diff --git a/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.hpp b/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.hpp index b305371d54..86c190b3ca 100644 --- a/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.hpp +++ b/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.hpp @@ -186,9 +186,6 @@ class MOVEIT_TRAJECTORY_EXECUTION_MANAGER_EXPORT TrajectoryExecutionManager /// Stop whatever executions are active, if any void stopExecution(bool auto_clear = true); - /// Clear the trajectories to execute - void clear(); - /// Enable or disable the monitoring of trajectory execution duration. If a controller takes /// longer than expected, the trajectory is canceled void enableExecutionDurationMonitoring(bool flag); @@ -286,6 +283,9 @@ class MOVEIT_TRAJECTORY_EXECUTION_MANAGER_EXPORT TrajectoryExecutionManager bool executePart(std::size_t part_index); bool waitForRobotToStop(const TrajectoryExecutionContext& context, double wait_time = 1.0); + /// Clear the trajectories to execute + void clear(); + void stopExecutionInternal(); void receiveEvent(const std_msgs::msg::String::ConstSharedPtr& event);