height_from_plane_map.cpp

Go to the documentation of this file.

#include "height_from_plane_map.h"

using namespace Eigen;

// [Constructor]
HeightFromPlaneMap::HeightFromPlaneMap(){

  //  cv::namedWindow(WINDOW);

  //string for port names

  // [init subscribers]
  coefficients_sub_.subscribe(nh_,ros::names::append(ros::this_node::getName(),"ModelCoefficients/input"), 1);
  pcl2_sub_.subscribe(nh_, ros::names::append(ros::this_node::getName(),"pcl2/input"), 1);
  sync_ = new message_filters::TimeSynchronizer<pcl::ModelCoefficients, sensor_msgs::PointCloud2>(coefficients_sub_, pcl2_sub_, 10);
  sync_->registerCallback(boost::bind(&HeightFromPlaneMap::height_map_callback, this, _1, _2));

  // [init publishers]
  pcl2_pub_ = nh_.advertise<sensor_msgs::PointCloud2>( ros::names::append(ros::this_node::getName(),"pcl2/output"), 1);

  // [init services]
  dynamic_reconfigure::Server<iri_pcl_filters::IriPclFiltersParametersConfig>::CallbackType reconfig_callback_type = boost::bind(&HeightFromPlaneMap::reconfigureCallback, this, _1, _2);

  dyn_reconfig_srv.setCallback(reconfig_callback_type); 

  // [init clients]
  
  // [init action servers]
  
  // [init action clients]

  ROS_INFO("starting height_from_plane_map");

}

void HeightFromPlaneMap::reconfigureCallback(iri_pcl_filters::IriPclFiltersParametersConfig &config, uint32_t level)
{
  ROS_INFO("Reconfigure request : distance th: %f lasting_cloud: %f #planes: %d ",
           config.distance_threshold, config.cloud_remaining_proportion_threshold, config.max_num_of_planes_removed);

  mutex.enter();
  mutex.exit();
  
}

// [subscriber callbacks]
void HeightFromPlaneMap::height_map_callback(const pcl::ModelCoefficients::ConstPtr& coefficients_msg, const sensor_msgs::PointCloud2::ConstPtr& pcl2_msg) { 

  pcl::fromROSMsg(*pcl2_msg, pcl_xyzrgb_);

  //process pointcloud
  std::vector<float> coefficients = coefficients_msg->values;

  
  pcl::PointCloud<pcl::PointXYZRGB>::iterator pt_iter_xyzrgb = pcl_xyzrgb_.begin();

  for (int rr = 0; rr < pcl_xyzrgb_.height; ++rr) {
    for (int cc = 0; cc < pcl_xyzrgb_.width; ++cc, ++pt_iter_xyzrgb) {
      if(!(isnan(pt_iter_xyzrgb->x) && isnan(pt_iter_xyzrgb->y) && isnan(pt_iter_xyzrgb->z))){
          //Note that we're using the rgb field as 'intensity'
        pt_iter_xyzrgb->rgb = distance_from_plane(pt_iter_xyzrgb->x, pt_iter_xyzrgb->y, pt_iter_xyzrgb->z, coefficients);
      }else{
          //TODO put a different value to nans
        pt_iter_xyzrgb->rgb = 0;
      }
    }
  }

  pcl::toROSMsg(pcl_xyzrgb_, pcl2_score_map_msg_);
  // [publish Point Cloud 2 and Depth Image]
  pcl2_pub_.publish(pcl2_score_map_msg_);
  ROS_INFO("Height score map published ");

}

float HeightFromPlaneMap::distance_from_plane(double x, double y, double z, std::vector<float> coefs){
    //[0] = a, [1] = b, [2] = c, [3] = d, of ax + by + cz + d = 0 plane
   float distance = 0;
   float mod = sqrt(coefs[0]*coefs[0] + coefs[1]*coefs[1] + coefs[2]*coefs[2] + coefs[3]*coefs[3]);
   distance = coefs[0]*x + coefs[1]*y + coefs[2]*z + coefs[3];
   distance = distance/mod;
   //TODO temporary fix: changing distance range for rviz display
   distance = distance*-1000;
   if(distance < 0)
       return -distance;
   return distance; 
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "height_from_plane_map");
  HeightFromPlaneMap height_from_plane_map;
  ros::spin();
  return 0;
}