leuze_laser_driver_node.cpp

Go to the documentation of this file.

#include "leuze_laser_driver_node.h"

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

  // [init publishers]
  this->status_publisher_ = this->public_node_handle_.advertise<iri_common_drivers_msgs::leuze_status>("status", 10);
  scan_publisher_ = public_node_handle_.advertise<sensor_msgs::LaserScan>("scan", 100);
  
  // [init subscribers]
  
  // [init services]
  this->change_wp_pair_server_ = this->public_node_handle_.advertiseService("change_wp_pair", &LeuzeLaserDriverNode::change_wp_pairCallback, this);
  
  // [init clients]
  
  // [init action servers]
  
  // [init action clients]

  event_server_ = CEventServer::instance();
  events_.push_back(driver_.getNewScanEventId());
  events_.push_back(driver_.getNoLaserEventId());
}

void LeuzeLaserDriverNode::mainNodeThread(void)
{
  // [fill msg Header if necessary]
  //<publisher_name>.header.stamp = ros::Time::now();
  //<publisher_name>.header.frame_id = "<publisher_topic_name>";

  // [fill msg structures]
  //this->leuze_status_msg_.data = my_var;
  //this->LaserScan_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->status_publisher_.publish(this->leuze_status_msg_);

  // wait for new laser scan event
  int event_id = event_server_->wait_first(events_);
  
  switch(event_id)
  {
    //New Scan Event
    case 0:
      //ROS_INFO("New Laser Scan!");

      driver_.lock();
        LaserScan_msg_ = driver_.getScan();
        leuze_status_msg_ = driver_.getStatus();
      driver_.unlock();

      scan_publisher_.publish(LaserScan_msg_);
      status_publisher_.publish(leuze_status_msg_);
      break;

    //No Laser Detected
    case 1:
      ROS_ERROR("Laser NOT detected, please check either the serial port or the power supply");
      break;
  }
}

/*  [subscriber callbacks] */

/*  [service callbacks] */
bool LeuzeLaserDriverNode::change_wp_pairCallback(iri_common_drivers_msgs::change_wp_pair::Request &req, iri_common_drivers_msgs::change_wp_pair::Response &res) 
{ 
  ROS_INFO("LeuzeLaserDriverNode::change_wp_pairCallback: New Request Received!"); 

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

  if(this->driver_.isRunning()) 
  { 
    if(driver_.changePair(req.new_pair))
      res.success=true;
    else
      res.success=false;
    //ROS_INFO("LeuzeLaserDriverNode::change_wp_pairCallback: Processin New Request!"); 
    //do operations with req and output on res 
    //res.data2 = req.data1 + my_var; 
  } 
  else 
  { 
    //ROS_INFO("LeuzeLaserDriverNode::change_wp_pairCallback: ERROR: driver is not on run mode yet."); 
    res.success=false;
  } 
 
  //unlock previously blocked shared variables 
  this->driver_.unlock(); 
  //this->change_wp_pair_mutex_.exit(); 

  return true; 
}

/*  [action callbacks] */

/*  [action requests] */

void LeuzeLaserDriverNode::postNodeOpenHook(void)
{
}

void LeuzeLaserDriverNode::addNodeDiagnostics(void)
{
}

void LeuzeLaserDriverNode::addNodeOpenedTests(void)
{
}

void LeuzeLaserDriverNode::addNodeStoppedTests(void)
{
}

void LeuzeLaserDriverNode::addNodeRunningTests(void)
{
}

void LeuzeLaserDriverNode::reconfigureNodeHook(int level)
{
}

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

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