feat(seyond): add base files and directories

Signed-off-by: David Wong <[email protected]>

nebula_common/include/nebula_common/nebula_common.hpp

@@ -327,7 +327,9 @@ enum class SensorModel {
   ROBOSENSE_BPEARL,
   ROBOSENSE_BPEARL_V3,
   ROBOSENSE_BPEARL_V4,
-  CONTINENTAL_ARS548
+  CONTINENTAL_ARS548,
+  SEYOND_FALCON_KINETIC,
+  SEYOND_ROBIN_W
 };
 
 /// @brief not used?
@@ -440,6 +442,12 @@ inline std::ostream & operator<<(std::ostream & os, nebula::drivers::SensorModel
     case SensorModel::CONTINENTAL_ARS548:
       os << "ARS548";
       break;
+    case SensorModel::SEYOND_FALCON_KINETIC:
+      os << "SEYOND_FALCON_KINETIC";
+      break;
+    case SensorModel::SEYOND_ROBIN_W:
+      os << "SEYOND_ROBIN_W";
+      break;
     case SensorModel::UNKNOWN:
       os << "Sensor Unknown";
       break;
@@ -544,6 +552,9 @@ inline SensorModel SensorModelFromString(const std::string & sensor_model)
   if (sensor_model == "Bpearl_V3") return SensorModel::ROBOSENSE_BPEARL_V3;
   if (sensor_model == "Bpearl_V4") return SensorModel::ROBOSENSE_BPEARL_V4;
   if (sensor_model == "ARS548") return SensorModel::CONTINENTAL_ARS548;
+  // Seyond
+  if (sensor_model == "Falcon") return SensorModel::SEYOND_FALCON_KINETIC;
+  if (sensor_model == "RobinW") return SensorModel::SEYOND_ROBIN_W;
   return SensorModel::UNKNOWN;
 }
 
@@ -593,6 +604,10 @@ inline std::string SensorModelToString(const SensorModel & sensor_model)
       return "Bpearl_V4";
     case SensorModel::CONTINENTAL_ARS548:
       return "ARS548";
+    case SensorModel::SEYOND_FALCON_KINETIC:
+      return "Falcon";
+    case SensorModel::SEYOND_ROBIN_W:
+      return "RoninW";
     default:
       return "UNKNOWN";
   }

nebula_common/include/nebula_common/seyond/seyond_common.hpp

@@ -0,0 +1,108 @@
+#ifndef NEBULA_SEYOND_COMMON_H
+#define NEBULA_SEYOND_COMMON_H
+
+#include "nebula_common/nebula_common.hpp"
+#include "nebula_common/nebula_status.hpp"
+
+#include <bitset>
+#include <cmath>
+#include <fstream>
+#include <iostream>
+#include <sstream>
+namespace nebula
+{
+namespace drivers
+{
+/// @brief struct for Seyond sensor configuration
+struct SeyondSensorConfiguration : public LidarConfigurationBase
+{
+  uint16_t gnss_port{};
+  double scan_phase{};
+  double dual_return_distance_threshold{};
+  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;
+  PtpSwitchType ptp_switch_type;
+};
+/// @brief Convert SeyondSensorConfiguration to string (Overloading the << operator)
+/// @param os
+/// @param arg
+/// @return stream
+inline std::ostream & operator<<(std::ostream & os, SeyondSensorConfiguration const & arg)
+{
+  os << (LidarConfigurationBase)(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
+     << ", PtpProfile:" << arg.ptp_profile << ", PtpDomain:" << std::to_string(arg.ptp_domain)
+     << ", PtpTransportType:" << arg.ptp_transport_type
+     << ", PtpSwitchType:" << arg.ptp_switch_type;
+  return os;
+}
+
+struct SeyondCalibrationConfigurationBase : public CalibrationConfigurationBase
+{
+
+};
+
+/// @brief struct for Seyond calibration configuration
+struct SeyondCalibrationConfiguration : public SeyondCalibrationConfigurationBase
+{
+  std::map<size_t, float> elev_angle_map;
+  std::map<size_t, float> azimuth_offset_map;
+};
+
+/*
+<option value="0">Last Return</option>
+<option value="1">Strongest Return</option>
+<option value="3">First Return</option>
+<option value="2">Last Return + Strongest Return</option>
+<option value="4">First Return + Last Return</option>
+<option value="5">First Return + Strongest Return</option>
+*/
+/*
+<option value="0">Last Return</option>
+<option value="1">Strongest Return</option>
+<option value="3">First Return</option>
+<option value="2">Last Return + Strongest Return</option>
+<option value="4">First Return + Strongest Return</option>
+<option value="5">First Return + Last Return</option>
+<option value="6">First Return + Last Return + Strongest Return</option>
+*/
+
+/// @brief Convert return mode name to ReturnMode enum (Seyond-specific ReturnModeFromString)
+/// @param return_mode Return mode name (Upper and lower case letters must match)
+/// @param sensor_model Model for correct conversion
+/// @return Corresponding ReturnMode
+inline ReturnMode ReturnModeFromStringSeyond(
+  const std::string & return_mode, const SensorModel & sensor_model)
+{
+  return ReturnMode::UNKNOWN;
+}
+
+/// @brief Convert return mode number to ReturnMode enum
+/// @param return_mode Return mode number from the hardware response
+/// @param sensor_model Model for correct conversion
+/// @return Corresponding ReturnMode
+inline ReturnMode ReturnModeFromIntSeyond(const int return_mode, const SensorModel & sensor_model)
+{
+  return ReturnMode::UNKNOWN;
+}
+
+/// @brief Convert ReturnMode enum to return mode number
+/// @param return_mode target ReturnMode
+/// @param sensor_model Model for correct conversion
+/// @return Corresponding return mode number for the hardware
+inline int IntFromReturnModeSeyond(const ReturnMode return_mode, const SensorModel & sensor_model)
+{
+
+  return -1;
+}
+
+}  // namespace drivers
+}  // namespace nebula
+
+#endif  // NEBULA_SEYOND_COMMON_H

nebula_decoders/CMakeLists.txt

@@ -30,6 +30,12 @@ include_directories(
 )
 
 # Lidar Decoders
+
+# Hesai
+ament_auto_add_library(nebula_decoders_seyond SHARED
+        src/nebula_decoders_seyond/seyond_driver.cpp
+        )
+
 # Hesai
 ament_auto_add_library(nebula_decoders_hesai SHARED
         src/nebula_decoders_hesai/hesai_driver.cpp

nebula_decoders/include/nebula_decoders/nebula_decoders_seyond/decoders/falcon.hpp

@@ -0,0 +1,32 @@
+#pragma once
+
+#include "nebula_decoders/nebula_decoders_seyond/decoders/seyond_packet.hpp"
+#include "nebula_decoders/nebula_decoders_seyond/decoders/seyond_sensor.hpp"
+
+namespace nebula
+{
+namespace drivers
+{
+
+namespace seyond_packet
+{
+
+#pragma pack(push, 1)
+
+struct PacketFalcon : public PacketBase<8, 32, 2, 100>
+{
+
+};
+
+#pragma pack(pop)
+
+}  // namespace seyond_packet
+
+class Falcon : public SeyondSensor<seyond_packet::PacketFalcon>
+{
+
+
+};
+
+}  // namespace drivers
+}  // namespace nebula

nebula_decoders/include/nebula_decoders/nebula_decoders_seyond/decoders/robin_w.hpp

@@ -0,0 +1,32 @@
+#pragma once
+
+#include "nebula_decoders/nebula_decoders_seyond/decoders/seyond_packet.hpp"
+#include "nebula_decoders/nebula_decoders_seyond/decoders/seyond_sensor.hpp"
+
+namespace nebula
+{
+namespace drivers
+{
+
+namespace seyond_packet
+{
+
+#pragma pack(push, 1)
+
+struct PacketRobinW : public PacketBase<8, 32, 2, 100>
+{
+
+};
+
+#pragma pack(pop)
+
+}  // namespace seyond_packet
+
+class RobinW : public SeyondSensor<seyond_packet::PacketRobinW>
+{
+
+
+};
+
+}  // namespace drivers
+}  // namespace nebula

nebula_decoders/include/nebula_decoders/nebula_decoders_seyond/decoders/seyond_decoder.hpp

@@ -0,0 +1,111 @@
+#pragma once
+
+#include "nebula_decoders/nebula_decoders_seyond/decoders/seyond_packet.hpp"
+#include "nebula_decoders/nebula_decoders_seyond/decoders/seyond_scan_decoder.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include "nebula_msgs/msg/nebula_packet.hpp"
+#include "nebula_msgs/msg/nebula_packets.hpp"
+
+namespace nebula
+{
+namespace drivers
+{
+
+template <typename SensorT>
+class SeyondDecoder : public SeyondScanDecoder
+{
+protected:
+  /// @brief Configuration for this decoder
+  const std::shared_ptr<const drivers::SeyondSensorConfiguration> sensor_configuration_;
+
+  /// @brief The sensor definition, used for return mode and time offset handling
+  SensorT sensor_{};
+
+  /// @brief Decodes azimuth/elevation angles given calibration/correction data
+  typename SensorT::angle_corrector_t angle_corrector_;
+
+  /// @brief The point cloud new points get added to
+  NebulaPointCloudPtr decode_pc_;
+  /// @brief The point cloud that is returned when a scan is complete
+  NebulaPointCloudPtr output_pc_;
+
+  /// @brief The last decoded packet
+  typename SensorT::packet_t packet_;
+  /// @brief The last azimuth processed
+  int last_phase_ = -1; // Dummy value to signal last_phase_ has not been set yet
+  /// @brief The timestamp of the last completed scan in nanoseconds
+  uint64_t output_scan_timestamp_ns_ = 0;
+  /// @brief The timestamp of the scan currently in progress
+  uint64_t decode_scan_timestamp_ns_ = 0;
+  /// @brief Whether a full scan has been processed
+  bool has_scanned_ = false;
+
+  rclcpp::Logger logger_;
+
+  /// @brief For each channel, its firing offset relative to the block in nanoseconds
+  std::array<int, SensorT::packet_t::N_CHANNELS> channel_firing_offset_ns_;
+  /// @brief For each return mode, the firing offset of each block relative to its packet in
+  /// nanoseconds
+  std::array<std::array<int, SensorT::packet_t::N_BLOCKS>, SensorT::packet_t::MAX_RETURNS>
+    block_firing_offset_ns_;
+
+  /// @brief Validates and parse NebulaPacket. Currently only checks size, not checksums etc.
+  /// @param packet The incoming NebulaPacket
+  /// @return Whether the packet was parsed successfully
+  bool parsePacket(const std::vector<uint8_t> & packet)
+  {
+    if (packet.size() < sizeof(typename SensorT::packet_t)) {
+      RCLCPP_ERROR_STREAM(
+        logger_, "Packet size mismatch:" << packet.size() << " | Expected at least:"
+                                         << sizeof(typename SensorT::packet_t));
+      return false;
+    }
+    if (std::memcpy(&packet_, packet.data(), sizeof(typename SensorT::packet_t))) {
+      // FIXME(mojomex) do validation?
+      // RCLCPP_DEBUG(logger_, "Packet parsed successfully");
+      return true;
+    }
+
+    RCLCPP_ERROR(logger_, "Packet memcopy failed");
+    return false;
+  }
+
+public:
+  /// @brief Constructor
+  /// @param sensor_configuration SensorConfiguration for this decoder
+  /// @param correction_data Calibration data for this decoder
+  explicit SeyondDecoder(
+    const std::shared_ptr<const SeyondSensorConfiguration> & sensor_configuration,
+    const std::shared_ptr<const typename SensorT::angle_corrector_t::correction_data_t> & correction_data)
+  : sensor_configuration_(sensor_configuration),
+    angle_corrector_(correction_data),
+    logger_(rclcpp::get_logger("SeyondDecoder"))
+  {
+    logger_.set_level(rclcpp::Logger::Level::Debug);
+    RCLCPP_INFO_STREAM(logger_, *sensor_configuration_);
+
+    decode_pc_.reset(new NebulaPointCloud);
+    output_pc_.reset(new NebulaPointCloud);
+
+    decode_pc_->reserve(SensorT::MAX_SCAN_BUFFER_POINTS);
+    output_pc_->reserve(SensorT::MAX_SCAN_BUFFER_POINTS);
+  }
+
+  int unpack(const std::vector<uint8_t> & packet) override
+  {
+    return -1;
+  }
+
+  bool hasScanned() override { return has_scanned_; }
+
+  std::tuple<drivers::NebulaPointCloudPtr, double> getPointcloud() override
+  {
+    double scan_timestamp_s = static_cast<double>(output_scan_timestamp_ns_) * 1e-9;
+    return std::make_pair(output_pc_, scan_timestamp_s);
+  }
+};
+
+}  // namespace drivers
+}  // namespace nebula