feat(sync_angle): hesai. Enable Clock Sync Features

.cspell.json

@@ -14,6 +14,13 @@
     "AT",
     "XT",
     "QT",
-    "XTM"
+    "XTM",
+    "Pdelay",
+    "gprmc",
+    "Vccint",
+    "vccint",
+    "adctp",
+    "Adctp",
+
   ]
 }

nebula_common/include/nebula_common/nebula_status.hpp

@@ -70,6 +70,9 @@ struct Status
       case Status::HTTP_CONNECTION_ERROR:
         os << "Http Connection Error";
         break;
+      case Status::WAITING_FOR_SENSOR_RESPONSE:
+        os << "Waiting for Sensor Response";
+        break;
       case Status::ERROR_1:
       default:
         os << "Generic Error";

nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp

@@ -52,6 +52,7 @@
 const uint8_t PTC_COMMAND_SET_TRIGGER_METHOD = 0x1b;
 const uint8_t PTC_COMMAND_SET_STANDBY_MODE = 0x1c;
 const uint8_t PTC_COMMAND_SET_RETURN_MODE = 0x1e;
+const uint8_t PTC_COMMAND_SET_CLOCK_SOURCE = 0x1f;
 const uint8_t PTC_COMMAND_SET_DESTINATION_IP = 0x20;
 const uint8_t PTC_COMMAND_SET_CONTROL_PORT = 0x21;
 const uint8_t PTC_COMMAND_SET_LIDAR_RANGE = 0x22;
@@ -76,6 +77,16 @@
 
 const uint16_t MTU_SIZE = 1500;
 
+const int PTP_PROFILE = 0; // Fixed: IEEE 1588v2
+const int PTP_DOMAIN_ID = 0; // 0-127, Default: 0
+const int PTP_NETWORK_TRANSPORT = 0; // 0: UDP/IP, 1: L2
+const int PTP_LOG_ANNOUNCE_INTERVAL = 1; // Time interval between Announce messages, in units of log seconds (default: 1)
+const int PTP_SYNC_INTERVAL = 1; //Time interval between Sync messages, in units of log seconds (default: 1)
+const int PTP_LOG_MIN_DELAY_INTERVAL = 0; //Minimum permitted mean time between Delay_Req messages, in units of log seconds (default: 0)
+
+const int HESAI_LIDAR_GPS_CLOCK_SOURCE = 0;
+const int HESAI_LIDAR_PTP_CLOCK_SOURCE = 1;
+
 /// @brief Hardware interface of hesai driver
 class HesaiHwInterface : NebulaHwInterfaceBase
 {
@@ -104,7 +115,7 @@
   int tm_fail_cnt_max = 0;
   std::timed_mutex tms_;
   int tms_fail_cnt = 0;
-  int tms_fail_cnt_max = 0;
+  int tms_fail_cnt_max = 3;
   bool wl = true;
   bool is_solid_state = false;
   int target_model_no;
@@ -159,6 +170,9 @@
   /// @param setup_sensor Whether to also initialize tcp_driver for sensor configuration
   /// @return Resulting status
   Status InitializeTcpDriver(bool setup_sensor = true);
+  /// @brief Closes the TcpDriver and related resources
+  /// @return Status result
+  Status FinalizeTcpDriver();
   /// @brief Parsing json string to property_tree
   /// @param str JSON string
   /// @return Parsed property_tree
@@ -724,6 +738,11 @@
   /// @param with_run Automatically executes run() of TcpDriver
   /// @return Resulting status
   Status GetLidarRange(bool with_run = true);
+
+  Status SetClockSource(std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int clock_source, bool with_run);
+  Status SetClockSource(std::shared_ptr<boost::asio::io_context> ctx, int clock_source, bool with_run);
+  Status SetClockSource(int clock_source, bool with_run = true);
+
   /// @brief Setting values with PTC_COMMAND_SET_PTP_CONFIG
   /// @param target_tcp_driver TcpDriver used
   /// @param profile IEEE timing and synchronization standard
@@ -864,6 +883,29 @@
   /// @param rpm spin_speed (300, 600, 1200)
   /// @return Resulting status
   HesaiStatus SetSpinSpeedAsyncHttp(uint16_t rpm);
+
+  HesaiStatus SetPtpConfigSyncHttp(
+    std::shared_ptr<boost::asio::io_context> ctx,
+    int profile,
+    int domain,
+    int network,
+    int logAnnounceInterval,
+    int logSyncInterval,
+    int logMinDelatReqInterval);
+  HesaiStatus SetPtpConfigSyncHttp(int profile,
+                                   int domain,
+                                   int network,
+                                   int logAnnounceInterval,
+                                   int logSyncInterval,
+                                   int logMinDelatReqInterval);
+  HesaiStatus SetSyncAngleSyncHttp(
+    std::shared_ptr<boost::asio::io_context> ctx,
+    int enable,
+    int angle);
+  HesaiStatus SetSyncAngleSyncHttp(int enable, int angle);
+
+
+
   /// @brief Getting lidar_monitor via HTTP API
   /// @param ctx IO Context
   /// @param str_callback Callback function for received string

nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp

@@ -84,15 +84,11 @@
     std::cout << "Starting UDP server on: " << *sensor_configuration_ << std::endl;
     cloud_udp_driver_->init_receiver(
       sensor_configuration_->host_ip, sensor_configuration_->data_port);
-    PrintError("init ok");
     cloud_udp_driver_->receiver()->open();
-    PrintError("open ok");
     cloud_udp_driver_->receiver()->bind();
-    PrintError("bind ok");
 
     cloud_udp_driver_->receiver()->asyncReceive(
       std::bind(&HesaiHwInterface::ReceiveCloudPacketCallback, this, std::placeholders::_1));
-    PrintError("async receive set");
   } catch (const std::exception & ex) {
     Status status = Status::UDP_CONNECTION_ERROR;
     std::cerr << status << sensor_configuration_->sensor_ip << ","
@@ -201,6 +197,17 @@
   return Status::OK;
 }
 
+Status HesaiHwInterface::FinalizeTcpDriver() {
+  try {
+    tcp_driver_->close();
+  }
+  catch(std::exception &e) {
+    PrintError("Error while finalizing the TcpDriver");
+    return Status::UDP_CONNECTION_ERROR;
+  }
+  return Status::OK;
+}
+
 boost::property_tree::ptree HesaiHwInterface::ParseJson(const std::string & str)
 {
   boost::property_tree::ptree tree;
@@ -2042,6 +2049,62 @@
     [this](HesaiLidarRangeAll & result) { std::cout << result << std::endl; }, with_run);
 }
 
+Status HesaiHwInterface::SetClockSource(
+  std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int clock_source,
+  bool with_run)
+{
+  std::vector<unsigned char> buf_vec;
+  int len = 1;
+  buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH);
+  buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW);
+  buf_vec.emplace_back(PTC_COMMAND_SET_CLOCK_SOURCE);
+  buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE);
+  buf_vec.emplace_back((len >> 24) & 0xff);
+  buf_vec.emplace_back((len >> 16) & 0xff);
+  buf_vec.emplace_back((len >> 8) & 0xff);
+  buf_vec.emplace_back((len >> 0) & 0xff);
+
+  buf_vec.emplace_back((clock_source >> 0) & 0xff);
+
+  if (!CheckLock(tms_, tms_fail_cnt, tms_fail_cnt_max, "SetClockSource")) {
+    return SetClockSource(target_tcp_driver, clock_source, with_run);
+  }
+  PrintDebug("SetClockSource: start");
+
+  target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tms_, "SetClockSource"); });
+  if (with_run) {
+    boost::system::error_code ec = target_tcp_driver->run();
+    if (ec) {
+      PrintError("HesaiHwInterface::SetClockSource: " + ec.message());
+    }
+#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE
+    std::cout << "ctx->run(): SetReturnMode" << std::endl;
+#endif
+  }
+
+  return Status::WAITING_FOR_SENSOR_RESPONSE;
+}
+Status HesaiHwInterface::SetClockSource(
+  std::shared_ptr<boost::asio::io_context> ctx, int clock_source, bool with_run)
+{
+  auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx);
+  tcp_driver_local->init_socket(
+    sensor_configuration_->sensor_ip, PandarTcpCommandPort, sensor_configuration_->host_ip,
+    PandarTcpCommandPort);
+  return SetClockSource(tcp_driver_local, clock_source, with_run);
+}
+Status HesaiHwInterface::SetClockSource(int clock_source, bool with_run)
+{
+  //*
+  if (with_run) {
+    if (tcp_driver_s_->GetIOContext()->stopped()) {
+      tcp_driver_s_->GetIOContext()->restart();
+    }
+  }
+  //*/
+  return SetClockSource(tcp_driver_s_, clock_source, with_run);
+}
+
 Status HesaiHwInterface::SetPtpConfig(
   std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int profile, int domain,
   int network, int logAnnounceInterval = 1, int logSyncInterval = 1, int logMinDelayReqInterval = 0,
@@ -2521,14 +2584,90 @@
   return SetSpinSpeedAsyncHttp(std::make_shared<boost::asio::io_context>(), rpm);
 }
 
+HesaiStatus HesaiHwInterface::SetPtpConfigSyncHttp(
+  std::shared_ptr<boost::asio::io_context> ctx,
+  int profile,
+  int domain,
+  int network,
+  int logAnnounceInterval,
+  int logSyncInterval,
+  int logMinDelatReqInterval)
+{
+  std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd;
+  auto st = GetHttpClientDriverOnce(ctx, hcd);
+  if (st != Status::OK) {
+    return st;
+  }
+
+  auto response = hcd->get(
+    (boost::format("/pandar.cgi?action=set&object=lidar&key=ptp_configuration&value={" \
+              "\"Profile\": %d," \
+              "\"Domain\": %d," \
+              "\"Network\": %d," \
+              "\"LogAnnounceInterval\": %d," \
+              "\"LogSyncInterval\": %d," \
+              "\"LogMinDelayReqInterval\": %d," \
+              "\"tsn_switch\": %d" \
+              "}")
+              % profile % domain % network % logAnnounceInterval % logSyncInterval %logMinDelatReqInterval % 0
+    ).str());
+  ctx->run();
+  PrintInfo(response);
+  return Status::OK;
+}
+
+HesaiStatus HesaiHwInterface::SetPtpConfigSyncHttp(int profile,
+                                                   int domain,
+                                                   int network,
+                                                   int logAnnounceInterval,
+                                                   int logSyncInterval,
+                                                   int logMinDelatReqInterval)
+{
+  return SetPtpConfigSyncHttp(std::make_shared<boost::asio::io_context>(),
+                              profile,
+                              domain,
+                              network,
+                              logAnnounceInterval,
+                              logSyncInterval,
+                              logMinDelatReqInterval);
+}
+
+HesaiStatus HesaiHwInterface::SetSyncAngleSyncHttp(
+  std::shared_ptr<boost::asio::io_context> ctx,
+  int enable,
+  int angle)
+{
+  std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd;
+  auto st = GetHttpClientDriverOnce(ctx, hcd);
+  if (st != Status::OK) {
+    return st;
+  }
+  auto tmp_str = (boost::format("/pandar.cgi?action=set&object=lidar_sync&key=sync_angle&value={" \
+            "\"sync\": %d," \
+            "\"syncAngle\": %d" \
+            "}") % enable % angle).str();
+  PrintInfo(tmp_str);
+  auto response = hcd->get(tmp_str);
+  ctx->run();
+  PrintInfo(response);
+  return Status::OK;
+}
+
+HesaiStatus HesaiHwInterface::SetSyncAngleSyncHttp(int enable, int angle)
+{
+  return SetSyncAngleSyncHttp(std::make_shared<boost::asio::io_context>(),
+                              enable,
+                              angle);
+}
+
 HesaiStatus HesaiHwInterface::GetLidarMonitorAsyncHttp(
   std::shared_ptr<boost::asio::io_context> ctx,
   std::function<void(const std::string & str)> str_callback)
 {
   std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd;
   auto st = GetHttpClientDriverOnce(ctx, hcd);
   if (st != Status::OK) {
-    PrintError("HesaiHwInterface::GetLidarMonitorAsyncHttp: cannnot GetHttpClientDriverOnce");
+    PrintError("HesaiHwInterface::GetLidarMonitorAsyncHttp: cannot GetHttpClientDriverOnce");
     return st;
   }
 
@@ -2624,22 +2763,51 @@
     t.join();
   }
 
-  set_flg = false;
-  auto sync_angle = static_cast<int>(hesai_config.sync_angle / 100);
-  auto scan_phase = static_cast<int>(sensor_configuration->scan_phase);
-  int sync_flg = 0 < scan_phase ? 1 : 0;
-  if (hesai_config.sync != sync_flg) {
-    set_flg = true;
-  } else if (0 < sync_flg && scan_phase != sync_angle) {
+  if (sensor_configuration->sensor_model != SensorModel::HESAI_PANDARAT128){
     set_flg = true;
-  }
-  if (set_flg) {
-    PrintInfo("current lidar sync: " + std::to_string(hesai_config.sync));
-    PrintInfo("current lidar sync_angle: " + std::to_string(hesai_config.sync_angle));
-    PrintInfo("current configuration scan_phase: " + std::to_string(scan_phase));
-    std::thread t([this, sync_flg, scan_phase] { SetSyncAngle(sync_flg, scan_phase); });
+    auto sync_angle = static_cast<int>(hesai_config.sync_angle / 100);
+    auto scan_phase = static_cast<int>(sensor_configuration->scan_phase);
+    int sync_flg = 1;
+    if (scan_phase != sync_angle) {
+      set_flg = true;
+    }
+    if (sync_flg && set_flg) {
+      PrintInfo("current lidar sync: " + std::to_string(hesai_config.sync));
+      PrintInfo("current lidar sync_angle: " + std::to_string(sync_angle));
+      PrintInfo("current configuration scan_phase: " + std::to_string(scan_phase));
+      std::thread t([this, sync_flg, scan_phase] {
+        SetSyncAngle(sync_flg, scan_phase);
+      });
+      t.join();
+    }
+
+    std::thread t([this] {
+      PrintInfo("Trying to set Clock source to PTP");
+      SetClockSource(HESAI_LIDAR_PTP_CLOCK_SOURCE);
+      PrintInfo("Trying to set PTP Config: IEEE 1588 v2, Domain: 0, Transport: UDP/IP");
+      SetPtpConfig(PTP_PROFILE,
+                   PTP_DOMAIN_ID,
+                   PTP_NETWORK_TRANSPORT,
+                   PTP_LOG_ANNOUNCE_INTERVAL,
+                   PTP_SYNC_INTERVAL,
+                   PTP_LOG_MIN_DELAY_INTERVAL
+      );
+    });
     t.join();
   }
+  else { //AT128 only supports PTP setup via HTTP
+    PrintInfo("Trying to set SyncAngle via HTTP");
+    SetSyncAngleSyncHttp(1,
+                         static_cast<int>(sensor_configuration->scan_phase));
+    PrintInfo("Trying to set PTP Config: IEEE 1588 v2, Domain: 0, Transport: UDP/IP via HTTP");
+    SetPtpConfigSyncHttp(PTP_PROFILE,
+                         PTP_DOMAIN_ID,
+                         PTP_NETWORK_TRANSPORT,
+                         PTP_LOG_ANNOUNCE_INTERVAL,
+                         PTP_SYNC_INTERVAL,
+                         PTP_LOG_MIN_DELAY_INTERVAL);
+
+  }
 
 #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE
   std::cout << "End CheckAndSetConfig(HesaiConfig)!!" << std::endl;
@@ -2874,6 +3042,9 @@
         tcp_driver_s_->close();
         tcp_driver_s_->open();
       }
+      else {
+        return true;
+      }
       return false;
     }
     fail_cnt = 0;

nebula_ros/include/nebula_ros/hesai/hesai_hw_interface_ros_wrapper.hpp

@@ -64,6 +64,7 @@ class HesaiHwInterfaceRosWrapper final : public rclcpp::Node, NebulaHwInterfaceW
 
 public:
   explicit HesaiHwInterfaceRosWrapper(const rclcpp::NodeOptions & options);
+  ~HesaiHwInterfaceRosWrapper() noexcept override;
   /// @brief Start point cloud streaming (Call CloudInterfaceStart of HwInterface)
   /// @return Resulting status
   Status StreamStart() override;