power_monitor.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_monitor.h"
00036 
00037 using namespace std;
00038 using namespace power_monitor;
00039 
00040 PowerMonitor::PowerMonitor() : master_state_(-1),
00041                                battery_update_timeout_(120)
00042 {
00043     ros::NodeHandle node;
00044     ros::NodeHandle pnh("~");
00045 
00046     static const string battery_server_topic = "battery/server2";
00047     static const string power_board_node     = "power_board";
00048     string estimator_type_str                = "advanced";
00049     double freq                              = 0.1;
00050 
00051     pnh.getParam("estimation_method",    estimator_type_str);
00052     pnh.getParam("frequency",            freq);
00053     pnh.getParam("battery_update_timeout", battery_update_timeout_);
00054 
00055     ros::Duration(2).sleep();
00056 
00057     // Register the estimators
00058     addEstimator(new FuelGaugePowerStateEstimator());
00059     addEstimator(new AdvancedPowerStateEstimator());
00060 
00061     // Setup the dynamic_reconfigure callback
00062     dynamic_reconfigure::Server<PowerMonitorConfig>::CallbackType config_callback = boost::bind(&PowerMonitor::configCallback, this, _1, _2);
00063     config_server_.setCallback(config_callback);
00064 
00065     // Set the active estimation method
00066     if (estimator_types_.size() == 0)
00067     {
00068         ROS_FATAL("No power state estimators defined. Shutting down");
00069         exit(1);
00070     }
00071     map<string, PowerStateEstimator::Type>::const_iterator i = estimator_types_.find(estimator_type_str);
00072     if (i == estimator_types_.end())
00073     {
00074         // Requested estimator is unknown. Default to first estimator
00075         string first_estimator_type_str = estimator_types_.begin()->first;
00076         ROS_ERROR("Unknown power state estimator type: %s. Defaulting to %s", estimator_type_str.c_str(), first_estimator_type_str.c_str());
00077         setActiveEstimator(estimator_types_.begin()->second);
00078     }
00079     else
00080         setActiveEstimator(i->second);
00081 
00082     power_state_pub_       = node.advertise<pr2_msgs::PowerState>("power_state", 5, true);
00083     power_state_pub_timer_ = node.createTimer(ros::Duration(1.0 / freq), &PowerMonitor::onPublishTimer, this);
00084     battery_server_sub_    = node.subscribe(battery_server_topic, 10, &PowerMonitor::batteryServerUpdate, this);
00085     power_node_sub_        = node.subscribe(power_board_node + "/state", 10, &PowerMonitor::powerNodeUpdate, this);
00086 }
00087 
00088 void PowerMonitor::addEstimator(PowerStateEstimator* est)
00089 {
00090     estimator_types_[est->getMethodName()] = est->getMethodType();
00091     estimators_[est->getMethodType()] = boost::shared_ptr<PowerStateEstimator>(est);
00092 }
00093 
00094 void PowerMonitor::configCallback(PowerMonitorConfig& config, uint32_t level)
00095 {
00096     setActiveEstimator((PowerStateEstimator::Type) config.power_state_estimator_);
00097 }
00098 
00099 bool PowerMonitor::setActiveEstimator(PowerStateEstimator::Type estimator_type)
00100 {
00101     map<PowerStateEstimator::Type, boost::shared_ptr<PowerStateEstimator> >::const_iterator i = estimators_.find(estimator_type);
00102     if (i == estimators_.end())
00103         return false;
00104     if (active_estimator_ == i->second)
00105         return true;
00106 
00107     if (active_estimator_ == boost::shared_ptr<PowerStateEstimator>())
00108         ROS_INFO("Power state estimator set to %s", i->second->getMethodName().c_str());
00109     else
00110         ROS_INFO("Power state estimator changed from %s to %s", active_estimator_->getMethodName().c_str(), i->second->getMethodName().c_str());
00111 
00112     active_estimator_ = i->second;
00113 
00114     return true;
00115 }
00116 
00117 void PowerMonitor::batteryServerUpdate(const boost::shared_ptr<const pr2_msgs::BatteryServer2>& battery_server)
00118 {
00119     boost::mutex::scoped_lock lock(update_mutex_);
00120 
00121     ROS_DEBUG("Received battery message: voltage=%.2f", battery_server->battery[0].battery_register[0x9] / 1000.0f);
00122 
00123     battery_servers_[battery_server->id] = battery_server;
00124 }
00125 
00126 void PowerMonitor::powerNodeUpdate(const boost::shared_ptr<const pr2_msgs::PowerBoardState>& power_board_state)
00127 {
00128     boost::mutex::scoped_lock lock(update_mutex_);
00129 
00130     ROS_DEBUG("Received power board state message: %s", masterStateToString(power_board_state->master_state).c_str());
00131 
00132     master_state_ = power_board_state->master_state;
00133 
00134     // Publish the power state immediately if we're shutting down. Want to ensure we record these data points.
00135     if (master_state_ == pr2_msgs::PowerBoardState::MASTER_SHUTDOWN)
00136     {
00137       ROS_WARN("Power board reports imminant shutdown");
00138       publishPowerState();
00139     }
00140 }
00141 
00142 string PowerMonitor::masterStateToString(int8_t master_state) const
00143 {
00144     switch (master_state)
00145     {
00146         case pr2_msgs::PowerBoardState::MASTER_NOPOWER:  return "No Power";
00147         case pr2_msgs::PowerBoardState::MASTER_STANDBY:  return "Standby";
00148         case pr2_msgs::PowerBoardState::MASTER_ON:       return "On";
00149         case pr2_msgs::PowerBoardState::MASTER_OFF:      return "Off";
00150         case pr2_msgs::PowerBoardState::MASTER_SHUTDOWN: return "Shutdown";
00151         default:                                         return "Unknown";
00152     }
00153 }
00154 
00155 PowerObservation PowerMonitor::extractObservation()
00156 {
00157     boost::mutex::scoped_lock lock(update_mutex_);
00158 
00159     vector<BatteryObservation> batteries;
00160     for (map<int, boost::shared_ptr<const pr2_msgs::BatteryServer2> >::iterator i = battery_servers_.begin(); i != battery_servers_.end(); i++)
00161     {
00162         const pr2_msgs::BatteryServer2* bs = i->second.get();
00163 
00164         ros::Time stamp = bs->header.stamp;
00165 
00166         for (unsigned int j = 0; j < bs->battery.size(); j++)
00167         {
00168             const pr2_msgs::BatteryState2& b = bs->battery[j];
00169 
00170             bool         ac_present = b.power_present;
00171             float        voltage    = b.battery_register[0x9] / 1000.0f;
00172             float        current    = b.battery_register[0xA] / 1000.0f;
00173             unsigned int rsc        = b.battery_register[0x0D];
00174             float        rem_cap    = b.battery_register[0x0F] / 1000.0f;
00175             unsigned int tte_min    = b.battery_register[0x12];
00176             unsigned int ttf_min    = b.battery_register[0x13];
00177 
00178             ros::Duration tte;
00179             if (tte_min > 0)
00180                 tte = ros::Duration(tte_min * 60, 0);
00181             else
00182                 tte = ros::Duration(-1, 0);
00183 
00184             ros::Duration ttf;
00185             if (ttf_min > 0)
00186                 ttf = ros::Duration(ttf_min * 60, 0);
00187             else
00188                 ttf = ros::Duration(-1, 0);
00189 
00190             if (voltage == 0.0)
00191                 continue;
00192 
00193             batteries.push_back(BatteryObservation(stamp, ac_present, voltage, current, rsc, rem_cap, tte, ttf));
00194 
00195             ROS_DEBUG("Battery %d.%d: %6.2f V %6.2f A %6.2f W (soc: %d, cap: %6.2f, tte: %dm, ttf: %dm)", bs->id, j + 1, voltage, current, current * voltage, rsc, rem_cap, tte_min, ttf_min);
00196         }
00197     }
00198 
00199     return PowerObservation(ros::Time::now(), master_state_, batteries);
00200 }
00201 
00202 ros::Time PowerMonitor::getLastBatteryUpdate() const
00203 {
00204   ros::Time rv;
00205   for (map<int, boost::shared_ptr<const pr2_msgs::BatteryServer2> >::const_iterator i = battery_servers_.begin(); i != battery_servers_.end(); i++)
00206   {
00207     const pr2_msgs::BatteryServer2* bs = i->second.get();
00208     if (bs->header.stamp > rv)
00209       rv = bs->header.stamp;
00210   }
00211 
00212   return rv;
00213 }
00214 
00215 void PowerMonitor::onPublishTimer(const ros::TimerEvent& e)
00216 {
00217   // Don't publish power data if we haven't received battery data in a timeout, #4851
00218   if (battery_update_timeout_ > 0 && ((ros::Time::now() - getLastBatteryUpdate()).toSec() > battery_update_timeout_))
00219   {
00220     ROS_WARN_THROTTLE(60, "Power monitor not publishing estimate, batteries have not recently updated. Check diagnostics.");
00221     return;
00222   }
00223 
00224   publishPowerState();
00225 }
00226 
00227 void PowerMonitor::publishPowerState()
00228 {
00229     boost::mutex::scoped_lock lock(publish_mutex_);
00230 
00231     // Extract info from the battery server
00232     PowerObservation obs = extractObservation();
00233     if (obs.getBatteries().size() == 0)
00234     {
00235         ROS_DEBUG("Nothing observed");
00236         return;
00237     }
00238 
00239     ROS_DEBUG("Power: %6.1f W. Min voltage: %6.2f V", obs.getTotalPower(), obs.getMinVoltage());
00240 
00241     // Give every estimator the chance to record this observation
00242     for (map<PowerStateEstimator::Type, boost::shared_ptr<PowerStateEstimator> >::const_iterator i = estimators_.begin(); i != estimators_.end(); i++)
00243         i->second.get()->recordObservation(obs);
00244 
00245     // Use the active estimator to estimate the time and capacity remaining
00246     ros::Time t = ros::Time::now();
00247     if (active_estimator_->canEstimate(t))
00248     {
00249         PowerStateEstimate estimate = active_estimator_->estimate(t);
00250         ROS_DEBUG("Remaining: %.0f min (%d%%)", estimate.time_remaining.toSec() / 60, estimate.relative_capacity);
00251 
00252         // Publish the power state estimate
00253         pr2_msgs::PowerState ps;
00254         ps.header.stamp      = ros::Time::now();
00255         ps.AC_present        = obs.getAcCount();
00256         ps.power_consumption = obs.getTotalPower();
00257         ps.prediction_method = active_estimator_->getMethodName();
00258         ps.relative_capacity = (int8_t) estimate.relative_capacity;
00259         ps.time_remaining    = estimate.time_remaining;
00260         power_state_pub_.publish(ps);
00261     }
00262 }
00263 
00264 int main(int argc, char** argv)
00265 {
00266     ros::init(argc, argv, "power_monitor");
00267     PowerMonitor monitor;
00268     ros::spin();
00269     return 0;
00270 }


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