Ask Your Question
0

Laser scan has one point

asked 2012-06-11 06:22:16 -0500

allenh1 gravatar image

updated 2012-06-11 07:37:46 -0500

While using pointcloud to laserscan, I am encountering problems. The scan is only outputting one point... How do I fix this?

Here is my Bag file. It contains the pointcloud and the single point laser scan. It should definitely have more points in the output scan, but I have no idea how to make it so...


I believe I have located the issue... In the cloud_to_scan.cpp file, the following code is found:

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->y;
      const float &z = it->z;

      if ( std::isnan(x) || std::isnan(y) || std::isnan(z) )
      {
        NODELET_DEBUG("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);
      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);
  }

it appears to be using the x and z for conversion... My code works in the x and y plane, not x and z. How should I fix this?

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2012-06-12 02:36:18 -0500

felix k gravatar image

updated 2012-06-12 23:27:53 -0500

In the laserscan message definition it says the following:

                     # in frame frame_id, angles are measured around 
                     # the positive Z axis (counterclockwise, if Z is up)
                     # with zero angle being forward along the x axis

So if you want to stay conform, you'd have to change your code to use the x-y-plane. If your scanner is turned mounted, adjust its tf frame to reflect that position.


Updated..

First, your bag file only contains only the laserscan, so I couldn't reproduce everything. rosbag info CloudAndScan.bag shows some stats.

The pointcloud indeed is expected to be in a camera frame. See parameters docs and part 2) on this answer. I think you only have to adjust your frame information.

If it won't work, I'd ask you to update your question with your pointcloud_to_laserscan parameters and upload a bag with scan, cloud and tf.

edit flag offensive delete link more

Comments

The code I posted is not mine. I coded in the x-y-plane.

allenh1 gravatar imageallenh1 ( 2012-06-12 03:44:54 -0500 )edit

Oh, sorry for misunderstanding, updating my answer in a moment..

felix k gravatar imagefelix k ( 2012-06-12 23:02:26 -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: 2012-06-11 06:22:16 -0500

Seen: 548 times

Last updated: Jun 12 '12