From 806db830f1336aad649f052418444940f0f99eb1 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Tue, 26 Nov 2024 20:42:34 +0900 Subject: [PATCH 01/10] chore(hesai): add `ptp_lock_threshold` parameter to Hesai JSON schema Signed-off-by: Max SCHMELLER --- nebula_ros/schema/sub/lidar_hesai.json | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/nebula_ros/schema/sub/lidar_hesai.json b/nebula_ros/schema/sub/lidar_hesai.json index b86a6d1a..1a043cd0 100644 --- a/nebula_ros/schema/sub/lidar_hesai.json +++ b/nebula_ros/schema/sub/lidar_hesai.json @@ -71,6 +71,13 @@ ], "description": "For automotive profile,'TSN' or 'NON_TSN'." }, + "ptp_lock_threshold": { + "type": "integer", + "default": "100", + "minimum": 1, + "maximum": 100, + "description": "The maximum sensor clock offset in microseconds from the master clock which will still be treated as being locked." + }, "sync_angle": { "type": "integer", "minimum": 0, @@ -86,4 +93,4 @@ "description": "The angle in degrees at which pointclouds are cut/published." } } -} +} \ No newline at end of file From c2594be751e5d3520b7b0261ccc9dc81169d3189 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Tue, 26 Nov 2024 20:47:42 +0900 Subject: [PATCH 02/10] chore(hesai): add `ptp_lock_threshold` as required parameter Signed-off-by: Max SCHMELLER --- nebula_ros/config/lidar/hesai/Pandar128E4X.param.yaml | 1 + nebula_ros/config/lidar/hesai/Pandar40P.param.yaml | 1 + nebula_ros/config/lidar/hesai/Pandar64.param.yaml | 1 + nebula_ros/config/lidar/hesai/PandarAT128.param.yaml | 1 + nebula_ros/config/lidar/hesai/PandarQT128.param.yaml | 1 + nebula_ros/config/lidar/hesai/PandarQT64.param.yaml | 1 + nebula_ros/config/lidar/hesai/PandarXT32.param.yaml | 1 + nebula_ros/config/lidar/hesai/PandarXT32M.param.yaml | 1 + nebula_ros/schema/Pandar128E4X.schema.json | 4 ++++ nebula_ros/schema/Pandar40P.schema.json | 4 ++++ nebula_ros/schema/Pandar64.schema.json | 4 ++++ nebula_ros/schema/PandarAT128.schema.json | 4 ++++ nebula_ros/schema/PandarQT128.schema.json | 4 ++++ nebula_ros/schema/PandarQT64.schema.json | 4 ++++ nebula_ros/schema/PandarXT32.schema.json | 4 ++++ nebula_ros/schema/PandarXT32M.schema.json | 4 ++++ 16 files changed, 40 insertions(+) diff --git a/nebula_ros/config/lidar/hesai/Pandar128E4X.param.yaml b/nebula_ros/config/lidar/hesai/Pandar128E4X.param.yaml index da32fc0e..87c59982 100644 --- a/nebula_ros/config/lidar/hesai/Pandar128E4X.param.yaml +++ b/nebula_ros/config/lidar/hesai/Pandar128E4X.param.yaml @@ -25,5 +25,6 @@ ptp_domain: 0 ptp_transport_type: L2 ptp_switch_type: TSN + ptp_lock_threshold: 100 retry_hw: true dual_return_distance_threshold: 0.1 diff --git a/nebula_ros/config/lidar/hesai/Pandar40P.param.yaml b/nebula_ros/config/lidar/hesai/Pandar40P.param.yaml index 332b2a1c..b74c2627 100644 --- a/nebula_ros/config/lidar/hesai/Pandar40P.param.yaml +++ b/nebula_ros/config/lidar/hesai/Pandar40P.param.yaml @@ -25,5 +25,6 @@ ptp_domain: 0 ptp_transport_type: UDP ptp_switch_type: TSN + ptp_lock_threshold: 100 retry_hw: true dual_return_distance_threshold: 0.1 diff --git a/nebula_ros/config/lidar/hesai/Pandar64.param.yaml b/nebula_ros/config/lidar/hesai/Pandar64.param.yaml index f731049f..ab115122 100644 --- a/nebula_ros/config/lidar/hesai/Pandar64.param.yaml +++ b/nebula_ros/config/lidar/hesai/Pandar64.param.yaml @@ -25,5 +25,6 @@ ptp_domain: 0 ptp_transport_type: UDP ptp_switch_type: TSN + ptp_lock_threshold: 100 retry_hw: true dual_return_distance_threshold: 0.1 diff --git a/nebula_ros/config/lidar/hesai/PandarAT128.param.yaml b/nebula_ros/config/lidar/hesai/PandarAT128.param.yaml index dff7789d..cb0e0082 100644 --- a/nebula_ros/config/lidar/hesai/PandarAT128.param.yaml +++ b/nebula_ros/config/lidar/hesai/PandarAT128.param.yaml @@ -26,5 +26,6 @@ ptp_domain: 0 ptp_transport_type: UDP ptp_switch_type: TSN + ptp_lock_threshold: 100 retry_hw: true dual_return_distance_threshold: 0.1 diff --git a/nebula_ros/config/lidar/hesai/PandarQT128.param.yaml b/nebula_ros/config/lidar/hesai/PandarQT128.param.yaml index 17779ee0..7b373ff5 100644 --- a/nebula_ros/config/lidar/hesai/PandarQT128.param.yaml +++ b/nebula_ros/config/lidar/hesai/PandarQT128.param.yaml @@ -25,5 +25,6 @@ ptp_domain: 0 ptp_transport_type: UDP ptp_switch_type: TSN + ptp_lock_threshold: 100 retry_hw: true dual_return_distance_threshold: 0.1 diff --git a/nebula_ros/config/lidar/hesai/PandarQT64.param.yaml b/nebula_ros/config/lidar/hesai/PandarQT64.param.yaml index f11d3007..cd24d439 100644 --- a/nebula_ros/config/lidar/hesai/PandarQT64.param.yaml +++ b/nebula_ros/config/lidar/hesai/PandarQT64.param.yaml @@ -25,5 +25,6 @@ ptp_domain: 0 ptp_transport_type: UDP ptp_switch_type: TSN + ptp_lock_threshold: 100 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 7288ec37..0b182c0f 100644 --- a/nebula_ros/config/lidar/hesai/PandarXT32.param.yaml +++ b/nebula_ros/config/lidar/hesai/PandarXT32.param.yaml @@ -25,5 +25,6 @@ ptp_domain: 0 ptp_transport_type: UDP ptp_switch_type: TSN + ptp_lock_threshold: 100 retry_hw: true dual_return_distance_threshold: 0.1 diff --git a/nebula_ros/config/lidar/hesai/PandarXT32M.param.yaml b/nebula_ros/config/lidar/hesai/PandarXT32M.param.yaml index cc1fffbc..20393387 100644 --- a/nebula_ros/config/lidar/hesai/PandarXT32M.param.yaml +++ b/nebula_ros/config/lidar/hesai/PandarXT32M.param.yaml @@ -25,5 +25,6 @@ ptp_domain: 0 ptp_transport_type: UDP ptp_switch_type: TSN + ptp_lock_threshold: 100 retry_hw: true dual_return_distance_threshold: 0.1 diff --git a/nebula_ros/schema/Pandar128E4X.schema.json b/nebula_ros/schema/Pandar128E4X.schema.json index 3dbb34f9..cae88b60 100644 --- a/nebula_ros/schema/Pandar128E4X.schema.json +++ b/nebula_ros/schema/Pandar128E4X.schema.json @@ -99,6 +99,9 @@ "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" }, @@ -130,6 +133,7 @@ "ptp_domain", "ptp_transport_type", "ptp_switch_type", + "ptp_lock_threshold", "retry_hw", "dual_return_distance_threshold" ], diff --git a/nebula_ros/schema/Pandar40P.schema.json b/nebula_ros/schema/Pandar40P.schema.json index abfa7769..e0b2ef83 100644 --- a/nebula_ros/schema/Pandar40P.schema.json +++ b/nebula_ros/schema/Pandar40P.schema.json @@ -90,6 +90,9 @@ "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" }, @@ -121,6 +124,7 @@ "ptp_domain", "ptp_transport_type", "ptp_switch_type", + "ptp_lock_threshold", "retry_hw", "dual_return_distance_threshold" ], diff --git a/nebula_ros/schema/Pandar64.schema.json b/nebula_ros/schema/Pandar64.schema.json index e006afa7..c8eea129 100644 --- a/nebula_ros/schema/Pandar64.schema.json +++ b/nebula_ros/schema/Pandar64.schema.json @@ -90,6 +90,9 @@ "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" }, @@ -121,6 +124,7 @@ "ptp_domain", "ptp_transport_type", "ptp_switch_type", + "ptp_lock_threshold", "retry_hw", "dual_return_distance_threshold" ], diff --git a/nebula_ros/schema/PandarAT128.schema.json b/nebula_ros/schema/PandarAT128.schema.json index f9780f76..7ef9cd4e 100644 --- a/nebula_ros/schema/PandarAT128.schema.json +++ b/nebula_ros/schema/PandarAT128.schema.json @@ -110,6 +110,9 @@ "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" }, @@ -142,6 +145,7 @@ "ptp_domain", "ptp_transport_type", "ptp_switch_type", + "ptp_lock_threshold", "retry_hw", "dual_return_distance_threshold" ], diff --git a/nebula_ros/schema/PandarQT128.schema.json b/nebula_ros/schema/PandarQT128.schema.json index 29af8d71..580e0d3a 100644 --- a/nebula_ros/schema/PandarQT128.schema.json +++ b/nebula_ros/schema/PandarQT128.schema.json @@ -93,6 +93,9 @@ "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" }, @@ -124,6 +127,7 @@ "ptp_domain", "ptp_transport_type", "ptp_switch_type", + "ptp_lock_threshold", "retry_hw", "dual_return_distance_threshold" ], diff --git a/nebula_ros/schema/PandarQT64.schema.json b/nebula_ros/schema/PandarQT64.schema.json index 811bad88..925f5196 100644 --- a/nebula_ros/schema/PandarQT64.schema.json +++ b/nebula_ros/schema/PandarQT64.schema.json @@ -90,6 +90,9 @@ "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" }, @@ -121,6 +124,7 @@ "ptp_domain", "ptp_transport_type", "ptp_switch_type", + "ptp_lock_threshold", "retry_hw", "dual_return_distance_threshold" ], diff --git a/nebula_ros/schema/PandarXT32.schema.json b/nebula_ros/schema/PandarXT32.schema.json index 77696754..d8c3e35e 100644 --- a/nebula_ros/schema/PandarXT32.schema.json +++ b/nebula_ros/schema/PandarXT32.schema.json @@ -93,6 +93,9 @@ "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" }, @@ -124,6 +127,7 @@ "ptp_domain", "ptp_transport_type", "ptp_switch_type", + "ptp_lock_threshold", "retry_hw", "dual_return_distance_threshold" ], diff --git a/nebula_ros/schema/PandarXT32M.schema.json b/nebula_ros/schema/PandarXT32M.schema.json index cbd1d2ae..9ad61e55 100644 --- a/nebula_ros/schema/PandarXT32M.schema.json +++ b/nebula_ros/schema/PandarXT32M.schema.json @@ -93,6 +93,9 @@ "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" }, @@ -124,6 +127,7 @@ "ptp_domain", "ptp_transport_type", "ptp_switch_type", + "ptp_lock_threshold", "retry_hw", "dual_return_distance_threshold" ], From 158d47f1b1e5631549de1e9f89676a863c19091d Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Tue, 26 Nov 2024 21:18:43 +0900 Subject: [PATCH 03/10] chore(hesai): add `ptp_lock_threshold` to HesaiSensorConfiguration and ROS2 wrapper Signed-off-by: Max SCHMELLER --- .../nebula_common/hesai/hesai_common.hpp | 4 +++- nebula_ros/src/hesai/hesai_ros_wrapper.cpp | 21 ++++++++++++------- 2 files changed, 16 insertions(+), 9 deletions(-) diff --git a/nebula_common/include/nebula_common/hesai/hesai_common.hpp b/nebula_common/include/nebula_common/hesai/hesai_common.hpp index 950b2710..a81e1768 100644 --- a/nebula_common/include/nebula_common/hesai/hesai_common.hpp +++ b/nebula_common/include/nebula_common/hesai/hesai_common.hpp @@ -49,6 +49,7 @@ struct HesaiSensorConfiguration : public LidarConfigurationBase uint8_t ptp_domain; PtpTransportType ptp_transport_type; PtpSwitchType ptp_switch_type; + uint8_t ptp_lock_threshold; }; /// @brief Convert HesaiSensorConfiguration to string (Overloading the << operator) /// @param os @@ -71,7 +72,8 @@ inline std::ostream & operator<<(std::ostream & os, HesaiSensorConfiguration con os << "PTP Profile: " << arg.ptp_profile << '\n'; os << "PTP Domain: " << std::to_string(arg.ptp_domain) << '\n'; os << "PTP Transport Type: " << arg.ptp_transport_type << '\n'; - os << "PTP Switch Type: " << arg.ptp_switch_type; + os << "PTP Switch Type: " << arg.ptp_switch_type << '\n'; + os << "PTP Lock Threshold: " << arg.ptp_lock_threshold; return os; } diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp index 136e5a08..858d7e0a 100644 --- a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp @@ -174,32 +174,37 @@ nebula::Status HesaiRosWrapper::declare_and_get_sensor_config_params() config.calibration_path = declare_parameter(calibration_parameter_name, param_read_write()); - auto _ptp_profile = declare_parameter("ptp_profile", param_read_only()); - config.ptp_profile = drivers::ptp_profile_from_string(_ptp_profile); + auto ptp_profile = declare_parameter("ptp_profile", param_read_only()); + config.ptp_profile = drivers::ptp_profile_from_string(ptp_profile); - auto _ptp_transport = declare_parameter("ptp_transport_type", param_read_only()); - config.ptp_transport_type = drivers::ptp_transport_type_from_string(_ptp_transport); + auto ptp_transport = declare_parameter("ptp_transport_type", param_read_only()); + config.ptp_transport_type = drivers::ptp_transport_type_from_string(ptp_transport); if ( config.ptp_transport_type != drivers::PtpTransportType::L2 && config.ptp_profile != drivers::PtpProfile::IEEE_1588v2 && config.ptp_profile != drivers::PtpProfile::UNKNOWN_PROFILE) { RCLCPP_WARN_STREAM( - get_logger(), "PTP transport was set to '" << _ptp_transport << "' but PTP profile '" - << _ptp_profile + get_logger(), "PTP transport was set to '" << ptp_transport << "' but PTP profile '" + << ptp_profile << "' only supports 'L2'. Setting it to 'L2'."); config.ptp_transport_type = drivers::PtpTransportType::L2; set_parameter(rclcpp::Parameter("ptp_transport_type", "L2")); } - auto _ptp_switch = declare_parameter("ptp_switch_type", param_read_only()); - config.ptp_switch_type = drivers::ptp_switch_type_from_string(_ptp_switch); + auto ptp_switch = declare_parameter("ptp_switch_type", param_read_only()); + config.ptp_switch_type = drivers::ptp_switch_type_from_string(ptp_switch); { rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); descriptor.integer_range = int_range(0, 127, 1); config.ptp_domain = declare_parameter("ptp_domain", descriptor); } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); + descriptor.integer_range = int_range(1, 100, 1); + config.ptp_lock_threshold = declare_parameter("ptp_lock_threshold", descriptor); + } auto new_cfg_ptr = std::make_shared(config); return validate_and_set_config(new_cfg_ptr); From 7bb9396ef8289a817dd78b376d8699647b586a67 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Tue, 26 Nov 2024 21:31:20 +0900 Subject: [PATCH 04/10] chore(hesai_hw_interface): add set/get_ptp_lock_offset member functions Signed-off-by: Max SCHMELLER --- .../hesai_hw_interface.hpp | 7 +++++++ .../hesai_hw_interface.cpp | 17 +++++++++++++++++ 2 files changed, 24 insertions(+) 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 b48fb636..6f738c0b 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 @@ -75,6 +75,8 @@ const uint8_t g_ptc_command_set_lidar_range = 0x22; const uint8_t g_ptc_command_get_lidar_range = 0x23; const uint8_t g_ptc_command_set_ptp_config = 0x24; const uint8_t g_ptc_command_get_ptp_config = 0x26; +const uint8_t g_ptp_command_set_ptp_lock_offset = 0x39; +const uint8_t g_ptp_command_get_ptp_lock_offset = 0x3a; const uint8_t g_ptc_command_reset = 0x25; const uint8_t g_ptc_command_set_rotate_direction = 0x2a; const uint8_t g_ptc_command_lidar_monitor = 0x27; @@ -342,6 +344,11 @@ class HesaiHwInterface /// @brief Getting data with PTC_COMMAND_GET_PTP_CONFIG /// @return Resulting status HesaiPtpConfig get_ptp_config(); + + Status set_ptp_lock_offset(uint8_t lock_offset); + + uint8_t get_ptp_lock_offset(); + /// @brief Sending command with PTC_COMMAND_RESET /// @return Resulting status Status send_reset(); 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 0efa6fb5..5e9ebd86 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 @@ -700,6 +700,23 @@ HesaiPtpConfig HesaiHwInterface::get_ptp_config() return hesai_ptp_config; } +Status HesaiHwInterface::set_ptp_lock_offset(uint8_t lock_offset_us) +{ + std::vector request_payload; + request_payload.emplace_back(lock_offset_us); + + auto response_or_err = send_receive(g_ptp_command_set_ptp_lock_offset, request_payload); + response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({}))); + return Status::OK; +} + +uint8_t HesaiHwInterface::get_ptp_lock_offset() +{ + auto response_or_err = send_receive(g_ptp_command_get_ptp_lock_offset); + auto response = response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({}))); + return check_size_and_parse(response); +} + Status HesaiHwInterface::send_reset() { auto response_or_err = send_receive(g_ptc_command_reset); From c6ac792d732a663abb6676960a66479050df645e Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 27 Nov 2024 13:59:16 +0900 Subject: [PATCH 05/10] feat(hesai): set PTP lock offset on configure, for all sensors except AT128 Signed-off-by: Max SCHMELLER --- .../nebula_hesai_hw_interfaces/hesai_hw_interface.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) 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 5e9ebd86..c0f349da 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 @@ -4,6 +4,7 @@ #include "nebula_common/hesai/hesai_common.hpp" #include "nebula_common/hesai/hesai_status.hpp" +#include "nebula_common/loggers/logger.hpp" #include "nebula_common/nebula_common.hpp" #include "nebula_common/nebula_status.hpp" #include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp" @@ -1086,6 +1087,15 @@ HesaiStatus HesaiHwInterface::check_and_set_config( t.join(); logger_->debug("Thread finished"); + uint8_t sensor_ptp_lock_threshold = get_ptp_lock_offset(); + if (sensor_ptp_lock_threshold != sensor_configuration_->ptp_lock_threshold) { + NEBULA_LOG_STREAM( + logger_->info, "changing sensor PTP lock offset from " + << static_cast(sensor_ptp_lock_threshold) << " to " + << static_cast(sensor_configuration_->ptp_lock_threshold)); + set_ptp_lock_offset(sensor_configuration_->ptp_lock_threshold); + } + std::this_thread::sleep_for(wait_time); } else { // AT128 only supports PTP setup via HTTP logger_->info("Trying to set SyncAngle via HTTP"); From 0746b0defd6be76e44c18627bbef1b67b26b775d Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 27 Nov 2024 05:13:20 +0000 Subject: [PATCH 06/10] ci(pre-commit): autofix --- nebula_ros/schema/sub/lidar_hesai.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nebula_ros/schema/sub/lidar_hesai.json b/nebula_ros/schema/sub/lidar_hesai.json index 1a043cd0..eb2e24c3 100644 --- a/nebula_ros/schema/sub/lidar_hesai.json +++ b/nebula_ros/schema/sub/lidar_hesai.json @@ -93,4 +93,4 @@ "description": "The angle in degrees at which pointclouds are cut/published." } } -} \ No newline at end of file +} From cfadb5a04f142bc7720b50e3fb86e958846d4470 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Thu, 28 Nov 2024 13:58:42 +0900 Subject: [PATCH 07/10] fix(hesai): print PTP lock threshold as a number, not a character --- nebula_common/include/nebula_common/hesai/hesai_common.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nebula_common/include/nebula_common/hesai/hesai_common.hpp b/nebula_common/include/nebula_common/hesai/hesai_common.hpp index a81e1768..4c30d551 100644 --- a/nebula_common/include/nebula_common/hesai/hesai_common.hpp +++ b/nebula_common/include/nebula_common/hesai/hesai_common.hpp @@ -73,7 +73,7 @@ inline std::ostream & operator<<(std::ostream & os, HesaiSensorConfiguration con os << "PTP Domain: " << std::to_string(arg.ptp_domain) << '\n'; os << "PTP Transport Type: " << arg.ptp_transport_type << '\n'; os << "PTP Switch Type: " << arg.ptp_switch_type << '\n'; - os << "PTP Lock Threshold: " << arg.ptp_lock_threshold; + os << "PTP Lock Threshold: " << std::to_string(arg.ptp_lock_threshold); return os; } From 3007f2dfd8c55a2c27cba84c428cdc699513cf03 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Fri, 29 Nov 2024 16:18:07 +0900 Subject: [PATCH 08/10] fix(hesai): set PTP lock offset only for OT128, QT128 Signed-off-by: Max SCHMELLER --- .../hesai_hw_interface.cpp | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) 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 c0f349da..b1287223 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 @@ -1087,13 +1087,17 @@ HesaiStatus HesaiHwInterface::check_and_set_config( t.join(); logger_->debug("Thread finished"); - uint8_t sensor_ptp_lock_threshold = get_ptp_lock_offset(); - if (sensor_ptp_lock_threshold != sensor_configuration_->ptp_lock_threshold) { - NEBULA_LOG_STREAM( - logger_->info, "changing sensor PTP lock offset from " - << static_cast(sensor_ptp_lock_threshold) << " to " - << static_cast(sensor_configuration_->ptp_lock_threshold)); - set_ptp_lock_offset(sensor_configuration_->ptp_lock_threshold); + if ( + sensor_configuration_->sensor_model == SensorModel::HESAI_PANDAR128_E4X || + sensor_configuration_->sensor_model == SensorModel::HESAI_PANDARQT128) { + uint8_t sensor_ptp_lock_threshold = get_ptp_lock_offset(); + if (sensor_ptp_lock_threshold != sensor_configuration_->ptp_lock_threshold) { + NEBULA_LOG_STREAM( + logger_->info, "changing sensor PTP lock offset from " + << static_cast(sensor_ptp_lock_threshold) << " to " + << static_cast(sensor_configuration_->ptp_lock_threshold)); + set_ptp_lock_offset(sensor_configuration_->ptp_lock_threshold); + } } std::this_thread::sleep_for(wait_time); From 97bbb78fff98a27706076c4534bc6dde04cc07ef Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 2 Dec 2024 07:48:10 +0000 Subject: [PATCH 09/10] ci(pre-commit): autofix --- .../src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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 b1287223..f25344bf 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 @@ -714,7 +714,8 @@ Status HesaiHwInterface::set_ptp_lock_offset(uint8_t lock_offset_us) uint8_t HesaiHwInterface::get_ptp_lock_offset() { auto response_or_err = send_receive(g_ptp_command_get_ptp_lock_offset); - auto response = response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({}))); + auto response = + response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({}))); return check_size_and_parse(response); } From a52297afd17770a56dc409ed3b62b81a62492aa9 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Mon, 2 Dec 2024 17:17:03 +0900 Subject: [PATCH 10/10] docs(hesai): describe effects of `ptp_lock_threshold` parameter Signed-off-by: Max SCHMELLER --- docs/parameters/vendors/hesai/common.md | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/docs/parameters/vendors/hesai/common.md b/docs/parameters/vendors/hesai/common.md index 359bba31..a804ccbb 100644 --- a/docs/parameters/vendors/hesai/common.md +++ b/docs/parameters/vendors/hesai/common.md @@ -24,6 +24,14 @@ While this is not found in the PTP standards, this influences how often the sens Set this to `TSN` if your switch supports gPTP or the AutoSAR protocol, and `NON_TSN` if the switch does not. In the latter case, the sensor will measure more often. +### `ptp_lock_threshold` + +_Only applies to `OT128` and `QT128`_ + +The maximum difference between the sensor and PTP master that will still be considered `locked` by the sensor, in microseconds. +When this threshold is crossed, the sensor will report its synchronization state to be `tracking`. +Nebula's hardware monitor treats only the `locked` state as `OK`, `tracking` as `WARNING` and `frozen` and `free run` as `ERROR`. + ## Scan Cutting and Field of View Scan cutting influences the time stamps of points and the point cloud headers.