battery_check.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, 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 
00037 
00038 #include <iostream>
00039 #include <string>
00040 #include <vector>
00041 
00042 #include <boost/bind.hpp>
00043 #include <boost/program_options.hpp>
00044 #include <boost/thread/thread.hpp>
00045 #include <boost/date_time.hpp>
00046 
00047 #include <ros/rate.h>
00048 #include <ros/time.h>
00049 
00050 #include "ocean.h"
00051 
00052 namespace po = boost::program_options;
00053 using namespace std;
00054 using namespace willowgarage::ocean;
00055 
00056 class BatteryServerChecker
00057 {
00058 private:
00059   int id_;
00060   string device_;
00061   int timeout_;
00062   ocean os;
00063   volatile bool stopRequest;
00064   
00065   vector<bool> present;
00066   vector<bool> inhibited;
00067   vector<bool> no_good;
00068   vector<ros::Time> last_update;
00069 
00070   boost::shared_ptr<boost::thread> runThread_;
00071 
00072   void run()
00073   {
00074     ros::Rate my_rate(50);
00075 
00076     while (!stopRequest)
00077     {
00078       os.run();
00079       for (int i = 0; i < os.server.MAX_BAT_COUNT; ++i)
00080       {
00081         present[i]     = os.server.battery[i].present;
00082         inhibited[i]   = os.server.battery[i].inhibited;
00083         no_good[i]     = os.server.battery[i].power_no_good;
00084         last_update[i] = os.server.battery[i].last_battery_update;
00085       }
00086         
00087       my_rate.sleep();
00088     }
00089   }
00090 
00091 public:
00092   BatteryServerChecker(int id, const string &dev, int timeout):
00093     id_(id), device_(dev), timeout_(timeout), os(id, 0), stopRequest(false)
00094   {
00095     os.initialize(dev);
00096 
00097     present.resize(os.server.MAX_BAT_COUNT);
00098     inhibited.resize(os.server.MAX_BAT_COUNT);
00099     no_good.resize(os.server.MAX_BAT_COUNT);
00100     last_update.resize(os.server.MAX_BAT_COUNT);
00101   }
00102 
00103   void start()
00104   {
00105     runThread_ = boost::shared_ptr<boost::thread>(new boost::thread(boost::bind(&BatteryServerChecker::run, this)));
00106   }
00107       
00108   void stop()
00109   {
00110     stopRequest = true;
00111  
00112     boost::posix_time::time_duration timeout = boost::posix_time::seconds(5);
00113     runThread_->timed_join(timeout);
00114   }
00115 
00116   bool batteryOK() const
00117   {
00118     bool ok = true; // Checks to make sure OK
00119     bool stale = false; // All batteries must have updated within timeout
00120 
00121     for (int i = 0; i < os.server.MAX_BAT_COUNT; ++i)
00122     {
00123       ok = ok && present[i]; 
00124       stale = ((ros::Time::now() - last_update[i]).toSec() > timeout_) || stale;
00125     }
00126 
00127     return ok && !stale;
00128   }
00129 
00130   string getStatus() const
00131   {
00132     stringstream ss;
00133     ss << "Port: " << device_ << "\n";
00134 
00135     bool ng = false;
00136     for (int i = 0; i < os.server.MAX_BAT_COUNT; ++i)
00137       {
00138         ss << "\tBattery " << i << ":\n";
00139         ss << "\t\tPresent: " << (present[i] ? "Yes" : "No") << "\n";
00140         ss << "\t\tInhibited: " << (inhibited[i] ? "Yes" : "No") << "\n";
00141         ss << "\t\tNo Good: " << (no_good[i] ? "Yes" : "No") << "\n";
00142         ss << "\t\tHas updated: " << (last_update[i] > ros::Time(1) ? "Yes" : "No") << "\n";
00143         ss << "\t\tTime Since Update: " << (ros::Time::now() - last_update[i]).toSec() << "\n";
00144         ng = ng || no_good[i];
00145       }
00146     if (ng)
00147       ss << "Warning: \"No Good\" flag enabled. This may cause battery problems.\n";
00148 
00149     return ss.str();
00150   }
00151 };
00152 
00153 int main(int argc, char** argv)
00154 {
00155   int duration, min_duration, timeout;
00156   po::options_description desc("battery_check port1 port2 ... [options]");
00157   desc.add_options()
00158     ("help,h", "Print help message and exit")
00159     ("verbose,v", "Verbose battery output")
00160     ("timeout,t", po::value<int>(&timeout)->default_value(15), "Timeout before stale")
00161     ("duration,d", po::value<int>(&duration)->default_value(60), "Maximum duration of test")
00162     ("min-duration,m", po::value<int>(&min_duration)->default_value(20), "Minimum duration of test")
00163     ("port", po::value<vector<string> >(), "Battery ports to check");
00164 
00165   po::positional_options_description p;
00166   p.add("port", -1);
00167 
00168   po::variables_map vm;
00169   po::store(po::command_line_parser(argc, argv).options(desc).positional(p).run(), vm);
00170   po::notify(vm);
00171 
00172   bool verbose = vm.count("verbose");
00173 
00174   if (vm.count("help"))
00175   {
00176     cout << desc << "\n";
00177     return 1;
00178   }
00179 
00180   if (!vm.count("port"))
00181   {
00182     fprintf(stderr, "No ports specified. Unable to check batteries.\n");
00183     return 1;
00184   }
00185 
00186   if (min_duration < timeout)
00187   {
00188     fprintf(stderr, "Timeout must be greater than the minimum duration. Unable check batteries.\n");
00189     return 1;
00190   }
00191 
00192   vector<string> ports(vm["port"].as< vector<string> >());
00193   vector<string>::iterator it;
00194 
00195   if (verbose)
00196   {
00197     cout << "Checking ports: ";
00198     for (it = ports.begin(); it != ports.end(); ++it)
00199       cout << *it << ", ";
00200     cout << "\n";
00201   }
00202 
00203   if (verbose)
00204     cout << "Running battery check. Waiting for battery drivers to initialize\n";
00205 
00206   ros::Time::init();
00207 
00208   vector<boost::shared_ptr<BatteryServerChecker> > checkers;
00209   for (uint i = 0; i < ports.size(); ++i)
00210   {    
00211     checkers.push_back(boost::shared_ptr<BatteryServerChecker>(new BatteryServerChecker(i, ports[i], timeout)));
00212     checkers[i]->start();                       
00213   }
00214 
00215   if (verbose)
00216     cout << "Battery monitoring started\n";
00217 
00218   ros::Rate my_rate(2);
00219   ros::Time startTime = ros::Time::now();
00220 
00221   ros::Duration min(min_duration);
00222   ros::Duration max(duration);
00223 
00224   bool all_ok = false;
00225   while (true)
00226   {
00227     my_rate.sleep();
00228     
00229     if (ros::Time::now() - startTime < min)
00230       continue;
00231 
00232     if (ros::Time::now() - startTime > max)
00233       break;
00234    
00235     bool now_ok = true;
00236     for (uint i = 0; i < checkers.size(); ++i)
00237       now_ok = checkers[i]->batteryOK() && now_ok;
00238 
00239     if (now_ok)
00240     {
00241       all_ok = true;
00242       break;
00243     }
00244   }
00245 
00246   if (verbose)
00247     cout << "Stopping battery monitors\n";
00248  
00249   for (uint i = 0; i < checkers.size(); ++i)
00250     checkers[i]->stop();
00251  
00252   if (verbose)
00253   {
00254     fprintf(stdout, "Battery status reports:\n");
00255     for (uint i = 0; i < checkers.size(); ++i)
00256       cout << checkers[i]->getStatus().c_str();
00257   }
00258 
00259   if (all_ok)
00260   {
00261     fprintf(stdout, "All batteries OK\n");
00262     return 0;
00263   }
00264 
00265   fprintf(stderr, "Not all batteries reported OK.\n");
00266   cerr << "Status: \n";
00267   for (uint i = 0; i < checkers.size(); ++i)
00268     cerr << "\tDevice " << i << ": " << (checkers[i]->batteryOK() ? string("OK") : string("Error")) << "\n";
00269   
00270   return 1;
00271 }


ocean_battery_driver
Author(s): Tully Foote, Curt Meyers
autogenerated on Tue Apr 22 2014 19:35:07