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__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   // Signalling Stuff
00230   boost::condition done_condition_;
00231   boost::mutex done_mutex_;
00232 
00233   // User Callbacks
00234   SimpleDoneCallback done_cb_;
00235   SimpleActiveCallback active_cb_;
00236   SimpleFeedbackCallback feedback_cb_;
00237 
00238   // Spin Thread Stuff
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_;  // Action client depends on callback_queue, so it must be destroyed before callback_queue
00245 
00246   // ***** Private Funcs *****
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   // Reset the old GoalHandle, so that our callbacks won't get called anymore
00323   gh_.reset();
00324 
00325   // Store all the callbacks
00326   done_cb_ = done_cb;
00327   active_cb_ = active_cb;
00328   feedback_cb_ = feedback_cb;
00329 
00330   cur_simple_state_ = SimpleGoalState::PENDING;
00331 
00332   // Send the goal to the ActionServer
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   // Hardcode how often we check for node.ok()
00583   ros::Duration loop_period = ros::Duration().fromSec(.1);
00584 
00585   while (nh_.ok()) {
00586     // Determine how long we should wait
00587     ros::Duration time_left = timeout_time - ros::Time::now();
00588 
00589     // Check if we're past the timeout time
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     // Truncate the time left
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   // See if the goal finishes in time
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   // It didn't finish in time, so we need to preempt it
00628   cancelGoal();
00629 
00630   // Now wait again and see if it finishes
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 }  // namespace actionlib
00642 
00643 #undef DEPRECATED
00644 
00645 #endif  // ACTIONLIB__CLIENT__SIMPLE_ACTION_CLIENT_H_


actionlib
Author(s): Eitan Marder-Eppstein, Vijay Pradeep
autogenerated on Tue Dec 19 2017 03:23:31