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__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
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
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_;
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
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
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 }
00307
00308 #endif // ACTIONLIB__CLIENT__ACTION_CLIENT_H_