teo_battery_monitor_driver_node.cpp

Go to the documentation of this file.

#include "teo_battery_monitor_driver_node.h"
#include <iostream>

TeoBatteryMonitorDriverNode::TeoBatteryMonitorDriverNode(ros::NodeHandle &nh) : 
  iri_base_driver::IriBaseNodeDriver<TeoBatteryMonitorDriver>(nh)
{
  //init class attributes if necessary
  this->loop_rate_ = 0.5;//in [Hz]

  // [init publishers]
  this->battery_status_publisher_ = this->public_node_handle_.advertise<iri_common_drivers_msgs::lifebatt_status>("battery_status", 100);
  
  // [init subscribers]
  this->segway_status_subscriber_ = this->public_node_handle_.subscribe("segway_status", 100, &TeoBatteryMonitorDriverNode::segway_status_callback, this);
  
  // [init services]
  
  // [init clients]
  
  // [init action servers]
  
  // [init action clients]
}

void TeoBatteryMonitorDriverNode::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(driver_.config_.print_info)
  {
    ROS_DEBUG_STREAM("LifeBatt voltage: " << this->battery_status_msg_.voltage << " mV");
    ROS_DEBUG_STREAM("LifeBatt temperature: " << this->battery_status_msg_.temperature << " *C");
    ROS_DEBUG_STREAM("LifeBatt status: " << (unsigned int)this->battery_status_msg_.status << "");
    ROS_DEBUG_STREAM("LifeBatt input_current: " << this->battery_status_msg_.input_current << "");
    ROS_DEBUG_STREAM("LifeBatt output_current: " << this->battery_status_msg_.output_current << "");
    ROS_DEBUG_STREAM("LifeBatt remaining_capacity: " << this->battery_status_msg_.remaining_capacity << " mAh");
    ROS_DEBUG_STREAM("LifeBatt time_to_charged: " << this->battery_status_msg_.time_to_charged << " m");
    ROS_DEBUG_STREAM("LifeBatt time_to_discharged: " << this->battery_status_msg_.time_to_discharged << " m");
    ROS_DEBUG_STREAM("Segways ui_battery: " << this->ui_0_ << " / " << this->ui_1_ << " V");
    ROS_DEBUG_STREAM("Segways powerbase_battery: " << this->powerbase_0_ << " / " << this->powerbase_1_ << " V");
  }

  // [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] */
void TeoBatteryMonitorDriverNode::segway_status_callback(const iri_segway_rmp_msgs::SegwayRMP400Status::ConstPtr& msg) 
{ 
  //ROS_INFO("TeoBatteryMonitorDriverNode::segway_status_callback: New Message Received"); 

  //use appropiate mutex to shared variables if necessary 
  //this->driver_.lock(); 
  //this->segway_status_mutex_.enter(); 

  //std::cout << msg->data << std::endl;
  this->ui_0_ = msg->rmp200[0].ui_battery;
  this->ui_1_ = msg->rmp200[1].ui_battery;
  this->powerbase_0_ = msg->rmp200[0].powerbase_battery;
  this->powerbase_1_ = msg->rmp200[1].powerbase_battery;

  //unlock previously blocked shared variables 
  //this->driver_.unlock(); 
  //this->segway_status_mutex_.exit(); 
}

/*  [service callbacks] */

/*  [action callbacks] */

/*  [action requests] */

void TeoBatteryMonitorDriverNode::postNodeOpenHook(void)
{
}

void TeoBatteryMonitorDriverNode::addNodeDiagnostics(void)
{
}

void TeoBatteryMonitorDriverNode::addNodeOpenedTests(void)
{
}

void TeoBatteryMonitorDriverNode::addNodeStoppedTests(void)
{
}

void TeoBatteryMonitorDriverNode::addNodeRunningTests(void)
{
}

void TeoBatteryMonitorDriverNode::reconfigureNodeHook(int level)
{
}

TeoBatteryMonitorDriverNode::~TeoBatteryMonitorDriverNode(void)
{
  // [free dynamic memory]
}

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