simple_action_client.h
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
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 #if defined(__GNUC__)
00051 #define DEPRECATED __attribute__((deprecated))
00052 #else
00053 #define DEPRECATED
00054 #endif
00055 
00056 
00057 namespace actionlib
00058 {
00059 
00067 template <class ActionSpec>
00068 class SimpleActionClient
00069 {
00070 private:
00071   ACTION_DEFINITION(ActionSpec);
00072   typedef ClientGoalHandle<ActionSpec> GoalHandleT;
00073   typedef SimpleActionClient<ActionSpec> SimpleActionClientT;
00074 
00075 public:
00076   typedef boost::function<void (const SimpleClientGoalState& state,  const ResultConstPtr& result) > SimpleDoneCallback;
00077   typedef boost::function<void () > SimpleActiveCallback;
00078   typedef boost::function<void (const FeedbackConstPtr& feedback) > SimpleFeedbackCallback;
00079 
00088   SimpleActionClient(const std::string& name, bool spin_thread = true) : cur_simple_state_(SimpleGoalState::PENDING)
00089   {
00090     initSimpleClient(nh_, name, spin_thread);
00091   }
00092 
00103   SimpleActionClient(ros::NodeHandle& n, const std::string& name, bool spin_thread = true) : cur_simple_state_(SimpleGoalState::PENDING)
00104   {
00105     initSimpleClient(n, name, spin_thread);
00106 
00107   }
00108 
00109   ~SimpleActionClient();
00110 
00125   bool waitForServer(const ros::Duration& timeout = ros::Duration(0,0) ) { return ac_->waitForActionServerToStart(timeout); }
00126 
00131   bool isServerConnected()
00132   {
00133     return ac_->isServerConnected();
00134   }
00135 
00145   void sendGoal(const Goal& goal,
00146                 SimpleDoneCallback     done_cb     = SimpleDoneCallback(),
00147                 SimpleActiveCallback   active_cb   = SimpleActiveCallback(),
00148                 SimpleFeedbackCallback feedback_cb = SimpleFeedbackCallback());
00149 
00161   SimpleClientGoalState sendGoalAndWait(const Goal& goal,
00162                                         const ros::Duration& execute_timeout = ros::Duration(0,0),
00163                                         const ros::Duration& preempt_timeout = ros::Duration(0,0));
00164 
00170   bool waitForResult(const ros::Duration& timeout = ros::Duration(0,0) );
00171 
00176   ResultConstPtr getResult();
00177 
00184   SimpleClientGoalState getState();
00185 
00192   void cancelAllGoals();
00193 
00198   void cancelGoalsAtAndBeforeTime(const ros::Time& time);
00199 
00203   void cancelGoal();
00204 
00211   void stopTrackingGoal();
00212 
00213 private:
00214   typedef ActionClient<ActionSpec> ActionClientT;
00215   ros::NodeHandle nh_;
00216   GoalHandleT gh_;
00217 
00218   SimpleGoalState cur_simple_state_;
00219 
00220   // Signalling Stuff
00221   boost::condition done_condition_;
00222   boost::mutex done_mutex_;
00223 
00224   // User Callbacks
00225   SimpleDoneCallback done_cb_;
00226   SimpleActiveCallback active_cb_;
00227   SimpleFeedbackCallback feedback_cb_;
00228 
00229   // Spin Thread Stuff
00230   boost::mutex terminate_mutex_;
00231   bool need_to_terminate_;
00232   boost::thread* spin_thread_;
00233   ros::CallbackQueue callback_queue;
00234 
00235   boost::scoped_ptr<ActionClientT> ac_;  // Action client depends on callback_queue, so it must be destroyed before callback_queue
00236 
00237   // ***** Private Funcs *****
00238   void initSimpleClient(ros::NodeHandle& n, const std::string& name, bool spin_thread);
00239   void handleTransition(GoalHandleT gh);
00240   void handleFeedback(GoalHandleT gh, const FeedbackConstPtr& feedback);
00241   void setSimpleState(const SimpleGoalState::StateEnum& next_state);
00242   void setSimpleState(const SimpleGoalState& next_state);
00243   void spinThread();
00244 };
00245 
00246 
00247 
00248 template<class ActionSpec>
00249 void SimpleActionClient<ActionSpec>::initSimpleClient(ros::NodeHandle& n, const std::string& name, bool spin_thread)
00250 {
00251   if (spin_thread)
00252   {
00253     ROS_DEBUG_NAMED("actionlib", "Spinning up a thread for the SimpleActionClient");
00254     need_to_terminate_ = false;
00255     spin_thread_ = new boost::thread(boost::bind(&SimpleActionClient<ActionSpec>::spinThread, this));
00256     ac_.reset(new ActionClientT(n, name, &callback_queue));
00257   }
00258   else
00259   {
00260     spin_thread_ = NULL;
00261     ac_.reset(new ActionClientT(n, name));
00262   }
00263 }
00264 
00265 template<class ActionSpec>
00266 SimpleActionClient<ActionSpec>::~SimpleActionClient()
00267 {
00268   if (spin_thread_)
00269   {
00270     {
00271       boost::mutex::scoped_lock terminate_lock(terminate_mutex_);
00272       need_to_terminate_ = true;
00273     }
00274     spin_thread_->join();
00275     delete spin_thread_;
00276   }
00277   gh_.reset();
00278   ac_.reset();
00279 }
00280 
00281 template<class ActionSpec>
00282 void SimpleActionClient<ActionSpec>::spinThread()
00283 {
00284   while (nh_.ok())
00285   {
00286     {
00287       boost::mutex::scoped_lock terminate_lock(terminate_mutex_);
00288       if (need_to_terminate_)
00289         break;
00290     }
00291     callback_queue.callAvailable(ros::WallDuration(0.1f));
00292   }
00293 }
00294 
00295 template<class ActionSpec>
00296 void SimpleActionClient<ActionSpec>::setSimpleState(const SimpleGoalState::StateEnum& next_state)
00297 {
00298   setSimpleState( SimpleGoalState(next_state) );
00299 }
00300 
00301 template<class ActionSpec>
00302 void SimpleActionClient<ActionSpec>::setSimpleState(const SimpleGoalState& next_state)
00303 {
00304   ROS_DEBUG_NAMED("actionlib", "Transitioning SimpleState from [%s] to [%s]",
00305             cur_simple_state_.toString().c_str(),
00306             next_state.toString().c_str());
00307   cur_simple_state_ = next_state;
00308 }
00309 
00310 template<class ActionSpec>
00311 void SimpleActionClient<ActionSpec>::sendGoal(const Goal& goal,
00312                                               SimpleDoneCallback     done_cb,
00313                                               SimpleActiveCallback   active_cb,
00314                                               SimpleFeedbackCallback feedback_cb)
00315 {
00316   // Reset the old GoalHandle, so that our callbacks won't get called anymore
00317   gh_.reset();
00318 
00319   // Store all the callbacks
00320   done_cb_     = done_cb;
00321   active_cb_   = active_cb;
00322   feedback_cb_ = feedback_cb;
00323 
00324   cur_simple_state_ = SimpleGoalState::PENDING;
00325 
00326   // Send the goal to the ActionServer
00327   gh_ = ac_->sendGoal(goal, boost::bind(&SimpleActionClientT::handleTransition, this, _1),
00328                             boost::bind(&SimpleActionClientT::handleFeedback, this, _1, _2));
00329 }
00330 
00331 template<class ActionSpec>
00332 SimpleClientGoalState SimpleActionClient<ActionSpec>::getState()
00333 {
00334   if (gh_.isExpired())
00335   {
00336     ROS_ERROR_NAMED("actionlib", "Trying to getState() when no goal is running. You are incorrectly using SimpleActionClient");
00337     return SimpleClientGoalState(SimpleClientGoalState::LOST);
00338   }
00339 
00340   CommState comm_state_ = gh_.getCommState();
00341 
00342   switch( comm_state_.state_)
00343   {
00344     case CommState::WAITING_FOR_GOAL_ACK:
00345     case CommState::PENDING:
00346     case CommState::RECALLING:
00347       return SimpleClientGoalState(SimpleClientGoalState::PENDING);
00348     case CommState::ACTIVE:
00349     case CommState::PREEMPTING:
00350       return SimpleClientGoalState(SimpleClientGoalState::ACTIVE);
00351     case CommState::DONE:
00352     {
00353       switch(gh_.getTerminalState().state_)
00354       {
00355         case TerminalState::RECALLED:
00356           return SimpleClientGoalState(SimpleClientGoalState::RECALLED, gh_.getTerminalState().text_);
00357         case TerminalState::REJECTED:
00358           return SimpleClientGoalState(SimpleClientGoalState::REJECTED, gh_.getTerminalState().text_);
00359         case TerminalState::PREEMPTED:
00360           return SimpleClientGoalState(SimpleClientGoalState::PREEMPTED, gh_.getTerminalState().text_);
00361         case TerminalState::ABORTED:
00362           return SimpleClientGoalState(SimpleClientGoalState::ABORTED, gh_.getTerminalState().text_);
00363         case TerminalState::SUCCEEDED:
00364           return SimpleClientGoalState(SimpleClientGoalState::SUCCEEDED, gh_.getTerminalState().text_);
00365         case TerminalState::LOST:
00366           return SimpleClientGoalState(SimpleClientGoalState::LOST, gh_.getTerminalState().text_);
00367         default:
00368           ROS_ERROR_NAMED("actionlib", "Unknown terminal state [%u]. This is a bug in SimpleActionClient", gh_.getTerminalState().state_);
00369           return SimpleClientGoalState(SimpleClientGoalState::LOST, gh_.getTerminalState().text_);
00370       }
00371     }
00372     case CommState::WAITING_FOR_RESULT:
00373     case CommState::WAITING_FOR_CANCEL_ACK:
00374     {
00375       switch (cur_simple_state_.state_)
00376       {
00377         case SimpleGoalState::PENDING:
00378           return SimpleClientGoalState(SimpleClientGoalState::PENDING);
00379         case SimpleGoalState::ACTIVE:
00380           return SimpleClientGoalState(SimpleClientGoalState::ACTIVE);
00381         case SimpleGoalState::DONE:
00382           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");
00383           return SimpleClientGoalState(SimpleClientGoalState::LOST);
00384         default:
00385           ROS_ERROR_NAMED("actionlib", "Got a SimpleGoalState of [%u]. This is a bug in SimpleActionClient", cur_simple_state_.state_);
00386       }
00387     }
00388     default:
00389       break;
00390   }
00391   ROS_ERROR_NAMED("actionlib", "Error trying to interpret CommState - %u", comm_state_.state_);
00392   return SimpleClientGoalState(SimpleClientGoalState::LOST);
00393 }
00394 
00395 template<class ActionSpec>
00396 typename SimpleActionClient<ActionSpec>::ResultConstPtr SimpleActionClient<ActionSpec>::getResult()
00397 {
00398   if (gh_.isExpired())
00399     ROS_ERROR_NAMED("actionlib", "Trying to getResult() when no goal is running. You are incorrectly using SimpleActionClient");
00400 
00401   if (gh_.getResult())
00402     return gh_.getResult();
00403 
00404   return ResultConstPtr(new Result);
00405 }
00406 
00407 
00408 template<class ActionSpec>
00409 void SimpleActionClient<ActionSpec>::cancelAllGoals()
00410 {
00411   ac_->cancelAllGoals();
00412 }
00413 
00414 template<class ActionSpec>
00415 void SimpleActionClient<ActionSpec>::cancelGoalsAtAndBeforeTime(const ros::Time& time)
00416 {
00417   ac_->cancelGoalsAtAndBeforeTime(time);
00418 }
00419 
00420 template<class ActionSpec>
00421 void SimpleActionClient<ActionSpec>::cancelGoal()
00422 {
00423   if (gh_.isExpired())
00424     ROS_ERROR_NAMED("actionlib", "Trying to cancelGoal() when no goal is running. You are incorrectly using SimpleActionClient");
00425 
00426   gh_.cancel();
00427 }
00428 
00429 template<class ActionSpec>
00430 void SimpleActionClient<ActionSpec>::stopTrackingGoal()
00431 {
00432   if (gh_.isExpired())
00433     ROS_ERROR_NAMED("actionlib", "Trying to stopTrackingGoal() when no goal is running. You are incorrectly using SimpleActionClient");
00434   gh_.reset();
00435 }
00436 
00437 template<class ActionSpec>
00438 void SimpleActionClient<ActionSpec>::handleFeedback(GoalHandleT gh, const FeedbackConstPtr& feedback)
00439 {
00440   if (gh_ != gh)
00441     ROS_ERROR_NAMED("actionlib", "Got a callback on a goalHandle that we're not tracking.  \
00442                This is an internal SimpleActionClient/ActionClient bug.  \
00443                This could also be a GoalID collision");
00444   if (feedback_cb_)
00445     feedback_cb_(feedback);
00446 }
00447 
00448 template<class ActionSpec>
00449 void SimpleActionClient<ActionSpec>::handleTransition(GoalHandleT gh)
00450 {
00451   CommState comm_state_ = gh.getCommState();
00452   switch (comm_state_.state_)
00453   {
00454     case CommState::WAITING_FOR_GOAL_ACK:
00455       ROS_ERROR_NAMED("actionlib", "BUG: Shouldn't ever get a transition callback for WAITING_FOR_GOAL_ACK");
00456       break;
00457     case CommState::PENDING:
00458       ROS_ERROR_COND( cur_simple_state_ != SimpleGoalState::PENDING,
00459                       "BUG: Got a transition to CommState [%s] when our in SimpleGoalState [%s]",
00460                       comm_state_.toString().c_str(), cur_simple_state_.toString().c_str());
00461       break;
00462     case CommState::ACTIVE:
00463       switch (cur_simple_state_.state_)
00464       {
00465         case SimpleGoalState::PENDING:
00466           setSimpleState(SimpleGoalState::ACTIVE);
00467           if (active_cb_)
00468             active_cb_();
00469           break;
00470         case SimpleGoalState::ACTIVE:
00471           break;
00472         case SimpleGoalState::DONE:
00473           ROS_ERROR_NAMED("actionlib", "BUG: Got a transition to CommState [%s] when in SimpleGoalState [%s]",
00474                     comm_state_.toString().c_str(), cur_simple_state_.toString().c_str());
00475           break;
00476         default:
00477           ROS_FATAL("Unknown SimpleGoalState %u", cur_simple_state_.state_);
00478           break;
00479       }
00480       break;
00481     case CommState::WAITING_FOR_RESULT:
00482       break;
00483     case CommState::WAITING_FOR_CANCEL_ACK:
00484       break;
00485     case CommState::RECALLING:
00486       ROS_ERROR_COND( cur_simple_state_ != SimpleGoalState::PENDING,
00487                       "BUG: Got a transition to CommState [%s] when our in SimpleGoalState [%s]",
00488                       comm_state_.toString().c_str(), cur_simple_state_.toString().c_str());
00489       break;
00490     case CommState::PREEMPTING:
00491       switch (cur_simple_state_.state_)
00492       {
00493         case SimpleGoalState::PENDING:
00494           setSimpleState(SimpleGoalState::ACTIVE);
00495           if (active_cb_)
00496             active_cb_();
00497           break;
00498         case SimpleGoalState::ACTIVE:
00499           break;
00500         case SimpleGoalState::DONE:
00501           ROS_ERROR_NAMED("actionlib", "BUG: Got a transition to CommState [%s] when in SimpleGoalState [%s]",
00502                      comm_state_.toString().c_str(), cur_simple_state_.toString().c_str());
00503           break;
00504         default:
00505           ROS_FATAL("Unknown SimpleGoalState %u", cur_simple_state_.state_);
00506           break;
00507       }
00508       break;
00509     case CommState::DONE:
00510       switch (cur_simple_state_.state_)
00511       {
00512         case SimpleGoalState::PENDING:
00513         case SimpleGoalState::ACTIVE:
00514           done_mutex_.lock();
00515           setSimpleState(SimpleGoalState::DONE);
00516           done_mutex_.unlock();
00517 
00518           if (done_cb_)
00519             done_cb_(getState(), gh.getResult());
00520 
00521           done_condition_.notify_all();
00522           break;
00523         case SimpleGoalState::DONE:
00524           ROS_ERROR_NAMED("actionlib", "BUG: Got a second transition to DONE");
00525           break;
00526         default:
00527           ROS_FATAL("Unknown SimpleGoalState %u", cur_simple_state_.state_);
00528           break;
00529       }
00530       break;
00531     default:
00532       ROS_ERROR_NAMED("actionlib", "Unknown CommState received [%u]", comm_state_.state_);
00533       break;
00534   }
00535 }
00536 
00537 template<class ActionSpec>
00538 bool SimpleActionClient<ActionSpec>::waitForResult(const ros::Duration& timeout )
00539 {
00540   if (gh_.isExpired())
00541   {
00542     ROS_ERROR_NAMED("actionlib", "Trying to waitForGoalToFinish() when no goal is running. You are incorrectly using SimpleActionClient");
00543     return false;
00544   }
00545 
00546   if (timeout < ros::Duration(0,0))
00547     ROS_WARN_NAMED("actionlib", "Timeouts can't be negative. Timeout is [%.2fs]", timeout.toSec());
00548 
00549   ros::Time timeout_time = ros::Time::now() + timeout;
00550 
00551   boost::mutex::scoped_lock lock(done_mutex_);
00552 
00553   // Hardcode how often we check for node.ok()
00554   ros::Duration loop_period = ros::Duration().fromSec(.1);
00555 
00556   while (nh_.ok())
00557   {
00558     // Determine how long we should wait
00559     ros::Duration time_left = timeout_time - ros::Time::now();
00560 
00561     // Check if we're past the timeout time
00562     if (timeout > ros::Duration(0,0) && time_left <= ros::Duration(0,0) )
00563     {
00564       break;
00565     }
00566     
00567     if (cur_simple_state_ == SimpleGoalState::DONE)
00568     {
00569       break;
00570     }
00571 
00572 
00573     // Truncate the time left
00574     if (time_left > loop_period || timeout == ros::Duration())
00575       time_left = loop_period;
00576 
00577     done_condition_.timed_wait(lock, boost::posix_time::milliseconds(time_left.toSec() * 1000.0f));
00578   }
00579   
00580   return (cur_simple_state_ == SimpleGoalState::DONE);
00581 }
00582 
00583 template<class ActionSpec>
00584 SimpleClientGoalState SimpleActionClient<ActionSpec>::sendGoalAndWait(const Goal& goal,
00585                                                                       const ros::Duration& execute_timeout,
00586                                                                       const ros::Duration& preempt_timeout)
00587 {
00588   sendGoal(goal);
00589 
00590   // See if the goal finishes in time
00591   if (waitForResult(execute_timeout))
00592   {
00593     ROS_DEBUG_NAMED("actionlib", "Goal finished within specified execute_timeout [%.2f]", execute_timeout.toSec());
00594     return getState();
00595   }
00596 
00597   ROS_DEBUG_NAMED("actionlib", "Goal didn't finish within specified execute_timeout [%.2f]", execute_timeout.toSec());
00598 
00599   // It didn't finish in time, so we need to preempt it
00600   cancelGoal();
00601 
00602   // Now wait again and see if it finishes
00603   if (waitForResult(preempt_timeout))
00604     ROS_DEBUG_NAMED("actionlib", "Preempt finished within specified preempt_timeout [%.2f]", preempt_timeout.toSec());
00605   else
00606     ROS_DEBUG_NAMED("actionlib", "Preempt didn't finish specified preempt_timeout [%.2f]", preempt_timeout.toSec());
00607   return getState();
00608 }
00609 
00610 }
00611 
00612 #undef DEPRECATED
00613 
00614 #endif // ACTIONLIB_SINGLE_GOAL_ACTION_CLIENT_H_


actionlib
Author(s): Eitan Marder-Eppstein, Vijay Pradeep
autogenerated on Thu Jan 2 2014 11:03:49