lasers_to_pointcloud_alg_node.cpp

Go to the documentation of this file.

#include "lasers_to_pointcloud_alg_node.h"

LasersToPointcloudAlgNode::LasersToPointcloudAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<LasersToPointcloudAlgorithm>()
{
  //init class attributes if necessary
  //this->loop_rate_ = 2;//in [Hz]

  counter_ = 0;
  new_scan1_ = new_scan2_ = false;

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

  // [init subscribers]
  this->scan2_subscriber_ = this->public_node_handle_.subscribe("scan2", 100, &LasersToPointcloudAlgNode::scan2_callback, this);
  this->scan1_subscriber_ = this->public_node_handle_.subscribe("scan1", 100, &LasersToPointcloudAlgNode::scan1_callback, this);


  // [init services]

  // [init clients]

  // [init action servers]

  // [init action clients]
}

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

void LasersToPointcloudAlgNode::mainNodeThread(void)
{
  // [fill msg structures]

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

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

  // [publish messages]
  if(new_scan1_ && new_scan2_)
  {
//    cloud_publisher_.publish(cloud1_);

    scan1_mutex_.enter();
    scan2_mutex_.enter();
      std::vector<unsigned char> all_data(cloud2_.data);
      all_data.reserve(cloud1_.data.size() + cloud2_.data.size());
      all_data.insert(all_data.end(), cloud1_.data.begin(), cloud1_.data.end());
      PointCloud2_msg_.header.seq      = counter_++;
      PointCloud2_msg_.header.stamp    = ros::Time::now(); //cloud1_.header.stamp;
      PointCloud2_msg_.header.frame_id = alg_.config_.base_frame; //cloud1_.header.frame_id;
      PointCloud2_msg_.fields          = cloud1_.fields;
      PointCloud2_msg_.is_bigendian    = cloud1_.is_bigendian;
      PointCloud2_msg_.point_step      = cloud1_.point_step;
      PointCloud2_msg_.row_step        = cloud1_.row_step;
      PointCloud2_msg_.is_dense        = cloud1_.is_dense;
      PointCloud2_msg_.height          = cloud1_.height;
      PointCloud2_msg_.width           = cloud1_.width+cloud2_.width;
      PointCloud2_msg_.data            = all_data;
    scan2_mutex_.exit();
    scan1_mutex_.exit();

    new_scan1_ = new_scan2_ = false;

    cloud_publisher_.publish(PointCloud2_msg_);
    ROS_DEBUG("Cloud published. Cloud stamp: %f width: %d height: %d",
              PointCloud2_msg_.header.stamp.toSec(),PointCloud2_msg_.width,PointCloud2_msg_.height);

  }
}

/*  [subscriber callbacks] */
void LasersToPointcloudAlgNode::scan2_callback(const sensor_msgs::LaserScan::ConstPtr& msg)
{
  bool t;
  scan2_mutex_.enter();
    sensor_msgs::PointCloud2 cloud;
    laser_projector_.projectLaser(*msg, cloud);
    t = pcl_ros::transformPointCloud (alg_.config_.base_frame, cloud, cloud2_, tfl_);
  scan2_mutex_.exit();
  if (t){
    new_scan2_ = true;
  }
}
void LasersToPointcloudAlgNode::scan1_callback(const sensor_msgs::LaserScan::ConstPtr& msg)
{
  bool t;
  scan1_mutex_.enter();
    sensor_msgs::PointCloud2 cloud;
    laser_projector_.projectLaser(*msg, cloud);
    t = pcl_ros::transformPointCloud (alg_.config_.base_frame, cloud, cloud1_, tfl_);
  scan1_mutex_.exit();
   if (t){
     new_scan1_ = true;
   }
}

/*  [service callbacks] */

/*  [action callbacks] */

/*  [action requests] */

void LasersToPointcloudAlgNode::node_config_update(Config &config, uint32_t level)
{
  this->alg_.lock();

  this->alg_.unlock();
}

void LasersToPointcloudAlgNode::addNodeDiagnostics(void)
{
}

/* main function */
int main(int argc,char *argv[])
{
  return algorithm_base::main<LasersToPointcloudAlgNode>(argc, argv, "lasers_to_pointcloud_alg_node");
}