Ask Your Question
0

Cloud to Scan: Some Points won't show up in laser scan

asked 2013-05-27 02:21:34 -0500

madmax gravatar image

Hi,
I am trying to tranfsorm my point cloud to a laser scan withe the pointcloud_to_laserscan package.
But although I tell the node to go from -Pi to Pi, some points don't show up in the laser scan.
I also set the range to 60m, so that shouldn't be the problem either.

Here is my point cloud:
image description

And here is my laser scan: image description

The code were the transformation is done also looks correct, so I don't know were the problem could be... The only thing I changed were the assigning of the coordinates, because I dont use a Kinect, I had to switch y and z.

 void callback(const PointCloud::ConstPtr& cloud)
 {
sensor_msgs::LaserScanPtr output(new sensor_msgs::LaserScan());
output->header = cloud->header;
output->header.frame_id = output_frame_id_; // Set output frame. Point clouds come from "optical" frame, scans come from corresponding mount frame
output->angle_min = angle_min_;
output->angle_max = angle_max_;
output->angle_increment = angle_increment_;
output->time_increment = 0.0;
output->scan_time = scan_time_;
output->range_min = range_min_;
output->range_max = range_max_;

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

for (PointCloud::const_iterator it = cloud->begin(); it != cloud->end(); ++it)
{
  const float &x = it->x;
  const float &y = it->z;
  const float &z = it->y;

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

  if (-y > max_height_ || -y < min_height_)
  {
    NODELET_DEBUG("rejected for height %f not in range (%f, %f)\n", x, min_height_, max_height_);
    continue;
  }

  double range_sq = z*z+x*x;

  if (range_sq < range_min_sq_) {
    NODELET_DEBUG("rejected for range %f below minimum value %f. Point: (%f, %f, %f)", range_sq, range_min_sq_, x, y, z);
    continue;
  }

  double angle = -atan2(x, z) + 1.57;
  if (angle < output->angle_min || angle > output->angle_max)
  {
    NODELET_DEBUG("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;


  if (output->ranges[index] * output->ranges[index] > range_sq)
    output->ranges[index] = sqrt(range_sq);
  }

pub_.publish(output);
}

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2013-05-27 03:38:53 -0500

Ben_S gravatar image

Judging by the line double angle = -atan2(x, z) + 1.57; your angle will be in the range of [-pi/2 ; 3/2 * pi]. I dont know, why pi/2 is added there, but if you want to receive a full 360 degrees scan, you should try setting min_angle to -pi/2 and max_angle to 3/2 * pi.

edit flag offensive delete link more

Comments

Yes, why didn't I see this :) But I had to delete the + pi/2 to get it work for me. Now I have a 360 degree scan but twisted per pi/2 ;)

madmax gravatar image madmax  ( 2013-05-27 04:17:34 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

Stats

Asked: 2013-05-27 02:21:34 -0500

Seen: 193 times

Last updated: May 27 '13