Problem while converting PointCloud topic into LaserScan topic with roscpp [closed]

edit

Hello,

I've been working on a standalone source file which is aimed to convert pointcloud topic data into laserscan topic . I was able to manipulate the code that pcl_to_scan package has. However, the laserscan output is observed to decrease over time. I simply can't understand why. Here is the simplified code that I'm currently working on:

#include <ros/ros.h>
#include <boost/foreach.hpp>

#include "pluginlib/class_list_macros.h"
#include "nodelet/nodelet.h"
#include "sensor_msgs/LaserScan.h"
#include "pcl/point_cloud.h"
#include "pcl_ros/point_cloud.h"
#include "pcl/point_types.h"
#include "pcl/ros/conversions.h"
#include <visualization_msgs/Marker.h>
#include <dynamic_reconfigure/server.h>

using namespace std;

typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;

ros::Publisher pub_;
ros::Subscriber sub_;
ros::Publisher marker_pub;

void callback(const PointCloud::ConstPtr& msg) 
{
  sensor_msgs::LaserScanPtr output(new sensor_msgs::LaserScan());

  output->header = msg->header;
  output->header.frame_id = "/laser"; 
  output->angle_min = -M_PI / 1.2 ; //Default: -M_PI/6
  output->angle_max = M_PI / 1.2 ; //Default: M_PI/6
  output->angle_increment = M_PI / 180.0 / (1.0 / 5.0) ; //Default: M_PI / 180.0 / 2.0
  output->time_increment = 0.0;
  output->scan_time = 1.0 / 30.0; //Default: 1.0 / 30.0
  output->range_min = 0.1;
  output->range_max = 10.0;

  uint32_t ranges_size = std::ceil(
          (output->angle_max - output->angle_min)
                / output->angle_increment);
  output->ranges.assign(ranges_size, output->range_max); 

  float min_z = 100;
  float max_z = -100;
  float min_y = 100;
  float max_y = -100;

  double max_height_ = 5;
  double min_height_ = 0;


  BOOST_FOREACH (const pcl::PointXYZ& pt, msg->points){
      float x = pt.x;
      float y = pt.y;
      float z = pt.z;

      if (z < min_z)
          min_z = z;
      if (z > max_z)
          max_z = z;

      if (y < min_y)
          min_y = y;
      if (y > max_y)
          max_y = y;

      if (std::isnan(x) || std::isnan(y) || std::isnan(z)) {
          ROS_INFO("rejected for nan in point(%f, %f, %f)\n", x, y, z);
          continue;
      }

      if (z > max_height_ || z < min_height_) { //max_height_ min_height_
          ROS_INFO("rejected for height %f not in range (%f, %f)\n", z, min_height_, max_height_);
          continue;
      }
      double angle = atan2(y, x);
      if (angle < output->angle_min || angle > output->angle_max) {
          ROS_INFO("rejected for angle %f not in range (%f, %f)\n",
                  angle, output->angle_min, output->angle_max);
          continue;
      }

      int index = (angle - output->angle_min) / output->angle_increment;
      double range_sq = y * y + x * x;
      if (output->ranges[index] * output->ranges[index] > range_sq)
          output->ranges[index] = sqrt(range_sq);

      ROS_INFO("Y: %f %f, Z: %f %f", min_y, max_y, min_z, max_z);

      pub_.publish(output);
  }

}

double min_height_, max_height_;
int32_t u_min_, u_max_;
std::string output_frame_id_;
bool dynamic_set;

int main(int argc, char** argv)
{
  ros::init(argc, argv, "sub_pcl");
  ros::NodeHandle nh;

  ros::Subscriber sub = nh.subscribe<PointCloud>("/cam3d/depth/points", 1, callback); 
  pub_ = nh.advertise<sensor_msgs::LaserScan> ("/scan", 10);

  ros::spin();
}

Although it gives the correct output in the beginning, it gets corrupted and decreases over time. Here is a demonstration of the problem with pictures:

Gazebo model sees a small edge like this:

And here are the rviz outputs over time: (First one is the most trustworthy one)

The output that belongs to this instance is like this:

> angle_min: -1.57079637051 angle_max:
> 1.57079637051 angle_increment: 0.0174532923847 time_increment: 0.0 scan_time ...