power_state_estimator.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, 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 
00035 #include "power_state_estimator.h"
00036 
00037 #include <errno.h>
00038 #include <stdlib.h>
00039 #include <fstream>
00040 #include <iostream>
00041 #include <sys/file.h>
00042 #include <sys/stat.h>
00043 #include <sys/types.h>
00044 
00045 #include "ros/ros.h"
00046 
00047 using namespace std;
00048 using namespace power_monitor;
00049 
00050 // PowerStateEstimator
00051 
00052 PowerStateEstimator::PowerStateEstimator() { }
00053 
00054 void PowerStateEstimator::recordObservation(const PowerObservation& obs)
00055 {
00056     obs_ = obs;
00057 }
00058 
00059 bool PowerStateEstimator::canEstimate(const ros::Time& t) const
00060 {
00061     return t <= ros::Time::now() && obs_.getBatteries().size() > 0;
00062 }
00063 
00064 // FuelGaugePowerStateEstimator
00065 
00066 string                    FuelGaugePowerStateEstimator::getMethodName() const { return "fuel gauge"; }
00067 PowerStateEstimator::Type FuelGaugePowerStateEstimator::getMethodType() const { return FuelGauge;    }
00068 
00069 PowerStateEstimate FuelGaugePowerStateEstimator::estimate(const ros::Time& t)
00070 {
00071     PowerStateEstimate ps;
00072     ps.time_remaining    = obs_.getAcCount() > 0 ? obs_.getMaxTimeToFull(t) : obs_.getMinTimeToEmpty(t);
00073     ps.relative_capacity = obs_.getMinRelativeStateOfCharge();
00074 
00075     return ps;
00076 }
00077 
00078 // AdvancedPowerStateEstimator
00079 
00080 const std::string AdvancedPowerStateEstimator::DEFAULT_LOG_FILE = "/var/ros/power_monitor/power.log";
00081 
00082 AdvancedPowerStateEstimator::AdvancedPowerStateEstimator()
00083 {
00084     ros::NodeHandle node("~");
00085 
00086     log_filename_ = DEFAULT_LOG_FILE;
00087     node.getParam("advanced_log_file", log_filename_);
00088     ROS_INFO("Using log file: %s", log_filename_.c_str());
00089 
00090     readObservations(log_);
00091     ROS_INFO("Read %d observations", (int) log_.size());
00092 }
00093 
00094 string                    AdvancedPowerStateEstimator::getMethodName() const { return "advanced"; }
00095 PowerStateEstimator::Type AdvancedPowerStateEstimator::getMethodType() const { return Advanced;   }
00096 
00097 void AdvancedPowerStateEstimator::recordObservation(const PowerObservation& obs)
00098 {
00099     PowerStateEstimator::recordObservation(obs);
00100 
00101     // Ignore any observation with less than 16 batteries
00102     if (obs.getBatteries().size() != 16)
00103         return;
00104 
00105     LogRecord record;
00106     record.sec                          = obs.getStamp().sec;
00107     record.master_state                 = obs.getMasterState();
00108     record.charging                     = obs.getAcCount();
00109     record.total_power                  = obs.getTotalPower();
00110     record.min_voltage                  = obs.getMinVoltage();
00111     record.min_relative_state_of_charge = obs.getMinRelativeStateOfCharge();
00112     record.total_remaining_capacity     = obs.getTotalRemainingCapacity();
00113     log_.push_back(record);
00114 
00115     saveObservation(obs);
00116 }
00117 
00122 bool AdvancedPowerStateEstimator::hasEverDischarged() const
00123 {
00124     for (vector<LogRecord>::const_iterator i = log_.begin(); i != log_.end(); i++)
00125         if ((*i).master_state == pr2_msgs::PowerBoardState::MASTER_SHUTDOWN)
00126             return true;
00127 
00128     return false;
00129 }
00130 
00131 PowerStateEstimate AdvancedPowerStateEstimator::estimate(const ros::Time& t)
00132 {
00133     PowerStateEstimate ps;
00134 
00135     // If we have history of the batteries being completely drained, then offset our estimate by the minimum reported capacity
00136     if (log_.size() > 0 && obs_.getAcCount() == 0 && hasEverDischarged())
00137     {
00138         // Get the minimum remaining capacity reported ever
00139         unsigned int min_rsc     = 100;
00140         float        min_rem_cap = 999999.9;
00141         for (vector<LogRecord>::const_iterator i = log_.begin(); i != log_.end(); i++)
00142         {
00143             if ((*i).master_state == pr2_msgs::PowerBoardState::MASTER_SHUTDOWN)
00144             {
00145                 min_rsc     = min(min_rsc,     (*i).min_relative_state_of_charge);
00146                 min_rem_cap = min(min_rem_cap, (*i).total_remaining_capacity);
00147             }
00148         }
00149 
00150         // @todo: should filter the noisy current
00151         float current        = obs_.getTotalPower() / obs_.getMinVoltage();
00152 
00153         float actual_rem_cap = obs_.getTotalRemainingCapacity() - min_rem_cap;
00154         float rem_hours      = actual_rem_cap / -current;
00155 
00156         ROS_DEBUG("current reported relative state of charge: %d", obs_.getMinRelativeStateOfCharge());
00157         ROS_DEBUG("minimum reported remaining capacity: %d", (int) min_rem_cap);
00158         ROS_DEBUG("minimum reported relative state of charge: %d", min_rsc);
00159         ROS_DEBUG("current: %.2f", current);
00160         ROS_DEBUG("report remaining capacity: %f", obs_.getTotalRemainingCapacity());
00161         ROS_DEBUG("time remaining: %.2f mins", rem_hours * 60);
00162 
00163         ps.time_remaining = ros::Duration(rem_hours * 60 * 60);
00164         ps.relative_capacity = int(100 * (obs_.getMinRelativeStateOfCharge() - min_rsc) / (100.0 - min_rsc));
00165     }
00166     else
00167     {
00168         // No history. Resort to fuel gauge method
00169         ROS_DEBUG("No history (resorting to fuel gauge)");
00170         ROS_DEBUG("AC count: %d", obs_.getAcCount());
00171         ROS_DEBUG("current reported relative state of charge: %d", obs_.getMinRelativeStateOfCharge());
00172         ROS_DEBUG("maximum reported time-to-full: %d", obs_.getMaxTimeToFull(t).sec);
00173         ROS_DEBUG("minimum reported time-to-empty: %d", obs_.getMinTimeToEmpty(t).sec);
00174 
00175         ps.time_remaining = obs_.getAcCount() > 0 ? obs_.getMaxTimeToFull(t) : obs_.getMinTimeToEmpty(t);
00176         ps.relative_capacity = obs_.getMinRelativeStateOfCharge();
00177     }
00178 
00179     return ps;
00180 }
00181 
00182 void AdvancedPowerStateEstimator::tokenize(const string& str, vector<string>& tokens, const string& delimiters)
00183 {
00184     string::size_type last_pos = str.find_first_not_of(delimiters, 0);
00185     string::size_type pos      = str.find_first_of(delimiters, last_pos);
00186 
00187     while (string::npos != pos || string::npos != last_pos)
00188     {
00189         tokens.push_back(str.substr(last_pos, pos - last_pos));
00190 
00191         last_pos = str.find_first_not_of(delimiters, pos);
00192         pos      = str.find_first_of(delimiters, last_pos);
00193     }
00194 }
00195 
00196 bool AdvancedPowerStateEstimator::readObservations(vector<LogRecord>& log)
00197 {
00198     ifstream f(log_filename_.c_str(), ios::in);
00199     if (!f.is_open())
00200         return false;
00201 
00202     // Consume header line
00203     string line;
00204     getline(f, line);
00205     if (!f.good())
00206     {
00207         ROS_ERROR("Error reading header from log file: %s", log_filename_.c_str());
00208         return false;
00209     }
00210 
00211     int line_num = 0;
00212     while (true)
00213     {
00214         line_num++;
00215 
00216         getline(f, line);
00217         if (!f.good())
00218             break;
00219 
00220         vector<string> tokens;
00221         tokenize(line, tokens, ",");
00222 
00223         try
00224         {
00225             if (tokens.size() == 7)
00226             {
00227                 LogRecord record;
00228                 record.sec                          = boost::lexical_cast<uint32_t>(tokens[0]);
00229                 record.master_state                 = boost::lexical_cast<int>(tokens[1]);
00230                 record.charging                     = boost::lexical_cast<int>(tokens[2]);
00231                 record.total_power                  = boost::lexical_cast<float>(tokens[3]);
00232                 record.min_voltage                  = boost::lexical_cast<float>(tokens[4]);
00233                 record.min_relative_state_of_charge = boost::lexical_cast<unsigned int>(tokens[5]);
00234                 record.total_remaining_capacity     = boost::lexical_cast<float>(tokens[6]);
00235                 log.push_back(record);
00236                 continue;
00237             }
00238         }
00239         catch (const boost::bad_lexical_cast& ex) { }
00240 
00241         ROS_DEBUG("Invalid line %d in log file: %s.", line_num, log_filename_.c_str());
00242     }
00243 
00244     f.close();
00245 
00246     return true;
00247 }
00248 
00249 bool AdvancedPowerStateEstimator::saveObservation(const PowerObservation& obs) const
00250 {
00251     // Check if the log file exists
00252     bool exists = false;
00253     FILE* f = fopen(log_filename_.c_str(), "r");
00254     if (f) {
00255         fclose(f);
00256         exists = true;
00257     }
00258 
00259     if (!exists)
00260     {
00261         // Check if the directory exists
00262         size_t last_dir_sep = log_filename_.rfind("/");
00263         if (last_dir_sep == string::npos)
00264         {
00265             ROS_ERROR("Invalid log file: %s", log_filename_.c_str());
00266             return false;
00267         }
00268 
00269         string log_dir = log_filename_.substr(0, last_dir_sep);
00270 
00271         struct stat s;
00272         if (stat(log_dir.c_str(), &s) != 0)
00273         {
00274             // Directory doesn't exist - create
00275             ROS_INFO("Creating log file directory: %s", log_dir.c_str());
00276 
00277             mode_t old_umask = umask(0);
00278             int res = mkdir(log_dir.c_str(), S_IRWXU | S_IRWXG | S_IRWXO);
00279             umask(old_umask);
00280             if (res != 0)      // create directory with permissions: rwxrwxrwx
00281             {
00282                 ROS_ERROR("Error creating power monitor log file directory");
00283                 return false;
00284             }
00285         }
00286     }
00287 
00288     // Open the log file for appending
00289     mode_t old_umask = umask(0);
00290     int fd = open(log_filename_.c_str(), O_WRONLY | O_APPEND | O_CREAT, S_IRUSR | S_IWUSR | S_IRGRP | S_IWGRP | S_IROTH | S_IWOTH);
00291     umask(old_umask);
00292     if (fd == -1)
00293     {
00294         ROS_ERROR("Error opening log file %s: %s", log_filename_.c_str(), strerror(errno));
00295         return false;
00296     }
00297     f = fdopen(fd, "a");
00298     if (f == NULL)
00299     {
00300         ROS_ERROR("Error opening log file for appending: %s", log_filename_.c_str());
00301         return false;
00302     }
00303 
00304     flock(fd, LOCK_SH);
00305     if (!exists)
00306         fprintf(f, "secs,master_state,charging,total_power,min_voltage,min_relative_state_of_charge,total_remaining_capacity\n");
00307     fprintf(f, "%d,%d,%d,%.3f,%.3f,%d,%.3f\n", obs.getStamp().sec, (int) obs.getMasterState(), obs.getAcCount(), obs.getTotalPower(), obs.getMinVoltage(), obs.getMinRelativeStateOfCharge(), obs.getTotalRemainingCapacity());
00308     fclose(f);
00309     close(fd);
00310 
00311     return true;
00312 }


power_monitor
Author(s): Tim Field, Curt Meyers
autogenerated on Tue Apr 22 2014 19:35:04