diff --git a/src/nebula/launch/nebula_launch.py b/src/nebula/launch/nebula_launch.py index 72a4c6f82..8f88621cc 100644 --- a/src/nebula/launch/nebula_launch.py +++ b/src/nebula/launch/nebula_launch.py @@ -33,6 +33,7 @@ "PandarXT16", "PandarXT32", "PandarXT32M", + "PandarFT120", ] SENSOR_MODELS_ROBOSENSE = ["Bpearl", "Helios"] SENSOR_MODELS_CONTINENTAL = ["ARS548", "SRR520"] diff --git a/src/nebula_core/nebula_core_common/include/nebula_core_common/nebula_common.hpp b/src/nebula_core/nebula_core_common/include/nebula_core_common/nebula_common.hpp index 0bcfdd490..5b918d6ad 100644 --- a/src/nebula_core/nebula_core_common/include/nebula_core_common/nebula_common.hpp +++ b/src/nebula_core/nebula_core_common/include/nebula_core_common/nebula_common.hpp @@ -193,6 +193,7 @@ enum class SensorModel : uint8_t { HESAI_PANDARAT128, HESAI_PANDAR128_E3X, HESAI_PANDAR128_E4X, + HESAI_PANDARFT120, VELODYNE_VLS128, VELODYNE_HDL64, VELODYNE_VLP32, @@ -257,6 +258,9 @@ inline std::ostream & operator<<(std::ostream & os, nebula::drivers::SensorModel case SensorModel::HESAI_PANDAR128_E4X: os << "Pandar128_E4X_OT"; break; + case SensorModel::HESAI_PANDARFT120: + os << "PandarFT120"; + break; case SensorModel::VELODYNE_VLS128: os << "VLS128"; break; @@ -409,6 +413,7 @@ inline SensorModel sensor_model_from_string(const std::string & sensor_model) if (sensor_model == "PandarQT64") return SensorModel::HESAI_PANDARQT64; if (sensor_model == "PandarQT128") return SensorModel::HESAI_PANDARQT128; if (sensor_model == "Pandar128E4X") return SensorModel::HESAI_PANDAR128_E4X; + if (sensor_model == "PandarFT120") return SensorModel::HESAI_PANDARFT120; // Velodyne if (sensor_model == "VLS128") return SensorModel::VELODYNE_VLS128; if (sensor_model == "HDL64") return SensorModel::VELODYNE_HDL64; @@ -451,6 +456,8 @@ inline std::string sensor_model_to_string(const SensorModel & sensor_model) return "PandarQT128"; case SensorModel::HESAI_PANDAR128_E4X: return "Pandar128E4X"; + case SensorModel::HESAI_PANDARFT120: + return "PandarFT120"; // Velodyne case SensorModel::VELODYNE_VLS128: return "VLS128"; diff --git a/src/nebula_hesai/nebula_hesai/config/PandarFT120.param.yaml b/src/nebula_hesai/nebula_hesai/config/PandarFT120.param.yaml new file mode 100644 index 000000000..657b1838b --- /dev/null +++ b/src/nebula_hesai/nebula_hesai/config/PandarFT120.param.yaml @@ -0,0 +1,40 @@ +/**: + ros__parameters: + host_ip: 192.168.1.10 + sensor_ip: 192.168.1.201 + multicast_ip: "" + data_port: 2368 + gnss_port: 10110 + udp_socket_receive_buffer_size_bytes: 5400000 + packet_mtu_size: 1500 + launch_hw: true + setup_sensor: true + udp_only: false + frame_id: hesai + diag_span: 1000 + min_range: 0.3 + max_range: 22.0 + cloud_min_angle: 40 + cloud_max_angle: 139 + sync_angle: 40 # 40-139 + cut_angle: 139.0 + sensor_model: PandarFT120 + calibration_file: $(find-pkg-share nebula_hesai_decoders)/calibration/$(var sensor_model).dat + calibration_download_enabled: true + rotation_speed: 600 + return_mode: Strongest + ptp_profile: automotive + ptp_domain: 0 + ptp_transport_type: L2 + ptp_switch_type: TSN + ptp_lock_threshold: 1 # 1-100 + retry_hw: true + dual_return_distance_threshold: 0.1 + diagnostics: + pointcloud_publish_rate: + frequency_ok: + min_hz: 9.5 + max_hz: 10.5 + frequency_warn: + min_hz: 9.0 + max_hz: 11.0 diff --git a/src/nebula_hesai/nebula_hesai/launch/hesai_launch_all_hw.xml b/src/nebula_hesai/nebula_hesai/launch/hesai_launch_all_hw.xml index b0bca0a97..11ede424c 100644 --- a/src/nebula_hesai/nebula_hesai/launch/hesai_launch_all_hw.xml +++ b/src/nebula_hesai/nebula_hesai/launch/hesai_launch_all_hw.xml @@ -10,6 +10,7 @@ + diff --git a/src/nebula_hesai/nebula_hesai/schema/PandarFT120.schema.json b/src/nebula_hesai/nebula_hesai/schema/PandarFT120.schema.json new file mode 100644 index 000000000..8e93805db --- /dev/null +++ b/src/nebula_hesai/nebula_hesai/schema/PandarFT120.schema.json @@ -0,0 +1,157 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "LiDAR Hesai FT120 parameters.", + "type": "object", + "definitions": { + "PandarFT120": { + "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" + }, + "udp_socket_receive_buffer_size_bytes": { + "$ref": "sub/lidar_hesai.json#/definitions/udp_socket_receive_buffer_size_bytes" + }, + "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" + }, + "sync_angle": { + "$ref": "sub/lidar_hesai.json#/definitions/sync_angle", + "minimum": 40, + "maximum": 139, + "default": 40 + }, + "cut_angle": { + "$ref": "sub/lidar_hesai.json#/definitions/cut_angle", + "minimum": 40.0, + "maximum": 139.0, + "default": 40.0 + }, + "sensor_model": { + "$ref": "sub/lidar_hesai.json#/definitions/sensor_model", + "enum": [ + "PandarFT120" + ] + }, + "calibration_file": { + "$ref": "sub/lidar_hesai.json#/definitions/calibration_file" + }, + "return_mode": { + "$ref": "sub/misc.json#/definitions/return_mode", + "enum": [ + "Strongest", + "First", + "FirstStrongest" + ] + }, + "ptp_profile": { + "$ref": "sub/lidar_hesai.json#/definitions/ptp_profile", + "default": "automotive" + }, + "ptp_domain": { + "$ref": "sub/lidar_hesai.json#/definitions/ptp_domain" + }, + "ptp_transport_type": { + "$ref": "sub/lidar_hesai.json#/definitions/ptp_transport_type", + "default": "L2" + }, + "ptp_switch_type": { + "$ref": "sub/lidar_hesai.json#/definitions/ptp_switch_type" + }, + "ptp_lock_threshold": { + "$ref": "sub/lidar_hesai.json#/definitions/ptp_lock_threshold" + }, + "retry_hw": { + "$ref": "sub/hardware.json#/definitions/retry_hw" + }, + "dual_return_distance_threshold": { + "$ref": "sub/misc.json#/definitions/dual_return_distance_threshold" + }, + "point_filters": { + "$ref": "sub/misc.json#/definitions/point_filters" + }, + "diagnostics": { + "$ref": "sub/lidar_hesai.json#/definitions/diagnostics", + "required": [ + "pointcloud_publish_rate", + "packet_loss" + ] + }, + "sync_diagnostics": { + "$ref": "sub/misc.json#/definitions/sync_diagnostics" + } + }, + "required": [ + "host_ip", + "sensor_ip", + "multicast_ip", + "data_port", + "udp_socket_receive_buffer_size_bytes", + "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", + "return_mode", + "ptp_profile", + "ptp_domain", + "ptp_transport_type", + "ptp_switch_type", + "ptp_lock_threshold", + "retry_hw", + "dual_return_distance_threshold", + "diagnostics" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/PandarFT120" + }, + "additionalProperties": false + }, + "required": [ + "ros__parameters" + ] + } + }, + "required": [ + "/**" + ], + "additionalProperties": false +} diff --git a/src/nebula_hesai/nebula_hesai/schema/lidar_hesai.json b/src/nebula_hesai/nebula_hesai/schema/lidar_hesai.json index b020f4a64..3735597c1 100644 --- a/src/nebula_hesai/nebula_hesai/schema/lidar_hesai.json +++ b/src/nebula_hesai/nebula_hesai/schema/lidar_hesai.json @@ -14,7 +14,8 @@ "PandarQT64", "PandarQT128", "Pandar128E4X", - "PandarAT128" + "PandarAT128", + "PandarFT120" ] }, "calibration_file": { diff --git a/src/nebula_hesai/nebula_hesai/src/hesai_ros_wrapper.cpp b/src/nebula_hesai/nebula_hesai/src/hesai_ros_wrapper.cpp index 5f92f2407..faf04b584 100644 --- a/src/nebula_hesai/nebula_hesai/src/hesai_ros_wrapper.cpp +++ b/src/nebula_hesai/nebula_hesai/src/hesai_ros_wrapper.cpp @@ -87,7 +87,8 @@ HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) bool lidar_range_supported = sensor_cfg_ptr_->sensor_model != drivers::SensorModel::HESAI_PANDARAT128 && - sensor_cfg_ptr_->sensor_model != drivers::SensorModel::HESAI_PANDAR64; + sensor_cfg_ptr_->sensor_model != drivers::SensorModel::HESAI_PANDAR64 && + sensor_cfg_ptr_->sensor_model != drivers::SensorModel::HESAI_PANDARFT120; if (hw_interface_wrapper_ && !use_udp_only && lidar_range_supported) { auto status = @@ -633,6 +634,8 @@ HesaiRosWrapper::get_calibration_result_t HesaiRosWrapper::get_calibration_data( if (sensor_cfg_ptr_->sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { calib = std::make_shared(); + } else if (sensor_cfg_ptr_->sensor_model == drivers::SensorModel::HESAI_PANDARFT120) { + calib = std::make_shared(); } else { calib = std::make_shared(); } diff --git a/src/nebula_hesai/nebula_hesai_common/include/nebula_hesai_common/hesai_common.hpp b/src/nebula_hesai/nebula_hesai_common/include/nebula_hesai_common/hesai_common.hpp index 8945572a2..ec900297d 100644 --- a/src/nebula_hesai/nebula_hesai_common/include/nebula_hesai_common/hesai_common.hpp +++ b/src/nebula_hesai/nebula_hesai_common/include/nebula_hesai_common/hesai_common.hpp @@ -591,6 +591,86 @@ struct HesaiCorrection : public HesaiCalibrationConfigurationBase } }; +/// @brief struct for Hesai correction configuration for solid state sensors (for FT120) +struct HesaiSolidStateCalibration : public HesaiCalibrationConfigurationBase +{ +public: + std::vector azimuth_adjust; + std::vector elevation_adjust; + + uint32_t col_count; + uint32_t row_count; + uint32_t resolution; + + nebula::Status load_from_bytes(const std::vector & buf) override + { + // get the matrix info from buffer + col_count = buf.at(6); + row_count = buf.at(7); + resolution = buf.at(8); + + const auto count{col_count * row_count}; + const auto count_bytes{4 * count}; + + // size check for the upcoming memcpy operations (0-8 + 2 arrays) + if (buf.size() < (9 + 2 * count_bytes)) { + return Status::INVALID_CALIBRATION_FILE; + } + + auto ref = &(buf[9]); + + azimuth_adjust.resize(count); + std::memcpy(azimuth_adjust.data(), ref, count_bytes); + + elevation_adjust.resize(count); + std::memcpy(elevation_adjust.data(), ref + count_bytes, count_bytes); + + return Status::OK; + } + + inline nebula::Status load_from_file(const std::string & calibration_file) override + { + std::ifstream stream(calibration_file, std::ios::in | std::ios::binary); + std::vector contents( + (std::istreambuf_iterator(stream)), std::istreambuf_iterator()); + + load_from_bytes(contents); + + return Status::OK; + } + + // from HesaiCorrection + nebula::Status save_to_file_from_bytes( + const std::string & calibration_file, const std::vector & buf) override + { + std::ofstream ofs(calibration_file, std::ios::trunc | std::ios::binary); + if (!ofs) { + std::cerr << "Could not create file: " << calibration_file << "\n"; + return Status::CANNOT_SAVE_FILE; + } + bool sop_received = false; + for (const auto & byte : buf) { + if (!sop_received) { + if (byte == 0xEE) { + sop_received = true; + } + } + if (sop_received) { + ofs << byte; + } + } + ofs.close(); + if (sop_received) return Status::OK; + return Status::INVALID_CALIBRATION_FILE; + } + + [[nodiscard]] std::tuple get_fov_padding() const override + { + // For FT120 should be enough + return {-0.1, 0.1}; + } +}; + /* @@ -623,6 +703,7 @@ inline ReturnMode return_mode_from_string_hesai( case SensorModel::HESAI_PANDAR128_E3X: case SensorModel::HESAI_PANDAR128_E4X: case SensorModel::HESAI_PANDARQT128: + case SensorModel::HESAI_PANDARFT120: if (return_mode == "Last") return ReturnMode::LAST; if (return_mode == "Strongest") return ReturnMode::STRONGEST; if (return_mode == "Dual" || return_mode == "LastStrongest") @@ -665,6 +746,7 @@ inline ReturnMode return_mode_from_int_hesai( case SensorModel::HESAI_PANDAR128_E3X: case SensorModel::HESAI_PANDAR128_E4X: case SensorModel::HESAI_PANDARQT128: + case SensorModel::HESAI_PANDARFT120: if (return_mode == 0) return ReturnMode::LAST; if (return_mode == 1) return ReturnMode::STRONGEST; if (return_mode == 2) return ReturnMode::DUAL_LAST_STRONGEST; @@ -705,6 +787,7 @@ inline int int_from_return_mode_hesai( case SensorModel::HESAI_PANDAR128_E3X: case SensorModel::HESAI_PANDAR128_E4X: case SensorModel::HESAI_PANDARQT128: + case SensorModel::HESAI_PANDARFT120: if (return_mode == ReturnMode::LAST) return 0; if (return_mode == ReturnMode::STRONGEST) return 1; if (return_mode == ReturnMode::DUAL || return_mode == ReturnMode::DUAL_LAST_STRONGEST) diff --git a/src/nebula_hesai/nebula_hesai_decoders/calibration/PandarFT120.dat b/src/nebula_hesai/nebula_hesai_decoders/calibration/PandarFT120.dat new file mode 100644 index 000000000..fb3221402 Binary files /dev/null and b/src/nebula_hesai/nebula_hesai_decoders/calibration/PandarFT120.dat differ diff --git a/src/nebula_hesai/nebula_hesai_decoders/include/nebula_hesai_decoders/decoders/angle_corrector_calibration_based_solid_state.hpp b/src/nebula_hesai/nebula_hesai_decoders/include/nebula_hesai_decoders/decoders/angle_corrector_calibration_based_solid_state.hpp new file mode 100644 index 000000000..9813f90ef --- /dev/null +++ b/src/nebula_hesai/nebula_hesai_decoders/include/nebula_hesai_decoders/decoders/angle_corrector_calibration_based_solid_state.hpp @@ -0,0 +1,123 @@ +// 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_core_decoders/angles.hpp" +#include "nebula_hesai_common/hesai_common.hpp" +#include "nebula_hesai_decoders/decoders/angle_corrector.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace nebula::drivers +{ + +template +class AngleCorrectorCalibrationBasedSolidState +: public AngleCorrector +{ +private: + std::array, ColumnN> corrected_angle_data; + +public: + explicit AngleCorrectorCalibrationBasedSolidState( + const std::shared_ptr & sensor_calibration, + double fov_start_azimuth_deg, double fov_end_azimuth_deg, double scan_cut_azimuth_deg) + { + // not used parameters + (void)fov_start_azimuth_deg; + (void)fov_end_azimuth_deg; + (void)scan_cut_azimuth_deg; + + if (sensor_calibration == nullptr) { + throw std::runtime_error( + "Cannot instantiate AngleCorrectorCalibrationBasedSolidState without calibration data"); + } + + // //////////////////////////////////////// + // Build lookup table + // //////////////////////////////////////// + + size_t calib_i = 0; + + const double res_coeff = + 0.01 * sensor_calibration->resolution * M_PI / 180.; // also, convert to rad + + for (size_t j = 0; j < ColumnN; j++) // column + { + for (size_t i = 0; i < RowN; i++) // row + { + // calibration vectors contain column-major ordered elevation and azimuth angles in degrees, + // in "cartesian order": + // - first value is lower left sensor point; + // - first column last point is top left sensor point; + // - last column first point is lower right sensor point; + // - last column last point is top right sensor point; + // Note: azimuth has a value of 0° when the point lies in the plane X=0 (so + // to get correct point coordinates, sine and cosine have to be used correctly, + // as described in user manual. See hesai_decoder.hpp for the implementation) + const double azi = sensor_calibration->azimuth_adjust.at(calib_i) * res_coeff; + const double ele = sensor_calibration->elevation_adjust.at(calib_i) * res_coeff; + + ++calib_i; + + auto pixel_angle_data = CorrectedAngleData(); + + pixel_angle_data.azimuth_rad = static_cast(azi); + pixel_angle_data.elevation_rad = static_cast(ele); + pixel_angle_data.sin_azimuth = static_cast(sin(azi)); + pixel_angle_data.cos_azimuth = static_cast(cos(azi)); + pixel_angle_data.sin_elevation = static_cast(sin(ele)); + pixel_angle_data.cos_elevation = static_cast(cos(ele)); + + corrected_angle_data[j][i] = pixel_angle_data; + } + } + } + + [[nodiscard]] CorrectedAngleData get_corrected_angle_data(uint32_t row_id, uint32_t col_id) const + { + return corrected_angle_data[col_id][row_id]; + } + + // this base method is not used for solid state sensor, as all angles came from + // get_corrected_angle_data + [[nodiscard]] CorrectedAzimuths get_corrected_azimuths( + uint32_t block_azimuth) const + { + // not used parameters + (void)block_azimuth; + return CorrectedAzimuths(); + }; + + static bool passed_emit_angle(uint32_t last_azimuth, uint32_t current_azimuth) + { + return last_azimuth > current_azimuth; + } + + static bool passed_timestamp_reset_angle(uint32_t last_azimuth, uint32_t current_azimuth) + { + return last_azimuth > current_azimuth; + } +}; + +} // namespace nebula::drivers diff --git a/src/nebula_hesai/nebula_hesai_decoders/include/nebula_hesai_decoders/decoders/hesai_decoder.hpp b/src/nebula_hesai/nebula_hesai_decoders/include/nebula_hesai_decoders/decoders/hesai_decoder.hpp index 78a74ee0a..638bdea8e 100644 --- a/src/nebula_hesai/nebula_hesai_decoders/include/nebula_hesai_decoders/decoders/hesai_decoder.hpp +++ b/src/nebula_hesai/nebula_hesai_decoders/include/nebula_hesai_decoders/decoders/hesai_decoder.hpp @@ -396,4 +396,397 @@ class HesaiDecoder : public HesaiScanDecoder } }; +template +class HesaiSolidStateDecoder : public HesaiScanDecoder // for Solid State sensor +{ +private: + struct ScanCutAngles + { + float fov_min; + float fov_max; + float scan_emit_angle; + }; + + struct DecodeFrame + { + NebulaPointCloudPtr pointcloud; + uint64_t scan_timestamp_ns{0}; + std::optional blockage_mask; + }; + + /// @brief Configuration for this decoder + const std::shared_ptr sensor_configuration_; + + /// @brief The sensor definition, used for return mode and time offset handling + SensorT sensor_{}; + + /// @brief A function that is called on each decoded pointcloud frame + pointcloud_callback_t pointcloud_callback_; + + /// @brief Decodes azimuth/elevation angles given calibration/correction data + typename SensorT::angle_corrector_t angle_corrector_; + + /// @brief Decodes functional safety data for supported sensors + std::shared_ptr> + functional_safety_decoder_; + + std::shared_ptr> packet_loss_detector_; + + /// @brief The last decoded packet + typename SensorT::packet_t packet_; + /// @brief The previous decoded packet (for dual return) + typename SensorT::packet_t previous_packet_; + + ScanCutAngles scan_cut_angles_; + uint32_t last_azimuth_id_ = 0; + + std::shared_ptr logger_; + + std::optional mask_filter_; + + std::shared_ptr blockage_mask_plugin_; + + /// @brief Decoded data of the frame currently being decoded to + DecodeFrame decode_frame_; + /// @brief Decoded data of the frame currently being output + DecodeFrame output_frame_; + + /// @brief Validates and parse PandarPacket. Checks size and, if present, CRC checksums. + /// @param packet The incoming PandarPacket + /// @return Whether the packet was parsed successfully + bool parse_packet(const std::vector & packet) + { + if (packet.size() < sizeof(typename SensorT::packet_t)) { + NEBULA_LOG_STREAM( + logger_->error, "Packet size mismatch: " << packet.size() << " | Expected at least: " + << sizeof(typename SensorT::packet_t)); + return false; + } + + if (!std::memcpy(&packet_, packet.data(), sizeof(typename SensorT::packet_t))) { + logger_->error("Packet memcopy failed"); + return false; + } + + return true; + } + + /// @brief Converts a group of returns (i.e. 1 for single return, 2 for dual return, etc.) to + /// points and appends them to the point cloud + /// @param start_block_id The first block in the group of returns + /// @param n_blocks The number of returns in the group (has to align with the `n_returns` field in + /// the packet footer) + void convert_returns(size_t start_block_id, size_t n_returns) + { + (void)start_block_id; + + uint64_t packet_timestamp_ns = hesai_packet::get_timestamp_ns(packet_); + uint32_t column_id = packet_.tail.column_id; + + std::vector return_units; + + // If the blockage mask plugin is not present, we can return early if distance checks fail + const bool filters_can_return_early = !blockage_mask_plugin_; + + const unsigned int return_idx = packet_.header.first_block_return; + + const auto return_type = sensor_.get_return_type( + static_cast(packet_.tail.return_mode), return_idx, + return_units); + + // dual return: store current packet and wait for the 2nd + if (n_returns == 2 && return_idx == 1) { + std::swap(packet_, previous_packet_); + return; + } + + for (size_t row_id = 0; row_id < SensorT::packet_t::n_channels; ++row_id) { + // For FT120, a "channel" is exactly one full column of readings from the sensor; + // only 1 return group is sent in a packet + + // Find the units corresponding to the same return group as the current one. + // These are used to find duplicates in multi-return mode. + return_units.clear(); + + return_units.push_back(&packet_.body.blocks[0].units[row_id]); + + // eventually, get the first return from the previous packet + if (return_idx == 2) { + return_units.push_back(&previous_packet_.body.blocks[0].units[row_id]); + } + + const CorrectedAngleData corrected_angle_data = + angle_corrector_.get_corrected_angle_data(row_id, column_id); + + for (size_t block_offset = 0; block_offset < n_returns; ++block_offset) { + auto & unit = *return_units[block_offset]; + + bool point_is_valid = true; + + if (unit.distance == 0) { + point_is_valid = false; + } + + float distance = get_distance(unit); + + if ( + distance < SensorT::min_range || SensorT::max_range < distance || + distance < sensor_configuration_->min_range || + sensor_configuration_->max_range < distance) { + point_is_valid = false; + } + + // the second return is transmitted using the following block, so in order to remove + // duplicated points, we should compare distance between points in this packet and in the + // previus one Keep only last (if any) of multiple points that are too close + if (block_offset != n_returns - 1) { + bool is_below_multi_return_threshold = false; + + for (size_t return_idx = 0; return_idx < n_returns; ++return_idx) { + if (return_idx == block_offset) { + continue; + } + + if ( + fabsf(get_distance(*return_units[return_idx]) - distance) < + sensor_configuration_->dual_return_distance_threshold) { + is_below_multi_return_threshold = true; + break; + } + } + + if (is_below_multi_return_threshold) { + point_is_valid = false; + } + } + + if (filters_can_return_early && !point_is_valid) { + continue; + } + + float azimuth = corrected_angle_data.azimuth_rad; + + const bool in_fov = + angle_is_between(scan_cut_angles_.fov_min, scan_cut_angles_.fov_max, azimuth); + if (!in_fov) { + continue; + } + + bool in_current_scan = + 0 != + column_id; // write in output_frame as frames are being swapped after this method exit + + auto & frame = in_current_scan ? decode_frame_ : output_frame_; + + if (frame.blockage_mask) { + frame.blockage_mask->update(azimuth, row_id, sensor_.get_blockage_type(unit.distance)); + } + + if (!point_is_valid) { + continue; + } + + NebulaPoint point; + point.distance = distance; + point.intensity = unit.reflectivity; + point.time_stamp = packet_timestamp_ns - frame.scan_timestamp_ns; + + point.return_type = static_cast(return_type); + point.channel = row_id; + + // Use sin/cos functions from calibration data from corrected_angle_data + const float xy_distance = distance * corrected_angle_data.cos_elevation; + point.x = xy_distance * corrected_angle_data.sin_azimuth; + point.y = xy_distance * corrected_angle_data.cos_azimuth; + point.z = distance * corrected_angle_data.sin_elevation; + + // The driver wrapper converts to degrees, expects radians + point.azimuth = corrected_angle_data.azimuth_rad; + point.elevation = corrected_angle_data.elevation_rad; + + if (!mask_filter_ || !mask_filter_->excluded(point)) { + frame.pointcloud->emplace_back(point); + } + } + } + } + + /// @brief Get the distance of the given unit in meters + float get_distance(const typename SensorT::packet_t::body_t::block_t::unit_t & unit) + { + return unit.distance * hesai_packet::get_dis_unit(packet_); + } + + /// @brief Get timestamp of point in nanoseconds, relative to scan timestamp. Includes firing time + /// offset correction for channel and block + /// @param scan_timestamp_ns Start timestamp of the current scan in nanoseconds + /// @param packet_timestamp_ns The timestamp of the current PandarPacket in nanoseconds + /// @param block_id The block index of the point + /// @param channel_id The channel index of the point + uint32_t get_point_time_relative( + uint64_t scan_timestamp_ns, uint64_t packet_timestamp_ns, size_t block_id, size_t channel_id) + { + (void)block_id; + (void)channel_id; + + // this is a flash solid state LIDAR, point_to_packet_offset_ns is 0 as measurements comes from + // the same light emission and there is non need to correct packet_to_scan_offset_ns + auto packet_to_scan_offset_ns = static_cast(packet_timestamp_ns - scan_timestamp_ns); + return packet_to_scan_offset_ns; + } + + DecodeFrame initialize_frame() const + { + DecodeFrame frame = {std::make_shared(), 0, std::nullopt}; + frame.pointcloud->reserve(SensorT::max_scan_buffer_points); + + if (blockage_mask_plugin_) { + frame.blockage_mask = point_filters::BlockageMask( + SensorT::fov_mdeg.azimuth, blockage_mask_plugin_->get_bin_width_mdeg(), + SensorT::packet_t::n_channels); + } + + return frame; + } + + /// @brief Called when a scan is complete, published and then clears the output frame. + void on_scan_complete() + { + double scan_timestamp_s = static_cast(output_frame_.scan_timestamp_ns) * 1e-9; + + if (pointcloud_callback_) { + pointcloud_callback_(output_frame_.pointcloud, scan_timestamp_s); + } + + if (blockage_mask_plugin_ && output_frame_.blockage_mask) { + blockage_mask_plugin_->callback_and_reset( + output_frame_.blockage_mask.value(), scan_timestamp_s); + } + + output_frame_.pointcloud->clear(); + } + +public: + /// @brief Constructor + /// @param sensor_configuration SensorConfiguration for this decoder + /// @param correction_data Calibration data for this decoder + explicit HesaiSolidStateDecoder( + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & + correction_data, + const std::shared_ptr & logger, + const std::shared_ptr> & + functional_safety_decoder, + const std::shared_ptr> & + packet_loss_detector, + std::shared_ptr blockage_mask_plugin) + : sensor_configuration_(sensor_configuration), + angle_corrector_( + correction_data, sensor_configuration_->cloud_min_angle, + sensor_configuration_->cloud_max_angle, sensor_configuration_->cut_angle), + functional_safety_decoder_(functional_safety_decoder), + packet_loss_detector_(packet_loss_detector), + scan_cut_angles_( + {static_cast(deg2rad(sensor_configuration_->cloud_min_angle)), + static_cast(deg2rad(sensor_configuration_->cloud_max_angle)), + static_cast(deg2rad(sensor_configuration_->cut_angle))}), + logger_(logger), + blockage_mask_plugin_(std::move(blockage_mask_plugin)), + decode_frame_(initialize_frame()), + output_frame_(initialize_frame()) + { + if (sensor_configuration->downsample_mask_path) { + mask_filter_ = point_filters::DownsampleMaskFilter( + sensor_configuration->downsample_mask_path.value(), SensorT::fov_mdeg.azimuth, + SensorT::peak_resolution_mdeg.azimuth, SensorT::packet_t::n_channels, + logger_->child("Downsample Mask"), true, sensor_.get_dither_transform()); + } + } + + void set_pointcloud_callback(pointcloud_callback_t callback) override + { + pointcloud_callback_ = std::move(callback); + } + + PacketDecodeResult unpack(const std::vector & packet) override + { + util::Stopwatch decode_watch; + + if (!parse_packet(packet)) { + return {PerformanceCounters{decode_watch.elapsed_ns()}, DecodeError::PACKET_PARSE_FAILED}; + } + if (packet_loss_detector_) { + packet_loss_detector_->update(packet_); + } + + // Even if the checksums of other parts of the packet are invalid, functional safety info + // is still checked. This is a null-op for sensors that do not support functional safety. + if (functional_safety_decoder_) { + functional_safety_decoder_->update(packet_); + } + + // FYI: This is where the CRC would be checked. Since this caused performance issues in the + // past, and since the frame check sequence of the packet is already checked by the NIC, we skip + // it here. + + // This is the first scan, set scan timestamp to whatever packet arrived first + // It is valid for a flash LIDAR sensor as the FT120 + if (decode_frame_.scan_timestamp_ns == 0) { + decode_frame_.scan_timestamp_ns = hesai_packet::get_timestamp_ns(packet_); + } + + bool did_scan_complete = false; + + const size_t n_returns = hesai_packet::get_n_returns(packet_.tail.return_mode); + const size_t block_id = 0; + const auto block_column_id = packet_.tail.column_id; + + // We have a new scan when new azimut (block_column_id) go back to first column + if (angle_corrector_.passed_timestamp_reset_angle(last_azimuth_id_, block_column_id)) { + uint64_t new_scan_timestamp_ns = hesai_packet::get_timestamp_ns(packet_); + + // Check FT120: it should always go into the "else" branch + if (sensor_configuration_->cut_angle == sensor_configuration_->cloud_max_angle) { + // In the non-360 deg case, if the cut angle and FoV end coincide, the old pointcloud has + // already been swapped and published before the timestamp reset angle is reached. Thus, + // the `decode` pointcloud is now empty and will be decoded to. Reset its timestamp. + decode_frame_.scan_timestamp_ns = new_scan_timestamp_ns; + decode_frame_.pointcloud->clear(); + } else { + // When not cutting at the end of the FoV (i.e. the FoV is 360 deg or a cut occurs + // somewhere within a non-360 deg FoV), the current scan is still being decoded to the + // `decode` pointcloud but at the same time, points for the next pointcloud are arriving + // and will be decoded to the `output` pointcloud (please forgive the naming for now). + // Thus, reset the output pointcloud's timestamp. + output_frame_.scan_timestamp_ns = new_scan_timestamp_ns; + } + } + + convert_returns(block_id, n_returns); + + if (angle_corrector_.passed_emit_angle(last_azimuth_id_, block_column_id)) { + // The current `decode` pointcloud is ready for publishing, swap buffers to continue with + // the `output` pointcloud as the `decode` pointcloud. + std::swap(decode_frame_, output_frame_); + did_scan_complete = true; + } + + last_azimuth_id_ = block_column_id; + + uint64_t decode_duration_ns = decode_watch.elapsed_ns(); + uint64_t callbacks_duration_ns = 0; + + if (did_scan_complete) { + util::Stopwatch callback_watch; + on_scan_complete(); + callbacks_duration_ns += callback_watch.elapsed_ns(); + } + + PacketMetadata metadata; + metadata.packet_timestamp_ns = hesai_packet::get_timestamp_ns(packet_); + metadata.did_scan_complete = did_scan_complete; + return {PerformanceCounters{decode_duration_ns - callbacks_duration_ns}, metadata}; + } +}; + } // namespace nebula::drivers diff --git a/src/nebula_hesai/nebula_hesai_decoders/include/nebula_hesai_decoders/decoders/hesai_packet.hpp b/src/nebula_hesai/nebula_hesai_decoders/include/nebula_hesai_decoders/decoders/hesai_packet.hpp index 189791e8a..7e3459e16 100644 --- a/src/nebula_hesai/nebula_hesai_decoders/include/nebula_hesai_decoders/decoders/hesai_packet.hpp +++ b/src/nebula_hesai/nebula_hesai_decoders/include/nebula_hesai_decoders/decoders/hesai_packet.hpp @@ -89,6 +89,29 @@ struct SecondsSinceEpoch } }; +struct Header19B // FT120 +{ + // manual, 3.1.2.1 + /// @brief Start of Packet, 0xEEFF + uint16_t sop; + uint8_t protocol_major; + uint8_t protocol_minor; + uint8_t reserved1[2]; + + // manual, 3.1.2.2 + uint16_t column_num; // 160 + uint16_t row_num; // 120 + uint8_t column_res; // 63, to be multiplied by standard coefficient of 0.01° + uint8_t row_res; // 63, to be multiplied by standard coefficient of 0.01° + uint8_t + first_block_return; // 0, single return, 1 dual return & block 1 returns first type of dual + // mode; 2 dual return & block 1 returns second type of dual mode; + uint8_t dis_unit; // 4, mm + uint8_t reserved2; + uint16_t block_row_num; // 120 + uint8_t reserved3[8]; +}; + struct Header12B { uint16_t sop; @@ -129,6 +152,13 @@ struct Unit4B uint8_t confidence_or_reserved; }; +struct Unit5B +{ + uint16_t distance; + uint8_t reflectivity; + uint16_t reserved; +}; + template struct Block { @@ -139,6 +169,17 @@ struct Block [[nodiscard]] uint32_t get_azimuth() const { return azimuth; } }; +template +struct NoAzimuthBlock +{ + UnitT units[UnitN]; + using unit_t = UnitT; + // FT120: azimuth came from angle correction file: each ray has its own angle + // uint32_t azimuth{0}; // do not add data into struct, or it changes data alignment + + [[nodiscard]] uint32_t get_azimuth() const { return 0; } +}; + template struct FineAzimuthBlock { diff --git a/src/nebula_hesai/nebula_hesai_decoders/include/nebula_hesai_decoders/decoders/hesai_sensor.hpp b/src/nebula_hesai/nebula_hesai_decoders/include/nebula_hesai_decoders/decoders/hesai_sensor.hpp index c4c990363..2f4d733e2 100644 --- a/src/nebula_hesai/nebula_hesai_decoders/include/nebula_hesai_decoders/decoders/hesai_sensor.hpp +++ b/src/nebula_hesai/nebula_hesai_decoders/include/nebula_hesai_decoders/decoders/hesai_sensor.hpp @@ -17,19 +17,24 @@ #include "nebula_core_decoders/point_filters/blockage_mask.hpp" #include "nebula_core_decoders/point_filters/downsample_mask.hpp" #include "nebula_hesai_decoders/decoders/angle_corrector_calibration_based.hpp" +#include "nebula_hesai_decoders/decoders/angle_corrector_calibration_based_solid_state.hpp" #include "nebula_hesai_decoders/decoders/angle_corrector_correction_based.hpp" #include "nebula_hesai_decoders/decoders/hesai_packet.hpp" #include #include +#include #include #include namespace nebula::drivers { -enum class AngleCorrectionType { CALIBRATION, CORRECTION }; +enum class AngleCorrectionType { CALIBRATION = 0, CORRECTION, SOLIDSTATE }; + +template +using static_switch = typename std::tuple_element>::type; /// @brief Base class for all sensor definitions /// @tparam PacketT The packet type of the sensor @@ -89,10 +94,15 @@ class HesaiSensor public: using packet_t = PacketT; - using angle_corrector_t = typename std::conditional< - (AngleCorrection == AngleCorrectionType::CALIBRATION), - AngleCorrectorCalibrationBased, - AngleCorrectorCorrectionBased>::type; + + using angle_corrector_t = static_switch< + static_cast(AngleCorrection), + AngleCorrectorCalibrationBased< + PacketT::n_channels, PacketT::degree_subdivisions>, // CALIBRATION + AngleCorrectorCorrectionBased, // CORRECTION + AngleCorrectorCalibrationBasedSolidState< + PacketT::n_channels, PacketT::degree_subdivisions>>; // SOLIDSTATE; degree_subdivisions + // represent the sensor column count HesaiSensor() = default; virtual ~HesaiSensor() = default; diff --git a/src/nebula_hesai/nebula_hesai_decoders/include/nebula_hesai_decoders/decoders/pandar_ft120.hpp b/src/nebula_hesai/nebula_hesai_decoders/include/nebula_hesai_decoders/decoders/pandar_ft120.hpp new file mode 100644 index 000000000..478beb0d7 --- /dev/null +++ b/src/nebula_hesai/nebula_hesai_decoders/include/nebula_hesai_decoders/decoders/pandar_ft120.hpp @@ -0,0 +1,123 @@ +// 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_hesai_decoders/decoders/functional_safety.hpp" +#include "nebula_hesai_decoders/decoders/hesai_packet.hpp" +#include "nebula_hesai_decoders/decoders/hesai_sensor.hpp" + +#include + +#include + +namespace nebula::drivers +{ + +namespace hesai_packet +{ + +#pragma pack(push, 1) + +struct TailFT120 +{ + uint8_t reserved1[7]; + uint16_t column_id; + uint8_t frame_id; // counter, 0-255; incremented at each new scan + uint8_t reserved2; + uint8_t return_mode; + uint16_t frame_period; // 100, ms (sensor works @ 10Hz) + SecondsSinceEpoch date_time; + uint32_t timestamp; + uint8_t factory_information; // fixed, 0x42 + uint32_t udp_sequence; + uint32_t crc_tail; + uint8_t + signature[16]; // packet AES signature, pre-header to crc_tail; 0, if no key set in sensor +}; + +struct PacketFT120 +: public PacketBase<1, 120, 2, 160> // using degreeSubdivisions as the column count, to be supplied + // in AngleCorrectorCalibrationBasedSolidState +{ + using body_t = Body< + NoAzimuthBlock, PacketFT120::n_blocks>; // manual, 3.1.2.3 + Header19B header; + body_t body; + TailFT120 tail; // tail contains ColumnID value, used to identify the column of sensor readings + // inside the packet + + /* Ignored optional fields */ + // 3.1.3. Ethernet tail, 4 more bytes for frame check sequence + // uint8_t cyber_security[32]; +}; + +#pragma pack(pop) + +} // namespace hesai_packet + +class PandarFT120 : public HesaiSensor +{ +private: +public: + static constexpr float min_range = 0.05; + static constexpr float max_range = 25.0; + static constexpr int32_t col_N = 160; + static constexpr int32_t row_N = 120; + static constexpr size_t max_scan_buffer_points = col_N * row_N; + static constexpr FieldOfView fov_mdeg{ + {40'000, 140'000}, {-37'500, 37'500}}; + static constexpr AnglePair peak_resolution_mdeg{ + (fov_mdeg.azimuth.end - fov_mdeg.azimuth.start) / col_N, + (fov_mdeg.elevation.end - fov_mdeg.elevation.start) / row_N, + }; + + int get_packet_relative_point_time_offset( + uint32_t block_id, uint32_t channel_id, const packet_t & packet) override + { + // avoid warning "unused parameter" + (void)block_id; + (void)channel_id; + (void)packet; + + return 0; // all measurements are took at the same time + } + + ReturnType get_return_type( + hesai_packet::return_mode::ReturnMode return_mode, unsigned int return_idx, + const std::vector & return_units) override + { + // we could get info directly from packet contents: + // - return_mode is a copy of PandarFT120.tail.return_mode + // - return_idx is a copy of PandarFT120.header.first_block_return + + (void)return_units; + + switch (return_mode) { + case hesai_packet::return_mode::SINGLE_FIRST: + return ReturnType::FIRST; + case hesai_packet::return_mode::SINGLE_STRONGEST: + return ReturnType::STRONGEST; + + case hesai_packet::return_mode::DUAL_FIRST_STRONGEST: + // return_idx is 1 or 2 + return return_idx == 1 ? ReturnType::FIRST : ReturnType::STRONGEST; + + default: + return ReturnType::UNKNOWN; + } + } +}; + +} // namespace nebula::drivers diff --git a/src/nebula_hesai/nebula_hesai_decoders/include/nebula_hesai_decoders/hesai_driver.hpp b/src/nebula_hesai/nebula_hesai_decoders/include/nebula_hesai_decoders/hesai_driver.hpp index 49b94302b..151670d4c 100644 --- a/src/nebula_hesai/nebula_hesai_decoders/include/nebula_hesai_decoders/hesai_driver.hpp +++ b/src/nebula_hesai/nebula_hesai_decoders/include/nebula_hesai_decoders/hesai_driver.hpp @@ -108,8 +108,10 @@ class HesaiDriver HesaiDriver() = delete; /// @brief Constructor /// @param sensor_configuration SensorConfiguration for this driver - /// @param calibration_configuration CalibrationConfiguration for this driver (either - /// HesaiCalibrationConfiguration for sensors other than AT128 or HesaiCorrection for AT128) + /// @param calibration_configuration CalibrationConfiguration for this driver ( + /// HesaiCalibrationConfiguration for rotating sensors other than AT128 or + /// HesaiCorrection for AT128 or + /// HesaiSolidStateCalibration for FT120 ) HesaiDriver( const std::shared_ptr & sensor_configuration, const std::shared_ptr & diff --git a/src/nebula_hesai/nebula_hesai_decoders/src/hesai_driver.cpp b/src/nebula_hesai/nebula_hesai_decoders/src/hesai_driver.cpp index adbf2bfa6..c1994ea45 100644 --- a/src/nebula_hesai/nebula_hesai_decoders/src/hesai_driver.cpp +++ b/src/nebula_hesai/nebula_hesai_decoders/src/hesai_driver.cpp @@ -10,6 +10,7 @@ #include "nebula_hesai_decoders/decoders/pandar_40.hpp" #include "nebula_hesai_decoders/decoders/pandar_64.hpp" #include "nebula_hesai_decoders/decoders/pandar_at128.hpp" +#include "nebula_hesai_decoders/decoders/pandar_ft120.hpp" #include "nebula_hesai_decoders/decoders/pandar_qt128.hpp" #include "nebula_hesai_decoders/decoders/pandar_qt64.hpp" #include "nebula_hesai_decoders/decoders/pandar_xt16.hpp" @@ -83,6 +84,12 @@ HesaiDriver::HesaiDriver( std::move(blockage_mask_plugin)); break; } + case SensorModel::HESAI_PANDARFT120: { + scan_decoder_ = initialize_decoder( + sensor_configuration, calibration_data, alive_cb, stuck_cb, status_cb, lost_cb, + std::move(blockage_mask_plugin)); + break; + } case SensorModel::UNKNOWN: driver_status_ = nebula::Status::INVALID_SENSOR_MODEL; throw std::runtime_error("Invalid sensor model."); @@ -110,7 +117,12 @@ std::shared_ptr HesaiDriver::initialize_decoder( auto packet_loss_detector = initialize_packet_loss_detector(lost_cb); using CalibT = typename SensorT::angle_corrector_t::correction_data_t; - return std::make_shared>( + + using HesaiDecoderTp = typename std::conditional< + (std::is_same::value), HesaiSolidStateDecoder, + HesaiDecoder>::type; + + return std::make_shared( sensor_configuration, std::dynamic_pointer_cast(calibration_configuration), logger_->child("Decoder"), functional_safety_decoder, packet_loss_detector, std::move(blockage_mask_plugin)); diff --git a/src/nebula_hesai/nebula_hesai_hw_interfaces/include/nebula_hesai_hw_interfaces/hesai_cmd_response.hpp b/src/nebula_hesai/nebula_hesai_hw_interfaces/include/nebula_hesai_hw_interfaces/hesai_cmd_response.hpp index 0bb2e2e4d..65c785b9d 100644 --- a/src/nebula_hesai/nebula_hesai_hw_interfaces/include/nebula_hesai_hw_interfaces/hesai_cmd_response.hpp +++ b/src/nebula_hesai/nebula_hesai_hw_interfaces/include/nebula_hesai_hw_interfaces/hesai_cmd_response.hpp @@ -280,6 +280,8 @@ struct HesaiInventoryBase case 40: case 48: return "PandarAT128"; + case 120: + return "PandarFT120"; default: return "Unknown(" + std::to_string(static_cast(model)) + ")"; } @@ -447,6 +449,59 @@ struct HesaiInventory_AT128 : public HesaiInventoryBase Internal value; }; +struct HesaiInventory_FT120 : public HesaiInventoryBase +{ + struct Internal // : public HesaiInventoryBase::Internal + { + // byte 0-15: SN + char sn[18]; + // byte 19: model name, char --> 17-24 + empty bytes + char product_name[32]; + char date_of_manufacture[16]; // yyyy-mm-dd + 6 empty bytes + uint8_t mac[6]; + char sw_ver[16]; + char unknown[16]; // contains string "unused" + char hw_ver[16]; // firmware version + char unknown2[16]; // contains string "3" + char control_fw_ver[16]; + char sensor_fw_ver[16]; + char unknown3[16]; // contains string "51e19f60" + char unknown4[16]; // contains string "a3db47f7" + char unknown5[4]; + uint8_t product_model; // ex: 78; char: x; dec: 120! + char unknown6[11]; // contains string "x" in the middle + }; + + explicit HesaiInventory_FT120(Internal value) : value(value) + { + std::memcpy(base.sn, value.sn, sizeof(base.sn)); + std::memcpy( + base.date_of_manufacture, value.date_of_manufacture, sizeof(base.date_of_manufacture)); + std::memcpy(base.mac, value.mac, sizeof(base.mac)); + std::memcpy(base.sw_ver, value.sw_ver, sizeof(base.sw_ver)); + std::memcpy(base.hw_ver, value.hw_ver, sizeof(base.hw_ver)); + std::memcpy(base.control_fw_ver, value.control_fw_ver, sizeof(base.control_fw_ver)); + std::memcpy(base.sensor_fw_ver, value.sensor_fw_ver, sizeof(base.sensor_fw_ver)); + } + + [[nodiscard]] uint8_t model_number() const override { return value.product_model; } + + [[nodiscard]] const HesaiInventoryBase::Internal & get() const override { return base; } + + [[nodiscard]] ordered_json sensor_specifics_to_json() const override + { + ordered_json j; + j["product_name"] = value.product_name; + j["mac"] = value.mac; + j["model"] = get_str_model(value.product_model); + return j; + } + +private: + Internal value; + HesaiInventoryBase::Internal base; +}; + /// @brief struct of PTC_COMMAND_GET_CONFIG_INFO struct HesaiConfigBase { @@ -566,6 +621,56 @@ struct HesaiConfig_OT128_AT128 : public HesaiConfigBase Internal value; }; +struct HesaiConfig_FT120 : public HesaiConfigBase +{ + struct Internal // structure does not begin like HesaiConfigBase::Internal + { + uint8_t ipaddr[4]; + uint8_t mask[4]; + uint8_t gateway[4]; + uint8_t dest_ipaddr[4]; + big_uint16_buf_t dest_LiDAR_udp_port; + uint8_t unknown1[4]; // here there are some numbers; their meaning is unknown... Samples: 126 + // 39 0 (maybe sync_angle) 200 (maybe fake rpm) + uint8_t unknown2[10]; + uint8_t unknown3; + uint8_t unknown4[8]; + big_uint16_buf_t dest_gps_udp_port; // byte 41 + + // here the last four bytes can be used to simulate some rotating sensors standard parameters + big_uint16_buf_t spin_rate; + uint8_t motor_status; + uint8_t unknown5; + }; + + explicit HesaiConfig_FT120(Internal value) : value(value) + { + std::memcpy(base.ipaddr, value.ipaddr, sizeof(base.ipaddr)); + std::memcpy(base.mask, value.mask, sizeof(base.mask)); + std::memcpy(base.gateway, value.gateway, sizeof(base.gateway)); + std::memcpy(base.dest_ipaddr, value.dest_ipaddr, sizeof(base.dest_ipaddr)); + + base.dest_LiDAR_udp_port = value.dest_LiDAR_udp_port; + base.dest_gps_udp_port = value.dest_gps_udp_port; + base.spin_rate = 600; + base.motor_status = value.motor_status; + } + + [[nodiscard]] const HesaiConfigBase::Internal & get() const override { return base; } + + [[nodiscard]] ordered_json sensor_specifics_to_json() const override + { + ordered_json j; + return j; + } + +private: + Internal value; + // adding a HesaiConfigBase::Internal private structure to be used for the output of the get() + // method + HesaiConfigBase::Internal base; +}; + struct HesaiConfig_XT_40P_64_QT128 : public HesaiConfigBase { struct Internal : public HesaiConfigBase::Internal @@ -668,7 +773,7 @@ inline std::ostream & operator<<(std::ostream & os, const HesaiLidarStatusBase & return os; } -struct HesaiLidarStatus_AT128_QT128 : public HesaiLidarStatusBase +struct HesaiLidarStatus_AT128_QT128_FT120 : public HesaiLidarStatusBase { struct Internal : public HesaiLidarStatusBase::Internal { @@ -700,7 +805,7 @@ struct HesaiLidarStatus_AT128_QT128 : public HesaiLidarStatusBase [[nodiscard]] const HesaiLidarStatusBase::Internal & get() const override { return value; } protected: - explicit HesaiLidarStatus_AT128_QT128(Internal value) : value(value) {} + explicit HesaiLidarStatus_AT128_QT128_FT120(Internal value) : value(value) {} [[nodiscard]] virtual std::array get_temperature_names() const = 0; @@ -708,9 +813,9 @@ struct HesaiLidarStatus_AT128_QT128 : public HesaiLidarStatusBase Internal value; }; -struct HesaiLidarStatusAT128 : public HesaiLidarStatus_AT128_QT128 +struct HesaiLidarStatusAT128 : public HesaiLidarStatus_AT128_QT128_FT120 { - explicit HesaiLidarStatusAT128(Internal value) : HesaiLidarStatus_AT128_QT128(value) {} + explicit HesaiLidarStatusAT128(Internal value) : HesaiLidarStatus_AT128_QT128_FT120(value) {} protected: [[nodiscard]] std::array get_temperature_names() const override @@ -723,9 +828,9 @@ struct HesaiLidarStatusAT128 : public HesaiLidarStatus_AT128_QT128 } }; -struct HesaiLidarStatusQT128 : public HesaiLidarStatus_AT128_QT128 +struct HesaiLidarStatusQT128 : public HesaiLidarStatus_AT128_QT128_FT120 { - explicit HesaiLidarStatusQT128(Internal value) : HesaiLidarStatus_AT128_QT128(value) {} + explicit HesaiLidarStatusQT128(Internal value) : HesaiLidarStatus_AT128_QT128_FT120(value) {} protected: [[nodiscard]] std::array get_temperature_names() const override @@ -796,6 +901,17 @@ struct HesaiLidarStatusOT128 : public HesaiLidarStatusBase Internal value; }; +struct HesaiLidarStatusFT120 : public HesaiLidarStatus_AT128_QT128_FT120 +{ + explicit HesaiLidarStatusFT120(Internal value) : HesaiLidarStatus_AT128_QT128_FT120(value) {} + +protected: + [[nodiscard]] std::array get_temperature_names() const override + { + return {"", "", "", "", "", "", "", "", ""}; + } +}; + struct HesaiLidarStatus_XT_40p : public HesaiLidarStatusBase { struct Internal : public HesaiLidarStatusBase::Internal diff --git a/src/nebula_hesai/nebula_hesai_hw_interfaces/src/hesai_hw_interface.cpp b/src/nebula_hesai/nebula_hesai_hw_interfaces/src/hesai_hw_interface.cpp index f01c69f3b..b186f2f9c 100644 --- a/src/nebula_hesai/nebula_hesai_hw_interfaces/src/hesai_hw_interface.cpp +++ b/src/nebula_hesai/nebula_hesai_hw_interfaces/src/hesai_hw_interface.cpp @@ -419,6 +419,10 @@ std::shared_ptr HesaiHwInterface::get_inventory() auto lidar_config = check_size_and_parse(response); return std::make_shared(lidar_config); } + case SensorModel::HESAI_PANDARFT120: { + auto lidar_config = check_size_and_parse(response); + return std::make_shared(lidar_config); + } } } @@ -443,6 +447,10 @@ std::shared_ptr HesaiHwInterface::get_config() auto lidar_config = check_size_and_parse(response); return std::make_shared(lidar_config); } + case SensorModel::HESAI_PANDARFT120: { + auto lidar_config = check_size_and_parse(response); + return std::make_shared(lidar_config); + } } } @@ -473,11 +481,19 @@ std::shared_ptr HesaiHwInterface::get_lidar_status() auto hesai_lidarstatus = check_size_and_parse(response); return std::make_shared(hesai_lidarstatus); } + case SensorModel::HESAI_PANDARFT120: { + auto hesai_lidarstatus = check_size_and_parse(response); + return std::make_shared(hesai_lidarstatus); + } } } Status HesaiHwInterface::set_spin_rate(uint16_t rpm) { + if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARFT120) { + return Status::SENSOR_CONFIG_ERROR; + } + std::vector request_payload; request_payload.emplace_back((rpm >> 8) & 0xff); request_payload.emplace_back(rpm & 0xff); @@ -599,7 +615,8 @@ Status HesaiHwInterface::set_lidar_range(int start_ddeg, int end_ddeg) { if ( sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARAT128 || - sensor_configuration_->sensor_model == SensorModel::HESAI_PANDAR64) { + sensor_configuration_->sensor_model == SensorModel::HESAI_PANDAR64 || + sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARFT120) { return Status::SENSOR_CONFIG_ERROR; } @@ -623,7 +640,8 @@ HesaiLidarRangeAll HesaiHwInterface::get_lidar_range() { if ( sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARAT128 || - sensor_configuration_->sensor_model == SensorModel::HESAI_PANDAR64) { + sensor_configuration_->sensor_model == SensorModel::HESAI_PANDAR64 || + sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARFT120) { throw std::runtime_error("Not supported on this sensor"); } @@ -707,7 +725,9 @@ bool HesaiHwInterface::get_up_close_blockage_detection() Status HesaiHwInterface::check_and_set_lidar_range( const HesaiCalibrationConfigurationBase & calibration) { - if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARAT128) { + if ( + sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARAT128 || + sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARFT120) { return Status::SENSOR_CONFIG_ERROR; } @@ -758,6 +778,12 @@ Status HesaiHwInterface::set_ptp_config( } std::vector request_payload; + + // On the FT120 there is always a reserved byte + if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARFT120) { + request_payload.emplace_back(1 & 0xff); + } + request_payload.emplace_back(profile & 0xff); request_payload.emplace_back(domain & 0xff); request_payload.emplace_back(network & 0xff); @@ -874,6 +900,10 @@ Status HesaiHwInterface::send_reset() Status HesaiHwInterface::set_rot_dir(int mode) { + if (sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARFT120) { + return Status::SENSOR_CONFIG_ERROR; + } + std::vector request_payload; request_payload.emplace_back(mode & 0xff); @@ -1120,24 +1150,27 @@ HesaiStatus HesaiHwInterface::check_and_set_config( std::this_thread::sleep_for(wait_time); } - auto current_rotation_speed = hesai_config.spin_rate; - if (sensor_configuration->rotation_speed != current_rotation_speed.value()) { - logger_->info( - "current lidar rotation_speed: " + - std::to_string(static_cast(current_rotation_speed.value()))); - logger_->info( - "current configuration rotation_speed: " + - std::to_string(sensor_configuration->rotation_speed)); - if (use_http_set_spin_rate()) { - set_spin_speed_async_http(sensor_configuration->rotation_speed); - } else { + // do not configure rotation for solid state sensors + if (sensor_configuration_->sensor_model != SensorModel::HESAI_PANDARFT120) { + auto current_rotation_speed = hesai_config.spin_rate; + if (sensor_configuration->rotation_speed != current_rotation_speed.value()) { logger_->info( - "Setting up spin rate via TCP." + std::to_string(sensor_configuration->rotation_speed)); - std::thread t( - [this, sensor_configuration] { set_spin_rate(sensor_configuration->rotation_speed); }); - t.join(); + "current lidar rotation_speed: " + + std::to_string(static_cast(current_rotation_speed.value()))); + logger_->info( + "current configuration rotation_speed: " + + std::to_string(sensor_configuration->rotation_speed)); + if (use_http_set_spin_rate()) { + set_spin_speed_async_http(sensor_configuration->rotation_speed); + } else { + logger_->info( + "Setting up spin rate via TCP." + std::to_string(sensor_configuration->rotation_speed)); + std::thread t( + [this, sensor_configuration] { set_spin_rate(sensor_configuration->rotation_speed); }); + t.join(); + } + std::this_thread::sleep_for(wait_time); } - std::this_thread::sleep_for(wait_time); } bool set_flg = false; @@ -1389,7 +1422,8 @@ HesaiStatus HesaiHwInterface::check_and_set_config() if ( sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARAT128 || - sensor_configuration_->sensor_model == SensorModel::HESAI_PANDAR64) { + sensor_configuration_->sensor_model == SensorModel::HESAI_PANDAR64 || + sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARFT120) { return Status::OK; } @@ -1419,6 +1453,7 @@ HesaiStatus HesaiHwInterface::check_and_set_config() 40: AT128? 42: OT128 48: ? +120: FT120 */ int HesaiHwInterface::nebula_model_to_hesai_model_no(nebula::drivers::SensorModel model) { @@ -1445,6 +1480,8 @@ int HesaiHwInterface::nebula_model_to_hesai_model_no(nebula::drivers::SensorMode return 42; case SensorModel::HESAI_PANDARAT128: return 48; + case SensorModel::HESAI_PANDARFT120: + return 120; // All other vendors and unknown sensors default: return -1; @@ -1475,6 +1512,7 @@ bool HesaiHwInterface::use_http_set_spin_rate(int model) case 38: case 42: case 48: + case 120: return false; } } @@ -1498,6 +1536,7 @@ bool HesaiHwInterface::use_http_get_lidar_monitor(int model) case 38: case 42: case 48: + case 120: return false; } }