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