From be6d89af69f4c6ba12f18ced7fe59f2b9a3f697e Mon Sep 17 00:00:00 2001 From: Miguel Prada Date: Thu, 28 Apr 2016 14:26:51 +0200 Subject: [PATCH 1/2] Add feedback sending capability to RealtimeServerGoalHandle. --- .../realtime_server_goal_handle.h | 19 +++++++++++++++++-- 1 file changed, 17 insertions(+), 2 deletions(-) diff --git a/include/realtime_tools/realtime_server_goal_handle.h b/include/realtime_tools/realtime_server_goal_handle.h index d407e451..9936c2b9 100644 --- a/include/realtime_tools/realtime_server_goal_handle.h +++ b/include/realtime_tools/realtime_server_goal_handle.h @@ -50,25 +50,31 @@ class RealtimeServerGoalHandle typedef actionlib::ServerGoalHandle GoalHandle; typedef boost::shared_ptr ResultPtr; + typedef boost::shared_ptr FeedbackPtr; uint8_t state_; bool req_abort_; bool req_succeed_; ResultConstPtr req_result_; + FeedbackConstPtr req_feedback_; public: GoalHandle gh_; ResultPtr preallocated_result_; // Preallocated so it can be used in realtime + FeedbackPtr preallocated_feedback_; // Preallocated so it can be used in realtime - RealtimeServerGoalHandle(GoalHandle &gh, const ResultPtr &preallocated_result = ResultPtr((Result*)NULL)) + RealtimeServerGoalHandle(GoalHandle &gh, const ResultPtr &preallocated_result = ResultPtr((Result*)NULL), const FeedbackPtr &preallocated_feedback = FeedbackPtr((Feedback*)NULL)) : req_abort_(false), req_succeed_(false), gh_(gh), - preallocated_result_(preallocated_result) + preallocated_result_(preallocated_result), + preallocated_feedback_(preallocated_feedback) { if (!preallocated_result_) preallocated_result_.reset(new Result); + if (!preallocated_feedback_) + preallocated_feedback_.reset(new Feedback); } void setAborted(ResultConstPtr result = ResultConstPtr((Result*)NULL)) @@ -89,6 +95,11 @@ class RealtimeServerGoalHandle } } + void setFeedback(FeedbackConstPtr feedback = FeedbackConstPtr((Feedback*)NULL)) + { + req_feedback_ = feedback; + } + bool valid() { return gh_.getGoal() != NULL; @@ -114,6 +125,10 @@ class RealtimeServerGoalHandle else gh_.setSucceeded(); } + if (req_feedback_) + { + gh_.publishFeedback(*req_feedback_); + } } } }; From 5a41a8d77adf56233699693557f9843667db7d2b Mon Sep 17 00:00:00 2001 From: Miguel Prada Date: Thu, 19 May 2016 16:57:07 +0200 Subject: [PATCH 2/2] Check goal status to avoid sending stale feedback if goal is not active anymore. --- include/realtime_tools/realtime_server_goal_handle.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/realtime_tools/realtime_server_goal_handle.h b/include/realtime_tools/realtime_server_goal_handle.h index 9936c2b9..ed1be2af 100644 --- a/include/realtime_tools/realtime_server_goal_handle.h +++ b/include/realtime_tools/realtime_server_goal_handle.h @@ -125,7 +125,7 @@ class RealtimeServerGoalHandle else gh_.setSucceeded(); } - if (req_feedback_) + if (req_feedback_ && gs.status == GoalStatus::ACTIVE) { gh_.publishFeedback(*req_feedback_); }