37 #ifndef ACTIONLIB__SERVER__ACTION_SERVER_IMP_H_
38 #define ACTIONLIB__SERVER__ACTION_SERVER_IMP_H_
46 template<
class ActionSpec>
51 : ActionServerBase<ActionSpec>(
52 boost::function<void(GoalHandle)>(),
boost::function<void(GoalHandle)>(), auto_start),
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());
63 template<
class ActionSpec>
64 ActionServer<ActionSpec>::ActionServer(
ros::NodeHandle n, std::string name)
65 : ActionServerBase<ActionSpec>(
66 boost::function<void(GoalHandle)>(),
boost::function<void(GoalHandle)>(), true),
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());
74 ActionServer<ActionSpec>::initialize();
79 template<
class ActionSpec>
80 ActionServer<ActionSpec>::ActionServer(
ros::NodeHandle n, std::string name,
81 boost::function<
void(GoalHandle)> goal_cb,
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());
97 template<
class ActionSpec>
105 if (this->started_) {
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());
114 template<
class ActionSpec>
122 if (this->started_) {
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());
131 template<
class ActionSpec>
136 template<
class ActionSpec>
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;}
148 result_pub_ = node_.advertise<ActionResult>(
"result",
static_cast<uint32_t
>(pub_queue_size));
150 node_.advertise<ActionFeedback>(
"feedback",
static_cast<uint32_t
>(pub_queue_size));
152 node_.advertise<actionlib_msgs::GoalStatusArray>(
"status",
153 static_cast<uint32_t
>(pub_queue_size),
true);
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;
163 node_.param(status_frequency_param_name, status_frequency, 5.0);
167 "You're using the deprecated status_frequency parameter, please switch to actionlib_status_frequency.");
169 node_.param(
"status_list_timeout", status_list_timeout, 5.0);
171 this->status_list_timeout_ =
ros::Duration(status_list_timeout);
173 if (status_frequency > 0) {
174 status_timer_ = node_.createTimer(
ros::Duration(1.0 / status_frequency),
175 boost::bind(&ActionServer::publishStatus,
this, boost::placeholders::_1));
178 goal_sub_ = node_.subscribe<ActionGoal>(
"goal",
static_cast<uint32_t
>(sub_queue_size),
182 node_.subscribe<actionlib_msgs::GoalID>(
"cancel",
static_cast<uint32_t
>(sub_queue_size),
186 template<
class ActionSpec>
187 void ActionServer<ActionSpec>::publishResult(
const actionlib_msgs::GoalStatus & status,
188 const Result & result)
190 boost::recursive_mutex::scoped_lock lock(this->lock_);
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);
202 template<
class ActionSpec>
203 void ActionServer<ActionSpec>::publishFeedback(
const actionlib_msgs::GoalStatus & status,
204 const Feedback & feedback)
206 boost::recursive_mutex::scoped_lock lock(this->lock_);
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());
214 feedback_pub_.publish(af);
217 template<
class ActionSpec>
220 boost::recursive_mutex::scoped_lock lock(this->lock_);
222 if (!this->started_) {
229 template<
class ActionSpec>
230 void ActionServer<ActionSpec>::publishStatus()
232 boost::recursive_mutex::scoped_lock lock(this->lock_);
234 actionlib_msgs::GoalStatusArray status_array;
238 status_array.status_list.resize(this->status_list_.size());
242 it != this->status_list_.end(); )
244 status_array.status_list[i] = (*it).status_;
247 if ((*it).handle_destruction_time_ !=
ros::Time() &&
248 (*it).handle_destruction_time_ + this->status_list_timeout_ <
ros::Time::now())
250 it = this->status_list_.erase(it);
257 status_pub_.publish(status_array);
261 #endif // ACTIONLIB__SERVER__ACTION_SERVER_IMP_H_