00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #ifndef ACTIONLIB__CLIENT__SIMPLE_ACTION_CLIENT_H_
00036 #define ACTIONLIB__CLIENT__SIMPLE_ACTION_CLIENT_H_
00037
00038 #include <boost/thread/condition.hpp>
00039 #include <boost/thread/mutex.hpp>
00040 #include <boost/scoped_ptr.hpp>
00041 #include <boost/concept_check.hpp>
00042
00043 #include <string>
00044
00045 #include "ros/ros.h"
00046 #include "ros/callback_queue.h"
00047 #include "actionlib/client/action_client.h"
00048 #include "actionlib/client/simple_goal_state.h"
00049 #include "actionlib/client/simple_client_goal_state.h"
00050 #include "actionlib/client/terminal_state.h"
00051
00052 #ifndef DEPRECATED
00053 #if defined(__GNUC__)
00054 #define DEPRECATED __attribute__((deprecated))
00055 #else
00056 #define DEPRECATED
00057 #endif
00058 #endif
00059
00060
00061 namespace actionlib
00062 {
00063
00071 template<class ActionSpec>
00072 class SimpleActionClient
00073 {
00074 private:
00075 ACTION_DEFINITION(ActionSpec);
00076 typedef ClientGoalHandle<ActionSpec> GoalHandleT;
00077 typedef SimpleActionClient<ActionSpec> SimpleActionClientT;
00078
00079 public:
00080 typedef boost::function<void (const SimpleClientGoalState & state,
00081 const ResultConstPtr & result)> SimpleDoneCallback;
00082 typedef boost::function<void ()> SimpleActiveCallback;
00083 typedef boost::function<void (const FeedbackConstPtr & feedback)> SimpleFeedbackCallback;
00084
00093 SimpleActionClient(const std::string & name, bool spin_thread = true)
00094 : cur_simple_state_(SimpleGoalState::PENDING)
00095 {
00096 initSimpleClient(nh_, name, spin_thread);
00097 }
00098
00109 SimpleActionClient(ros::NodeHandle & n, const std::string & name, bool spin_thread = true)
00110 : cur_simple_state_(SimpleGoalState::PENDING)
00111 {
00112 initSimpleClient(n, name, spin_thread);
00113 }
00114
00115 ~SimpleActionClient();
00116
00131 bool waitForServer(const ros::Duration & timeout = ros::Duration(0, 0) ) const
00132 {
00133 return ac_->waitForActionServerToStart(timeout);
00134 }
00135
00140 bool isServerConnected() const
00141 {
00142 return ac_->isServerConnected();
00143 }
00144
00154 void sendGoal(const Goal & goal,
00155 SimpleDoneCallback done_cb = SimpleDoneCallback(),
00156 SimpleActiveCallback active_cb = SimpleActiveCallback(),
00157 SimpleFeedbackCallback feedback_cb = SimpleFeedbackCallback());
00158
00170 SimpleClientGoalState sendGoalAndWait(const Goal & goal,
00171 const ros::Duration & execute_timeout = ros::Duration(0, 0),
00172 const ros::Duration & preempt_timeout = ros::Duration(0, 0));
00173
00179 bool waitForResult(const ros::Duration & timeout = ros::Duration(0, 0));
00180
00185 ResultConstPtr getResult() const;
00186
00193 SimpleClientGoalState getState() const;
00194
00201 void cancelAllGoals();
00202
00207 void cancelGoalsAtAndBeforeTime(const ros::Time & time);
00208
00212 void cancelGoal();
00213
00220 void stopTrackingGoal();
00221
00222 private:
00223 typedef ActionClient<ActionSpec> ActionClientT;
00224 ros::NodeHandle nh_;
00225 GoalHandleT gh_;
00226
00227 SimpleGoalState cur_simple_state_;
00228
00229
00230 boost::condition done_condition_;
00231 boost::mutex done_mutex_;
00232
00233
00234 SimpleDoneCallback done_cb_;
00235 SimpleActiveCallback active_cb_;
00236 SimpleFeedbackCallback feedback_cb_;
00237
00238
00239 boost::mutex terminate_mutex_;
00240 bool need_to_terminate_;
00241 boost::thread * spin_thread_;
00242 ros::CallbackQueue callback_queue;
00243
00244 boost::scoped_ptr<ActionClientT> ac_;
00245
00246
00247 void initSimpleClient(ros::NodeHandle & n, const std::string & name, bool spin_thread);
00248 void handleTransition(GoalHandleT gh);
00249 void handleFeedback(GoalHandleT gh, const FeedbackConstPtr & feedback);
00250 void setSimpleState(const SimpleGoalState::StateEnum & next_state);
00251 void setSimpleState(const SimpleGoalState & next_state);
00252 void spinThread();
00253 };
00254
00255
00256 template<class ActionSpec>
00257 void SimpleActionClient<ActionSpec>::initSimpleClient(ros::NodeHandle & n, const std::string & name,
00258 bool spin_thread)
00259 {
00260 if (spin_thread) {
00261 ROS_DEBUG_NAMED("actionlib", "Spinning up a thread for the SimpleActionClient");
00262 need_to_terminate_ = false;
00263 spin_thread_ =
00264 new boost::thread(boost::bind(&SimpleActionClient<ActionSpec>::spinThread, this));
00265 ac_.reset(new ActionClientT(n, name, &callback_queue));
00266 } else {
00267 spin_thread_ = NULL;
00268 ac_.reset(new ActionClientT(n, name));
00269 }
00270 }
00271
00272 template<class ActionSpec>
00273 SimpleActionClient<ActionSpec>::~SimpleActionClient()
00274 {
00275 if (spin_thread_) {
00276 {
00277 boost::mutex::scoped_lock terminate_lock(terminate_mutex_);
00278 need_to_terminate_ = true;
00279 }
00280 spin_thread_->join();
00281 delete spin_thread_;
00282 }
00283 gh_.reset();
00284 ac_.reset();
00285 }
00286
00287 template<class ActionSpec>
00288 void SimpleActionClient<ActionSpec>::spinThread()
00289 {
00290 while (nh_.ok()) {
00291 {
00292 boost::mutex::scoped_lock terminate_lock(terminate_mutex_);
00293 if (need_to_terminate_) {
00294 break;
00295 }
00296 }
00297 callback_queue.callAvailable(ros::WallDuration(0.1f));
00298 }
00299 }
00300
00301 template<class ActionSpec>
00302 void SimpleActionClient<ActionSpec>::setSimpleState(const SimpleGoalState::StateEnum & next_state)
00303 {
00304 setSimpleState(SimpleGoalState(next_state) );
00305 }
00306
00307 template<class ActionSpec>
00308 void SimpleActionClient<ActionSpec>::setSimpleState(const SimpleGoalState & next_state)
00309 {
00310 ROS_DEBUG_NAMED("actionlib", "Transitioning SimpleState from [%s] to [%s]",
00311 cur_simple_state_.toString().c_str(),
00312 next_state.toString().c_str());
00313 cur_simple_state_ = next_state;
00314 }
00315
00316 template<class ActionSpec>
00317 void SimpleActionClient<ActionSpec>::sendGoal(const Goal & goal,
00318 SimpleDoneCallback done_cb,
00319 SimpleActiveCallback active_cb,
00320 SimpleFeedbackCallback feedback_cb)
00321 {
00322
00323 gh_.reset();
00324
00325
00326 done_cb_ = done_cb;
00327 active_cb_ = active_cb;
00328 feedback_cb_ = feedback_cb;
00329
00330 cur_simple_state_ = SimpleGoalState::PENDING;
00331
00332
00333 gh_ = ac_->sendGoal(goal, boost::bind(&SimpleActionClientT::handleTransition, this, _1),
00334 boost::bind(&SimpleActionClientT::handleFeedback, this, _1, _2));
00335 }
00336
00337 template<class ActionSpec>
00338 SimpleClientGoalState SimpleActionClient<ActionSpec>::getState() const
00339 {
00340 if (gh_.isExpired()) {
00341 ROS_ERROR_NAMED("actionlib",
00342 "Trying to getState() when no goal is running. You are incorrectly using SimpleActionClient");
00343 return SimpleClientGoalState(SimpleClientGoalState::LOST);
00344 }
00345
00346 CommState comm_state_ = gh_.getCommState();
00347
00348 switch (comm_state_.state_) {
00349 case CommState::WAITING_FOR_GOAL_ACK:
00350 case CommState::PENDING:
00351 case CommState::RECALLING:
00352 return SimpleClientGoalState(SimpleClientGoalState::PENDING);
00353 case CommState::ACTIVE:
00354 case CommState::PREEMPTING:
00355 return SimpleClientGoalState(SimpleClientGoalState::ACTIVE);
00356 case CommState::DONE:
00357 {
00358 switch (gh_.getTerminalState().state_) {
00359 case TerminalState::RECALLED:
00360 return SimpleClientGoalState(SimpleClientGoalState::RECALLED,
00361 gh_.getTerminalState().text_);
00362 case TerminalState::REJECTED:
00363 return SimpleClientGoalState(SimpleClientGoalState::REJECTED,
00364 gh_.getTerminalState().text_);
00365 case TerminalState::PREEMPTED:
00366 return SimpleClientGoalState(SimpleClientGoalState::PREEMPTED,
00367 gh_.getTerminalState().text_);
00368 case TerminalState::ABORTED:
00369 return SimpleClientGoalState(SimpleClientGoalState::ABORTED,
00370 gh_.getTerminalState().text_);
00371 case TerminalState::SUCCEEDED:
00372 return SimpleClientGoalState(SimpleClientGoalState::SUCCEEDED,
00373 gh_.getTerminalState().text_);
00374 case TerminalState::LOST:
00375 return SimpleClientGoalState(SimpleClientGoalState::LOST, gh_.getTerminalState().text_);
00376 default:
00377 ROS_ERROR_NAMED("actionlib",
00378 "Unknown terminal state [%u]. This is a bug in SimpleActionClient",
00379 gh_.getTerminalState().state_);
00380 return SimpleClientGoalState(SimpleClientGoalState::LOST, gh_.getTerminalState().text_);
00381 }
00382 }
00383 case CommState::WAITING_FOR_RESULT:
00384 case CommState::WAITING_FOR_CANCEL_ACK:
00385 {
00386 switch (cur_simple_state_.state_) {
00387 case SimpleGoalState::PENDING:
00388 return SimpleClientGoalState(SimpleClientGoalState::PENDING);
00389 case SimpleGoalState::ACTIVE:
00390 return SimpleClientGoalState(SimpleClientGoalState::ACTIVE);
00391 case SimpleGoalState::DONE:
00392 ROS_ERROR_NAMED("actionlib",
00393 "In WAITING_FOR_RESULT or WAITING_FOR_CANCEL_ACK, yet we are in SimpleGoalState DONE. This is a bug in SimpleActionClient");
00394 return SimpleClientGoalState(SimpleClientGoalState::LOST);
00395 default:
00396 ROS_ERROR_NAMED("actionlib",
00397 "Got a SimpleGoalState of [%u]. This is a bug in SimpleActionClient",
00398 cur_simple_state_.state_);
00399 }
00400 }
00401 default:
00402 break;
00403 }
00404 ROS_ERROR_NAMED("actionlib", "Error trying to interpret CommState - %u", comm_state_.state_);
00405 return SimpleClientGoalState(SimpleClientGoalState::LOST);
00406 }
00407
00408 template<class ActionSpec>
00409 typename SimpleActionClient<ActionSpec>::ResultConstPtr SimpleActionClient<ActionSpec>::getResult()
00410 const
00411 {
00412 if (gh_.isExpired()) {
00413 ROS_ERROR_NAMED("actionlib",
00414 "Trying to getResult() when no goal is running. You are incorrectly using SimpleActionClient");
00415 }
00416
00417 if (gh_.getResult()) {
00418 return gh_.getResult();
00419 }
00420
00421 return ResultConstPtr(new Result);
00422 }
00423
00424
00425 template<class ActionSpec>
00426 void SimpleActionClient<ActionSpec>::cancelAllGoals()
00427 {
00428 ac_->cancelAllGoals();
00429 }
00430
00431 template<class ActionSpec>
00432 void SimpleActionClient<ActionSpec>::cancelGoalsAtAndBeforeTime(const ros::Time & time)
00433 {
00434 ac_->cancelGoalsAtAndBeforeTime(time);
00435 }
00436
00437 template<class ActionSpec>
00438 void SimpleActionClient<ActionSpec>::cancelGoal()
00439 {
00440 if (gh_.isExpired()) {
00441 ROS_ERROR_NAMED("actionlib",
00442 "Trying to cancelGoal() when no goal is running. You are incorrectly using SimpleActionClient");
00443 }
00444
00445 gh_.cancel();
00446 }
00447
00448 template<class ActionSpec>
00449 void SimpleActionClient<ActionSpec>::stopTrackingGoal()
00450 {
00451 if (gh_.isExpired()) {
00452 ROS_ERROR_NAMED("actionlib",
00453 "Trying to stopTrackingGoal() when no goal is running. You are incorrectly using SimpleActionClient");
00454 }
00455 gh_.reset();
00456 }
00457
00458 template<class ActionSpec>
00459 void SimpleActionClient<ActionSpec>::handleFeedback(GoalHandleT gh,
00460 const FeedbackConstPtr & feedback)
00461 {
00462 if (gh_ != gh) {
00463 ROS_ERROR_NAMED("actionlib",
00464 "Got a callback on a goalHandle that we're not tracking. \
00465 This is an internal SimpleActionClient/ActionClient bug. \
00466 This could also be a GoalID collision");
00467 }
00468 if (feedback_cb_) {
00469 feedback_cb_(feedback);
00470 }
00471 }
00472
00473 template<class ActionSpec>
00474 void SimpleActionClient<ActionSpec>::handleTransition(GoalHandleT gh)
00475 {
00476 CommState comm_state_ = gh.getCommState();
00477 switch (comm_state_.state_) {
00478 case CommState::WAITING_FOR_GOAL_ACK:
00479 ROS_ERROR_NAMED("actionlib",
00480 "BUG: Shouldn't ever get a transition callback for WAITING_FOR_GOAL_ACK");
00481 break;
00482 case CommState::PENDING:
00483 ROS_ERROR_COND(cur_simple_state_ != SimpleGoalState::PENDING,
00484 "BUG: Got a transition to CommState [%s] when our in SimpleGoalState [%s]",
00485 comm_state_.toString().c_str(), cur_simple_state_.toString().c_str());
00486 break;
00487 case CommState::ACTIVE:
00488 switch (cur_simple_state_.state_) {
00489 case SimpleGoalState::PENDING:
00490 setSimpleState(SimpleGoalState::ACTIVE);
00491 if (active_cb_) {
00492 active_cb_();
00493 }
00494 break;
00495 case SimpleGoalState::ACTIVE:
00496 break;
00497 case SimpleGoalState::DONE:
00498 ROS_ERROR_NAMED("actionlib",
00499 "BUG: Got a transition to CommState [%s] when in SimpleGoalState [%s]",
00500 comm_state_.toString().c_str(), cur_simple_state_.toString().c_str());
00501 break;
00502 default:
00503 ROS_FATAL("Unknown SimpleGoalState %u", cur_simple_state_.state_);
00504 break;
00505 }
00506 break;
00507 case CommState::WAITING_FOR_RESULT:
00508 break;
00509 case CommState::WAITING_FOR_CANCEL_ACK:
00510 break;
00511 case CommState::RECALLING:
00512 ROS_ERROR_COND(cur_simple_state_ != SimpleGoalState::PENDING,
00513 "BUG: Got a transition to CommState [%s] when our in SimpleGoalState [%s]",
00514 comm_state_.toString().c_str(), cur_simple_state_.toString().c_str());
00515 break;
00516 case CommState::PREEMPTING:
00517 switch (cur_simple_state_.state_) {
00518 case SimpleGoalState::PENDING:
00519 setSimpleState(SimpleGoalState::ACTIVE);
00520 if (active_cb_) {
00521 active_cb_();
00522 }
00523 break;
00524 case SimpleGoalState::ACTIVE:
00525 break;
00526 case SimpleGoalState::DONE:
00527 ROS_ERROR_NAMED("actionlib",
00528 "BUG: Got a transition to CommState [%s] when in SimpleGoalState [%s]",
00529 comm_state_.toString().c_str(), cur_simple_state_.toString().c_str());
00530 break;
00531 default:
00532 ROS_FATAL("Unknown SimpleGoalState %u", cur_simple_state_.state_);
00533 break;
00534 }
00535 break;
00536 case CommState::DONE:
00537 switch (cur_simple_state_.state_) {
00538 case SimpleGoalState::PENDING:
00539 case SimpleGoalState::ACTIVE:
00540 {
00541 boost::mutex::scoped_lock lock(done_mutex_);
00542 setSimpleState(SimpleGoalState::DONE);
00543 }
00544
00545 if (done_cb_) {
00546 done_cb_(getState(), gh.getResult());
00547 }
00548
00549 done_condition_.notify_all();
00550 break;
00551 case SimpleGoalState::DONE:
00552 ROS_ERROR_NAMED("actionlib", "BUG: Got a second transition to DONE");
00553 break;
00554 default:
00555 ROS_FATAL("Unknown SimpleGoalState %u", cur_simple_state_.state_);
00556 break;
00557 }
00558 break;
00559 default:
00560 ROS_ERROR_NAMED("actionlib", "Unknown CommState received [%u]", comm_state_.state_);
00561 break;
00562 }
00563 }
00564
00565 template<class ActionSpec>
00566 bool SimpleActionClient<ActionSpec>::waitForResult(const ros::Duration & timeout)
00567 {
00568 if (gh_.isExpired()) {
00569 ROS_ERROR_NAMED("actionlib",
00570 "Trying to waitForGoalToFinish() when no goal is running. You are incorrectly using SimpleActionClient");
00571 return false;
00572 }
00573
00574 if (timeout < ros::Duration(0, 0)) {
00575 ROS_WARN_NAMED("actionlib", "Timeouts can't be negative. Timeout is [%.2fs]", timeout.toSec());
00576 }
00577
00578 ros::Time timeout_time = ros::Time::now() + timeout;
00579
00580 boost::mutex::scoped_lock lock(done_mutex_);
00581
00582
00583 ros::Duration loop_period = ros::Duration().fromSec(.1);
00584
00585 while (nh_.ok()) {
00586
00587 ros::Duration time_left = timeout_time - ros::Time::now();
00588
00589
00590 if (timeout > ros::Duration(0, 0) && time_left <= ros::Duration(0, 0) ) {
00591 break;
00592 }
00593
00594 if (cur_simple_state_ == SimpleGoalState::DONE) {
00595 break;
00596 }
00597
00598
00599
00600 if (time_left > loop_period || timeout == ros::Duration()) {
00601 time_left = loop_period;
00602 }
00603
00604 done_condition_.timed_wait(lock, boost::posix_time::milliseconds(time_left.toSec() * 1000.0f));
00605 }
00606
00607 return cur_simple_state_ == SimpleGoalState::DONE;
00608 }
00609
00610 template<class ActionSpec>
00611 SimpleClientGoalState SimpleActionClient<ActionSpec>::sendGoalAndWait(const Goal & goal,
00612 const ros::Duration & execute_timeout,
00613 const ros::Duration & preempt_timeout)
00614 {
00615 sendGoal(goal);
00616
00617
00618 if (waitForResult(execute_timeout)) {
00619 ROS_DEBUG_NAMED("actionlib", "Goal finished within specified execute_timeout [%.2f]",
00620 execute_timeout.toSec());
00621 return getState();
00622 }
00623
00624 ROS_DEBUG_NAMED("actionlib", "Goal didn't finish within specified execute_timeout [%.2f]",
00625 execute_timeout.toSec());
00626
00627
00628 cancelGoal();
00629
00630
00631 if (waitForResult(preempt_timeout)) {
00632 ROS_DEBUG_NAMED("actionlib", "Preempt finished within specified preempt_timeout [%.2f]",
00633 preempt_timeout.toSec());
00634 } else {
00635 ROS_DEBUG_NAMED("actionlib", "Preempt didn't finish specified preempt_timeout [%.2f]",
00636 preempt_timeout.toSec());
00637 }
00638 return getState();
00639 }
00640
00641 }
00642
00643 #undef DEPRECATED
00644
00645 #endif // ACTIONLIB__CLIENT__SIMPLE_ACTION_CLIENT_H_