action_server_imp.h
Go to the documentation of this file.
1 /*********************************************************************
2 *
3 * Software License Agreement (BSD License)
4 *
5 * Copyright (c) 2008, Willow Garage, Inc.
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of Willow Garage, Inc. nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 *
35 * Author: Eitan Marder-Eppstein
36 *********************************************************************/
37 #ifndef ACTIONLIB__SERVER__ACTION_SERVER_IMP_H_
38 #define ACTIONLIB__SERVER__ACTION_SERVER_IMP_H_
39 
40 #include <list>
41 #include <string>
42 #include <ros/ros.h>
43 
44 namespace actionlib
45 {
46 template<class ActionSpec>
49  std::string name,
50  bool auto_start)
51 : ActionServerBase<ActionSpec>(
52  boost::function<void(GoalHandle)>(), boost::function<void(GoalHandle)>(), auto_start),
53  node_(n, name)
54 {
55  // if we're to autostart... then we'll initialize things
56  if (this->started_) {
57  ROS_WARN_NAMED("actionlib",
58  "You've passed in true for auto_start for the C++ action server at [%s]. You should always pass in false to avoid race conditions.",
59  node_.getNamespace().c_str());
60  }
61 }
62 
63 template<class ActionSpec>
65 : ActionServerBase<ActionSpec>(
66  boost::function<void(GoalHandle)>(), boost::function<void(GoalHandle)>(), true),
67  node_(n, name)
68 {
69  // if we're to autostart... then we'll initialize things
70  if (this->started_) {
71  ROS_WARN_NAMED("actionlib",
72  "You've passed in true for auto_start for the C++ action server at [%s]. You should always pass in false to avoid race conditions.",
73  node_.getNamespace().c_str());
75  publishStatus();
76  }
77 }
78 
79 template<class ActionSpec>
81  boost::function<void(GoalHandle)> goal_cb,
82  boost::function<void(GoalHandle)> cancel_cb,
83  bool auto_start)
84 : ActionServerBase<ActionSpec>(goal_cb, cancel_cb, auto_start),
85  node_(n, name)
86 {
87  // if we're to autostart... then we'll initialize things
88  if (this->started_) {
89  ROS_WARN_NAMED("actionlib",
90  "You've passed in true for auto_start for the C++ action server at [%s]. You should always pass in false to avoid race conditions.",
91  node_.getNamespace().c_str());
93  publishStatus();
94  }
95 }
96 
97 template<class ActionSpec>
99  boost::function<void(GoalHandle)> goal_cb,
100  boost::function<void(GoalHandle)> cancel_cb)
101 : ActionServerBase<ActionSpec>(goal_cb, cancel_cb, true),
102  node_(n, name)
103 {
104  // if we're to autostart... then we'll initialize things
105  if (this->started_) {
106  ROS_WARN_NAMED("actionlib",
107  "You've passed in true for auto_start for the C++ action server at [%s]. You should always pass in false to avoid race conditions.",
108  node_.getNamespace().c_str());
110  publishStatus();
111  }
112 }
113 
114 template<class ActionSpec>
116  boost::function<void(GoalHandle)> goal_cb,
117  bool auto_start)
118 : ActionServerBase<ActionSpec>(goal_cb, boost::function<void(GoalHandle)>(), auto_start),
119  node_(n, name)
120 {
121  // if we're to autostart... then we'll initialize things
122  if (this->started_) {
123  ROS_WARN_NAMED("actionlib",
124  "You've passed in true for auto_start for the C++ action server at [%s]. You should always pass in false to avoid race conditions.",
125  node_.getNamespace().c_str());
127  publishStatus();
128  }
129 }
130 
131 template<class ActionSpec>
133 {
134 }
135 
136 template<class ActionSpec>
138 {
139  // read the queue size for each of the publish & subscribe components of the action
140  // server
141  int pub_queue_size;
142  int sub_queue_size;
143  node_.param("actionlib_server_pub_queue_size", pub_queue_size, 50);
144  node_.param("actionlib_server_sub_queue_size", sub_queue_size, 50);
145  if (pub_queue_size < 0) {pub_queue_size = 50;}
146  if (sub_queue_size < 0) {sub_queue_size = 50;}
147 
148  result_pub_ = node_.advertise<ActionResult>("result", static_cast<uint32_t>(pub_queue_size));
149  feedback_pub_ =
150  node_.advertise<ActionFeedback>("feedback", static_cast<uint32_t>(pub_queue_size));
151  status_pub_ =
152  node_.advertise<actionlib_msgs::GoalStatusArray>("status",
153  static_cast<uint32_t>(pub_queue_size), true);
154 
155  // read the frequency with which to publish status from the parameter server
156  // if not specified locally explicitly, use search param to find actionlib_status_frequency
157  double status_frequency, status_list_timeout;
158  if (!node_.getParam("status_frequency", status_frequency)) {
159  std::string status_frequency_param_name;
160  if (!node_.searchParam("actionlib_status_frequency", status_frequency_param_name)) {
161  status_frequency = 5.0;
162  } else {
163  node_.param(status_frequency_param_name, status_frequency, 5.0);
164  }
165  } else {
166  ROS_WARN_NAMED("actionlib",
167  "You're using the deprecated status_frequency parameter, please switch to actionlib_status_frequency.");
168  }
169  node_.param("status_list_timeout", status_list_timeout, 5.0);
170 
171  this->status_list_timeout_ = ros::Duration(status_list_timeout);
172 
173  if (status_frequency > 0) {
174  status_timer_ = node_.createTimer(ros::Duration(1.0 / status_frequency),
175  boost::bind(&ActionServer::publishStatus, this, _1));
176  }
177 
178  goal_sub_ = node_.subscribe<ActionGoal>("goal", static_cast<uint32_t>(sub_queue_size),
179  boost::bind(&ActionServerBase<ActionSpec>::goalCallback, this, _1));
180 
181  cancel_sub_ =
182  node_.subscribe<actionlib_msgs::GoalID>("cancel", static_cast<uint32_t>(sub_queue_size),
183  boost::bind(&ActionServerBase<ActionSpec>::cancelCallback, this, _1));
184 }
185 
186 template<class ActionSpec>
187 void ActionServer<ActionSpec>::publishResult(const actionlib_msgs::GoalStatus & status,
188  const Result & result)
189 {
190  boost::recursive_mutex::scoped_lock lock(this->lock_);
191  // we'll create a shared_ptr to pass to ROS to limit copying
192  boost::shared_ptr<ActionResult> ar(new ActionResult);
193  ar->header.stamp = ros::Time::now();
194  ar->status = status;
195  ar->result = result;
196  ROS_DEBUG_NAMED("actionlib", "Publishing result for goal with id: %s and stamp: %.2f",
197  status.goal_id.id.c_str(), status.goal_id.stamp.toSec());
198  result_pub_.publish(ar);
199  publishStatus();
200 }
201 
202 template<class ActionSpec>
203 void ActionServer<ActionSpec>::publishFeedback(const actionlib_msgs::GoalStatus & status,
204  const Feedback & feedback)
205 {
206  boost::recursive_mutex::scoped_lock lock(this->lock_);
207  // we'll create a shared_ptr to pass to ROS to limit copying
208  boost::shared_ptr<ActionFeedback> af(new ActionFeedback);
209  af->header.stamp = ros::Time::now();
210  af->status = status;
211  af->feedback = feedback;
212  ROS_DEBUG_NAMED("actionlib", "Publishing feedback for goal with id: %s and stamp: %.2f",
213  status.goal_id.id.c_str(), status.goal_id.stamp.toSec());
215 }
216 
217 template<class ActionSpec>
219 {
220  boost::recursive_mutex::scoped_lock lock(this->lock_);
221  // we won't publish status unless we've been started
222  if (!this->started_) {
223  return;
224  }
225 
226  publishStatus();
227 }
228 
229 template<class ActionSpec>
231 {
232  boost::recursive_mutex::scoped_lock lock(this->lock_);
233  // build a status array
234  actionlib_msgs::GoalStatusArray status_array;
235 
236  status_array.header.stamp = ros::Time::now();
237 
238  status_array.status_list.resize(this->status_list_.size());
239 
240  unsigned int i = 0;
241  for (typename std::list<StatusTracker<ActionSpec> >::iterator it = this->status_list_.begin();
242  it != this->status_list_.end(); )
243  {
244  status_array.status_list[i] = (*it).status_;
245 
246  // check if the item is due for deletion from the status list
247  if ((*it).handle_destruction_time_ != ros::Time() &&
248  (*it).handle_destruction_time_ + this->status_list_timeout_ < ros::Time::now())
249  {
250  it = this->status_list_.erase(it);
251  } else {
252  ++it;
253  }
254  ++i;
255  }
256 
257  status_pub_.publish(status_array);
258 }
259 
260 } // namespace actionlib
261 #endif // ACTIONLIB__SERVER__ACTION_SERVER_IMP_H_
ActionServer(ros::NodeHandle n, std::string name, boost::function< void(GoalHandle)> goal_cb, boost::function< void(GoalHandle)> cancel_cb, bool auto_start)
Constructor for an ActionServer.
virtual ~ActionServer()
Destructor for the ActionServer.
ros::Subscriber goal_sub_
void publish(const boost::shared_ptr< M > &message) const
#define ROS_WARN_NAMED(name,...)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ros::Publisher status_pub_
virtual void initialize()
Initialize all ROS connections and setup timers.
boost::recursive_mutex lock_
#define ROS_DEBUG_NAMED(name,...)
A class for storing the status of each goal the action server is currently working on...
ros::Publisher feedback_pub_
bool searchParam(const std::string &key, std::string &result) const
bool param(const std::string &param_name, T &param_val, const T &default_val) const
std::list< StatusTracker< ActionSpec > > status_list_
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
const std::string & getNamespace() const
The ActionServerBase implements the logic for an ActionServer.
virtual void publishResult(const actionlib_msgs::GoalStatus &status, const Result &result)
Publishes a result for a given goal.
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
Encapsulates a state machine for a given goal that the user can trigger transitions on...
virtual void publishStatus()
Explicitly publish status.
bool getParam(const std::string &key, std::string &s) const
ros::Subscriber cancel_sub_
static Time now()
ros::Publisher result_pub_
ros::NodeHandle node_
virtual void publishFeedback(const actionlib_msgs::GoalStatus &status, const Feedback &feedback)
Publishes feedback for a given goal.


actionlib
Author(s): Eitan Marder-Eppstein, Vijay Pradeep, Mikael Arguedas
autogenerated on Mon Aug 24 2020 03:40:47