server.cpp
Go to the documentation of this file.
1 
2 #include <iostream>
3 #include <vector>
4 #include <unistd.h>
5 #include <stdlib.h>
6 #include <time.h>
7 #include <boost/shared_ptr.hpp>
8 #include <boost/bind.hpp>
9 #include <boost/ref.hpp>
10 #include <boost/program_options.hpp>
11 #include <boost/thread/thread.hpp>
12 
13 #include <log4cxx/logger.h>
14 
15 #include "ocean.h"
16 #include "ros/ros.h"
17 #include "diagnostic_msgs/DiagnosticArray.h"
18 #include "diagnostic_msgs/DiagnosticStatus.h"
21 #include "pr2_msgs/BatteryServer.h" //This is here to send a copy of the previous message
22 
23 using namespace std;
24 using namespace ros;
25 using namespace willowgarage::ocean;
26 namespace po = boost::program_options;
27 
28 static const float BATTERY_TEMP_WARN = 50.0;
29 
30 float toFloat(const int &value)
31 {
32  int tmp = value;
33  if(tmp & 0x8000)
34  tmp = tmp - 65536;
35  float result = tmp / 1000.0;
36  return result;
37 }
38 
39 float tempToCelcius(const int &iKelvin)
40 {
41  float fKelvin = (float) (iKelvin * 0.1);
42  return (fKelvin - (float) 273.15);
43 }
44 
45 class server
46 {
47  public:
48 
49  server( const int &majorID, const std::string &dev, const int debug_level = 0 ) :
50  majorID(majorID), debug_level(debug_level), serial_device("/dev/ttyUSB0"), stopRequest(false),
51  has_warned_temp_alarm_(false), has_warned_no_good_(false)
52  {
53  std::stringstream ss;
54 
55  ros::NodeHandle private_handle("~");
56 
57  if(dev.empty())
58  {
59  string tmp_device;
60  ss.str("");
61  ss << "/dev/ttyUSB" << majorID; //create a default device based on majorID
62  serial_device = ss.str();
63 
64  ss.str("");
65  ss << "port" << majorID;
66  bool result = private_handle.getParam( ss.str(), tmp_device );
67  if(result == true)
68  {
69  ROS_INFO("Using %s from getParam.\n", ss.str().c_str());
70  serial_device = tmp_device;
71  }
72  else
73  ROS_INFO("Defaulting to: %s", serial_device.c_str());
74 
75  }
76  else
77  {
78  ROS_INFO("Overriding device with argument: %s", dev.c_str() );
79  serial_device = dev;
80  }
81 
82  private_handle.param("lag_timeout", lag_timeout_, 60);
83  private_handle.param("stale_timeout", stale_timeout_, 120);
84 
85  //
86  //printf("device=%s debug_level=%d\n", argv[1], atoi(argv[2]));
87  //cout << "device=" << serial_device << " debug_level=" << debug_level << endl;
88 
89  }
90 
91  void start()
92  {
93  myThread = boost::shared_ptr<boost::thread>(new boost::thread(boost::bind(&server::run, this)));
94  }
95 
96 
97  void stop()
98  {
99  stopRequest = true;
100  myThread->join();
101  }
102 
103  private:
104 
106  int majorID;
108  std::string serial_device;
109  volatile bool stopRequest;
111  int lag_timeout_, stale_timeout_;
112  bool has_warned_temp_alarm_, has_warned_no_good_;
113 
114  void run()
115  {
116  std::stringstream ss;
117 
118  //
119  // Need to make the queue size big enough that each thread can publish without
120  // concern that one message it quickly replaced by another threads message.
121  //
122  ros::Publisher pub = handle.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 10);
123  ros::Publisher bs2 = handle.advertise<pr2_msgs::BatteryServer2>("battery/server2", 10);
124  ros::Publisher bs = handle.advertise<pr2_msgs::BatteryServer>("battery/server", 10);
125 
126  ros::Rate rate(100); //set the rate we scan the device for input
127  diagnostic_msgs::DiagnosticArray msg_out;
129  Time lastTime, currentTime, startTime;
130  Duration MESSAGE_TIME(2,0); //the message output rate
131  ocean os( majorID, debug_level);
132  os.initialize(serial_device.c_str());
133  //os.read_file(serial_device.c_str());
134 
135  pr2_msgs::BatteryServer oldserver;
136  oldserver.battery.resize(4);
137 
138  lastTime = Time::now();
139  startTime = Time::now();
140 
141  while(handle.ok() && (stopRequest == false))
142  {
143  rate.sleep();
144  //ros::spinOnce();
145  currentTime = Time::now();
146 
147  if((os.run() > 0) && ((currentTime - lastTime) > MESSAGE_TIME))
148  {
149 
150  // First publish our internal data
151  os.server.header.stamp = ros::Time::now();
152  bs2.publish(os.server);
153 
154  oldserver.id = os.server.id;
155  oldserver.lastTimeSystem = os.server.last_system_update.sec;
156  oldserver.timeLeft = os.server.time_left.toSec()/60;
157  oldserver.averageCharge = os.server.average_charge;
158  oldserver.message = os.server.message;
159  oldserver.lastTimeController = os.server.last_controller_update.sec;
160  oldserver.present = os.server.battery[0].present * 1 + os.server.battery[1].present * 2 + os.server.battery[2].present * 4 + os.server.battery[3].present * 8;
161  oldserver.charging = os.server.battery[0].charging * 1 + os.server.battery[1].charging * 2 + os.server.battery[2].charging * 4 + os.server.battery[3].charging * 8;
162  oldserver.discharging = os.server.battery[0].discharging * 1 + os.server.battery[1].discharging * 2 + os.server.battery[2].discharging * 4 + os.server.battery[3].discharging * 8;
163  //oldserver.reserved = os.server.battery[0].reserved * 1 + os.server.battery[1].reserved * 2 + os.server.battery[2].reserved * 4 + os.server.battery[3].reserved * 8;
164  oldserver.powerPresent = os.server.battery[0].power_present * 1 + os.server.battery[1].power_present * 2 + os.server.battery[2].power_present * 4 + os.server.battery[3].power_present * 8;
165  oldserver.powerNG = os.server.battery[0].power_no_good * 1 + os.server.battery[1].power_no_good * 2 + os.server.battery[2].power_no_good * 4 + os.server.battery[3].power_no_good * 8;
166  oldserver.inhibited = os.server.battery[0].inhibited * 1 + os.server.battery[1].inhibited * 2 + os.server.battery[2].inhibited * 4 + os.server.battery[3].inhibited * 8;
167 
168  for(int xx = 0; xx < os.server.MAX_BAT_COUNT; ++xx)
169  {
170  oldserver.battery[xx].lastTimeBattery = os.server.battery[xx].last_battery_update.sec;
171  for(unsigned int yy = 0; yy < os.regListLength; ++yy)
172  {
173  oldserver.battery[xx].batReg[yy] = os.server.battery[xx].battery_register[yy];
174  oldserver.battery[xx].batRegFlag[yy] = os.server.battery[xx].battery_update_flag[yy];
175  oldserver.battery[xx].batRegTime[yy] = os.server.battery[xx].battery_register_update[yy].sec;
176  }
177  }
178 
179  oldserver.header.stamp = ros::Time::now();
180  bs.publish(oldserver);
181 
182  lastTime = currentTime;
183 
184  stat.values.clear();
185 
186  ss.str("");
187  ss << "IBPS " << majorID;
188  stat.name = ss.str();
189  stat.level = diagnostic_msgs::DiagnosticStatus::OK;
190  stat.message = "OK";
191 
192  stat.add("Time Remaining (min)", (os.server.time_left.toSec()/60));
193  stat.add("Average charge (percent)", os.server.average_charge );
194  Duration elapsed = currentTime - os.server.last_system_update;
195  stat.add("Time since update (s)", elapsed.toSec());
196 
197  msg_out.header.stamp = ros::Time::now();
198  msg_out.status.push_back(stat);
199 
200  for(int xx = 0; xx < os.server.MAX_BAT_COUNT; ++xx)
201  {
202  stat.values.clear();
203 
204  ss.str("");
205  ss << "Smart Battery " << majorID << "." << xx;
206  stat.name = ss.str();
207  stat.level = diagnostic_msgs::DiagnosticStatus::OK;
208  stat.message = "OK";
209 
210  if(!os.server.battery[xx].present)
211  {
212  stat.add("Battery Present", "False");
213  stat.level = diagnostic_msgs::DiagnosticStatus::ERROR;
214  stat.message = "Not present";
215  }
216  else
217  {
218  stat.add("Battery Present", "True");
219  stat.add("Charging", (os.server.battery[xx].charging) ? "True":"False");
220  stat.add("Discharging", (os.server.battery[xx].discharging) ? "True":"False");
221  stat.add("Power Present", (os.server.battery[xx].power_present) ? "True":"False");
222  stat.add("No Good", (os.server.battery[xx].power_no_good) ? "True":"False");
223  stat.add("Charge Inhibited", (os.server.battery[xx].inhibited) ? "True":"False");
224 
225  int16_t voltage = -1;
226  int16_t design_voltage = -1;
227  int16_t relative_charge = -1;
228  int16_t absolute_charge = -1;
229 
230  for(unsigned int yy = 0; yy < os.regListLength; ++yy)
231  {
232  unsigned addr = os.regList[yy].address;
233  if(os.server.battery[xx].battery_update_flag[addr])
234  {
235  ss.str("");
236  if(os.regList[yy].unit != "")
237  ss << os.regList[yy].name << " (" << os.regList[yy].unit << ")";
238  else
239  ss << os.regList[yy].name;
240 
241  switch(addr) {
242  case 0x1b: //Address of manufactureDate
243  {
244  std::stringstream date;
245  date.str("");
246 
247  unsigned int day = os.server.battery[xx].battery_register[addr] & 0x1F;
248  unsigned int month = (os.server.battery[xx].battery_register[addr] >> 5) & 0xF;
249  unsigned int year = (os.server.battery[xx].battery_register[addr] >> 9) + 1980;
250  date << month << "/" << day << "/" << year;
251 
252  stat.add("Manufacture Date (MDY)", date.str());
253  }
254  break;
255  case 0x8: //Address of Temperature
256  {
257  float celsius = tempToCelcius(os.server.battery[xx].battery_register[addr]);
258  if(celsius > BATTERY_TEMP_WARN)
259  {
260  ostringstream warn;
261  warn << "High Temperature Warning > " << BATTERY_TEMP_WARN << "C";
262  stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::WARN, warn.str());
263  }
264 
265  stat.add("Temperature (C)", celsius);
266  }
267  break;
268  case 0x1c: // Serial Number
269  {
270  stat.add("Serial Number", os.server.battery[xx].battery_register[addr]);
271 
272  std::stringstream hw_id;
273  hw_id << os.server.battery[xx].battery_register[addr];
274 
275  stat.hardware_id = hw_id.str();
276  }
277  break;
278  case 0x09: // Voltage
279  voltage = os.server.battery[xx].battery_register[addr];
280  stat.add( ss.str(), os.server.battery[xx].battery_register[addr]);
281  break;
282  case 0x19: // Design Voltage
283  design_voltage = os.server.battery[xx].battery_register[addr];
284  stat.add( ss.str(), os.server.battery[xx].battery_register[addr]);
285  break;
286  case 0x0d: // Relative state of charge
287  relative_charge = os.server.battery[xx].battery_register[addr];
288  stat.add( ss.str(), os.server.battery[xx].battery_register[addr]);
289  break;
290  case 0x0e: // Absolute state of charge
291  absolute_charge = os.server.battery[xx].battery_register[addr];
292  stat.add( ss.str(), os.server.battery[xx].battery_register[addr]);
293  break;
294  default:
295  stat.add( ss.str(), os.server.battery[xx].battery_register[addr]);
296  break;
297  }
298  }
299  }
300 
301  elapsed = currentTime - os.server.battery[xx].last_battery_update;
302  if (os.server.battery[xx].last_battery_update >= Time(1))
303  stat.add("Time since update (s)", elapsed.toSec());
304  else
305  stat.add("Time since update (s)", "N/A");
306 
307  if ( absolute_charge >= 0 && relative_charge >= 0 ) {
308  if( absolute_charge < relative_charge/2 ) {
309  stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::WARN, "Battery capacity low, please replace.");
310  }
311  }
312 
313  if ( voltage >= 0 && design_voltage >= 0 ) {
314  if( voltage < design_voltage/2 ) {
315  stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::WARN, "Battery voltage too low, please replace.");
316  }
317  }
318 
319  // Mark batteries as stale if they don't update
320  // Give them "grace period" on startup to update before we report error
321  bool updateGracePeriod = (currentTime - startTime).toSec() < stale_timeout_;
322  if (os.server.battery[xx].last_battery_update <= Time(1) && updateGracePeriod)
323  stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::WARN, "Waiting for battery update");
324  else if (stale_timeout_ > 0 && elapsed.toSec() > stale_timeout_)
325  stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::ERROR, "No updates");
326  else if (lag_timeout_ > 0 && elapsed.toSec() > lag_timeout_)
327  stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::WARN, "Stale updates");
328 
329 
330  // Warn for over temp alarm
331  // If power present and not charging, not full, and temp >= 46C
332  // 0x0d is "Relative State of Charge"
333  if (os.server.battery[xx].power_present && !os.server.battery[xx].charging
334  && os.server.battery[xx].battery_register[0x0d] < 90
335  && tempToCelcius(os.server.battery[xx].battery_register[0x8]) > 46.0)
336  {
337  stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::WARN, "Charge Inhibited, High Temperature");
338  if (!has_warned_temp_alarm_)
339  {
340  ROS_WARN("Over temperature alarm found on battery %d. Battery will not charge.", xx);
341  has_warned_temp_alarm_ = true;
342  }
343  }
344 
345  // Check for battery status code, not sure if this works.
346  if (os.server.battery[xx].battery_register[0x16] & 0x1000)
347  stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::WARN, "Over Temp Alarm");
348 
349  // Report a console warning if battery is "No Good"
350  // This may be a problem with the battery, but we're not sure.
351  if (os.server.battery[xx].power_no_good && !has_warned_no_good_)
352  {
353  ROS_WARN("Battery %d reports \"No Good\".", xx);
354  has_warned_no_good_ = true;
355  }
356 
357  msg_out.status.push_back(stat);
358  }
359  }
360 
361  pub.publish(msg_out);
362  msg_out.status.clear();
363 
364  }
365  }
366  }
367 
368 };
369 
370 int main(int argc, char** argv)
371 {
372  string tmp_device;
373  int debug_level;
374  int max_ports;
375 
376  po::options_description desc("Allowed options");
377  desc.add_options()
378  ("help", "this help message")
379  ("debug", po::value<int>(&debug_level)->default_value(0), "debug level")
380  ("count", po::value<int>(&max_ports)->default_value(4), "number of ports to monitor")
381  ("dev", po::value<string>(&tmp_device), "serial device to open");
382 
383  po::variables_map vm;
384  po::store(po::parse_command_line( argc, argv, desc), vm);
385  po::notify(vm);
386 
387  if( vm.count("help"))
388  {
389  cout << desc << "\n";
390  return 1;
391  }
392 
393  ros::init(argc, argv, "ocean_server");
394  ros::NodeHandle handle;
395  ros::NodeHandle private_handle("~");
396 
397  //majorID = serial_device.at(serial_device.length() - 1) - '0';
398 
400  log4cxx::LoggerPtr my_logger = log4cxx::Logger::getLogger(ROSCONSOLE_DEFAULT_NAME);
401  fprintf(stderr, "Logger Name: %s\n", ROSCONSOLE_DEFAULT_NAME);
402 
403  if( my_logger->getLevel() == 0 ) //has anyone set our level??
404  {
405  // Set the ROS logger
406  my_logger->setLevel(ros::console::g_level_lookup[ros::console::levels::Info]);
407  }
408 
409  private_handle.getParam( "number_of_ports", max_ports );
410  ROS_INFO("number_of_ports=%d", max_ports);
411  private_handle.getParam( "debug_level", debug_level );
412  ROS_DEBUG("debug_level=%d", debug_level);
413 
414  vector<server> server_list;
415 
416  for(int xx = 0; xx < max_ports; ++xx)
417  server_list.push_back(server( xx, tmp_device, debug_level));
418 
419  for(int xx = 0; xx < max_ports; ++xx)
420  server_list[xx].start();
421 
422  ros::spin(); //wait for ros to shut us down
423 
424 
425  for(int xx = 0; xx < max_ports; ++xx)
426  server_list[xx].stop();
427 
428 }
static const unsigned regListLength
Definition: ocean.h:100
void start()
Definition: server.cpp:91
const std::string name
Definition: ocean.h:32
ROSCPP_DECL void start()
void publish(const boost::shared_ptr< M > &message) const
int main(int argc, char **argv)
Definition: server.cpp:370
pr2_msgs::BatteryServer2 server
Definition: ocean.h:102
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
volatile bool stopRequest
Definition: server.cpp:109
void run()
Definition: server.cpp:114
float tempToCelcius(const int &iKelvin)
Definition: server.cpp:39
server(const int &majorID, const std::string &dev, const int debug_level=0)
Definition: server.cpp:49
#define ROSCONSOLE_AUTOINIT
float toFloat(const int &value)
Definition: server.cpp:30
#define ROS_WARN(...)
ros::NodeHandle handle
Definition: server.cpp:105
void initialize(const std::string &input_dev)
Definition: ocean.cpp:98
void stop()
Definition: server.cpp:97
const std::string unit
Definition: ocean.h:33
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
std::string serial_device
Definition: server.cpp:108
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ROSCPP_DECL void spin()
static const float BATTERY_TEMP_WARN
Definition: server.cpp:28
void mergeSummary(unsigned char lvl, const std::string s)
int majorID
Definition: server.cpp:106
bool getParam(const std::string &key, std::string &s) const
bool has_warned_temp_alarm_
Definition: server.cpp:112
static Time now()
boost::shared_ptr< boost::thread > myThread
Definition: server.cpp:110
bool ok() const
void add(const std::string &key, const T &val)
int debug_level
Definition: server.cpp:107
#define ROSCONSOLE_DEFAULT_NAME
int stale_timeout_
Definition: server.cpp:111
static const struct regPair regList[]
Definition: ocean.h:99
#define ROS_DEBUG(...)


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