Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

move TrajectoryExecutionManager::clear() to private (backport #3226) #3238

Merged
merged 1 commit into from
Jan 12, 2025
Merged
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
move TrajectoryExecutionManager::clear() to private (#3226)
* move TrajectoryExecutionManager::clear() to private

Signed-off-by: Dongya Jiang <[email protected]>

* Apply suggestions from code review

Co-authored-by: Robert Haschke <[email protected]>

---------

Signed-off-by: Dongya Jiang <[email protected]>
Co-authored-by: Robert Haschke <[email protected]>
(cherry picked from commit 3ec75ab)
patrickKXMD authored and mergify[bot] committed Jan 12, 2025

Verified

This commit was created on GitHub.com and signed with GitHub’s verified signature. The key has expired.
commit db20f1e287e0e5383bfbbd9ee95b7cec5020c2f8
Original file line number Diff line number Diff line change
@@ -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"),
Original file line number Diff line number Diff line change
@@ -106,7 +106,6 @@ void MoveGroupExecuteTrajectoryAction::executePath(const std::shared_ptr<ExecTra
{
RCLCPP_INFO(getLogger(), "Execution request received");

context_->trajectory_execution_manager_->clear();
if (context_->trajectory_execution_manager_->push(goal->get_goal()->trajectory, goal->get_goal()->controller_names))
{
setExecuteTrajectoryState(MONITOR, goal);
1 change: 0 additions & 1 deletion moveit_ros/planning/plan_execution/src/plan_execution.cpp
Original file line number Diff line number Diff line change
@@ -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;
Original file line number Diff line number Diff line change
@@ -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);