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 #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   // Signalling Stuff
00223   boost::condition done_condition_;
00224   boost::mutex done_mutex_;
00225 
00226   // User Callbacks
00227   SimpleDoneCallback done_cb_;
00228   SimpleActiveCallback active_cb_;
00229   SimpleFeedbackCallback feedback_cb_;
00230 
00231   // Spin Thread Stuff
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_;  // Action client depends on callback_queue, so it must be destroyed before callback_queue
00238 
00239   // ***** Private Funcs *****
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   // Reset the old GoalHandle, so that our callbacks won't get called anymore
00319   gh_.reset();
00320 
00321   // Store all the callbacks
00322   done_cb_     = done_cb;
00323   active_cb_   = active_cb;
00324   feedback_cb_ = feedback_cb;
00325 
00326   cur_simple_state_ = SimpleGoalState::PENDING;
00327 
00328   // Send the goal to the ActionServer
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   // Hardcode how often we check for node.ok()
00556   ros::Duration loop_period = ros::Duration().fromSec(.1);
00557 
00558   while (nh_.ok())
00559   {
00560     // Determine how long we should wait
00561     ros::Duration time_left = timeout_time - ros::Time::now();
00562 
00563     // Check if we're past the timeout time
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     // Truncate the time left
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   // See if the goal finishes in time
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   // It didn't finish in time, so we need to preempt it
00602   cancelGoal();
00603 
00604   // Now wait again and see if it finishes
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_


actionlib
Author(s): Eitan Marder-Eppstein, Vijay Pradeep
autogenerated on Fri Aug 28 2015 10:04:41