Robotics StackExchange | Archived questions

Problem while converting PointCloud topic into LaserScan topic with roscpp

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 pcltoscan 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:

image description

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

image description

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: 0.0333333350718 range_min:
> 0.10000000149 range_max: 10.0 ranges: [0.8371890783309937,
> 0.8371157646179199, 0.8371505737304688, 0.841155469417572, 0.8416852355003357, 0.8424785137176514, 0.7871079444885254, 0.7877516150474548, 0.7892191410064697, 0.7909254431724548, 0.7928680777549744, 0.7950447797775269, 0.7974526882171631, 0.8000890612602234, 0.8029510378837585, 0.8060353398323059, 0.8093387484550476, 0.8137710094451904, 0.8175548315048218, 0.8215464353561401, 0.8268224000930786, 0.831267774105072, 0.8370987176895142, 0.8419783115386963, 0.8483396172523499, 0.8549846410751343, 0.8604996800422668, 0.8676356673240662, 0.8750333786010742, 0.882685124874115, 0.892191469669342, 0.900374710559845, 0.9087872505187988, 0.9191746711730957, 0.9280658960342407, 0.939006507396698, 0.9502314925193787, 0.9617291688919067, 0.9734877347946167, 0.9875211715698242, 0.9998075366020203, 1.014427661895752, 1.0293407440185547, 1.0445315837860107, 1.0622141361236572, 1.0779519081115723, 1.0962239503860474, 1.1171224117279053, 1.1359834671020508, 1.157501220703125, 1.1817551851272583, 1.2038718461990356, 1.2287416458129883, 1.256429672241211, 1.2844353914260864, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0, 11.0]

image description

image description

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: 0.0333333350718 range_min:
> 0.10000000149 range_max: 10.0 ranges: [0.11726024001836777,
> 0.05766232684254646, 0.04177795350551605, 0.02987378090620041, 0.025909584015607834, 0.021949056535959244, 0.017994627356529236, 0.046077899634838104, 0.01405144389718771, 0.0584421269595623, 0.03429332748055458, 0.010132638737559319, 0.026504002511501312, 0.04287064075469971, 0.03901761397719383, 0.022650254890322685, 0.035191018134355545, 0.060264796018600464, 0.006283995695412159, 0.05273519456386566, 0.04019985720515251, 0.027660993859171867, 0.036499835550785065, 0.015129179693758488, 0.023996248841285706, 0.03286304697394371, 0.09480049461126328, 0.0381767563521862, 0.02931303158402443, 0.020446542650461197, 0.01158352755010128, 0.04898853227496147, 0.02588552236557007, 0.05444787070155144, 0.03138109669089317, 0.017083730548620224, 0.0369064100086689, 0.022636234760284424, 0.04800030216574669, 0.02820809744298458, 0.03378802165389061, 0.04495486989617348, 0.05611952021718025, 0.08399122208356857, 0.1616191416978836, 0.0028102879878133535, 0.08396531641483307, 0.05610218644142151, 0.044940974563360214, 0.033777572214603424, 0.028199369087815285, 0.047970641404390335, 0.022629227489233017, 0.03688358888030052, 0.017078444361686707, 0.03136168420314789, 0.054380614310503006, 0.02586950734257698, 0.048928000032901764, 0.011579941026866436, 0.020433885976672173, 0.029285836964845657, 0.03812955319881439, 0.09447945654392242, 0.03282240405678749, 0.02397397719323635, 0.01511981338262558, 0.036443427205085754, 0.027626775205135345, 0.040125325322151184, 0.05260495841503143, 0.006282049231231213, 0.06007887050509453, 0.03512575477361679, 0.022622225806117058, 0.03893321752548218, 0.04276470094919205, 0.026463015004992485, 0.01012636348605156, 0.034219127148389816, 0.05822576582431793, 0.014038392342627048, 0.04593560844659805, 0.017972351983189583, 0.021915104240179062, 0.025861503556370735, 0.029809128493070602, 0.041648901998996735, 0.05741326883435249, 0.11621594429016113, 0.11621594429016113, 0.05741326883435249, 0.041648901998996735, 0.029809124767780304, 0.025861501693725586, 0.021915104240179062, 0.017972351983189583, 0.045935604721307755, 0.014038392342627048, 0.05822575092315674, 0.03421912342309952, 0.01012636348605156, 0.026463011279702187, 0.04276468977332115, 0.03893321007490158, 0.02262222208082676, 0.035125747323036194, 0.060078851878643036, 0.006282049231231213, 0.05260493978857994, 0.04012531787157059, 0.027626771479845047, 0.03644341602921486, 0.015119810588657856, 0.023973971605300903, 0.032822392880916595, 11.0, 0.0381295420229435, 0.029285825788974762, 0.020433882251381874, 0.011579939164221287, 11.0, 0.025869499891996384, 11.0, 0.03136167302727699, 0.017078440636396408, 0.03688357397913933, 0.02262921817600727, 11.0, 0.02819935604929924, 0.03377755358815193, 11.0, 11.0, 11.0, 11.0, 0.0028102879878133535, 11.0, 11.0, 11.0, 11.0, 0.028208084404468536, 11.0, 0.02263622358441353, 11.0, 0.017083728685975075, 11.0, 11.0, 0.025885511189699173, 11.0, 0.011583525687456131, 0.02044653333723545, 11.0, 11.0, 11.0, 11.0, 0.02399623766541481, 0.015129175037145615, 11.0, 11.0, 11.0, 11.0, 0.006283994298428297, 11.0, 11.0, 0.02265024557709694, 11.0, 11.0, 11.0, 0.01013263687491417, 11.0, 11.0, 0.014051438309252262, 11.0, 0.01799461990594864, 0.021949047222733498, 11.0, 11.0, 11.0, 11.0, 11.0]

image description

Note that we are simulating the kinect input with gazeborosopenni_kinect. It gives /cam3d/depth/points output. So, all we have to do is to convert that pointcloud topic into a steady laserscan topic.

I can't understand what goes wrong. Is it a problem with the conversion or the subscribe/advertise ? Any help will be greately appreciated.

Asked by mozcelikors on 2013-08-18 13:55:58 UTC

Comments

Answers