server_goal_handle_imp.h
Go to the documentation of this file.
1 /*********************************************************************
2 *
3 * Software License Agreement (BSD License)
4 *
5 * Copyright (c) 2008, Willow Garage, Inc.
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of Willow Garage, Inc. nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 *
35 * Author: Eitan Marder-Eppstein
36 *********************************************************************/
37 #ifndef ACTIONLIB__SERVER__SERVER_GOAL_HANDLE_IMP_H_
38 #define ACTIONLIB__SERVER__SERVER_GOAL_HANDLE_IMP_H_
39 
40 #include <list>
41 #include <string>
42 
43 #include "ros/console.h"
44 
45 namespace actionlib
46 {
47 template<class ActionSpec>
48 ServerGoalHandle<ActionSpec>::ServerGoalHandle()
49 : as_(nullptr) {}
50 
51 template<class ActionSpec>
52 ServerGoalHandle<ActionSpec>::ServerGoalHandle(const ServerGoalHandle & gh)
53 : status_it_(gh.status_it_), goal_(gh.goal_), as_(gh.as_), handle_tracker_(gh.handle_tracker_),
54  guard_(gh.guard_) {}
55 
56 template<class ActionSpec>
57 void ServerGoalHandle<ActionSpec>::setAccepted(const std::string & text)
58 {
59  if (as_ == nullptr) {
60  ROS_ERROR_NAMED("actionlib",
61  "You are attempting to call methods on an uninitialized goal handle");
62  return;
63  }
64 
65  // check to see if we can use the action server
66  DestructionGuard::ScopedProtector protector(*guard_);
67  if (!protector.isProtected()) {
68  ROS_ERROR_NAMED("actionlib",
69  "The ActionServer associated with this GoalHandle is invalid. Did you delete the ActionServer before the GoalHandle?");
70  return;
71  }
72 
73  ROS_DEBUG_NAMED("actionlib", "Accepting goal, id: %s, stamp: %.2f",
74  getGoalID().id.c_str(), getGoalID().stamp.toSec());
75  if (goal_) {
76  boost::recursive_mutex::scoped_lock lock(as_->lock_);
77  unsigned int status = (*status_it_).status_.status;
78 
79  // if we were pending before, then we'll go active
80  if (status == actionlib_msgs::GoalStatus::PENDING) {
81  (*status_it_).status_.status = actionlib_msgs::GoalStatus::ACTIVE;
82  (*status_it_).status_.text = text;
83  as_->publishStatus();
84  } else if (status == actionlib_msgs::GoalStatus::RECALLING) {
85  // if we were recalling before, now we'll go to preempting
86  (*status_it_).status_.status = actionlib_msgs::GoalStatus::PREEMPTING;
87  (*status_it_).status_.text = text;
88  as_->publishStatus();
89  } else {
90  ROS_ERROR_NAMED("actionlib",
91  "To transition to an active state, the goal must be in a pending or recalling state, it is currently in state: %d",
92  (*status_it_).status_.status);
93  }
94  } else {
95  ROS_ERROR_NAMED("actionlib", "Attempt to set status on an uninitialized ServerGoalHandle");
96  }
97 }
98 
99 template<class ActionSpec>
100 void ServerGoalHandle<ActionSpec>::setCanceled(const Result & result, const std::string & text)
101 {
102  if (as_ == nullptr) {
103  ROS_ERROR_NAMED("actionlib",
104  "You are attempting to call methods on an uninitialized goal handle");
105  return;
106  }
107 
108  // check to see if we can use the action server
109  DestructionGuard::ScopedProtector protector(*guard_);
110  if (!protector.isProtected()) {
111  ROS_ERROR_NAMED("actionlib",
112  "The ActionServer associated with this GoalHandle is invalid. Did you delete the ActionServer before the GoalHandle?");
113  return;
114  }
115 
116  ROS_DEBUG_NAMED("actionlib", "Setting status to canceled on goal, id: %s, stamp: %.2f",
117  getGoalID().id.c_str(), getGoalID().stamp.toSec());
118  if (goal_) {
119  boost::recursive_mutex::scoped_lock lock(as_->lock_);
120  unsigned int status = (*status_it_).status_.status;
121  if (status == actionlib_msgs::GoalStatus::PENDING ||
122  status == actionlib_msgs::GoalStatus::RECALLING)
123  {
124  (*status_it_).status_.status = actionlib_msgs::GoalStatus::RECALLED;
125  (*status_it_).status_.text = text;
126  as_->publishResult((*status_it_).status_, result);
127  } else if (status == actionlib_msgs::GoalStatus::ACTIVE ||
128  status == actionlib_msgs::GoalStatus::PREEMPTING) {
129  (*status_it_).status_.status = actionlib_msgs::GoalStatus::PREEMPTED;
130  (*status_it_).status_.text = text;
131  as_->publishResult((*status_it_).status_, result);
132  } else {
133  ROS_ERROR_NAMED("actionlib",
134  "To transition to a cancelled state, the goal must be in a pending, recalling, active, or preempting state, it is currently in state: %d",
135  (*status_it_).status_.status);
136  }
137  } else {
138  ROS_ERROR_NAMED("actionlib", "Attempt to set status on an uninitialized ServerGoalHandle");
139  }
140 }
141 
142 template<class ActionSpec>
143 void ServerGoalHandle<ActionSpec>::setRejected(const Result & result, const std::string & text)
144 {
145  if (as_ == nullptr) {
146  ROS_ERROR_NAMED("actionlib",
147  "You are attempting to call methods on an uninitialized goal handle");
148  return;
149  }
150 
151  // check to see if we can use the action server
152  DestructionGuard::ScopedProtector protector(*guard_);
153  if (!protector.isProtected()) {
154  ROS_ERROR_NAMED("actionlib",
155  "The ActionServer associated with this GoalHandle is invalid. Did you delete the ActionServer before the GoalHandle?");
156  return;
157  }
158 
159  ROS_DEBUG_NAMED("actionlib", "Setting status to rejected on goal, id: %s, stamp: %.2f",
160  getGoalID().id.c_str(), getGoalID().stamp.toSec());
161  if (goal_) {
162  boost::recursive_mutex::scoped_lock lock(as_->lock_);
163  unsigned int status = (*status_it_).status_.status;
164  if (status == actionlib_msgs::GoalStatus::PENDING ||
165  status == actionlib_msgs::GoalStatus::RECALLING)
166  {
167  (*status_it_).status_.status = actionlib_msgs::GoalStatus::REJECTED;
168  (*status_it_).status_.text = text;
169  as_->publishResult((*status_it_).status_, result);
170  } else {
171  ROS_ERROR_NAMED("actionlib",
172  "To transition to a rejected state, the goal must be in a pending or recalling state, it is currently in state: %d",
173  (*status_it_).status_.status);
174  }
175  } else {
176  ROS_ERROR_NAMED("actionlib", "Attempt to set status on an uninitialized ServerGoalHandle");
177  }
178 }
179 
180 template<class ActionSpec>
181 void ServerGoalHandle<ActionSpec>::setAborted(const Result & result, const std::string & text)
182 {
183  if (as_ == nullptr) {
184  ROS_ERROR_NAMED("actionlib",
185  "You are attempting to call methods on an uninitialized goal handle");
186  return;
187  }
188 
189  // check to see if we can use the action server
190  DestructionGuard::ScopedProtector protector(*guard_);
191  if (!protector.isProtected()) {
192  ROS_ERROR_NAMED("actionlib",
193  "The ActionServer associated with this GoalHandle is invalid. Did you delete the ActionServer before the GoalHandle?");
194  return;
195  }
196 
197  ROS_DEBUG_NAMED("actionlib", "Setting status to aborted on goal, id: %s, stamp: %.2f",
198  getGoalID().id.c_str(), getGoalID().stamp.toSec());
199  if (goal_) {
200  boost::recursive_mutex::scoped_lock lock(as_->lock_);
201  unsigned int status = (*status_it_).status_.status;
202  if (status == actionlib_msgs::GoalStatus::PREEMPTING ||
203  status == actionlib_msgs::GoalStatus::ACTIVE)
204  {
205  (*status_it_).status_.status = actionlib_msgs::GoalStatus::ABORTED;
206  (*status_it_).status_.text = text;
207  as_->publishResult((*status_it_).status_, result);
208  } else {
209  ROS_ERROR_NAMED("actionlib",
210  "To transition to an aborted state, the goal must be in a preempting or active state, it is currently in state: %d",
211  status);
212  }
213  } else {
214  ROS_ERROR_NAMED("actionlib", "Attempt to set status on an uninitialized ServerGoalHandle");
215  }
216 }
217 
218 template<class ActionSpec>
219 void ServerGoalHandle<ActionSpec>::setSucceeded(const Result & result, const std::string & text)
220 {
221  if (as_ == nullptr) {
222  ROS_ERROR_NAMED("actionlib",
223  "You are attempting to call methods on an uninitialized goal handle");
224  return;
225  }
226 
227  // check to see if we can use the action server
228  DestructionGuard::ScopedProtector protector(*guard_);
229  if (!protector.isProtected()) {
230  ROS_ERROR_NAMED("actionlib",
231  "The ActionServer associated with this GoalHandle is invalid. Did you delete the ActionServer before the GoalHandle?");
232  return;
233  }
234 
235  ROS_DEBUG_NAMED("actionlib", "Setting status to succeeded on goal, id: %s, stamp: %.2f",
236  getGoalID().id.c_str(), getGoalID().stamp.toSec());
237  if (goal_) {
238  boost::recursive_mutex::scoped_lock lock(as_->lock_);
239  unsigned int status = (*status_it_).status_.status;
240  if (status == actionlib_msgs::GoalStatus::PREEMPTING ||
241  status == actionlib_msgs::GoalStatus::ACTIVE)
242  {
243  (*status_it_).status_.status = actionlib_msgs::GoalStatus::SUCCEEDED;
244  (*status_it_).status_.text = text;
245  as_->publishResult((*status_it_).status_, result);
246  } else {
247  ROS_ERROR_NAMED("actionlib",
248  "To transition to a succeeded state, the goal must be in a preempting or active state, it is currently in state: %d",
249  status);
250  }
251  } else {
252  ROS_ERROR_NAMED("actionlib", "Attempt to set status on an uninitialized ServerGoalHandle");
253  }
254 }
255 
256 template<class ActionSpec>
257 void ServerGoalHandle<ActionSpec>::publishFeedback(const Feedback & feedback)
258 {
259  if (as_ == nullptr) {
260  ROS_ERROR_NAMED("actionlib",
261  "You are attempting to call methods on an uninitialized goal handle");
262  return;
263  }
264 
265  // check to see if we can use the action server
266  DestructionGuard::ScopedProtector protector(*guard_);
267  if (!protector.isProtected()) {
268  ROS_ERROR_NAMED("actionlib",
269  "The ActionServer associated with this GoalHandle is invalid. Did you delete the ActionServer before the GoalHandle?");
270  return;
271  }
272 
273  ROS_DEBUG_NAMED("actionlib", "Publishing feedback for goal, id: %s, stamp: %.2f",
274  getGoalID().id.c_str(), getGoalID().stamp.toSec());
275  if (goal_) {
276  boost::recursive_mutex::scoped_lock lock(as_->lock_);
277  as_->publishFeedback((*status_it_).status_, feedback);
278  } else {
279  ROS_ERROR_NAMED("actionlib",
280  "Attempt to publish feedback on an uninitialized ServerGoalHandle");
281  }
282 }
283 
284 template<class ActionSpec>
285 bool ServerGoalHandle<ActionSpec>::isValid() const
286 {
287  return goal_ && as_ != nullptr;
288 }
289 
290 template<class ActionSpec>
291 boost::shared_ptr<const typename ServerGoalHandle<ActionSpec>::Goal> ServerGoalHandle<ActionSpec>::
292 getGoal() const
293 {
294  // if we have a goal that is non-null
295  if (goal_) {
296  // create the deleter for our goal subtype
297  EnclosureDeleter<const ActionGoal> d(goal_);
298  return boost::shared_ptr<const Goal>(&(goal_->goal), d);
299  }
300  return boost::shared_ptr<const Goal>();
301 }
302 
303 template<class ActionSpec>
304 actionlib_msgs::GoalID ServerGoalHandle<ActionSpec>::getGoalID() const
305 {
306  if (goal_ && as_ != nullptr) {
307  DestructionGuard::ScopedProtector protector(*guard_);
308  if (protector.isProtected()) {
309  boost::recursive_mutex::scoped_lock lock(as_->lock_);
310  return (*status_it_).status_.goal_id;
311  } else {
312  return actionlib_msgs::GoalID();
313  }
314  } else {
315  ROS_ERROR_NAMED("actionlib",
316  "Attempt to get a goal id on an uninitialized ServerGoalHandle or one that has no ActionServer associated with it.");
317  return actionlib_msgs::GoalID();
318  }
319 }
320 
321 template<class ActionSpec>
322 actionlib_msgs::GoalStatus ServerGoalHandle<ActionSpec>::getGoalStatus() const
323 {
324  if (goal_ && as_ != nullptr) {
325  DestructionGuard::ScopedProtector protector(*guard_);
326  if (protector.isProtected()) {
327  boost::recursive_mutex::scoped_lock lock(as_->lock_);
328  return (*status_it_).status_;
329  } else {
330  return actionlib_msgs::GoalStatus();
331  }
332  } else {
333  ROS_ERROR_NAMED("actionlib",
334  "Attempt to get goal status on an uninitialized ServerGoalHandle or one that has no ActionServer associated with it.");
335  return actionlib_msgs::GoalStatus();
336  }
337 }
338 
339 template<class ActionSpec>
340 ServerGoalHandle<ActionSpec> & ServerGoalHandle<ActionSpec>::operator=(const ServerGoalHandle & gh)
341 {
342  status_it_ = gh.status_it_;
343  goal_ = gh.goal_;
344  as_ = gh.as_;
345  handle_tracker_ = gh.handle_tracker_;
346  guard_ = gh.guard_;
347  return *this;
348 }
349 
350 template<class ActionSpec>
351 bool ServerGoalHandle<ActionSpec>::operator==(const ServerGoalHandle & other) const
352 {
353  if (!goal_ && !other.goal_) {
354  return true;
355  }
356 
357  if (!goal_ || !other.goal_) {
358  return false;
359  }
360 
361  actionlib_msgs::GoalID my_id = getGoalID();
362  actionlib_msgs::GoalID their_id = other.getGoalID();
363  return my_id.id == their_id.id;
364 }
365 
366 template<class ActionSpec>
367 bool ServerGoalHandle<ActionSpec>::operator!=(const ServerGoalHandle & other) const
368 {
369  return !(*this == other);
370 }
371 
372 template<class ActionSpec>
373 ServerGoalHandle<ActionSpec>::ServerGoalHandle(
374  typename std::list<StatusTracker<ActionSpec> >::iterator status_it,
375  ActionServerBase<ActionSpec> * as, boost::shared_ptr<void> handle_tracker,
376  boost::shared_ptr<DestructionGuard> guard)
377 : status_it_(status_it), goal_((*status_it).goal_),
378  as_(as), handle_tracker_(handle_tracker), guard_(guard) {}
379 
380 template<class ActionSpec>
381 bool ServerGoalHandle<ActionSpec>::setCancelRequested()
382 {
383  if (as_ == nullptr) {
384  ROS_ERROR_NAMED("actionlib",
385  "You are attempting to call methods on an uninitialized goal handle");
386  return false;
387  }
388 
389  // check to see if we can use the action server
390  DestructionGuard::ScopedProtector protector(*guard_);
391  if (!protector.isProtected()) {
392  ROS_ERROR_NAMED("actionlib",
393  "The ActionServer associated with this GoalHandle is invalid. Did you delete the ActionServer before the GoalHandle?");
394  return false;
395  }
396 
397  ROS_DEBUG_NAMED("actionlib",
398  "Transitioning to a cancel requested state on goal id: %s, stamp: %.2f",
399  getGoalID().id.c_str(), getGoalID().stamp.toSec());
400  if (goal_) {
401  boost::recursive_mutex::scoped_lock lock(as_->lock_);
402  unsigned int status = (*status_it_).status_.status;
403  if (status == actionlib_msgs::GoalStatus::PENDING) {
404  (*status_it_).status_.status = actionlib_msgs::GoalStatus::RECALLING;
405  as_->publishStatus();
406  return true;
407  }
408 
409  if (status == actionlib_msgs::GoalStatus::ACTIVE) {
410  (*status_it_).status_.status = actionlib_msgs::GoalStatus::PREEMPTING;
411  as_->publishStatus();
412  return true;
413  }
414  }
415  return false;
416 }
417 } // namespace actionlib
418 #endif // ACTIONLIB__SERVER__SERVER_GOAL_HANDLE_IMP_H_
actionlib::ServerGoalHandle::setCancelRequested
bool setCancelRequested()
A private method to set status to PENDING or RECALLING.
Definition: server_goal_handle_imp.h:416
actionlib::ServerGoalHandle::as_
ActionServerBase< ActionSpec > * as_
Definition: server_goal_handle.h:222
actionlib::DestructionGuard::ScopedProtector::isProtected
bool isProtected()
Checks if the ScopedProtector successfully protected the DestructionGuard.
Definition: destruction_guard.h:145
actionlib::DestructionGuard::ScopedProtector
Protects a DestructionGuard until this object goes out of scope.
Definition: destruction_guard.h:128
actionlib::ServerGoalHandle::handle_tracker_
boost::shared_ptr< void > handle_tracker_
Definition: server_goal_handle.h:223
boost::shared_ptr
actionlib::ActionServerBase
The ActionServerBase implements the logic for an ActionServer.
Definition: action_server_base.h:99
actionlib::ServerGoalHandle
Encapsulates a state machine for a given goal that the user can trigger transitions on....
Definition: server_goal_handle.h:98
ROS_ERROR_NAMED
#define ROS_ERROR_NAMED(name,...)
actionlib::ServerGoalHandle::goal_
boost::shared_ptr< const ActionGoal > goal_
Definition: server_goal_handle.h:221
console.h
ROS_DEBUG_NAMED
#define ROS_DEBUG_NAMED(name,...)
actionlib::ServerGoalHandle::ServerGoalHandle
ServerGoalHandle()
Default constructor for a ServerGoalHandle.
Definition: server_goal_handle_imp.h:83
actionlib::ServerGoalHandle::guard_
boost::shared_ptr< DestructionGuard > guard_
Definition: server_goal_handle.h:224
setup.d
d
Definition: setup.py:6
actionlib::ServerGoalHandle::getGoalID
actionlib_msgs::GoalID getGoalID() const
Accessor for the goal id associated with the ServerGoalHandle.
Definition: server_goal_handle_imp.h:339
actionlib::ServerGoalHandle::status_it_
std::list< StatusTracker< ActionSpec > >::iterator status_it_
Definition: server_goal_handle.h:220
actionlib
Definition: action_definition.h:40
actionlib::EnclosureDeleter
Definition: enclosure_deleter.h:92

actionlib
Author(s): Eitan Marder-Eppstein, Vijay Pradeep, Mikael Arguedas
autogenerated on Fri May 19 2023 02:36:55