Prototype (outputs pointclouds but nothing more)

.cspell.json

@@ -5,6 +5,7 @@
   "words": [
     "adctp",
     "Adctp",
+    "aeva",
     "applicate",
     "AT",
     "autosar",

nebula_common/include/nebula_common/aeva/types.hpp

@@ -0,0 +1,151 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#pragma once
+
+#include <pcl/impl/point_types.hpp>
+
+#include <cmath>
+#include <cstddef>
+#include <cstdint>
+#include <string>
+#include <type_traits>
+#include <vector>
+namespace nebula::drivers::aeva
+{
+
+#pragma pack(push, 1)
+
+template <typename storage_t, size_t n_fractional_bits>
+struct FixedPoint
+{
+  [[nodiscard]] float value() const { return value_ * std::pow(2., -1. * n_fractional_bits); }
+
+private:
+  storage_t value_;
+};
+
+struct SomeIpHeader
+{
+  uint16_t service_id;
+  uint16_t method_id;
+  uint32_t message_length;
+  uint16_t client_id;
+  uint16_t session_id;
+  uint8_t protocol;
+  uint8_t interface_version;
+  uint8_t message_type;
+  uint8_t return_code;
+  uint32_t tp_header_offset : 28;
+  uint32_t tp_header : 4;
+};
+
+struct MessageHeader
+{
+  uint8_t major_version : 4;
+  uint8_t minor_version : 4;
+  uint8_t message_type;
+  uint16_t sequence_id;
+  uint32_t message_length;
+  uint64_t acquisition_time_ns;
+  uint64_t publish_time_ns;
+};
+
+struct PointcloudMsgSubheaderAndMetadata
+{
+  uint16_t aeva_marker;
+  uint8_t platform;
+  uint8_t reserved_1;
+  uint16_t ns_per_index;
+  uint64_t first_point_timestamp_ns;
+  int32_t frame_sync_index;
+  uint32_t period_ns;
+  uint32_t n_entries;
+  uint32_t capacity;
+  uint16_t num_beams : 4;
+  uint16_t num_peaks : 2;
+  uint16_t range_scale : 10;
+  uint8_t line_index;
+  uint8_t max_line_index;
+  uint32_t face_index : 4;
+  uint32_t n_faces : 4;
+  uint32_t reserved_2 : 3;
+  uint32_t sensitivity_mode : 3;
+  uint32_t frame_parity : 1;
+  uint32_t reserved_3 : 1;
+  uint32_t window_measurement : 1;
+  uint32_t reserved_5 : 1;
+  uint32_t discard_line : 1;
+  uint32_t ping_pong : 1;
+  uint32_t reserved_6 : 12;
+  FixedPoint<int16_t, 9> mirror_rps;
+  uint16_t dither_step : 5;
+  uint16_t max_dither_step : 5;
+  uint16_t reserved_7 : 6;
+  uint8_t reserved_8[12];
+};
+
+struct PointCloudPoint
+{
+  FixedPoint<int16_t, 15> azimuth;
+  FixedPoint<int16_t, 15> elevation;
+  FixedPoint<uint16_t, 7> range;
+  FixedPoint<int16_t, 8> velocity;
+  uint8_t intensity;
+  uint8_t signal_quality;
+  uint32_t beam_id : 3;
+  uint32_t peak_id : 2;
+  uint32_t line_transition : 1;
+  uint32_t valid : 1;
+  uint32_t dynamic : 1;
+  uint32_t reserved_1 : 1;
+  uint32_t up_sweep : 1;
+  uint32_t in_ambiguity_region : 1;
+  uint32_t reserved_2 : 9;
+  uint32_t peak_width : 4;
+  uint32_t is_single_detection : 1;
+  uint32_t reserved_3 : 7;
+};
+
+struct PointCloudMessageFragment
+{
+  std::vector<PointCloudPoint> points;
+};
+
+#pragma pack(pop)
+
+struct EIGEN_ALIGN16 PointXYZVIRCAEDT
+{
+  float x;
+  float y;
+  float z;
+  float v;
+  uint8_t intensity;
+  uint8_t return_type;
+  uint16_t channel;
+  float azimuth;
+  float elevation;
+  float distance;
+  uint32_t time_stamp;
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+}  // namespace nebula::drivers::aeva
+
+POINT_CLOUD_REGISTER_POINT_STRUCT(  // NOLINT
+  nebula::drivers::aeva::PointXYZVIRCAEDT,
+  (float, x, x)(float, y, y)(float, z, z)(float, v, v)(std::uint8_t, intensity, intensity)(
+    std::uint8_t, return_type,
+    return_type)(std::uint16_t, channel, channel)(float, azimuth, azimuth)(
+    float, elevation, elevation)(float, distance, distance)(std::uint32_t, time_stamp, time_stamp))

nebula_common/include/nebula_common/loggers/console_logger.hpp

@@ -0,0 +1,57 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#pragma once
+
+#include "nebula_common/loggers/logger.hpp"
+
+#include <cstdio>
+#include <iostream>
+#include <istream>
+#include <memory>
+#include <ostream>
+#include <sstream>
+#include <string>
+#include <utility>
+
+namespace nebula::drivers::loggers
+{
+
+class ConsoleLogger : public Logger
+{
+public:
+  explicit ConsoleLogger(std::string name) : name_(std::move(name)) {}
+
+  void debug(const std::string & message) override { printTagged(std::cout, "DEBUG", message); }
+  void info(const std::string & message) override { printTagged(std::cout, "INFO", message); }
+  void warn(const std::string & message) override { printTagged(std::cerr, "WARN", message); }
+  void error(const std::string & message) override { printTagged(std::cerr, "ERROR", message); }
+
+  std::shared_ptr<Logger> child(const std::string & name) override
+  {
+    return std::make_shared<ConsoleLogger>(name_ + "." + name);
+  }
+
+private:
+  const std::string name_;
+
+  void printTagged(std::ostream & os, const std::string & severity, const std::string & message)
+  {
+    // In multithreaded logging, building the string first (... + ...) and then shifting to the
+    // stream will ensure that no other logger outputs between string fragments
+    os << ("[" + name_ + "][" + severity + "] " + message) << std::endl;
+  }
+};
+
+}  // namespace nebula::drivers::loggers

nebula_common/include/nebula_common/loggers/logger.hpp

@@ -0,0 +1,41 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#pragma once
+
+#include <memory>
+#include <string>
+
+namespace nebula::drivers::loggers
+{
+
+class Logger
+{
+public:
+  Logger() = default;
+  Logger(const Logger &) = default;
+  Logger(Logger &&) = delete;
+  Logger & operator=(const Logger &) = default;
+  Logger & operator=(Logger &&) = delete;
+  virtual ~Logger() = default;
+
+  virtual void debug(const std::string & message) = 0;
+  virtual void info(const std::string & message) = 0;
+  virtual void warn(const std::string & message) = 0;
+  virtual void error(const std::string & message) = 0;
+
+  virtual std::shared_ptr<Logger> child(const std::string & name) = 0;
+};
+
+}  // namespace nebula::drivers::loggers

nebula_common/include/nebula_common/util/expected.hpp

@@ -15,6 +15,7 @@
 #pragma once
 
 #include <exception>
+#include <stdexcept>
 #include <string>
 #include <variant>
 
@@ -45,12 +46,13 @@ struct bad_expected_access : public std::exception
 template <typename T, typename E>
 struct expected
 {
-  /// @brief Whether the expected instance holds a value (as opposed to an error).
-  /// Call this before trying to access values via `value()`.
+  /// @brief Whether the expected instance holds a value (as opposed to an
+  /// error). Call this before trying to access values via `value()`.
   /// @return True if a value is contained, false if an error is contained
   bool has_value() { return std::holds_alternative<T>(value_); }
 
-  /// @brief Retrieve the value, or throw `bad_expected_access` if an error is contained.
+  /// @brief Retrieve the value, or throw `bad_expected_access` if an error is
+  /// contained.
   /// @return The value of type `T`
   T value()
   {
@@ -60,16 +62,17 @@ struct expected
     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_)
   {
     if (has_value()) return value();
     return default_;
   }
 
-  /// @brief If the instance has a value, return the value, else throw std::runtime_error(error_msg)
+  /// @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)
@@ -78,7 +81,18 @@ struct expected
     throw std::runtime_error(error_msg);
   }
 
-  /// @brief Retrieve the error, or throw `bad_expected_access` if a value is contained.
+  /// @brief If the instance has a value, return the value, else throw the
+  /// contained error instance.
+  /// @return The value
+  /// @throws The error of type `E` if no value is present
+  T value_or_throw()
+  {
+    if (has_value()) return value();
+    throw error();
+  }
+
+  /// @brief Retrieve the error, or throw `bad_expected_access` if a value is
+  /// contained.
   /// @return The error of type `E`
   E error()
   {
@@ -88,18 +102,18 @@ struct expected
     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_)
   {
     if (!has_value()) return error();
     return default_;
   }
 
-  expected(const T & value) { value_ = value; }  // NOLINT(runtime/explicit)
+  expected(const T & value) : value_(value) {}  // NOLINT(runtime/explicit)
 
-  expected(const E & error) { value_ = error; }  // NOLINT(runtime/explicit)
+  expected(const E & error) : value_(error) {}  // NOLINT(runtime/explicit)
 
 private:
   std::variant<T, E> value_;

nebula_common/include/nebula_common/util/parsing.hpp

@@ -0,0 +1,13 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.