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
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
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
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
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
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
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
00140
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
00156
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
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
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
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
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
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 }
00261 #endif // ACTIONLIB__SERVER__ACTION_SERVER_IMP_H_