feat: add robosense support

.cspell.json

@@ -31,6 +31,13 @@
     "Vccint",
     "XT",
     "XTM",
-    "DHAVE"
+    "DHAVE",
+    "Bpearl",
+    "Helios",
+    "Msop",
+    "Difop",
+    "gptp",
+    "Idat",
+    "Vdat"
   ]
 }

nebula_common/include/nebula_common/nebula_common.hpp

@@ -323,6 +323,10 @@ enum class SensorModel {
   VELODYNE_VLP32MR,
   VELODYNE_HDL32,
   VELODYNE_VLP16,
+  ROBOSENSE_HELIOS,
+  ROBOSENSE_BPEARL,
+  ROBOSENSE_BPEARL_V3,
+  ROBOSENSE_BPEARL_V4,
 };
 
 /// @brief not used?
@@ -401,6 +405,18 @@ inline std::ostream & operator<<(std::ostream & os, nebula::drivers::SensorModel
     case SensorModel::VELODYNE_VLP16:
       os << "VLP16";
       break;
+    case SensorModel::ROBOSENSE_HELIOS:
+      os << "HELIOS";
+      break;
+    case SensorModel::ROBOSENSE_BPEARL:
+      os << "BPEARL";
+      break;
+    case SensorModel::ROBOSENSE_BPEARL_V3:
+      os << "BPEARL V3.0";
+      break;
+    case SensorModel::ROBOSENSE_BPEARL_V4:
+      os << "BPEARL V4.0";
+      break;
     case SensorModel::UNKNOWN:
       os << "Sensor Unknown";
       break;
@@ -424,6 +440,7 @@ struct SensorConfigurationBase
   double max_range;
   bool remove_nans;  /// todo: consider changing to only_finite
   std::vector<PointField> fields;
+  bool use_sensor_time{false};
 };
 
 /// @brief Convert SensorConfigurationBase to string (Overloading the << operator)
@@ -436,7 +453,8 @@ inline std::ostream & operator<<(
   os << "SensorModel: " << arg.sensor_model << ", ReturnMode: " << arg.return_mode
      << ", HostIP: " << arg.host_ip << ", SensorIP: " << arg.sensor_ip
      << ", FrameID: " << arg.frame_id << ", DataPort: " << arg.data_port
-     << ", Frequency: " << arg.frequency_ms << ", MTU: " << arg.packet_mtu_size;
+     << ", Frequency: " << arg.frequency_ms << ", MTU: " << arg.packet_mtu_size
+     << ", Use sensor time: " << arg.use_sensor_time;
   return os;
 }
 
@@ -468,9 +486,63 @@ inline SensorModel SensorModelFromString(const std::string & sensor_model)
   if (sensor_model == "VLP32MR") return SensorModel::VELODYNE_VLP32MR;
   if (sensor_model == "HDL32") return SensorModel::VELODYNE_HDL32;
   if (sensor_model == "VLP16") return SensorModel::VELODYNE_VLP16;
+  // Robosense
+  if (sensor_model == "Helios") return SensorModel::ROBOSENSE_HELIOS;
+  if (sensor_model == "Bpearl") return SensorModel::ROBOSENSE_BPEARL;
+  if (sensor_model == "Bpearl_V3") return SensorModel::ROBOSENSE_BPEARL_V3;
+  if (sensor_model == "Bpearl_V4") return SensorModel::ROBOSENSE_BPEARL_V4;
   return SensorModel::UNKNOWN;
 }
 
+inline std::string SensorModelToString(const SensorModel & sensor_model)
+{
+  switch (sensor_model) {
+    // Hesai
+    case SensorModel::HESAI_PANDAR64:
+      return "Pandar64";
+    case SensorModel::HESAI_PANDAR40P:
+      return "Pandar40P";
+    case SensorModel::HESAI_PANDAR40M:
+      return "Pandar40M";
+    case SensorModel::HESAI_PANDARXT32:
+      return "PandarXT32";
+    case SensorModel::HESAI_PANDARXT32M:
+      return "PandarXT32M";
+    case SensorModel::HESAI_PANDARAT128:
+      return "PandarAT128";
+    case SensorModel::HESAI_PANDARQT64:
+      return "PandarQT64";
+    case SensorModel::HESAI_PANDARQT128:
+      return "PandarQT128";
+    case SensorModel::HESAI_PANDAR128_E4X:
+      return "Pandar128E4X";
+    // Velodyne
+    case SensorModel::VELODYNE_VLS128:
+      return "VLS128";
+    case SensorModel::VELODYNE_HDL64:
+      return "HDL64";
+    case SensorModel::VELODYNE_VLP32:
+      return "VLP32";
+    case SensorModel::VELODYNE_VLP32MR:
+      return "VLP32MR";
+    case SensorModel::VELODYNE_HDL32:
+      return "HDL32";
+    case SensorModel::VELODYNE_VLP16:
+      return "VLP16";
+    // Robosense
+    case SensorModel::ROBOSENSE_HELIOS:
+      return "Helios";
+    case SensorModel::ROBOSENSE_BPEARL:
+      return "Bpearl";
+    case SensorModel::ROBOSENSE_BPEARL_V3:
+      return "Bpearl_V3";
+    case SensorModel::ROBOSENSE_BPEARL_V4:
+      return "Bpearl_V4";
+    default:
+      return "UNKNOWN";
+  }
+}
+
 /// @brief Convert return mode name to ReturnMode enum
 /// @param return_mode Return mode name (Upper and lower case letters must match)
 /// @return Corresponding ReturnMode
@@ -496,12 +568,18 @@ pcl::PointCloud<PointXYZIRADT>::Ptr convertPointXYZIRCAEDTToPointXYZIRADT(
 /// @brief Converts degrees to radians
 /// @param radians
 /// @return degrees
-static inline float deg2rad(double degrees) { return degrees * M_PI / 180.0; }
+static inline float deg2rad(double degrees)
+{
+  return degrees * M_PI / 180.0;
+}
 
 /// @brief Converts radians to degrees
 /// @param radians
 /// @return degrees
-static inline float rad2deg(double radians) { return radians * 180.0 / M_PI; }
+static inline float rad2deg(double radians)
+{
+  return radians * 180.0 / M_PI;
+}
 }  // namespace drivers
 }  // namespace nebula
 

nebula_common/include/nebula_common/robosense/robosense_common.hpp

@@ -0,0 +1,202 @@
+#pragma once
+
+#include "nebula_common/nebula_common.hpp"
+#include "nebula_common/nebula_status.hpp"
+
+#include <bitset>
+#include <cmath>
+#include <fstream>
+#include <iostream>
+#include <optional>
+#include <sstream>
+#include <vector>
+
+namespace nebula
+{
+namespace drivers
+{
+
+// Flag for detecting Bpearl version
+constexpr uint8_t BPEARL_V4_FLAG = 0x04;
+
+/// @brief struct for Robosense sensor configuration
+struct RobosenseSensorConfiguration : SensorConfigurationBase
+{
+  uint16_t gnss_port{};  // difop
+  double scan_phase{};   // start/end angle
+  double dual_return_distance_threshold{};
+  uint16_t rotation_speed;
+  uint16_t cloud_min_angle;
+  uint16_t cloud_max_angle;
+};
+
+/// @brief Convert RobosenseSensorConfiguration to string (Overloading the << operator)
+/// @param os
+/// @param arg
+/// @return stream
+inline std::ostream & operator<<(std::ostream & os, RobosenseSensorConfiguration const & arg)
+{
+  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;
+  return os;
+}
+
+/// @brief Convert return mode name to ReturnMode enum (Robosense-specific ReturnModeFromString)
+/// @param return_mode Return mode name (Upper and lower case letters must match)
+/// @return Corresponding ReturnMode
+inline ReturnMode ReturnModeFromStringRobosense(const std::string & return_mode)
+{
+  if (return_mode == "Dual") return ReturnMode::DUAL;
+  if (return_mode == "Strongest") return ReturnMode::SINGLE_STRONGEST;
+  if (return_mode == "Last") return ReturnMode::SINGLE_LAST;
+  if (return_mode == "First") return ReturnMode::SINGLE_FIRST;
+
+  return ReturnMode::UNKNOWN;
+}
+
+size_t GetChannelSize(const SensorModel & model)
+{
+  switch (model) {
+    case SensorModel::ROBOSENSE_BPEARL_V3:
+      return 32;
+    case SensorModel::ROBOSENSE_HELIOS:
+      return 32;
+  }
+}
+
+struct ChannelCorrection
+{
+  float azimuth{NAN};
+  float elevation{NAN};
+  uint16_t channel{};
+
+  [[nodiscard]] bool has_value() const { return !std::isnan(azimuth) && !std::isnan(elevation); }
+};
+
+/// @brief struct for Robosense calibration configuration
+struct RobosenseCalibrationConfiguration : CalibrationConfigurationBase
+{
+  std::vector<ChannelCorrection> calibration;
+
+  void SetChannelSize(const size_t channel_num) { calibration.resize(channel_num); }
+
+  template <typename stream_t>
+  inline nebula::Status LoadFromStream(stream_t & stream)
+  {
+    std::string header;
+    std::getline(stream, header);
+
+    char sep;
+    int laser_id;
+    float elevation;
+    float azimuth;
+    Status load_status = Status::OK;
+    for (size_t i = 0; i < calibration.size(); ++i) {
+      stream >> laser_id >> sep >> elevation >> sep >> azimuth;
+
+      if (laser_id <= 0 || laser_id > calibration.size()) {
+        std::cout << "Invalid laser id: " << laser_id << std::endl;
+        load_status = Status::INVALID_CALIBRATION_FILE;
+      }
+      if (std::isnan(elevation) || std::isnan(azimuth)) {
+        std::cout << "Invalid calibration data" << laser_id << "," << elevation << "," << azimuth
+                  << std::endl;
+        load_status = Status::INVALID_CALIBRATION_FILE;
+      }
+      if (
+        calibration[laser_id - 1].has_value() && calibration[laser_id - 1].elevation != elevation &&
+        calibration[laser_id - 1].azimuth != azimuth) {
+        std::cout << "Duplicate calibration data for laser id: " << laser_id << std::endl;
+        load_status = Status::INVALID_CALIBRATION_FILE;
+      }
+
+      ChannelCorrection correction{azimuth, elevation};
+      calibration[laser_id - 1] = correction;
+    }
+
+    for (auto & calib : calibration) {
+      if (!calib.has_value()) {
+        std::cout << calib.elevation << "," << calib.azimuth << std::endl;
+        std::cout << "Missing calibration data" << std::endl;
+        load_status = Status::INVALID_CALIBRATION_FILE;
+      }
+    }
+
+    if (load_status != Status::OK) {
+      for (auto & correction : calibration) {
+        correction.elevation = NAN;
+        correction.azimuth = NAN;
+      }
+    }
+
+    return load_status;
+  }
+
+  inline nebula::Status LoadFromFile(const std::string & calibration_file)
+  {
+    std::ifstream ifs(calibration_file);
+    if (!ifs) {
+      return Status::INVALID_CALIBRATION_FILE;
+    }
+
+    const auto status = LoadFromStream(ifs);
+    ifs.close();
+    return status;
+  }
+
+  /// @brief Loading calibration data (not used)
+  /// @param calibration_content
+  /// @return Resulting status
+  inline nebula::Status LoadFromString(const std::string & calibration_content)
+  {
+    std::stringstream ss;
+    ss << calibration_content;
+
+    const auto status = LoadFromStream(ss);
+    return status;
+  }
+
+  //  inline nebula::Status LoadFromDifop(const std::string & calibration_file)
+
+  /// @brief Saving calibration data (not used)
+  /// @param calibration_file
+  /// @return Resulting status
+  inline nebula::Status SaveFile(const std::string & calibration_file)
+  {
+    std::ofstream ofs(calibration_file);
+    if (!ofs) {
+      return Status::CANNOT_SAVE_FILE;
+    }
+    ofs << "Laser id,Elevation,Azimuth" << std::endl;
+
+    for (size_t i = 0; i < calibration.size(); ++i) {
+      auto laser_id = i + 1;
+      float elevation = calibration[i].elevation;
+      float azimuth = calibration[i].azimuth;
+      ofs << laser_id << "," << elevation << "," << azimuth << std::endl;
+    }
+
+    ofs.close();
+    return Status::OK;
+  }
+
+  [[nodiscard]] inline ChannelCorrection GetCorrection(const size_t channel_id) const
+  {
+    return calibration[channel_id];
+  }
+
+  void CreateCorrectedChannels()
+  {
+    for(auto& correction : calibration) {
+      uint16_t channel = 0;
+      for(const auto& compare:calibration) {
+        if(compare.elevation < correction.elevation) ++channel;
+      }
+      correction.channel = channel;
+    }
+  }
+};
+
+}  // namespace drivers
+}  // namespace nebula

nebula_decoders/CMakeLists.txt

@@ -18,6 +18,7 @@ find_package(PCL REQUIRED)
 find_package(pcl_conversions REQUIRED)
 find_package(yaml-cpp REQUIRED)
 find_package(nebula_common REQUIRED)
+find_package(robosense_msgs REQUIRED)
 
 include_directories(
         include
@@ -41,6 +42,15 @@ ament_auto_add_library(nebula_decoders_velodyne SHARED
         src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp
         )
 
+# Robosense
+ament_auto_add_library(nebula_decoders_robosense SHARED
+        src/nebula_decoders_robosense/robosense_driver.cpp
+        )
+
+ament_auto_add_library(nebula_decoders_robosense_info SHARED
+        src/nebula_decoders_robosense/robosense_info_driver.cpp
+        )
+
 if(BUILD_TESTING)
     find_package(ament_lint_auto REQUIRED)
     ament_lint_auto_find_test_dependencies()