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. diff --git a/nebula_common/include/nebula_common/hesai/hesai_common.hpp b/nebula_common/include/nebula_common/hesai/hesai_common.hpp index 950b2710..4c30d551 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: " << std::to_string(arg.ptp_lock_threshold); return os; } 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..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 @@ -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" @@ -700,6 +701,24 @@ 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); @@ -1069,6 +1088,19 @@ HesaiStatus HesaiHwInterface::check_and_set_config( t.join(); logger_->debug("Thread finished"); + 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); } else { // AT128 only supports PTP setup via HTTP logger_->info("Trying to set SyncAngle via HTTP"); 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" ], diff --git a/nebula_ros/schema/sub/lidar_hesai.json b/nebula_ros/schema/sub/lidar_hesai.json index b86a6d1a..eb2e24c3 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, 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);