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

.cspell.json

@@ -13,6 +13,7 @@
     "DHAVE",
     "Difop",
     "extrinsics",
+    "fout",
     "gprmc",
     "gptp",
     "Helios",
@@ -32,6 +33,8 @@
     "PANDARQT",
     "PANDARXT",
     "Pdelay",
+    "Piyush",
+    "piyushk",
     "QT",
     "rclcpp",
     "schedutil",
@@ -46,8 +49,6 @@
     "Vdat",
     "XT",
     "XTM",
-    "piyushk",
-    "Piyush",
-    "fout"
+    "yukkysaito"
   ]
-}
+}

nebula_common/include/nebula_common/hesai/hesai_common.hpp

@@ -8,7 +8,10 @@
 #include <cmath>
 #include <fstream>
 #include <iostream>
+#include <map>
 #include <sstream>
+#include <string>
+#include <vector>
 namespace nebula
 {
 namespace drivers
@@ -47,7 +50,8 @@ 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;
+  virtual nebula::Status SaveToFileFromBytes(
+    const std::string & calibration_file, const std::vector<uint8_t> & buf) = 0;
 };
 
 /// @brief struct for Hesai calibration configuration
@@ -63,12 +67,13 @@ struct HesaiCalibrationConfiguration : public HesaiCalibrationConfigurationBase
       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) {
+  nebula::Status LoadFromBytes(const std::vector<uint8_t> & buf)
+  {
     std::string calibration_string = std::string(buf.begin(), buf.end());
     return LoadFromString(calibration_string);
   }
@@ -82,14 +87,12 @@ struct HesaiCalibrationConfiguration : public HesaiCalibrationConfigurationBase
     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;
       }
@@ -100,10 +103,9 @@ struct HesaiCalibrationConfiguration : public HesaiCalibrationConfigurationBase
         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;
   }
@@ -129,7 +131,9 @@ struct HesaiCalibrationConfiguration : public HesaiCalibrationConfigurationBase
     return Status::OK;
   }
 
-  nebula::Status SaveToFileFromBytes(const std::string & calibration_file, const std::vector<uint8_t> & buf) override {
+  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);
   }
@@ -138,7 +142,8 @@ struct HesaiCalibrationConfiguration : public HesaiCalibrationConfigurationBase
   /// @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) {
@@ -176,9 +181,8 @@ struct HesaiCorrection : public HesaiCalibrationConfigurationBase
   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;
@@ -285,7 +289,7 @@ struct HesaiCorrection : public HesaiCalibrationConfigurationBase
     //    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);
     }
     LoadFromBytes(buf);
@@ -298,7 +302,8 @@ struct HesaiCorrection : public HesaiCalibrationConfigurationBase
   /// @param correction_file path
   /// @param buf correction binary
   /// @return Resulting status
-  inline nebula::Status SaveToFileFromBytes(const std::string & correction_file, const std::vector<uint8_t> & buf) override
+  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);
@@ -308,21 +313,20 @@ struct HesaiCorrection : public HesaiCalibrationConfigurationBase
     }
     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;
   }
 
@@ -452,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,
-  UNKNOWN_PROFILE
-};
+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,8 +608,9 @@ 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;
@@ -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;
 

nebula_common/include/nebula_common/util/expected.hpp

@@ -1,23 +1,22 @@
 #pragma once
 
-#include <variant>
-#include <string>
 #include <exception>
+#include <string>
+#include <variant>
 
 namespace nebula
 {
 namespace util
 {
 
-struct bad_expected_access : public std::exception {
-  bad_expected_access(const std::string & msg) : std::exception(), msg_(msg) {}
+struct bad_expected_access : public std::exception
+{
+  explicit bad_expected_access(const std::string & msg) : std::exception(), msg_(msg) {}
 
-  const char* what() const noexcept override {
-    return msg_.c_str();
-  }
+  const char * what() const noexcept override { return msg_.c_str(); }
 
 private:
-  const std::string msg_; 
+  const std::string msg_;
 };
 
 /// @brief A poor man's backport of C++23's std::expected.
@@ -39,14 +38,16 @@ struct expected
 
   /// @brief Retrieve the value, or throw `bad_expected_access` if an error is contained.
   /// @return The value of type `T`
-  T value() {
+  T value()
+  {
     if (!has_value()) {
       throw bad_expected_access("value() called but containing error");
     }
-    return std::get<T>(value_); 
+    return std::get<T>(value_);
   }
 
-  /// @brief Return the contained value, or, if an error is contained, return the given `default_` instead.
+  /// @brief Return the contained value, or, if an error is contained, return the given `default_`
+  /// instead.
   /// @return The contained value, if any, else `default_`
   T value_or(const T & default_)
   {
@@ -57,34 +58,38 @@ struct expected
   /// @brief If the instance has a value, return the value, else throw std::runtime_error(error_msg)
   /// @param error_msg The message to be included in the thrown exception
   /// @return The value
-  T value_or_throw(const std::string & error_msg) {
+  T value_or_throw(const std::string & error_msg)
+  {
     if (has_value()) return value();
     throw std::runtime_error(error_msg);
   }
 
   /// @brief Retrieve the error, or throw `bad_expected_access` if a value is contained.
   /// @return The error of type `E`
-  E error() {
+  E error()
+  {
     if (has_value()) {
       throw bad_expected_access("error() called but containing value");
     }
-    return std::get<E>(value_); 
+    return std::get<E>(value_);
   }
 
-  /// @brief Return the contained error, or, if a value is contained, return the given `default_` instead.
+  /// @brief Return the contained error, or, if a value is contained, return the given `default_`
+  /// instead.
   /// @return The contained error, if any, else `default_`
-  E error_or(const E & default_) { 
+  E error_or(const E & default_)
+  {
     if (!has_value()) return error();
-    return default_; 
-    }
+    return default_;
+  }
 
-  expected(const T & value) { value_ = value; }
+  explicit expected(const T & value) { value_ = value; }
 
-  expected(const E & error) { value_ = error; }
+  explicit expected(const E & error) { value_ = error; }
 
 private:
   std::variant<T, E> value_;
 };
 
 }  // namespace util
-}  // namespace nebula
+}  // namespace nebula

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp

@@ -44,8 +44,9 @@ class AngleCorrector
   /// @param sync_azimuth The azimuth set in the sensor configuration, for which the
   /// timestamp is aligned to the full second
   /// @return true if the current azimuth is in a different scan than the last one, false otherwise
-  virtual bool hasScanned(uint32_t current_azimuth, uint32_t last_azimuth, uint32_t sync_azimuth) = 0;
+  virtual bool hasScanned(
+    uint32_t current_azimuth, uint32_t last_azimuth, uint32_t sync_azimuth) = 0;
 };
 
 }  // namespace drivers
-}  // namespace nebula
+}  // namespace nebula

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_calibration_based.hpp

@@ -4,6 +4,7 @@
 #include "nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp"
 
 #include <cstdint>
+#include <memory>
 
 namespace nebula
 {
@@ -79,10 +80,10 @@ class AngleCorrectorCalibrationBased : public AngleCorrector<HesaiCalibrationCon
       (MAX_AZIMUTH_LEN + current_azimuth - sync_azimuth) % MAX_AZIMUTH_LEN;
     uint32_t last_diff_from_sync =
       (MAX_AZIMUTH_LEN + last_azimuth - sync_azimuth) % MAX_AZIMUTH_LEN;
-      
+
     return current_diff_from_sync < last_diff_from_sync;
   }
 };
 
 }  // namespace drivers
-}  // namespace nebula
+}  // namespace nebula