43 #include <sys/types.h> 52 PowerStateEstimator::PowerStateEstimator() { }
59 bool PowerStateEstimator::canEstimate(
const ros::Time& t)
const 66 string FuelGaugePowerStateEstimator::getMethodName()
const {
return "fuel gauge"; }
72 ps.
time_remaining = obs_.getAcCount() > 0 ? obs_.getMaxTimeToFull(t) : obs_.getMinTimeToEmpty(t);
80 const std::string AdvancedPowerStateEstimator::DEFAULT_LOG_FILE =
"/var/ros/power_monitor/power.log";
82 AdvancedPowerStateEstimator::AdvancedPowerStateEstimator()
86 log_filename_ = DEFAULT_LOG_FILE;
87 node.
getParam(
"advanced_log_file", log_filename_);
88 ROS_INFO(
"Using log file: %s", log_filename_.c_str());
90 readObservations(log_);
91 ROS_INFO(
"Read %d observations", (
int) log_.size());
94 string AdvancedPowerStateEstimator::getMethodName()
const {
return "advanced"; }
99 PowerStateEstimator::recordObservation(obs);
113 log_.push_back(record);
115 saveObservation(obs);
122 bool AdvancedPowerStateEstimator::hasEverDischarged()
const 124 for (vector<LogRecord>::const_iterator i = log_.begin(); i != log_.end(); i++)
125 if ((*i).master_state == pr2_msgs::PowerBoardState::MASTER_SHUTDOWN)
136 if (log_.size() > 0 && obs_.getAcCount() == 0 && hasEverDischarged())
139 unsigned int min_rsc = 100;
140 float min_rem_cap = 999999.9;
141 for (vector<LogRecord>::const_iterator i = log_.begin(); i != log_.end(); i++)
143 if ((*i).master_state == pr2_msgs::PowerBoardState::MASTER_SHUTDOWN)
145 min_rsc =
min(min_rsc, (*i).min_relative_state_of_charge);
146 min_rem_cap =
min(min_rem_cap, (*i).total_remaining_capacity);
151 float current = obs_.getTotalPower() / obs_.getMinVoltage();
153 float actual_rem_cap = obs_.getTotalRemainingCapacity() - min_rem_cap;
154 float rem_hours = actual_rem_cap / -current;
156 ROS_DEBUG(
"current reported relative state of charge: %d", obs_.getMinRelativeStateOfCharge());
157 ROS_DEBUG(
"minimum reported remaining capacity: %d", (
int) min_rem_cap);
158 ROS_DEBUG(
"minimum reported relative state of charge: %d", min_rsc);
159 ROS_DEBUG(
"current: %.2f", current);
160 ROS_DEBUG(
"report remaining capacity: %f", obs_.getTotalRemainingCapacity());
161 ROS_DEBUG(
"time remaining: %.2f mins", rem_hours * 60);
164 ps.
relative_capacity = int(100 * (obs_.getMinRelativeStateOfCharge() - min_rsc) / (100.0 - min_rsc));
169 ROS_DEBUG(
"No history (resorting to fuel gauge)");
170 ROS_DEBUG(
"AC count: %d", obs_.getAcCount());
171 ROS_DEBUG(
"current reported relative state of charge: %d", obs_.getMinRelativeStateOfCharge());
172 ROS_DEBUG(
"maximum reported time-to-full: %d", obs_.getMaxTimeToFull(t).sec);
173 ROS_DEBUG(
"minimum reported time-to-empty: %d", obs_.getMinTimeToEmpty(t).sec);
175 ps.
time_remaining = obs_.getAcCount() > 0 ? obs_.getMaxTimeToFull(t) : obs_.getMinTimeToEmpty(t);
182 void AdvancedPowerStateEstimator::tokenize(
const string& str, vector<string>& tokens,
const string& delimiters)
184 string::size_type last_pos = str.find_first_not_of(delimiters, 0);
185 string::size_type pos = str.find_first_of(delimiters, last_pos);
187 while (string::npos != pos || string::npos != last_pos)
189 tokens.push_back(str.substr(last_pos, pos - last_pos));
191 last_pos = str.find_first_not_of(delimiters, pos);
192 pos = str.find_first_of(delimiters, last_pos);
196 bool AdvancedPowerStateEstimator::readObservations(vector<LogRecord>& log)
198 ifstream
f(log_filename_.c_str(), ios::in);
207 ROS_ERROR(
"Error reading header from log file: %s", log_filename_.c_str());
220 vector<string> tokens;
221 tokenize(line, tokens,
",");
225 if (tokens.size() == 7)
228 record.
sec = boost::lexical_cast<uint32_t>(tokens[0]);
229 record.
master_state = boost::lexical_cast<
int>(tokens[1]);
230 record.
charging = boost::lexical_cast<
int>(tokens[2]);
231 record.
total_power = boost::lexical_cast<
float>(tokens[3]);
232 record.
min_voltage = boost::lexical_cast<
float>(tokens[4]);
235 log.push_back(record);
239 catch (
const boost::bad_lexical_cast& ex) { }
241 ROS_DEBUG(
"Invalid line %d in log file: %s.", line_num, log_filename_.c_str());
253 FILE*
f = fopen(log_filename_.c_str(),
"r");
262 size_t last_dir_sep = log_filename_.rfind(
"/");
263 if (last_dir_sep == string::npos)
265 ROS_ERROR(
"Invalid log file: %s", log_filename_.c_str());
269 string log_dir = log_filename_.substr(0, last_dir_sep);
272 if (stat(log_dir.c_str(), &s) != 0)
275 ROS_INFO(
"Creating log file directory: %s", log_dir.c_str());
277 mode_t old_umask = umask(0);
278 int res = mkdir(log_dir.c_str(), S_IRWXU | S_IRWXG | S_IRWXO);
282 ROS_ERROR(
"Error creating power monitor log file directory");
289 mode_t old_umask = umask(0);
290 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);
294 ROS_ERROR(
"Error opening log file %s: %s", log_filename_.c_str(), strerror(errno));
300 ROS_ERROR(
"Error opening log file for appending: %s", log_filename_.c_str());
306 fprintf(f,
"secs,master_state,charging,total_power,min_voltage,min_relative_state_of_charge,total_remaining_capacity\n");
307 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());
ros::Duration time_remaining
unsigned int relative_capacity
float total_remaining_capacity
unsigned int min_relative_state_of_charge
float getMinVoltage() const
float getTotalPower() const
int8_t getMasterState() const
unsigned int getAcCount() const
unsigned int getMinRelativeStateOfCharge() const
bool getParam(const std::string &key, std::string &s) const
ROSCPP_DECL bool exists(const std::string &service_name, bool print_failure_reason)
const ros::Time & getStamp() const
float getTotalRemainingCapacity() const
const std::vector< BatteryObservation > & getBatteries() const