Skip to content

Commit

Permalink
Reset status variables before calling sendGoal()
Browse files Browse the repository at this point in the history
This fixes a race condition, where the execution status is set
to RUNNING after it the doneCallback has been called, e.g.
because the controller immediately reports success.
  • Loading branch information
rhaschke authored and LeroyR committed Jul 11, 2024
1 parent 36da5ed commit 878d291
Showing 1 changed file with 2 additions and 2 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -62,11 +62,11 @@ bool FollowJointTrajectoryControllerHandle::sendTrajectory(const moveit_msgs::Ro

control_msgs::FollowJointTrajectoryGoal goal = goal_template_;
goal.trajectory = trajectory.joint_trajectory;
done_ = false;
last_exec_ = moveit_controller_manager::ExecutionStatus::RUNNING;
controller_action_client_->sendGoal(
goal, [this](const auto& state, const auto& result) { controllerDoneCallback(state, result); },
[this] { controllerActiveCallback(); }, [this](const auto& feedback) { controllerFeedbackCallback(feedback); });
done_ = false;
last_exec_ = moveit_controller_manager::ExecutionStatus::RUNNING;
return true;
}

Expand Down

0 comments on commit 878d291

Please sign in to comment.