driver_node.h
Go to the documentation of this 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_NODE_H__
00037 #define __DRIVER_BASE__DRIVER_NODE_H__
00038 
00039 #include <diagnostic_updater/diagnostic_updater.h>
00040 #include <self_test/self_test.h>
00041 #include <ros/node_handle.h>
00042 #include <boost/thread.hpp>
00043 #include <boost/bind.hpp>
00044 #include <driver_base/SensorLevels.h>
00045 #include <signal.h>
00046 #include <dynamic_reconfigure/server.h>
00047 
00048 namespace driver_base
00049 {
00050 
00051 class AbstractDriverNode
00052 {
00053 public:
00054   static int ctrl_c_hit_count_; 
00055   
00056   static void hupCalled(int sig)
00057   {
00058     ROS_WARN("Unexpected SIGHUP caught. Ignoring it.");
00059   }
00060   
00061   static void sigCalled(int sig)
00062   {
00063     ctrl_c_hit_count_++;
00064   }
00065 };
00066   
00067 template <class DriverType>
00068 int main(int argc, char **argv, std::string name)
00069 {
00072   ros::init(argc, argv, name);
00073   //ros::init(argc, argv, name, ros::init_options::NoSigintHandler);
00074   //signal(SIGINT, &AbstractDriverNode::sigCalled);
00075   //signal(SIGTERM, &AbstractDriverNode::sigCalled);
00076   signal(SIGHUP, &AbstractDriverNode::hupCalled);
00077   ros::NodeHandle nh;
00078   DriverType driver(nh);
00079   return driver.spin();
00081 }
00082   
00083 template <class Driver>
00084 class DriverNode : public AbstractDriverNode
00085 {
00086 public:
00087   //typedef char state_t;
00088   typedef typename Driver::Config Config;
00089  
00090 protected:
00091   // Hooks
00092   virtual void addDiagnostics() = 0; 
00093   virtual void addStoppedTests() = 0;
00094   virtual void addOpenedTests() = 0;
00095   virtual void addRunningTests() = 0;
00096   virtual void reconfigureHook(int level) = 0; 
00097 
00098   // Helper classes
00099   ros::NodeHandle node_handle_;
00100   ros::NodeHandle private_node_handle_;
00101   self_test::TestRunner self_test_;
00102   diagnostic_updater::Updater diagnostic_;
00103   
00104   Driver driver_;
00105   dynamic_reconfigure::Server<Config> reconfigure_server_; 
00106   diagnostic_updater::CompositeDiagnosticTask driver_status_diagnostic_;
00107   // driver_ declaration must precede reconfigure_server_ for constructor
00108   // calling order reasons.
00109 
00110 private:
00111   // Subscriber tracking
00112   int num_subscribed_topics_; // Number of topics that have subscribers.
00113   
00114   //static const state_t DISABLED = 0;
00115   //static const state_t LAZY_ON = 1;
00116   //static const state_t ALWAYS_ON = 2;
00117   //static const state_t SELF_TEST = 3;
00118   //static const state_t EXITING = 4;
00119 
00120   //state_t state_;
00121                           
00122   boost::shared_ptr<boost::thread> ros_thread_;
00123   
00124   int exit_status_;
00125 
00126   // The device
00127   typedef typename Driver::state_t drv_state_t;
00128 
00129   drv_state_t pre_self_test_driver_state_;
00130     
00131   diagnostic_updater::FunctionDiagnosticTask driver_status_standard_diagnostic_;
00132 
00133   void reconfigure(Config &config, uint32_t level)
00134   {
00136     ROS_DEBUG("Reconfigure called at level %x.", level);
00137     boost::recursive_mutex::scoped_lock lock(driver_.mutex_);
00138     
00139     drv_state_t orig_state = driver_.getState();
00140   
00141     if ((level | driver_base::SensorLevels::RECONFIGURE_STOP) == level)
00142     {
00143       driver_.stop();
00144       if (!driver_.isStopped())
00145         ROS_ERROR("Failed to stop streaming before reconfiguring. Reconfiguration may fail.");
00146     }
00147   
00148     if ((level | driver_base::SensorLevels::RECONFIGURE_CLOSE) == level)
00149     {
00150       driver_.close();
00151       if (!driver_.isClosed())
00152         ROS_ERROR("Failed to close device before reconfiguring. Reconfiguration may fail.");
00153     }
00154   
00155     driver_.config_update(config, level);
00156     config = driver_.config_;
00157     reconfigureHook(level);
00158   
00159     driver_.goState(orig_state);
00160 
00161     /* Other errors will show up when this happens. No need to have it
00162      * here.
00163     if (driver_.getState() != orig_state)
00164     {
00165       ROS_ERROR("Failed to resume original device state after reconfiguring. The requested configuration may contain errors.");
00166     }
00167     */
00168     
00169     ROS_DEBUG("Reconfigure completed.");
00170   }
00171 
00172 private:
00173 /*  void connectCallback(const ros::PublisherPtr &pub)
00174   {
00175     if (pub.numSubscribers == 1)
00176       num_subscribed_topics_++;
00177 
00178     if (num_subscribed_topics_ == 1)
00179       start();
00180   }
00181 
00182   void disconnectCallback(const ros::PublisherPtr &pub)
00183   {
00184     if (pub.numSubscribers == 0)
00185       num_subscribed_topics_++;
00186 
00187     if (num_subscribed_topics_ == 0)
00188       stop();
00189   }*/
00190 
00191   void prepareDiagnostics()
00192   {
00193     driver_status_diagnostic_.addTask(&driver_status_standard_diagnostic_);
00194     diagnostic_.add(driver_status_diagnostic_);
00195     addDiagnostics();
00196   }
00197 
00198   void statusDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat)
00199   {
00200     if (driver_.isRunning())
00201       stat.summary(0, "OK");
00202     else
00203       stat.summary(2, "not running");
00204     
00205     stat.add("Driver state:", driver_.getStateName());
00206     stat.add("Latest status message:", driver_.getStatusMessage());
00208   }
00209 
00210   void prepareSelfTests()
00211   {
00212     self_test_.add( "Interruption Test", this, &DriverNode::interruptionTest );
00213     addStoppedTests();
00214     self_test_.add( "Connection Test", this, &DriverNode::openTest );
00215     self_test_.add( "ID Test", this, &DriverNode::idTest );
00216     addOpenedTests();
00217     self_test_.add( "Start Streaming Test", this, &DriverNode::runTest );
00218     addRunningTests();
00219     self_test_.add( "Stop Streaming Test", this, &DriverNode::stopTest );
00220     self_test_.add( "Disconnection Test", this, &DriverNode::closeTest );
00221     self_test_.add( "Resume Activity", this, &DriverNode::resumeTest );
00222   } 
00223 
00224   void interruptionTest(diagnostic_updater::DiagnosticStatusWrapper& status)
00225   {
00226     pre_self_test_driver_state_ = driver_.getState();
00227     driver_.goClosed();
00228     
00229     drv_state_t s = driver_.getState();
00230 
00231     if (num_subscribed_topics_ > 0)  
00232       status.summary(1, "There were active subscribers.  Running of self test interrupted operations.");
00233     else if (s != Driver::CLOSED)
00234       status.summaryf(2, "Could not close device, currently in state %s.", Driver::getStateName(s).c_str());
00235     else
00236       status.summary(0, "No operation interrupted.");
00237   } 
00238   
00239   void reliableGoStateTest(diagnostic_updater::DiagnosticStatusWrapper &status, drv_state_t target_state)
00240   {
00241     const int max_tries = 5;
00242     ROS_ASSERT(max_tries > 0);
00243     int retries;
00244     drv_state_t current_state;
00245     for (retries = 0; retries < max_tries; retries++)
00246     {
00247       sleep(1);
00248       driver_.goState(target_state);
00249 
00250       current_state = driver_.getState();
00251       if (current_state == target_state)
00252         break;
00253     }
00254 
00255     std::string target_state_name = Driver::getStateName(target_state);
00256     std::string current_state_name = Driver::getStateName(current_state);
00257     
00258     // We carefully avoid rechecking the state here, as it may have
00259     // changed since we last checked. We aren't here to check that we
00260     // stayed in the target state, just want to check that we made it
00261     // there...
00262     if (retries >= max_tries) 
00263       status.summaryf(2, "Failed to go to %s state despite %u retries, now in state %s.", target_state_name.c_str(), retries, current_state_name.c_str());
00264     else if (retries)
00265       status.summaryf(1, "Successfully went to state %s after %u retries.", target_state_name.c_str(), retries);
00266     else
00267       status.summaryf(0, "Successfully went to state %s.", target_state_name.c_str());
00268   }
00269 
00270   void openTest(diagnostic_updater::DiagnosticStatusWrapper& status)
00271   {
00272     reliableGoStateTest(status, Driver::OPENED);
00273   }
00274   
00275   void idTest(diagnostic_updater::DiagnosticStatusWrapper& status)
00276   {
00277     std::string ID = driver_.getID();
00278     if (ID.compare(""))
00279     {
00280       status.summaryf(0, "Device ID is %s", ID.c_str());
00281       self_test_.setID(ID);
00282     }
00283     else
00284       status.summaryf(2, "Error reading Device ID");
00285   }
00286 
00287   void runTest(diagnostic_updater::DiagnosticStatusWrapper& status)
00288   {
00289     reliableGoStateTest(status, Driver::RUNNING);
00290   } 
00291 
00292   void stopTest(diagnostic_updater::DiagnosticStatusWrapper& status)
00293   {
00294     reliableGoStateTest(status, Driver::OPENED);
00295   } 
00296   
00297   void closeTest(diagnostic_updater::DiagnosticStatusWrapper& status)
00298   {
00299     reliableGoStateTest(status, Driver::CLOSED);
00300   } 
00301   
00302   void resumeTest(diagnostic_updater::DiagnosticStatusWrapper& status)
00303   {
00304     reliableGoStateTest(status, pre_self_test_driver_state_);
00305   }
00306   
00307 public:
00308   virtual ~DriverNode() {}
00309 
00310   int spin()
00311   {
00312     prepareDiagnostics();
00313     prepareSelfTests();
00314     typename dynamic_reconfigure::Server<Config>::CallbackType f = boost::bind(&DriverNode::reconfigure, this, _1, _2);
00315     reconfigure_server_.setCallback(f);
00316     
00317     ros_thread_.reset(new boost::thread(boost::bind(&ros::spin)));
00319     assert(ros_thread_);
00320                
00321     driver_.goRunning();
00322 
00324     std::string last_status_message;
00325     while (node_handle_.ok() && /*state_ != EXITING &&*/ !ctrl_c_hit_count_)
00326     {
00327       {
00328         // Must lock here, otherwise operations like reconfigure will cause
00329         // the test to pass, and an unnecessary restart will happen when the 
00330         // reconfigure is done.
00331         boost::recursive_mutex::scoped_lock lock_(driver_.mutex_);
00332 
00333         if (!driver_.isRunning())
00334         {             
00335           std::string new_status_message = driver_.getStatusMessage();
00336           if ((last_status_message != new_status_message || driver_.getRecoveryComplete()) 
00337               && !driver_.getStatusOk())
00338           {
00339             ROS_ERROR("%s", new_status_message.c_str()); 
00340             driver_.clearRecoveryComplete();
00341             last_status_message = new_status_message;
00342           }
00343           ros::WallDuration(1).sleep();
00344           driver_.goClosed(); 
00345           driver_.goRunning();
00346         }
00347 
00348         diagnostic_.update();
00349         self_test_.checkTest();
00350       }
00351       ros::WallDuration(0.1).sleep();
00352     }
00353 
00354     driver_.goClosed();
00355 
00356     ros::shutdown();
00357     
00358     if (ros_thread_ && !ros_thread_->timed_join((boost::posix_time::milliseconds) 2000))
00359     {
00360       ROS_ERROR("ROS thread did not die after two seconds. Pretending that it did. This is probably a bad sign.");
00361     }
00362     ros_thread_.reset();
00363 
00364     return 0; 
00365   }
00366   
00367   DriverNode(ros::NodeHandle &nh) : 
00368     node_handle_(nh), 
00369     private_node_handle_("~"), 
00370     self_test_(node_handle_), 
00371     diagnostic_(), 
00372     reconfigure_server_(driver_.mutex_, ros::NodeHandle("~")),
00373     driver_status_diagnostic_("Driver Status"),
00374     driver_status_standard_diagnostic_("Driver Status", boost::bind(&DriverNode::statusDiagnostic, this, _1))
00375   {
00376     num_subscribed_topics_ = 0; 
00377     exit_status_ = 0;
00378 
00379     // Defer most initialization to spin so that virtual calls in derived
00380     // classes will succeed.
00381   }
00382 };
00383 
00384 int AbstractDriverNode::ctrl_c_hit_count_ = 0;
00385   
00386 // @todo exit status.
00387 // @todo take over ctrl_c.
00388 
00389 };
00390 
00391 #endif


driver_base
Author(s): Blaise Gassend
autogenerated on Wed Aug 26 2015 11:21:01