tibi_dabo_battery_monitor_driver_node.cpp

Go to the documentation of this file.

#include "tibi_dabo_battery_monitor_driver_node.h"
//#include <curses.h>
#include <stdlib.h>
TibiDaboBatteryMonitorDriverNode::TibiDaboBatteryMonitorDriverNode(ros::NodeHandle &nh) : 
  iri_base_driver::IriBaseNodeDriver<TibiDaboBatteryMonitorDriver>(nh)
{
  //init class attributes if necessary
  this->loop_rate_ = 1;//in [Hz]

  // [init publishers]
  this->battery_status_publisher_ = this->public_node_handle_.advertise<tibi_dabo_msgs::battery_status>("battery_status", 100);
  
  // [init subscribers]
  
  // [init services]
  
  // [init clients]
  
  // [init action servers]
  
  // [init action clients]
  //initscr();
}

void TibiDaboBatteryMonitorDriverNode::mainNodeThread(void)
{
  TBatteryInfo info;

  //lock access to driver if necessary
  this->driver_.lock();

  this->driver_.get_battery_info(&info);
  // [fill msg Header if necessary]
  //<publisher_name>.header.stamp = ros::Time::now();
  //<publisher_name>.header.frame_id = "<publisher_topic_name>";
  this->battery_status_msg_.voltage=info.battery_voltage;
  this->battery_status_msg_.temperature=(info.temperature1+info.temperature2)/2.0;
  this->battery_status_msg_.status=info.status;
  this->battery_status_msg_.input_current=info.current_in;
  this->battery_status_msg_.output_current=info.current_out;
  this->battery_status_msg_.remaining_capacity=info.rem_capacity;
  this->battery_status_msg_.time_to_charged=info.time_to_charged;
  this->battery_status_msg_.time_to_discharged=info.time_to_discharged;
  if(this->driver_.check_alarm())
  {
    int ok=system("beep"); //flash();//beep();
    ROS_ERROR("TibiDaboBatteryMonitorDriverNode::mainNodeThread: battery alarm! %d",ok);
  }
  else if(this->driver_.check_warning())
  {
    int ok=system("beep"); //flash();//beep();
    ROS_ERROR("TibiDaboBatteryMonitorDriverNode::mainNodeThread: battery warning! %d",ok);
  }
  // [fill msg structures]
  //this->battery_status_msg_.data = my_var;
  
  // [fill srv structure and make request to the server]
  
  // [fill action structure and make request to the action server]

  // [publish messages]
  this->battery_status_publisher_.publish(this->battery_status_msg_);

  //unlock access to driver if previously blocked
  this->driver_.unlock();
}

/*  [subscriber callbacks] */

/*  [service callbacks] */

/*  [action callbacks] */

/*  [action requests] */

void TibiDaboBatteryMonitorDriverNode::postNodeOpenHook(void)
{
}

void TibiDaboBatteryMonitorDriverNode::addNodeDiagnostics(void)
{
}

void TibiDaboBatteryMonitorDriverNode::addNodeOpenedTests(void)
{
}

void TibiDaboBatteryMonitorDriverNode::addNodeStoppedTests(void)
{
}

void TibiDaboBatteryMonitorDriverNode::addNodeRunningTests(void)
{
}

void TibiDaboBatteryMonitorDriverNode::reconfigureNodeHook(int level)
{
}

TibiDaboBatteryMonitorDriverNode::~TibiDaboBatteryMonitorDriverNode(void)
{
  // [free dynamic memory]
  //endwin();
}

/* main function */
int main(int argc,char *argv[])
{
  return driver_base::main<TibiDaboBatteryMonitorDriverNode>(argc, argv, "tibi_dabo_battery_monitor_driver_node");
}