Skip to content

Latest commit

 

History

History
268 lines (199 loc) · 6.54 KB

08_how_to_decode_online_lidar.md

File metadata and controls

268 lines (199 loc) · 6.54 KB
Raw

8 How to decode online LiDAR

8.1 Introduction

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

For more info, please refer to the demo app rs_driver/demo/demo_online.cpp.

Also refer to the demo app rs_driver/demo/demo_online_multi_lidars.cpp. It is an example of multiple LiDARs.

Here the definitions of point and point cloud, is from the project file.

rs_driver/src/rs_driver/msg/point_cloud_msg.hpp, rs_driver/src/rs_driver/msg/pcl_point_cloud_msg.hpp

8.2 Steps

8.2.1 Define Point

Point is the base unit of Point Cloud. rs_driver supports these member variables.

  • x -- The x coordinate of point. float type.
  • y -- The y coordinate of point. float type.
  • z -- The z coordinate of point. float type.
  • intensity -- The intensity of point. uint8_t type.
  • timestamp -- The timestamp of point. double type. It may be generated by the LiDAR or rs_driver on the host machine.
  • ring -- The ring ID of the point, which represents the row/channel number. Take RS80 as an example, 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.

8.2.2 Define Point Cloud

Below is the definition of point cloud.

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;
};

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

This definition is a template class. It needs a Point type as template parameter.

typedef PointXYZI PointT;
typedef PointCloudT<PointT> PointCloudMsg;

8.2.3 Define the driver object

Define a driver object.

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

8.2.4 Configure the driver parameter

Define a RSDriverParam variable and configure it.

  • InputType::ONLINE_LIDAR means that the driver get packets from an online LiDAR.
  • LidarType::RS16 means a RS16 LiDAR.
  • Also set the local MSOP/DIFOP ports.
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
  ...
}

8.2.5 Define and register Point Cloud callbacks

  • User is supposed to provide free point cloud to rs_driver. Here is the first callback.
SyncQueue<std::shared_ptr<PointCloudMsg>> free_cloud_queue;

std::shared_ptr<PointCloudMsg> driverGetPointCloudFromCallerCallback(void)
{
  std::shared_ptr<PointCloudMsg> msg = free_cloud_queue.pop();
  if (msg.get() != NULL)
  {
    return msg;
  }

  return std::make_shared<PointCloudMsg>();
}
  • rs_driver returns stuffed point cloud to user. Here is the second callback.
SyncQueue<std::shared_ptr<PointCloudMsg>> stuffed_cloud_queue;

void driverReturnPointCloudToCallerCallback(std::shared_ptr<PointCloudMsg> msg)
{
  stuffed_cloud_queue.push(msg);

  RS_MSG << "msg: " << msg->seq << " point cloud size: " << msg->points.size() << RS_REND;
}

Note: The driver calls these two callback functions in the MSOP/DIFOP handling thread, so don't do any time-consuming task in them.

  • User creates a new thread to processes the point cloud.
void processCloud(void)
{
  while (1)
  {
    std::shared_ptr<PointCloudMsg> msg = stuffed_cloud_queue.popWait();
    if (msg.get() == NULL)
    {
      continue;
    }

    // Well, it is time to process the point cloud msg, even it is time-consuming.
    RS_MSG << "msg: " << msg->seq << " point cloud size: " << msg->points.size() << RS_REND;

    free_cloud_queue.push(msg);
  }
}
  • Register them to rs_driver.
int main()
{
  ...
  driver.regPointCloudCallback(driverReturnPointCloudToCallerCallback, 
                               driverReturnPointCloudToCallerCallback);
  ...
}

8.2.6 Define and register exception callbacks

  • When an error happens, rs_driver informs user. Here is the exception callback.
void exceptionCallback(const Error &code)
{
  RS_WARNING << "Error code : " << code.toString() << RS_REND;
}

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

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

8.2.7 Initialize the driver

Initialize rs_driver with the the RSDriverParam object.

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

8.2.8 Start the driver

Start rs_driver.

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

8.2.9 Congratulation

Compile rs_driver and run it. It should print message as below.

RoboSense Lidar-Driver Linux online demo start......
msg: 0 point cloud size: 96
msg: 1 point cloud size: 28800
msg: 2 point cloud size: 28800
msg: 3 point cloud size: 28800
msg: 4 point cloud size: 28800
msg: 5 point cloud size: 28800
msg: 6 point cloud size: 28800
msg: 7 point cloud size: 28832
msg: 8 point cloud size: 28800
msg: 9 point cloud size: 28800

Please refer to Online LiDAR - Advanced Topics for more info about how to configure rs_driver.