00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
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
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_;
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
00217 goal_pub_ = queue_advertise<ActionGoal>("goal", 10,
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", 10,
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