feat(hesai): add support for xt16

docs/supported_sensors.md

@@ -17,6 +17,7 @@ The `sensor_model` parameter below decides which sensor driver is launched.
 | ------------ | -------------- | ----------------------- | ----------- |
 | Pandar64     | Pandar64       | Pandar64.param.yaml     | ⚠️          |
 | Pandar 40P   | Pandar40P      | Pandar40P.param.yaml    | ✅          |
+| Pandar XT16  | PandarXT16     | PandarXT16.param.yaml   | ⚠️          |
 | Pandar XT32  | PandarXT32     | PandarXT32.param.yaml   | ✅          |
 | Pandar XT32M | PandarXT32M    | PandarXT32M.param.yaml  | ⚠️          |
 | Pandar QT64  | PandarQT64     | PandarQT64.param.yaml   | ✅          |

mkdocs.yml

@@ -48,6 +48,7 @@ nav:
           - QT64: parameters/vendors/hesai/qt64.md
           - QT128: parameters/vendors/hesai/qt128.md
           - AT128: parameters/vendors/hesai/at128.md
+          - XT16: parameters/vendors/hesai/xt16.md
           - XT32: parameters/vendors/hesai/xt32.md
           - XT32M: parameters/vendors/hesai/xt32m.md
       - Velodyne:

nebula_common/include/nebula_common/hesai/hesai_common.hpp

@@ -433,6 +433,7 @@ inline ReturnMode return_mode_from_string_hesai(
   const std::string & return_mode, const SensorModel & sensor_model)
 {
   switch (sensor_model) {
+    case SensorModel::HESAI_PANDARXT16:
     case SensorModel::HESAI_PANDARXT32:
     case SensorModel::HESAI_PANDARXT32M:
     case SensorModel::HESAI_PANDAR128_E3X:
@@ -474,6 +475,7 @@ inline ReturnMode return_mode_from_int_hesai(
   const int return_mode, const SensorModel & sensor_model)
 {
   switch (sensor_model) {
+    case SensorModel::HESAI_PANDARXT16:
     case SensorModel::HESAI_PANDARXT32:
     case SensorModel::HESAI_PANDARXT32M:
     case SensorModel::HESAI_PANDAR128_E3X:
@@ -513,6 +515,7 @@ inline int int_from_return_mode_hesai(
   const ReturnMode return_mode, const SensorModel & sensor_model)
 {
   switch (sensor_model) {
+    case SensorModel::HESAI_PANDARXT16:
     case SensorModel::HESAI_PANDARXT32:
     case SensorModel::HESAI_PANDARXT32M:
     case SensorModel::HESAI_PANDAR128_E3X:

nebula_common/include/nebula_common/nebula_common.hpp

@@ -326,6 +326,7 @@ enum class SensorModel {
   HESAI_PANDAR40M,
   HESAI_PANDARQT64,
   HESAI_PANDARQT128,
+  HESAI_PANDARXT16,
   HESAI_PANDARXT32,
   HESAI_PANDARXT32M,
   HESAI_PANDARAT128,
@@ -393,6 +394,9 @@ inline std::ostream & operator<<(std::ostream & os, nebula::drivers::SensorModel
     case SensorModel::HESAI_PANDARQT128:
       os << "PandarQT128";
       break;
+    case SensorModel::HESAI_PANDARXT16:
+      os << "PandarXT16";
+      break;
     case SensorModel::HESAI_PANDARXT32:
       os << "PandarXT32";
       break;
@@ -558,6 +562,7 @@ inline SensorModel sensor_model_from_string(const std::string & sensor_model)
   if (sensor_model == "Pandar64") return SensorModel::HESAI_PANDAR64;
   if (sensor_model == "Pandar40P") return SensorModel::HESAI_PANDAR40P;
   if (sensor_model == "Pandar40M") return SensorModel::HESAI_PANDAR40M;
+  if (sensor_model == "PandarXT16") return SensorModel::HESAI_PANDARXT16;
   if (sensor_model == "PandarXT32") return SensorModel::HESAI_PANDARXT32;
   if (sensor_model == "PandarXT32M") return SensorModel::HESAI_PANDARXT32M;
   if (sensor_model == "PandarAT128") return SensorModel::HESAI_PANDARAT128;
@@ -592,6 +597,8 @@ inline std::string sensor_model_to_string(const SensorModel & sensor_model)
       return "Pandar40P";
     case SensorModel::HESAI_PANDAR40M:
       return "Pandar40M";
+    case SensorModel::HESAI_PANDARXT16:
+      return "PandarXT16";
     case SensorModel::HESAI_PANDARXT32:
       return "PandarXT32";
     case SensorModel::HESAI_PANDARXT32M:

nebula_decoders/calibration/hesai/PandarXT16.csv

@@ -0,0 +1,17 @@
+Channel,Elevation,Azimuth
+1,15,0
+2,13,0
+3,11,0
+4,9,0
+5,7,0
+6,5,0
+7,3,0
+8,1,0
+9,-1,0
+10,-3,0
+11,-5,0
+12,-7,0
+13,-9,0
+14,-11,0
+15,-13,0
+16,-15,0

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt16.hpp

@@ -0,0 +1,84 @@
+// 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 "nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp"
+#include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_sensor.hpp"
+#include "nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32.hpp"
+
+#include <vector>
+
+namespace nebula::drivers
+{
+
+namespace hesai_packet
+{
+
+#pragma pack(push, 1)
+
+using TailXT16 = TailXT32;
+
+struct PacketXT16 : public PacketBase<8, 16, 2, 100>
+{
+  using body_t = Body<Block<Unit4B, PacketXT16::n_channels>, PacketXT16::n_blocks>;
+  Header12B header;
+  body_t body;
+  TailXT16 tail;
+  uint32_t udp_sequence;
+};
+
+#pragma pack(pop)
+
+}  // namespace hesai_packet
+
+class PandarXT16 : public HesaiSensor<hesai_packet::PacketXT16>
+{
+public:
+  static constexpr float min_range = 0.05f;
+  static constexpr float max_range = 120.0f;
+  static constexpr size_t max_scan_buffer_points = 128000;
+
+  int get_packet_relative_point_time_offset(
+    uint32_t block_id, uint32_t channel_id, const packet_t & packet) override
+  {
+    auto n_returns = hesai_packet::get_n_returns(packet.tail.return_mode);
+    int block_offset_ns = 5632 - 50000 * ((8 - block_id - 1) / n_returns);
+    int channel_offset_ns = 368 + 3024 * channel_id;
+    return block_offset_ns + channel_offset_ns;
+  }
+
+  ReturnType get_return_type(
+    hesai_packet::return_mode::ReturnMode return_mode, unsigned int return_idx,
+    const std::vector<const typename packet_t::body_t::block_t::unit_t *> & return_units) override
+  {
+    auto return_type =
+      HesaiSensor<packet_t>::get_return_type(return_mode, return_idx, return_units);
+    if (return_type == ReturnType::IDENTICAL) {
+      return return_type;
+    }
+
+    // This sensor orders returns in the opposite order, so the return_type needs to be flipped too
+    if (return_mode == hesai_packet::return_mode::DUAL_FIRST_LAST) {
+      if (return_type == ReturnType::FIRST)
+        return_type = ReturnType::LAST;
+      else if (return_type == ReturnType::LAST)
+        return_type = ReturnType::FIRST;
+    }
+
+    return return_type;
+  }
+};
+
+}  // namespace nebula::drivers

nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp

@@ -10,6 +10,7 @@
 #include "nebula_decoders/nebula_decoders_hesai/decoders/pandar_at128.hpp"
 #include "nebula_decoders/nebula_decoders_hesai/decoders/pandar_qt128.hpp"
 #include "nebula_decoders/nebula_decoders_hesai/decoders/pandar_qt64.hpp"
+#include "nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt16.hpp"
 #include "nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32.hpp"
 #include "nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32m.hpp"
 
@@ -38,6 +39,9 @@ HesaiDriver::HesaiDriver(
     case SensorModel::HESAI_PANDARQT128:
       scan_decoder_ = initialize_decoder<PandarQT128>(sensor_configuration, calibration_data);
       break;
+    case SensorModel::HESAI_PANDARXT16:
+      scan_decoder_ = initialize_decoder<PandarXT16>(sensor_configuration, calibration_data);
+      break;
     case SensorModel::HESAI_PANDARXT32:
       scan_decoder_ = initialize_decoder<PandarXT32>(sensor_configuration, calibration_data);
       break;

nebula_examples/launch/hesai_offline.xml

@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 <launch>
-    <arg name="sensor_model" description="Pandar64|Pandar40P|PandarXT32|PandarXT32M|PandarAT128|PandarQT64"/>
+    <arg name="sensor_model" description="Pandar64|Pandar40P|PandarXT16|PandarXT32|PandarXT32M|PandarAT128|PandarQT64"/>
     <arg name="return_mode" default="Dual" description="See readme for supported return modes"/>
     <arg name="frame_id"  default="hesai"/>
     <arg name="scan_phase" default="0.0" />

nebula_examples/launch/hesai_offline_bag_pcd.xml

@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 <launch>
-    <arg name="sensor_model" description="Pandar64|Pandar40P|PandarXT32|PandarXT32M|PandarAT128|PandarQT64"/>
+    <arg name="sensor_model" description="Pandar64|Pandar40P|PandarXT16|PandarXT32|PandarXT32M|PandarAT128|PandarQT64"/>
     <arg name="return_mode" default="Dual" description="See readme for supported return modes"/>
     <arg name="frame_id"  default="hesai"/>
     <arg name="scan_phase" default="0.0" />

nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp

@@ -304,7 +304,7 @@ struct HesaiInventory_OT128 : public HesaiInventoryBase
   Internal value;
 };
 
-struct HesaiInventory_XT32_40P : public HesaiInventoryBase
+struct HesaiInventory_XT16_32_40P : public HesaiInventoryBase
 {
   struct Internal : public HesaiInventoryBase::Internal
   {
@@ -315,7 +315,7 @@ struct HesaiInventory_XT32_40P : public HesaiInventoryBase
     uint8_t reserved[11];
   };
 
-  explicit HesaiInventory_XT32_40P(Internal value) : value(value) {}
+  explicit HesaiInventory_XT16_32_40P(Internal value) : value(value) {}
 
   [[nodiscard]] uint8_t model_number() const override { return value.product_model; }
 

nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp

@@ -95,6 +95,7 @@ const uint8_t TCP_ERROR_INCOMPLETE_RESPONSE = 8;
 
 const uint16_t PANDARQT64_PACKET_SIZE = 1072;
 const uint16_t PANDARQT128_PACKET_SIZE = 1127;
+const uint16_t PANDARXT16_PACKET_SIZE = 568;
 const uint16_t PANDARXT32_PACKET_SIZE = 1080;
 const uint16_t PANDARXT32M_PACKET_SIZE = 820;
 const uint16_t PANDARAT128_PACKET_SIZE = 1118;

nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp

@@ -349,10 +349,11 @@ std::shared_ptr<HesaiInventoryBase> HesaiHwInterface::GetInventory()
 
   switch (sensor_configuration_->sensor_model) {
     default:
+    case SensorModel::HESAI_PANDARXT16:
     case SensorModel::HESAI_PANDARXT32:
     case SensorModel::HESAI_PANDAR40P: {
-      auto lidar_config = CheckSizeAndParse<HesaiInventory_XT32_40P::Internal>(response);
-      return std::make_shared<HesaiInventory_XT32_40P>(lidar_config);
+      auto lidar_config = CheckSizeAndParse<HesaiInventory_XT16_32_40P::Internal>(response);
+      return std::make_shared<HesaiInventory_XT16_32_40P>(lidar_config);
     }
     case SensorModel::HESAI_PANDARQT128: {
       auto lidar_config = CheckSizeAndParse<HesaiInventory_QT128::Internal>(response);
@@ -379,6 +380,7 @@ std::shared_ptr<HesaiConfigBase> HesaiHwInterface::GetConfig()
     case SensorModel::HESAI_PANDAR40P:
     case SensorModel::HESAI_PANDAR64:
     case SensorModel::HESAI_PANDARQT128:
+    case SensorModel::HESAI_PANDARXT16:
     case SensorModel::HESAI_PANDARXT32: {
       auto lidar_config = CheckSizeAndParse<HesaiConfig_XT_40P_64_QT128::Internal>(response);
       return std::make_shared<HesaiConfig_XT_40P_64_QT128>(lidar_config);
@@ -400,6 +402,7 @@ std::shared_ptr<HesaiLidarStatusBase> HesaiHwInterface::GetLidarStatus()
     default:
     case SensorModel::HESAI_PANDAR40P:
     case SensorModel::HESAI_PANDAR64:
+    case SensorModel::HESAI_PANDARXT16:
     case SensorModel::HESAI_PANDARXT32: {
       auto hesai_lidarstatus = CheckSizeAndParse<HesaiLidarStatus_XT_40p::Internal>(response);
       return std::make_shared<HesaiLidarStatus_XT_40p>(hesai_lidarstatus);
@@ -1034,6 +1037,7 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig(
         sensor_configuration->sensor_model == SensorModel::HESAI_PANDAR40P ||
         sensor_configuration->sensor_model == SensorModel::HESAI_PANDAR64 ||
         sensor_configuration->sensor_model == SensorModel::HESAI_PANDARQT64 ||
+        sensor_configuration->sensor_model == SensorModel::HESAI_PANDARXT16 ||
         sensor_configuration->sensor_model == SensorModel::HESAI_PANDARXT32 ||
         sensor_configuration->sensor_model == SensorModel::HESAI_PANDARXT32M) {
         logger_->info("Trying to set Clock source to PTP");
@@ -1197,6 +1201,8 @@ int HesaiHwInterface::NebulaModelToHesaiModelNo(nebula::drivers::SensorModel mod
       return 17;
     case SensorModel::HESAI_PANDARXT32:
       return 25;
+    case SensorModel::HESAI_PANDARXT16:
+      return 26;
     case SensorModel::HESAI_PANDARQT128:
       return 32;
     case SensorModel::HESAI_PANDARXT32M:

nebula_ros/config/lidar/hesai/PandarXT16.param.yaml

@@ -0,0 +1,29 @@
+/**:
+  ros__parameters:
+    host_ip: 255.255.255.255
+    sensor_ip: 192.168.1.201
+    multicast_ip: ""
+    data_port: 2368
+    gnss_port: 10110
+    packet_mtu_size: 1500
+    launch_hw: true
+    setup_sensor: true
+    udp_only: false
+    frame_id: hesai
+    diag_span: 1000
+    min_range: 0.05
+    max_range: 120.0
+    cloud_min_angle: 0
+    cloud_max_angle: 360
+    sync_angle: 0
+    cut_angle: 0.0
+    sensor_model: PandarXT16
+    calibration_file: $(find-pkg-share nebula_decoders)/calibration/hesai/$(var sensor_model).csv
+    rotation_speed: 600
+    return_mode: Dual
+    ptp_profile: 1588v2
+    ptp_domain: 0
+    ptp_transport_type: UDP
+    ptp_switch_type: TSN
+    retry_hw: true
+    dual_return_distance_threshold: 0.1

nebula_ros/config/lidar/hesai/PandarXT32.param.yaml

@@ -11,8 +11,8 @@
     udp_only: false
     frame_id: hesai
     diag_span: 1000
-    min_range: 0.3
-    max_range: 300.0
+    min_range: 0.05
+    max_range: 120.0
     cloud_min_angle: 0
     cloud_max_angle: 360
     sync_angle: 0

nebula_ros/launch/hesai_launch_all_hw.xml

@@ -7,6 +7,7 @@
     <choice value="PandarAT128" />
     <choice value="PandarQT64" />
     <choice value="PandarQT128" />
+    <choice value="PandarXT16" />
     <choice value="PandarXT32" />
     <choice value="PandarXT32M" />
   </arg>

nebula_ros/launch/nebula_launch.py

@@ -30,6 +30,7 @@
     "PandarAT128",
     "PandarQT64",
     "PandarQT128",
+    "PandarXT16",
     "PandarXT32",
     "PandarXT32M",
 ]