jackal_diagnostic_updater.cpp

Go to the documentation of this file.

#include <string>
#include <sys/types.h>
#include <ifaddrs.h>

#include "boost/algorithm/string/predicate.hpp"
#include "diagnostic_updater/update_functions.h"
#include "jackal_base/jackal_diagnostic_updater.h"


namespace jackal_base
{

JackalDiagnosticUpdater::JackalDiagnosticUpdater()
{
  setHardwareID("unknown");

  add("General", this, &JackalDiagnosticUpdater::generalDiagnostics);
  add("Battery", this, &JackalDiagnosticUpdater::batteryDiagnostics);
  add("User voltage supplies", this, &JackalDiagnosticUpdater::voltageDiagnostics);
  add("Current consumption", this, &JackalDiagnosticUpdater::currentDiagnostics);
  add("Power consumption", this, &JackalDiagnosticUpdater::powerDiagnostics);

  // The arrival of this message runs the update() method and triggers the above callbacks.
  status_sub_ = nh_.subscribe("status", 5, &JackalDiagnosticUpdater::statusCallback, this);

  // These message frequencies are reported on separately.
  ros::param::param("~expected_imu_frequency", expected_imu_frequency_, 50.0);
  imu_diagnostic_ = new diagnostic_updater::TopicDiagnostic("/imu/data_raw", *this,
      diagnostic_updater::FrequencyStatusParam(&expected_imu_frequency_, &expected_imu_frequency_, 0.15),
      diagnostic_updater::TimeStampStatusParam(-1, 1.0));
  imu_sub_ = nh_.subscribe("/imu/data_raw", 5, &JackalDiagnosticUpdater::imuCallback, this);

  ros::param::param("~expected_navsat_frequency", expected_navsat_frequency_, 10.0);
  ros::param::param<std::string>("~navsat_frequency_sentence", navsat_frequency_sentence_, "$GPRMC");
  navsat_diagnostic_ = new diagnostic_updater::TopicDiagnostic("/navsat/nmea_sentence", *this,
      diagnostic_updater::FrequencyStatusParam(&expected_navsat_frequency_, &expected_navsat_frequency_, 0.1),
      diagnostic_updater::TimeStampStatusParam(-1, 1.0));
  navsat_sub_ = nh_.subscribe("/navsat/nmea_sentence", 5, &JackalDiagnosticUpdater::navsatCallback, this);

  // Publish whether the wireless interface has an IP address every second.
  ros::param::param<std::string>("~wireless_interface", wireless_interface_, "wlan0");
  ROS_INFO_STREAM("Checking for wireless connectivity on interface: " << wireless_interface_);
  wifi_connected_pub_ = nh_.advertise<std_msgs::Bool>("wifi_connected", 1);
  wireless_monitor_timer_ = nh_.createTimer(ros::Duration(1.0),
      &JackalDiagnosticUpdater::wirelessMonitorCallback, this);
}

void JackalDiagnosticUpdater::generalDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
{
  stat.add("MCU uptime (s)", last_status_->mcu_uptime.toSec());
  stat.addf("Motor drivers energized", "%s", last_status_->drivers_active ? "true" : "false");

  if (last_status_->driver_external_stop_present)
  {
    if (last_status_->driver_external_stop_stopped)
    {
      stat.add("External stop", "Present, asserting stop");
    }
    else
    {
      stat.add("External stop", "Present, not asserting stop");
    }
  }
  else
  {
    stat.add("External stop", "Absent");
  }

  if (last_status_->driver_external_stop_stopped)
  {
    stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "External stop device asserting motor stop.");
  }
  else if (!last_status_->drivers_active)
  {
    stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Motor drivers not energized.");
  }
  else
  {
    stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "System OK.");
  }
}

void JackalDiagnosticUpdater::batteryDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
{
  stat.add("Battery Voltage (V)", last_status_->measured_battery);

  if (last_status_->measured_battery > 30.0)
  {
    stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Battery overvoltage.");
  }
  else if (last_status_->measured_battery < 1.0)
  {
    stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Battery voltage not detected, check BATT fuse.");
  }
  else if (last_status_->measured_battery < 20.0)
  {
    stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Battery critically under voltage.");
  }
  else if (last_status_->measured_battery < 24.0)
  {
    stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Battery low voltage.");
  }
  else
  {
    stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Battery OK.");
  }
}

void JackalDiagnosticUpdater::voltageDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
{
  stat.add("12V Supply (V)", last_status_->measured_12v);
  stat.add("5V Supply (V)", last_status_->measured_5v);

  if (last_status_->measured_12v > 12.5 || last_status_->measured_5v > 5.5)
  {
    stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR,
        "User supply overvoltage. Accessories may be damaged.");
  }
  else if (last_status_->measured_12v < 1.0 || last_status_->measured_5v < 1.0)
  {
    stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "User supplies absent. Check tray fuses.");
  }
  else if (last_status_->measured_12v < 11.0 || last_status_->measured_5v < 4.0)
  {
    stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Voltage supplies undervoltage. Check loading levels.");
  }
  else
  {
    stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "User supplies OK.");
  }
}

void JackalDiagnosticUpdater::currentDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
{
  stat.add("Drive current (A)", last_status_->drive_current);
  stat.add("User current (A)", last_status_->user_current);
  stat.add("Computer current (A)", last_status_->computer_current);
  stat.add("Total current (A)", last_status_->total_current);

  if (last_status_->total_current > 32.0)
  {
    stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Current draw critical.");
  }
  else if (last_status_->total_current > 20.0)
  {
    stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Current draw warning.");
  }
  else if (last_status_->total_current > 10.0)
  {
    stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Current draw requires monitoring.");
  }
  else
  {
    stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Current draw nominal.");
  }
}

void JackalDiagnosticUpdater::powerDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
{
  stat.add("Total power consumption (Wh)", last_status_->total_power_consumed);

  if (last_status_->total_power_consumed > 260.0)
  {
    stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Power consumed exceeds capacity of standard battery.");
  }
  else if (last_status_->total_power_consumed > 220.0)
  {
    stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Power consumed approaches capacity of standard battery.");
  }
  else
  {
    stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Battery OK.");
  }
}

void JackalDiagnosticUpdater::statusCallback(const jackal_msgs::Status::ConstPtr& status)
{
  // Fresh data from the MCU, publish a diagnostic update.
  last_status_ = status;
  setHardwareID(last_status_->hardware_id);
  update();
}

void JackalDiagnosticUpdater::imuCallback(const sensor_msgs::Imu::ConstPtr& msg)
{
  imu_diagnostic_->tick(msg->header.stamp);
}

void JackalDiagnosticUpdater::navsatCallback(const nmea_msgs::Sentence::ConstPtr& msg)
{
  if (boost::starts_with(msg->sentence, navsat_frequency_sentence_))
  {
    navsat_diagnostic_->tick(msg->header.stamp);
  }
}

void JackalDiagnosticUpdater::wirelessMonitorCallback(const ros::TimerEvent& te)
{
  std_msgs::Bool wifi_connected_msg;
  wifi_connected_msg.data = false;

  // Get system structure of interface IP addresses.
  struct ifaddrs* ifa_head;
  if (getifaddrs(&ifa_head) != 0)
  {
    ROS_WARN("System call getifaddrs returned error code. Unable to detect network interfaces.");
    return;
  }

  // Iterate structure looking for the wireless interface.
  struct ifaddrs* ifa_current = ifa_head;
  while (ifa_current != NULL)
  {
    if (strcmp(ifa_current->ifa_name, wireless_interface_.c_str()) == 0)
    {
      int family = ifa_current->ifa_addr->sa_family;
      if (family == AF_INET || family == AF_INET6)
      {
        wifi_connected_msg.data = true;
        break;
      }
    }

    ifa_current = ifa_current->ifa_next;
  }

  // Free structure, publish result message.
  freeifaddrs(ifa_head);
  wifi_connected_pub_.publish(wifi_connected_msg);
}

}  // namespace jackal_base