observation.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 <stdlib.h>
00038 #include <fstream>
00039 #include <iostream>
00040 
00041 #include "ros/ros.h"
00042 
00043 using namespace std;
00044 using namespace power_monitor;
00045 
00046 // PowerObservation
00047 
00048 PowerObservation::PowerObservation() { }
00049 
00050 PowerObservation::PowerObservation(const ros::Time& stamp, int8_t master_state, const vector<BatteryObservation>& batteries) : stamp_(stamp), master_state_(master_state), batteries_(batteries) { }
00051 
00052 const ros::Time&                  PowerObservation::getStamp()       const { return stamp_;        }
00053 int8_t                            PowerObservation::getMasterState() const { return master_state_; }
00054 const vector<BatteryObservation>& PowerObservation::getBatteries()   const { return batteries_;    }
00055 
00056 unsigned int PowerObservation::getAcCount() const
00057 {
00058     unsigned int ac_count = 0;
00059     for (unsigned int i = 0; i < batteries_.size(); i++)
00060         if (batteries_[i].isAcPresent())
00061             ac_count++;
00062 
00063     return ac_count;
00064 }
00065 
00066 float PowerObservation::getTotalPower() const
00067 {
00068     float total_power = 0.0f;
00069     for (unsigned int i = 0; i < batteries_.size(); i++)
00070         total_power += batteries_[i].getPower();
00071 
00072     return total_power;
00073 }
00074 
00078 float PowerObservation::getMinVoltage() const
00079 {
00080     float min_voltage = 9999.9f;
00081     for (unsigned int i = 0; i < batteries_.size(); i++)
00082         min_voltage = min(min_voltage, batteries_[i].getVoltage());
00083 
00084     return min_voltage;
00085 }
00086 
00090 unsigned int PowerObservation::getMinRelativeStateOfCharge() const
00091 {
00092     unsigned int min_rsc = 999;
00093     for (unsigned int i = 0; i < batteries_.size(); i++)
00094         min_rsc = min(min_rsc, batteries_[i].getRelativeStateOfCharge());
00095 
00096     return min_rsc;
00097 }
00098 
00099 float PowerObservation::getTotalRemainingCapacity() const
00100 {
00101     float rem_cap = 0.0f;
00102     for (unsigned int i = 0; i < batteries_.size(); i++)
00103         rem_cap += batteries_[i].getRemainingCapacity();
00104 
00105     return rem_cap;
00106 }
00107 
00111 ros::Duration PowerObservation::getMinTimeToEmpty(const ros::Time& t) const
00112 {
00113     ros::Duration min_tte(-1, 0);
00114 
00115     int count = 0;
00116 
00117     for (unsigned int i = 0; i < batteries_.size(); i++)
00118     {
00119         const BatteryObservation& b = batteries_[i];
00120 
00121         if (b.isAcPresent())
00122             continue;
00123 
00124         ros::Duration staleness = t - b.getStamp();
00125 
00126         ros::Duration tte(0);
00127         if (staleness < b.getTimeToEmpty())
00128             tte = b.getTimeToEmpty() - staleness;
00129 
00130         if (count == 0)
00131             min_tte = tte;
00132         else
00133             min_tte = min(min_tte, tte);
00134 
00135         count++;
00136     }
00137 
00138     return min_tte;
00139 }
00140 
00144 ros::Duration PowerObservation::getMaxTimeToFull(const ros::Time& t) const
00145 {
00146     ros::Duration max_ttf(-1, 0);
00147 
00148     int count = 0;
00149 
00150     for (unsigned int i = 0; i < batteries_.size(); i++)
00151     {
00152         const BatteryObservation& b = batteries_[i];
00153 
00154         if (!b.isAcPresent())
00155             continue;
00156 
00157         ros::Duration staleness = t - b.getStamp();
00158 
00159         ros::Duration ttf(0);
00160         if (staleness < b.getTimeToFull())
00161             ttf = b.getTimeToFull() - staleness;
00162 
00163         if (count == 0)
00164             max_ttf = ttf;
00165         else
00166             max_ttf = max(max_ttf, ttf);
00167 
00168         count++;
00169     }
00170 
00171     return max_ttf;
00172 }
00173 
00174 // BatteryObservation
00175 
00176 BatteryObservation::BatteryObservation(const ros::Time& stamp, bool ac_present, float voltage, float current,
00177                                        unsigned int relative_state_of_charge, float remaining_capacity,
00178                                        const ros::Duration& time_to_empty, const ros::Duration& time_to_full)
00179     : stamp_(stamp), ac_present_(ac_present), voltage_(voltage), current_(current), relative_state_of_charge_(relative_state_of_charge),
00180       remaining_capacity_(remaining_capacity), time_to_empty_(time_to_empty), time_to_full_(time_to_full)
00181 {
00182 }
00183 
00184 const ros::Time& BatteryObservation::getStamp() const { return stamp_; }
00185 
00186 bool                 BatteryObservation::isAcPresent()              const { return ac_present_;               }
00187 float                BatteryObservation::getVoltage()               const { return voltage_;                  }
00188 float                BatteryObservation::getCurrent()               const { return current_;                  }
00189 unsigned int         BatteryObservation::getRelativeStateOfCharge() const { return relative_state_of_charge_; }
00190 float                BatteryObservation::getRemainingCapacity()     const { return remaining_capacity_;       }
00191 const ros::Duration& BatteryObservation::getTimeToEmpty()           const { return time_to_empty_;            }
00192 const ros::Duration& BatteryObservation::getTimeToFull()            const { return time_to_full_;             }
00193 
00194 float BatteryObservation::getPower() const
00195 {
00196     return voltage_ * current_;
00197 }


power_monitor
Author(s): Tim Field, Curt Meyers
autogenerated on Thu Jun 6 2019 21:11:01