puai_wj_2021/velodyne_lidar/velodyne_driver/rawdata.h

// Copyright (C) 2007, 2009, 2010, 2012, 2019 Yaxin Liu, Patrick Beeson, Jack O'Quin, Joshua Whitley
// All rights reserved.
//
// Software License Agreement (BSD License 2.0)
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
//
//  * Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//  * Redistributions in binary form must reproduce the above
//    copyright notice, this list of conditions and the following
//    disclaimer in the documentation and/or other materials provided
//    with the distribution.
//  * Neither the name of {copyright_holder} nor the names of its
//    contributors may be used to endorse or promote products derived
//    from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.

/** @file
 *
 *  @brief Interfaces for interpreting raw packets from the Velodyne 3D LIDAR.
 *
 *  @author Yaxin Liu
 *  @author Patrick Beeson
 *  @author Jack O'Quin
 *  @author Youchen Tang
 */

#ifndef VELODYNE_POINTCLOUD_RAWDATA_H
#define VELODYNE_POINTCLOUD_RAWDATA_H

#include <errno.h>
#include <stdint.h>
#include <string>
#include <math.h>
#include <vector>
#include <mutex>
#include <atomic>
#include <chrono>
#include <glog/logging.h>
#include <Eigen/Core>
// #include <pcl/point_types.h>
// #include <pcl/point_cloud.h>

#include "calibration.h"

struct Lidar_point
{
  double x;
  double y;
  double z;
  uint16_t ring;
  uint16_t azimuth;
  float distance;
  float intensity;
  float timestamp;

  Lidar_point()
  {
    x = y = z = distance = 0.0;
    intensity = 0;
    ring = 0;
    azimuth = 0;
    timestamp = 0;
  }

  Lidar_point(double x, double y, double z, uint16_t ring, uint16_t azimuth, float distance, float intensity, float timestamp)
  {
    this->x = x;
    this->y = y;
    this->z = z;
    this->ring = ring;
    this->azimuth = azimuth;
    this->distance = distance;
    this->intensity = intensity;
    this->timestamp = timestamp;
  }

  Lidar_point(const Lidar_point &point)
  {
    *this = point;
  }

  Lidar_point &operator=(const Lidar_point &point)
  {
    x = point.x;
    y = point.y;
    z = point.z;
    intensity = point.intensity;
    ring = point.ring;
    azimuth = point.azimuth;
    distance = point.distance;
    timestamp = point.timestamp;
    return *this;
  }

  void transform(Eigen::Matrix<double, 4, 4> transform_matrix)
  {
    Eigen::Vector4d point{x, y, z, 1};
    point = transform_matrix * point;
    x = point.x() / point.w();
    y = point.y() / point.w();
    z = point.z() / point.w();
    // x = transform_matrix(0, 0)*point[0] + transform_matrix(0, 1)*point[1]+transform_matrix(0, 2)*point[2] + transform_matrix(0, 3);
    // y = transform_matrix(1, 0)*point[0] + transform_matrix(1, 1)*point[1]+transform_matrix(1, 2)*point[2] + transform_matrix(1, 3);
    // z = transform_matrix(2, 0)*point[0] + transform_matrix(2, 1)*point[1]+transform_matrix(2, 2)*point[2] + transform_matrix(2, 3);
    // printf("point: %.6f %.6f %.6f %.6f \n", x, y, z, point.w());
  }
};

// struct PointXYZIRT
// {
//   PCL_ADD_POINT4D;                // quad-word XYZ
//   float intensity;                ///< laser intensity reading
//   uint16_t ring;                  ///< laser ring number
//   float time;                     ///< laser time reading
//   EIGEN_MAKE_ALIGNED_OPERATOR_NEW // ensure proper alignment
// } EIGEN_ALIGN16;

// POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZIRT,
//                                   (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(uint16_t, ring, ring)(float, time, time))

namespace velodyne_rawdata
{

  /**
 * Raw Velodyne packet constants and structures.
 */
  static const int SIZE_BLOCK = 100;
  static const int RAW_SCAN_SIZE = 3;
  static const int SCANS_PER_BLOCK = 32;
  static const int BLOCK_DATA_SIZE = (SCANS_PER_BLOCK * RAW_SCAN_SIZE);

  static const float ROTATION_RESOLUTION = 0.01f;    // [deg]
  static const uint16_t ROTATION_MAX_UNITS = 36000u; // [deg/100]

  /** @todo make this work for both big and little-endian machines */
  static const uint16_t UPPER_BANK = 0xeeff;
  static const uint16_t LOWER_BANK = 0xddff;

  /** Special Defines for VLP16 support **/
  static const int VLP16_FIRINGS_PER_BLOCK = 2;
  static const int VLP16_SCANS_PER_FIRING = 16;
  static const float VLP16_BLOCK_TDURATION = 110.592f; // [µs]
  static const float VLP16_DSR_TOFFSET = 2.304f;       // [µs]
  static const float VLP16_FIRING_TOFFSET = 55.296f;   // [µs]

  /** \brief Raw Velodyne data block.
 *
 *  Each block contains data from either the upper or lower laser
 *  bank.  The device returns three times as many upper bank blocks.
 *
 *  use stdint.h types, so things work with both 64 and 32-bit machines
 */
  typedef struct raw_block
  {
    uint16_t header;   ///< UPPER_BANK or LOWER_BANK
    uint16_t rotation; ///< 0-35999, divide by 100 to get degrees
    uint8_t data[BLOCK_DATA_SIZE];
  } raw_block_t;

  /** used for unpacking the first two data bytes in a block
 *
 *  They are packed into the actual data stream misaligned.  I doubt
 *  this works on big endian machines.
 */
  union two_bytes
  {
    uint16_t uint;
    uint8_t bytes[2];
  };

  static const int PACKET_SIZE = 1206;
  static const int BLOCKS_PER_PACKET = 12;
  static const int PACKET_STATUS_SIZE = 4;
  static const int SCANS_PER_PACKET = (SCANS_PER_BLOCK * BLOCKS_PER_PACKET);

  /** Special Definitions for VLS128 support **/
  // These are used to detect which bank of 32 lasers is in this block
  static const uint16_t VLS128_BANK_1 = 0xeeff;
  static const uint16_t VLS128_BANK_2 = 0xddff;
  static const uint16_t VLS128_BANK_3 = 0xccff;
  static const uint16_t VLS128_BANK_4 = 0xbbff;

  static const float VLS128_CHANNEL_TDURATION = 2.665f;   // [µs] Channels corresponds to one laser firing
  static const float VLS128_SEQ_TDURATION = 53.3f;        // [µs] Sequence is a set of laser firings including recharging
  static const float VLS128_TOH_ADJUSTMENT = 8.7f;        // [µs] μs. Top Of the Hour is aligned with the fourth firing group in a firing sequence.
  static const float VLS128_DISTANCE_RESOLUTION = 0.004f; // [m]
  static const float VLS128_MODEL_ID = 161;

  /** \brief Raw Velodyne packet.
 *
 *  revolution is described in the device manual as incrementing
 *    (mod 65536) for each physical turn of the device.  Our device
 *    seems to alternate between two different values every third
 *    packet.  One value increases, the other decreases.
 *
 *  \todo figure out if revolution is only present for one of the
 *  two types of status fields
 *
 *  status has either a temperature encoding or the microcode level
 */
  typedef struct raw_packet
  {
    raw_block_t blocks[BLOCKS_PER_PACKET];
    uint16_t revolution;
    uint8_t status[PACKET_STATUS_SIZE];
  } raw_packet_t;

  /** \brief Velodyne data conversion class */
  class RawData
  {
  public:
    /** configuration parameters */
    typedef struct
    {
      std::string model;
      std::string calibrationFile; ///< calibration file name
      double max_range;            ///< maximum range to publish
      double min_range;            ///< minimum range to publish
      int min_angle;               ///< minimum angle to publish
      int max_angle;               ///< maximum angle to publish

      double tmp_min_angle;
      double tmp_max_angle;
    } Config;

    RawData();
    ~RawData()
    {
    }

    /** \brief Set up for data processing.
   *
   *  Perform initializations needed before data processing can
   *  begin:
   *
   *    - read device-specific angles calibration
   *
   *  @param model model name, i.e. VLP16 32E 32C 128S
   *  @param calib_file absolute path to the calibration file
   *  @param max_range maximum range of point distance, in meter
   *  @param min_range minimum range of point distance, in meter
   *  @param min_angle minimum azimuth of point, in degree
   *  @param max_angle maximum azimuth of point, in degree
   *  @returns an optional calibration
   */
    bool setup(std::string model, std::string calib_file, double max_range, double min_range, int min_angle, int max_angle);

    void setupSinCosCache();
    void setupAzimuthCache();
    bool loadCalibration();

    /** \brief Set up for data processing offline.
   * Performs the same initialization as in setup, in the abscence of a ros::NodeHandle.
   * this method is useful if unpacking data directly from bag files, without passing
   * through a communication overhead.
   *
   * @param calibration_file path to the calibration file
   * @param max_range_ cutoff for maximum range
   * @param min_range_ cutoff for minimum range
   * @returns 0 if successful;
   *           errno value for failure
   */
    int setupOffline(std::string calibration_file, double max_range_, double min_range_);

    // unpack velodyne lidar packets, convert to points and stored in data vector
    void unpack(const unsigned char *pkt, std::vector<Lidar_point> &data);

    void setParameters(double min_range, double max_range, double view_direction, double view_width);

    int scansPerPacket() const;

    void getCloud(std::vector<Lidar_point> &data);

    void getConfig(Config &conf){
      conf.model = config_.model;
      conf.calibrationFile = config_.calibrationFile;
      conf.max_range = config_.max_range;
      conf.min_range = config_.min_range;
      conf.min_angle = config_.min_angle;
      conf.max_angle = config_.max_angle;
      conf.tmp_min_angle = config_.tmp_min_angle;
      conf.tmp_max_angle = config_.tmp_max_angle;
    }

    void getCalib(velodyne_pointcloud::Calibration &calib)
    {
      calib.distance_resolution_m = calibration_.distance_resolution_m;
      calib.laser_corrections_map = calibration_.laser_corrections_map;
      calib.laser_corrections = calibration_.laser_corrections;
      calib.num_lasers = calibration_.num_lasers;
      calib.initialized = calibration_.initialized;
    }

    bool isInitialized() { return mb_initialized; }

  private:
    Config config_;
    bool mb_initialized;

    /**
   * Calibration file
   */
    velodyne_pointcloud::Calibration calibration_;
    float sin_rot_table_[ROTATION_MAX_UNITS];
    float cos_rot_table_[ROTATION_MAX_UNITS];

    // Caches the azimuth percent offset for the VLS-128 laser firings
    float vls_128_laser_azimuth_cache[16];

    // timing offset lookup table
    std::vector<std::vector<float>> timing_offsets;

    /** \brief setup per-point timing offsets
   * 
   *  Runs during initialization and determines the firing time for each point in the scan
   * 
   *  NOTE: Does not support all sensors yet (vlp16, vlp32, and hdl32 are currently supported)
   */
    bool buildTimings();

    /** add private function to handle the VLP16 **/
    // add range limit and utc timestamp by yct
    void unpack_vlp16(const unsigned char *pkt, std::vector<Lidar_point> &data);

    void unpack_vls128(const unsigned char *pkt, std::vector<Lidar_point> &data);

    /** in-line test whether a point is in range */
    inline bool pointInRange(float range)
    {
      return (range >= config_.min_range && range <= config_.max_range);
    }
  };

} // namespace velodyne_rawdata

#endif // VELODYNE_POINTCLOUD_RAWDATA_H