feat: hesai ptp setup parameters

nebula_common/include/nebula_common/hesai/hesai_common.hpp

@@ -22,6 +22,9 @@ struct HesaiSensorConfiguration : SensorConfigurationBase
   uint16_t rotation_speed;
   uint16_t cloud_min_angle;
   uint16_t cloud_max_angle;
+  PtpProfile ptp_profile;
+  uint8_t ptp_domain;
+  PtpTransportType ptp_transport_type;
 };
 /// @brief Convert HesaiSensorConfiguration to string (Overloading the << operator)
 /// @param os
@@ -32,7 +35,9 @@ inline std::ostream & operator<<(std::ostream & os, HesaiSensorConfiguration con
   os << (SensorConfigurationBase)(arg) << ", GnssPort: " << arg.gnss_port
      << ", ScanPhase:" << arg.scan_phase << ", RotationSpeed:" << arg.rotation_speed
      << ", FOV(Start):" << arg.cloud_min_angle << ", FOV(End):" << arg.cloud_max_angle
-     << ", DualReturnDistanceThreshold:" << arg.dual_return_distance_threshold;
+     << ", DualReturnDistanceThreshold:" << arg.dual_return_distance_threshold
+     << ", PtpProfile:" << arg.ptp_profile << ", PtpDomain:" << std::to_string(arg.ptp_domain)
+     << ", PtpTransportType:" << arg.ptp_transport_type;
   return os;
 }
 
@@ -48,21 +53,10 @@ struct HesaiCalibrationConfiguration : CalibrationConfigurationBase
     if (!ifs) {
       return Status::INVALID_CALIBRATION_FILE;
     }
-
-    std::string header;
-    std::getline(ifs, header);
-
-    char sep;
-    int laser_id;
-    float elevation;
-    float azimuth;
-    while (!ifs.eof()) {
-      ifs >> laser_id >> sep >> elevation >> sep >> azimuth;
-      elev_angle_map[laser_id - 1] = elevation;
-      azimuth_offset_map[laser_id - 1] = azimuth;
-    }
+    std::ostringstream ss;
+    ss << ifs.rdbuf(); // reading data
     ifs.close();
-    return Status::OK;
+    return LoadFromString(ss.str());
   }
 
   /// @brief Loading calibration data
@@ -72,28 +66,31 @@ struct HesaiCalibrationConfiguration : CalibrationConfigurationBase
   {
     std::stringstream ss;
     ss << calibration_content;
-
-    std::string header;
-    std::getline(ss, header);
-
-    char sep;
-    int laser_id;
-    float elevation;
-    float azimuth;
-
     std::string line;
-    while (std::getline(ss, line)) {
-      if (line.empty()) {
+    constexpr size_t expected_cols = 3;
+    while(std::getline(ss, line)) {
+      boost::char_separator<char> sep(",");
+      boost::tokenizer<boost::char_separator<char>> tok(line, sep);
+
+      std::vector<std::string> actual_tokens(tok.begin(), tok.end());
+      if (actual_tokens.size() < expected_cols
+        || actual_tokens.size() > expected_cols
+        ) {
+        std::cerr << "Ignoring line with unexpected data:" << line << std::endl;
         continue;
       }
-      std::stringstream line_ss;
-      line_ss << line;
-      line_ss >> laser_id >> sep >> elevation >> sep >> azimuth;
-      elev_angle_map[laser_id - 1] = elevation;
-      azimuth_offset_map[laser_id - 1] = azimuth;
-//      std::cout << "laser_id=" << laser_id << ", elevation=" << elevation << ", azimuth=" << azimuth << std::endl;
+
+      try {
+        int laser_id = std::stoi(actual_tokens[0]);
+        float elevation = std::stof(actual_tokens[1]);
+        float azimuth = std::stof(actual_tokens[2]);
+        elev_angle_map[laser_id - 1] = elevation;
+        azimuth_offset_map[laser_id - 1] = azimuth;
+      } catch (const std::invalid_argument& ia) {
+        continue;
+      }
+
     }
-//    std::cout << "LoadFromString fin" << std::endl;
     return Status::OK;
   }
 
@@ -129,9 +126,7 @@ struct HesaiCalibrationConfiguration : CalibrationConfigurationBase
       return Status::CANNOT_SAVE_FILE;
     }
     ofs << calibration_string;
-//    std::cout << calibration_string << std::endl;
     ofs.close();
-
     return Status::OK;
   }
 };
@@ -169,21 +164,15 @@ struct HesaiCorrection
     delimiter = (buf[index] & 0xff) << 8 | ((buf[index + 1] & 0xff));
     versionMajor = buf[index + 2] & 0xff;
     versionMinor = buf[index + 3] & 0xff;
-    std::cout << "versionMajor=" << static_cast<int>(versionMajor) << std::endl;
-    std::cout << "versionMinor=" << static_cast<int>(versionMinor) << std::endl;
     channelNumber = buf[index + 4] & 0xff;
-    std::cout << "channelNumber=" << static_cast<int>(channelNumber) << std::endl;
     mirrorNumber = buf[index + 5] & 0xff;
-    std::cout << "mirrorNumber=" << static_cast<int>(mirrorNumber) << std::endl;
     frameNumber = buf[index + 6] & 0xff;
-    std::cout << "frameNumber=" << static_cast<int>(frameNumber) << std::endl;
     index += 7;
     for (uint8_t i = 0; i < 8; i++) {
       frameConfig[i] = buf[index] & 0xff;
       index++;
     }
     resolution = buf[index] & 0xff;
-    std::cout << "resolution=" << static_cast<int>(resolution) << std::endl;
     index++;
     switch (versionMinor) {
       case 5:
@@ -216,14 +205,9 @@ struct HesaiCorrection
           index++;
         }
 
-        // 230328 add
         for (uint8_t i = 0; i < mirrorNumber; i++) {
           startFrame[i] *= resolution;
           endFrame[i] *= resolution;
-          std::cout << "startFrame[" << static_cast<int>(i)
-                    << "]=" << static_cast<int>(startFrame[i]) << std::endl;
-          std::cout << "endFrame[" << static_cast<int>(i) << "]=" << static_cast<int>(endFrame[i])
-                    << std::endl;
         }
         for (uint8_t i = 0; i < channelNumber; i++) {
           azimuth[i] *= resolution;
@@ -261,20 +245,6 @@ struct HesaiCorrection
           index++;
         }
 
-        for (uint8_t i = 0; i < mirrorNumber; i++) {
-          std::cout << "startFrame[" << static_cast<int>(i)
-                    << "]=" << static_cast<int>(startFrame[i]) << std::endl;
-          std::cout << "endFrame[" << static_cast<int>(i) << "]=" << static_cast<int>(endFrame[i])
-                    << std::endl;
-          /*
-        startFrame[i] *= 2.56;
-        endFrame[i] *= 2.56;
-        std::cout << "startFrame[" << static_cast<int>(i) << "]=" << static_cast<int>(startFrame[i])
-        << std::endl; std::cout << "endFrame[" << static_cast<int>(i) << "]=" <<
-        static_cast<int>(endFrame[i]) << std::endl;
-        */
-        }
-
         break;
 
       default:
@@ -392,6 +362,8 @@ inline ReturnMode ReturnModeFromStringHesai(
   switch (sensor_model) {
     case SensorModel::HESAI_PANDARXT32M:
     case SensorModel::HESAI_PANDARAT128:
+    case SensorModel::HESAI_PANDAR128_E4X:
+    case SensorModel::HESAI_PANDARQT128:
       if (return_mode == "Last") return ReturnMode::LAST;
       if (return_mode == "Strongest") return ReturnMode::STRONGEST;
       if (return_mode == "LastStrongest") return ReturnMode::DUAL_LAST_STRONGEST;
@@ -424,6 +396,8 @@ inline ReturnMode ReturnModeFromIntHesai(const int return_mode, const SensorMode
   switch (sensor_model) {
     case SensorModel::HESAI_PANDARXT32M:
     case SensorModel::HESAI_PANDARAT128:
+    case SensorModel::HESAI_PANDAR128_E4X:
+    case SensorModel::HESAI_PANDARQT128:
       if (return_mode == 0) return ReturnMode::LAST;
       if (return_mode == 1) return ReturnMode::STRONGEST;
       if (return_mode == 2) return ReturnMode::DUAL_LAST_STRONGEST;
@@ -455,6 +429,8 @@ inline int IntFromReturnModeHesai(const ReturnMode return_mode, const SensorMode
   switch (sensor_model) {
     case SensorModel::HESAI_PANDARXT32M:
     case SensorModel::HESAI_PANDARAT128:
+    case SensorModel::HESAI_PANDAR128_E4X:
+    case SensorModel::HESAI_PANDARQT128:
       if (return_mode == ReturnMode::LAST) return 0;
       if (return_mode == ReturnMode::STRONGEST) return 1;
       if (return_mode == ReturnMode::DUAL_LAST_STRONGEST

nebula_common/include/nebula_common/nebula_common.hpp

@@ -2,7 +2,7 @@
 #define NEBULA_COMMON_H
 
 #include <nebula_common/point_types.hpp>
-
+#include <boost/tokenizer.hpp>
 #include <map>
 #include <ostream>
 #include <string>
@@ -341,6 +341,19 @@ enum class datatype {
   FLOAT64 = 8
 };
 
+enum class PtpProfile {
+  IEEE_1588v2 = 0,
+  IEEE_802_1AS,
+  IEEE_802_1AS_AUTO,
+  PROFILE_UNKNOWN
+};
+
+enum class PtpTransportType {
+  UDP_IP = 0,
+  L2,
+  UNKNOWN_TRANSPORT
+};
+
 /// @brief not used?
 struct PointField
 {
@@ -556,6 +569,80 @@ inline ReturnMode ReturnModeFromString(const std::string & return_mode)
   return ReturnMode::UNKNOWN;
 }
 
+/// @brief Converts String to PTP Profile
+/// @param ptp_profile Profile as String
+/// @return Corresponding PtpProfile
+inline PtpProfile PtpProfileFromString(const std::string & ptp_profile)
+{
+  // Hesai
+  auto tmp_str = ptp_profile;
+  std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(),
+                 [](unsigned char c){ return std::tolower(c); });
+  if (tmp_str == "1588v2") return PtpProfile::IEEE_1588v2;
+  if (tmp_str == "802.1as") return PtpProfile::IEEE_802_1AS;
+  if (tmp_str == "automotive") return PtpProfile::IEEE_802_1AS_AUTO;
+
+  return PtpProfile::PROFILE_UNKNOWN;
+}
+
+/// @brief Convert PtpProfile enum to string (Overloading the << operator)
+/// @param os
+/// @param arg
+/// @return stream
+inline std::ostream & operator<<(std::ostream & os, nebula::drivers::PtpProfile const & arg)
+{
+  switch (arg) {
+    case PtpProfile::IEEE_1588v2:
+      os << "IEEE_1588v2";
+      break;
+    case PtpProfile::IEEE_802_1AS:
+      os << "IEEE_802.1AS";
+      break;
+    case PtpProfile::IEEE_802_1AS_AUTO:
+      os << "IEEE_802.1AS Automotive";
+      break;
+    case PtpProfile::PROFILE_UNKNOWN:
+      os << "UNKNOWN";
+      break;
+  }
+  return os;
+}
+
+/// @brief Converts String to PTP TransportType
+/// @param transport_type Transport as String
+/// @return Corresponding PtpTransportType
+inline PtpTransportType PtpTransportTypeFromString(const std::string & transport_type)
+{
+  // Hesai
+  auto tmp_str = transport_type;
+  std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(),
+                 [](unsigned char c){ return std::tolower(c); });
+  if (tmp_str == "udp") return PtpTransportType::UDP_IP;
+  if (tmp_str == "l2") return PtpTransportType::L2;
+
+  return PtpTransportType::UNKNOWN_TRANSPORT;
+}
+
+/// @brief Convert PtpTransportType enum to string (Overloading the << operator)
+/// @param os
+/// @param arg
+/// @return stream
+inline std::ostream & operator<<(std::ostream & os, nebula::drivers::PtpTransportType const & arg)
+{
+  switch (arg) {
+    case PtpTransportType::UDP_IP:
+      os << "UDP/IP";
+      break;
+    case PtpTransportType::L2:
+      os << "L2";
+      break;
+    case PtpTransportType::UNKNOWN_TRANSPORT:
+      os << "UNKNOWN";
+      break;
+  }
+  return os;
+}
+
 [[maybe_unused]] pcl::PointCloud<PointXYZIR>::Ptr convertPointXYZIRADTToPointXYZIR(
   const pcl::PointCloud<PointXYZIRADT>::ConstPtr & input_pointcloud);
 

nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp

@@ -77,9 +77,6 @@ const uint16_t PANDAR128_E4X_EXTENDED_PACKET_SIZE = 1117;
 
 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)
@@ -116,7 +113,7 @@ class HesaiHwInterface : NebulaHwInterfaceBase
   std::timed_mutex tms_;
   int tms_fail_cnt = 0;
   int tms_fail_cnt_max = 3;
-  bool wl = true;
+  bool wl = false;
   bool is_solid_state = false;
   int target_model_no;