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);
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,
155  SimpleDoneCallback done_cb = SimpleDoneCallback(),
156  SimpleActiveCallback active_cb = SimpleActiveCallback(),
157  SimpleFeedbackCallback feedback_cb = SimpleFeedbackCallback());
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:
225  GoalHandleT gh_;
226 
228 
229  // Signalling Stuff
230  boost::condition done_condition_;
231  boost::mutex done_mutex_;
232 
233  // User Callbacks
234  SimpleDoneCallback done_cb_;
235  SimpleActiveCallback active_cb_;
236  SimpleFeedbackCallback feedback_cb_;
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()) {
341  ROS_ERROR_NAMED("actionlib",
342  "Trying to getState() when no goal is running. You are incorrectly using SimpleActionClient");
344  }
345 
346  CommState comm_state_ = gh_.getCommState();
347 
348  switch (comm_state_.state_) {
350  case CommState::PENDING:
353  case CommState::ACTIVE:
356  case CommState::DONE:
357  {
358  switch (gh_.getTerminalState().state_) {
374  case TerminalState::LOST:
376  default:
377  ROS_ERROR_NAMED("actionlib",
378  "Unknown terminal state [%u]. This is a bug in SimpleActionClient",
381  }
382  }
385  {
386  switch (cur_simple_state_.state_) {
392  ROS_ERROR_NAMED("actionlib",
393  "In WAITING_FOR_RESULT or WAITING_FOR_CANCEL_ACK, yet we are in SimpleGoalState DONE. This is a bug in SimpleActionClient");
395  default:
396  ROS_ERROR_NAMED("actionlib",
397  "Got a SimpleGoalState of [%u]. This is a bug in SimpleActionClient",
399  }
400  }
401  default:
402  break;
403  }
404  ROS_ERROR_NAMED("actionlib", "Error trying to interpret CommState - %u", comm_state_.state_);
406 }
407 
408 template<class ActionSpec>
410 const
411 {
412  if (gh_.isExpired()) {
413  ROS_ERROR_NAMED("actionlib",
414  "Trying to getResult() when no goal is running. You are incorrectly using SimpleActionClient");
415  }
416 
417  if (gh_.getResult()) {
418  return gh_.getResult();
419  }
420 
421  return ResultConstPtr(new Result);
422 }
423 
424 
425 template<class ActionSpec>
427 {
428  ac_->cancelAllGoals();
429 }
430 
431 template<class ActionSpec>
433 {
434  ac_->cancelGoalsAtAndBeforeTime(time);
435 }
436 
437 template<class ActionSpec>
439 {
440  if (gh_.isExpired()) {
441  ROS_ERROR_NAMED("actionlib",
442  "Trying to cancelGoal() when no goal is running. You are incorrectly using SimpleActionClient");
443  }
444 
445  gh_.cancel();
446 }
447 
448 template<class ActionSpec>
450 {
451  if (gh_.isExpired()) {
452  ROS_ERROR_NAMED("actionlib",
453  "Trying to stopTrackingGoal() when no goal is running. You are incorrectly using SimpleActionClient");
454  }
455  gh_.reset();
456 }
457 
458 template<class ActionSpec>
460  const FeedbackConstPtr & feedback)
461 {
462  if (gh_ != gh) {
463  ROS_ERROR_NAMED("actionlib",
464  "Got a callback on a goalHandle that we're not tracking. \
465  This is an internal SimpleActionClient/ActionClient bug. \
466  This could also be a GoalID collision");
467  }
468  if (feedback_cb_) {
469  feedback_cb_(feedback);
470  }
471 }
472 
473 template<class ActionSpec>
475 {
476  CommState comm_state_ = gh.getCommState();
477  switch (comm_state_.state_) {
479  ROS_ERROR_NAMED("actionlib",
480  "BUG: Shouldn't ever get a transition callback for WAITING_FOR_GOAL_ACK");
481  break;
482  case CommState::PENDING:
484  "BUG: Got a transition to CommState [%s] when our in SimpleGoalState [%s]",
485  comm_state_.toString().c_str(), cur_simple_state_.toString().c_str());
486  break;
487  case CommState::ACTIVE:
488  switch (cur_simple_state_.state_) {
491  if (active_cb_) {
492  active_cb_();
493  }
494  break;
496  break;
498  ROS_ERROR_NAMED("actionlib",
499  "BUG: Got a transition to CommState [%s] when in SimpleGoalState [%s]",
500  comm_state_.toString().c_str(), cur_simple_state_.toString().c_str());
501  break;
502  default:
503  ROS_FATAL("Unknown SimpleGoalState %u", cur_simple_state_.state_);
504  break;
505  }
506  break;
508  break;
510  break;
513  "BUG: Got a transition to CommState [%s] when our in SimpleGoalState [%s]",
514  comm_state_.toString().c_str(), cur_simple_state_.toString().c_str());
515  break;
517  switch (cur_simple_state_.state_) {
520  if (active_cb_) {
521  active_cb_();
522  }
523  break;
525  break;
527  ROS_ERROR_NAMED("actionlib",
528  "BUG: Got a transition to CommState [%s] when in SimpleGoalState [%s]",
529  comm_state_.toString().c_str(), cur_simple_state_.toString().c_str());
530  break;
531  default:
532  ROS_FATAL("Unknown SimpleGoalState %u", cur_simple_state_.state_);
533  break;
534  }
535  break;
536  case CommState::DONE:
537  switch (cur_simple_state_.state_) {
540  {
541  boost::mutex::scoped_lock lock(done_mutex_);
543  }
544 
545  if (done_cb_) {
546  done_cb_(getState(), gh.getResult());
547  }
548 
549  done_condition_.notify_all();
550  break;
552  ROS_ERROR_NAMED("actionlib", "BUG: Got a second transition to DONE");
553  break;
554  default:
555  ROS_FATAL("Unknown SimpleGoalState %u", cur_simple_state_.state_);
556  break;
557  }
558  break;
559  default:
560  ROS_ERROR_NAMED("actionlib", "Unknown CommState received [%u]", comm_state_.state_);
561  break;
562  }
563 }
564 
565 template<class ActionSpec>
567 {
568  if (gh_.isExpired()) {
569  ROS_ERROR_NAMED("actionlib",
570  "Trying to waitForGoalToFinish() when no goal is running. You are incorrectly using SimpleActionClient");
571  return false;
572  }
573 
574  if (timeout < ros::Duration(0, 0)) {
575  ROS_WARN_NAMED("actionlib", "Timeouts can't be negative. Timeout is [%.2fs]", timeout.toSec());
576  }
577 
578  ros::Time timeout_time = ros::Time::now() + timeout;
579 
580  boost::mutex::scoped_lock lock(done_mutex_);
581 
582  // Hardcode how often we check for node.ok()
583  ros::Duration loop_period = ros::Duration().fromSec(.1);
584 
585  while (nh_.ok()) {
586  // Determine how long we should wait
587  ros::Duration time_left = timeout_time - ros::Time::now();
588 
589  // Check if we're past the timeout time
590  if (timeout > ros::Duration(0, 0) && time_left <= ros::Duration(0, 0) ) {
591  break;
592  }
593 
595  break;
596  }
597 
598 
599  // Truncate the time left
600  if (time_left > loop_period || timeout == ros::Duration()) {
601  time_left = loop_period;
602  }
603 
604  done_condition_.timed_wait(lock, boost::posix_time::milliseconds(time_left.toSec() * 1000.0f));
605  }
606 
608 }
609 
610 template<class ActionSpec>
612  const ros::Duration & execute_timeout,
613  const ros::Duration & preempt_timeout)
614 {
615  sendGoal(goal);
616 
617  // See if the goal finishes in time
618  if (waitForResult(execute_timeout)) {
619  ROS_DEBUG_NAMED("actionlib", "Goal finished within specified execute_timeout [%.2f]",
620  execute_timeout.toSec());
621  return getState();
622  }
623 
624  ROS_DEBUG_NAMED("actionlib", "Goal didn't finish within specified execute_timeout [%.2f]",
625  execute_timeout.toSec());
626 
627  // It didn't finish in time, so we need to preempt it
628  cancelGoal();
629 
630  // Now wait again and see if it finishes
631  if (waitForResult(preempt_timeout)) {
632  ROS_DEBUG_NAMED("actionlib", "Preempt finished within specified preempt_timeout [%.2f]",
633  preempt_timeout.toSec());
634  } else {
635  ROS_DEBUG_NAMED("actionlib", "Preempt didn't finish specified preempt_timeout [%.2f]",
636  preempt_timeout.toSec());
637  }
638  return getState();
639 }
640 
641 } // namespace actionlib
642 
643 #undef DEPRECATED
644 
645 #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.
SimpleActionClient< ActionSpec > SimpleActionClientT
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.
SimpleActionClient(const std::string &name, bool spin_thread=true)
Simple constructor.
#define ROS_DEBUG_NAMED(name,...)
std::string toString() const
Definition: comm_state.h:86
boost::function< void()> SimpleActiveCallback
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
ClientGoalHandle< ActionSpec > GoalHandleT
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
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
autogenerated on Thu Dec 21 2017 03:40:06