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


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