Thor LiDAR Plugin: No Point Cloud in sample_lidar_replay

[guyuechao]

DRIVE OS Version: 7.0.3

Issue Description:

I developed a new LiDAR plugin with reference to the NVLidar Plugin.

However, the point cloud fails to render when using the sample_lidar_replay tool.

I added debug logs to verify that parseDataBuffer is invoked successfully, and point cloud data is written to the output interface.

How can I further troubleshoot the plugin code?
As I understand it, parseDataBuffer is the final step for the plugin to process point clouds.

[SivaRamaKrishnaNV]

Dear @guyuechao ,
Does that mean issue is with recording data or rendering only? Also, could you please attach logs and code as text in topic instead of image.

[guyuechao]

Dear @SivaRamaKrishnaNV

Thank you for your reply.

The same code logic works correctly for rendering in the build artifact targeting the Orin platform.

Below is the implementation of the parseDataBuffer function.

//################################################################################
//###################### Lidar Specific Plugin Functions #########################
//################################################################################
dwStatus DemoLidar::parseDataBuffer(dwLidarDecodedPacket* output, const uint64_t hostTimestamp) {
  while (m_packetQueue.size() > 0) {
    // Get pointer to next packet in queue
    rawPacket* p_rawPacket = &(m_packetQueue.front());
    InnoDataPacket *pkt = reinterpret_cast<InnoDataPacket *>(&(p_rawPacket->rawData[PAYLOAD_OFFSET]));

    if (m_currentIdx == -1) {
      m_currentIdx = pkt->idx;
      m_frameTimestamp = pkt->common.ts_start_us;
    }

    if (pkt->idx != m_currentIdx) {
      m_currentIdx = pkt->idx;
      p_rawPacket = nullptr;
      m_packetsPerFrame = m_packetsCounter;
      m_packetsCounter = 0;

      output->hostTimestamp = hostTimestamp;
      // microseconds
      output->sensorTimestamp = static_cast<int64_t>(m_frameTimestamp);
      m_frameTimestamp = pkt->common.ts_start_us;
      output->maxPoints = MAX_POINTS_PER_SPIN;
      output->nPoints = m_lidarOutput.n_points;
      output->scanComplete = true;

      dwLidarPointXYZI* xyzi = const_cast<dwLidarPointXYZI*>(output->pointsXYZI);
      dwLidarPointRTHI* rthi = const_cast<dwLidarPointRTHI*>(output->pointsRTHI);
      for (size_t i = 0; i < output->nPoints; ++i) {
        xyzi[i].x = m_lidarOutput.xyzi[i].x;
        xyzi[i].y = m_lidarOutput.xyzi[i].y;
        xyzi[i].z = m_lidarOutput.xyzi[i].z;
        xyzi[i].intensity = m_lidarOutput.xyzi[i].intensity;

        rthi[i].radius = m_lidarOutput.rthi[i].radius;
        rthi[i].theta = m_lidarOutput.rthi[i].theta;
        rthi[i].phi = m_lidarOutput.rthi[i].phi;
        rthi[i].intensity = m_lidarOutput.rthi[i].intensity;
      }
      log_info("frame complete, idx=%d, n_points=%d, packets_per_frame=%d", pkt->idx, output->nPoints,
                    m_packetsPerFrame);
      log_info("point: x=%f, y=%f, z=%f, intensity=%f", m_lidarOutput.xyzi[0].x, m_lidarOutput.xyzi[0].y,
                    m_lidarOutput.xyzi[0].z, m_lidarOutput.xyzi[0].intensity);
      std::memset(&m_lidarOutput, 0, sizeof(LidarPacket));
      m_lidarOutput.n_points = 0;  // ensure n_points is reset
      return DW_SUCCESS;
    }

    m_packetsCounter++;

    // parse data and store in m_lidarOutput
    parseDataPacket(pkt);

    // Release pointer to packet
    p_rawPacket = nullptr;
    // Remove packet from queue
    m_packetQueue.pop();
  }
  return DW_NOT_READY;
}

[guyuechao]

One additional observation to add:

I compared the source code of the sample_lidar_replay tool for Thor and Orin inside the Docker container, located at the path:

/usr/local/driveworks/samples/src/sensors/lidar/lidar_replay/

The left side of the image shows the Thor version, and the right side shows the Orin version.

• Thor: LiDAR data must be stored in returnData in the DwLidarDecodedReturn format using spherical coordinates. The lidar_replay tool internally converts these to Cartesian coordinates for visualization.
• Orin: Raw XYZ Cartesian coordinate points are stored directly and rendered as point clouds without conversion.

There is a significant discrepancy in how published point cloud data is stored. However, the corresponding Plugin sample code for Thor contains no matching modifications to accommodate this difference.

[guyuechao]

I populated the returnData based on the original Plugin code, and the content can now display properly.

The code is shown below: