fix(hesai): set PTP lock offset on sensor configuration

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.

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;
 }
 

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();

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 @@
   return hesai_ptp_config;
 }
 
+Status HesaiHwInterface::set_ptp_lock_offset(uint8_t lock_offset_us)
+{
+  std::vector<uint8_t> 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<uint8_t>(response);
+}
+
 Status HesaiHwInterface::send_reset()
 {
   auto response_or_err = send_receive(g_ptc_command_reset);
@@ -1069,6 +1088,19 @@
     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<int>(sensor_ptp_lock_threshold) << " to "
+                           << static_cast<int>(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");

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

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

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

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

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

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

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

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

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"
       ],

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"
       ],

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"
       ],

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"
       ],

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"
       ],

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"
       ],

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"
       ],

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"
       ],

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,

nebula_ros/src/hesai/hesai_ros_wrapper.cpp

@@ -174,32 +174,37 @@
   config.calibration_path =
     declare_parameter<std::string>(calibration_parameter_name, param_read_write());
 
-  auto _ptp_profile = declare_parameter<std::string>("ptp_profile", param_read_only());
-  config.ptp_profile = drivers::ptp_profile_from_string(_ptp_profile);
+  auto ptp_profile = declare_parameter<std::string>("ptp_profile", param_read_only());
+  config.ptp_profile = drivers::ptp_profile_from_string(ptp_profile);
 
-  auto _ptp_transport = declare_parameter<std::string>("ptp_transport_type", param_read_only());
-  config.ptp_transport_type = drivers::ptp_transport_type_from_string(_ptp_transport);
+  auto ptp_transport = declare_parameter<std::string>("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<std::string>("ptp_switch_type", param_read_only());
-  config.ptp_switch_type = drivers::ptp_switch_type_from_string(_ptp_switch);
+  auto ptp_switch = declare_parameter<std::string>("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<uint8_t>("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<uint8_t>("ptp_lock_threshold", descriptor);
+  }
 
   auto new_cfg_ptr = std::make_shared<const nebula::drivers::HesaiSensorConfiguration>(config);
   return validate_and_set_config(new_cfg_ptr);