battery_check.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2010, 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 
37 
38 #include <iostream>
39 #include <string>
40 #include <vector>
41 
42 #include <boost/bind.hpp>
43 #include <boost/program_options.hpp>
44 #include <boost/thread/thread.hpp>
45 #include <boost/date_time.hpp>
46 
47 #include <ros/rate.h>
48 #include <ros/time.h>
49 
50 #include "ocean.h"
51 
52 namespace po = boost::program_options;
53 using namespace std;
54 using namespace willowgarage::ocean;
55 
57 {
58 private:
59  int id_;
60  string device_;
61  int timeout_;
63  volatile bool stopRequest;
64 
65  vector<bool> present;
66  vector<bool> inhibited;
67  vector<bool> no_good;
68  vector<ros::Time> last_update;
69 
71 
72  void run()
73  {
74  ros::Rate my_rate(50);
75 
76  while (!stopRequest)
77  {
78  os.run();
79  for (int i = 0; i < os.server.MAX_BAT_COUNT; ++i)
80  {
81  present[i] = os.server.battery[i].present;
82  inhibited[i] = os.server.battery[i].inhibited;
83  no_good[i] = os.server.battery[i].power_no_good;
84  last_update[i] = os.server.battery[i].last_battery_update;
85  }
86 
87  my_rate.sleep();
88  }
89  }
90 
91 public:
92  BatteryServerChecker(int id, const string &dev, int timeout):
93  id_(id), device_(dev), timeout_(timeout), os(id, 0), stopRequest(false)
94  {
95  os.initialize(dev);
96 
97  present.resize(os.server.MAX_BAT_COUNT);
98  inhibited.resize(os.server.MAX_BAT_COUNT);
99  no_good.resize(os.server.MAX_BAT_COUNT);
100  last_update.resize(os.server.MAX_BAT_COUNT);
101  }
102 
103  void start()
104  {
105  runThread_ = boost::shared_ptr<boost::thread>(new boost::thread(boost::bind(&BatteryServerChecker::run, this)));
106  }
107 
108  void stop()
109  {
110  stopRequest = true;
111 
112  boost::posix_time::time_duration timeout = boost::posix_time::seconds(5);
113  runThread_->timed_join(timeout);
114  }
115 
116  bool batteryOK() const
117  {
118  bool ok = true; // Checks to make sure OK
119  bool stale = false; // All batteries must have updated within timeout
120 
121  for (int i = 0; i < os.server.MAX_BAT_COUNT; ++i)
122  {
123  ok = ok && present[i];
124  stale = ((ros::Time::now() - last_update[i]).toSec() > timeout_) || stale;
125  }
126 
127  return ok && !stale;
128  }
129 
130  string getStatus() const
131  {
132  stringstream ss;
133  ss << "Port: " << device_ << "\n";
134 
135  bool ng = false;
136  for (int i = 0; i < os.server.MAX_BAT_COUNT; ++i)
137  {
138  ss << "\tBattery " << i << ":\n";
139  ss << "\t\tPresent: " << (present[i] ? "Yes" : "No") << "\n";
140  ss << "\t\tInhibited: " << (inhibited[i] ? "Yes" : "No") << "\n";
141  ss << "\t\tNo Good: " << (no_good[i] ? "Yes" : "No") << "\n";
142  ss << "\t\tHas updated: " << (last_update[i] > ros::Time(1) ? "Yes" : "No") << "\n";
143  ss << "\t\tTime Since Update: " << (ros::Time::now() - last_update[i]).toSec() << "\n";
144  ng = ng || no_good[i];
145  }
146  if (ng)
147  ss << "Warning: \"No Good\" flag enabled. This may cause battery problems.\n";
148 
149  return ss.str();
150  }
151 };
152 
153 int main(int argc, char** argv)
154 {
155  int duration, min_duration, timeout;
156  po::options_description desc("battery_check port1 port2 ... [options]");
157  desc.add_options()
158  ("help,h", "Print help message and exit")
159  ("verbose,v", "Verbose battery output")
160  ("timeout,t", po::value<int>(&timeout)->default_value(15), "Timeout before stale")
161  ("duration,d", po::value<int>(&duration)->default_value(60), "Maximum duration of test")
162  ("min-duration,m", po::value<int>(&min_duration)->default_value(20), "Minimum duration of test")
163  ("port", po::value<vector<string> >(), "Battery ports to check");
164 
165  po::positional_options_description p;
166  p.add("port", -1);
167 
168  po::variables_map vm;
169  po::store(po::command_line_parser(argc, argv).options(desc).positional(p).run(), vm);
170  po::notify(vm);
171 
172  bool verbose = vm.count("verbose");
173 
174  if (vm.count("help"))
175  {
176  cout << desc << "\n";
177  return 1;
178  }
179 
180  if (!vm.count("port"))
181  {
182  fprintf(stderr, "No ports specified. Unable to check batteries.\n");
183  return 1;
184  }
185 
186  if (min_duration < timeout)
187  {
188  fprintf(stderr, "Timeout must be greater than the minimum duration. Unable check batteries.\n");
189  return 1;
190  }
191 
192  vector<string> ports(vm["port"].as< vector<string> >());
193  vector<string>::iterator it;
194 
195  if (verbose)
196  {
197  cout << "Checking ports: ";
198  for (it = ports.begin(); it != ports.end(); ++it)
199  cout << *it << ", ";
200  cout << "\n";
201  }
202 
203  if (verbose)
204  cout << "Running battery check. Waiting for battery drivers to initialize\n";
205 
206  ros::Time::init();
207 
208  vector<boost::shared_ptr<BatteryServerChecker> > checkers;
209  for (uint i = 0; i < ports.size(); ++i)
210  {
211  checkers.push_back(boost::shared_ptr<BatteryServerChecker>(new BatteryServerChecker(i, ports[i], timeout)));
212  checkers[i]->start();
213  }
214 
215  if (verbose)
216  cout << "Battery monitoring started\n";
217 
218  ros::Rate my_rate(2);
219  ros::Time startTime = ros::Time::now();
220 
221  ros::Duration min(min_duration);
222  ros::Duration max(duration);
223 
224  bool all_ok = false;
225  while (true)
226  {
227  my_rate.sleep();
228 
229  if (ros::Time::now() - startTime < min)
230  continue;
231 
232  if (ros::Time::now() - startTime > max)
233  break;
234 
235  bool now_ok = true;
236  for (uint i = 0; i < checkers.size(); ++i)
237  now_ok = checkers[i]->batteryOK() && now_ok;
238 
239  if (now_ok)
240  {
241  all_ok = true;
242  break;
243  }
244  }
245 
246  if (verbose)
247  cout << "Stopping battery monitors\n";
248 
249  for (uint i = 0; i < checkers.size(); ++i)
250  checkers[i]->stop();
251 
252  if (verbose)
253  {
254  fprintf(stdout, "Battery status reports:\n");
255  for (uint i = 0; i < checkers.size(); ++i)
256  cout << checkers[i]->getStatus().c_str();
257  }
258 
259  if (all_ok)
260  {
261  fprintf(stdout, "All batteries OK\n");
262  return 0;
263  }
264 
265  fprintf(stderr, "Not all batteries reported OK.\n");
266  cerr << "Status: \n";
267  for (uint i = 0; i < checkers.size(); ++i)
268  cerr << "\tDevice " << i << ": " << (checkers[i]->batteryOK() ? string("OK") : string("Error")) << "\n";
269 
270  return 1;
271 }
volatile bool stopRequest
vector< ros::Time > last_update
vector< bool > inhibited
boost::shared_ptr< boost::thread > runThread_
pr2_msgs::BatteryServer2 server
Definition: ocean.h:102
string getStatus() const
void initialize(const std::string &input_dev)
Definition: ocean.cpp:98
static void init()
def default_value(type)
ROSCPP_DECL bool ok()
BatteryServerChecker(int id, const string &dev, int timeout)
bool sleep()
bool batteryOK() const
static Time now()
vector< bool > no_good
vector< bool > present
int main(int argc, char **argv)


ocean_battery_driver
Author(s): Tully Foote, Curt Meyers
autogenerated on Wed Jun 5 2019 20:40:35