hokuyo_laser_driver_node.cpp

Go to the documentation of this file.

#include "hokuyo_laser_driver_node.h"
#include <cmath>

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


  // [init publishers]
  this->scan_publisher = this->public_node_handle_.advertise<sensor_msgs::LaserScan>("scan", 100);
  if(driver_.config_.is_multiecho){
    this->scan_publisher2 = this->public_node_handle_.advertise<sensor_msgs::LaserScan>("scan2", 100);
    this->scan_publisher3 = this->public_node_handle_.advertise<sensor_msgs::LaserScan>("scan3", 100);
  }

  // [init subscribers]

  // [init services]

  // [init clients]

  // [init action servers]

  // [init action clients]

  event_server_ = CEventServer::instance();
}

void HokuyoLaserDriverNode::mainNodeThread(void)
{
  //lock access to driver if necessary


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

  // [fill msg structures]
  //this->LaserScan_msg.data = my_var;

  int current_event = 0;

  ROS_DEBUG("WAIT FOR SCAN");

  // Get Scan
  try
  {
    current_event = event_server_->wait_first(driver_.evnts,5000);
    switch( current_event )
      {
        default:
        case 0:
          // new scan
          this->driver_.lock();
          mutex_scan_.enter();
          this->driver_.getScan(last_scan_);
          mutex_scan_.exit();
          this->driver_.unlock();
          ROS_DEBUG("HokuyoLaserDriverNode::mainNodeThread NEW SCAN");
          break;
        case 1:
          ROS_DEBUG("HokuyoLaserDriverNode::mainNodeThread END SCANS");
          // end of scans
          break;
        case 2:
          // error
          ROS_DEBUG("HokuyoLaserDriverNode::mainNodeThread ERROR");
          break;
      }
  }
  catch(CException &e)
  {
    cout << e.what() << endl;
  }

  // Build message
  // header

    ROS_DEBUG("BUILD MESSAGE");
    mutex_scan_.enter();
    ros::Time t_laser( last_scan_.t.seconds(), last_scan_.t.nanoseconds() );
    this->LaserScan_msg.header.stamp    = t_laser;                       // [s]
    this->driver_.lock();
    this->LaserScan_msg.header.frame_id = this->driver_.config_.frame_id;   // string
  // configuration
    this->LaserScan_msg.angle_min       = this->driver_.config_.angle_min;  // [rad]
    this->LaserScan_msg.angle_max       = this->driver_.config_.angle_max;  // [rad]
    this->LaserScan_msg.angle_increment = this->driver_.angle_increment;    // [rad]
    this->LaserScan_msg.time_increment  = this->driver_.time_increment;     // [s]
    this->LaserScan_msg.scan_time       = this->driver_.scan_time;          // [s]
    this->LaserScan_msg.range_min       = this->driver_.range_min;          // [m]
    this->LaserScan_msg.range_max       = this->driver_.range_max;          // [m]

    is_multiecho_ = driver_.config_.is_multiecho;
    this->driver_.unlock();
  // ranges
    vector<float> franges,franges2,franges3;
    franges.reserve(last_scan_.distance.size());
    franges2.reserve(last_scan_.distance.size());
    franges3.reserve(last_scan_.distance.size());
    // Unit conversion: [mm] (int) -> [m] (float)
    for(uint i=0;i<last_scan_.distance.size();i++)
    {
      franges.push_back((float)(last_scan_.distance[i][0])/1000);
      // multiecho
      if(is_multiecho_){
        if(last_scan_.distance[i].size()==2)
        {
          franges2.push_back((float)(last_scan_.distance[i][1])/1000);
          franges3.push_back(franges.back());
        }
        else if(last_scan_.distance[i].size()==3)
        {
          franges3.push_back((float)(last_scan_.distance[i][2])/1000);
          franges2.push_back(franges.back());
        }
        else
        {
          franges2.push_back(franges.back());
          franges3.push_back(franges.back());
        }
      }
    }
  // intensities
    vector<float> fintens;
    fintens.reserve(last_scan_.intensity.size());
     //vector<float> fintens(last_scan_.intensity.begin(), last_scan_.intensity.end());
//     this->LaserScan_msg.intensities = fintens;                        // relative
    for(uint i=0;i<last_scan_.intensity.size();i++)
      fintens.push_back((float)(last_scan_.intensity[i][0]));

    this->LaserScan_msg.intensities = fintens;
    this->LaserScan_msg.ranges = franges;                        // [m]
    mutex_scan_.exit();

  // [fill srv structure and make request to the server]

  // [fill action structure and make request to the action server]

  ROS_DEBUG("PUBLISH MESSAGE");
  // [publish messages]
  this->scan_publisher.publish(this->LaserScan_msg);
  // multiecho
  if(is_multiecho_)
  {
    this->LaserScan2_msg = this->LaserScan_msg;
    this->LaserScan3_msg = this->LaserScan_msg;
    this->LaserScan2_msg.ranges = franges2;                        // [m]
    this->LaserScan3_msg.ranges = franges3;                        // [m]
    this->scan_publisher2.publish(this->LaserScan2_msg);
    this->scan_publisher3.publish(this->LaserScan3_msg);
  }

  //unlock access to driver if previously blocked

}

/*  [subscriber callbacks] */

/*  [service callbacks] */

/*  [action callbacks] */

/*  [action requests] */

void HokuyoLaserDriverNode::postNodeOpenHook(void)
{
}

void HokuyoLaserDriverNode::addNodeDiagnostics(void)
{
}

void HokuyoLaserDriverNode::addNodeOpenedTests(void)
{
}

void HokuyoLaserDriverNode::addNodeStoppedTests(void)
{
}

void HokuyoLaserDriverNode::addNodeRunningTests(void)
{
}

void HokuyoLaserDriverNode::reconfigureNodeHook(int level)
{
}

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

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