simple_action_client.h
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 #ifndef ACTIONLIB__CLIENT__SIMPLE_ACTION_CLIENT_H_
36 #define ACTIONLIB__CLIENT__SIMPLE_ACTION_CLIENT_H_
37 
38 #include <boost/thread/condition.hpp>
39 #include <boost/thread/mutex.hpp>
40 #include <boost/scoped_ptr.hpp>
41 #include <boost/concept_check.hpp>
42 
43 #include <string>
44 
45 #include "ros/ros.h"
46 #include "ros/callback_queue.h"
51 
52 #ifndef DEPRECATED
53 #if defined(__GNUC__)
54 #define DEPRECATED __attribute__((deprecated))
55 #else
56 #define DEPRECATED
57 #endif
58 #endif
59 
60 
61 namespace actionlib
62 {
63 
71 template<class ActionSpec>
73 {
74 private:
75  ACTION_DEFINITION(ActionSpec)
76  typedef ClientGoalHandle<ActionSpec> GoalHandleT;
78 
79 public:
80  typedef boost::function<void (const SimpleClientGoalState & state,
81  const ResultConstPtr & result)> SimpleDoneCallback;
82  typedef boost::function<void ()> SimpleActiveCallback;
83  typedef boost::function<void (const FeedbackConstPtr & feedback)> SimpleFeedbackCallback;
84 
93  SimpleActionClient(const std::string & name, bool spin_thread = true)
95  {
96  initSimpleClient(nh_, name, spin_thread);
97  }
98 
109  SimpleActionClient(ros::NodeHandle & n, const std::string & name, bool spin_thread = true)
111  {
112  initSimpleClient(n, name, spin_thread);
113  }
114 
116 
131  bool waitForServer(const ros::Duration & timeout = ros::Duration(0, 0) ) const
132  {
133  return ac_->waitForActionServerToStart(timeout);
134  }
135 
140  bool isServerConnected() const
141  {
142  return ac_->isServerConnected();
143  }
144 
154  void sendGoal(const Goal & goal,
158 
170  SimpleClientGoalState sendGoalAndWait(const Goal & goal,
171  const ros::Duration & execute_timeout = ros::Duration(0, 0),
172  const ros::Duration & preempt_timeout = ros::Duration(0, 0));
173 
179  bool waitForResult(const ros::Duration & timeout = ros::Duration(0, 0));
180 
185  ResultConstPtr getResult() const;
186 
194 
201  void cancelAllGoals();
202 
207  void cancelGoalsAtAndBeforeTime(const ros::Time & time);
208 
212  void cancelGoal();
213 
220  void stopTrackingGoal();
221 
222 private:
226 
228 
229  // Signalling Stuff
230  boost::condition done_condition_;
231  boost::mutex done_mutex_;
232 
233  // User Callbacks
237 
238  // Spin Thread Stuff
239  boost::mutex terminate_mutex_;
241  boost::thread * spin_thread_;
243 
244  boost::scoped_ptr<ActionClientT> ac_; // Action client depends on callback_queue, so it must be destroyed before callback_queue
245 
246  // ***** Private Funcs *****
247  void initSimpleClient(ros::NodeHandle & n, const std::string & name, bool spin_thread);
248  void handleTransition(GoalHandleT gh);
249  void handleFeedback(GoalHandleT gh, const FeedbackConstPtr & feedback);
250  void setSimpleState(const SimpleGoalState::StateEnum & next_state);
251  void setSimpleState(const SimpleGoalState & next_state);
252  void spinThread();
253 };
254 
255 
256 template<class ActionSpec>
258  bool spin_thread)
259 {
260  if (spin_thread) {
261  ROS_DEBUG_NAMED("actionlib", "Spinning up a thread for the SimpleActionClient");
262  need_to_terminate_ = false;
263  spin_thread_ =
264  new boost::thread(boost::bind(&SimpleActionClient<ActionSpec>::spinThread, this));
265  ac_.reset(new ActionClientT(n, name, &callback_queue));
266  } else {
267  spin_thread_ = NULL;
268  ac_.reset(new ActionClientT(n, name));
269  }
270 }
271 
272 template<class ActionSpec>
274 {
275  if (spin_thread_) {
276  {
277  boost::mutex::scoped_lock terminate_lock(terminate_mutex_);
278  need_to_terminate_ = true;
279  }
280  spin_thread_->join();
281  delete spin_thread_;
282  }
283  gh_.reset();
284  ac_.reset();
285 }
286 
287 template<class ActionSpec>
289 {
290  while (nh_.ok()) {
291  {
292  boost::mutex::scoped_lock terminate_lock(terminate_mutex_);
293  if (need_to_terminate_) {
294  break;
295  }
296  }
298  }
299 }
300 
301 template<class ActionSpec>
303 {
304  setSimpleState(SimpleGoalState(next_state) );
305 }
306 
307 template<class ActionSpec>
309 {
310  ROS_DEBUG_NAMED("actionlib", "Transitioning SimpleState from [%s] to [%s]",
311  cur_simple_state_.toString().c_str(),
312  next_state.toString().c_str());
313  cur_simple_state_ = next_state;
314 }
315 
316 template<class ActionSpec>
318  SimpleDoneCallback done_cb,
319  SimpleActiveCallback active_cb,
320  SimpleFeedbackCallback feedback_cb)
321 {
322  // Reset the old GoalHandle, so that our callbacks won't get called anymore
323  gh_.reset();
324 
325  // Store all the callbacks
326  done_cb_ = done_cb;
327  active_cb_ = active_cb;
328  feedback_cb_ = feedback_cb;
329 
331 
332  // Send the goal to the ActionServer
333  gh_ = ac_->sendGoal(goal, boost::bind(&SimpleActionClientT::handleTransition, this, _1),
334  boost::bind(&SimpleActionClientT::handleFeedback, this, _1, _2));
335 }
336 
337 template<class ActionSpec>
339 {
340  if (gh_.isExpired()) {
342  }
343 
344  CommState comm_state_ = gh_.getCommState();
345 
346  switch (comm_state_.state_) {
348  case CommState::PENDING:
351  case CommState::ACTIVE:
354  case CommState::DONE:
355  {
356  switch (gh_.getTerminalState().state_) {
372  case TerminalState::LOST:
374  default:
375  ROS_ERROR_NAMED("actionlib",
376  "Unknown terminal state [%u]. This is a bug in SimpleActionClient",
379  }
380  }
383  {
384  switch (cur_simple_state_.state_) {
390  ROS_ERROR_NAMED("actionlib",
391  "In WAITING_FOR_RESULT or WAITING_FOR_CANCEL_ACK, yet we are in SimpleGoalState DONE. This is a bug in SimpleActionClient");
393  default:
394  ROS_ERROR_NAMED("actionlib",
395  "Got a SimpleGoalState of [%u]. This is a bug in SimpleActionClient",
397  }
398  }
399  default:
400  break;
401  }
402  ROS_ERROR_NAMED("actionlib", "Error trying to interpret CommState - %u", comm_state_.state_);
404 }
405 
406 template<class ActionSpec>
408 const
409 {
410  if (gh_.isExpired()) {
411  ROS_ERROR_NAMED("actionlib",
412  "Trying to getResult() when no goal is running. You are incorrectly using SimpleActionClient");
413  }
414 
415  if (gh_.getResult()) {
416  return gh_.getResult();
417  }
418 
419  return ResultConstPtr(new Result);
420 }
421 
422 
423 template<class ActionSpec>
425 {
426  ac_->cancelAllGoals();
427 }
428 
429 template<class ActionSpec>
431 {
432  ac_->cancelGoalsAtAndBeforeTime(time);
433 }
434 
435 template<class ActionSpec>
437 {
438  if (gh_.isExpired()) {
439  ROS_ERROR_NAMED("actionlib",
440  "Trying to cancelGoal() when no goal is running. You are incorrectly using SimpleActionClient");
441  }
442 
443  gh_.cancel();
444 }
445 
446 template<class ActionSpec>
448 {
449  if (gh_.isExpired()) {
450  ROS_ERROR_NAMED("actionlib",
451  "Trying to stopTrackingGoal() when no goal is running. You are incorrectly using SimpleActionClient");
452  }
453  gh_.reset();
454 }
455 
456 template<class ActionSpec>
458  const FeedbackConstPtr & feedback)
459 {
460  if (gh_ != gh) {
461  ROS_ERROR_NAMED("actionlib",
462  "Got a callback on a goalHandle that we're not tracking. \
463  This is an internal SimpleActionClient/ActionClient bug. \
464  This could also be a GoalID collision");
465  }
466  if (feedback_cb_) {
467  feedback_cb_(feedback);
468  }
469 }
470 
471 template<class ActionSpec>
473 {
474  CommState comm_state_ = gh.getCommState();
475  switch (comm_state_.state_) {
477  ROS_ERROR_NAMED("actionlib",
478  "BUG: Shouldn't ever get a transition callback for WAITING_FOR_GOAL_ACK");
479  break;
480  case CommState::PENDING:
482  "BUG: Got a transition to CommState [%s] when our in SimpleGoalState [%s]",
483  comm_state_.toString().c_str(), cur_simple_state_.toString().c_str());
484  break;
485  case CommState::ACTIVE:
486  switch (cur_simple_state_.state_) {
489  if (active_cb_) {
490  active_cb_();
491  }
492  break;
494  break;
496  ROS_ERROR_NAMED("actionlib",
497  "BUG: Got a transition to CommState [%s] when in SimpleGoalState [%s]",
498  comm_state_.toString().c_str(), cur_simple_state_.toString().c_str());
499  break;
500  default:
501  ROS_FATAL("Unknown SimpleGoalState %u", cur_simple_state_.state_);
502  break;
503  }
504  break;
506  break;
508  break;
511  "BUG: Got a transition to CommState [%s] when our in SimpleGoalState [%s]",
512  comm_state_.toString().c_str(), cur_simple_state_.toString().c_str());
513  break;
515  switch (cur_simple_state_.state_) {
518  if (active_cb_) {
519  active_cb_();
520  }
521  break;
523  break;
525  ROS_ERROR_NAMED("actionlib",
526  "BUG: Got a transition to CommState [%s] when in SimpleGoalState [%s]",
527  comm_state_.toString().c_str(), cur_simple_state_.toString().c_str());
528  break;
529  default:
530  ROS_FATAL("Unknown SimpleGoalState %u", cur_simple_state_.state_);
531  break;
532  }
533  break;
534  case CommState::DONE:
535  switch (cur_simple_state_.state_) {
538  {
539  boost::mutex::scoped_lock lock(done_mutex_);
541  }
542 
543  if (done_cb_) {
544  done_cb_(getState(), gh.getResult());
545  }
546 
547  done_condition_.notify_all();
548  break;
550  ROS_ERROR_NAMED("actionlib", "BUG: Got a second transition to DONE");
551  break;
552  default:
553  ROS_FATAL("Unknown SimpleGoalState %u", cur_simple_state_.state_);
554  break;
555  }
556  break;
557  default:
558  ROS_ERROR_NAMED("actionlib", "Unknown CommState received [%u]", comm_state_.state_);
559  break;
560  }
561 }
562 
563 template<class ActionSpec>
565 {
566  if (gh_.isExpired()) {
567  ROS_ERROR_NAMED("actionlib",
568  "Trying to waitForGoalToFinish() when no goal is running. You are incorrectly using SimpleActionClient");
569  return false;
570  }
571 
572  if (timeout < ros::Duration(0, 0)) {
573  ROS_WARN_NAMED("actionlib", "Timeouts can't be negative. Timeout is [%.2fs]", timeout.toSec());
574  }
575 
576  ros::Time timeout_time = ros::Time::now() + timeout;
577 
578  boost::mutex::scoped_lock lock(done_mutex_);
579 
580  // Hardcode how often we check for node.ok()
581  ros::Duration loop_period = ros::Duration().fromSec(.1);
582 
583  while (nh_.ok()) {
584  // Determine how long we should wait
585  ros::Duration time_left = timeout_time - ros::Time::now();
586 
587  // Check if we're past the timeout time
588  if (timeout > ros::Duration(0, 0) && time_left <= ros::Duration(0, 0) ) {
589  break;
590  }
591 
593  break;
594  }
595 
596 
597  // Truncate the time left
598  if (time_left > loop_period || timeout == ros::Duration()) {
599  time_left = loop_period;
600  }
601 
602  done_condition_.timed_wait(lock,
603  boost::posix_time::milliseconds(static_cast<int64_t>(time_left.toSec() * 1000.0f)));
604  }
605 
607 }
608 
609 template<class ActionSpec>
611  const ros::Duration & execute_timeout,
612  const ros::Duration & preempt_timeout)
613 {
614  sendGoal(goal);
615 
616  // See if the goal finishes in time
617  if (waitForResult(execute_timeout)) {
618  ROS_DEBUG_NAMED("actionlib", "Goal finished within specified execute_timeout [%.2f]",
619  execute_timeout.toSec());
620  return getState();
621  }
622 
623  ROS_DEBUG_NAMED("actionlib", "Goal didn't finish within specified execute_timeout [%.2f]",
624  execute_timeout.toSec());
625 
626  // It didn't finish in time, so we need to preempt it
627  cancelGoal();
628 
629  // Now wait again and see if it finishes
630  if (waitForResult(preempt_timeout)) {
631  ROS_DEBUG_NAMED("actionlib", "Preempt finished within specified preempt_timeout [%.2f]",
632  preempt_timeout.toSec());
633  } else {
634  ROS_DEBUG_NAMED("actionlib", "Preempt didn't finish specified preempt_timeout [%.2f]",
635  preempt_timeout.toSec());
636  }
637  return getState();
638 }
639 
640 } // namespace actionlib
641 
642 #undef DEPRECATED
643 
644 #endif // ACTIONLIB__CLIENT__SIMPLE_ACTION_CLIENT_H_
boost::scoped_ptr< ActionClientT > ac_
boost::function< void(const FeedbackConstPtr &feedback)> SimpleFeedbackCallback
#define ROS_FATAL(...)
bool waitForServer(const ros::Duration &timeout=ros::Duration(0, 0)) const
Waits for the ActionServer to connect to this client.
#define ROS_WARN_NAMED(name,...)
f
bool waitForResult(const ros::Duration &timeout=ros::Duration(0, 0))
Blocks until this goal finishes.
Full interface to an ActionServer.
Definition: action_client.h:59
#define ROS_ERROR_COND(cond,...)
bool isServerConnected() const
Checks if the action client is successfully connected to the action server.
#define ROS_DEBUG_NAMED(name,...)
std::string toString() const
Definition: comm_state.h:86
StateEnum
Defines the various states the SimpleGoalState can be in.
Thin wrapper around an enum in order to help interpret the state of the communication state machine...
Definition: comm_state.h:47
Thin wrapper around an enum in order providing a simplified version of the communication state...
void cancelGoal()
Cancel the goal that we are currently pursuing.
Duration & fromSec(double t)
void setSimpleState(const SimpleGoalState::StateEnum &next_state)
SimpleClientGoalState sendGoalAndWait(const Goal &goal, const ros::Duration &execute_timeout=ros::Duration(0, 0), const ros::Duration &preempt_timeout=ros::Duration(0, 0))
Sends a goal to the ActionServer, and waits until the goal completes or a timeout is exceeded...
void cancelGoalsAtAndBeforeTime(const ros::Time &time)
Cancel all goals that were stamped at and before the specified time.
bool isExpired() const
Checks if this goal handle is tracking a goal.
A Simple client implementation of the ActionInterface which supports only one goal at a time...
SimpleActionClient(ros::NodeHandle &n, const std::string &name, bool spin_thread=true)
Constructor with namespacing options.
void handleTransition(GoalHandleT gh)
boost::function< void(const SimpleClientGoalState &state, const ResultConstPtr &result)> SimpleDoneCallback
std::string toString() const
#define ACTION_DEFINITION(ActionSpec)
void sendGoal(const Goal &goal, SimpleDoneCallback done_cb=SimpleDoneCallback(), SimpleActiveCallback active_cb=SimpleActiveCallback(), SimpleFeedbackCallback feedback_cb=SimpleFeedbackCallback())
Sends a goal to the ActionServer, and also registers callbacks.
CommState getCommState() const
Get the state of this goal&#39;s communication state machine from interaction with the server...
void reset()
Stops goal handle from tracking a goal.
TerminalState getTerminalState() const
Get the terminal state information for this goal.
#define ROS_ERROR_NAMED(name,...)
static Time now()
void cancel()
Sends a cancel message for this specific goal to the ActionServer.
void stopTrackingGoal()
Stops tracking the state of the current goal. Unregisters this goal&#39;s callbacks.
ActionClient< ActionSpec > ActionClientT
SimpleClientGoalState getState() const
Get the state information for this goal.
bool ok() const
Client side handle to monitor goal progress.
ResultConstPtr getResult() const
Get the Result of the current goal.
void initSimpleClient(ros::NodeHandle &n, const std::string &name, bool spin_thread)
void cancelAllGoals()
Cancel all goals currently running on the action server.
void handleFeedback(GoalHandleT gh, const FeedbackConstPtr &feedback)
SimpleFeedbackCallback feedback_cb_
ResultConstPtr getResult() const
Get result associated with this goal.


actionlib
Author(s): Eitan Marder-Eppstein, Vijay Pradeep, Mikael Arguedas
autogenerated on Mon Aug 24 2020 03:40:47