How to decode online Lidar

1 Introduction

This document illustrates how to decode the MSOP/DIFOP packets from an online Lidar with the driver's API.

2 Design Consideration

The driver is designed to achieve these objectives.

Parallel the construction and the process of point cloud.
Avoid to copy point cloud.
Avoid to malloc/free point cloud repeatly.

Below is the supposed interaction between rs_driver and user's code.

rs_driver runs in its thread construct_thread. It

Gets free point cloud from user. User fetches it from a free cloud queue free_point_cloud_queue.
Parses packets and constructs point cloud.
Returns stuffed point cloud to user.
User's code is supposed to shift it to the queue stuffed_point_cloud_queue.

User's code runs in its thread process_thread. It

Fetches stuffed point cloud from the queue stuffed_point_cloud_queue
Process the point cloud
Return the point cloud back to the queue free_point_cloud_queue.

3 Steps

Below is the steps to use the driver's API.

3.1 Define Point Type

Point is the base unit of Point Cloud. The driver supposes that it may have these member variables.

x -- float type. The x coordinate of point.
y -- float type. The y coordinate of point.
z -- float type. The z coordinate of point.
intensity -- uint8_t type. The intensity of point.
timestamp -- double type. The timestamp of point. It may generated by the Lidar or on the host machine.
ring -- The ring ID of the point, which represents the row number. e.g. For RS80, the range of ring ID is 0~79 (from bottom to top).

Below are some examples:

The point type contains x, y, z, intensity

  struct PointXYZI
  {
    float x;
    float y;
    float z;
    uint8_t intensity;
    ...
  };

If using PCL, a simple way is to use pcl::PointXYZI.
The point type contains x, y, z, intensity, timestamp, ring

  struct PointXYZIRT
  {
    float x;
    float y;
    float z;
    uint8_t intensity;
    double timestamp;
    uint16_t ring;
    ...
  };

Here user may add new member variables, remove member variables, or change the order of them, but should not change names or types of them, otherwise the behaviors might be unexpected.

3.2 Define Point Cloud Type

The driver allows user to change the point cloud type, as long as it is compatible to the definition below.

  template <typename T_Point>
  class PointCloudT
  {
  public:
    typedef T_Point PointT;
    typedef std::vector<PointT> VectorT;

    uint32_t height = 0;    ///< Height of point cloud
    uint32_t width = 0;     ///< Width of point cloud
    bool is_dense = false;  ///< If is_dense is true, the point cloud does not contain NAN points
    double timestamp = 0.0; ///< Timestamp of point cloud
    uint32_t seq = 0;       ///< Sequence number of message

    VectorT points;
  };
  
  typedef PointXYZI PointT;
  typedef PointCloudT<PointT> PointCloudMsg;

Here user may add new members, and change the order of these members, but should not change or remove them.

3.3 Point in Point Cloud

Here is an example of 5_lasers Lidar.

The Lidar scans block by block. It starts from b0c0(block 0 channel/laser 0), b0c1, b0c2, b0c3, b0c4, and then go to b1c0, b1c1, b1c2, b1c3, b1c4, and so on.

At the same time, it saves the points in a point vector, point by point, as below.

3.4 Define the driver object

Define a driver object.

int main()
{
  LidarDriver<PointCloudMsg> driver;          ///< Declare the driver object
  ...
}

3.5 Configure the driver parameter

Define a RSDriverParam variable and configure it.

InputType::ONLINE_LIDAR means that the driver get packets from a online Lidar.
LidarType::RS16 means a RS16 Lidar.
Also set the local MSOP/DIFOP ports. Please use a 3rd party tool, such as Wireshark, to get them.

int main()
{
  ...
  RSDriverParam param;                             ///< Create a parameter object
  param.input_type = InputType::ONLINE_LIDAR;      /// get packet from online lidar
  param.input_param.msop_port = 6699;             ///< Set the lidar msop port number, the default is 6699
  param.input_param.difop_port = 7788;            ///< Set the lidar difop port number, the default is 7788
  param.lidar_type = LidarType::RS16;             ///< Set the lidar type. Make sure this type is correct
  ...
}

3.6 Define and register Point Cloud callbacks

As said above, user is supposed to provide free point cloud to the driver.

std::shared_ptr<PointCloudMsg> pointCloudGetCallback(void)
{
  return std::make_shared<PointCloudMsg>();
}

Also user is supposed to accept stuffed point cloud from the driver.

void pointCloudPutCallback(std::shared_ptr<PointCloudMsg> msg)
{
  RS_MSG << "msg: " << msg->seq << " point cloud size: " << msg->points.size() << RS_REND;
}

Note:

For simplicity, free_point_cloud_queue/stuffed_point_cloud_queue are not implemented here. This is NOT suggested.
The driver calls these two callback functions in its contruct_thread, so don't do any time-consuming task in them.

int main()
{
  ...
  driver.regPointCloudCallback(pointCloudGetCallback, pointCloudPutCallback); ///< Register the point cloud callback functions
  ...
}

3.7 Define and register exception callbacks

When an error happens, the driver will inform user. User is supposed to get it via a callback function.

void exceptionCallback(const Error &code)
{
  RS_WARNING << "Error code : " << code.toString() << RS_REND;
}

Once again, don't do any time-consuming task in it.

int main()
{
  ...
driver.regExceptionCallback(exceptionCallback);  ///<Register the exception callback function
  ...
}

3.8 Initialize the driver

Call the initialization function with the the RSDriverParam object.

int main()
{
  ...
  if (!driver.init(param))                         ///< Call the init function and pass the parameter
  {
    RS_ERROR << "Driver Initialize Error..." << RS_REND;
    return -1;
  }
  ...
}

Please register call callback functions before call init(), because errors may occur during the initialization.

3.9 Start the driver

Start the driver.

int main()
{
  ...
  driver.start();                                  ///< Call the start function. The driver thread will start
  ...
}

4 Congratulation

You have finished the demo tutorial of the RoboSense LiDAR driver!

Please find the complete demo code rs_driver/demo/demo_online.cpp in the project directory.

Feel free to contact us if you have any questions.

how_to_decode_online_lidar.md 6.7 KB Vēsture Neapstrādāts