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__ACTION_CLIENT_H_
00036 #define ACTIONLIB__CLIENT__ACTION_CLIENT_H_
00037 
00038 #include <actionlib/client/client_helpers.h>
00039 #include <actionlib/client/connection_monitor.h>
00040 #include <actionlib/destruction_guard.h>
00041 
00042 #include <boost/thread/condition.hpp>
00043 #include <string>
00044 
00045 #include "ros/ros.h"
00046 #include "ros/callback_queue_interface.h"
00047 
00048 namespace actionlib
00049 {
00050 
00058 template<class ActionSpec>
00059 class ActionClient
00060 {
00061 public:
00062   typedef ClientGoalHandle<ActionSpec> GoalHandle;
00063 
00064 private:
00065   ACTION_DEFINITION(ActionSpec);
00066   typedef ActionClient<ActionSpec> ActionClientT;
00067   typedef boost::function<void (GoalHandle)> TransitionCallback;
00068   typedef boost::function<void (GoalHandle, const FeedbackConstPtr &)> FeedbackCallback;
00069   typedef boost::function<void (const ActionGoalConstPtr)> SendGoalFunc;
00070 
00071 public:
00080   ActionClient(const std::string & name, ros::CallbackQueueInterface * queue = NULL)
00081   : n_(name), guard_(new DestructionGuard()),
00082     manager_(guard_)
00083   {
00084     initClient(queue);
00085   }
00086 
00097   ActionClient(const ros::NodeHandle & n, const std::string & name,
00098     ros::CallbackQueueInterface * queue = NULL)
00099   : n_(n, name), guard_(new DestructionGuard()),
00100     manager_(guard_)
00101   {
00102     initClient(queue);
00103   }
00104 
00105   ~ActionClient()
00106   {
00107     ROS_DEBUG_NAMED("actionlib", "ActionClient: Waiting for destruction guard to clean up");
00108     guard_->destruct();
00109     ROS_DEBUG_NAMED("actionlib", "ActionClient: destruction guard destruct() done");
00110   }
00111 
00112 
00118   GoalHandle sendGoal(const Goal & goal,
00119     TransitionCallback transition_cb = TransitionCallback(),
00120     FeedbackCallback feedback_cb = FeedbackCallback())
00121   {
00122     ROS_DEBUG_NAMED("actionlib", "about to start initGoal()");
00123     GoalHandle gh = manager_.initGoal(goal, transition_cb, feedback_cb);
00124     ROS_DEBUG_NAMED("actionlib", "Done with initGoal()");
00125 
00126     return gh;
00127   }
00128 
00135   void cancelAllGoals()
00136   {
00137     actionlib_msgs::GoalID cancel_msg;
00138     // CancelAll policy encoded by stamp=0, id=0
00139     cancel_msg.stamp = ros::Time(0, 0);
00140     cancel_msg.id = "";
00141     cancel_pub_.publish(cancel_msg);
00142   }
00143 
00148   void cancelGoalsAtAndBeforeTime(const ros::Time & time)
00149   {
00150     actionlib_msgs::GoalID cancel_msg;
00151     cancel_msg.stamp = time;
00152     cancel_msg.id = "";
00153     cancel_pub_.publish(cancel_msg);
00154   }
00155 
00169   bool waitForActionServerToStart(const ros::Duration & timeout = ros::Duration(0, 0) )
00170   {
00171     // if ros::Time::isSimTime(), then wait for it to become valid
00172     if (!ros::Time::waitForValid(ros::WallDuration(timeout.sec, timeout.nsec))) {
00173       return false;
00174     }
00175 
00176     if (connection_monitor_) {
00177       return connection_monitor_->waitForActionServerToStart(timeout, n_);
00178     } else {
00179       return false;
00180     }
00181   }
00182 
00187   bool isServerConnected()
00188   {
00189     return connection_monitor_->isServerConnected();
00190   }
00191 
00192 private:
00193   ros::NodeHandle n_;
00194 
00195   boost::shared_ptr<DestructionGuard> guard_;
00196   GoalManager<ActionSpec> manager_;
00197 
00198   ros::Subscriber result_sub_;
00199   ros::Subscriber feedback_sub_;
00200 
00201   boost::shared_ptr<ConnectionMonitor> connection_monitor_;   // Have to destroy subscribers and publishers before the connection_monitor_, since we call callbacks in the connection_monitor_
00202 
00203   ros::Publisher goal_pub_;
00204   ros::Publisher cancel_pub_;
00205   ros::Subscriber status_sub_;
00206 
00207   void sendGoalFunc(const ActionGoalConstPtr & action_goal)
00208   {
00209     goal_pub_.publish(action_goal);
00210   }
00211 
00212   void sendCancelFunc(const actionlib_msgs::GoalID & cancel_msg)
00213   {
00214     cancel_pub_.publish(cancel_msg);
00215   }
00216 
00217   void initClient(ros::CallbackQueueInterface * queue)
00218   {
00219     ros::Time::waitForValid();
00220     // read parameters indicating publish/subscribe queue sizes
00221     int pub_queue_size;
00222     int sub_queue_size;
00223     n_.param("actionlib_client_pub_queue_size", pub_queue_size, 10);
00224     n_.param("actionlib_client_sub_queue_size", sub_queue_size, 1);
00225     if (pub_queue_size < 0) {pub_queue_size = 10;}
00226     if (sub_queue_size < 0) {sub_queue_size = 1;}
00227 
00228     status_sub_ = queue_subscribe("status", static_cast<uint32_t>(sub_queue_size),
00229         &ActionClientT::statusCb, this, queue);
00230     feedback_sub_ = queue_subscribe("feedback", static_cast<uint32_t>(sub_queue_size),
00231         &ActionClientT::feedbackCb, this, queue);
00232     result_sub_ = queue_subscribe("result", static_cast<uint32_t>(sub_queue_size),
00233         &ActionClientT::resultCb, this, queue);
00234 
00235     connection_monitor_.reset(new ConnectionMonitor(feedback_sub_, result_sub_));
00236 
00237     // Start publishers and subscribers
00238     goal_pub_ = queue_advertise<ActionGoal>("goal", static_cast<uint32_t>(pub_queue_size),
00239         boost::bind(&ConnectionMonitor::goalConnectCallback, connection_monitor_, _1),
00240         boost::bind(&ConnectionMonitor::goalDisconnectCallback, connection_monitor_, _1),
00241         queue);
00242     cancel_pub_ =
00243       queue_advertise<actionlib_msgs::GoalID>("cancel", static_cast<uint32_t>(pub_queue_size),
00244         boost::bind(&ConnectionMonitor::cancelConnectCallback, connection_monitor_, _1),
00245         boost::bind(&ConnectionMonitor::cancelDisconnectCallback, connection_monitor_, _1),
00246         queue);
00247 
00248     manager_.registerSendGoalFunc(boost::bind(&ActionClientT::sendGoalFunc, this, _1));
00249     manager_.registerCancelFunc(boost::bind(&ActionClientT::sendCancelFunc, this, _1));
00250   }
00251 
00252   template<class M>
00253   ros::Publisher queue_advertise(const std::string & topic, uint32_t queue_size,
00254     const ros::SubscriberStatusCallback & connect_cb,
00255     const ros::SubscriberStatusCallback & disconnect_cb,
00256     ros::CallbackQueueInterface * queue)
00257   {
00258     ros::AdvertiseOptions ops;
00259     ops.init<M>(topic, queue_size, connect_cb, disconnect_cb);
00260     ops.tracked_object = ros::VoidPtr();
00261     ops.latch = false;
00262     ops.callback_queue = queue;
00263     return n_.advertise(ops);
00264   }
00265 
00266   template<class M, class T>
00267   ros::Subscriber queue_subscribe(const std::string & topic, uint32_t queue_size, void (T::* fp)(
00268       const ros::MessageEvent<M const> &), T * obj, ros::CallbackQueueInterface * queue)
00269   {
00270     ros::SubscribeOptions ops;
00271     ops.callback_queue = queue;
00272     ops.topic = topic;
00273     ops.queue_size = queue_size;
00274     ops.md5sum = ros::message_traits::md5sum<M>();
00275     ops.datatype = ros::message_traits::datatype<M>();
00276     ops.helper = ros::SubscriptionCallbackHelperPtr(
00277       new ros::SubscriptionCallbackHelperT<const ros::MessageEvent<M const> &>(
00278         boost::bind(fp, obj, _1)
00279       )
00280       );
00281     return n_.subscribe(ops);
00282   }
00283 
00284   void statusCb(const ros::MessageEvent<actionlib_msgs::GoalStatusArray const> & status_array_event)
00285   {
00286     ROS_DEBUG_NAMED("actionlib", "Getting status over the wire.");
00287     if (connection_monitor_) {
00288       connection_monitor_->processStatus(
00289         status_array_event.getConstMessage(), status_array_event.getPublisherName());
00290     }
00291     manager_.updateStatuses(status_array_event.getConstMessage());
00292   }
00293 
00294   void feedbackCb(const ros::MessageEvent<ActionFeedback const> & action_feedback)
00295   {
00296     manager_.updateFeedbacks(action_feedback.getConstMessage());
00297   }
00298 
00299   void resultCb(const ros::MessageEvent<ActionResult const> & action_result)
00300   {
00301     manager_.updateResults(action_result.getConstMessage());
00302   }
00303 };
00304 
00305 
00306 }  // namespace actionlib
00307 
00308 #endif  // ACTIONLIB__CLIENT__ACTION_CLIENT_H_


actionlib
Author(s): Eitan Marder-Eppstein, Vijay Pradeep
autogenerated on Thu Apr 19 2018 02:52:35