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

* fix(hesai_decoder): print config instead of config address

* refactor(nebula_ros): combine Hesai wrapper nodes into one

This is step one of the single node refactoring of Nebula.
In this step, only the three Hesai wrapper nodes were combined into one, and no optimizations have been done that are made possible by this refactoring.

The next step is to do those optimizations (e.g. get rid of unnecessary pointcloud copying).

* refactor: remove pandarScan pub/sub, decode one packet at a time

* reword later

* temporary progress

* fix(hesai_ros_wrapper): remove changes wrongly merged during rebase

* fix(hesai_ros_wrapper): increase packet buffering to stop internal packet loss

* feat(nebula_common): add instrumentation tools

* feat(hesai_ros_wrapper): instrument code on critical path

* disable instrumentation

* feat(hesai): move received buffer into ros message instead of copy

* feat(hesai_ros_wrapper: add thread-safe queue between udp receiver and decoder, decode in separate thread

* feat(nebula_common): more instrumentation tools

* fix: update link to transport_drivers fork

* update GitHub PR view

* refactor(mt_queue): make variables more readable

* perf(hesai_ros_wrapper): update queue capacity to alleviate packet drops on ECU

* chore(hesai_ros_wrapper): remove unused pub/sub

* refactor(hesai_ros_wrapper): clean up control flow, member variables

* fix(hesai_hw_interface): add error handling

* check PTC command error codes, throw exception if necessary
* perform size checks before parsing responses
* emit errors on too-large payload size

* fix(hesai_hw_monitor_ros_wrapper): fixed wrong range given for S/N copy

* fix(hesai): print uint8, uint16 as numbers

* feat(hesai_ros_wrapper): publish/subscribe to legacy pandar_packets on demand

* change launch file back to single-threaded container

* attempt to make queue contention less bad

* chore(hesai_ros_wrapper): reduce logging output

* feat(hesai_ros_wrapper): print warning when connected to HW and receiving /pandar_packets

* feat(nebula_launch.py): throw error when trying to launch Hesai sensor and refer to hesai_launch_all_hw.xml

* refactor(nebula_ros): split single wrapper file into 3 sub-wrappers

* chore: update cspell ignore

* chore(velodyne_calibration_decoder): fix spelling

* chore(nebula_common): remove debug code

* chore(velodyne_calibration_decoder): fix spelling

* fix(hesai_decoder):  initialize last_phase to prevent empty pointcloud published on startup

* fix(nebula_tests): make Hesai tests compile again

* fix(nebula_examples): make Hesai examples compile again

* chore(velodyne_calibration_decoder): fix spelling once and for all

* fix(nebula_examples): fix test failure (remove ament_lint_common)

* fix(hesai_hw_interface): add missing check for PTC error, make error type more readable

* refactor(hesai): re-introduce parameter update mechanism

* feat(hesai_ros): add watchdog timer for pointcloud output

* fix(hesai): change to possibly more accurate high_resolution_clock

* fix(hesai): fix crash on QT128

* refactor(hesai_hw_interface): refactor repeated error handling code

* fix(hesai_launch_all_hw.xml): set valid RPM when AT128 is selected

* refactor(hesai_decoder): remove redundant arguments for correction/calibration

* refactor(nebula_ros): move mt_queue to common

* chore(nebula_examples): change output rosbag format to NebulaPackets, clean up code

* chore(hesai/decoder_wrapper): clarify watchdog behavior in decoder

* chore(hesai/hw_monitor_wrapper): fix typo in diagnostics name

* chore(hesai_ros_decoder_test): fix unclear naming in console output

* chore: run pre-commit and implement suggested fixes (not including copyright yet)

* fix(expected.hpp): revert explicit constructors

* chore: remove erroneously added node_modules folder

* chore(gitconfig): exclude node_modules from git

* fix(hesai/decoder_wrapper): store downloaded sensor calibration without colliding filenames

* chore(nebula_examples,nebula_tests): remove more dangling comments and improve formatting

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