Merge remote-tracking branch 'origin/develop' into hesai-better-fov

.github/workflows/documentation.yml

@@ -1,3 +1,4 @@
+# cspell: ignore nojekyll
 name: documentation
 on:
   push:

nebula_common/include/nebula_common/hesai/hesai_common.hpp

@@ -17,13 +17,15 @@
 
 #include "nebula_common/nebula_common.hpp"
 #include "nebula_common/nebula_status.hpp"
+#include "nebula_common/util/string_conversions.hpp"
 
 #include <algorithm>
 #include <cmath>
 #include <fstream>
 #include <iostream>
 #include <map>
 #include <sstream>
+#include <stdexcept>
 #include <string>
 #include <tuple>
 #include <vector>
@@ -428,28 +430,34 @@ inline ReturnMode ReturnModeFromStringHesai(
   const std::string & return_mode, const SensorModel & sensor_model)
 {
   switch (sensor_model) {
+    case SensorModel::HESAI_PANDARXT32:
     case SensorModel::HESAI_PANDARXT32M:
-    case SensorModel::HESAI_PANDARAT128:
+    case SensorModel::HESAI_PANDAR128_E3X:
     case SensorModel::HESAI_PANDAR128_E4X:
     case SensorModel::HESAI_PANDARQT128:
       if (return_mode == "Last") return ReturnMode::LAST;
       if (return_mode == "Strongest") return ReturnMode::STRONGEST;
-      if (return_mode == "LastStrongest") return ReturnMode::DUAL_LAST_STRONGEST;
+      if (return_mode == "Dual" || return_mode == "LastStrongest")
+        return ReturnMode::DUAL_LAST_STRONGEST;
       if (return_mode == "First") return ReturnMode::FIRST;
       if (return_mode == "LastFirst") return ReturnMode::DUAL_LAST_FIRST;
       if (return_mode == "FirstStrongest") return ReturnMode::DUAL_FIRST_STRONGEST;
-      if (return_mode == "Dual") return ReturnMode::DUAL;
       break;
     case SensorModel::HESAI_PANDARQT64:
       if (return_mode == "Last") return ReturnMode::LAST;
-      if (return_mode == "Dual") return ReturnMode::DUAL;
+      if (return_mode == "Dual" || return_mode == "LastFirst") return ReturnMode::DUAL_LAST_FIRST;
       if (return_mode == "First") return ReturnMode::FIRST;
       break;
-    default:
+    case SensorModel::HESAI_PANDARAT128:
+    case SensorModel::HESAI_PANDAR64:
+    case SensorModel::HESAI_PANDAR40P:
       if (return_mode == "Last") return ReturnMode::LAST;
       if (return_mode == "Strongest") return ReturnMode::STRONGEST;
-      if (return_mode == "Dual") return ReturnMode::DUAL;
+      if (return_mode == "Dual" || return_mode == "LastStrongest")
+        return ReturnMode::DUAL_LAST_STRONGEST;
       break;
+    default:
+      throw std::runtime_error("Unsupported sensor model: " + util::to_string(sensor_model));
   }
 
   return ReturnMode::UNKNOWN;
@@ -462,8 +470,9 @@ inline ReturnMode ReturnModeFromStringHesai(
 inline ReturnMode ReturnModeFromIntHesai(const int return_mode, const SensorModel & sensor_model)
 {
   switch (sensor_model) {
+    case SensorModel::HESAI_PANDARXT32:
     case SensorModel::HESAI_PANDARXT32M:
-    case SensorModel::HESAI_PANDARAT128:
+    case SensorModel::HESAI_PANDAR128_E3X:
     case SensorModel::HESAI_PANDAR128_E4X:
     case SensorModel::HESAI_PANDARQT128:
       if (return_mode == 0) return ReturnMode::LAST;
@@ -475,14 +484,18 @@ inline ReturnMode ReturnModeFromIntHesai(const int return_mode, const SensorMode
       break;
     case SensorModel::HESAI_PANDARQT64:
       if (return_mode == 0) return ReturnMode::LAST;
-      if (return_mode == 2) return ReturnMode::DUAL;
+      if (return_mode == 2) return ReturnMode::DUAL_LAST_FIRST;
       if (return_mode == 3) return ReturnMode::FIRST;
       break;
-    default:
+    case SensorModel::HESAI_PANDARAT128:
+    case SensorModel::HESAI_PANDAR64:
+    case SensorModel::HESAI_PANDAR40P:
       if (return_mode == 0) return ReturnMode::LAST;
       if (return_mode == 1) return ReturnMode::STRONGEST;
-      if (return_mode == 2) return ReturnMode::DUAL;
+      if (return_mode == 2) return ReturnMode::DUAL_LAST_STRONGEST;
       break;
+    default:
+      throw std::runtime_error("Unsupported sensor model: " + util::to_string(sensor_model));
   }
 
   return ReturnMode::UNKNOWN;
@@ -495,28 +508,34 @@ inline ReturnMode ReturnModeFromIntHesai(const int return_mode, const SensorMode
 inline int IntFromReturnModeHesai(const ReturnMode return_mode, const SensorModel & sensor_model)
 {
   switch (sensor_model) {
+    case SensorModel::HESAI_PANDARXT32:
     case SensorModel::HESAI_PANDARXT32M:
-    case SensorModel::HESAI_PANDARAT128:
+    case SensorModel::HESAI_PANDAR128_E3X:
     case SensorModel::HESAI_PANDAR128_E4X:
     case SensorModel::HESAI_PANDARQT128:
       if (return_mode == ReturnMode::LAST) return 0;
       if (return_mode == ReturnMode::STRONGEST) return 1;
-      if (return_mode == ReturnMode::DUAL_LAST_STRONGEST || return_mode == ReturnMode::DUAL)
+      if (return_mode == ReturnMode::DUAL || return_mode == ReturnMode::DUAL_LAST_STRONGEST)
         return 2;
       if (return_mode == ReturnMode::FIRST) return 3;
       if (return_mode == ReturnMode::DUAL_LAST_FIRST) return 4;
       if (return_mode == ReturnMode::DUAL_FIRST_STRONGEST) return 5;
       break;
     case SensorModel::HESAI_PANDARQT64:
       if (return_mode == ReturnMode::LAST) return 0;
-      if (return_mode == ReturnMode::DUAL) return 2;
+      if (return_mode == ReturnMode::DUAL || return_mode == ReturnMode::DUAL_LAST_FIRST) return 2;
       if (return_mode == ReturnMode::FIRST) return 3;
       break;
-    default:
+    case SensorModel::HESAI_PANDARAT128:
+    case SensorModel::HESAI_PANDAR64:
+    case SensorModel::HESAI_PANDAR40P:
       if (return_mode == ReturnMode::LAST) return 0;
       if (return_mode == ReturnMode::STRONGEST) return 1;
-      if (return_mode == ReturnMode::DUAL) return 2;
+      if (return_mode == ReturnMode::DUAL || return_mode == ReturnMode::DUAL_LAST_STRONGEST)
+        return 2;
       break;
+    default:
+      throw std::runtime_error("Unsupported sensor model: " + util::to_string(sensor_model));
   }
 
   return -1;

nebula_common/include/nebula_common/util/string_conversions.hpp

@@ -0,0 +1,44 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#pragma once
+
+#include <ostream>
+#include <sstream>
+#include <string>
+#include <type_traits>
+
+namespace nebula::util
+{
+
+template <typename T, typename = void>
+struct IsStreamable : std::false_type
+{
+};
+
+template <typename T>
+struct IsStreamable<T, std::void_t<decltype(std::declval<std::ostream &>() << std::declval<T>())> >
+: std::true_type
+{
+};
+
+template <typename T>
+std::enable_if_t<IsStreamable<T>::value, std::string> to_string(const T & value)
+{
+  std::stringstream ss{};
+  ss << value;
+  return ss.str();
+}
+
+}  // namespace nebula::util

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp

@@ -35,7 +35,7 @@ enum ReturnMode {
   DUAL_FIRST_LAST = 0x3b,
   DUAL_FIRST_STRONGEST = 0x3c,
   TRIPLE_FIRST_LAST_STRONGEST = 0x3d,
-  DUAL_STRONGEST_SECONDSTRONGEST = 0x3,
+  DUAL_STRONGEST_SECONDSTRONGEST = 0x3e,
 };
 }  // namespace return_mode
 

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

@@ -69,6 +69,10 @@ static const float VLP16_BLOCK_DURATION = 110.592f;  // [µs]
 static const float VLP16_DSR_TOFFSET = 2.304f;       // [µs]
 static const float VLP16_FIRING_TOFFSET = 55.296f;   // [µs]
 
+/** Special Defines for VLP32 support **/
+static const float VLP32_CHANNEL_DURATION = 2.304f;  // [µs]
+static const float VLP32_SEQ_DURATION = 55.296f;     // [µs]
+
 /** Special Definitions for VLS128 support **/
 static const float VLP128_DISTANCE_RESOLUTION = 0.004f;  // [m]
 

nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp

@@ -226,10 +226,15 @@ void Vlp16Decoder::unpack(const std::vector<uint8_t> & packet, int32_t packet_se
               distance > sensor_configuration_->min_range &&
               distance < sensor_configuration_->max_range) {
               // Correct for the laser rotation as a function of timing during the firings.
-              const float azimuth_corrected_f =
+              float azimuth_corrected_f =
                 azimuth +
                 (azimuth_diff * ((dsr * VLP16_DSR_TOFFSET) + (firing * VLP16_FIRING_TOFFSET)) /
-                 VLP16_BLOCK_DURATION);
+                 VLP16_BLOCK_DURATION) -
+                corrections.rot_correction * 180.0 / M_PI * 100;
+
+              if (azimuth_corrected_f < 0.0) {
+                azimuth_corrected_f += 36000.0;
+              }
               const uint16_t azimuth_corrected =
                 (static_cast<uint16_t>(round(azimuth_corrected_f))) % 36000;
 
@@ -245,13 +250,8 @@ void Vlp16Decoder::unpack(const std::vector<uint8_t> & packet, int32_t packet_se
                 // Convert polar coordinates to Euclidean XYZ.
                 const float cos_vert_angle = corrections.cos_vert_correction;
                 const float sin_vert_angle = corrections.sin_vert_correction;
-                const float cos_rot_correction = corrections.cos_rot_correction;
-                const float sin_rot_correction = corrections.sin_rot_correction;
-
-                const float cos_rot_angle = cos_rot_table_[azimuth_corrected] * cos_rot_correction +
-                                            sin_rot_table_[azimuth_corrected] * sin_rot_correction;
-                const float sin_rot_angle = sin_rot_table_[azimuth_corrected] * cos_rot_correction -
-                                            cos_rot_table_[azimuth_corrected] * sin_rot_correction;
+                const float cos_rot_angle = cos_rot_table_[azimuth_corrected];
+                const float sin_rot_angle = sin_rot_table_[azimuth_corrected];
 
                 // Compute the distance in the xy plane (w/o accounting for rotation).
                 const float xy_distance = distance * cos_vert_angle;

nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp

@@ -138,16 +138,44 @@ void Vlp32Decoder::unpack(const std::vector<uint8_t> & packet, int32_t packet_se
   checkAndHandleScanComplete(packet, packet_seconds, phase_);
 
   const raw_packet_t * raw = (const raw_packet_t *)packet.data();
+  float last_azimuth_diff = 0;
+  uint16_t azimuth_next;
   uint8_t return_mode = packet[RETURN_MODE_INDEX];
   const bool dual_return = (return_mode == RETURN_MODE_DUAL);
 
-  for (int i = 0; i < BLOCKS_PER_PACKET; i++) {
+  for (uint i = 0; i < BLOCKS_PER_PACKET; i++) {
     int bank_origin = 0;
     if (raw->blocks[i].header == LOWER_BANK) {
       // lower bank lasers are [32..63]
       bank_origin = 32;
     }
-    for (int j = 0, k = 0; j < SCANS_PER_BLOCK; j++, k += RAW_SCAN_SIZE) {
+    float azimuth_diff;
+    uint16_t azimuth;
+
+    // Calculate difference between current and next block's azimuth angle.
+    if (i == 0) {
+      azimuth = raw->blocks[i].rotation;
+    } else {
+      azimuth = azimuth_next;
+    }
+    if (i < static_cast<uint>(BLOCKS_PER_PACKET - (1 + dual_return))) {
+      // Get the next block rotation to calculate how far we rotate between blocks
+      azimuth_next = raw->blocks[i + (1 + dual_return)].rotation;
+
+      // Finds the difference between two successive blocks
+      azimuth_diff = static_cast<float>((36000 + azimuth_next - azimuth) % 36000);
+
+      // This is used when the last block is next to predict rotation amount
+      last_azimuth_diff = azimuth_diff;
+    } else {
+      // This makes the assumption the difference between the last block and the next packet is the
+      // same as the last to the second to last.
+      // Assumes RPM doesn't change much between blocks.
+      azimuth_diff =
+        (i == static_cast<uint>(BLOCKS_PER_PACKET - (4 * dual_return) - 1)) ? 0 : last_azimuth_diff;
+    }
+
+    for (uint j = 0, k = 0; j < SCANS_PER_BLOCK; j++, k += RAW_SCAN_SIZE) {
       float x, y, z;
       uint8_t intensity;
       const uint8_t laser_number = j + bank_origin;
@@ -201,13 +229,17 @@ void Vlp32Decoder::unpack(const std::vector<uint8_t> & packet, int32_t packet_se
             raw->blocks[i].rotation >= sensor_configuration_->cloud_min_angle * 100))) {
           const float cos_vert_angle = corrections.cos_vert_correction;
           const float sin_vert_angle = corrections.sin_vert_correction;
-          const float cos_rot_correction = corrections.cos_rot_correction;
-          const float sin_rot_correction = corrections.sin_rot_correction;
+          float azimuth_corrected_f =
+            azimuth + (azimuth_diff * VLP32_CHANNEL_DURATION / VLP32_SEQ_DURATION * j) -
+            corrections.rot_correction * 180.0 / M_PI * 100;
+          if (azimuth_corrected_f < 0) {
+            azimuth_corrected_f += 36000;
+          }
+          const uint16_t azimuth_corrected =
+            (static_cast<uint16_t>(std::round(azimuth_corrected_f))) % 36000;
 
-          const float cos_rot_angle = cos_rot_table_[block.rotation] * cos_rot_correction +
-                                      sin_rot_table_[block.rotation] * sin_rot_correction;
-          const float sin_rot_angle = sin_rot_table_[block.rotation] * cos_rot_correction -
-                                      cos_rot_table_[block.rotation] * sin_rot_correction;
+          const float cos_rot_angle = cos_rot_table_[azimuth_corrected];
+          const float sin_rot_angle = sin_rot_table_[azimuth_corrected];
 
           const float horiz_offset = corrections.horiz_offset_correction;
           const float vert_offset = corrections.vert_offset_correction;

nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp

@@ -247,9 +247,14 @@ void Vls128Decoder::unpack(const std::vector<uint8_t> & packet, int32_t packet_s
           }
 
           // Correct for the laser rotation as a function of timing during the firings.
-          const float azimuth_corrected_f =
-            azimuth + (azimuth_diff * vls_128_laser_azimuth_cache_[firing_order]);
-          const uint16_t azimuth_corrected = ((uint16_t)round(azimuth_corrected_f)) % 36000;
+          float azimuth_corrected_f = azimuth +
+                                      (azimuth_diff * vls_128_laser_azimuth_cache_[firing_order]) -
+                                      corrections.rot_correction * 180.0 / M_PI * 100;
+
+          if (azimuth_corrected_f < 0.0) {
+            azimuth_corrected_f += 36000.0;
+          }
+          const uint16_t azimuth_corrected = ((uint16_t)std::round(azimuth_corrected_f)) % 36000;
 
           if (
             distance > sensor_configuration_->min_range &&
@@ -266,13 +271,8 @@ void Vls128Decoder::unpack(const std::vector<uint8_t> & packet, int32_t packet_s
               // convert polar coordinates to Euclidean XYZ.
               const float cos_vert_angle = corrections.cos_vert_correction;
               const float sin_vert_angle = corrections.sin_vert_correction;
-              const float cos_rot_correction = corrections.cos_rot_correction;
-              const float sin_rot_correction = corrections.sin_rot_correction;
-
-              const float cos_rot_angle = cos_rot_table_[azimuth_corrected] * cos_rot_correction +
-                                          sin_rot_table_[azimuth_corrected] * sin_rot_correction;
-              const float sin_rot_angle = sin_rot_table_[azimuth_corrected] * cos_rot_correction -
-                                          cos_rot_table_[azimuth_corrected] * sin_rot_correction;
+              const float cos_rot_angle = cos_rot_table_[azimuth_corrected];
+              const float sin_rot_angle = sin_rot_table_[azimuth_corrected];
 
               // Compute the distance in the xy plane (w/o accounting for rotation).
               const float xy_distance = distance * cos_vert_angle;

nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp

@@ -2,6 +2,7 @@
 
 #include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp"
 
+#include <sstream>
 #include <stdexcept>
 
 // #define WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE
@@ -246,7 +247,9 @@ boost::property_tree::ptree HesaiHwInterface::ParseJson(const std::string & str)
 {
   boost::property_tree::ptree tree;
   try {
-    boost::property_tree::read_json(str, tree);
+    std::stringstream ss;
+    ss << str;
+    boost::property_tree::read_json(ss, tree);
   } catch (boost::property_tree::json_parser_error & e) {
     std::cerr << e.what() << std::endl;
   }
@@ -496,7 +499,7 @@ HesaiLidarRangeAll HesaiHwInterface::GetLidarRange()
     throw std::runtime_error("Response payload too short");
   }
 
-  HesaiLidarRangeAll hesai_range_all;
+  HesaiLidarRangeAll hesai_range_all{};
   hesai_range_all.method = response[0];
   switch (hesai_range_all.method) {
     case 0:  // for all channels
@@ -640,6 +643,12 @@ HesaiLidarMonitor HesaiHwInterface::GetLidarMonitor()
   }
 
   auto response_or_err = SendReceive(PTC_COMMAND_LIDAR_MONITOR);
+
+  // FIXME(mojomex): this is a hotfix for sensors that do not support this command
+  if (!response_or_err.has_value()) {
+    return HesaiLidarMonitor{};
+  }
+
   auto response = response_or_err.value_or_throw(PrettyPrintPTCError(response_or_err.error_or({})));
   return CheckSizeAndParse<HesaiLidarMonitor>(response);
 }

nebula_ros/schema/Pandar40P.schema.json

@@ -65,6 +65,7 @@
           "enum": [
             "Last",
             "Strongest",
+            "LastStrongest",
             "Dual"
           ]
         },

nebula_ros/schema/Pandar64.schema.json

@@ -62,6 +62,7 @@
           "enum": [
             "Last",
             "Strongest",
+            "LastStrongest",
             "Dual"
           ]
         },