Generation of organized point clouds

[Bart Gallet]

Hello,

Unless I am mistaken, the point clouds generated by this plugin are not organized (i.e. the data is organized as a 2D array, with a height larger than one).

How hard would it be to allow for organized point clouds?

Thanks

Bart

[Kevin Hallenbeck]

The goal of this simulator is to produce data in the exact same format and layout as the ROS driver that interfaces to physical Velodyne LIDAR sensors. Producing organized point clouds would be against this goal.
https://wiki.ros.org/velodyne_pointcloud

[Bart Gallet]

Thanks for your reply Kevin,

my VLP16 (the one in the real world that is) does produce an organized point cloud when I set the parameter organize_cloud:=true, here
https://github.com/ros-drivers/velodyne/blob/master/velodyne_pointcloud/launch/VLP16_points.launch

I get a height of 1824 and a width of 16

Thanks
Bart

[Kevin Hallenbeck]

It looks pretty straightforward. There’s a slightly different PointCloud2 structure, but that’s it.

[Bart Gallet]

I agree - I can take a stab at it. Do you accept 3rd party pull requests?

[Kevin Hallenbeck]

Sure, I’d merge a pull request for this

[Bart Gallet]

Great, since I am doing this on my employer’s dime (IneritialSense), I will be doing this from their Bitbucket account.

Thanks

[Kevin Hallenbeck]

The organize_cloud parameter is added in PR #10. I can merge and create a release when you verify it works in your setup.

[Bart Gallet]

Thanks Kevin

I’ll let you know when I am done.

Bart

[Bart Gallet]

I am running into a segmentation error when I convert it to a pcl::PointCloud<pcl::PointXYZ>

void PointCloudSegmentationRosWrapper::cloudCB(const sensor_msgs::PointCloud2ConstPtr& cloud_msg)

    {
          // Extract point cloud from message
          //
          pcl::PCLPointCloud2* cloud2 = new pcl::PCLPointCloud2;
          pcl_conversions::toPCL(*cloud_msg, *cloud2);


          // Convert the data to pcl::PointCloud<pcl::PointXYZ>
          //
          pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
          pcl::fromPCLPointCloud2(*cloud2,*cloud);      // CRASHES HERE 
    }

Putting the debugger on it, I noticed that the row-step is 9680.

I think the correct value for row-step is 352, we have 16 channels and each point is 22, so 22*16 = 352.

I have also verified this with the data coming from my VLP16.

So what fixed it for me is the following (see // *** NEW **** tags in the code below), but I have not tested this with organize_cloud disabled.

void GazeboRosVelodyneLaser::OnScan(ConstLaserScanStampedPtr& _msg)
{
#if GAZEBO_MAJOR_VERSION >= 7
  const ignition::math::Angle maxAngle = parent_ray_sensor_->AngleMax();
  const ignition::math::Angle minAngle = parent_ray_sensor_->AngleMin();

  const double maxRange = parent_ray_sensor_->RangeMax();
  const double minRange = parent_ray_sensor_->RangeMin();

  const int rayCount = parent_ray_sensor_->RayCount();
  const int rangeCount = parent_ray_sensor_->RangeCount();

  const int verticalRayCount = parent_ray_sensor_->VerticalRayCount();
  const int verticalRangeCount = parent_ray_sensor_->VerticalRangeCount();

  const ignition::math::Angle verticalMaxAngle = parent_ray_sensor_->VerticalAngleMax();
  const ignition::math::Angle verticalMinAngle = parent_ray_sensor_->VerticalAngleMin();
#else
  math::Angle maxAngle = parent_ray_sensor_->GetAngleMax();
  math::Angle minAngle = parent_ray_sensor_->GetAngleMin();

  const double maxRange = parent_ray_sensor_->GetRangeMax();
  const double minRange = parent_ray_sensor_->GetRangeMin();

  const int rayCount = parent_ray_sensor_->GetRayCount();
  const int rangeCount = parent_ray_sensor_->GetRangeCount();

  const int verticalRayCount = parent_ray_sensor_->GetVerticalRayCount();
  const int verticalRangeCount = parent_ray_sensor_->GetVerticalRangeCount();

  const math::Angle verticalMaxAngle = parent_ray_sensor_->GetVerticalAngleMax();
  const math::Angle verticalMinAngle = parent_ray_sensor_->GetVerticalAngleMin();
#endif

  const double yDiff = maxAngle.Radian() - minAngle.Radian();
  const double pDiff = verticalMaxAngle.Radian() - verticalMinAngle.Radian();

  const double MIN_RANGE = std::max(min_range_, minRange);
  const double MAX_RANGE = std::min(max_range_, maxRange);
  const double MIN_INTENSITY = min_intensity_;

  // Populate message fields
  const uint32_t POINT_STEP = 22;
  sensor_msgs::PointCloud2 msg;
  msg.header.frame_id = frame_name_;
  msg.header.stamp = ros::Time(_msg->time().sec(), _msg->time().nsec());
  msg.fields.resize(6);
  msg.fields[0].name = "x";
  msg.fields[0].offset = 0;
  msg.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
  msg.fields[0].count = 1;
  msg.fields[1].name = "y";
  msg.fields[1].offset = 4;
  msg.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
  msg.fields[1].count = 1;
  msg.fields[2].name = "z";
  msg.fields[2].offset = 8;
  msg.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
  msg.fields[2].count = 1;
  msg.fields[3].name = "intensity";
  msg.fields[3].offset = 12;
  msg.fields[3].datatype = sensor_msgs::PointField::FLOAT32;
  msg.fields[3].count = 1;
  msg.fields[4].name = "ring";
  msg.fields[4].offset = 16;
  msg.fields[4].datatype = sensor_msgs::PointField::UINT16;
  msg.fields[4].count = 1;
  msg.fields[5].name = "time";
  msg.fields[5].offset = 18;
  msg.fields[5].datatype = sensor_msgs::PointField::FLOAT32;
  msg.fields[5].count = 1;
  msg.data.resize(verticalRangeCount * rangeCount * POINT_STEP);

  int i, j;
  uint8_t *ptr = msg.data.data();
  msg.height = 0;                                                                             // *** NEW   **** 
  for (i = 0; i < rangeCount; i++) {
    for (j = 0; j < verticalRangeCount; j++) {

      // Range
      double r = _msg->scan().ranges(i + j * rangeCount);
      // Intensity
      double intensity = _msg->scan().intensities(i + j * rangeCount);
      // Ignore points that lay outside range bands or optionally, beneath a
      // minimum intensity level.
      if ((MIN_RANGE >= r) || (r >= MAX_RANGE) || (intensity < MIN_INTENSITY) ) {
        if (!organize_cloud_) {
          continue;
        }
      }

      // Noise
      if (gaussian_noise_ != 0.0) {
        r += gaussianKernel(0,gaussian_noise_);
      }

      // Get angles of ray to get xyz for point
      double yAngle;
      double pAngle;

      if (rangeCount > 1) {
        yAngle = i * yDiff / (rangeCount -1) + minAngle.Radian();
      } else {
        yAngle = minAngle.Radian();
      }

      if (verticalRayCount > 1) {
        pAngle = j * pDiff / (verticalRangeCount -1) + verticalMinAngle.Radian();
      } else {
        pAngle = verticalMinAngle.Radian();
      }

      // pAngle is rotated by yAngle:
      if ((MIN_RANGE < r) && (r < MAX_RANGE)) {
        *((float*)(ptr + 0)) = r * cos(pAngle) * cos(yAngle);
        *((float*)(ptr + 4)) = r * cos(pAngle) * sin(yAngle);
#if GAZEBO_MAJOR_VERSION > 2
        *((float*)(ptr + 8)) = r * sin(pAngle);
#else
        *((float*)(ptr + 8)) = -r * sin(pAngle);
#endif
        *((float*)(ptr + 12)) = intensity;
#if GAZEBO_MAJOR_VERSION > 2
        *((uint16_t*)(ptr + 16)) = j; // ring
#else
        *((uint16_t*)(ptr + 16)) = verticalRangeCount - 1 - j; // ring
#endif
        *((float*)(ptr + 18)) = 0.0; // time
        ptr += POINT_STEP;
      } else if (organize_cloud_) {
        *((float*)(ptr + 0)) = nanf("");
        *((float*)(ptr + 4)) = nanf("");
        *((float*)(ptr + 8)) = nanf("");
        *((float*)(ptr + 12)) = nanf("");
#if GAZEBO_MAJOR_VERSION > 2
        *((uint16_t*)(ptr + 16)) = j; // ring
#else
        *((uint16_t*)(ptr + 16)) = verticalRangeCount - 1 - j; // ring
#endif
        *((float*)(ptr + 18)) = 0.0; // time
        ptr += POINT_STEP;
      }
    }

    if(organize_cloud_)                                                                           // *** NEW   **** 
      msg.height++;                                                                               // *** NEW   **** 
  }

  // Populate message with number of valid points
  if (organize_cloud_) {
    msg.width = verticalRangeCount;
    msg.is_dense = false;
  } else {
    msg.width = 1;
    msg.is_dense = true;
  }
  msg.data.resize(ptr - msg.data.data()); // Shrink to actual size
  msg.point_step = POINT_STEP;
  msg.row_step = POINT_STEP * verticalRangeCount;                                               // *** NEW   **** 
  //msg.row_step = msg.data.size() / msg.width;
  //msg.height = msg.row_step / POINT_STEP;
  msg.is_bigendian = false;

  // Publish output
  pub_.publish(msg);
}

‌

[Kevin Hallenbeck]

How about this fix: https://bitbucket.org/DataspeedInc/velodyne_simulator/commits/48520e30f8722994c7a5ab824d839b0b22af0185

[Bart Gallet]

Kevin,

That fix works.

Thanks

Bart

[Kevin Hallenbeck]

PR #10 merged and release pushed to rosdistro. It should be available in the next sync.

[Bart Gallet]

Thank you Kevin!