robot_activity.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  *
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2018, University of Luxembourg
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of University of Luxembourg nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *
35  * Author: Maciej Zurad
36  *********************************************************************/
38 
39 #include <string>
40 #include <vector>
41 
42 #define PRINT_FUNC_CALL(state) \
43  ROS_DEBUG_STREAM(#state << "() method called")
44 
45 namespace robot_activity
46 {
47 
48 RobotActivity::RobotActivity(int argc, char* argv[],
49  const std::string& name_space,
50  const std::string& name)
51  : node_namespace_(name_space),
52  node_name_(name),
53  state_request_callback_queue_()
54 {
55  if (ros::isInitialized())
56  {
58  return;
59  }
60 
61  if (node_name_.empty())
62  {
63  ros::init(argc, argv, "robot_activity", ros::init_options::AnonymousName);
65  }
66  else
67  ros::init(argc, argv, name);
68 }
69 
71 {
72  ROS_DEBUG_STREAM("RobotActivity dtor [" << getNamespace() << "]");
73 }
74 
76 {
79 
80  ros::param::param<bool>("~wait_for_supervisor", wait_for_supervisor_, true);
81  ROS_INFO_STREAM("wait_for_supervisor = "
82  << std::boolalpha << wait_for_supervisor_);
83 
84  process_state_pub_ = node_handle_private_->advertise<robot_activity_msgs::State>("/heartbeat", 0, true);
85  process_error_pub_ = node_handle_private_->advertise<robot_activity_msgs::Error>("/error", 0, true);
86 
87  if (wait_for_supervisor_)
88  {
89  ros::Rate poll_rate(100);
90  while (process_state_pub_.getNumSubscribers() == 0 && ros::ok())
91  poll_rate.sleep();
92  }
93 
94  notifyState();
95 
96  ros::param::param<bool>("~autostart_after_reconfigure", autostart_after_reconfigure_, false);
97  ROS_INFO_STREAM("autostart_after_reconfigure = "
98  << std::boolalpha << autostart_after_reconfigure_);
99 
100  terminate_server_ = registerStateChangeRequest("robot_activity/terminate", {State::TERMINATED}); // NOLINT
101  reconfigure_server_ = registerStateChangeRequest("robot_activity/reconfigure",
102  {
104  autostart_after_reconfigure_ ? State::RUNNING : State::STOPPED
105  }); // NOLINT
106 
107  restart_server_ = registerStateChangeRequest("robot_activity/restart", {State::STOPPED, State::RUNNING}); // NOLINT
108  start_server_ = registerStateChangeRequest("robot_activity/start", {State::RUNNING}); // NOLINT
109  stop_server_ = registerStateChangeRequest("robot_activity/stop", {State::STOPPED}); // NOLINT
110  pause_server_ = registerStateChangeRequest("robot_activity/pause", {State::PAUSED}); // NOLINT
111 
112  float heartbeat_rate;
113  ros::param::param<float>("~heartbeat_rate", heartbeat_rate, 1.0f);
114  ROS_INFO("heartbeat_rate = %.3f [Hz]", heartbeat_rate);
115 
116  IsolatedAsyncTimer::LambdaCallback heartbeat_callback = [this]()
117  {
118  notifyState();
119  };
120  heartbeat_timer_ = std::make_shared<IsolatedAsyncTimer>(
122  heartbeat_callback,
123  heartbeat_rate,
124  false);
125 
126  ros::param::param<bool>("~autostart", autostart_, false);
127 
128  state_request_spinner_ = std::make_shared<ros::AsyncSpinner>(1, &state_request_callback_queue_);
129  state_request_spinner_->start();
130 
131  autostart_ = autostart_ || autostart;
132  ROS_INFO_STREAM("autostart = " << std::boolalpha << autostart_);
133 
134  if (autostart_)
136  else
138 
139  return *this;
140 }
141 
142 void RobotActivity::run(uint8_t threads)
143 {
144  runAsync(threads);
146 }
147 
148 void RobotActivity::runAsync(uint8_t threads)
149 {
150  global_callback_queue_spinner_ = std::make_shared<ros::AsyncSpinner>(threads);
152 }
153 
155 {
156  return current_state_;
157 }
158 
159 void RobotActivity::notifyError(uint8_t error_type,
160  const std::string& function,
161  const std::string& description)
162 {
163  ROS_DEBUG_STREAM("Publishing error msg with code: "
164  << error_type << " function: " << function
165  << " description: " << description);
166  robot_activity_msgs::Error error_msg;
167  error_msg.header.stamp = ros::Time::now();
168  error_msg.node_name = getNamespace();
169  error_msg.error_type = error_type;
170  error_msg.function = function;
171  error_msg.description = description;
172  process_error_pub_.publish(error_msg);
173 }
174 
175 std::shared_ptr<IsolatedAsyncTimer> RobotActivity::registerIsolatedTimer(
176  const IsolatedAsyncTimer::LambdaCallback& callback,
177  const float& frequency,
178  bool stoppable,
179  bool autostart,
180  bool oneshot)
181 {
182  auto isolated_async_timer = std::make_shared<IsolatedAsyncTimer>(
184  callback,
185  frequency,
186  stoppable,
187  autostart,
188  oneshot);
189  process_timers_.push_back(isolated_async_timer);
190  return isolated_async_timer;
191 }
192 
193 std::string RobotActivity::getNamespace() const
194 {
196  return node_handle_private_->getNamespace();
197  else
198  return std::string();
199 }
200 
201 
203 {
204  PRINT_FUNC_CALL("create");
205  onCreate();
206  return true;
207 }
208 
210 {
211  PRINT_FUNC_CALL("terminate");
212  onTerminate();
213  return true;
214  // ros::Rate(2).sleep();
215  // ros::shutdown();
216 }
217 
219 {
220  PRINT_FUNC_CALL("configure");
221  return onConfigure();
222 }
223 
225 {
226  PRINT_FUNC_CALL("unconfigure");
227  return onUnconfigure();
228 }
229 
231 {
232  PRINT_FUNC_CALL("start");
233  for (const auto & timer : process_timers_)
234  {
235  ROS_DEBUG("Starting timer");
236  timer->start();
237  }
238  return onStart();
239 }
240 
242 {
243  PRINT_FUNC_CALL("stop");
244  for (const auto & timer : process_timers_)
245  {
246  ROS_DEBUG("Stopping timer");
247  timer->stop();
248  }
249  return onStop();
250 }
251 
253 {
254  PRINT_FUNC_CALL("resume");
255  for (const auto & timer : process_timers_)
256  {
257  ROS_DEBUG("Resuming timer");
258  timer->resume();
259  }
260  return onResume();
261 }
262 
264 {
265  PRINT_FUNC_CALL("pause");
266  for (const auto & timer : process_timers_)
267  {
268  ROS_DEBUG("Pausing timer");
269  timer->pause();
270  }
271  return onPause();
272 }
273 
275  const std::string& service_name,
276  const std::vector<State>& states)
277 {
279  "Registering state transition request for state " << service_name);
280  using std_srvs::Empty;
281  EmptyServiceCallback callback = [ = ](Empty::Request & req, Empty::Response & res)
282  {
283  bool success = true;
284  for (const auto & s : states)
285  {
286  success = success && transitionToState(s);
287  }
288  return success;
289  };
290 
291  auto options = ros::AdvertiseServiceOptions::create<Empty>(
292  service_name,
293  callback,
296 
297  return node_handle_private_->advertiseService(options);
298 }
299 
301 {
302  ROS_DEBUG("Heartbeat sent!");
303  robot_activity_msgs::State state_msg;
304  state_msg.header.stamp = ros::Time::now();
305  state_msg.node_name = getNamespace();
306  state_msg.state = static_cast<uint8_t>(current_state_);
307  process_state_pub_.publish(state_msg);
308 }
309 
311 {
312  const State& starting_state = current_state_;
313  if (starting_state == goal_state)
314  {
315  ROS_WARN_STREAM("Node is already at state " << goal_state);
316  return false;
317  }
318 
319  while (current_state_ != goal_state)
320  {
321  auto from_state = static_cast<uint8_t>(current_state_);
322  auto to_state = static_cast<uint8_t>(goal_state);
323  State next_state = STATE_TRANSITIONS_PATHS[from_state][to_state];
324  if (next_state == State::INVALID)
325  {
326  ROS_WARN_STREAM("There is no transition path from [" << starting_state
327  << "] to [" << goal_state << "]");
328  return false;
329  }
330  bool transition_result = changeState(next_state);
331  if (!transition_result)
332  {
333  ROS_WARN_STREAM("Transition from [" << starting_state
334  << "] to [" << goal_state << "] has failed during ["
335  << current_state_ << "]");
336  return false;
337  }
338  }
339  return true;
340 }
341 
342 bool RobotActivity::changeState(const State& new_state)
343 {
344  uint8_t from_state = static_cast<uint8_t>(current_state_);
345  uint8_t to_state = static_cast<uint8_t>(new_state);
346  MemberLambdaCallback callback = STATE_TRANSITIONS[from_state][to_state];
347  if (callback == nullptr)
348  {
350  "Tried changing state from [" << current_state_
351  << "] to [" << new_state << "]. Transition does NOT exist!");
352  return false;
353  }
354  bool transition_result = boost::bind(callback, this)();
355  if (transition_result)
356  {
358  "Chaning state from [" << current_state_ << "] to ["
359  << new_state << "] succeeded!");
360  current_state_ = new_state;
361  }
362  else
363  {
365  "Changing state from [" << current_state_ << "] to ["
366  << new_state << "] failed!");
367  }
368  notifyState();
369  return transition_result;
370 }
371 
372 std::ostream& operator<<(std::ostream& os, State state)
373 {
374  switch (state)
375  {
376  case State::INVALID :
377  os << "INVALID";
378  break;
379  case State::LAUNCHING :
380  os << "LAUNCHING";
381  break;
382  case State::UNCONFIGURED :
383  os << "UNCONFIGURED";
384  break;
385  case State::STOPPED :
386  os << "STOPPED";
387  break;
388  case State::PAUSED :
389  os << "PAUSED";
390  break;
391  case State::RUNNING :
392  os << "RUNNING";
393  break;
394  case State::TERMINATED :
395  os << "TERMINATED";
396  break;
397  default :
398  os.setstate(std::ios_base::failbit);
399  }
400  return os;
401 }
402 
404 {
405  /* Valid transitions for State::INVALID */
406  {nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, nullptr},
407  /* Valid transitions for State::LAUNCHING */
408  {nullptr, nullptr, &RobotActivity::create, nullptr, nullptr, nullptr, nullptr},
409  /* Valid transitions for State::UNCONFIGURED */
410  {nullptr, nullptr, nullptr, &RobotActivity::configure, nullptr, nullptr, &RobotActivity::terminate},
411  /* Valid transitions for State::STOPPED */
412  {nullptr, nullptr, &RobotActivity::unconfigure, nullptr, &RobotActivity::start, nullptr, nullptr},
413  /* Valid transitions for State::PAUSED */
414  {nullptr, nullptr, nullptr, &RobotActivity::stop, nullptr, &RobotActivity::resume, nullptr},
415  /* Valid transitions for State::RUNNING */
416  {nullptr, nullptr, nullptr, nullptr, &RobotActivity::pause, nullptr, nullptr},
417  /* Valid transitions for State::TERMINATED */
418  {nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, nullptr}
419 };
420 
421 
423 {
424  {
425  /* State::INVALID to other states */
428  State::INVALID
429  }, // NOLINT
430  {
431  /* State::LAUNCHING to other states */
434  State::UNCONFIGURED
435  }, // NOLINT
436  {
437  /* State::UNCONFIGURED to other states */
441  }, // NOLINT
442  {
443  /* State::STOPPED to other states */
446  State::UNCONFIGURED
447  }, // NOLINT
448  {
449  /* State::PAUSED to other states */
452  State::STOPPED
453  }, // NOLINT
454  {
455  /* State::RUNNING to other states */
458  State::PAUSED
459  }, // NOLINT
460  {
461  /* State::TERMINATED to other states */
464  State::INVALID
465  }
466 };
467 
468 } // namespace robot_activity
void notifyError(uint8_t error_type, const std::string &function, const std::string &description)
Sends an error message to a global error topic "/error".
static const StateTransitions STATE_TRANSITIONS
2D array of direct state transitions with values being the corresponding functions to be called durin...
bool create()
Called automatically, when transition from LAUNCHING to UNCONFIGURED. RobotActivity calls this functi...
ros::ServiceServer stop_server_
ServiceServer for serving stop service.
boost::shared_ptr< void const > VoidConstPtr
static const StateTransitionPaths STATE_TRANSITIONS_PATHS
2D array of paths between states.
void publish(const boost::shared_ptr< M > &message) const
bool start()
Called automatically, when transition from STOPPED to PAUSED. RobotActivity calls this function direc...
virtual ~RobotActivity()
Virtual destructor.
virtual bool onPause()=0
Function to be defined by the user. Called at the end of transition from RUNNING to PAUSED...
bool wait_for_supervisor_
Whether to wait for the supervisor during the init function. Waiting means that there has to be at le...
bool(RobotActivity::* MemberLambdaCallback)()
std::vector< std::shared_ptr< robot_activity::IsolatedAsyncTimer > > process_timers_
Vector of shared pointers to isolated timers created by register isolated timer.
std::function< void(void)> LambdaCallback
ROSCPP_DECL bool isInitialized()
XmlRpcServer s
std::shared_ptr< ros::AsyncSpinner > state_request_spinner_
Async spinner for serving state change requests.
ros::NodeHandlePtr node_handle_private_
#define PRINT_FUNC_CALL(state)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ros::ServiceServer registerStateChangeRequest(const std::string &service_name, const std::vector< State > &states)
Registers a ROS service server listening for state change requests.
State StateTransitionPaths[static_cast< uint8_t >(State::Count)][static_cast< uint8_t >(State::Count)]
bool autostart_
Whether to automatically start (transition to RUNNING) at the end of init function if not the node wi...
bool changeState(const State &new_state)
Changes state from current to new. Direct transition must exist. The appropriate function will be cal...
ROSCPP_DECL const std::string & getName()
std::shared_ptr< ros::AsyncSpinner > global_callback_queue_spinner_
Shared pointer to async spinner that serves the global callback queue.
bool terminate()
Called automatically, when transition from UNCONFIGURED to TERMINATED. RobotActivity calls this funct...
boost::shared_ptr< NodeHandle > NodeHandlePtr
bool autostart_after_reconfigure_
Whether to autostart after reconfigure service is called.
bool stop()
Called automatically, when transition from PAUSED to STOPPED. RobotActivity calls this function direc...
boost::function< bool(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) > EmptyServiceCallback
ros::Publisher process_error_pub_
ROS Publisher of error messages.
ros::Publisher process_state_pub_
ROS Publisher of heartbeat messages and state changes messages.
void notifyState() const
Sends a heartbeat message with the current state.
Class for adding node lifecycle to ROS processes.
State current_state_
Current state of the RobotActivity. Initially, LAUNCHING state.
bool unconfigure()
Called automatically, when transition from STOPPED to UNCONFIGURED. RobotActivity calls this function...
#define ROS_INFO(...)
std::shared_ptr< robot_activity::IsolatedAsyncTimer > heartbeat_timer_
Shared pointer to the isolated timer that sends heartbeat messages.
virtual void onCreate()=0
Function to be defined by the user. Called at the end of transition from LAUNCHING to UNCONFIGURED...
State getState()
Returns the current state.
virtual bool onConfigure()=0
Function to be defined by the user. Called at the end of transition from UNCONFIGURED to STOPPED...
ROSCPP_DECL bool ok()
#define ROS_FATAL_STREAM_ONCE(args)
ros::ServiceServer reconfigure_server_
ServiceServer for serving reconfigure service.
ros::ServiceServer pause_server_
ServiceServer for serving pause service.
virtual bool onStart()=0
Function to be defined by the user. Called at the end of transition from STOPPED to PAUSED...
ros::ServiceServer restart_server_
ServiceServer for serving restart service, where restart means transition to STOPPED and then RUNNING...
std::shared_ptr< IsolatedAsyncTimer > registerIsolatedTimer(const IsolatedAsyncTimer::LambdaCallback &callback, const float &frequency, bool stoppable=true, bool autostart=false, bool oneshot=false)
Register an isolated async timer.
#define ROS_WARN_STREAM(args)
#define ROS_DEBUG_STREAM(args)
ros::ServiceServer start_server_
ServiceServer for serving start service.
RobotActivity class implements ROS node lifecycle.
bool sleep()
MemberLambdaCallback StateTransitions[static_cast< uint8_t >(State::Count)][static_cast< uint8_t >(State::Count)]
ros::NodeHandlePtr node_handle_
virtual bool onStop()=0
Function to be defined by the user. Called at the end of transition from PAUSED to STOPPED...
virtual void onTerminate()=0
Function to be defined by the user. Called at the end of transition from UNCONFIGURED to TERMINATED...
virtual bool onUnconfigure()=0
Function to be defined by the user. Called at the end of transition from STOPPED to UNCONFIGURED...
bool pause()
Called automatically, when transition from RUNNING to PAUSED. RobotActivity calls this function direc...
uint32_t getNumSubscribers() const
bool resume()
Called automatically, when transition from PAUSED to RUNNING. RobotActivity calls this function direc...
#define ROS_INFO_STREAM(args)
State
RobotActivity state enum.
bool transitionToState(const State &new_state)
Transitions to a new state. Path must exists between the current and the new state. All appropriate function will be called when transition to the goal state.
std::string node_name_
Name of the actual ROS node.
static Time now()
std::ostream & operator<<(std::ostream &os, State state)
Overridden operator<< for easy State enum printing.
RobotActivity()=delete
Default constructor is deleted.
bool configure()
Called automatically, when transition from UNCONFIGURED to STOPPED. RobotActivity calls this function...
void runAsync(uint8_t threads=0)
Spins an amount of threads to serve the global callback queue. The call is non-blocking.
#define ROS_ERROR_STREAM(args)
std::string node_namespace_
Node&#39;s namespace, if empty then the private node handle resolves to ~ otheerwise private node handle ...
ros::ServiceServer terminate_server_
ServiceServer for serving terminate service.
virtual bool onResume()=0
Function to be defined by the user. Called at the end of transition from PAUSED to RUNNING...
std::string getNamespace() const
Returns the full private namespace.
RobotActivity & init(bool autostart=false)
Initializes the RobotActivity.
ros::CallbackQueue state_request_callback_queue_
Callback queue for state change request services.
void run(uint8_t threads=0)
Spins an amount of threads to serve the global callback queue. The call is blocking.
ROSCPP_DECL void waitForShutdown()
#define ROS_DEBUG(...)


robot_activity
Author(s): Maciej ZURAD
autogenerated on Thu Jun 6 2019 19:32:17