battery_diagnostics.cpp

Go to the documentation of this file.

/* This node to incoming voltage level from the central battery in the segbot platform. 
 * The default values are that of the segbot_v2, where the marine battery data is published through the arduino.
 * You must change the launch file parameters to adjust to other segbot models, if needed.
 *
 * This node sends emails and requires that the local smtp client is setup properly.
 * Please follow the instructions on the BWI Wiki.
 */

#include "ros/ros.h"
#include "ros/time.h"
#include "std_msgs/Header.h"
#include "std_msgs/Float32.h"
#include "diagnostic_msgs/DiagnosticStatus.h"
#include "diagnostic_msgs/DiagnosticArray.h"
#include "diagnostic_msgs/KeyValue.h"
#include "sensor_msgs/BatteryState.h"
#include <ros/package.h>
#include <vector>
#include <cmath>
#include <stdio.h>
#include <stdlib.h>
#include <fstream>
#include <unistd.h>
#include <boost/thread.hpp>
#include <boost/lexical_cast.hpp>

float voltage;
double alpha;

void voltageCallback(const sensor_msgs::BatteryState::ConstPtr& msg) {
  voltage = alpha * msg->voltage + (1.0 - alpha) * voltage;
}

std::string getLocalHostname() {
  char szHostName[1048];
  std::string error = "error";
  if ( gethostname(szHostName, 1048) == 0 ) {
    return std::string(szHostName);
  }
  return error;
}

/*
 * Given the voltage and the parameters of the model, return the estimated time remaining in hours
  */
float getBatteryEstimate(double A, double K, double C, float voltage, double cutoffVoltage) {
  double max_life = std::log((cutoffVoltage - C) / -A) / K;
  int negation = 1;
  //Catch the case where the starting voltage is greater than the model accounts for.
  //This can happen because the exponentially weighted voltage is initialized higher than the realized voltage.
  if( voltage > C) {
     A *= -1;
     negation = -1;
  }
  double cur_life = std::log((voltage - C) / -A) / K;
  return (max_life - negation * cur_life) / 2; //Model is in 30-minute units. Divide by 2 to get intervals of 1 hour.
}

int sendMail(std::string outboundList, std::string sender) {
  std::string lhn = getLocalHostname();
  if(lhn.compare("error") == 0)
    lhn = "a robot";
  std::string msg = "Greetings,\nJust so you know, " + lhn +"'s battery is low. Please seek this robot and bring it to the lab for charging.\nBest,\nBuilding Wide Intelligence\n\nNOTE: This is an autogenerated message. Please don't respond";
  int retval = -1;
  FILE *mailpipe = popen("/usr/lib/sendmail -t", "w");
  if (mailpipe != NULL) {
    std::string to = "To: " + outboundList + "\n";
    std::string from = "From: " + sender + "\n";
    std::string subject = "Subject: Segbot Alert: " + lhn + " Has a Low Battery\n";
    fputs(to.c_str(), mailpipe);
    fputs(from.c_str(), mailpipe);
    fputs(subject.c_str(), mailpipe);
    fwrite(msg.c_str(), 1, strlen(msg.c_str()), mailpipe);
    fwrite(".\n", 1, 2, mailpipe);
    pclose(mailpipe);
    retval = 0;
  } else {
    perror("Failed to invoke sendmail");
  }
  return retval;
}

int main(int argc, char **argv) {
  ros::init(argc, argv, "battery_estimator");
  ros::NodeHandle n;
  
  double range, threshold;
  bool sentMail = false;
  std::string volt_topic;
  
  //parameters
  n.param("weighted_average_const", alpha, .05);
  n.param("voltage_threshold", threshold, 10.9);
  n.param("range_percent", range, 0.5);
  n.param("voltage_topic", volt_topic, (std::string) "/battery0");
  voltage = threshold * 1.25;            //initialize with a value that won't trigger mail service

  ros::Publisher battery_life_pub = n.advertise<diagnostic_msgs::DiagnosticArray>("diagnostics", 10);
  ros::Subscriber voltage_sub = n.subscribe(volt_topic, 10, voltageCallback);
  ros::Rate loop_rate(1);                //1hz
  diagnostic_msgs::DiagnosticStatus voltages;
  diagnostic_msgs::DiagnosticStatus status;
  diagnostic_msgs::DiagnosticArray diagAr;
  diagnostic_msgs::KeyValue status_val;
  diagnostic_msgs::KeyValue voltages_val;
  voltages.hardware_id = "005";
  voltages.name = "voltage";
  voltages_val.value = voltage;
  voltages_val.key = "segbot voltage: ";
  status.hardware_id = "005";
  status.name = "Remaining Battery Estimator";       //must match what the aggregator config file expects
  status_val.key = "Battery life remaining (hours)";
  status_val.value = "Unknown. Battery profile not set.";
  std::ostringstream ss;
  bool profileExists = false;

  //Does there exist a battery profile? If so, acquire the parameters it defines
  std::string path = ros::package::getPath("segbot_sensors");
  path += "/config/battery_profile";
  std::ifstream infile(path.c_str());
  profileExists = infile.good();
  double A, K, C, value = 0.0;
  int count = 0;
  
  while (infile >> value) {
    switch (count) {
      case (0) : A = value; break;
      case (1) : K = value; break;
      case (2) : C = value; break;
      default: ROS_ERROR("ERROR. Profile has too many arguments. Abandoning battery estimation.\n");
               profileExists = false;
               break;
    }
    count += 1;
  }

  while (ros::ok()) {
    voltages_val.value = boost::lexical_cast<std::string>(voltage).substr(0,4);
    voltages.message = voltages_val.value;
    diagAr.header.stamp = ros::Time::now();
    diagAr.header.frame_id = 1;
    
    if (profileExists)
      status_val.value = boost::lexical_cast<std::string>(getBatteryEstimate(A, K, C, voltage, threshold+range)).substr(0,4);
    
    if (voltage > range + threshold) {
      status.message = status_val.value + " hours ";
      status.level = 0;                   // 0 = OK
      voltages.level = 0;
    } else if (voltage > threshold && voltage < (range + threshold)) {
      status.message = "Battery low, return to lab.";
      status.level = 1;                   //1 = WARN
      voltages.level = 1;
      if (!sentMail) {
        //To maintain steady diag readings, run sendmail on thread
        sentMail = true;
        bool paramSuccess;
        std::string outbound;
        paramSuccess = n.getParam("email_alert_outbound", outbound);
        std::string send;
        paramSuccess &= n.getParam("email_alert_sender", send);
        if (!paramSuccess) {
          std::cout << "Error reading send mail parameters. Check launchfile. Emails not sent." << std::endl;
        } else {
          boost::thread mailThread(sendMail, outbound, send);
        }
      }
    } else if (!voltage) {
      voltages.message = "No voltage data";
      voltages.level = 2;
      status.message = "Cannot extrapolate battery charge. No voltage data.";
      status.level = 2;
    } else {
      status.message = "Battery CRITICALLY low, or voltmeter data is inaccurate (or missing). Ensure the volt sensor is connected properly and its publisher relaying data.";
      status.level = 2;                    //2 = CRITICAL
      voltages.level = 2;
      voltages.message = "ERROR: Ensure accurate volt readings!";
    }
    //Clearing messages reduces memory overhead, but limits the perception of changes in state in the diagnostics viewer.
    status.values.clear();
    status.values.push_back(status_val);
    voltages.values.clear();
    voltages.values.push_back(voltages_val);
    diagAr.status.clear();
    diagAr.status.push_back(status);
    diagAr.status.push_back(voltages);
    battery_life_pub.publish(diagAr);
    ros::spinOnce();
    loop_rate.sleep();
  }
  return 0;
}