server.cpp
Go to the documentation of this file.
00001 
00002 #include <iostream>
00003 #include <vector>
00004 #include <unistd.h>
00005 #include <stdlib.h>
00006 #include <time.h>
00007 #include <boost/shared_ptr.hpp>
00008 #include <boost/bind.hpp>
00009 #include <boost/ref.hpp>
00010 #include <boost/program_options.hpp>
00011 #include <boost/thread/thread.hpp>
00012 
00013 #include <log4cxx/logger.h>
00014 
00015 #include "ocean.h"
00016 #include "ros/ros.h"
00017 #include "diagnostic_msgs/DiagnosticArray.h"
00018 #include "diagnostic_msgs/DiagnosticStatus.h"
00019 #include "diagnostic_updater/DiagnosticStatusWrapper.h"
00020 #include "rosconsole/macros_generated.h"
00021 #include "pr2_msgs/BatteryServer.h" //This is here to send a copy of the previous message
00022 
00023 using namespace std;
00024 using namespace ros;
00025 using namespace willowgarage::ocean;
00026 namespace po = boost::program_options;
00027 
00028 static const float BATTERY_TEMP_WARN = 50.0;
00029 
00030 float toFloat(const int &value)
00031 {
00032   int tmp = value;
00033   if(tmp & 0x8000)
00034     tmp = tmp - 65536;
00035   float result = tmp / 1000.0;
00036   return result;
00037 }
00038 
00039 float tempToCelcius(const int &iKelvin)
00040 {
00041   float fKelvin = (float) (iKelvin * 0.1);
00042   return (fKelvin - (float) 273.15);
00043 }
00044 
00045 class server
00046 {
00047   public:
00048 
00049     server( const int &majorID, const std::string &dev, const int debug_level = 0 ) : 
00050       majorID(majorID), debug_level(debug_level), serial_device("/dev/ttyUSB0"), stopRequest(false),
00051       has_warned_temp_alarm_(false), has_warned_no_good_(false)
00052     {
00053       std::stringstream ss;
00054 
00055       ros::NodeHandle private_handle("~");
00056 
00057       if(dev.empty())
00058       {
00059         string tmp_device;
00060         ss.str("");
00061         ss << "/dev/ttyUSB" << majorID; //create a default device based on majorID
00062         serial_device = ss.str();
00063 
00064         ss.str("");
00065         ss << "port" << majorID;
00066         bool result = private_handle.getParam( ss.str(), tmp_device );
00067         if(result == true)
00068         {
00069           ROS_INFO("Using %s from getParam.\n", ss.str().c_str());
00070           serial_device = tmp_device;
00071         }
00072         else
00073           ROS_INFO("Defaulting to: %s", serial_device.c_str());
00074 
00075       }
00076       else
00077       {
00078         ROS_INFO("Overriding device with argument: %s", dev.c_str() );
00079         serial_device = dev;
00080       }
00081 
00082       private_handle.param("lag_timeout", lag_timeout_, 60);
00083       private_handle.param("stale_timeout", stale_timeout_, 120);
00084 
00085       //
00086       //printf("device=%s  debug_level=%d\n", argv[1], atoi(argv[2]));
00087       //cout << "device=" << serial_device <<  "  debug_level=" << debug_level << endl;
00088 
00089     }
00090 
00091     void start()
00092     {
00093       myThread = boost::shared_ptr<boost::thread>(new boost::thread(boost::bind(&server::run, this)));
00094     }
00095 
00096 
00097     void stop()
00098     {
00099       stopRequest = true;
00100       myThread->join();
00101     }
00102 
00103   private:
00104 
00105     ros::NodeHandle handle;
00106     int majorID;
00107     int debug_level;
00108     std::string serial_device;
00109     volatile bool stopRequest;
00110     boost::shared_ptr<boost::thread> myThread;
00111     int lag_timeout_, stale_timeout_;
00112     bool has_warned_temp_alarm_, has_warned_no_good_;
00113 
00114     void run()
00115     {
00116       std::stringstream ss;
00117 
00118       //
00119       //  Need to make the queue size big enough that each thread can publish without
00120       //  concern that one message it quickly replaced by another threads message.
00121       //
00122       ros::Publisher pub    = handle.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 10);
00123       ros::Publisher bs2    = handle.advertise<pr2_msgs::BatteryServer2>("battery/server2", 10);
00124       ros::Publisher bs     = handle.advertise<pr2_msgs::BatteryServer>("battery/server", 10);
00125 
00126       ros::Rate rate(100);   //set the rate we scan the device for input
00127       diagnostic_msgs::DiagnosticArray msg_out;
00128       diagnostic_updater::DiagnosticStatusWrapper stat;
00129       Time lastTime, currentTime, startTime;
00130       Duration MESSAGE_TIME(2,0);    //the message output rate
00131       ocean os( majorID, debug_level);
00132       os.initialize(serial_device.c_str());
00133       //os.read_file(serial_device.c_str());
00134 
00135       pr2_msgs::BatteryServer oldserver;
00136       oldserver.battery.resize(4);
00137 
00138       lastTime = Time::now();
00139       startTime = Time::now();
00140 
00141       while(handle.ok() && (stopRequest == false))
00142       {
00143         rate.sleep();
00144         //ros::spinOnce();
00145         currentTime = Time::now();
00146 
00147         if((os.run() > 0) && ((currentTime - lastTime) > MESSAGE_TIME))
00148         {
00149 
00150           // First publish our internal data
00151           os.server.header.stamp = ros::Time::now();
00152           bs2.publish(os.server);
00153 
00154           oldserver.id = os.server.id;
00155           oldserver.lastTimeSystem = os.server.last_system_update.sec;
00156           oldserver.timeLeft = os.server.time_left.toSec()/60;
00157           oldserver.averageCharge = os.server.average_charge;
00158           oldserver.message = os.server.message;
00159           oldserver.lastTimeController = os.server.last_controller_update.sec;
00160           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;
00161           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;
00162           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;
00163           //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;
00164           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;
00165           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;
00166           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;
00167 
00168           for(int xx = 0; xx < os.server.MAX_BAT_COUNT; ++xx)
00169           {
00170             oldserver.battery[xx].lastTimeBattery = os.server.battery[xx].last_battery_update.sec;
00171             for(unsigned int yy = 0; yy < os.regListLength; ++yy)
00172             {
00173               oldserver.battery[xx].batReg[yy] = os.server.battery[xx].battery_register[yy];
00174               oldserver.battery[xx].batRegFlag[yy] = os.server.battery[xx].battery_update_flag[yy];
00175               oldserver.battery[xx].batRegTime[yy] = os.server.battery[xx].battery_register_update[yy].sec;
00176             }
00177           }
00178 
00179           oldserver.header.stamp = ros::Time::now();
00180           bs.publish(oldserver);
00181 
00182           lastTime = currentTime;
00183 
00184           stat.values.clear();
00185 
00186           ss.str("");
00187           ss << "IBPS " << majorID;
00188           stat.name = ss.str();
00189           stat.level = diagnostic_msgs::DiagnosticStatus::OK;
00190           stat.message = "OK";
00191           
00192           stat.add("Time Remaining (min)", (os.server.time_left.toSec()/60));
00193           stat.add("Average charge (percent)", os.server.average_charge );
00194           Duration elapsed = currentTime - os.server.last_system_update;
00195           stat.add("Time since update (s)", elapsed.toSec());
00196 
00197           msg_out.header.stamp = ros::Time::now();
00198           msg_out.status.push_back(stat);
00199 
00200           for(int xx = 0; xx < os.server.MAX_BAT_COUNT; ++xx)
00201           {
00202             stat.values.clear();
00203             
00204             ss.str("");
00205             ss << "Smart Battery " << majorID << "." << xx;
00206             stat.name = ss.str();
00207             stat.level = diagnostic_msgs::DiagnosticStatus::OK;
00208             stat.message = "OK";
00209 
00210             if(!os.server.battery[xx].present)
00211             {
00212               stat.add("Battery Present", "False");
00213               stat.level = diagnostic_msgs::DiagnosticStatus::ERROR;
00214               stat.message = "Not present";
00215             }
00216             else
00217             {
00218               stat.add("Battery Present", "True");
00219               stat.add("Charging", (os.server.battery[xx].charging) ? "True":"False");
00220               stat.add("Discharging", (os.server.battery[xx].discharging) ? "True":"False");
00221               stat.add("Power Present", (os.server.battery[xx].power_present) ? "True":"False");
00222               stat.add("No Good", (os.server.battery[xx].power_no_good) ? "True":"False");
00223               stat.add("Charge Inhibited", (os.server.battery[xx].inhibited) ? "True":"False");
00224 
00225               int16_t voltage = -1;
00226               int16_t design_voltage = -1;
00227               int16_t relative_charge = -1;
00228               int16_t absolute_charge = -1;
00229               
00230               for(unsigned int yy = 0; yy < os.regListLength; ++yy)
00231               {
00232                 unsigned addr = os.regList[yy].address;
00233                 if(os.server.battery[xx].battery_update_flag[addr])
00234                 {
00235                   ss.str("");
00236                   if(os.regList[yy].unit != "")
00237                     ss << os.regList[yy].name << " (" << os.regList[yy].unit << ")";
00238                   else
00239                     ss << os.regList[yy].name;
00240                   
00241                   switch(addr) {
00242                      case 0x1b:  //Address of manufactureDate
00243                         {
00244                            std::stringstream date;
00245                            date.str("");
00246 
00247                            unsigned int day = os.server.battery[xx].battery_register[addr] & 0x1F;
00248                            unsigned int month = (os.server.battery[xx].battery_register[addr] >> 5) & 0xF;
00249                            unsigned int year = (os.server.battery[xx].battery_register[addr] >> 9) + 1980;
00250                            date << month << "/" << day << "/" << year;
00251 
00252                            stat.add("Manufacture Date (MDY)", date.str());
00253                         }
00254                         break;
00255                      case 0x8:  //Address of Temperature
00256                         {
00257                            float celsius = tempToCelcius(os.server.battery[xx].battery_register[addr]);
00258                            if(celsius > BATTERY_TEMP_WARN)
00259                            {
00260                               ostringstream warn;
00261                               warn << "High Temperature Warning > " << BATTERY_TEMP_WARN << "C";
00262                               stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::WARN, warn.str());
00263                            }
00264 
00265                            stat.add("Temperature (C)", celsius);
00266                         }
00267                         break;
00268                      case 0x1c: // Serial Number
00269                         {
00270                            stat.add("Serial Number", os.server.battery[xx].battery_register[addr]);
00271 
00272                            std::stringstream hw_id;
00273                            hw_id << os.server.battery[xx].battery_register[addr];
00274 
00275                            stat.hardware_id = hw_id.str();
00276                         }
00277                         break;
00278                      case 0x09: // Voltage
00279                         voltage = os.server.battery[xx].battery_register[addr];
00280                         stat.add( ss.str(), os.server.battery[xx].battery_register[addr]);
00281                         break;
00282                      case 0x19: // Design Voltage
00283                         design_voltage = os.server.battery[xx].battery_register[addr];
00284                         stat.add( ss.str(), os.server.battery[xx].battery_register[addr]);
00285                         break;
00286                      case 0x0d: // Relative state of charge
00287                         relative_charge = os.server.battery[xx].battery_register[addr];
00288                         stat.add( ss.str(), os.server.battery[xx].battery_register[addr]);
00289                         break;
00290                      case 0x0e: // Absolute state of charge
00291                         absolute_charge = os.server.battery[xx].battery_register[addr];
00292                         stat.add( ss.str(), os.server.battery[xx].battery_register[addr]);
00293                         break;
00294                      default:
00295                         stat.add( ss.str(), os.server.battery[xx].battery_register[addr]);
00296                         break;
00297                   }
00298                 }
00299               }
00300 
00301               elapsed = currentTime - os.server.battery[xx].last_battery_update;
00302               if (os.server.battery[xx].last_battery_update >= Time(1))
00303                 stat.add("Time since update (s)", elapsed.toSec());
00304               else
00305                 stat.add("Time since update (s)", "N/A");
00306               
00307               if ( absolute_charge >= 0 && relative_charge >= 0 ) {
00308                  if( absolute_charge < relative_charge/2 ) {
00309                     stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::WARN, "Battery capacity low, please replace.");
00310                  }
00311               }
00312 
00313               if ( voltage >= 0 && design_voltage >= 0 ) {
00314                  if( voltage < design_voltage/2 ) {
00315                     stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::WARN, "Battery voltage too low, please replace.");
00316                  }
00317               }
00318 
00319               // Mark batteries as stale if they don't update
00320               // Give them "grace period" on startup to update before we report error
00321               bool updateGracePeriod = (currentTime - startTime).toSec() < stale_timeout_;
00322               if (os.server.battery[xx].last_battery_update <= Time(1) && updateGracePeriod)
00323                 stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::WARN, "Waiting for battery update");
00324               else if (stale_timeout_ > 0 && elapsed.toSec() > stale_timeout_)
00325                 stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::ERROR, "No updates");
00326               else if (lag_timeout_ > 0 && elapsed.toSec() > lag_timeout_)
00327                 stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::WARN, "Stale updates");
00328 
00329 
00330               // Warn for over temp alarm
00331               // If power present and not charging, not full, and temp >= 46C
00332               // 0x0d is "Relative State of Charge"
00333               if (os.server.battery[xx].power_present && !os.server.battery[xx].charging 
00334                   && os.server.battery[xx].battery_register[0x0d] < 90
00335                   && tempToCelcius(os.server.battery[xx].battery_register[0x8]) > 46.0)
00336                 {
00337                   stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::WARN, "Charge Inhibited, High Temperature");
00338                   if (!has_warned_temp_alarm_)
00339                     {
00340                       ROS_WARN("Over temperature alarm found on battery %d. Battery will not charge.", xx);
00341                       has_warned_temp_alarm_ = true;
00342                     }
00343                 }
00344 
00345               // Check for battery status code, not sure if this works.
00346               if (os.server.battery[xx].battery_register[0x16] & 0x1000)
00347                   stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::WARN, "Over Temp Alarm");
00348 
00349               // Report a console warning if battery is "No Good"
00350               // This may be a problem with the battery, but we're not sure.
00351               if (os.server.battery[xx].power_no_good && !has_warned_no_good_)
00352                 {
00353                   ROS_WARN("Battery %d reports \"No Good\".", xx);
00354                   has_warned_no_good_ = true;
00355                 }
00356                           
00357               msg_out.status.push_back(stat);
00358             }
00359           }
00360 
00361           pub.publish(msg_out);
00362           msg_out.status.clear();
00363 
00364         }
00365       }
00366     }
00367 
00368 };
00369 
00370 int main(int argc, char** argv)
00371 {
00372   string tmp_device;
00373   int debug_level;
00374   int max_ports;
00375 
00376   po::options_description desc("Allowed options");
00377   desc.add_options()
00378     ("help", "this help message")
00379     ("debug", po::value<int>(&debug_level)->default_value(0), "debug level")
00380     ("count", po::value<int>(&max_ports)->default_value(4), "number of ports to monitor")
00381     ("dev", po::value<string>(&tmp_device), "serial device to open");
00382 
00383   po::variables_map vm;
00384   po::store(po::parse_command_line( argc, argv, desc), vm);
00385   po::notify(vm);
00386 
00387   if( vm.count("help"))
00388   {
00389     cout << desc << "\n";
00390     return 1;
00391   }
00392 
00393   ros::init(argc, argv, "ocean_server");
00394   ros::NodeHandle handle;
00395   ros::NodeHandle private_handle("~");
00396 
00397   //majorID = serial_device.at(serial_device.length() - 1) - '0';
00398 
00399   ROSCONSOLE_AUTOINIT;
00400   log4cxx::LoggerPtr my_logger = log4cxx::Logger::getLogger(ROSCONSOLE_DEFAULT_NAME);
00401   fprintf(stderr, "Logger Name: %s\n", ROSCONSOLE_DEFAULT_NAME);
00402 
00403   if( my_logger->getLevel() == 0 )    //has anyone set our level??
00404   {
00405     // Set the ROS logger
00406     my_logger->setLevel(ros::console::g_level_lookup[ros::console::levels::Info]);
00407   }
00408 
00409   private_handle.getParam( "number_of_ports", max_ports );
00410   ROS_INFO("number_of_ports=%d", max_ports);
00411   private_handle.getParam( "debug_level", debug_level );
00412   ROS_DEBUG("debug_level=%d", debug_level);
00413 
00414   vector<server> server_list;
00415 
00416   for(int xx = 0; xx < max_ports; ++xx)
00417     server_list.push_back(server( xx, tmp_device, debug_level));
00418 
00419   for(int xx = 0; xx < max_ports; ++xx)
00420     server_list[xx].start();
00421 
00422   ros::spin(); //wait for ros to shut us down
00423 
00424 
00425   for(int xx = 0; xx < max_ports; ++xx)
00426     server_list[xx].stop();
00427 
00428 }


ocean_battery_driver
Author(s): Tully Foote, Curt Meyers
autogenerated on Thu Jun 6 2019 21:10:57