driver_base: driver_base: driver.h Source File

00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 // Author: Blaise Gassend
00036 #ifndef __DRIVER_BASE__DRIVER_H__
00037 #define __DRIVER_BASE__DRIVER_H__
00038 
00039 #include <ros/ros.h>
00040 #include <boost/function.hpp>
00041 #include <boost/thread/recursive_mutex.hpp>
00042 #include <boost/thread/mutex.hpp>
00043 #include <stdarg.h>
00044 #include <cstdio>
00045 
00046 namespace driver_base
00047 {
00048 
00068 class Driver
00069 {
00070 public:  
00071   typedef char state_t;
00072 
00073 protected:
00074   state_t state_;
00075 
00076   virtual void doOpen() = 0;
00077   virtual void doClose() = 0;
00078   virtual void doStart() = 0;
00079   virtual void doStop() = 0;
00080 
00081   typedef boost::function< void() > hookFunction;
00082   hookFunction postOpenHook;
00083   
00084 private:  
00085   std::string status_message_;
00086   boost::mutex status_message_mutex_;
00087   bool status_ok_;
00088   bool status_recovery_complete_;
00089 
00090 public:
00091   void setPostOpenHook(hookFunction f)
00092   {
00093     postOpenHook = f;
00094   }
00095 
00096   virtual std::string getID() = 0;
00097   
00098   boost::recursive_mutex mutex_; 
00099 
00100   static const state_t CLOSED = 0; // Not connected to the hardware.
00101   static const state_t OPENED = 1; // Connected to the hardware, ready to start streaming.
00102   static const state_t RUNNING = 2; // Streaming data.
00103 
00104   bool goState(state_t target)
00105   {
00106     boost::recursive_mutex::scoped_lock lock(mutex_);
00107 
00108     if (state_ > target)
00109       return lowerState(target);
00110 
00111     if (state_ < target)
00112       return raiseState(target);
00113 
00114     return true;
00115   }
00116 
00117   bool raiseState(state_t target)
00118   {
00119     boost::recursive_mutex::scoped_lock lock(mutex_);
00120 
00121     switch (getState())  
00122     {
00123       case Driver::CLOSED:
00124         if (target <= Driver::CLOSED)
00125           return true;
00126         if (!tryTransition(Driver::OPENED, &Driver::doOpen))
00127           return false;
00128 
00129       case Driver::OPENED:
00130         if (target <= Driver::OPENED)
00131           return true;
00132         if (!tryTransition(Driver::RUNNING, &Driver::doStart))
00133           return false;
00134 
00135       default:
00136         return target <= getState();
00137     }
00138   }
00139 
00140   bool lowerState(state_t target)
00141   {
00142     boost::recursive_mutex::scoped_lock lock(mutex_);
00143 
00144     switch (getState())  
00145     {
00146       case Driver::RUNNING:
00147         if (target >= Driver::RUNNING)
00148           return true;
00149         if (!tryTransition(Driver::OPENED, &Driver::doStop))
00150           return false;
00151 
00152       case Driver::OPENED:
00153         if (target >= Driver::OPENED)
00154           return true;
00155         if (!tryTransition(Driver::CLOSED, &Driver::doClose))
00156           return false;
00157 
00158       default:
00159         return target >= getState();
00160     }
00161   }
00162 
00163   bool goRunning()
00164   {
00165     return goState(Driver::RUNNING);
00166   }
00167 
00168   bool goOpened()
00169   {
00170     return goState(Driver::OPENED);
00171   }
00172 
00173   bool goClosed()
00174   {
00175     return goState(Driver::CLOSED);
00176   }
00177 
00178   bool stop()
00179   {
00180     return lowerState(Driver::OPENED);
00181   }
00182 
00183   bool start()
00184   {
00185     return raiseState(Driver::RUNNING);
00186   }
00187   
00188   bool open()
00189   {
00190     return raiseState(Driver::OPENED);
00191   }
00192   
00193   bool close()
00194   {
00195     return lowerState(Driver::CLOSED);
00196   }
00197   
00198   bool isRunning()
00199   {
00200     return getState() == Driver::RUNNING;
00201   }
00202 
00203   bool isOpened()
00204   {
00205     return getState() == Driver::OPENED;
00206   }
00207 
00208   bool isClosed()
00209   {
00210     return getState() == Driver::CLOSED;
00211   }
00212   
00213   bool isStopped()
00214   {
00215     state_t s = getState();
00216     return s == Driver::CLOSED || s == Driver::OPENED;
00217   }
00218 
00219   state_t getState()
00220   {
00221     return state_;
00222   }
00223 
00224   const std::string getStateName()
00225   {
00226     return getStateName(state_);
00227   }
00228   
00229   static const std::string &getStateName(state_t s)
00230   {
00231     static const std::string names[4] = {
00232       std::string("CLOSED"),
00233       std::string("OPENED"),
00234       std::string("RUNNING"),
00235       std::string("Unknown")
00236     };
00237   
00238     if (s >= 0 && s <= 2)
00239       return names[(int) s];
00240     else
00241       return names[3];
00242   }
00243 
00244   Driver() : state_(CLOSED), status_ok_(false), status_recovery_complete_(false) {}
00245   virtual ~Driver() {}
00246 
00247   bool getStatusOk()
00248   {
00249     return status_ok_;
00250   }
00251   
00252   bool getRecoveryComplete()
00253   {
00254     return status_recovery_complete_;
00255   }
00256 
00257   void clearRecoveryComplete()
00258   {
00259     status_recovery_complete_ = false;
00260   }
00261   
00262   const std::string getStatusMessage()
00263   { // Not returning by reference for thread safety.
00264     boost::mutex::scoped_lock lock_(status_message_mutex_);
00265     return status_message_;
00266   }
00267 
00268   // Set ok to true if the status message is not an error.
00269   // Set recovery_complete if the device is now fully functioning, and any
00270   // subsequent error should be considered as an error.
00271   void setStatusMessage(const std::string &msg, bool ok = false, bool recovery_complete = false)
00272   {
00273     boost::mutex::scoped_lock lock_(status_message_mutex_);
00274     ROS_DEBUG("%s", msg.c_str());
00275     status_message_ = msg;
00276     status_ok_ = ok;
00277     status_recovery_complete_ |= recovery_complete;
00278   }
00279 
00280   void setStatusMessagef(const char *format, ...)
00281   {
00282     va_list va;
00283     char buff[1000]; // @todo This could be done more elegantly.
00284     va_start(va, format);
00285     if (vsnprintf(buff, sizeof(buff), format, va) >= (int) sizeof(buff))
00286       ROS_DEBUG("Really long string in Driver::setStatusMessagef, it was trunccated.");
00287     setStatusMessage(std::string(buff));
00288     va_end(va);
00289   }
00290 
00291 private:  
00292   const std::string &getTransitionName(void (Driver::*transition)())
00293   {
00294     static const std::string open = std::string("open");
00295     static const std::string close = std::string("close");
00296     static const std::string start = std::string("start");
00297     static const std::string stop = std::string("stop");
00298     static const std::string unknown = std::string("Unknown");
00299 
00300     if (transition == &Driver::doOpen)
00301       return open;
00302     if (transition == &Driver::doClose)
00303       return close;
00304     if (transition == &Driver::doStart)
00305       return start;
00306     if (transition == &Driver::doStop)
00307       return stop;
00308     
00309     return unknown;
00310   }
00311 
00312   bool tryTransition(state_t target, void (Driver::*transition)())
00313   {
00314     boost::recursive_mutex::scoped_lock lock_(mutex_);
00315     state_t orig = state_;
00316     ROS_DEBUG("Trying transition %s from %s to %s.", getTransitionName(transition).c_str(), getStateName(orig).c_str(), getStateName(target).c_str());
00317     try
00318     {
00319       (this->*transition)();
00320     }
00321     catch (...) 
00322     //catch (std::Exception e)
00323     {
00324       ROS_WARN("Caught exception in transition %s from %s to %s.\n", getTransitionName(transition).c_str(), getStateName(orig).c_str(), getStateName(target).c_str());
00325       //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());
00326     }
00327     bool out = state_ == target;
00328     if (out && transition == &Driver::doOpen)
00329       postOpenHook();
00330     ROS_DEBUG("Transition %s from %s to %s %s.", getTransitionName(transition).c_str(), getStateName(orig).c_str(), getStateName(target).c_str(), out ? "succeeded" : "failed");
00331     return out;
00332   }
00333 };
00334 
00336 
00337 };
00338 
00339 #endif
00340