hole_detection_alg_node.cpp

Go to the documentation of this file.

#include "hole_detection_alg_node.h"

HoleDetectionAlgNode::HoleDetectionAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<HoleDetectionAlgorithm>(),hole_min_p(60),box_y_end(0),box_z_ini(1),
  box_y_ini(-0.1),box_z_end(1.8),box_x(0.9)
{
  //init class attributes if necessary
  //this->loop_rate_ = 2;//in [Hz]

  // [init publishers]
  this->hole_obstacle_publisher_ = this->public_node_handle_.advertise<sensor_msgs::PointCloud>("hole_obstacle", 10);
  this->hole_all_publisher_ = this->public_node_handle_.advertise<sensor_msgs::PointCloud2>("hole_zone", 10);
  // [init subscribers]
  this->input_subscriber_ = this->public_node_handle_.subscribe("input", 10, &HoleDetectionAlgNode::input_callback, this);
 
        
  // [init services]
  
  // [init clients]
  
  // [init action servers]
  
  // [init action clients]
}

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

void HoleDetectionAlgNode::mainNodeThread(void)
{
  // [fill msg structures]
  //this->PointCloud2_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]
}
//PointCloud2_msg_
/*  [subscriber callbacks] */
void HoleDetectionAlgNode::input_callback(const sensor_msgs::PointCloud2::ConstPtr& msg) 
{ 
  ROS_INFO("HoleDetectionAlgNode::input_callback: New Message Received"); 



  pcl::fromROSMsg (*msg, cloud_in);

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_for (new pcl::PointCloud<pcl::PointXYZ>);
  cloud_for->header.frame_id = cloud_in.header.frame_id;
  cloud_for->is_dense=false;
  cloud_for->header.stamp = ros::Time::now ();
  
 //use appropiate mutex to shared variables if necessary 
 
  this->alg_.lock(); 

  cloud_in_rgb.points.resize(cloud_in.size());
  for (size_t i = 0; i < cloud_in.points.size(); i++) {
    cloud_in_rgb.points[i].x = cloud_in.points[i].x;
    cloud_in_rgb.points[i].y = cloud_in.points[i].y;
    cloud_in_rgb.points[i].z = cloud_in.points[i].z;
    cloud_in_rgb.points[i].r = 0;
    cloud_in_rgb.points[i].g = 0;
    cloud_in_rgb.points[i].b = 0;
  }
  cloud_in_rgb.header.frame_id = cloud_in.header.frame_id;
  cloud_in_rgb.height = cloud_in.height;
  cloud_in_rgb.width = cloud_in.width;
  // cloud_in_rgb.fields=
  // cloud_in_rgb.is_bigendian=
  // cloud_in_rgb.point_step=
  // cloud_in_rgb.row_step=
  // cloud_in_rgb.is_dense=

  cloud_in_rgb.is_dense=false;
  cloud_in_rgb.header.stamp = ros::Time::now ();

  hole_detect.cloud_all(hole_min_p, box_y_end, box_z_ini, box_x, box_y_ini, box_z_end, Xl, Xc, Xr, Y, cloud_in_rgb, cloud_for);
  //unlock previously blocked shared variables 
  this->alg_.unlock();  
  

        pcl::toROSMsg (*cloud_for, PointCloud2_msg_);
  pcl::toROSMsg (cloud_in_rgb, PointCloud2_msg_all);
  
  sensor_msgs::convertPointCloud2ToPointCloud(PointCloud2_msg_, PointCloud_msg_);

  
  this->hole_obstacle_publisher_.publish(this->PointCloud_msg_);
  this->hole_all_publisher_.publish(this->PointCloud2_msg_all);
  
  //use appropiate mutex to shared variables if necessary 
  //this->alg_.lock(); 
  //this->input_mutex_.enter(); 

  //std::cout << msg->data << std::endl; 

  //unlock previously blocked shared variables 
  //this->alg_.unlock(); 
  //this->input_mutex_.exit(); 
}

/*  [service callbacks] */

/*  [action callbacks] */

/*  [action requests] */

void HoleDetectionAlgNode::node_config_update(Config &config, uint32_t level)
{
  this->alg_.lock();
hole_min_p=config.hole_min_p;
box_y_end=config.box_y_end;
box_z_ini=config.box_z_ini;
box_z_end=config.box_z_end;
box_y_ini=config.box_y_ini;
box_x=config.box_x;
  this->alg_.unlock();
}

void HoleDetectionAlgNode::addNodeDiagnostics(void)
{
}

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