controller_template.hpp
Go to the documentation of this file.
1 #ifndef __CONTROLLER_TEMPLATE__
2 #define __CONTROLLER_TEMPLATE__
3 
5 #include <ros/package.h>
6 #include <ros/ros.h>
7 #include <sensor_msgs/JointState.h>
8 #include <cmath>
10 
12 {
13 const double MAX_DT = 0.5;
14 
19 {
20  public:
22  virtual ~ControllerBase();
23 
32  virtual sensor_msgs::JointState updateControl(
33  const sensor_msgs::JointState &current_state,
34  const ros::Duration &dt) = 0;
35 
42  virtual bool isActive() const = 0;
43 
47  virtual void resetInternalState() = 0;
48 };
49 
54 template <class ActionClass, class ActionGoal, class ActionFeedback,
55  class ActionResult>
57 {
58  public:
59  ControllerTemplate(const std::string &action_name,
61  virtual ~ControllerTemplate();
62 
66  virtual sensor_msgs::JointState updateControl(
67  const sensor_msgs::JointState &current_state, const ros::Duration &dt);
68 
69  virtual bool isActive() const;
70 
71  virtual void resetInternalState();
72 
73  protected:
77  virtual sensor_msgs::JointState controlAlgorithm(
78  const sensor_msgs::JointState &current_state,
79  const ros::Duration &dt) = 0;
80 
87  virtual bool parseGoal(boost::shared_ptr<const ActionGoal> goal) = 0;
88 
92  virtual void resetController() = 0;
93 
102  virtual bool customDefaultBehavior() { return false; };
103 
113  sensor_msgs::JointState lastState(const sensor_msgs::JointState &current);
114 
116  ActionFeedback feedback_;
117  ActionResult result_;
118 
119  private:
124  void startActionlib();
125 
129  virtual bool goalCB();
130 
134  virtual void preemptCB();
135 
139  void resetFlags();
140 
141  std::string action_name_;
143  std::shared_ptr<BagManager> bag_manager_;
144  sensor_msgs::JointState last_state_;
145  bool has_state_, acquired_goal_;
146 };
147 
148 template <class ActionClass, class ActionGoal, class ActionFeedback,
149  class ActionResult>
151  ControllerTemplate(const std::string &action_name, ros::NodeHandle nh)
152  : action_name_(action_name), nh_(nh)
153 {
154  resetFlags();
155  startActionlib();
156 }
157 
158 template <class ActionClass, class ActionGoal, class ActionFeedback,
159  class ActionResult>
160 sensor_msgs::JointState ControllerTemplate<
161  ActionClass, ActionGoal, ActionFeedback,
162  ActionResult>::updateControl(const sensor_msgs::JointState &current_state,
163  const ros::Duration &dt)
164 {
165  if (!customDefaultBehavior())
166  {
167  if (!action_server_->isActive() || !acquired_goal_)
168  {
169  return lastState(current_state);
170  }
171  }
172 
173  ROS_DEBUG_THROTTLE(10, "Calling %s control algorithm", action_name_.c_str());
174  if (dt.toSec() > MAX_DT) // lost communication for too much time
175  {
176  ROS_ERROR_STREAM(action_name_ << " did not receive updates for more than "
177  << MAX_DT << " seconds, aborting");
178  action_server_->setAborted(result_);
179  return lastState(current_state);
180  }
181 
182  sensor_msgs::JointState ret = controlAlgorithm(current_state, dt);
183  if (action_server_->isActive())
184  {
185  if (!customDefaultBehavior())
186  {
187  action_server_->publishFeedback(feedback_);
188  }
189 
190  if (bag_manager_)
191  {
192  bag_manager_->write(feedback_); // build log file
193  }
194  }
195 
196  if (customDefaultBehavior())
197  {
198  action_server_->publishFeedback(feedback_);
199  }
200 
201  if (!customDefaultBehavior() && !action_server_->isActive())
202  {
204  return lastState(current_state);
205  }
206 
207  // verify sanity of values
208  for (unsigned int i = 0; i < ret.name.size(); i++)
209  {
210  if (!std::isfinite(ret.position[i]) || !std::isfinite(ret.velocity[i]))
211  {
212  ROS_ERROR("Invalid joint states in %s", action_name_.c_str());
213  return lastState(current_state);
214  }
215  }
216 
217  return ret;
218 }
219 
220 template <class ActionClass, class ActionGoal, class ActionFeedback,
221  class ActionResult>
222 bool ControllerTemplate<ActionClass, ActionGoal, ActionFeedback,
223  ActionResult>::isActive() const
224 {
225  if (!action_server_->isActive())
226  {
227  return false;
228  }
229 
230  return true;
231 }
232 
233 template <class ActionClass, class ActionGoal, class ActionFeedback,
234  class ActionResult>
235 void ControllerTemplate<ActionClass, ActionGoal, ActionFeedback,
236  ActionResult>::resetInternalState()
237 {
238  if (action_server_->isActive())
239  {
240  action_server_->setAborted();
241  }
242 
243  resetFlags();
244  resetController();
245 }
246 
247 template <class ActionClass, class ActionGoal, class ActionFeedback,
248  class ActionResult>
249 void ControllerTemplate<ActionClass, ActionGoal, ActionFeedback,
250  ActionResult>::resetFlags()
251 {
252  has_state_ = false;
253  acquired_goal_ = false;
254 
255  if (bag_manager_)
256  {
257  bag_manager_.reset();
258  }
259 }
260 
261 template <class ActionClass, class ActionGoal, class ActionFeedback,
262  class ActionResult>
263 sensor_msgs::JointState ControllerTemplate<
264  ActionClass, ActionGoal, ActionFeedback,
265  ActionResult>::lastState(const sensor_msgs::JointState &current)
266 {
267  if (current.position.size() == 0) // Invalid state
268  {
269  ROS_WARN("lastState got invalid state");
270  return last_state_;
271  }
272 
273  if (!has_state_)
274  {
275  last_state_ = current;
276  for (unsigned long i = 0; i < last_state_.velocity.size(); i++)
277  {
278  last_state_.velocity[i] = 0.0;
279  }
280 
281  has_state_ = true;
282  }
283 
284  return last_state_;
285 }
286 
287 template <class ActionClass, class ActionGoal, class ActionFeedback,
288  class ActionResult>
289 bool ControllerTemplate<ActionClass, ActionGoal, ActionFeedback,
290  ActionResult>::goalCB()
291 {
292  boost::shared_ptr<const ActionGoal> goal = action_server_->acceptNewGoal();
293  acquired_goal_ = false;
294 
295  if (!parseGoal(goal))
296  {
297  action_server_->setAborted(result_);
298  return false;
299  }
300 
301  acquired_goal_ = true;
302 
303  if (nh_.hasParam("record_bag")) // controller is supposed to produce a log
304  {
305  std::string package_name, path, topic;
306 
307  topic = nh_.resolveName(action_name_) + std::string("/feedback");
308 
309  if (nh_.getParam("record_bag/package", package_name))
310  {
311  path = ros::package::getPath(package_name) + "/bags/";
312  bag_manager_ = std::make_shared<BagManager>(path, topic);
313  }
314  else if (nh_.getParam("record_bag/path", path))
315  {
316  bag_manager_ = std::make_shared<BagManager>(path, topic);
317  }
318  else
319  {
320  ROS_WARN(
321  "No record_bag/package or record_bag/path parameters detected! No "
322  "bag will be recorded");
323  }
324  }
325 
326  ROS_INFO("New goal received in %s", action_name_.c_str());
327  return true;
328 }
329 
330 template <class ActionClass, class ActionGoal, class ActionFeedback,
331  class ActionResult>
332 void ControllerTemplate<ActionClass, ActionGoal, ActionFeedback,
333  ActionResult>::preemptCB()
334 {
335  action_server_->setPreempted(result_);
336  ROS_WARN("%s preempted!", action_name_.c_str());
338 }
339 
340 template <class ActionClass, class ActionGoal, class ActionFeedback,
341  class ActionResult>
342 ControllerTemplate<ActionClass, ActionGoal, ActionFeedback,
343  ActionResult>::~ControllerTemplate()
344 {
345  action_server_->shutdown();
346 }
347 
348 template <class ActionClass, class ActionGoal, class ActionFeedback,
349  class ActionResult>
350 void ControllerTemplate<ActionClass, ActionGoal, ActionFeedback,
351  ActionResult>::startActionlib()
352 {
353  // Initialize actionlib server
357  false));
358 
359  // Register callbacks
360  action_server_->registerGoalCallback(
361  boost::bind(&ControllerTemplate::goalCB, this));
362  action_server_->registerPreemptCallback(
363  boost::bind(&ControllerTemplate::preemptCB, this));
364 
365  action_server_->start();
366 
367  ROS_INFO("%s initialized successfully!", action_name_.c_str());
368 }
369 
371 } // namespace generic_control_toolbox
372 
373 #endif
sensor_msgs::JointState lastState(const sensor_msgs::JointState &current)
ControllerTemplate(const std::string &action_name, ros::NodeHandle nh=ros::NodeHandle("~"))
ControllerBase(ros::NodeHandle nh=ros::NodeHandle("~"))
#define ROS_ERROR(...)
#define ROS_WARN(...)
#define ROS_INFO(...)
bool getParam(const std::string &key, std::string &s) const
boost::shared_ptr< ControllerBase > BasePtr
std::string resolveName(const std::string &name, bool remap=true) const
bool hasParam(const std::string &key) const
ROSLIB_DECL std::string getPath(const std::string &package_name)
boost::shared_ptr< actionlib::SimpleActionServer< ActionClass > > action_server_
virtual sensor_msgs::JointState updateControl(const sensor_msgs::JointState &current_state, const ros::Duration &dt)=0
virtual bool parseGoal(boost::shared_ptr< const ActionGoal > goal)=0
#define ROS_ERROR_STREAM(args)
virtual sensor_msgs::JointState controlAlgorithm(const sensor_msgs::JointState &current_state, const ros::Duration &dt)=0
#define ROS_DEBUG_THROTTLE(period,...)
virtual sensor_msgs::JointState updateControl(const sensor_msgs::JointState &current_state, const ros::Duration &dt)


generic_control_toolbox
Author(s): diogo
autogenerated on Mon Feb 28 2022 22:24:37