40 PowerMonitor::PowerMonitor() : master_state_(-1),
41 battery_update_timeout_(120)
46 static const string battery_server_topic =
"battery/server2";
47 static const string power_board_node =
"power_board";
48 string estimator_type_str =
"advanced";
51 pnh.
getParam(
"estimation_method", estimator_type_str);
62 dynamic_reconfigure::Server<PowerMonitorConfig>::CallbackType config_callback = boost::bind(&
PowerMonitor::configCallback,
this, _1, _2);
68 ROS_FATAL(
"No power state estimators defined. Shutting down");
71 map<string, PowerStateEstimator::Type>::const_iterator i =
estimator_types_.find(estimator_type_str);
76 ROS_ERROR(
"Unknown power state estimator type: %s. Defaulting to %s", estimator_type_str.c_str(), first_estimator_type_str.c_str());
101 map<PowerStateEstimator::Type, boost::shared_ptr<PowerStateEstimator> >::const_iterator i =
estimators_.find(estimator_type);
108 ROS_INFO(
"Power state estimator set to %s", i->second->getMethodName().c_str());
110 ROS_INFO(
"Power state estimator changed from %s to %s",
active_estimator_->getMethodName().c_str(), i->second->getMethodName().c_str());
121 ROS_DEBUG(
"Received battery message: voltage=%.2f", battery_server->battery[0].battery_register[0x9] / 1000.0f);
135 if (master_state_ == pr2_msgs::PowerBoardState::MASTER_SHUTDOWN)
137 ROS_WARN(
"Power board reports imminant shutdown");
144 switch (master_state)
146 case pr2_msgs::PowerBoardState::MASTER_NOPOWER:
return "No Power";
147 case pr2_msgs::PowerBoardState::MASTER_STANDBY:
return "Standby";
148 case pr2_msgs::PowerBoardState::MASTER_ON:
return "On";
149 case pr2_msgs::PowerBoardState::MASTER_OFF:
return "Off";
150 case pr2_msgs::PowerBoardState::MASTER_SHUTDOWN:
return "Shutdown";
151 default:
return "Unknown";
159 vector<BatteryObservation> batteries;
162 const pr2_msgs::BatteryServer2* bs = i->second.get();
166 for (
unsigned int j = 0; j < bs->battery.size(); j++)
168 const pr2_msgs::BatteryState2& b = bs->battery[j];
170 bool ac_present = b.power_present;
171 float voltage = b.battery_register[0x9] / 1000.0f;
172 float current = b.battery_register[0xA] / 1000.0f;
173 unsigned int rsc = b.battery_register[0x0D];
174 float rem_cap = b.battery_register[0x0F] / 1000.0f;
175 unsigned int tte_min = b.battery_register[0x12];
176 unsigned int ttf_min = b.battery_register[0x13];
193 batteries.push_back(
BatteryObservation(stamp, ac_present, voltage, current, rsc, rem_cap, tte, ttf));
195 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);
207 const pr2_msgs::BatteryServer2* bs = i->second.get();
208 if (bs->header.stamp > rv)
209 rv = bs->header.stamp;
220 ROS_WARN_THROTTLE(60,
"Power monitor not publishing estimate, batteries have not recently updated. Check diagnostics.");
243 i->second.get()->recordObservation(obs);
253 pr2_msgs::PowerState ps;
264 int main(
int argc,
char** argv)
void batteryServerUpdate(const boost::shared_ptr< const pr2_msgs::BatteryServer2 > &battery_server)
boost::mutex update_mutex_
#define ROS_WARN_THROTTLE(rate,...)
void publish(const boost::shared_ptr< M > &message) const
ros::Duration time_remaining
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
bool setActiveEstimator(PowerStateEstimator::Type estimator_type)
unsigned int relative_capacity
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
boost::shared_ptr< PowerStateEstimator > active_estimator_
float getMinVoltage() const
std::map< PowerStateEstimator::Type, boost::shared_ptr< PowerStateEstimator > > estimators_
std::map< int, boost::shared_ptr< const pr2_msgs::BatteryServer2 > > battery_servers_
boost::mutex publish_mutex_
float getTotalPower() const
ros::Publisher power_state_pub_
ros::Subscriber power_node_sub_
std::string masterStateToString(int8_t master_state) const
void onPublishTimer(const ros::TimerEvent &e)
void addEstimator(PowerStateEstimator *estimator)
ros::Timer power_state_pub_timer_
PowerObservation extractObservation()
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
dynamic_reconfigure::Server< power_monitor::PowerMonitorConfig > config_server_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
virtual std::string getMethodName() const =0
unsigned int getAcCount() const
double battery_update_timeout_
virtual PowerStateEstimator::Type getMethodType() const =0
bool getParam(const std::string &key, std::string &s) const
int main(int argc, char **argv)
const std::vector< BatteryObservation > & getBatteries() const
void configCallback(power_monitor::PowerMonitorConfig &config, uint32_t level)
ros::Time getLastBatteryUpdate() const
std::map< std::string, PowerStateEstimator::Type > estimator_types_
ros::Subscriber battery_server_sub_
void powerNodeUpdate(const boost::shared_ptr< const pr2_msgs::PowerBoardState > &power_board_state)