Merge branch 'main' into enable-sync-angle

.github/workflows/build-and-test-differential.yaml

@@ -74,7 +74,7 @@ jobs:
 
       - name: Get modified files
         id: get-modified-files
-        uses: tj-actions/changed-files@v36
+        uses: tj-actions/changed-files@v35
         with:
           files: |
             **/*.cpp

.github/workflows/build-and-test.yaml

@@ -24,6 +24,14 @@ jobs:
       - name: Check out repository
         uses: actions/checkout@v3
 
+      - name: Free disk space (Ubuntu)
+        uses: jlumbroso/free-disk-space@v1.2.0
+        with:
+          tool-cache: false
+          dotnet: false
+          swap-storage: false
+          large-packages: false
+
       - name: Remove exec_depend
         uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1
 

README.md

@@ -164,9 +164,8 @@ Parameters shared by all supported models:
 | frequency_ms                   | uint16 | 100             | milliseconds, > 0 | Time per scan                  |
 | packet_mtu_size                | uint16 | 1500            |                   | Packet MTU size                |
 | rotation_speed                 | uint16 | 600             |                   | Rotation speed                 |
-| rotation_speed                 | uint16 | 600             |                   | Rotation speed                 |
 | cloud_min_angle                | uint16 | 0               | degrees [0, 360]  | FoV start angle                |
-| cloud_max_angle                | uint16 | 360             | degrees [0, 360]  | FoV end angle                  |
+| cloud_max_angle                | uint16 | 359             | degrees [0, 360]  | FoV end angle                  |
 | dual_return_distance_threshold | double | 0.1             |                   | Dual return distance threshold |
 | diag_span                      | uint16 | 1000            | milliseconds, > 0 | Diagnostic span                |
 | setup_sensor                   | bool   | True            | True, False       | Configure sensor settings      |
@@ -201,6 +200,8 @@ Parameters shared by all supported models:
 | gnss_port       | uint16 | 2369            |                   | GNSS port       |
 | frequency_ms    | uint16 | 100             | milliseconds, > 0 | Time per scan   |
 | packet_mtu_size | uint16 | 1500            |                   | Packet MTU size |
+| cloud_min_angle | uint16 | 0               | degrees [0, 360]  | FoV start angle |
+| cloud_max_angle | uint16 | 359             | degrees [0, 360]  | FoV end angle   |
 
 #### Driver parameters
 
@@ -210,7 +211,8 @@ Parameters shared by all supported models:
 | calibration_file | string |          |                      | LiDAR calibration file                  |
 | min_range        | double | 0.3      | meters, >= 0.3       | Minimum point range published           |
 | max_range        | double | 300.0    | meters, <= 300.0     | Maximum point range published           |
-| view_width       | double | 360.0    | degrees [0.0, 360.0] | Horizontal FOV centered at `scan_phase` |
+| cloud_min_angle  | uint16 | 0        | degrees [0, 360]     | FoV start angle                         |
+| cloud_max_angle  | uint16 | 359      | degrees [0, 360]     | FoV end angle                           |
 
 ## Software design overview
 

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp

@@ -8,7 +8,6 @@
 #include "nebula_decoders/nebula_decoders_common/nebula_driver_base.hpp"
 #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp"
 
-#include "pandar_msgs/msg/pandar_jumbo_packet.hpp"
 #include "pandar_msgs/msg/pandar_packet.hpp"
 #include "pandar_msgs/msg/pandar_scan.hpp"
 

nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp

@@ -39,7 +39,7 @@ 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 double ROTATION_RESOLUTION = 0.01;     // [deg]
 static const uint16_t ROTATION_MAX_UNITS = 36000u;  // [deg/100]
 
 static const size_t RETURN_MODE_INDEX = 1204;
@@ -149,7 +149,7 @@ class VelodyneScanDecoder
   bool has_scanned_ = true;
   double dual_return_distance_threshold_{};  // Velodyne does this internally, this will not be
                                              // implemented here
-  double first_timestamp{};
+  double scan_timestamp_{};
 
   /// @brief SensorConfiguration for this decoder
   std::shared_ptr<drivers::VelodyneSensorConfiguration> sensor_configuration_;

nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp16_decoder.hpp

@@ -13,6 +13,7 @@ namespace drivers
 {
 namespace vlp16
 {
+  constexpr uint32_t MAX_POINTS = 300000;
 /// @brief Velodyne LiDAR decoder (VLP16)
 class Vlp16Decoder : public VelodyneScanDecoder
 {
@@ -48,8 +49,10 @@ class Vlp16Decoder : public VelodyneScanDecoder
   bool parsePacket(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) override;
   float sin_rot_table_[ROTATION_MAX_UNITS];
   float cos_rot_table_[ROTATION_MAX_UNITS];
+  float rotation_radians_[ROTATION_MAX_UNITS];
   int phase_;
   int max_pts_;
+  std::vector<std::vector<float>> timing_offsets_;
 };
 
 }  // namespace vlp16

nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp32_decoder.hpp

@@ -48,6 +48,8 @@ class Vlp32Decoder : public VelodyneScanDecoder
   bool parsePacket(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) override;
   float sin_rot_table_[ROTATION_MAX_UNITS];
   float cos_rot_table_[ROTATION_MAX_UNITS];
+  float rotation_radians_[ROTATION_MAX_UNITS];
+  std::vector<std::vector<float>> timing_offsets_;
   int phase_;
   int max_pts_;
 };

nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vls128_decoder.hpp

@@ -48,9 +48,11 @@ class Vls128Decoder : public VelodyneScanDecoder
   bool parsePacket(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) override;
   float sin_rot_table_[ROTATION_MAX_UNITS];
   float cos_rot_table_[ROTATION_MAX_UNITS];
+  float rotation_radians_[ROTATION_MAX_UNITS];
   float vls_128_laser_azimuth_cache_[16];
   int phase_;
   int max_pts_;
+  std::vector<std::vector<float>> timing_offsets_;
 };
 
 }  // namespace vls128

nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp

@@ -16,17 +16,40 @@ Vlp16Decoder::Vlp16Decoder(
   sensor_configuration_ = sensor_configuration;
   calibration_configuration_ = calibration_configuration;
 
-  first_timestamp = std::numeric_limits<double>::max();
+  scan_timestamp_ = -1;
 
   scan_pc_.reset(new NebulaPointCloud);
   overflow_pc_.reset(new NebulaPointCloud);
 
   // Set up cached values for sin and cos of all the possible headings
   for (uint16_t rot_index = 0; rot_index < ROTATION_MAX_UNITS; ++rot_index) {
     float rotation = angles::from_degrees(ROTATION_RESOLUTION * rot_index);
+    rotation_radians_[rot_index] = rotation;
     cos_rot_table_[rot_index] = cosf(rotation);
     sin_rot_table_[rot_index] = sinf(rotation);
   }
+  // timing table calculation, from velodyne user manual p.64
+  timing_offsets_.resize(BLOCKS_PER_PACKET);
+  for (size_t i=0; i < timing_offsets_.size(); ++i){
+    timing_offsets_[i].resize(32);
+  }
+  double full_firing_cycle_s = 55.296 * 1e-6;
+  double single_firing_s = 2.304 * 1e-6;
+  double data_block_index, data_point_index;
+  bool dual_mode = sensor_configuration_->return_mode==ReturnMode::DUAL;
+  // compute timing offsets
+  for (size_t x = 0; x < timing_offsets_.size(); ++x){
+    for (size_t y = 0; y < timing_offsets_[x].size(); ++y){
+      if (dual_mode){
+        data_block_index = (x - (x % 2)) + (y / 16);
+      }
+      else{
+        data_block_index = (x * 2) + (y / 16);
+      }
+      data_point_index = y % 16;
+      timing_offsets_[x][y] = (full_firing_cycle_s * data_block_index) + (single_firing_s * data_point_index);
+    }
+  }
 
   phase_ = (uint16_t)round(sensor_configuration_->scan_phase * 100);
 }
@@ -35,19 +58,21 @@ bool Vlp16Decoder::hasScanned() { return has_scanned_; }
 
 std::tuple<drivers::NebulaPointCloudPtr, double> Vlp16Decoder::get_pointcloud()
 {
-  int phase = (uint16_t)round(sensor_configuration_->scan_phase * 100);
+  double phase = angles::from_degrees(sensor_configuration_->scan_phase);
   if (!scan_pc_->points.empty()) {
-    uint16_t current_azimuth = (int)scan_pc_->points.back().azimuth * 100;
-    uint16_t phase_diff = (36000 + current_azimuth - phase) % 36000;
-    while (phase_diff < 18000 && scan_pc_->points.size() > 0) {
+    auto current_azimuth = scan_pc_->points.back().azimuth;
+    auto phase_diff = static_cast<size_t>(angles::to_degrees(2*M_PI + current_azimuth - phase)) % 360;
+    while (phase_diff < M_PI_2 && !scan_pc_->points.empty()) {
       overflow_pc_->points.push_back(scan_pc_->points.back());
       scan_pc_->points.pop_back();
-      current_azimuth = (int)scan_pc_->points.back().azimuth * 100;
-      phase_diff = (36000 + current_azimuth - phase) % 36000;
+      current_azimuth = scan_pc_->points.back().azimuth;
+      phase_diff = static_cast<size_t>(angles::to_degrees(2*M_PI + current_azimuth - phase)) % 360;
     }
     overflow_pc_->width = overflow_pc_->points.size();
+    scan_pc_->width = scan_pc_->points.size();
+    scan_pc_->height = 1;
   }
-  return std::make_tuple(scan_pc_, first_timestamp);
+  return std::make_tuple(scan_pc_, scan_timestamp_);
 }
 
 int Vlp16Decoder::pointsPerPacket()
@@ -61,7 +86,7 @@ void Vlp16Decoder::reset_pointcloud(size_t n_pts)
   max_pts_ = n_pts * pointsPerPacket();
   scan_pc_->points.reserve(max_pts_);
   reset_overflow();  // transfer existing overflow points to the cleared pointcloud
-  first_timestamp = std::numeric_limits<double>::max();
+  scan_timestamp_ = -1;
 }
 
 void Vlp16Decoder::reset_overflow()
@@ -138,6 +163,11 @@ void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa
             other_return.bytes[1] =
               block % 2 ? raw->blocks[block - 1].data[k + 1] : raw->blocks[block + 1].data[k + 1];
           }
+          // Apply timestamp if this is the first new packet in the scan.
+          auto block_timestamp = rclcpp::Time(velodyne_packet.stamp).seconds();
+          if (scan_timestamp_ < 0) {
+            scan_timestamp_ = block_timestamp;
+          }
           // Do not process if there is no return, or in dual return mode and the first and last
           // echos are the same.
           if (
@@ -193,14 +223,9 @@ void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa
                 const float x_coord = xy_distance * cos_rot_angle;     // velodyne y
                 const float y_coord = -(xy_distance * sin_rot_angle);  // velodyne x
                 const float z_coord = distance * sin_vert_angle;       // velodyne z
-                const float intensity = current_block.data[k + 2];
+                const uint8_t intensity = current_block.data[k + 2];
 
-                const double time_stamp =
-                  (block * 2 + firing) * 55.296 / 1000.0 / 1000.0 + dsr * 2.304 / 1000.0 / 1000.0;
-                auto ts = rclcpp::Time(velodyne_packet.stamp).seconds();
-                if (ts < first_timestamp) {
-                  first_timestamp = ts;
-                }
+                double point_time_offset =  timing_offsets_[block][firing * 16 + dsr];
 
                 // Determine return type.
                 uint8_t return_type;
@@ -212,12 +237,12 @@ void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa
                        other_return.bytes[1] == current_return.bytes[1])) {
                       return_type = static_cast<uint8_t>(drivers::ReturnType::IDENTICAL);
                     } else {
-                      const float other_intensity = block % 2 ? raw->blocks[block - 1].data[k + 2]
+                      const uint8_t other_intensity = block % 2 ? raw->blocks[block - 1].data[k + 2]
                                                               : raw->blocks[block + 1].data[k + 2];
-                      bool first = other_return.uint < current_return.uint ? 0 : 1;
-                      bool strongest = other_intensity < intensity ? 1 : 0;
+                      bool first = current_return.uint > other_return.uint ;
+                      bool strongest = intensity > other_intensity;
                       if (other_intensity == intensity) {
-                        strongest = first ? 0 : 1;
+                        strongest = !first;
                       }
                       if (first && strongest) {
                         return_type = static_cast<uint8_t>(drivers::ReturnType::FIRST_STRONGEST);
@@ -247,8 +272,12 @@ void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa
                 current_point.z = z_coord;
                 current_point.return_type = return_type;
                 current_point.channel = corrections.laser_ring;
-                current_point.azimuth = azimuth_corrected;
-                current_point.time_stamp = time_stamp;
+                current_point.azimuth = rotation_radians_[azimuth_corrected];
+                current_point.elevation = sin_vert_angle;
+                auto point_ts = block_timestamp - scan_timestamp_ + point_time_offset;
+                if (point_ts < 0)
+                  point_ts = 0;
+                current_point.time_stamp = static_cast<uint32_t>(point_ts*1e9);
                 current_point.intensity = intensity;
                 current_point.distance = distance;
                 scan_pc_->points.emplace_back(current_point);