action_server_imp.h
Go to the documentation of this file.
00001 /*********************************************************************
00002 *
00003 * Software License Agreement (BSD License)
00004 *
00005 *  Copyright (c) 2008, Willow Garage, Inc.
00006 *  All rights reserved.
00007 *
00008 *  Redistribution and use in source and binary forms, with or without
00009 *  modification, are permitted provided that the following conditions
00010 *  are met:
00011 *
00012 *   * Redistributions of source code must retain the above copyright
00013 *     notice, this list of conditions and the following disclaimer.
00014 *   * Redistributions in binary form must reproduce the above
00015 *     copyright notice, this list of conditions and the following
00016 *     disclaimer in the documentation and/or other materials provided
00017 *     with the distribution.
00018 *   * Neither the name of Willow Garage, Inc. nor the names of its
00019 *     contributors may be used to endorse or promote products derived
00020 *     from this software without specific prior written permission.
00021 *
00022 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 *  POSSIBILITY OF SUCH DAMAGE.
00034 *
00035 * Author: Eitan Marder-Eppstein
00036 *********************************************************************/
00037 #ifndef ACTIONLIB__SERVER__ACTION_SERVER_IMP_H_
00038 #define ACTIONLIB__SERVER__ACTION_SERVER_IMP_H_
00039 
00040 #include <list>
00041 #include <string>
00042 #include <ros/ros.h>
00043 
00044 namespace actionlib
00045 {
00046 template<class ActionSpec>
00047 ActionServer<ActionSpec>::ActionServer(
00048   ros::NodeHandle n,
00049   std::string name,
00050   bool auto_start)
00051 : ActionServerBase<ActionSpec>(
00052     boost::function<void(GoalHandle)>(), boost::function<void(GoalHandle)>(), auto_start),
00053   node_(n, name)
00054 {
00055   // if we're to autostart... then we'll initialize things
00056   if (this->started_) {
00057     ROS_WARN_NAMED("actionlib",
00058       "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.",
00059       node_.getNamespace().c_str());
00060   }
00061 }
00062 
00063 template<class ActionSpec>
00064 ActionServer<ActionSpec>::ActionServer(ros::NodeHandle n, std::string name)
00065 : ActionServerBase<ActionSpec>(
00066     boost::function<void(GoalHandle)>(), boost::function<void(GoalHandle)>(), true),
00067   node_(n, name)
00068 {
00069   // if we're to autostart... then we'll initialize things
00070   if (this->started_) {
00071     ROS_WARN_NAMED("actionlib",
00072       "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.",
00073       node_.getNamespace().c_str());
00074     ActionServer<ActionSpec>::initialize();
00075     publishStatus();
00076   }
00077 }
00078 
00079 template<class ActionSpec>
00080 ActionServer<ActionSpec>::ActionServer(ros::NodeHandle n, std::string name,
00081   boost::function<void(GoalHandle)> goal_cb,
00082   boost::function<void(GoalHandle)> cancel_cb,
00083   bool auto_start)
00084 : ActionServerBase<ActionSpec>(goal_cb, cancel_cb, auto_start),
00085   node_(n, name)
00086 {
00087   // if we're to autostart... then we'll initialize things
00088   if (this->started_) {
00089     ROS_WARN_NAMED("actionlib",
00090       "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.",
00091       node_.getNamespace().c_str());
00092     ActionServer<ActionSpec>::initialize();
00093     publishStatus();
00094   }
00095 }
00096 
00097 template<class ActionSpec>
00098 ActionServer<ActionSpec>::ActionServer(ros::NodeHandle n, std::string name,
00099   boost::function<void(GoalHandle)> goal_cb,
00100   boost::function<void(GoalHandle)> cancel_cb)
00101 : ActionServerBase<ActionSpec>(goal_cb, cancel_cb, true),
00102   node_(n, name)
00103 {
00104   // if we're to autostart... then we'll initialize things
00105   if (this->started_) {
00106     ROS_WARN_NAMED("actionlib",
00107       "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.",
00108       node_.getNamespace().c_str());
00109     ActionServer<ActionSpec>::initialize();
00110     publishStatus();
00111   }
00112 }
00113 
00114 template<class ActionSpec>
00115 ActionServer<ActionSpec>::ActionServer(ros::NodeHandle n, std::string name,
00116   boost::function<void(GoalHandle)> goal_cb,
00117   bool auto_start)
00118 : ActionServerBase<ActionSpec>(goal_cb, boost::function<void(GoalHandle)>(), auto_start),
00119   node_(n, name)
00120 {
00121   // if we're to autostart... then we'll initialize things
00122   if (this->started_) {
00123     ROS_WARN_NAMED("actionlib",
00124       "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.",
00125       node_.getNamespace().c_str());
00126     ActionServer<ActionSpec>::initialize();
00127     publishStatus();
00128   }
00129 }
00130 
00131 template<class ActionSpec>
00132 ActionServer<ActionSpec>::~ActionServer()
00133 {
00134 }
00135 
00136 template<class ActionSpec>
00137 void ActionServer<ActionSpec>::initialize()
00138 {
00139   // read the queue size for each of the publish & subscribe components of the action
00140   // server
00141   int pub_queue_size;
00142   int sub_queue_size;
00143   node_.param("actionlib_server_pub_queue_size", pub_queue_size, 50);
00144   node_.param("actionlib_server_sub_queue_size", sub_queue_size, 50);
00145   if (pub_queue_size < 0) {pub_queue_size = 50;}
00146   if (sub_queue_size < 0) {sub_queue_size = 50;}
00147 
00148   result_pub_ = node_.advertise<ActionResult>("result", static_cast<uint32_t>(pub_queue_size));
00149   feedback_pub_ =
00150     node_.advertise<ActionFeedback>("feedback", static_cast<uint32_t>(pub_queue_size));
00151   status_pub_ =
00152     node_.advertise<actionlib_msgs::GoalStatusArray>("status",
00153       static_cast<uint32_t>(pub_queue_size), true);
00154 
00155   // read the frequency with which to publish status from the parameter server
00156   // if not specified locally explicitly, use search param to find actionlib_status_frequency
00157   double status_frequency, status_list_timeout;
00158   if (!node_.getParam("status_frequency", status_frequency)) {
00159     std::string status_frequency_param_name;
00160     if (!node_.searchParam("actionlib_status_frequency", status_frequency_param_name)) {
00161       status_frequency = 5.0;
00162     } else {
00163       node_.param(status_frequency_param_name, status_frequency, 5.0);
00164     }
00165   } else {
00166     ROS_WARN_NAMED("actionlib",
00167       "You're using the deprecated status_frequency parameter, please switch to actionlib_status_frequency.");
00168   }
00169   node_.param("status_list_timeout", status_list_timeout, 5.0);
00170 
00171   this->status_list_timeout_ = ros::Duration(status_list_timeout);
00172 
00173   if (status_frequency > 0) {
00174     status_timer_ = node_.createTimer(ros::Duration(1.0 / status_frequency),
00175         boost::bind(&ActionServer::publishStatus, this, _1));
00176   }
00177 
00178   goal_sub_ = node_.subscribe<ActionGoal>("goal", static_cast<uint32_t>(sub_queue_size),
00179       boost::bind(&ActionServerBase<ActionSpec>::goalCallback, this, _1));
00180 
00181   cancel_sub_ =
00182     node_.subscribe<actionlib_msgs::GoalID>("cancel", static_cast<uint32_t>(sub_queue_size),
00183       boost::bind(&ActionServerBase<ActionSpec>::cancelCallback, this, _1));
00184 }
00185 
00186 template<class ActionSpec>
00187 void ActionServer<ActionSpec>::publishResult(const actionlib_msgs::GoalStatus & status,
00188   const Result & result)
00189 {
00190   boost::recursive_mutex::scoped_lock lock(this->lock_);
00191   // we'll create a shared_ptr to pass to ROS to limit copying
00192   boost::shared_ptr<ActionResult> ar(new ActionResult);
00193   ar->header.stamp = ros::Time::now();
00194   ar->status = status;
00195   ar->result = result;
00196   ROS_DEBUG_NAMED("actionlib", "Publishing result for goal with id: %s and stamp: %.2f",
00197     status.goal_id.id.c_str(), status.goal_id.stamp.toSec());
00198   result_pub_.publish(ar);
00199   publishStatus();
00200 }
00201 
00202 template<class ActionSpec>
00203 void ActionServer<ActionSpec>::publishFeedback(const actionlib_msgs::GoalStatus & status,
00204   const Feedback & feedback)
00205 {
00206   boost::recursive_mutex::scoped_lock lock(this->lock_);
00207   // we'll create a shared_ptr to pass to ROS to limit copying
00208   boost::shared_ptr<ActionFeedback> af(new ActionFeedback);
00209   af->header.stamp = ros::Time::now();
00210   af->status = status;
00211   af->feedback = feedback;
00212   ROS_DEBUG_NAMED("actionlib", "Publishing feedback for goal with id: %s and stamp: %.2f",
00213     status.goal_id.id.c_str(), status.goal_id.stamp.toSec());
00214   feedback_pub_.publish(af);
00215 }
00216 
00217 template<class ActionSpec>
00218 void ActionServer<ActionSpec>::publishStatus(const ros::TimerEvent &)
00219 {
00220   boost::recursive_mutex::scoped_lock lock(this->lock_);
00221   // we won't publish status unless we've been started
00222   if (!this->started_) {
00223     return;
00224   }
00225 
00226   publishStatus();
00227 }
00228 
00229 template<class ActionSpec>
00230 void ActionServer<ActionSpec>::publishStatus()
00231 {
00232   boost::recursive_mutex::scoped_lock lock(this->lock_);
00233   // build a status array
00234   actionlib_msgs::GoalStatusArray status_array;
00235 
00236   status_array.header.stamp = ros::Time::now();
00237 
00238   status_array.status_list.resize(this->status_list_.size());
00239 
00240   unsigned int i = 0;
00241   for (typename std::list<StatusTracker<ActionSpec> >::iterator it = this->status_list_.begin();
00242     it != this->status_list_.end(); )
00243   {
00244     status_array.status_list[i] = (*it).status_;
00245 
00246     // check if the item is due for deletion from the status list
00247     if ((*it).handle_destruction_time_ != ros::Time() &&
00248       (*it).handle_destruction_time_ + this->status_list_timeout_ < ros::Time::now())
00249     {
00250       it = this->status_list_.erase(it);
00251     } else {
00252       ++it;
00253     }
00254     ++i;
00255   }
00256 
00257   status_pub_.publish(status_array);
00258 }
00259 
00260 }  // namespace actionlib
00261 #endif  // ACTIONLIB__SERVER__ACTION_SERVER_IMP_H_


actionlib
Author(s): Eitan Marder-Eppstein, Vijay Pradeep, Mikael Arguedas
autogenerated on Sat Feb 16 2019 03:21:28