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
00043 namespace actionlib
00044 {
00045 template<class ActionSpec>
00046 ActionServer<ActionSpec>::ActionServer(
00047 ros::NodeHandle n,
00048 std::string name,
00049 bool auto_start)
00050 : ActionServerBase<ActionSpec>(
00051 boost::function<void(GoalHandle)>(), boost::function<void(GoalHandle)>(), auto_start),
00052 node_(n, name)
00053 {
00054
00055 if (this->started_) {
00056 ROS_WARN_NAMED("actionlib",
00057 "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.",
00058 node_.getNamespace().c_str());
00059 }
00060 }
00061
00062 template<class ActionSpec>
00063 ActionServer<ActionSpec>::ActionServer(ros::NodeHandle n, std::string name)
00064 : ActionServerBase<ActionSpec>(
00065 boost::function<void(GoalHandle)>(), boost::function<void(GoalHandle)>(), true),
00066 node_(n, name)
00067 {
00068
00069 if (this->started_) {
00070 ROS_WARN_NAMED("actionlib",
00071 "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.",
00072 node_.getNamespace().c_str());
00073 initialize();
00074 publishStatus();
00075 }
00076 }
00077
00078 template<class ActionSpec>
00079 ActionServer<ActionSpec>::ActionServer(ros::NodeHandle n, std::string name,
00080 boost::function<void(GoalHandle)> goal_cb,
00081 boost::function<void(GoalHandle)> cancel_cb,
00082 bool auto_start)
00083 : ActionServerBase<ActionSpec>(goal_cb, cancel_cb, auto_start),
00084 node_(n, name)
00085 {
00086
00087 if (this->started_) {
00088 ROS_WARN_NAMED("actionlib",
00089 "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.",
00090 node_.getNamespace().c_str());
00091 initialize();
00092 publishStatus();
00093 }
00094 }
00095
00096 template<class ActionSpec>
00097 ActionServer<ActionSpec>::ActionServer(ros::NodeHandle n, std::string name,
00098 boost::function<void(GoalHandle)> goal_cb,
00099 boost::function<void(GoalHandle)> cancel_cb)
00100 : ActionServerBase<ActionSpec>(goal_cb, cancel_cb, true),
00101 node_(n, name)
00102 {
00103
00104 if (this->started_) {
00105 ROS_WARN_NAMED("actionlib",
00106 "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.",
00107 node_.getNamespace().c_str());
00108 initialize();
00109 publishStatus();
00110 }
00111 }
00112
00113 template<class ActionSpec>
00114 ActionServer<ActionSpec>::ActionServer(ros::NodeHandle n, std::string name,
00115 boost::function<void(GoalHandle)> goal_cb,
00116 bool auto_start)
00117 : ActionServerBase<ActionSpec>(goal_cb, boost::function<void(GoalHandle)>(), auto_start),
00118 node_(n, name)
00119 {
00120
00121 if (this->started_) {
00122 ROS_WARN_NAMED("actionlib",
00123 "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.",
00124 node_.getNamespace().c_str());
00125 initialize();
00126 publishStatus();
00127 }
00128 }
00129
00130 template<class ActionSpec>
00131 ActionServer<ActionSpec>::~ActionServer()
00132 {
00133 }
00134
00135 template<class ActionSpec>
00136 void ActionServer<ActionSpec>::initialize()
00137 {
00138
00139
00140 int pub_queue_size;
00141 int sub_queue_size;
00142 node_.param("actionlib_server_pub_queue_size", pub_queue_size, 50);
00143 node_.param("actionlib_server_sub_queue_size", sub_queue_size, 50);
00144 if (pub_queue_size < 0) {pub_queue_size = 50;}
00145 if (sub_queue_size < 0) {sub_queue_size = 50;}
00146
00147 result_pub_ = node_.advertise<ActionResult>("result", static_cast<uint32_t>(pub_queue_size));
00148 feedback_pub_ =
00149 node_.advertise<ActionFeedback>("feedback", static_cast<uint32_t>(pub_queue_size));
00150 status_pub_ =
00151 node_.advertise<actionlib_msgs::GoalStatusArray>("status",
00152 static_cast<uint32_t>(pub_queue_size), true);
00153
00154
00155
00156 double status_frequency, status_list_timeout;
00157 if (!node_.getParam("status_frequency", status_frequency)) {
00158 std::string status_frequency_param_name;
00159 if (!node_.searchParam("actionlib_status_frequency", status_frequency_param_name)) {
00160 status_frequency = 5.0;
00161 } else {
00162 node_.param(status_frequency_param_name, status_frequency, 5.0);
00163 }
00164 } else {
00165 ROS_WARN_NAMED("actionlib",
00166 "You're using the deprecated status_frequency parameter, please switch to actionlib_status_frequency.");
00167 }
00168 node_.param("status_list_timeout", status_list_timeout, 5.0);
00169
00170 this->status_list_timeout_ = ros::Duration(status_list_timeout);
00171
00172 if (status_frequency > 0) {
00173 status_timer_ = node_.createTimer(ros::Duration(1.0 / status_frequency),
00174 boost::bind(&ActionServer::publishStatus, this, _1));
00175 }
00176
00177 goal_sub_ = node_.subscribe<ActionGoal>("goal", static_cast<uint32_t>(sub_queue_size),
00178 boost::bind(&ActionServerBase<ActionSpec>::goalCallback, this, _1));
00179
00180 cancel_sub_ =
00181 node_.subscribe<actionlib_msgs::GoalID>("cancel", static_cast<uint32_t>(sub_queue_size),
00182 boost::bind(&ActionServerBase<ActionSpec>::cancelCallback, this, _1));
00183 }
00184
00185 template<class ActionSpec>
00186 void ActionServer<ActionSpec>::publishResult(const actionlib_msgs::GoalStatus & status,
00187 const Result & result)
00188 {
00189 boost::recursive_mutex::scoped_lock lock(this->lock_);
00190
00191 boost::shared_ptr<ActionResult> ar(new ActionResult);
00192 ar->header.stamp = ros::Time::now();
00193 ar->status = status;
00194 ar->result = result;
00195 ROS_DEBUG_NAMED("actionlib", "Publishing result for goal with id: %s and stamp: %.2f",
00196 status.goal_id.id.c_str(), status.goal_id.stamp.toSec());
00197 result_pub_.publish(ar);
00198 publishStatus();
00199 }
00200
00201 template<class ActionSpec>
00202 void ActionServer<ActionSpec>::publishFeedback(const actionlib_msgs::GoalStatus & status,
00203 const Feedback & feedback)
00204 {
00205 boost::recursive_mutex::scoped_lock lock(this->lock_);
00206
00207 boost::shared_ptr<ActionFeedback> af(new ActionFeedback);
00208 af->header.stamp = ros::Time::now();
00209 af->status = status;
00210 af->feedback = feedback;
00211 ROS_DEBUG_NAMED("actionlib", "Publishing feedback for goal with id: %s and stamp: %.2f",
00212 status.goal_id.id.c_str(), status.goal_id.stamp.toSec());
00213 feedback_pub_.publish(af);
00214 }
00215
00216 template<class ActionSpec>
00217 void ActionServer<ActionSpec>::publishStatus(const ros::TimerEvent &)
00218 {
00219 boost::recursive_mutex::scoped_lock lock(this->lock_);
00220
00221 if (!this->started_) {
00222 return;
00223 }
00224
00225 publishStatus();
00226 }
00227
00228 template<class ActionSpec>
00229 void ActionServer<ActionSpec>::publishStatus()
00230 {
00231 boost::recursive_mutex::scoped_lock lock(this->lock_);
00232
00233 actionlib_msgs::GoalStatusArray status_array;
00234
00235 status_array.header.stamp = ros::Time::now();
00236
00237 status_array.status_list.resize(this->status_list_.size());
00238
00239 unsigned int i = 0;
00240 for (typename std::list<StatusTracker<ActionSpec> >::iterator it = this->status_list_.begin();
00241 it != this->status_list_.end(); )
00242 {
00243 status_array.status_list[i] = (*it).status_;
00244
00245
00246 if ((*it).handle_destruction_time_ != ros::Time() &&
00247 (*it).handle_destruction_time_ + this->status_list_timeout_ < ros::Time::now())
00248 {
00249 it = this->status_list_.erase(it);
00250 } else {
00251 ++it;
00252 }
00253 ++i;
00254 }
00255
00256 status_pub_.publish(status_array);
00257 }
00258
00259 }
00260 #endif // ACTIONLIB__SERVER__ACTION_SERVER_IMP_H_