driver.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2009, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 // Author: Blaise Gassend
36 #ifndef __DRIVER_BASE__DRIVER_H__
37 #define __DRIVER_BASE__DRIVER_H__
38 
39 #include <ros/ros.h>
40 #include <boost/function.hpp>
41 #include <boost/thread/recursive_mutex.hpp>
42 #include <boost/thread/mutex.hpp>
43 #include <stdarg.h>
44 #include <cstdio>
45 
46 namespace driver_base
47 {
48 
68 class Driver
69 {
70 public:
71  typedef char state_t;
72 
73 protected:
74  state_t state_;
75 
76  virtual void doOpen() = 0;
77  virtual void doClose() = 0;
78  virtual void doStart() = 0;
79  virtual void doStop() = 0;
80 
81  typedef boost::function< void() > hookFunction;
82  hookFunction postOpenHook;
83 
84 private:
85  std::string status_message_;
86  boost::mutex status_message_mutex_;
87  bool status_ok_;
89 
90 public:
91  void setPostOpenHook(hookFunction f)
92  {
93  postOpenHook = f;
94  }
95 
96  virtual std::string getID() = 0;
97 
98  boost::recursive_mutex mutex_;
99 
100  static const state_t CLOSED = 0; // Not connected to the hardware.
101  static const state_t OPENED = 1; // Connected to the hardware, ready to start streaming.
102  static const state_t RUNNING = 2; // Streaming data.
103 
104  bool goState(state_t target)
105  {
106  boost::recursive_mutex::scoped_lock lock(mutex_);
107 
108  if (state_ > target)
109  return lowerState(target);
110 
111  if (state_ < target)
112  return raiseState(target);
113 
114  return true;
115  }
116 
117  bool raiseState(state_t target)
118  {
119  boost::recursive_mutex::scoped_lock lock(mutex_);
120 
121  switch (getState())
122  {
123  case Driver::CLOSED:
124  if (target <= Driver::CLOSED)
125  return true;
127  return false;
128 
129  case Driver::OPENED:
130  if (target <= Driver::OPENED)
131  return true;
133  return false;
134 
135  default:
136  return target <= getState();
137  }
138  }
139 
140  bool lowerState(state_t target)
141  {
142  boost::recursive_mutex::scoped_lock lock(mutex_);
143 
144  switch (getState())
145  {
146  case Driver::RUNNING:
147  if (target >= Driver::RUNNING)
148  return true;
150  return false;
151 
152  case Driver::OPENED:
153  if (target >= Driver::OPENED)
154  return true;
156  return false;
157 
158  default:
159  return target >= getState();
160  }
161  }
162 
163  bool goRunning()
164  {
165  return goState(Driver::RUNNING);
166  }
167 
168  bool goOpened()
169  {
170  return goState(Driver::OPENED);
171  }
172 
173  bool goClosed()
174  {
175  return goState(Driver::CLOSED);
176  }
177 
178  bool stop()
179  {
180  return lowerState(Driver::OPENED);
181  }
182 
183  bool start()
184  {
185  return raiseState(Driver::RUNNING);
186  }
187 
188  bool open()
189  {
190  return raiseState(Driver::OPENED);
191  }
192 
193  bool close()
194  {
195  return lowerState(Driver::CLOSED);
196  }
197 
198  bool isRunning()
199  {
200  return getState() == Driver::RUNNING;
201  }
202 
203  bool isOpened()
204  {
205  return getState() == Driver::OPENED;
206  }
207 
208  bool isClosed()
209  {
210  return getState() == Driver::CLOSED;
211  }
212 
213  bool isStopped()
214  {
215  state_t s = getState();
216  return s == Driver::CLOSED || s == Driver::OPENED;
217  }
218 
219  state_t getState()
220  {
221  return state_;
222  }
223 
224  const std::string getStateName()
225  {
226  return getStateName(state_);
227  }
228 
229  static const std::string &getStateName(state_t s)
230  {
231  static const std::string names[4] = {
232  std::string("CLOSED"),
233  std::string("OPENED"),
234  std::string("RUNNING"),
235  std::string("Unknown")
236  };
237 
238  if (s >= 0 && s <= 2)
239  return names[(int) s];
240  else
241  return names[3];
242  }
243 
244  Driver() : state_(CLOSED), status_ok_(false), status_recovery_complete_(false) {}
245  virtual ~Driver() {}
246 
247  bool getStatusOk()
248  {
249  return status_ok_;
250  }
251 
253  {
255  }
256 
258  {
259  status_recovery_complete_ = false;
260  }
261 
262  const std::string getStatusMessage()
263  { // Not returning by reference for thread safety.
264  boost::mutex::scoped_lock lock_(status_message_mutex_);
265  return status_message_;
266  }
267 
268  // Set ok to true if the status message is not an error.
269  // Set recovery_complete if the device is now fully functioning, and any
270  // subsequent error should be considered as an error.
271  void setStatusMessage(const std::string &msg, bool ok = false, bool recovery_complete = false)
272  {
273  boost::mutex::scoped_lock lock_(status_message_mutex_);
274  ROS_DEBUG("%s", msg.c_str());
275  status_message_ = msg;
276  status_ok_ = ok;
277  status_recovery_complete_ |= recovery_complete;
278  }
279 
280  void setStatusMessagef(const char *format, ...)
281  {
282  va_list va;
283  char buff[1000]; // @todo This could be done more elegantly.
284  va_start(va, format);
285  if (vsnprintf(buff, sizeof(buff), format, va) >= (int) sizeof(buff))
286  ROS_DEBUG("Really long string in Driver::setStatusMessagef, it was trunccated.");
287  setStatusMessage(std::string(buff));
288  va_end(va);
289  }
290 
291 private:
292  const std::string &getTransitionName(void (Driver::*transition)())
293  {
294  static const std::string open = std::string("open");
295  static const std::string close = std::string("close");
296  static const std::string start = std::string("start");
297  static const std::string stop = std::string("stop");
298  static const std::string unknown = std::string("Unknown");
299 
300  if (transition == &Driver::doOpen)
301  return open;
302  if (transition == &Driver::doClose)
303  return close;
304  if (transition == &Driver::doStart)
305  return start;
306  if (transition == &Driver::doStop)
307  return stop;
308 
309  return unknown;
310  }
311 
312  bool tryTransition(state_t target, void (Driver::*transition)())
313  {
314  boost::recursive_mutex::scoped_lock lock_(mutex_);
315  state_t orig = state_;
316  ROS_DEBUG("Trying transition %s from %s to %s.", getTransitionName(transition).c_str(), getStateName(orig).c_str(), getStateName(target).c_str());
317  try
318  {
319  (this->*transition)();
320  }
321  catch (...)
322  //catch (std::Exception e)
323  {
324  ROS_WARN("Caught exception in transition %s from %s to %s.\n", getTransitionName(transition).c_str(), getStateName(orig).c_str(), getStateName(target).c_str());
325  //ROS_WARN("Caught exception in transition from %s to %s.\n%s", e.what(), getTransitionName(transition).c_str(), getStateName(orig).c_str(), getStateName(target).c_str());
326  }
327  bool out = state_ == target;
328  if (out && transition == &Driver::doOpen)
329  postOpenHook();
330  ROS_DEBUG("Transition %s from %s to %s %s.", getTransitionName(transition).c_str(), getStateName(orig).c_str(), getStateName(target).c_str(), out ? "succeeded" : "failed");
331  return out;
332  }
333 };
334 
336 
337 };
338 
339 #endif
340 
virtual void doOpen()=0
bool getRecoveryComplete()
Definition: driver.h:252
state_t getState()
Definition: driver.h:219
void clearRecoveryComplete()
Definition: driver.h:257
static const state_t CLOSED
Definition: driver.h:100
virtual ~Driver()
Definition: driver.h:245
static const state_t RUNNING
Definition: driver.h:102
bool getStatusOk()
Definition: driver.h:247
static const std::string & getStateName(state_t s)
Definition: driver.h:229
XmlRpcServer s
bool status_recovery_complete_
Definition: driver.h:88
bool isStopped()
Definition: driver.h:213
#define ROS_WARN(...)
std::string status_message_
Definition: driver.h:85
hookFunction postOpenHook
Definition: driver.h:82
void setPostOpenHook(hookFunction f)
Definition: driver.h:91
const std::string getStatusMessage()
Definition: driver.h:262
bool raiseState(state_t target)
Definition: driver.h:117
bool isRunning()
Definition: driver.h:198
state_t state_
Definition: driver.h:74
void setStatusMessagef(const char *format,...)
Definition: driver.h:280
bool lowerState(state_t target)
Definition: driver.h:140
bool goRunning()
Definition: driver.h:163
bool tryTransition(state_t target, void(Driver::*transition)())
Definition: driver.h:312
ROSCPP_DECL bool ok()
const std::string getStateName()
Definition: driver.h:224
const std::string & getTransitionName(void(Driver::*transition)())
Definition: driver.h:292
boost::function< void() > hookFunction
Definition: driver.h:81
bool goState(state_t target)
Definition: driver.h:104
boost::recursive_mutex mutex_
Definition: driver.h:98
void setStatusMessage(const std::string &msg, bool ok=false, bool recovery_complete=false)
Definition: driver.h:271
boost::mutex status_message_mutex_
Definition: driver.h:86
virtual void doStart()=0
virtual void doClose()=0
static const state_t OPENED
Definition: driver.h:101
virtual void doStop()=0
virtual std::string getID()=0
#define ROS_DEBUG(...)


driver_base
Author(s): Blaise Gassend
autogenerated on Thu Apr 11 2019 02:07:21