hokuyo_laser3d_driver_node.cpp

Go to the documentation of this file.

#include "hokuyo_laser3d_driver_node.h"

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

  //this->evnts.push_back(NEW3DSCAN);       // event 0

  // [init publishers]
  this->slice_publisher_ = this->public_node_handle_.advertise<sensor_msgs::PointCloud2>("slice", 100);
  this->cloud_publisher_ = this->public_node_handle_.advertise<sensor_msgs::PointCloud2>("cloud", 100);

  // [init subscribers]

  // [init services]
  this->get_3d_scan_server_ = this->public_node_handle_.advertiseService("get_3d_scan", &HokuyoLaser3dDriverNode::get_3d_scanCallback, this);
  this->do_noncontinuous = false;

  event_server = CEventServer::instance();
  // [init clients]

  // [init action servers]

  // [init action clients]
}

void HokuyoLaser3dDriverNode::mainNodeThread(void)
{
  this->driver_.lock();

  PointCloudTimeStamped tall, nuvol;

  if( this->driver_.config_.continuous )
  {
    try
    {
      uint party = event_server->wait_first(driver_.getENDList());

      if( party == 1 )
      {
        if( this->driver_.config_.slices )
        {
          tall.cloud.reserve(1081);
          driver_.getSlice(tall);
          createPointCloud2(tall, Slice_msg_);
          slice_publisher_.publish(Slice_msg_);
        }
      }else
      {
        if( this->driver_.config_.clouds )
        {
          driver_.getCloud(nuvol);
          createPointCloud2(nuvol, Cloud_msg_);
          cloud_publisher_.publish(Cloud_msg_);
        }
      }
    }
    catch(CException &e)
    {
      ROS_ERROR("'%s'",e.what().c_str());
    }
  }

  this->driver_.unlock();
}

/*  [subscriber callbacks] */

/*  [service callbacks] */
bool HokuyoLaser3dDriverNode::get_3d_scanCallback(iri_hokuyo_laser3d::Get3DScan::Request &req, iri_hokuyo_laser3d::Get3DScan::Response &res)
{
  //lock access to driver if necessary
  //this->driver_.lock();

  ROS_INFO("PointCloud Requested");

  if( !this->driver_.config_.continuous )
  {
    this->do_noncontinuous = true;
    res.success = 1;
  }else{
    res.success = 0;
  }
  //unlock driver if previously blocked
 // this->driver_.unlock();

  return true;
}

/*  [action callbacks] */

/*  [action requests] */

void HokuyoLaser3dDriverNode::postNodeOpenHook(void)
{
}

void HokuyoLaser3dDriverNode::addNodeDiagnostics(void)
{
}

void HokuyoLaser3dDriverNode::addNodeOpenedTests(void)
{
}

void HokuyoLaser3dDriverNode::addNodeStoppedTests(void)
{
}

void HokuyoLaser3dDriverNode::addNodeRunningTests(void)
{
}

void HokuyoLaser3dDriverNode::reconfigureNodeHook(int level)
{
}

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

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


void HokuyoLaser3dDriverNode::createPointCloud2(const PointCloudTimeStamped & cld, sensor_msgs::PointCloud2 & pointcloud)
{
  // Header
  pointcloud.header.frame_id = this->driver_.config_.frame_id;
  pointcloud.header.stamp = ros::Time::now ();

  // 2D structure of the point cloud. If the cloud is unordered, height is
  // 1 and width is the length of the point cloud.
  
  pointcloud.height = 1;
  pointcloud.width = cld.cloud.size();

  pointcloud.fields.resize(5);
  pointcloud.fields[0].name = "x";
  pointcloud.fields[1].name = "y";
  pointcloud.fields[2].name = "z";
  pointcloud.fields[3].name = "intensity";

  // Set all the fields types accordingly
  int offset = 0;
  for (size_t s = 0; s < pointcloud.fields.size(); ++s, offset += sizeof(float))
  {
    pointcloud.fields[s].offset   = offset;
    pointcloud.fields[s].count    = 1;
    pointcloud.fields[s].datatype = sensor_msgs::PointField::FLOAT32;
  }

  pointcloud.point_step = offset; // size of a point in bytes (sizeof(float)*fields.size())
  pointcloud.row_step   = pointcloud.point_step*pointcloud.width;
  pointcloud.is_dense   = false;
  pointcloud.data.resize(pointcloud.row_step*pointcloud.height);

  for (uint v = 0; v < pointcloud.height; v++)
  {
    for (uint u = 0; u < pointcloud.width; u++)
    {
      int index = v * pointcloud.width + u;
      float *pstep = (float*)&pointcloud.data[(index) * pointcloud.point_step];
      pstep[0] = cld.cloud[index].x;
      pstep[1] = cld.cloud[index].y;
      pstep[2] = cld.cloud[index].z;
      pstep[3] = cld.cloud[index].intensity;
    }
  }

}