feat: support of Hesai FT120 sensor #412
There are no files selected for viewing
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 | ||
|
Collaborator
There was a problem hiding this comment. (unconfirmed) might cause a column of points from an old scan to turn up in the new scan.
Author
There was a problem hiding this comment. This is linked to above comment. |
||
| 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 | ||
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 | ||
| } |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -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<int32_t> azimuth_adjust; | ||
| std::vector<int32_t> elevation_adjust; | ||
|
|
||
| uint32_t col_count; | ||
| uint32_t row_count; | ||
| uint32_t resolution; | ||
|
|
||
| nebula::Status load_from_bytes(const std::vector<uint8_t> & 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]); | ||
|
Collaborator
There was a problem hiding this comment. Perform size checking here (are there really count_bytes after the given offset?) |
||
|
|
||
| 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<uint8_t> contents( | ||
| (std::istreambuf_iterator<char>(stream)), std::istreambuf_iterator<char>()); | ||
|
|
||
| load_from_bytes(contents); | ||
|
|
||
| return Status::OK; | ||
| } | ||
|
|
||
| // from HesaiCorrection | ||
| nebula::Status save_to_file_from_bytes( | ||
| const std::string & calibration_file, const std::vector<uint8_t> & 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<float, float> get_fov_padding() const override | ||
| { | ||
| // For FT120 should be enough | ||
| return {-0.1, 0.1}; | ||
|
|
||
| } | ||
| }; | ||
|
|
||
| /* | ||
| <option value="0">Last Return</option> | ||
| <option value="1">Strongest Return</option> | ||
|
|
@@ -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: | ||
|
Collaborator
There was a problem hiding this comment. According to https://www.hesaitech.com/wp-content/uploads/2025/05/FT120_User_Manual_F01-en-250510.pdf, FT120 only supports Strongest, First and FirstStrongest modes. Please adjust accordingly.
Author
There was a problem hiding this comment. I've added the sensor to this case block because it already process the correct strings for the return modes. Do you prefer to have a dedicated case block? |
||
| 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) | ||
|
|
||
Binary file not shown.
Oops, something went wrong.
Oops, something went wrong.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
(unconfirmed) Given angluar azimuth resolution of 0.625deg, rounding down here could cause the last column to be filtered out.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
(Nebula would have to be changed to allow centi- or milli-degree settings here to fix this)
Uh oh!
There was an error while loading. Please reload this page.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I should think how to verify the issue.
Relevant part is at the end of
hesai_decoder.hpp.new_scan_timestamp_nstooutput_frame_, as we are processing a new scan but the buffers are not swapped yet;For this reason, we should always impose that
cut_angle != cloud_max_angle: due to inner working of the sensor, it made no sense to have a cut_angle which is not associated to the sensor minimum (= transmit old decode frame when we start processing a new one) or maximum column (we have completed one frame, send it). Ifcut_angle != cloud_max_angle, the timestamp is going to be assigned tooutput_frame_and the procedure should be correct.