diff --git a/docs/parameters/vendors/hesai/xt16.md b/docs/parameters/vendors/hesai/xt16.md new file mode 100644 index 000000000..cd5e509c5 --- /dev/null +++ b/docs/parameters/vendors/hesai/xt16.md @@ -0,0 +1 @@ +{{ json_to_markdown("nebula_ros/schema/PandarXT16.schema.json") }} diff --git a/docs/supported_sensors.md b/docs/supported_sensors.md index 407713e00..9cb4a310d 100644 --- a/docs/supported_sensors.md +++ b/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 | ✅ | diff --git a/mkdocs.yml b/mkdocs.yml index 4d002789f..c12ef8e40 100644 --- a/mkdocs.yml +++ b/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: diff --git a/nebula_common/include/nebula_common/hesai/hesai_common.hpp b/nebula_common/include/nebula_common/hesai/hesai_common.hpp index 950b2710d..b0b9fa842 100644 --- a/nebula_common/include/nebula_common/hesai/hesai_common.hpp +++ b/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: diff --git a/nebula_common/include/nebula_common/nebula_common.hpp b/nebula_common/include/nebula_common/nebula_common.hpp index 36f7f60c3..1ab25ef76 100644 --- a/nebula_common/include/nebula_common/nebula_common.hpp +++ b/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: diff --git a/nebula_decoders/calibration/hesai/PandarXT16.csv b/nebula_decoders/calibration/hesai/PandarXT16.csv new file mode 100644 index 000000000..adc85d411 --- /dev/null +++ b/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 diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt16.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt16.hpp new file mode 100644 index 000000000..60bfd898a --- /dev/null +++ b/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 + +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, PacketXT16::n_blocks>; + Header12B header; + body_t body; + TailXT16 tail; + uint32_t udp_sequence; +}; + +#pragma pack(pop) + +} // namespace hesai_packet + +class PandarXT16 : public HesaiSensor +{ +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 & return_units) override + { + auto return_type = + HesaiSensor::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 diff --git a/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp b/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp index 1fd543ef7..6884d537f 100644 --- a/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp +++ b/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(sensor_configuration, calibration_data); break; + case SensorModel::HESAI_PANDARXT16: + scan_decoder_ = initialize_decoder(sensor_configuration, calibration_data); + break; case SensorModel::HESAI_PANDARXT32: scan_decoder_ = initialize_decoder(sensor_configuration, calibration_data); break; diff --git a/nebula_examples/launch/hesai_offline.xml b/nebula_examples/launch/hesai_offline.xml index 152c09e86..8c6382e5a 100644 --- a/nebula_examples/launch/hesai_offline.xml +++ b/nebula_examples/launch/hesai_offline.xml @@ -1,6 +1,6 @@ - + diff --git a/nebula_examples/launch/hesai_offline_bag_pcd.xml b/nebula_examples/launch/hesai_offline_bag_pcd.xml index acb604321..cedf9492d 100644 --- a/nebula_examples/launch/hesai_offline_bag_pcd.xml +++ b/nebula_examples/launch/hesai_offline_bag_pcd.xml @@ -1,6 +1,6 @@ - + diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp index e036ac5c3..51e73759c 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp +++ b/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; } diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp index dc5103ab3..6ae2d4524 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp +++ b/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; diff --git a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp index 29302bbc4..6adcfcfc1 100644 --- a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp @@ -349,10 +349,11 @@ std::shared_ptr HesaiHwInterface::GetInventory() switch (sensor_configuration_->sensor_model) { default: + case SensorModel::HESAI_PANDARXT16: case SensorModel::HESAI_PANDARXT32: case SensorModel::HESAI_PANDAR40P: { - auto lidar_config = CheckSizeAndParse(response); - return std::make_shared(lidar_config); + auto lidar_config = CheckSizeAndParse(response); + return std::make_shared(lidar_config); } case SensorModel::HESAI_PANDARQT128: { auto lidar_config = CheckSizeAndParse(response); @@ -379,6 +380,7 @@ std::shared_ptr 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(response); return std::make_shared(lidar_config); @@ -400,6 +402,7 @@ std::shared_ptr HesaiHwInterface::GetLidarStatus() default: case SensorModel::HESAI_PANDAR40P: case SensorModel::HESAI_PANDAR64: + case SensorModel::HESAI_PANDARXT16: case SensorModel::HESAI_PANDARXT32: { auto hesai_lidarstatus = CheckSizeAndParse(response); return std::make_shared(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: diff --git a/nebula_ros/config/lidar/hesai/PandarXT16.param.yaml b/nebula_ros/config/lidar/hesai/PandarXT16.param.yaml new file mode 100644 index 000000000..a09517908 --- /dev/null +++ b/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 diff --git a/nebula_ros/config/lidar/hesai/PandarXT32.param.yaml b/nebula_ros/config/lidar/hesai/PandarXT32.param.yaml index 7288ec371..da07ca978 100644 --- a/nebula_ros/config/lidar/hesai/PandarXT32.param.yaml +++ b/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 diff --git a/nebula_ros/launch/hesai_launch_all_hw.xml b/nebula_ros/launch/hesai_launch_all_hw.xml index 5307d2613..75bb94a81 100644 --- a/nebula_ros/launch/hesai_launch_all_hw.xml +++ b/nebula_ros/launch/hesai_launch_all_hw.xml @@ -7,6 +7,7 @@ + diff --git a/nebula_ros/launch/nebula_launch.py b/nebula_ros/launch/nebula_launch.py index ca0eaaacb..7cb1de407 100644 --- a/nebula_ros/launch/nebula_launch.py +++ b/nebula_ros/launch/nebula_launch.py @@ -30,6 +30,7 @@ "PandarAT128", "PandarQT64", "PandarQT128", + "PandarXT16", "PandarXT32", "PandarXT32M", ] diff --git a/nebula_ros/schema/PandarXT16.schema.json b/nebula_ros/schema/PandarXT16.schema.json new file mode 100644 index 000000000..a743b2fbb --- /dev/null +++ b/nebula_ros/schema/PandarXT16.schema.json @@ -0,0 +1,151 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "LiDAR Hesai PandarXT16 parameters.", + "type": "object", + "definitions": { + "PandarXT16": { + "type": "object", + "properties": { + "host_ip": { + "$ref": "sub/communication.json#/definitions/host_ip" + }, + "sensor_ip": { + "$ref": "sub/communication.json#/definitions/sensor_ip" + }, + "multicast_ip": { + "$ref": "sub/lidar_hesai.json#/definitions/multicast_ip" + }, + "data_port": { + "$ref": "sub/communication.json#/definitions/data_port" + }, + "gnss_port": { + "$ref": "sub/lidar_hesai.json#/definitions/gnss_port" + }, + "packet_mtu_size": { + "$ref": "sub/communication.json#/definitions/packet_mtu_size" + }, + "launch_hw": { + "$ref": "sub/hardware.json#/definitions/launch_hw" + }, + "setup_sensor": { + "$ref": "sub/hardware.json#/definitions/setup_sensor" + }, + "udp_only": { + "$ref": "sub/hardware.json#/definitions/udp_only" + }, + "frame_id": { + "$ref": "sub/topic.json#/definitions/frame_id" + }, + "diag_span": { + "$ref": "sub/topic.json#/definitions/diag_span" + }, + "min_range": { + "$ref": "sub/misc.json#/definitions/min_range" + }, + "max_range": { + "$ref": "sub/misc.json#/definitions/max_range" + }, + "cloud_min_angle": { + "$ref": "sub/misc.json#/definitions/cloud_min_angle" + }, + "cloud_max_angle": { + "$ref": "sub/misc.json#/definitions/cloud_max_angle" + }, + "sync_angle": { + "$ref": "sub/lidar_hesai.json#/definitions/sync_angle" + }, + "cut_angle": { + "$ref": "sub/lidar_hesai.json#/definitions/cut_angle" + }, + "sensor_model": { + "$ref": "sub/lidar_hesai.json#/definitions/sensor_model", + "enum": [ + "PandarXT16" + ] + }, + "calibration_file": { + "$ref": "sub/lidar_hesai.json#/definitions/calibration_file" + }, + "rotation_speed": { + "$ref": "sub/lidar_hesai.json#/definitions/rotation_speed" + }, + "return_mode": { + "$ref": "sub/misc.json#/definitions/return_mode", + "enum": [ + "Last", + "Strongest", + "LastStrongest", + "First", + "LastFirst", + "FirstStrongest", + "Dual" + ] + }, + "ptp_profile": { + "$ref": "sub/lidar_hesai.json#/definitions/ptp_profile" + }, + "ptp_domain": { + "$ref": "sub/lidar_hesai.json#/definitions/ptp_domain" + }, + "ptp_transport_type": { + "$ref": "sub/lidar_hesai.json#/definitions/ptp_transport_type" + }, + "ptp_switch_type": { + "$ref": "sub/lidar_hesai.json#/definitions/ptp_switch_type" + }, + "retry_hw": { + "$ref": "sub/hardware.json#/definitions/retry_hw" + }, + "dual_return_distance_threshold": { + "$ref": "sub/misc.json#/definitions/dual_return_distance_threshold" + } + }, + "required": [ + "host_ip", + "sensor_ip", + "multicast_ip", + "data_port", + "gnss_port", + "packet_mtu_size", + "launch_hw", + "setup_sensor", + "udp_only", + "frame_id", + "diag_span", + "cloud_min_angle", + "cloud_max_angle", + "sync_angle", + "cut_angle", + "sensor_model", + "calibration_file", + "rotation_speed", + "return_mode", + "ptp_profile", + "ptp_domain", + "ptp_transport_type", + "ptp_switch_type", + "retry_hw", + "dual_return_distance_threshold" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/PandarXT16" + }, + "additionalProperties": false + }, + "required": [ + "ros__parameters" + ] + } + }, + "required": [ + "/**" + ], + "additionalProperties": false +} diff --git a/nebula_ros/schema/sub/lidar_hesai.json b/nebula_ros/schema/sub/lidar_hesai.json index b86a6d1ad..b58397a33 100644 --- a/nebula_ros/schema/sub/lidar_hesai.json +++ b/nebula_ros/schema/sub/lidar_hesai.json @@ -8,6 +8,7 @@ "enum": [ "Pandar64", "Pandar40P", + "PandarXT16", "PandarXT32", "PandarXT32M", "PandarQT64", diff --git a/nebula_tests/data/hesai/xt16/1732848127890747768/1732848127890747768_0.db3 b/nebula_tests/data/hesai/xt16/1732848127890747768/1732848127890747768_0.db3 new file mode 100644 index 000000000..646848dd9 Binary files /dev/null and b/nebula_tests/data/hesai/xt16/1732848127890747768/1732848127890747768_0.db3 differ diff --git a/nebula_tests/data/hesai/xt16/1732848127890747768/metadata.yaml b/nebula_tests/data/hesai/xt16/1732848127890747768/metadata.yaml new file mode 100644 index 000000000..1fa75fe82 --- /dev/null +++ b/nebula_tests/data/hesai/xt16/1732848127890747768/metadata.yaml @@ -0,0 +1,26 @@ +rosbag2_bagfile_information: + version: 5 + storage_identifier: sqlite3 + duration: + nanoseconds: 302526840 + starting_time: + nanoseconds_since_epoch: 1732848127890747768 + message_count: 4 + topics_with_message_count: + - topic_metadata: + name: /pandar_packets + type: pandar_msgs/msg/PandarScan + serialization_format: cdr + offered_qos_profiles: "- history: 1\n depth: 5\n reliability: 2\n durability: 2\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 9223372036\n nsec: 854775807\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false" + message_count: 4 + compression_format: "" + compression_mode: "" + relative_file_paths: + - 1732848127890747768_0.db3 + files: + - path: 1732848127890747768_0.db3 + starting_time: + nanoseconds_since_epoch: 1732848127890747768 + duration: + nanoseconds: 302526840 + message_count: 4 diff --git a/nebula_tests/data/hesai/xt16/1732848128093928016.pcd b/nebula_tests/data/hesai/xt16/1732848128093928016.pcd new file mode 100644 index 000000000..fe857cf69 Binary files /dev/null and b/nebula_tests/data/hesai/xt16/1732848128093928016.pcd differ diff --git a/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp b/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp index 1b98f91fe..e6402e098 100644 --- a/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp +++ b/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp @@ -24,7 +24,7 @@ namespace nebula::test { -const nebula::ros::HesaiRosDecoderTestParams TEST_CONFIGS[8] = { +const nebula::ros::HesaiRosDecoderTestParams TEST_CONFIGS[9] = { {"Pandar40P", "Dual", "Pandar40P.csv", "40p/1673400149412331409", "hesai", 0, 0.0, 0., 360., 0.3f, 200.f}, {"Pandar64", "Dual", "Pandar64.csv", "64/1673403880599376836", "hesai", 0, 0.0, 0., 360., 0.3f, @@ -33,6 +33,8 @@ const nebula::ros::HesaiRosDecoderTestParams TEST_CONFIGS[8] = { 150.0, 30., 150., 1.f, 180.f}, {"PandarQT64", "Dual", "PandarQT64.csv", "qt64/1673401195788312575", "hesai", 0, 0.0, 0., 360., 0.1f, 60.f}, + {"PandarXT16", "Dual", "PandarXT16.csv", "xt16/1732848127890747768", "hesai", 0, 0.0, 0., 360., + 0.05f, 120.f}, {"PandarXT32", "Dual", "PandarXT32.csv", "xt32/1673400677802009732", "hesai", 0, 0.0, 0., 360., 0.05f, 120.f}, {"PandarXT32M", "LastStrongest", "PandarXT32M.csv", "xt32m/1660893203042895158", "hesai", 0, 0.0,