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.

Sign up for GitHub

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

Jump to bottom

Updated RT goal handle to handle cancel requests #22

Merged
merged 1 commit into from
Nov 1, 2017
Merged

Updated RT goal handle to handle cancel requests #22

Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
28 changes: 24 additions & 4 deletions include/realtime_tools/realtime_server_goal_handle.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -55,6 +55,7 @@ class RealtimeServerGoalHandle
uint8_t state_;

bool req_abort_;
bool req_cancel_;
Copy link
Contributor

Choose a reason for hiding this comment

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

needs to be initialized in constructor

bool req_succeed_;
ResultConstPtr req_result_;
FeedbackConstPtr req_feedback_;
Expand All @@ -66,6 +67,7 @@ class RealtimeServerGoalHandle

RealtimeServerGoalHandle(GoalHandle &gh, const ResultPtr &preallocated_result = ResultPtr((Result*)NULL), const FeedbackPtr &preallocated_feedback = FeedbackPtr((Feedback*)NULL))
: req_abort_(false),
req_cancel_(false),
req_succeed_(false),
gh_(gh),
preallocated_result_(preallocated_result),
Expand All @@ -79,16 +81,25 @@ class RealtimeServerGoalHandle

void setAborted(ResultConstPtr result = ResultConstPtr((Result*)NULL))
{
if (!req_succeed_ && !req_abort_)
if (!req_succeed_ && !req_abort_ && !req_cancel_)
{
req_result_ = result;
req_abort_ = true;
}
}

void setCanceled(ResultConstPtr result = ResultConstPtr((Result*)NULL))
{
if (!req_succeed_ && !req_abort_ && !req_cancel_)
{
req_result_ = result;
req_cancel_ = true;
}
}

void setSucceeded(ResultConstPtr result = ResultConstPtr((Result*)NULL))
{
if (!req_succeed_ && !req_abort_)
if (!req_succeed_ && !req_abort_ && !req_cancel_)
{
req_result_ = result;
req_succeed_ = true;
Expand All @@ -111,14 +122,23 @@ class RealtimeServerGoalHandle
if (valid())
{
actionlib_msgs::GoalStatus gs = gh_.getGoalStatus();
if (req_abort_ && gs.status == GoalStatus::ACTIVE)
if (req_abort_ && (gs.status == GoalStatus::ACTIVE ||
gs.status == GoalStatus::PREEMPTING))
{
if (req_result_)
gh_.setAborted(*req_result_);
else
gh_.setAborted();
}
else if (req_succeed_ && gs.status == GoalStatus::ACTIVE)
else if (req_cancel_ && gs.status == GoalStatus::PREEMPTING)
{
if (req_result_)
gh_.setCanceled(*req_result_);
else
gh_.setCanceled();
}
else if (req_succeed_ && (gs.status == GoalStatus::ACTIVE ||
gs.status == GoalStatus::PREEMPTING))
{
if (req_result_)
gh_.setSucceeded(*req_result_);
Expand Down