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