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_ACTION_CLIENT_H_
00036 #define ACTIONLIB_ACTION_CLIENT_H_
00037 
00038 #include <boost/thread/condition.hpp>
00039 
00040 #include "ros/ros.h"
00041 #include "ros/callback_queue_interface.h"
00042 #include <actionlib/client/client_helpers.h>
00043 #include <actionlib/client/connection_monitor.h>
00044 #include <actionlib/destruction_guard.h>
00045 
00046 namespace actionlib
00047 {
00048 
00056 template <class ActionSpec>
00057 class ActionClient
00058 {
00059 public:
00060   typedef ClientGoalHandle<ActionSpec> GoalHandle;
00061 
00062 private:
00063   ACTION_DEFINITION(ActionSpec);
00064   typedef ActionClient<ActionSpec> ActionClientT;
00065   typedef boost::function<void (GoalHandle) > TransitionCallback;
00066   typedef boost::function<void (GoalHandle, const FeedbackConstPtr&) > FeedbackCallback;
00067   typedef boost::function<void (const ActionGoalConstPtr)> SendGoalFunc;
00068 
00069 public:
00078  ActionClient(const std::string& name, ros::CallbackQueueInterface* queue = NULL)
00079   : n_(name), guard_(new DestructionGuard()),
00080     manager_(guard_)
00081   {
00082     initClient(queue);
00083   }
00084 
00095   ActionClient(const ros::NodeHandle& n, const std::string& name, ros::CallbackQueueInterface* queue = NULL)
00096     : n_(n, name), guard_(new DestructionGuard()),
00097     manager_(guard_)
00098   {
00099     initClient(queue);
00100   }
00101 
00102   ~ActionClient()
00103   {
00104     ROS_DEBUG_NAMED("actionlib", "ActionClient: Waiting for destruction guard to clean up");
00105     guard_->destruct();
00106     ROS_DEBUG_NAMED("actionlib", "ActionClient: destruction guard destruct() done");
00107   }
00108 
00109 
00115   GoalHandle sendGoal(const Goal& goal,
00116                       TransitionCallback transition_cb = TransitionCallback(),
00117                       FeedbackCallback   feedback_cb   = FeedbackCallback())
00118   {
00119     ROS_DEBUG_NAMED("actionlib", "about to start initGoal()");
00120     GoalHandle gh = manager_.initGoal(goal, transition_cb, feedback_cb);
00121     ROS_DEBUG_NAMED("actionlib", "Done with initGoal()");
00122 
00123     return gh;
00124   }
00125 
00132   void cancelAllGoals()
00133   {
00134     actionlib_msgs::GoalID cancel_msg;
00135     // CancelAll policy encoded by stamp=0, id=0
00136     cancel_msg.stamp = ros::Time(0,0);
00137     cancel_msg.id = "";
00138     cancel_pub_.publish(cancel_msg);
00139   }
00140 
00145   void cancelGoalsAtAndBeforeTime(const ros::Time& time)
00146   {
00147     actionlib_msgs::GoalID cancel_msg;
00148     cancel_msg.stamp = time;
00149     cancel_msg.id = "";
00150     cancel_pub_.publish(cancel_msg);
00151   }
00152 
00166   bool waitForActionServerToStart(const ros::Duration& timeout = ros::Duration(0,0) )
00167   {
00168     if (connection_monitor_)
00169       return connection_monitor_->waitForActionServerToStart(timeout, n_);
00170     else
00171       return false;
00172   }
00173 
00178   bool isServerConnected()
00179   {
00180     return connection_monitor_->isServerConnected();
00181   }
00182 
00183 private:
00184   ros::NodeHandle n_;
00185 
00186   boost::shared_ptr<DestructionGuard> guard_;
00187   GoalManager<ActionSpec> manager_;
00188 
00189   ros::Subscriber result_sub_;
00190   ros::Subscriber feedback_sub_;
00191 
00192   boost::shared_ptr<ConnectionMonitor> connection_monitor_;   // Have to destroy subscribers and publishers before the connection_monitor_, since we call callbacks in the connection_monitor_
00193 
00194   ros::Publisher  goal_pub_;
00195   ros::Publisher  cancel_pub_;
00196   ros::Subscriber status_sub_;
00197 
00198   void sendGoalFunc(const ActionGoalConstPtr& action_goal)
00199   {
00200     goal_pub_.publish(action_goal);
00201   }
00202 
00203   void sendCancelFunc(const actionlib_msgs::GoalID& cancel_msg)
00204   {
00205     cancel_pub_.publish(cancel_msg);
00206   }
00207 
00208   void initClient(ros::CallbackQueueInterface* queue)
00209   {
00210     status_sub_   = queue_subscribe("status",   1, &ActionClientT::statusCb,   this, queue);
00211     feedback_sub_ = queue_subscribe("feedback", 1, &ActionClientT::feedbackCb, this, queue);
00212     result_sub_   = queue_subscribe("result",   1, &ActionClientT::resultCb,   this, queue);
00213 
00214     connection_monitor_.reset(new ConnectionMonitor(feedback_sub_, status_sub_));
00215 
00216     // Start publishers and subscribers
00217     goal_pub_ = queue_advertise<ActionGoal>("goal", 1,
00218                                             boost::bind(&ConnectionMonitor::goalConnectCallback,    connection_monitor_, _1),
00219                                             boost::bind(&ConnectionMonitor::goalDisconnectCallback, connection_monitor_, _1),
00220                                             queue);
00221     cancel_pub_ = queue_advertise<actionlib_msgs::GoalID>("cancel", 1,
00222                                             boost::bind(&ConnectionMonitor::cancelConnectCallback,    connection_monitor_, _1),
00223                                             boost::bind(&ConnectionMonitor::cancelDisconnectCallback, connection_monitor_, _1),
00224                                             queue);
00225 
00226     manager_.registerSendGoalFunc(boost::bind(&ActionClientT::sendGoalFunc, this, _1));
00227     manager_.registerCancelFunc(boost::bind(&ActionClientT::sendCancelFunc, this, _1));
00228 
00229   }
00230 
00231   template <class M>
00232   ros::Publisher queue_advertise(const std::string& topic, uint32_t queue_size,
00233                                  const ros::SubscriberStatusCallback& connect_cb,
00234                                  const ros::SubscriberStatusCallback& disconnect_cb,
00235                                  ros::CallbackQueueInterface* queue)
00236   {
00237     ros::AdvertiseOptions ops;
00238     ops.init<M>(topic, queue_size, connect_cb, disconnect_cb);
00239     ops.tracked_object = ros::VoidPtr();
00240     ops.latch = false;
00241     ops.callback_queue = queue;
00242     return n_.advertise(ops);
00243   }
00244 
00245   template<class M, class T>
00246   ros::Subscriber queue_subscribe(const std::string& topic, uint32_t queue_size, void(T::*fp)(const boost::shared_ptr<M const>&), T* obj, ros::CallbackQueueInterface* queue)
00247   {
00248     ros::SubscribeOptions ops;
00249     ops.init<M>(topic, queue_size, boost::bind(fp, obj, _1));
00250     ops.transport_hints = ros::TransportHints();
00251     ops.callback_queue = queue;
00252     return n_.subscribe(ops);
00253   }
00254 
00255   void statusCb(const actionlib_msgs::GoalStatusArrayConstPtr& status_array)
00256   {
00257     ROS_DEBUG_NAMED("actionlib", "Getting status over the wire.");
00258     if (connection_monitor_)
00259       connection_monitor_->processStatus(status_array);
00260     manager_.updateStatuses(status_array);
00261   }
00262 
00263   void feedbackCb(const ActionFeedbackConstPtr& action_feedback)
00264   {
00265     manager_.updateFeedbacks(action_feedback);
00266   }
00267 
00268   void resultCb(const ActionResultConstPtr& action_result)
00269   {
00270     manager_.updateResults(action_result);
00271   }
00272 };
00273 
00274 
00275 }
00276 
00277 #endif


actionlib
Author(s): Eitan Marder-Eppstein, Vijay Pradeep
autogenerated on Sun Oct 5 2014 22:02:55