refactor(hesai)!: combine Hesai ROS wrappers into a single node

.cspell.json

@@ -6,47 +6,49 @@
     "adctp",
     "Adctp",
     "AT",
+    "autosar",
     "block_id",
+    "Bpearl",
+    "calib",
+    "DHAVE",
+    "Difop",
     "extrinsics",
+    "fout",
     "gprmc",
+    "gptp",
+    "Helios",
     "Hesai",
+    "Idat",
+    "ipaddr",
+    "manc",
     "memcpy",
     "mkdoxy",
+    "Msop",
     "nohup",
     "nproc",
+    "ntoa",
     "pandar",
     "PANDAR",
     "PANDARAT",
     "PANDARQT",
     "PANDARXT",
     "Pdelay",
+    "Piyush",
+    "piyushk",
     "QT",
     "rclcpp",
     "schedutil",
+    "srvs",
     "STD_COUT",
     "stds",
     "struct",
     "structs",
     "UDP_SEQ",
     "vccint",
     "Vccint",
+    "Vdat",
     "XT",
     "XTM",
-    "DHAVE",
-    "Bpearl",
-    "Helios",
-    "Msop",
-    "Difop",
-    "gptp",
-    "Idat",
-    "Vdat",
-    "autosar",
-    "srvs",
-    "manc",
-    "ipaddr",
-    "ntoa",
-    "piyushk",
-    "Piyush",
-    "fout"
+    "yukkysaito"
   ]
 }

.gitignore

@@ -29,3 +29,6 @@ site/
 
 # qcreator stuff
 CMakeLists.txt.user
+
+# pre-commit
+node_modules/

build_depends.repos

@@ -2,5 +2,5 @@ repositories:
   # TCP version of transport drivers
   transport_drivers:
     type: git
-    url: https://github.com/MapIV/transport_drivers
-    version: boost
+    url: https://github.com/mojomex/transport_drivers
+    version: mutable-buffer-in-udp-callback

nebula_common/CMakeLists.txt

@@ -40,6 +40,8 @@ ament_auto_add_library(nebula_common SHARED
         src/velodyne/velodyne_calibration_decoder.cpp
         )
 
+target_link_libraries(nebula_common yaml-cpp)
+
 ament_auto_package()
 
 # Set ROS_DISTRO macros

nebula_common/include/nebula_common/hesai/hesai_common.hpp

@@ -8,13 +8,16 @@
 #include <cmath>
 #include <fstream>
 #include <iostream>
+#include <map>
 #include <sstream>
+#include <string>
+#include <vector>
 namespace nebula
 {
 namespace drivers
 {
 /// @brief struct for Hesai sensor configuration
-struct HesaiSensorConfiguration : LidarConfigurationBase
+struct HesaiSensorConfiguration : public LidarConfigurationBase
 {
   uint16_t gnss_port{};
   double scan_phase{};
@@ -43,24 +46,38 @@ inline std::ostream & operator<<(std::ostream & os, HesaiSensorConfiguration con
   return os;
 }
 
+struct HesaiCalibrationConfigurationBase : public CalibrationConfigurationBase
+{
+  virtual nebula::Status LoadFromBytes(const std::vector<uint8_t> & buf) = 0;
+  virtual nebula::Status LoadFromFile(const std::string & calibration_file) = 0;
+  virtual nebula::Status SaveToFileFromBytes(
+    const std::string & calibration_file, const std::vector<uint8_t> & buf) = 0;
+};
+
 /// @brief struct for Hesai calibration configuration
-struct HesaiCalibrationConfiguration : CalibrationConfigurationBase
+struct HesaiCalibrationConfiguration : public HesaiCalibrationConfigurationBase
 {
   std::map<size_t, float> elev_angle_map;
   std::map<size_t, float> azimuth_offset_map;
 
-  inline nebula::Status LoadFromFile(const std::string & calibration_file)
+  inline nebula::Status LoadFromFile(const std::string & calibration_file) override
   {
     std::ifstream ifs(calibration_file);
     if (!ifs) {
       return Status::INVALID_CALIBRATION_FILE;
     }
     std::ostringstream ss;
-    ss << ifs.rdbuf(); // reading data
+    ss << ifs.rdbuf();  // reading data
     ifs.close();
     return LoadFromString(ss.str());
   }
 
+  nebula::Status LoadFromBytes(const std::vector<uint8_t> & buf)
+  {
+    std::string calibration_string = std::string(buf.begin(), buf.end());
+    return LoadFromString(calibration_string);
+  }
+
   /// @brief Loading calibration data
   /// @param calibration_content
   /// @return Resulting status
@@ -70,14 +87,12 @@ struct HesaiCalibrationConfiguration : CalibrationConfigurationBase
     ss << calibration_content;
     std::string line;
     constexpr size_t expected_cols = 3;
-    while(std::getline(ss, line)) {
+    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
-        ) {
+      if (actual_tokens.size() < expected_cols || actual_tokens.size() > expected_cols) {
         std::cerr << "Ignoring line with unexpected data:" << line << std::endl;
         continue;
       }
@@ -88,18 +103,17 @@ struct HesaiCalibrationConfiguration : CalibrationConfigurationBase
         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) {
+      } catch (const std::invalid_argument & ia) {
         continue;
       }
-
     }
     return Status::OK;
   }
 
   /// @brief Saving calibration data (not used)
   /// @param calibration_file
   /// @return Resulting status
-  inline nebula::Status SaveFile(const std::string & calibration_file)
+  inline nebula::Status SaveToFile(const std::string & calibration_file)
   {
     std::ofstream ofs(calibration_file);
     if (!ofs) {
@@ -117,11 +131,19 @@ struct HesaiCalibrationConfiguration : CalibrationConfigurationBase
     return Status::OK;
   }
 
+  nebula::Status SaveToFileFromBytes(
+    const std::string & calibration_file, const std::vector<uint8_t> & buf) override
+  {
+    std::string calibration_string = std::string(buf.begin(), buf.end());
+    return SaveFileFromString(calibration_file, calibration_string);
+  }
+
   /// @brief Saving calibration data from string
   /// @param calibration_file path
   /// @param calibration_string calibration string
   /// @return Resulting status
-  inline nebula::Status SaveFileFromString(const std::string & calibration_file, const std::string & calibration_string)
+  inline nebula::Status SaveFileFromString(
+    const std::string & calibration_file, const std::string & calibration_string)
   {
     std::ofstream ofs(calibration_file);
     if (!ofs) {
@@ -134,7 +156,7 @@ struct HesaiCalibrationConfiguration : CalibrationConfigurationBase
 };
 
 /// @brief struct for Hesai correction configuration (for AT)
-struct HesaiCorrection
+struct HesaiCorrection : public HesaiCalibrationConfigurationBase
 {
   uint16_t delimiter;
   uint8_t versionMajor;
@@ -156,12 +178,11 @@ struct HesaiCorrection
   /// @brief Load correction data from file
   /// @param buf Binary buffer
   /// @return Resulting status
-  inline nebula::Status LoadFromBinary(const std::vector<uint8_t> & buf)
+  inline nebula::Status LoadFromBytes(const std::vector<uint8_t> & buf) override
   {
     size_t index;
-    for (index = 0; index < buf.size()-1; index++) {
-      if(buf[index]==0xee && buf[index+1]==0xff)
-        break;
+    for (index = 0; index < buf.size() - 1; index++) {
+      if (buf[index] == 0xee && buf[index + 1] == 0xff) break;
     }
     delimiter = (buf[index] & 0xff) << 8 | ((buf[index + 1] & 0xff));
     versionMajor = buf[index + 2] & 0xff;
@@ -258,7 +279,7 @@ struct HesaiCorrection
   /// @brief Load correction data from file
   /// @param correction_file path
   /// @return Resulting status
-  inline nebula::Status LoadFromFile(const std::string & correction_file)
+  inline nebula::Status LoadFromFile(const std::string & correction_file) override
   {
     std::ifstream ifs(correction_file, std::ios::in | std::ios::binary);
     if (!ifs) {
@@ -268,10 +289,10 @@ struct HesaiCorrection
     //    int cnt = 0;
     while (!ifs.eof()) {
       unsigned char c;
-      ifs.read((char *)&c, sizeof(unsigned char));
+      ifs.read(reinterpret_cast<char *>(&c), sizeof(unsigned char));
       buf.emplace_back(c);
     }
-    LoadFromBinary(buf);
+    LoadFromBytes(buf);
 
     ifs.close();
     return Status::OK;
@@ -281,7 +302,8 @@ struct HesaiCorrection
   /// @param correction_file path
   /// @param buf correction binary
   /// @return Resulting status
-  inline nebula::Status SaveFileFromBinary(const std::string & correction_file, const std::vector<uint8_t> & buf)
+  inline nebula::Status SaveToFileFromBytes(
+    const std::string & correction_file, const std::vector<uint8_t> & buf) override
   {
     std::cerr << "Saving in:" << correction_file << "\n";
     std::ofstream ofs(correction_file, std::ios::trunc | std::ios::binary);
@@ -291,21 +313,20 @@ struct HesaiCorrection
     }
     std::cerr << "Writing start...." << buf.size() << "\n";
     bool sop_received = false;
-    for (const auto &byte : buf) {
+    for (const auto & byte : buf) {
       if (!sop_received) {
         if (byte == 0xEE) {
           std::cerr << "SOP received....\n";
           sop_received = true;
         }
       }
-      if(sop_received) {
+      if (sop_received) {
         ofs << byte;
       }
     }
     std::cerr << "Closing file\n";
     ofs.close();
-    if(sop_received)
-      return Status::OK;
+    if (sop_received) return Status::OK;
     return Status::INVALID_CALIBRATION_FILE;
   }
 
@@ -435,8 +456,8 @@ inline int IntFromReturnModeHesai(const ReturnMode return_mode, const SensorMode
     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
-      || return_mode == ReturnMode::DUAL) return 2;
+      if (return_mode == ReturnMode::DUAL_LAST_STRONGEST || return_mode == ReturnMode::DUAL)
+        return 2;
       if (return_mode == ReturnMode::FIRST) return 3;
       if (return_mode == ReturnMode::DUAL_LAST_FIRST) return 4;
       if (return_mode == ReturnMode::DUAL_FIRST_STRONGEST) return 5;

nebula_common/include/nebula_common/nebula_common.hpp

@@ -2,7 +2,10 @@
 #define NEBULA_COMMON_H
 
 #include <nebula_common/point_types.hpp>
+
 #include <boost/tokenizer.hpp>
+
+#include <algorithm>
 #include <map>
 #include <ostream>
 #include <string>
@@ -342,24 +345,11 @@ enum class datatype {
   FLOAT64 = 8
 };
 
-enum class PtpProfile {
-  IEEE_1588v2 = 0,
-  IEEE_802_1AS,
-  IEEE_802_1AS_AUTO,
-  PROFILE_UNKNOWN
-};
+enum class PtpProfile { IEEE_1588v2 = 0, IEEE_802_1AS, IEEE_802_1AS_AUTO, UNKNOWN_PROFILE };
 
-enum class PtpTransportType {
-  UDP_IP = 0,
-  L2,
-  UNKNOWN_TRANSPORT
-};
+enum class PtpTransportType { UDP_IP = 0, L2, UNKNOWN_TRANSPORT };
 
-enum class PtpSwitchType {
-  NON_TSN = 0,
-  TSN,
-  UNKNOWN_SWITCH
-};
+enum class PtpSwitchType { NON_TSN = 0, TSN, UNKNOWN_SWITCH };
 
 /// @brief not used?
 struct PointField
@@ -618,13 +608,14 @@ 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); });
+  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;
+  return PtpProfile::UNKNOWN_PROFILE;
 }
 
 /// @brief Convert PtpProfile enum to string (Overloading the << operator)
@@ -643,7 +634,7 @@ inline std::ostream & operator<<(std::ostream & os, nebula::drivers::PtpProfile
     case PtpProfile::IEEE_802_1AS_AUTO:
       os << "IEEE_802.1AS Automotive";
       break;
-    case PtpProfile::PROFILE_UNKNOWN:
+    case PtpProfile::UNKNOWN_PROFILE:
       os << "UNKNOWN";
       break;
   }
@@ -657,8 +648,9 @@ inline PtpTransportType PtpTransportTypeFromString(const std::string & transport
 {
   // Hesai
   auto tmp_str = transport_type;
-  std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(),
-                 [](unsigned char c){ return std::tolower(c); });
+  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;
 
@@ -692,8 +684,9 @@ inline PtpSwitchType PtpSwitchTypeFromString(const std::string & switch_type)
 {
   // Hesai
   auto tmp_str = switch_type;
-  std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(),
-                 [](unsigned char c){ return std::tolower(c); });
+  std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(), [](unsigned char c) {
+    return std::tolower(c);
+  });
   if (tmp_str == "tsn") return PtpSwitchType::TSN;
   if (tmp_str == "non_tsn") return PtpSwitchType::NON_TSN;