action_client.h
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 #ifndef ACTIONLIB__CLIENT__ACTION_CLIENT_H_
36 #define ACTIONLIB__CLIENT__ACTION_CLIENT_H_
37 
41 
42 #include <boost/thread/condition.hpp>
43 #include <string>
44 
45 #include "ros/ros.h"
47 
48 namespace actionlib
49 {
50 
58 template<class ActionSpec>
59 class ActionClient
60 {
61 public:
62  typedef ClientGoalHandle<ActionSpec> GoalHandle;
63 
64 private:
65  ACTION_DEFINITION(ActionSpec)
66  typedef ActionClient<ActionSpec> ActionClientT;
67  typedef boost::function<void (GoalHandle)> TransitionCallback;
68  typedef boost::function<void (GoalHandle, const FeedbackConstPtr &)> FeedbackCallback;
69  typedef boost::function<void (const ActionGoalConstPtr)> SendGoalFunc;
70 
71 public:
80  ActionClient(const std::string & name, ros::CallbackQueueInterface * queue = nullptr)
81  : n_(name), guard_(new DestructionGuard()),
83  {
84  initClient(queue);
85  }
86 
97  ActionClient(const ros::NodeHandle & n, const std::string & name,
98  ros::CallbackQueueInterface * queue = nullptr)
99  : n_(n, name), guard_(new DestructionGuard()),
101  {
102  initClient(queue);
103  }
104 
105  ~ActionClient()
106  {
107  ROS_DEBUG_NAMED("actionlib", "ActionClient: Waiting for destruction guard to clean up");
108  guard_->destruct();
109  ROS_DEBUG_NAMED("actionlib", "ActionClient: destruction guard destruct() done");
110  }
111 
112 
118  GoalHandle sendGoal(const Goal & goal,
119  TransitionCallback transition_cb = TransitionCallback(),
120  FeedbackCallback feedback_cb = FeedbackCallback())
121  {
122  ROS_DEBUG_NAMED("actionlib", "about to start initGoal()");
123  GoalHandle gh = manager_.initGoal(goal, transition_cb, feedback_cb);
124  ROS_DEBUG_NAMED("actionlib", "Done with initGoal()");
125 
126  return gh;
127  }
128 
135  void cancelAllGoals()
136  {
137  actionlib_msgs::GoalID cancel_msg;
138  // CancelAll policy encoded by stamp=0, id=0
139  cancel_msg.stamp = ros::Time(0, 0);
140  cancel_msg.id = "";
141  cancel_pub_.publish(cancel_msg);
142  }
143 
148  void cancelGoalsAtAndBeforeTime(const ros::Time & time)
149  {
150  actionlib_msgs::GoalID cancel_msg;
151  cancel_msg.stamp = time;
152  cancel_msg.id = "";
153  cancel_pub_.publish(cancel_msg);
154  }
155 
170  {
171  // if ros::Time::isSimTime(), then wait for it to become valid
172  if (!ros::Time::waitForValid(ros::WallDuration(timeout.sec, timeout.nsec))) {
173  return false;
174  }
175 
176  if (connection_monitor_) {
177  return connection_monitor_->waitForActionServerToStart(timeout, n_);
178  } else {
179  return false;
180  }
181  }
182 
187  bool isServerConnected()
188  {
189  return connection_monitor_->isServerConnected();
190  }
191 
192 private:
194 
197 
200 
201  boost::shared_ptr<ConnectionMonitor> connection_monitor_; // Have to destroy subscribers and publishers before the connection_monitor_, since we call callbacks in the connection_monitor_
202 
206 
207  void sendGoalFunc(const ActionGoalConstPtr & action_goal)
208  {
209  goal_pub_.publish(action_goal);
210  }
211 
212  void sendCancelFunc(const actionlib_msgs::GoalID & cancel_msg)
213  {
214  cancel_pub_.publish(cancel_msg);
215  }
216 
218  {
220  // read parameters indicating publish/subscribe queue sizes
221  int pub_queue_size;
222  int sub_queue_size;
223  n_.param("actionlib_client_pub_queue_size", pub_queue_size, 10);
224  n_.param("actionlib_client_sub_queue_size", sub_queue_size, -1);
225  if (pub_queue_size < 0) {pub_queue_size = 10;}
226  if (sub_queue_size < 0) {sub_queue_size = 0;}
227 
228  status_sub_ = queue_subscribe("status", static_cast<uint32_t>(sub_queue_size),
229  &ActionClientT::statusCb, this, queue);
230  feedback_sub_ = queue_subscribe("feedback", static_cast<uint32_t>(sub_queue_size),
231  &ActionClientT::feedbackCb, this, queue);
232  result_sub_ = queue_subscribe("result", static_cast<uint32_t>(sub_queue_size),
233  &ActionClientT::resultCb, this, queue);
234 
236 
237  // Start publishers and subscribers
238  goal_pub_ = queue_advertise<ActionGoal>("goal", static_cast<uint32_t>(pub_queue_size),
239  boost::bind(&ConnectionMonitor::goalConnectCallback, connection_monitor_, boost::placeholders::_1),
240  boost::bind(&ConnectionMonitor::goalDisconnectCallback, connection_monitor_, boost::placeholders::_1),
241  queue);
242  cancel_pub_ =
243  queue_advertise<actionlib_msgs::GoalID>("cancel", static_cast<uint32_t>(pub_queue_size),
244  boost::bind(&ConnectionMonitor::cancelConnectCallback, connection_monitor_, boost::placeholders::_1),
245  boost::bind(&ConnectionMonitor::cancelDisconnectCallback, connection_monitor_, boost::placeholders::_1),
246  queue);
247 
248  manager_.registerSendGoalFunc(boost::bind(&ActionClientT::sendGoalFunc, this, boost::placeholders::_1));
249  manager_.registerCancelFunc(boost::bind(&ActionClientT::sendCancelFunc, this, boost::placeholders::_1));
250  }
251 
252  template<class M>
253  ros::Publisher queue_advertise(const std::string & topic, uint32_t queue_size,
254  const ros::SubscriberStatusCallback & connect_cb,
255  const ros::SubscriberStatusCallback & disconnect_cb,
257  {
259  ops.init<M>(topic, queue_size, connect_cb, disconnect_cb);
261  ops.latch = false;
262  ops.callback_queue = queue;
263  return n_.advertise(ops);
264  }
265 
266  template<class M, class T>
267  ros::Subscriber queue_subscribe(const std::string & topic, uint32_t queue_size, void (T::* fp)(
269  {
271  ops.callback_queue = queue;
272  ops.topic = topic;
273  ops.queue_size = queue_size;
274  ops.md5sum = ros::message_traits::md5sum<M>();
275  ops.datatype = ros::message_traits::datatype<M>();
278  boost::bind(fp, obj, boost::placeholders::_1)
279  )
280  );
281  return n_.subscribe(ops);
282  }
283 
285  {
286  ROS_DEBUG_NAMED("actionlib", "Getting status over the wire.");
287  if (connection_monitor_) {
288  connection_monitor_->processStatus(
289  status_array_event.getConstMessage(), status_array_event.getPublisherName());
290  }
291  manager_.updateStatuses(status_array_event.getConstMessage());
292  }
293 
294  void feedbackCb(const ros::MessageEvent<ActionFeedback const> & action_feedback)
295  {
296  manager_.updateFeedbacks(action_feedback.getConstMessage());
297  }
298 
299  void resultCb(const ros::MessageEvent<ActionResult const> & action_result)
300  {
301  manager_.updateResults(action_result.getConstMessage());
302  }
303 };
304 
305 
306 } // namespace actionlib
307 
308 #endif // ACTIONLIB__CLIENT__ACTION_CLIENT_H_
actionlib::ActionClient::queue_advertise
ros::Publisher queue_advertise(const std::string &topic, uint32_t queue_size, const ros::SubscriberStatusCallback &connect_cb, const ros::SubscriberStatusCallback &disconnect_cb, ros::CallbackQueueInterface *queue)
Definition: action_client.h:317
actionlib::ConnectionMonitor::cancelConnectCallback
void cancelConnectCallback(const ros::SingleSubscriberPublisher &pub)
Definition: connection_monitor.cpp:119
actionlib::ActionClient::result_sub_
ros::Subscriber result_sub_
Definition: action_client.h:262
actionlib::ConnectionMonitor::goalConnectCallback
void goalConnectCallback(const ros::SingleSubscriberPublisher &pub)
Definition: connection_monitor.cpp:62
ros::SubscriptionCallbackHelperPtr
boost::shared_ptr< SubscriptionCallbackHelper > SubscriptionCallbackHelperPtr
ros::Publisher
actionlib::ActionClient::connection_monitor_
boost::shared_ptr< ConnectionMonitor > connection_monitor_
Definition: action_client.h:265
boost::shared_ptr
ACTION_DEFINITION
#define ACTION_DEFINITION(ActionSpec)
Definition: action_definition.h:77
actionlib::ActionClient::feedbackCb
void feedbackCb(const ros::MessageEvent< ActionFeedback const > &action_feedback)
Definition: action_client.h:358
actionlib::ActionClient::ActionClient
ActionClient(const std::string &name, ros::CallbackQueueInterface *queue=nullptr)
Simple constructor.
Definition: action_client.h:144
ros.h
ros::AdvertiseOptions::callback_queue
CallbackQueueInterface * callback_queue
actionlib::ActionClient::queue_subscribe
ros::Subscriber queue_subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(const ros::MessageEvent< M const > &), T *obj, ros::CallbackQueueInterface *queue)
Definition: action_client.h:331
callback_queue_interface.h
ros::SubscribeOptions::topic
std::string topic
ros::SubscribeOptions::callback_queue
CallbackQueueInterface * callback_queue
ros::SubscriptionCallbackHelperT
ros::MessageEvent::getPublisherName
const std::string & getPublisherName() const
actionlib::ConnectionMonitor::goalDisconnectCallback
void goalDisconnectCallback(const ros::SingleSubscriberPublisher &pub)
Definition: connection_monitor.cpp:82
ros::SubscriberStatusCallback
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
actionlib::ActionClient::manager_
GoalManager< ActionSpec > manager_
Definition: action_client.h:260
ros::MessageEvent::getConstMessage
const boost::shared_ptr< ConstMessage > & getConstMessage() const
actionlib::ActionClient::ActionClientT
ActionClient< ActionSpec > ActionClientT
Definition: action_client.h:130
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
actionlib::ActionClient::sendCancelFunc
void sendCancelFunc(const actionlib_msgs::GoalID &cancel_msg)
Definition: action_client.h:276
ros::NodeHandle::advertise
Publisher advertise(AdvertiseOptions &ops)
ros::AdvertiseOptions
client_helpers.h
actionlib::ActionClient::resultCb
void resultCb(const ros::MessageEvent< ActionResult const > &action_result)
Definition: action_client.h:363
ros::SubscribeOptions::md5sum
std::string md5sum
actionlib::ActionClient::~ActionClient
~ActionClient()
Definition: action_client.h:169
actionlib::ActionClient::goal_pub_
ros::Publisher goal_pub_
Definition: action_client.h:267
actionlib::ConnectionMonitor::cancelDisconnectCallback
void cancelDisconnectCallback(const ros::SingleSubscriberPublisher &pub)
Definition: connection_monitor.cpp:139
actionlib::ConnectionMonitor
Definition: connection_monitor.h:85
actionlib::ActionClient::cancelAllGoals
void cancelAllGoals()
Cancel all goals currently running on the action server.
Definition: action_client.h:199
ros::AdvertiseOptions::latch
bool latch
ROS_DEBUG_NAMED
#define ROS_DEBUG_NAMED(name,...)
actionlib::ActionClient::status_sub_
ros::Subscriber status_sub_
Definition: action_client.h:269
connection_monitor.h
actionlib::ActionClient::initClient
void initClient(ros::CallbackQueueInterface *queue)
Definition: action_client.h:281
destruction_guard.h
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, const boost::function< void(C)> &callback, const VoidConstPtr &tracked_object=VoidConstPtr(), const TransportHints &transport_hints=TransportHints())
actionlib::ActionClient::n_
ros::NodeHandle n_
Definition: action_client.h:257
actionlib::ActionClient::sendGoalFunc
void sendGoalFunc(const ActionGoalConstPtr &action_goal)
Definition: action_client.h:271
actionlib::ActionClient::TransitionCallback
boost::function< void(GoalHandle)> TransitionCallback
Definition: action_client.h:131
ros::SubscribeOptions
ros::Time
actionlib::ActionClient::GoalHandle
ClientGoalHandle< ActionSpec > GoalHandle
Definition: action_client.h:126
ros::SubscribeOptions::queue_size
uint32_t queue_size
ros::AdvertiseOptions::init
void init(const std::string &_topic, uint32_t _queue_size, const SubscriberStatusCallback &_connect_cb=SubscriberStatusCallback(), const SubscriberStatusCallback &_disconnect_cb=SubscriberStatusCallback())
actionlib::ActionClient::cancelGoalsAtAndBeforeTime
void cancelGoalsAtAndBeforeTime(const ros::Time &time)
Cancel all goals that were stamped at and before the specified time.
Definition: action_client.h:212
actionlib
Definition: action_definition.h:40
ros::Time::waitForValid
static bool waitForValid()
actionlib::ActionClient::isServerConnected
bool isServerConnected()
Checks if the action client is successfully connected to the action server.
Definition: action_client.h:251
actionlib::ActionClient::feedback_sub_
ros::Subscriber feedback_sub_
Definition: action_client.h:263
ros::NodeHandle::param
T param(const std::string &param_name, const T &default_val) const
actionlib::ActionClient::statusCb
void statusCb(const ros::MessageEvent< actionlib_msgs::GoalStatusArray const > &status_array_event)
Definition: action_client.h:348
ros::SubscribeOptions::datatype
std::string datatype
ros::VoidPtr
boost::shared_ptr< void > VoidPtr
ros::WallDuration
ros::SubscribeOptions::helper
SubscriptionCallbackHelperPtr helper
ros::AdvertiseOptions::tracked_object
VoidConstPtr tracked_object
ros::Duration
actionlib::ActionClient::guard_
boost::shared_ptr< DestructionGuard > guard_
Definition: action_client.h:259
actionlib::ActionClient::waitForActionServerToStart
bool waitForActionServerToStart(const ros::Duration &timeout=ros::Duration(0, 0))
Waits for the ActionServer to connect to this client Often, it can take a second for the action serve...
Definition: action_client.h:233
actionlib::ActionClient::sendGoal
GoalHandle sendGoal(const Goal &goal, TransitionCallback transition_cb=TransitionCallback(), FeedbackCallback feedback_cb=FeedbackCallback())
Sends a goal to the ActionServer, and also registers callbacks.
Definition: action_client.h:182
actionlib::GoalManager
Definition: client_helpers.h:102
actionlib::ActionClient::SendGoalFunc
boost::function< void(const ActionGoalConstPtr)> SendGoalFunc
Definition: action_client.h:133
ros::CallbackQueueInterface
actionlib::ActionClient::cancel_pub_
ros::Publisher cancel_pub_
Definition: action_client.h:268
actionlib::ActionClient::FeedbackCallback
boost::function< void(GoalHandle, const FeedbackConstPtr &)> FeedbackCallback
Definition: action_client.h:132
ros::NodeHandle
ros::MessageEvent
ros::Subscriber


actionlib
Author(s): Eitan Marder-Eppstein, Vijay Pradeep, Mikael Arguedas
autogenerated on Fri May 19 2023 02:36:55