actionlib: actionlib: simple_action

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 
00120   bool waitForServer(const ros::Duration& timeout = ros::Duration(0,0) ) { return ac_->waitForActionServerToStart(timeout); }
00121 
00126   bool isServerConnected()
00127   {
00128     return ac_->isServerConnected();
00129   }
00130 
00140   void sendGoal(const Goal& goal,
00141                 SimpleDoneCallback     done_cb     = SimpleDoneCallback(),
00142                 SimpleActiveCallback   active_cb   = SimpleActiveCallback(),
00143                 SimpleFeedbackCallback feedback_cb = SimpleFeedbackCallback());
00144 
00156   SimpleClientGoalState sendGoalAndWait(const Goal& goal,
00157                                         const ros::Duration& execute_timeout = ros::Duration(0,0),
00158                                         const ros::Duration& preempt_timeout = ros::Duration(0,0));
00159 
00165   bool waitForResult(const ros::Duration& timeout = ros::Duration(0,0) );
00166 
00171   ResultConstPtr getResult();
00172 
00179   SimpleClientGoalState getState();
00180 
00187   void cancelAllGoals();
00188 
00193   void cancelGoalsAtAndBeforeTime(const ros::Time& time);
00194 
00198   void cancelGoal();
00199 
00206   void stopTrackingGoal();
00207 
00208 private:
00209   typedef ActionClient<ActionSpec> ActionClientT;
00210   ros::NodeHandle nh_;
00211   GoalHandleT gh_;
00212 
00213   SimpleGoalState cur_simple_state_;
00214 
00215   // Signalling Stuff
00216   boost::condition done_condition_;
00217   boost::mutex done_mutex_;
00218 
00219   // User Callbacks
00220   SimpleDoneCallback done_cb_;
00221   SimpleActiveCallback active_cb_;
00222   SimpleFeedbackCallback feedback_cb_;
00223 
00224   // Spin Thread Stuff
00225   boost::mutex terminate_mutex_;
00226   bool need_to_terminate_;
00227   boost::thread* spin_thread_;
00228   ros::CallbackQueue callback_queue;
00229 
00230   boost::scoped_ptr<ActionClientT> ac_;  // Action client depends on callback_queue, so it must be destroyed before callback_queue
00231 
00232   // ***** Private Funcs *****
00233   void initSimpleClient(ros::NodeHandle& n, const std::string& name, bool spin_thread);
00234   void handleTransition(GoalHandleT gh);
00235   void handleFeedback(GoalHandleT gh, const FeedbackConstPtr& feedback);
00236   void setSimpleState(const SimpleGoalState::StateEnum& next_state);
00237   void setSimpleState(const SimpleGoalState& next_state);
00238   void spinThread();
00239 };
00240 
00241 
00242 
00243 template<class ActionSpec>
00244 void SimpleActionClient<ActionSpec>::initSimpleClient(ros::NodeHandle& n, const std::string& name, bool spin_thread)
00245 {
00246   if (spin_thread)
00247   {
00248     ROS_DEBUG_NAMED("actionlib", "Spinning up a thread for the SimpleActionClient");
00249     need_to_terminate_ = false;
00250     spin_thread_ = new boost::thread(boost::bind(&SimpleActionClient<ActionSpec>::spinThread, this));
00251     ac_.reset(new ActionClientT(n, name, &callback_queue));
00252   }
00253   else
00254   {
00255     spin_thread_ = NULL;
00256     ac_.reset(new ActionClientT(n, name));
00257   }
00258 }
00259 
00260 template<class ActionSpec>
00261 SimpleActionClient<ActionSpec>::~SimpleActionClient()
00262 {
00263   if (spin_thread_)
00264   {
00265     {
00266       boost::mutex::scoped_lock terminate_lock(terminate_mutex_);
00267       need_to_terminate_ = true;
00268     }
00269     spin_thread_->join();
00270     delete spin_thread_;
00271   }
00272   gh_.reset();
00273   ac_.reset();
00274 }
00275 
00276 template<class ActionSpec>
00277 void SimpleActionClient<ActionSpec>::spinThread()
00278 {
00279   while (nh_.ok())
00280   {
00281     {
00282       boost::mutex::scoped_lock terminate_lock(terminate_mutex_);
00283       if (need_to_terminate_)
00284         break;
00285     }
00286     callback_queue.callAvailable(ros::WallDuration(0.1f));
00287   }
00288 }
00289 
00290 template<class ActionSpec>
00291 void SimpleActionClient<ActionSpec>::setSimpleState(const SimpleGoalState::StateEnum& next_state)
00292 {
00293   setSimpleState( SimpleGoalState(next_state) );
00294 }
00295 
00296 template<class ActionSpec>
00297 void SimpleActionClient<ActionSpec>::setSimpleState(const SimpleGoalState& next_state)
00298 {
00299   ROS_DEBUG_NAMED("actionlib", "Transitioning SimpleState from [%s] to [%s]",
00300             cur_simple_state_.toString().c_str(),
00301             next_state.toString().c_str());
00302   cur_simple_state_ = next_state;
00303 }
00304 
00305 template<class ActionSpec>
00306 void SimpleActionClient<ActionSpec>::sendGoal(const Goal& goal,
00307                                               SimpleDoneCallback     done_cb,
00308                                               SimpleActiveCallback   active_cb,
00309                                               SimpleFeedbackCallback feedback_cb)
00310 {
00311   // Reset the old GoalHandle, so that our callbacks won't get called anymore
00312   gh_.reset();
00313 
00314   // Store all the callbacks
00315   done_cb_     = done_cb;
00316   active_cb_   = active_cb;
00317   feedback_cb_ = feedback_cb;
00318 
00319   cur_simple_state_ = SimpleGoalState::PENDING;
00320 
00321   // Send the goal to the ActionServer
00322   gh_ = ac_->sendGoal(goal, boost::bind(&SimpleActionClientT::handleTransition, this, _1),
00323                             boost::bind(&SimpleActionClientT::handleFeedback, this, _1, _2));
00324 }
00325 
00326 template<class ActionSpec>
00327 SimpleClientGoalState SimpleActionClient<ActionSpec>::getState()
00328 {
00329   if (gh_.isExpired())
00330   {
00331     ROS_ERROR_NAMED("actionlib", "Trying to getState() when no goal is running. You are incorrectly using SimpleActionClient");
00332     return SimpleClientGoalState(SimpleClientGoalState::LOST);
00333   }
00334 
00335   CommState comm_state_ = gh_.getCommState();
00336 
00337   switch( comm_state_.state_)
00338   {
00339     case CommState::WAITING_FOR_GOAL_ACK:
00340     case CommState::PENDING:
00341     case CommState::RECALLING:
00342       return SimpleClientGoalState(SimpleClientGoalState::PENDING);
00343     case CommState::ACTIVE:
00344     case CommState::PREEMPTING:
00345       return SimpleClientGoalState(SimpleClientGoalState::ACTIVE);
00346     case CommState::DONE:
00347     {
00348       switch(gh_.getTerminalState().state_)
00349       {
00350         case TerminalState::RECALLED:
00351           return SimpleClientGoalState(SimpleClientGoalState::RECALLED, gh_.getTerminalState().text_);
00352         case TerminalState::REJECTED:
00353           return SimpleClientGoalState(SimpleClientGoalState::REJECTED, gh_.getTerminalState().text_);
00354         case TerminalState::PREEMPTED:
00355           return SimpleClientGoalState(SimpleClientGoalState::PREEMPTED, gh_.getTerminalState().text_);
00356         case TerminalState::ABORTED:
00357           return SimpleClientGoalState(SimpleClientGoalState::ABORTED, gh_.getTerminalState().text_);
00358         case TerminalState::SUCCEEDED:
00359           return SimpleClientGoalState(SimpleClientGoalState::SUCCEEDED, gh_.getTerminalState().text_);
00360         case TerminalState::LOST:
00361           return SimpleClientGoalState(SimpleClientGoalState::LOST, gh_.getTerminalState().text_);
00362         default:
00363           ROS_ERROR_NAMED("actionlib", "Unknown terminal state [%u]. This is a bug in SimpleActionClient", gh_.getTerminalState().state_);
00364           return SimpleClientGoalState(SimpleClientGoalState::LOST, gh_.getTerminalState().text_);
00365       }
00366     }
00367     case CommState::WAITING_FOR_RESULT:
00368     case CommState::WAITING_FOR_CANCEL_ACK:
00369     {
00370       switch (cur_simple_state_.state_)
00371       {
00372         case SimpleGoalState::PENDING:
00373           return SimpleClientGoalState(SimpleClientGoalState::PENDING);
00374         case SimpleGoalState::ACTIVE:
00375           return SimpleClientGoalState(SimpleClientGoalState::ACTIVE);
00376         case SimpleGoalState::DONE:
00377           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");
00378           return SimpleClientGoalState(SimpleClientGoalState::LOST);
00379         default:
00380           ROS_ERROR_NAMED("actionlib", "Got a SimpleGoalState of [%u]. This is a bug in SimpleActionClient", cur_simple_state_.state_);
00381       }
00382     }
00383     default:
00384       break;
00385   }
00386   ROS_ERROR_NAMED("actionlib", "Error trying to interpret CommState - %u", comm_state_.state_);
00387   return SimpleClientGoalState(SimpleClientGoalState::LOST);
00388 }
00389 
00390 template<class ActionSpec>
00391 typename SimpleActionClient<ActionSpec>::ResultConstPtr SimpleActionClient<ActionSpec>::getResult()
00392 {
00393   if (gh_.isExpired())
00394     ROS_ERROR_NAMED("actionlib", "Trying to getResult() when no goal is running. You are incorrectly using SimpleActionClient");
00395 
00396   if (gh_.getResult())
00397     return gh_.getResult();
00398 
00399   return ResultConstPtr(new Result);
00400 }
00401 
00402 
00403 template<class ActionSpec>
00404 void SimpleActionClient<ActionSpec>::cancelAllGoals()
00405 {
00406   ac_->cancelAllGoals();
00407 }
00408 
00409 template<class ActionSpec>
00410 void SimpleActionClient<ActionSpec>::cancelGoalsAtAndBeforeTime(const ros::Time& time)
00411 {
00412   ac_->cancelGoalsAtAndBeforeTime(time);
00413 }
00414 
00415 template<class ActionSpec>
00416 void SimpleActionClient<ActionSpec>::cancelGoal()
00417 {
00418   if (gh_.isExpired())
00419     ROS_ERROR_NAMED("actionlib", "Trying to cancelGoal() when no goal is running. You are incorrectly using SimpleActionClient");
00420 
00421   gh_.cancel();
00422 }
00423 
00424 template<class ActionSpec>
00425 void SimpleActionClient<ActionSpec>::stopTrackingGoal()
00426 {
00427   if (gh_.isExpired())
00428     ROS_ERROR_NAMED("actionlib", "Trying to stopTrackingGoal() when no goal is running. You are incorrectly using SimpleActionClient");
00429   gh_.reset();
00430 }
00431 
00432 template<class ActionSpec>
00433 void SimpleActionClient<ActionSpec>::handleFeedback(GoalHandleT gh, const FeedbackConstPtr& feedback)
00434 {
00435   if (gh_ != gh)
00436     ROS_ERROR_NAMED("actionlib", "Got a callback on a goalHandle that we're not tracking.  \
00437                This is an internal SimpleActionClient/ActionClient bug.  \
00438                This could also be a GoalID collision");
00439   if (feedback_cb_)
00440     feedback_cb_(feedback);
00441 }
00442 
00443 template<class ActionSpec>
00444 void SimpleActionClient<ActionSpec>::handleTransition(GoalHandleT gh)
00445 {
00446   CommState comm_state_ = gh.getCommState();
00447   switch (comm_state_.state_)
00448   {
00449     case CommState::WAITING_FOR_GOAL_ACK:
00450       ROS_ERROR_NAMED("actionlib", "BUG: Shouldn't ever get a transition callback for WAITING_FOR_GOAL_ACK");
00451       break;
00452     case CommState::PENDING:
00453       ROS_ERROR_COND( cur_simple_state_ != SimpleGoalState::PENDING,
00454                       "BUG: Got a transition to CommState [%s] when our in SimpleGoalState [%s]",
00455                       comm_state_.toString().c_str(), cur_simple_state_.toString().c_str());
00456       break;
00457     case CommState::ACTIVE:
00458       switch (cur_simple_state_.state_)
00459       {
00460         case SimpleGoalState::PENDING:
00461           setSimpleState(SimpleGoalState::ACTIVE);
00462           if (active_cb_)
00463             active_cb_();
00464           break;
00465         case SimpleGoalState::ACTIVE:
00466           break;
00467         case SimpleGoalState::DONE:
00468           ROS_ERROR_NAMED("actionlib", "BUG: Got a transition to CommState [%s] when in SimpleGoalState [%s]",
00469                     comm_state_.toString().c_str(), cur_simple_state_.toString().c_str());
00470           break;
00471         default:
00472           ROS_FATAL("Unknown SimpleGoalState %u", cur_simple_state_.state_);
00473           break;
00474       }
00475       break;
00476     case CommState::WAITING_FOR_RESULT:
00477       break;
00478     case CommState::WAITING_FOR_CANCEL_ACK:
00479       break;
00480     case CommState::RECALLING:
00481       ROS_ERROR_COND( cur_simple_state_ != SimpleGoalState::PENDING,
00482                       "BUG: Got a transition to CommState [%s] when our in SimpleGoalState [%s]",
00483                       comm_state_.toString().c_str(), cur_simple_state_.toString().c_str());
00484       break;
00485     case CommState::PREEMPTING:
00486       switch (cur_simple_state_.state_)
00487       {
00488         case SimpleGoalState::PENDING:
00489           setSimpleState(SimpleGoalState::ACTIVE);
00490           if (active_cb_)
00491             active_cb_();
00492           break;
00493         case SimpleGoalState::ACTIVE:
00494           break;
00495         case SimpleGoalState::DONE:
00496           ROS_ERROR_NAMED("actionlib", "BUG: Got a transition to CommState [%s] when in SimpleGoalState [%s]",
00497                      comm_state_.toString().c_str(), cur_simple_state_.toString().c_str());
00498           break;
00499         default:
00500           ROS_FATAL("Unknown SimpleGoalState %u", cur_simple_state_.state_);
00501           break;
00502       }
00503       break;
00504     case CommState::DONE:
00505       switch (cur_simple_state_.state_)
00506       {
00507         case SimpleGoalState::PENDING:
00508         case SimpleGoalState::ACTIVE:
00509           done_mutex_.lock();
00510           setSimpleState(SimpleGoalState::DONE);
00511           done_mutex_.unlock();
00512 
00513           if (done_cb_)
00514             done_cb_(getState(), gh.getResult());
00515 
00516           done_condition_.notify_all();
00517           break;
00518         case SimpleGoalState::DONE:
00519           ROS_ERROR_NAMED("actionlib", "BUG: Got a second transition to DONE");
00520           break;
00521         default:
00522           ROS_FATAL("Unknown SimpleGoalState %u", cur_simple_state_.state_);
00523           break;
00524       }
00525       break;
00526     default:
00527       ROS_ERROR_NAMED("actionlib", "Unknown CommState received [%u]", comm_state_.state_);
00528       break;
00529   }
00530 }
00531 
00532 template<class ActionSpec>
00533 bool SimpleActionClient<ActionSpec>::waitForResult(const ros::Duration& timeout )
00534 {
00535   if (gh_.isExpired())
00536   {
00537     ROS_ERROR_NAMED("actionlib", "Trying to waitForGoalToFinish() when no goal is running. You are incorrectly using SimpleActionClient");
00538     return false;
00539   }
00540 
00541   if (timeout < ros::Duration(0,0))
00542     ROS_WARN_NAMED("actionlib", "Timeouts can't be negative. Timeout is [%.2fs]", timeout.toSec());
00543 
00544   ros::Time timeout_time = ros::Time::now() + timeout;
00545 
00546   boost::mutex::scoped_lock lock(done_mutex_);
00547 
00548   // Hardcode how often we check for node.ok()
00549   ros::Duration loop_period = ros::Duration().fromSec(.1);
00550 
00551   while (nh_.ok())
00552   {
00553     // Determine how long we should wait
00554     ros::Duration time_left = timeout_time - ros::Time::now();
00555 
00556     // Check if we're past the timeout time
00557     if (timeout > ros::Duration(0,0) && time_left <= ros::Duration(0,0) )
00558     {
00559       break;
00560     }
00561     
00562     if (cur_simple_state_ == SimpleGoalState::DONE)
00563     {
00564       break;
00565     }
00566 
00567 
00568     // Truncate the time left
00569     if (time_left > loop_period || timeout == ros::Duration())
00570       time_left = loop_period;
00571 
00572     done_condition_.timed_wait(lock, boost::posix_time::milliseconds(time_left.toSec() * 1000.0f));
00573   }
00574   
00575   return (cur_simple_state_ == SimpleGoalState::DONE);
00576 }
00577 
00578 template<class ActionSpec>
00579 SimpleClientGoalState SimpleActionClient<ActionSpec>::sendGoalAndWait(const Goal& goal,
00580                                                                       const ros::Duration& execute_timeout,
00581                                                                       const ros::Duration& preempt_timeout)
00582 {
00583   sendGoal(goal);
00584 
00585   // See if the goal finishes in time
00586   if (waitForResult(execute_timeout))
00587   {
00588     ROS_DEBUG_NAMED("actionlib", "Goal finished within specified execute_timeout [%.2f]", execute_timeout.toSec());
00589     return getState();
00590   }
00591 
00592   ROS_DEBUG_NAMED("actionlib", "Goal didn't finish within specified execute_timeout [%.2f]", execute_timeout.toSec());
00593 
00594   // It didn't finish in time, so we need to preempt it
00595   cancelGoal();
00596 
00597   // Now wait again and see if it finishes
00598   if (waitForResult(preempt_timeout))
00599     ROS_DEBUG_NAMED("actionlib", "Preempt finished within specified preempt_timeout [%.2f]", preempt_timeout.toSec());
00600   else
00601     ROS_DEBUG_NAMED("actionlib", "Preempt didn't finish specified preempt_timeout [%.2f]", preempt_timeout.toSec());
00602   return getState();
00603 }
00604 
00605 }
00606 
00607 #undef DEPRECATED
00608 
00609 #endif // ACTIONLIB_SINGLE_GOAL_ACTION_CLIENT_H_
simple_action_client.h
