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

Cancel in-progress trajectories. #400

Merged
merged 11 commits into from
Aug 4, 2018
6 changes: 5 additions & 1 deletion src/control/ros/RosTrajectoryExecutor.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -234,7 +234,11 @@ void RosTrajectoryExecutor::step(
//==============================================================================
void RosTrajectoryExecutor::abort()
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'd prefer "cancel" instead of "abort". It seems like "cancel" and "abort" are different terms in ros_control; "cancel" is where the client requested to cancel the trajectory, and "abort" is where the server had to stop it for some other reason (e.g. force threshold has been violated).

http://wiki.ros.org/actionlib/DetailedDescription

{
// TODO: cancel the actionlib goal (once there is support in ReWD controller)
std::lock_guard<std::mutex> lock(mMutex);
DART_UNUSED(lock); // Suppress unused variable warning.

if (mInProgress)
mGoalHandle.cancel();
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Would it be useful to print a warning something like "Attempting to abort trajectory execution but no trajectory in progress."?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

That sounds like a good idea. We should do this to the other TrajectoryExecutor::abort implementations as well.

}

} // namespace ros
Expand Down