diff --git a/.clang-tidy b/.clang-tidy index c19664331..06c54854b 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -1,204 +1,204 @@ Checks: " -*, - # boost-use-to-string, - # bugprone-argument-comment, - # bugprone-assert-side-effect, - # bugprone-bad-signal-to-kill-thread, - # bugprone-bool-pointer-implicit-conversion, - # bugprone-branch-clone, - # bugprone-copy-constructor-init, - # bugprone-dangling-handle, - # bugprone-dynamic-static-initializers, - # bugprone-exception-escape, - # bugprone-fold-init-type, + boost-use-to-string, + bugprone-argument-comment, + bugprone-assert-side-effect, + bugprone-bad-signal-to-kill-thread, + bugprone-bool-pointer-implicit-conversion, + bugprone-branch-clone, + bugprone-copy-constructor-init, + bugprone-dangling-handle, + bugprone-dynamic-static-initializers, + bugprone-exception-escape, + bugprone-fold-init-type, bugprone-forward-declaration-namespace, - # bugprone-forwarding-reference-overload, - # bugprone-inaccurate-erase, - # bugprone-incorrect-roundings, - # bugprone-integer-division, - # bugprone-lambda-function-name, - # bugprone-macro-parentheses, - # bugprone-macro-repeated-side-effects, - # bugprone-misplaced-operator-in-strlen-in-alloc, - # bugprone-misplaced-widening-cast, - # bugprone-move-forwarding-reference, - # bugprone-multiple-statement-macro, - # bugprone-not-null-terminated-result, - # bugprone-parent-virtual-call, - # bugprone-posix-return, - # bugprone-signed-char-misuse, - # bugprone-sizeof-container, - # bugprone-sizeof-expression, - # bugprone-string-constructor, - # bugprone-string-integer-assignment, - # bugprone-string-literal-with-embedded-nul, - # bugprone-suspicious-enum-usage, - # bugprone-suspicious-memset-usage, - # bugprone-suspicious-missing-comma, - # bugprone-suspicious-semicolon, - # bugprone-suspicious-string-compare, - # bugprone-swapped-arguments, - # bugprone-terminating-continue, - # bugprone-throw-keyword-missing, - # bugprone-too-small-loop-variable, - # bugprone-unchecked-optional-access, - # bugprone-undefined-memory-manipulation, - # bugprone-undelegated-constructor, - # bugprone-unhandled-self-assignment, - # bugprone-unused-raii, - # bugprone-unused-return-value, - # bugprone-use-after-move, - # bugprone-virtual-near-miss, - # cppcoreguidelines-avoid-goto, - # cppcoreguidelines-init-variables, - # cppcoreguidelines-interfaces-global-init, - # cppcoreguidelines-macro-usage, - # cppcoreguidelines-narrowing-conversions, - # cppcoreguidelines-no-malloc, - # cppcoreguidelines-pro-bounds-pointer-arithmetic, - # cppcoreguidelines-pro-type-const-cast, - # cppcoreguidelines-pro-type-cstyle-cast, - # cppcoreguidelines-pro-type-member-init, - # cppcoreguidelines-pro-type-reinterpret-cast, - # cppcoreguidelines-pro-type-static-cast-downcast, - # cppcoreguidelines-pro-type-union-access, - # cppcoreguidelines-slicing, - # cppcoreguidelines-special-member-functions, - # google-build-explicit-make-pair, + bugprone-forwarding-reference-overload, + bugprone-inaccurate-erase, + bugprone-incorrect-roundings, + bugprone-integer-division, + bugprone-lambda-function-name, + bugprone-macro-parentheses, + bugprone-macro-repeated-side-effects, + bugprone-misplaced-operator-in-strlen-in-alloc, + bugprone-misplaced-widening-cast, + bugprone-move-forwarding-reference, + bugprone-multiple-statement-macro, + bugprone-not-null-terminated-result, + bugprone-parent-virtual-call, + bugprone-posix-return, + bugprone-signed-char-misuse, + bugprone-sizeof-container, + bugprone-sizeof-expression, + bugprone-string-constructor, + bugprone-string-integer-assignment, + bugprone-string-literal-with-embedded-nul, + bugprone-suspicious-enum-usage, + bugprone-suspicious-memset-usage, + bugprone-suspicious-missing-comma, + bugprone-suspicious-semicolon, + bugprone-suspicious-string-compare, + bugprone-swapped-arguments, + bugprone-terminating-continue, + bugprone-throw-keyword-missing, + bugprone-too-small-loop-variable, + bugprone-unchecked-optional-access, + bugprone-undefined-memory-manipulation, + bugprone-undelegated-constructor, + bugprone-unhandled-self-assignment, + bugprone-unused-raii, + bugprone-unused-return-value, + bugprone-use-after-move, + bugprone-virtual-near-miss, + cppcoreguidelines-avoid-goto, + cppcoreguidelines-init-variables, + cppcoreguidelines-interfaces-global-init, + cppcoreguidelines-macro-usage, + cppcoreguidelines-narrowing-conversions, + cppcoreguidelines-no-malloc, + cppcoreguidelines-pro-bounds-pointer-arithmetic, + cppcoreguidelines-pro-type-const-cast, + cppcoreguidelines-pro-type-cstyle-cast, + cppcoreguidelines-pro-type-member-init, + cppcoreguidelines-pro-type-reinterpret-cast, + cppcoreguidelines-pro-type-static-cast-downcast, + cppcoreguidelines-pro-type-union-access, + cppcoreguidelines-slicing, + cppcoreguidelines-special-member-functions, + google-build-explicit-make-pair, google-build-namespaces, google-build-using-namespace, - # google-explicit-constructor, - # google-global-names-in-headers, - # google-upgrade-googletest-case, - # hicpp-exception-baseclass, - # hicpp-multiway-paths-covered, - # hicpp-no-assembler, - # hicpp-signed-bitwise, + google-explicit-constructor, + google-global-names-in-headers, + google-upgrade-googletest-case, + hicpp-exception-baseclass, + hicpp-multiway-paths-covered, + hicpp-no-assembler, + hicpp-signed-bitwise, llvm-namespace-comment, - # misc-definitions-in-headers, - # misc-misplaced-const, - # misc-new-delete-overloads, - # misc-non-copyable-objects, - # misc-redundant-expression, - # misc-static-assert, - # misc-throw-by-value-catch-by-reference, - # misc-unconventional-assign-operator, - # misc-uniqueptr-reset-release, - # misc-unused-alias-decls, - # misc-unused-parameters, - # misc-unused-using-decls, + misc-definitions-in-headers, + misc-misplaced-const, + misc-new-delete-overloads, + misc-non-copyable-objects, + misc-redundant-expression, + misc-static-assert, + misc-throw-by-value-catch-by-reference, + misc-unconventional-assign-operator, + misc-uniqueptr-reset-release, + misc-unused-alias-decls, + misc-unused-parameters, + misc-unused-using-decls, modernize-concat-nested-namespaces, - # modernize-deprecated-headers, - # modernize-deprecated-ios-base-aliases, - # modernize-loop-convert, - # modernize-make-shared, - # modernize-make-unique, - # modernize-pass-by-value, - # modernize-raw-string-literal, - # modernize-redundant-void-arg, - # modernize-replace-auto-ptr, - # modernize-replace-disallow-copy-and-assign-macro, - # modernize-replace-random-shuffle, - # modernize-return-braced-init-list, - # modernize-shrink-to-fit, - # modernize-unary-static-assert, - # modernize-use-auto, - # modernize-use-bool-literals, - # modernize-use-default-member-init, - # modernize-use-emplace, - # modernize-use-equals-default, - # modernize-use-equals-delete, - # modernize-use-nodiscard, - # modernize-use-noexcept, - # modernize-use-nullptr, - # modernize-use-override, - # modernize-use-transparent-functors, - # modernize-use-uncaught-exceptions, - # modernize-use-using, - # openmp-use-default-none, - # performance-faster-string-find, - # performance-for-range-copy, - # performance-implicit-conversion-in-loop, - # performance-inefficient-algorithm, - # performance-inefficient-string-concatenation, - # performance-inefficient-vector-operation, - # performance-move-const-arg, - # performance-move-constructor-init, - # performance-no-automatic-move, - # performance-no-int-to-ptr, - # performance-noexcept-move-constructor, - # performance-trivially-destructible, - # performance-type-promotion-in-math-fn, - # performance-unnecessary-copy-initialization, - # performance-unnecessary-value-param, - # portability-simd-intrinsics, - # readability-const-return-type, - # readability-container-size-empty, - # readability-convert-member-functions-to-static, - # readability-delete-null-pointer, - # readability-else-after-return, - # readability-function-cognitive-complexity, - # readability-identifier-naming, - # readability-inconsistent-declaration-parameter-name, - # readability-isolate-declaration, - # readability-make-member-function-const, - # readability-misleading-indentation, - # readability-misplaced-array-index, - # readability-non-const-parameter, - # readability-redundant-access-specifiers, - # readability-redundant-control-flow, - # readability-redundant-declaration, - # readability-redundant-function-ptr-dereference, - # readability-redundant-member-init, - # readability-redundant-smartptr-get, - # readability-redundant-string-cstr, - # readability-redundant-string-init, - # readability-simplify-boolean-expr, - # readability-simplify-subscript-expr, - # readability-static-accessed-through-instance, + modernize-deprecated-headers, + modernize-deprecated-ios-base-aliases, + modernize-loop-convert, + modernize-make-shared, + modernize-make-unique, + modernize-pass-by-value, + modernize-raw-string-literal, + modernize-redundant-void-arg, + modernize-replace-auto-ptr, + modernize-replace-disallow-copy-and-assign-macro, + modernize-replace-random-shuffle, + modernize-return-braced-init-list, + modernize-shrink-to-fit, + modernize-unary-static-assert, + modernize-use-auto, + modernize-use-bool-literals, + modernize-use-default-member-init, + modernize-use-emplace, + modernize-use-equals-default, + modernize-use-equals-delete, + modernize-use-nodiscard, + modernize-use-noexcept, + modernize-use-nullptr, + modernize-use-override, + modernize-use-transparent-functors, + modernize-use-uncaught-exceptions, + modernize-use-using, + openmp-use-default-none, + performance-faster-string-find, + performance-for-range-copy, + performance-implicit-conversion-in-loop, + performance-inefficient-algorithm, + performance-inefficient-string-concatenation, + performance-inefficient-vector-operation, + performance-move-const-arg, + performance-move-constructor-init, + performance-no-automatic-move, + performance-no-int-to-ptr, + performance-noexcept-move-constructor, + performance-trivially-destructible, + performance-type-promotion-in-math-fn, + performance-unnecessary-copy-initialization, + performance-unnecessary-value-param, + portability-simd-intrinsics, + readability-const-return-type, + readability-container-size-empty, + readability-convert-member-functions-to-static, + readability-delete-null-pointer, + readability-else-after-return, + readability-function-cognitive-complexity, + readability-identifier-naming, + readability-inconsistent-declaration-parameter-name, + readability-isolate-declaration, + readability-make-member-function-const, + readability-misleading-indentation, + readability-misplaced-array-index, + readability-non-const-parameter, + readability-redundant-access-specifiers, + readability-redundant-control-flow, + readability-redundant-declaration, + readability-redundant-function-ptr-dereference, + readability-redundant-member-init, + readability-redundant-smartptr-get, + readability-redundant-string-cstr, + readability-redundant-string-init, + readability-simplify-boolean-expr, + readability-simplify-subscript-expr, + readability-static-accessed-through-instance, readability-static-definition-in-anonymous-namespace, - # readability-string-compare, - # readability-uniqueptr-delete-release" + readability-string-compare, + readability-uniqueptr-delete-release" -# WarningsAsErrors: " -# boost-use-to-string, -# bugprone-dangling-handle, -# bugprone-fold-init-type, -# bugprone-inaccurate-erase, -# bugprone-incorrect-roundings, -# bugprone-misplaced-widening-cast, -# bugprone-sizeof-container, -# bugprone-sizeof-expression, -# bugprone-string-constructor, -# bugprone-suspicious-enum-usage, -# bugprone-suspicious-memset-usage, -# bugprone-suspicious-missing-comma, -# bugprone-suspicious-semicolon, -# bugprone-swapped-arguments, -# bugprone-unused-raii, -# bugprone-use-after-move, -# llvm-namespace-comment, -# misc-non-copyable-objects, -# misc-redundant-expression, -# misc-throw-by-value-catch-by-reference, -# misc-unused-alias-decls, -# misc-unused-parameters, -# misc-unused-using-decls, -# modernize-deprecated-headers, -# modernize-redundant-void-arg, -# modernize-use-bool-literals, -# modernize-use-emplace, -# modernize-use-equals-default, -# modernize-use-equals-delete, -# modernize-use-nullptr, -# modernize-use-override, -# modernize-use-using, -# performance-faster-string-find, -# performance-inefficient-algorithm, -# readability-make-member-function-const, -# readability-misleading-indentation, -# readability-misplaced-array-index, -# readability-string-compare" +WarningsAsErrors: " + boost-use-to-string, + bugprone-dangling-handle, + bugprone-fold-init-type, + bugprone-inaccurate-erase, + bugprone-incorrect-roundings, + bugprone-misplaced-widening-cast, + bugprone-sizeof-container, + bugprone-sizeof-expression, + bugprone-string-constructor, + bugprone-suspicious-enum-usage, + bugprone-suspicious-memset-usage, + bugprone-suspicious-missing-comma, + bugprone-suspicious-semicolon, + bugprone-swapped-arguments, + bugprone-unused-raii, + bugprone-use-after-move, + llvm-namespace-comment, + misc-non-copyable-objects, + misc-redundant-expression, + misc-throw-by-value-catch-by-reference, + misc-unused-alias-decls, + misc-unused-parameters, + misc-unused-using-decls, + modernize-deprecated-headers, + modernize-redundant-void-arg, + modernize-use-bool-literals, + modernize-use-emplace, + modernize-use-equals-default, + modernize-use-equals-delete, + modernize-use-nullptr, + modernize-use-override, + modernize-use-using, + performance-faster-string-find, + performance-inefficient-algorithm, + readability-make-member-function-const, + readability-misleading-indentation, + readability-misplaced-array-index, + readability-string-compare" HeaderFilterRegex: ^(?!\/usr)(?!\/opt) diff --git a/.cspell.json b/.cspell.json index 4c14cb9f3..3bce77d74 100644 --- a/.cspell.json +++ b/.cspell.json @@ -5,6 +5,7 @@ "words": [ "adctp", "Adctp", + "aeva", "applicate", "AT", "autosar", @@ -21,6 +22,7 @@ "gptp", "Helios", "Hesai", + "hfov", "horiz", "Idat", "ipaddr", diff --git a/docs/parameters/vendors/aeva/aeries2.md b/docs/parameters/vendors/aeva/aeries2.md new file mode 100644 index 000000000..079f0079b --- /dev/null +++ b/docs/parameters/vendors/aeva/aeries2.md @@ -0,0 +1 @@ +{{ json_to_markdown("nebula_ros/schema/Aeries2.schema.json") }} diff --git a/docs/point_types.md b/docs/point_types.md index 16ce6e0ce..e01a9f24b 100644 --- a/docs/point_types.md +++ b/docs/point_types.md @@ -20,6 +20,22 @@ These definitions can be found in the `nebula_common/include/point_types.hpp`. | `distance` | `float` | `m` | Distance from the sensor origin. | | `timestamp` | `uint32` | `ns` | Time of detection relative to the pointcloud timestamp. | +## PointXYZVIRCAEDT + +| Field | Type | Units | Description | +| --------------- | -------- | ----- | --------------------------------------------------------------------------- | +| `x` | `float` | `m` | The point's cartesian x coordinate. | +| `y` | `float` | `m` | The point's cartesian y coordinate. | +| `z` | `float` | `m` | The point's cartesian z coordinate. | +| `distance_rate` | `float` | `m/s` | The point's velocity component in the direction of the sensor's origin. | +| `intensity` | `uint8` | | The intensity of the return as reported by the sensor. | +| `return type` | `uint8` | | Whether the point was the first, strongest, last, etc. of multiple returns. | +| `channel` | `uint16` | | The ID of the laser channel that produced the point. | +| `azimuth` | `float` | `rad` | The point's azimuth in polar coordinates. | +| `elevation` | `float` | `rad` | The point's elevation in polar coordinates. | +| `distance` | `float` | `m` | The point's distance from the sensor origin. | +| `timestamp` | `uint32` | `ns` | The time the point was detected relative to the pointcloud timestamp. | + ## [Deprecated] PointXYZIR | Field | Type | Units | Description | diff --git a/docs/supported_sensors.md b/docs/supported_sensors.md index 407713e00..971232ce5 100644 --- a/docs/supported_sensors.md +++ b/docs/supported_sensors.md @@ -41,6 +41,14 @@ The `sensor_model` parameter below decides which sensor driver is launched. | Bpearl | Bpearl | Bpearl.param.yaml | ⚠️ | | Helios | Helios | Helios.param.yaml | ⚠️ | +## Aeva LiDARs + +| Model | `sensor_model` | Configuration file | Test status | +| --------- | -------------- | ------------------ | ----------- | +| Aeries II | Aeries2 | Aeries2.param.yaml | ⚠️\* | + +\*: The Aeries II has been tested with firmware version 14.0.0. + ## Continental radars | Model | `sensor_model` | Configuration file | Test status | diff --git a/mkdocs.yml b/mkdocs.yml index 4d002789f..c5aca2028 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -63,6 +63,8 @@ nav: - parameters/vendors/continental/common.md - ARS548: parameters/vendors/continental/ars548.md - SRR520: parameters/vendors/continental/srr520.md + - Aeva: + - Aeries II: parameters/vendors/aeva/aeries2.md - Point cloud types: point_types.md - Design: design.md - Tutorials: tutorials.md diff --git a/nebula_common/CMakeLists.txt b/nebula_common/CMakeLists.txt index 9a17fb217..e3ce0c3bf 100644 --- a/nebula_common/CMakeLists.txt +++ b/nebula_common/CMakeLists.txt @@ -4,7 +4,7 @@ project(nebula_common) find_package(ament_cmake_auto REQUIRED) find_package(PCL REQUIRED COMPONENTS common) find_package(yaml-cpp REQUIRED) -find_package(nlohmann_json REQUIRED) +find_package(nlohmann_json 3.10.5 REQUIRED) # Default to C++17 if (NOT CMAKE_CXX_STANDARD) diff --git a/nebula_common/include/nebula_common/aeva/config_types.hpp b/nebula_common/include/nebula_common/aeva/config_types.hpp new file mode 100644 index 000000000..b56948979 --- /dev/null +++ b/nebula_common/include/nebula_common/aeva/config_types.hpp @@ -0,0 +1,67 @@ +// 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/nebula_common.hpp" +#include "nebula_common/util/parsing.hpp" + +#include + +#include +#include +#include + +namespace nebula::drivers::aeva +{ + +using nlohmann::json; + +struct Aeries2Config : public SensorConfigurationBase +{ + std::string sensor_ip; + json tree; + + [[nodiscard]] std::optional get_return_mode() const + { + auto mode_name = util::get_if_exists(tree, {"dsp_control", "second_peak_type"}); + + if (!mode_name) return {}; + if (mode_name == "strongest") return ReturnMode::DUAL_STRONGEST_SECONDSTRONGEST; + if (mode_name == "farthest") return ReturnMode::DUAL_STRONGEST_LAST; + if (mode_name == "closest") return ReturnMode::DUAL_STRONGEST_FIRST; + + return ReturnMode::UNKNOWN; + } + + friend std::ostream & operator<<(std::ostream & os, const Aeries2Config & arg) + { + os << "Aeva Aeries II Sensor Configuration:\n"; + os << "Sensor Model: " << arg.sensor_model << '\n'; + os << "Frame ID: " << arg.frame_id << '\n'; + os << "Sensor IP: " << arg.sensor_ip; + + for (const auto & category : arg.tree.items()) { + os << '\n' << category.key() << ":"; + auto category_settings = category.value(); + for (const auto & setting : category_settings.items()) { + os << "\n " << setting.key() << ": " << setting.value(); + } + } + + return os; + } +}; + +} // namespace nebula::drivers::aeva diff --git a/nebula_common/include/nebula_common/aeva/packet_types.hpp b/nebula_common/include/nebula_common/aeva/packet_types.hpp new file mode 100644 index 000000000..d6936cf8f --- /dev/null +++ b/nebula_common/include/nebula_common/aeva/packet_types.hpp @@ -0,0 +1,196 @@ +// 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 + +#include +#include +#include +#include +#include +#include + +namespace nebula::drivers::aeva +{ +using nlohmann::json; + +#pragma pack(push, 1) + +template +struct FixedPoint +{ + [[nodiscard]] float value() const { return value_ * std::pow(2., -1. * n_fractional_bits); } + +private: + storage_t value_; +}; + +template +struct Padding +{ + uint8_t padding[n_bytes]; // NOLINT +}; + +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; + int64_t acquisition_time_ns; + int64_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 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 azimuth; + FixedPoint elevation; + FixedPoint range; + FixedPoint 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 PointCloudMessage +{ + PointcloudMsgSubheaderAndMetadata header; + std::vector points; +}; + +enum class TelemetryDataType : uint8_t { + kUInt8 = 0, + kInt8 = 1, + kUInt16 = 2, + kInt16 = 3, + kUInt32 = 4, + kInt32 = 5, + kUInt64 = 6, + kInt64 = 7, + kFloat = 8, + kDouble = 9, + kChar = 10 +}; + +enum class ReconfigRequestType : uint8_t { + kManifestRequest = 0, + kManifestResponse = 1, + kChangeRequest = 2, + kChangeApproved = 3, + kChangeIgnored = 4, + kInvalid = 5 +}; + +inline std::ostream & operator<<(std::ostream & os, const ReconfigRequestType & arg) +{ + switch (arg) { + case ReconfigRequestType::kManifestRequest: + return os << "manifest request"; + case ReconfigRequestType::kManifestResponse: + return os << "manifest response"; + case ReconfigRequestType::kChangeRequest: + return os << "change request"; + case ReconfigRequestType::kChangeApproved: + return os << "change approved"; + case ReconfigRequestType::kChangeIgnored: + return os << "change ignored"; + default: + return os << "invalid: " + std::to_string(static_cast(arg)); + } +} + +struct HealthCode +{ + explicit HealthCode(uint32_t value) : value_(value) {} + + [[nodiscard]] bool is_error() const { return (value_ & g_error_mask) != 0; } + [[nodiscard]] uint32_t get() const { return value_ & ~g_error_mask; } + +private: + uint32_t value_; + static const uint32_t g_error_mask = 1u << 31u; +}; + +struct ReconfigMessage +{ + ReconfigRequestType type = ReconfigRequestType::kInvalid; + std::optional body; +}; + +#pragma pack(pop) + +} // namespace nebula::drivers::aeva diff --git a/nebula_common/include/nebula_common/aeva/point_types.hpp b/nebula_common/include/nebula_common/aeva/point_types.hpp new file mode 100644 index 000000000..c16298eb9 --- /dev/null +++ b/nebula_common/include/nebula_common/aeva/point_types.hpp @@ -0,0 +1,46 @@ +// 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 + +namespace nebula::drivers::aeva +{ + +struct EIGEN_ALIGN16 PointXYZVIRCAEDT +{ + float x; + float y; + float z; + float range_rate; + 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, range_rate, range_rate)(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)) diff --git a/nebula_common/include/nebula_common/loggers/console_logger.hpp b/nebula_common/include/nebula_common/loggers/console_logger.hpp new file mode 100644 index 000000000..45d0ee465 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include +#include + +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 child(const std::string & name) override + { + return std::make_shared(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 diff --git a/nebula_common/include/nebula_common/loggers/logger.hpp b/nebula_common/include/nebula_common/loggers/logger.hpp new file mode 100644 index 000000000..3807994a3 --- /dev/null +++ b/nebula_common/include/nebula_common/loggers/logger.hpp @@ -0,0 +1,48 @@ +// 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 +#include +#include + +#define NEBULA_LOG_STREAM(log_func, stream_args) \ + { \ + auto msg = (std::stringstream{} << stream_args).str(); \ + log_func(msg); \ + } + +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 child(const std::string & name) = 0; +}; + +} // namespace nebula::drivers::loggers diff --git a/nebula_common/include/nebula_common/nebula_common.hpp b/nebula_common/include/nebula_common/nebula_common.hpp index 36f7f60c3..e8ba4e01b 100644 --- a/nebula_common/include/nebula_common/nebula_common.hpp +++ b/nebula_common/include/nebula_common/nebula_common.hpp @@ -64,6 +64,7 @@ enum class ReturnMode : uint8_t { FIRST, DUAL_LAST_FIRST, DUAL_FIRST_STRONGEST, + DUAL_STRONGEST_SECONDSTRONGEST, DUAL }; @@ -196,6 +197,8 @@ inline uint8_t return_mode_to_int(const ReturnMode & mode) case ReturnMode::DUAL: return 18; break; + case ReturnMode::DUAL_STRONGEST_SECONDSTRONGEST: + return 19; default: case ReturnMode::UNKNOWN: return 0; @@ -306,6 +309,9 @@ inline std::ostream & operator<<(std::ostream & os, nebula::drivers::ReturnMode case ReturnMode::DUAL_FIRST_STRONGEST: os << "FirstStrongest"; break; + case ReturnMode::DUAL_STRONGEST_SECONDSTRONGEST: + os << "StrongestSecondstrongest"; + break; case ReturnMode::DUAL: os << "Dual"; break; @@ -341,7 +347,8 @@ enum class SensorModel { ROBOSENSE_BPEARL_V3, ROBOSENSE_BPEARL_V4, CONTINENTAL_ARS548, - CONTINENTAL_SRR520 + CONTINENTAL_SRR520, + AEVA_AERIES2 }; /// @brief not used? @@ -441,6 +448,9 @@ inline std::ostream & operator<<(std::ostream & os, nebula::drivers::SensorModel case SensorModel::CONTINENTAL_SRR520: os << "SRR520"; break; + case SensorModel::AEVA_AERIES2: + os << "Aeries II"; + break; case SensorModel::UNKNOWN: os << "Sensor Unknown"; break; @@ -579,6 +589,7 @@ inline SensorModel sensor_model_from_string(const std::string & sensor_model) // Continental if (sensor_model == "ARS548") return SensorModel::CONTINENTAL_ARS548; if (sensor_model == "SRR520") return SensorModel::CONTINENTAL_SRR520; + if (sensor_model == "Aeries2") return SensorModel::AEVA_AERIES2; return SensorModel::UNKNOWN; } @@ -629,6 +640,8 @@ inline std::string sensor_model_to_string(const SensorModel & sensor_model) return "ARS548"; case SensorModel::CONTINENTAL_SRR520: return "SRR520"; + case SensorModel::AEVA_AERIES2: + return "Aeries II"; default: return "UNKNOWN"; } diff --git a/nebula_common/include/nebula_common/util/expected.hpp b/nebula_common/include/nebula_common/util/expected.hpp index 1d4333443..4bd6296f4 100644 --- a/nebula_common/include/nebula_common/util/expected.hpp +++ b/nebula_common/include/nebula_common/util/expected.hpp @@ -15,6 +15,7 @@ #pragma once #include +#include #include #include @@ -45,12 +46,13 @@ struct bad_expected_access : public std::exception template 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(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,8 +62,8 @@ struct expected return std::get(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_) { @@ -69,7 +71,8 @@ struct expected 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,8 +102,8 @@ struct expected return std::get(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_) { @@ -97,9 +111,9 @@ struct expected 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 value_; diff --git a/nebula_ros/include/nebula_ros/common/mt_queue.hpp b/nebula_common/include/nebula_common/util/mt_queue.hpp similarity index 98% rename from nebula_ros/include/nebula_ros/common/mt_queue.hpp rename to nebula_common/include/nebula_common/util/mt_queue.hpp index 40f26253d..beb2abf62 100644 --- a/nebula_ros/include/nebula_ros/common/mt_queue.hpp +++ b/nebula_common/include/nebula_common/util/mt_queue.hpp @@ -32,7 +32,7 @@ class MtQueue public: explicit MtQueue(size_t capacity) : capacity_(capacity) {} - bool try_push(T && value) + bool tryPush(T && value) { std::unique_lock lock(this->mutex_); bool can_push = queue_.size() < capacity_; diff --git a/nebula_common/include/nebula_common/util/parsing.hpp b/nebula_common/include/nebula_common/util/parsing.hpp new file mode 100644 index 000000000..425902506 --- /dev/null +++ b/nebula_common/include/nebula_common/util/parsing.hpp @@ -0,0 +1,72 @@ +// 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 + +#include +#include + +#include +#include +#include + +namespace nebula::util +{ + +using nlohmann::json; + +inline std::vector to_json_path(const std::string & dot_delimited_path) +{ + std::vector result; + boost::split(result, dot_delimited_path, boost::is_any_of(".")); + return result; +} + +inline json to_json_tree(const json & node, const std::vector & path) +{ + json result = node; + for (auto it = path.crbegin(); it != path.crend(); ++it) { + auto current_key = *it; + result = {{current_key, result}}; + } + + return result; +} + +template +bool update_if_exists(const json & node, const std::vector & path, T & out_value) +{ + auto * current_node = &node; + for (const auto & current_key : path) { + if (!current_node->contains(current_key)) return false; + current_node = ¤t_node->at(current_key); + } + + out_value = current_node->template get(); + return true; +} + +template +std::optional get_if_exists(const json & node, const std::vector & path) +{ + if (T result; update_if_exists(node, path, result)) { + return result; + } + + return {}; +} + +} // namespace nebula::util diff --git a/nebula_decoders/CMakeLists.txt b/nebula_decoders/CMakeLists.txt index e94afcf2a..89548f164 100644 --- a/nebula_decoders/CMakeLists.txt +++ b/nebula_decoders/CMakeLists.txt @@ -115,11 +115,32 @@ target_include_directories(nebula_decoders_continental PUBLIC ${nebula_msgs_INCLUDE_DIRS} ) +# Aeva +add_library(nebula_decoders_aeva SHARED +src/nebula_decoders_aeva/aeva_aeries2_decoder.cpp + +) +target_link_libraries(nebula_decoders_aeva PUBLIC + ${aeva_msgs_TARGETS} + ${diagnostic_msgs_TARGETS} + ${boost_udp_driver_TARGETS} + ${nebula_common_TARGETS} + ${nebula_msgs_TARGETS} +) +target_include_directories(nebula_decoders_aeva PUBLIC + ${aeva_msgs_INCLUDE_DIRS} + ${diagnostic_msgs_INCLUDE_DIRS} + ${boost_udp_driver_INCLUDE_DIRS} + ${nebula_common_INCLUDE_DIRS} + ${nebula_msgs_INCLUDE_DIRS} +) + install(TARGETS nebula_decoders_hesai EXPORT export_nebula_decoders_hesai) install(TARGETS nebula_decoders_velodyne EXPORT export_nebula_decoders_velodyne) install(TARGETS nebula_decoders_robosense EXPORT export_nebula_decoders_robosense) install(TARGETS nebula_decoders_robosense_info EXPORT export_nebula_decoders_robosense_info) install(TARGETS nebula_decoders_continental EXPORT export_nebula_decoders_continental) +install(TARGETS nebula_decoders_aeva EXPORT export_nebula_decoders_aeva) install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) if(BUILD_TESTING) @@ -133,6 +154,7 @@ ament_export_targets(export_nebula_decoders_velodyne) ament_export_targets(export_nebula_decoders_robosense) ament_export_targets(export_nebula_decoders_robosense_info) ament_export_targets(export_nebula_decoders_continental) +ament_export_targets(export_nebula_decoders_aeva) install( DIRECTORY calibration diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_aeva/aeva_aeries2_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_aeva/aeva_aeries2_decoder.hpp new file mode 100644 index 000000000..708653dd3 --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_aeva/aeva_aeries2_decoder.hpp @@ -0,0 +1,73 @@ +// 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 +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace nebula::drivers +{ + +class AevaAeries2Decoder +{ +public: + using AevaPoint = aeva::PointXYZVIRCAEDT; + using AevaPointCloud = pcl::PointCloud; + using AevaPointCloudUniquePtr = std::unique_ptr; + + using callback_t = std::function; + + AevaAeries2Decoder() : cloud_state_({std::make_unique(), 0}) {} + + void process_pointcloud_message(const aeva::PointCloudMessage & message); + + void register_point_cloud_callback(callback_t callback); + + void on_parameter_change(ReturnMode return_mode); + +private: + struct DecoderState + { + int32_t new_frame_index; + uint64_t time_per_marker_point_ns; + size_t line_index; + size_t point_index; + uint64_t absolute_time_ns; + }; + + struct PointcloudState + { + std::unique_ptr cloud; + uint64_t timestamp; + }; + + [[nodiscard]] ReturnType get_return_type(uint32_t peak_id) const; + + callback_t callback_; + std::atomic return_mode_{ReturnMode::UNKNOWN}; + PointcloudState cloud_state_{}; +}; +} // namespace nebula::drivers diff --git a/nebula_decoders/src/nebula_decoders_aeva/aeva_aeries2_decoder.cpp b/nebula_decoders/src/nebula_decoders_aeva/aeva_aeries2_decoder.cpp new file mode 100644 index 000000000..96fc09c68 --- /dev/null +++ b/nebula_decoders/src/nebula_decoders_aeva/aeva_aeries2_decoder.cpp @@ -0,0 +1,101 @@ +// Copyright 2024 TIER IV, Inc. + +#include "nebula_decoders/nebula_decoders_aeva/aeva_aeries2_decoder.hpp" + +#include + +#include + +namespace nebula::drivers +{ + +void AevaAeries2Decoder::process_pointcloud_message(const aeva::PointCloudMessage & message) +{ + DecoderState state{ + message.header.frame_sync_index, message.header.ns_per_index, message.header.line_index, 0, + message.header.first_point_timestamp_ns}; + + for (size_t i = 0; i < message.points.size(); ++i) { + const auto & raw_point = message.points[i]; + + // Point time increases every time a marker point (beam=0, peak=0) is encountered + if (raw_point.beam_id == 0 && raw_point.peak_id == 0) { + state.absolute_time_ns += state.time_per_marker_point_ns; + } + + if (static_cast(i) == state.new_frame_index) { + if (callback_) { + callback_(std::move(cloud_state_.cloud), cloud_state_.timestamp); + } + // Cloud time gets reset below, on the first VALID point that will be + // put in the cloud. This guarantees that the earliest point(s) in the cloud + // have relative time 0 + cloud_state_ = {std::make_unique(), 0}; + } + + if (raw_point.line_transition) { + state.line_index++; + } + + if (!raw_point.valid) { + continue; + } + + if (cloud_state_.cloud->empty()) { + cloud_state_.timestamp = state.absolute_time_ns; + } + + AevaPoint point; + + point.distance = raw_point.range.value(); + point.azimuth = -raw_point.azimuth.value() * M_PI_2f; + point.elevation = raw_point.elevation.value() * M_PI_4f; + + ReturnType return_type = get_return_type(raw_point.peak_id); + + point.return_type = static_cast(return_type); + + float xy_distance = point.distance * std::cos(point.elevation); + point.x = xy_distance * std::sin(point.azimuth); + point.y = xy_distance * std::cos(point.azimuth); + point.z = point.distance * std::sin(point.elevation); + + point.range_rate = raw_point.velocity.value(); + point.intensity = raw_point.intensity; + + point.time_stamp = static_cast(state.absolute_time_ns - cloud_state_.timestamp); + point.channel = static_cast(state.line_index); + + cloud_state_.cloud->emplace_back(point); + } +} + +ReturnType AevaAeries2Decoder::get_return_type(uint32_t peak_id) const +{ + if (peak_id == 0) return ReturnType::STRONGEST; + if (peak_id > 1) return ReturnType::UNKNOWN; + + switch (return_mode_.load()) { + case ReturnMode::DUAL_STRONGEST_FIRST: + return ReturnType::FIRST; + case ReturnMode::DUAL_STRONGEST_LAST: + return ReturnType::LAST; + case ReturnMode::DUAL_STRONGEST_SECONDSTRONGEST: + return ReturnType::SECONDSTRONGEST; + default: + return ReturnType::UNKNOWN; + } +} + +void AevaAeries2Decoder::on_parameter_change(ReturnMode return_mode) +{ + return_mode_.store(return_mode); +} + +void AevaAeries2Decoder::register_point_cloud_callback( + std::function, uint64_t)> callback) +{ + callback_ = std::move(callback); +} + +} // namespace nebula::drivers diff --git a/nebula_hw_interfaces/CMakeLists.txt b/nebula_hw_interfaces/CMakeLists.txt index 2577ea9a5..a52c52a15 100644 --- a/nebula_hw_interfaces/CMakeLists.txt +++ b/nebula_hw_interfaces/CMakeLists.txt @@ -2,13 +2,13 @@ cmake_minimum_required(VERSION 3.14) project(nebula_hw_interfaces) # Default to C++17 -if (NOT CMAKE_CXX_STANDARD) +if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) -endif () +endif() -if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic -Wunused-function) -endif () +endif() find_package(ament_cmake_auto REQUIRED) find_package(boost_tcp_driver) @@ -53,7 +53,6 @@ target_link_libraries(nebula_hw_interfaces_velodyne PUBLIC ${boost_tcp_driver_LIBRARIES} ${boost_udp_driver_LIBRARIES} ${velodyne_msgs_TARGETS} - ) target_include_directories(nebula_hw_interfaces_velodyne PUBLIC ${boost_udp_driver_INCLUDE_DIRS} @@ -68,7 +67,6 @@ target_link_libraries(nebula_hw_interfaces_robosense PUBLIC ${boost_tcp_driver_LIBRARIES} ${boost_udp_driver_LIBRARIES} ${robosense_msgs_TARGETS} - ) target_include_directories(nebula_hw_interfaces_robosense PUBLIC ${boost_udp_driver_INCLUDE_DIRS} @@ -91,10 +89,23 @@ target_include_directories(nebula_hw_interfaces_continental PUBLIC ${ros2_socketcan_INCLUDE_DIRS} ) +add_library(nebula_hw_interfaces_aeva SHARED + src/nebula_aeva_hw_interfaces/aeva_hw_interface.cpp +) +target_link_libraries(nebula_hw_interfaces_aeva PUBLIC + ${boost_tcp_driver_LIBRARIES} + ${boost_udp_driver_LIBRARIES} +) +target_include_directories(nebula_hw_interfaces_aeva PUBLIC + ${boost_tcp_driver_INCLUDE_DIRS} + ${boost_udp_driver_INCLUDE_DIRS} +) + install(TARGETS nebula_hw_interfaces_hesai EXPORT export_nebula_hw_interfaces_hesai) install(TARGETS nebula_hw_interfaces_velodyne EXPORT export_nebula_hw_interfaces_velodyne) install(TARGETS nebula_hw_interfaces_robosense EXPORT export_nebula_hw_interfaces_robosense) install(TARGETS nebula_hw_interfaces_continental EXPORT export_nebula_hw_interfaces_continental) +install(TARGETS nebula_hw_interfaces_aeva EXPORT export_nebula_hw_interfaces_aeva) install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) if(BUILD_TESTING) @@ -107,6 +118,7 @@ ament_export_targets(export_nebula_hw_interfaces_hesai) ament_export_targets(export_nebula_hw_interfaces_velodyne) ament_export_targets(export_nebula_hw_interfaces_robosense) ament_export_targets(export_nebula_hw_interfaces_continental) +ament_export_targets(export_nebula_hw_interfaces_aeva) ament_export_dependencies( boost_tcp_driver diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/aeva_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/aeva_hw_interface.hpp new file mode 100644 index 000000000..9185b2a30 --- /dev/null +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/aeva_hw_interface.hpp @@ -0,0 +1,131 @@ +// 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_hw_interfaces/nebula_hw_interfaces_aeva/connections/health.hpp" +#include "nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/pointcloud.hpp" +#include "nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/reconfig.hpp" +#include "nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/telemetry.hpp" +#include "nebula_hw_interfaces/nebula_hw_interfaces_common/connections/byte_stream.hpp" +#include "nebula_hw_interfaces/nebula_hw_interfaces_common/connections/tcp_receiver.hpp" +#include "nebula_hw_interfaces/nebula_hw_interfaces_common/connections/tcp_sender.hpp" + +#include +#include +#include + +#include +#include +#include + +namespace nebula::drivers +{ + +using connections::TcpSender; +using connections::TcpStream; +using connections::aeva::HealthParser; +using connections::aeva::PointcloudParser; +using connections::aeva::ReconfigParser; +using connections::aeva::TelemetryParser; +using nlohmann::json; + +class AevaHwInterface +{ +public: + /** + * @brief Construct a new AevaHwInterface which tries to connect to the sensor specified in + * `config`. Throws on connection failure. + * + * @param logger the logger the HW interface will log to + * @param config The configuration containing the sensor IP, whether to not to setup the sensor, + * and the sensor settings + */ + AevaHwInterface( + const std::shared_ptr & logger, bool setup_sensor, + const std::shared_ptr & config) + : AevaHwInterface( + logger, setup_sensor, config, make_pointcloud_api(*config), make_telemetry_api(*config), + make_reconfig_api(*config, logger), make_health_api(*config)) + { + } + + /** + * @brief Construct a new AevaHwInterface which takes in given instances of the API endpoints it + * should use. This is useful for testing or offline playback. + * + * @param logger The logger the HW interface uses + * @param config The sensor configuration + * @param pointcloud_api The pointcloud endpoint. Can be null. + * @param telemetry_api The telemetry endpoint. Can be null. + * @param reconfig_api The reconfig endpoint. Can be null. + * @param health_api The health endpoint. Can be null. + */ + AevaHwInterface( + const std::shared_ptr & logger, bool setup_sensor, + const std::shared_ptr & config, + std::shared_ptr pointcloud_api, + std::shared_ptr telemetry_api, std::shared_ptr reconfig_api, + std::shared_ptr health_api) + : setup_sensor_(setup_sensor), + logger_(logger), + pointcloud_api_(std::move(pointcloud_api)), + telemetry_api_(std::move(telemetry_api)), + reconfig_api_(std::move(reconfig_api)), + health_api_(std::move(health_api)) + { + if (setup_sensor_ && reconfig_api_) { + logger_->info("Configuring sensor..."); + on_config_change(config); + logger_->info("Config OK"); + } + } + + void on_config_change(const std::shared_ptr & config); + + void register_cloud_packet_callback(PointcloudParser::callback_t callback); + + void register_raw_cloud_packet_callback(connections::ObservableByteStream::callback_t callback); + + void register_health_callback(HealthParser::callback_t callback); + + void register_telemetry_callback(TelemetryParser::callback_t callback); + +private: + void try_reconfig( + const json & manifest, const std::string & node_name, const std::string & key, + const json & value); + + static std::shared_ptr make_reconfig_api( + const aeva::Aeries2Config & config, const std::shared_ptr & logger); + + static std::shared_ptr make_pointcloud_api(const aeva::Aeries2Config & config); + + static std::shared_ptr make_health_api(const aeva::Aeries2Config & config); + + static std::shared_ptr make_telemetry_api(const aeva::Aeries2Config & config); + + const bool setup_sensor_; + std::shared_ptr logger_; + std::shared_ptr pointcloud_api_; + std::shared_ptr telemetry_api_; + std::shared_ptr reconfig_api_; + std::shared_ptr health_api_; + + /// Fetching the sensor manifest sometimes times out. In those cases, retry the below number of + /// times. + static const uint8_t g_manifest_retries = 3; +}; + +} // namespace nebula::drivers diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/aeva_api.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/aeva_api.hpp new file mode 100644 index 000000000..55999a904 --- /dev/null +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/aeva_api.hpp @@ -0,0 +1,290 @@ +// 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_hw_interfaces/nebula_hw_interfaces_common/connections/byte_stream.hpp" +#include "nebula_hw_interfaces/nebula_hw_interfaces_common/connections/byte_view.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace nebula::drivers::connections::aeva +{ + +static const uint16_t g_aeva_header = 0xAEFA; + +static const uint16_t g_port_spherical_point_cloud = 41000; +static const uint16_t g_port_health = 41001; +static const uint16_t g_port_config = 41002; +static const uint16_t g_port_telemetry = 41003; +static const uint16_t g_port_calibration = 41005; +static const uint16_t g_port_image = 41006; +static const uint16_t g_port_reconfig_request = 21901; +static const uint16_t g_port_reconfig_response = 41007; +static const uint16_t g_port_log = 41009; +static const uint16_t g_port_imu = 41010; + +enum class AevaStreamType : uint16_t { + kSphericalPointCloud = 0, + kHealth = 1, + kConfig = 2, + kTelemetry = 3, + kVelocityEstimate = 4, + kCalibration = 5, + kImage = 6, + kReconfig = 7, + kVehicleStateEstimate = 8, + kLog = 9, + kImu = 10, + kObjectList = 12, + kEstimatedDetectionRange = 33, + kUnknown = 0xFFFFu +}; + +using nebula::drivers::aeva::MessageHeader; +using nebula::drivers::aeva::SomeIpHeader; + +class ParseError : public std::exception +{ +}; + +class MismatchError : public ParseError +{ +public: + MismatchError(const std::string & message, int64_t expected, int64_t actual) + : expected(expected), + actual(actual), + message_(message + ": expected " + std::to_string(expected) + ", got " + std::to_string(actual)) + { + } + + const int64_t expected; + const int64_t actual; + + [[nodiscard]] const char * what() const noexcept override { return message_.c_str(); } + +private: + const std::string message_; +}; + +template +void expect_eq(A actual, E expected, const std::string & message) +{ + auto cast_actual = static_cast(actual); + auto cast_expected = static_cast(expected); + if (cast_actual != cast_expected) throw MismatchError(message, cast_expected, cast_actual); +} + +template +void expect_geq(A actual, E expected, const std::string & message) +{ + auto cast_actual = static_cast(actual); + auto cast_expected = static_cast(expected); + if (cast_actual < cast_expected) throw MismatchError(message, cast_expected, cast_actual); +} + +template +T pull_and_parse( + const std::vector::const_iterator & cbegin, + const std::vector::const_iterator & cend) +{ + if (std::distance(cbegin, cend) != sizeof(T)) { + throw std::out_of_range("Number of bytes provided does not match type's size."); + } + + T result{}; + memcpy(&result, &*cbegin, sizeof(T)); + return result; +} + +template +T pull_and_parse(const std::vector & stream) +{ + return pull_and_parse(stream.cbegin(), stream.cend()); +} + +template +T pull_and_parse(PullableByteStream & stream) +{ + std::vector buffer; + stream.read(buffer, sizeof(T)); + return pull_and_parse(buffer); +} + +template +T pull_and_parse(ByteView & stream) +{ + auto consumed = stream.consume(sizeof(T)).value_or_throw(); + return pull_and_parse(consumed.cbegin(), consumed.cend()); +} + +template +void serialize(std::vector & out_bytes, const T & object) +{ + size_t old_size = out_bytes.size(); + size_t new_size = old_size + sizeof(T); + out_bytes.resize(new_size); + memcpy(&out_bytes.at(old_size), &object, sizeof(T)); +} + +/** + * @brief Handles a single datastream from an Aeva sensor. While this API can + * decode any supported stream type, routing several different streams through + * one API instance is not supported. Instead, create one instance per data + * stream. + */ +template +class AevaParser : public ObservableByteStream +{ +public: + explicit AevaParser(std::shared_ptr incoming_byte_stream) + : incoming_(std::move(incoming_byte_stream)) + { + if (!incoming_) { + throw std::invalid_argument("Incoming byte stream cannot be null"); + } + + thread_ = std::thread([this]() { + while (running_) on_low_level_message(); + }); + } + + AevaParser(const AevaParser &) = default; + AevaParser(AevaParser &&) noexcept = default; + AevaParser & operator=(const AevaParser &) = default; + AevaParser & operator=(AevaParser &&) noexcept = default; + + ~AevaParser() override + { + running_ = false; + thread_.join(); + } + + void register_bytes_callback(callback_t callback) override + { + bytes_callback_ = std::move(callback); + } + +protected: + /** + * @brief Attempts to read one message from the stream, blocking. Parses the SomeIP and + * MessageHeader part of an API message before calling the higher-level `onMessage` parser. + */ + void on_low_level_message() + { + std::vector some_ip_raw; + incoming_->read(some_ip_raw, sizeof(SomeIpHeader)); + if (bytes_callback_) { + bytes_callback_(some_ip_raw); + } + + auto some_ip = pull_and_parse(some_ip_raw); + expect_eq(some_ip.service_id, 0xAEFAu, "Aeva service header mismatch"); + expect_eq(some_ip.method_id, StreamId, "Unexpected method ID"); + auto payload_length = some_ip.message_length - 12; + + std::vector payload_raw; + incoming_->read(payload_raw, payload_length); + if (bytes_callback_) { + bytes_callback_(payload_raw); + } + + auto payload_view = ByteView(payload_raw); + + auto message_header = pull_and_parse(payload_view); + expect_eq(message_header.message_type, StreamId, "Unexpected message type"); + expect_eq(message_header.message_length, payload_length, "Payload size mismatch"); + + on_message(message_header, payload_view); + } + + virtual void on_message( + const MessageHeader & /* message_header */, ByteView & /* payload_bytes */) + { + // Why this refuses to compile with a pure virtual function is beyond me. + assert(false); + } + +private: + std::thread thread_; + std::atomic_bool running_{true}; + + std::shared_ptr incoming_; + callback_t bytes_callback_; +}; + +template +class AevaSender +{ +public: + explicit AevaSender(std::shared_ptr outgoing_byte_stream) + : outgoing_(std::move(outgoing_byte_stream)) + { + } + +protected: + /** + * @brief Sends the serialized message payload `bytes` over the outgoing byte stream, prepending + * the SomeIp and Message headers. + */ + void send_message(std::vector bytes) + { + sequence_number_++; + + std::vector out_bytes; + out_bytes.reserve(sizeof(SomeIpHeader) + sizeof(MessageHeader) + bytes.size()); + + auto some_ip_payload_size = static_cast(bytes.size() + sizeof(MessageHeader) + 12); + SomeIpHeader some_ip{ + 0xAEFA, + static_cast(StreamId), + some_ip_payload_size, + 0xFFF0, + sequence_number_, + 2, + 1, + 1, + 0, + 0, + 0}; + + serialize(out_bytes, some_ip); + + auto message_length = static_cast(sizeof(MessageHeader) + bytes.size()); + MessageHeader message_header{1, 1, static_cast(StreamId), 0, message_length, 0, 0}; + serialize(out_bytes, message_header); + out_bytes.insert(out_bytes.end(), bytes.begin(), bytes.end()); + + outgoing_->write(out_bytes); + } + +private: + std::shared_ptr outgoing_; + uint16_t sequence_number_{}; +}; + +} // namespace nebula::drivers::connections::aeva diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/health.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/health.hpp new file mode 100644 index 000000000..1b8f1e1d1 --- /dev/null +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/health.hpp @@ -0,0 +1,75 @@ +// 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_hw_interfaces/nebula_hw_interfaces_aeva/connections/aeva_api.hpp" + +#include + +#include + +#include +#include +#include +#include +#include +#include + +namespace nebula::drivers::connections::aeva +{ + +using drivers::aeva::HealthCode; + +class HealthParser : public AevaParser +{ +public: + using callback_t = std::function)>; + + explicit HealthParser(std::shared_ptr incoming_byte_stream) + : AevaParser(std::move(incoming_byte_stream)) + { + } + + void register_callback(callback_t callback) { callback_ = std::move(callback); } + +protected: + void on_message(const MessageHeader & message_header, ByteView & payload_bytes) override + { + auto n_entries = pull_and_parse(payload_bytes); + + expect_eq( + n_entries * sizeof(uint32_t), + message_header.message_length - sizeof(MessageHeader) - sizeof(n_entries), + "Unexpected size of health code list"); + + std::vector entries; + entries.reserve(n_entries); + + for (size_t i = 0; i < n_entries; ++i) { + auto health_code_raw = payload_bytes.consume_unsafe(sizeof(uint32_t)).cbegin(); + auto entry = boost::endian::load_little_u32(health_code_raw.base()); + entries.emplace_back(entry); + } + + if (callback_) { + callback_(entries); + } + } + +private: + callback_t callback_; +}; + +} // namespace nebula::drivers::connections::aeva diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/pointcloud.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/pointcloud.hpp new file mode 100644 index 000000000..62c368b2f --- /dev/null +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/pointcloud.hpp @@ -0,0 +1,70 @@ +// 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_hw_interfaces/nebula_hw_interfaces_aeva/connections/aeva_api.hpp" + +#include + +#include +#include + +namespace nebula::drivers::connections::aeva +{ + +using nebula::drivers::aeva::PointCloudMessage; +using nebula::drivers::aeva::PointcloudMsgSubheaderAndMetadata; +using nebula::drivers::aeva::PointCloudPoint; + +class PointcloudParser : public AevaParser +{ +public: + using callback_t = std::function; + + explicit PointcloudParser(std::shared_ptr incoming_byte_stream) + : AevaParser(std::move(incoming_byte_stream)) + { + } + + void register_callback(callback_t callback) { callback_ = std::move(callback); } + +protected: + void on_message(const MessageHeader & message_header, ByteView & payload_bytes) override + { + PointCloudMessage message{}; + message.header = pull_and_parse(payload_bytes); + expect_eq(message.header.aeva_marker, 0xAE5Au, "Pointcloud Aeva marker mismatch"); + expect_eq( + message.header.n_entries * sizeof(PointCloudPoint), + message_header.message_length - sizeof(MessageHeader) - + sizeof(PointcloudMsgSubheaderAndMetadata), + "Payload bytes and point bytes mismatch"); + + message.points.reserve(message.header.n_entries); + for (size_t i = 0; i < message.header.n_entries; ++i) { + auto point = pull_and_parse(payload_bytes); + message.points.emplace_back(point); + } + + if (callback_) { + callback_(message); + } + } + +private: + callback_t callback_; +}; + +} // namespace nebula::drivers::connections::aeva diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/reconfig.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/reconfig.hpp new file mode 100644 index 000000000..46c45582a --- /dev/null +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/reconfig.hpp @@ -0,0 +1,272 @@ +// 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 "nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/aeva_api.hpp" +#include "nebula_hw_interfaces/nebula_hw_interfaces_common/connections/byte_stream.hpp" + +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace nebula::drivers::connections::aeva +{ + +using drivers::aeva::ReconfigMessage; +using drivers::aeva::ReconfigRequestType; +using nlohmann::json; +using namespace std::chrono_literals; // NOLINT + +class ReconfigParser : public AevaParser, + public AevaSender +{ +public: + using callback_t = std::function; + + explicit ReconfigParser( + std::shared_ptr incoming_byte_stream, + std::shared_ptr outgoing_byte_stream, + std::shared_ptr logger) + : AevaParser(std::move(incoming_byte_stream)), + AevaSender(std::move(outgoing_byte_stream)), + logger_(std::move(logger)) + { + } + + json get_manifest() + { + ReconfigMessage request{ReconfigRequestType::kManifestRequest, {}}; + auto responses = do_request(request, g_n_manifest_responses_expected); + + json result{}; + + for (const auto & response : responses) { + if (!response.body) { + throw std::runtime_error("Expected manifest body but got empty response"); + } + + result.update(*response.body); + } + + return result; + } + + json set_parameter(std::string node_name, std::string key, json value) + { + json request_body = {{node_name, {{key, {{"value", value}}}}}}; + ReconfigMessage request = {ReconfigRequestType::kChangeRequest, request_body}; + + auto response = do_request(request); + + if (response.type != aeva::ReconfigRequestType::kChangeApproved) { + std::stringstream ss{}; + ss << "change request failed"; + if (response.body) { + ss << " with body: " << *response.body << std::endl; + } + + throw std::runtime_error(ss.str()); + } + + if (!response.body) { + throw std::runtime_error("Expected change request response body but got empty response"); + } + + return *response.body; + } + +private: + ReconfigMessage do_request(const ReconfigMessage & request) + { + auto responses = do_request(request, 1); + return responses.at(0); + } + + std::vector do_request( + const ReconfigMessage & request, size_t n_responses_expected) + { + std::lock_guard lock(mtx_inflight_); + + // //////////////////////////////////////// + // Build request headers and serialize + // //////////////////////////////////////// + + uint64_t request_id = std::rand(); + + NEBULA_LOG_STREAM(logger_->debug, "Sent " << request.type << ", id=" << request_id); + + ReconfigRequestType type = request.type; + uint8_t encoding_type = 0; + uint16_t reserved = 0; + + std::vector message_payload{}; + if (request.body) { + std::string body_string = nlohmann::to_string(*request.body); + message_payload = std::vector(body_string.begin(), body_string.end()); + } + + auto data_size = static_cast(message_payload.size()); + + std::vector bytes; + bytes.reserve( + sizeof(request_id) + sizeof(type) + sizeof(encoding_type) + sizeof(reserved) + + sizeof(data_size) + data_size); + serialize(bytes, request_id); + serialize(bytes, type); + serialize(bytes, encoding_type); + serialize(bytes, reserved); + serialize(bytes, data_size); + bytes.insert(bytes.end(), message_payload.begin(), message_payload.end()); + + // //////////////////////////////////////// + // Send and wait for response with timeout + // //////////////////////////////////////// + + std::timed_mutex tm_callback_timeout; + tm_callback_timeout.lock(); + + std::mutex mtx_responses; + std::vector responses{}; + responses.reserve(n_responses_expected); + + auto request_valid = std::make_shared(true); + + callback_ = [this, request_valid, n_responses_expected, request_id, &responses, &mtx_responses, + &tm_callback_timeout](uint64_t response_id, const auto & message) { + if (!*request_valid) { + NEBULA_LOG_STREAM( + logger_->error, "Received " << message.type << ", id=" << response_id + << "for no longer valid request id=" << request_id); + return; + } + + if (response_id != request_id) { + // Spurious message reached this callback, the references to `response` and `tm` might not + // be valid Exit without trying to access possibly invalid references and let request + // timeout + NEBULA_LOG_STREAM( + logger_->error, "Spurious " << message.type << " received, id=" << response_id + << ". Expected id=" << request_id); + return; + } + + { + std::lock_guard lock(mtx_responses); + responses.push_back(message); + + if (responses.size() == n_responses_expected) { + tm_callback_timeout.unlock(); + } + } + }; + + send_message(bytes); + auto request_timed_out = !tm_callback_timeout.try_lock_for(20s); + + { + *request_valid = false; + callback_ = {}; + } + + if (request_timed_out) { + throw std::runtime_error("Request timed out"); + } + + // //////////////////////////////////////// + // Do validation and return response + // //////////////////////////////////////// + + auto response_type_valid = [&](const ReconfigMessage & response) { + return (request.type == aeva::ReconfigRequestType::kManifestRequest && + response.type == aeva::ReconfigRequestType::kManifestResponse) || + (request.type == aeva::ReconfigRequestType::kChangeRequest && + (response.type == aeva::ReconfigRequestType::kChangeApproved || + response.type == aeva::ReconfigRequestType::kChangeIgnored)); + }; + + std::vector response_types; + response_types.reserve(responses.size()); + for (const auto & response : responses) { + response_types.push_back((std::stringstream{} << response.type).str()); + } + + NEBULA_LOG_STREAM( + logger_->debug, "Got " + boost::join(response_types, ", ") + " for " + << request.type << ", id=" << request_id); + + if (!std::all_of(responses.cbegin(), responses.cend(), response_type_valid)) { + std::stringstream msg{}; + msg << "Invalid responses for " << request.type << ", id=" << request_id << ": "; + msg << boost::join(response_types, "; "); + throw std::runtime_error(msg.str()); + } + + return responses; + } + +protected: + void on_message(const MessageHeader & message_header, ByteView & payload_bytes) override + { + ReconfigMessage message{}; + + auto request_id = pull_and_parse(payload_bytes); + message.type = pull_and_parse(payload_bytes); + auto encoding_type = pull_and_parse(payload_bytes); + expect_eq(encoding_type, 0, "Unsupported encoding type"); + + payload_bytes.consume(2).value_or_throw(); + + auto data_size = pull_and_parse(payload_bytes); + auto data_raw = payload_bytes.consume(data_size).value_or_throw(); + + expect_eq( + data_size, + message_header.message_length - sizeof(MessageHeader) - sizeof(request_id) - + sizeof(message.type) - sizeof(encoding_type) - 2 - sizeof(data_size), + "Unexpected data size field"); + + message.body = nlohmann::json::parse(data_raw.cbegin(), data_raw.cend()); + + if (callback_) { + callback_(request_id, message); + } + } + +private: + std::shared_ptr logger_; + callback_t callback_; + std::mutex mtx_inflight_; + + // scanner, calibration, system_config, spc_converter, dsp_control, self_test, window_measurement + static const size_t g_n_manifest_responses_expected = 7; +}; + +} // namespace nebula::drivers::connections::aeva diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/telemetry.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/telemetry.hpp new file mode 100644 index 000000000..c3eb0e874 --- /dev/null +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/telemetry.hpp @@ -0,0 +1,207 @@ +// 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_hw_interfaces/nebula_hw_interfaces_aeva/connections/aeva_api.hpp" +#include "nebula_hw_interfaces/nebula_hw_interfaces_common/connections/byte_view.hpp" + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace nebula::drivers::connections::aeva +{ + +using nebula::drivers::aeva::TelemetryDataType; +using nlohmann::json; + +using namespace boost::endian; // NOLINT + +namespace telemetry_detail +{ + +const std::vector g_type_overrides = { + "display_all_points", + "hfov_adjustment_deg", + "hfov_rotation_deg", + "PTP_master_offset_in_ns", + "target_below_or_near_min_range", +}; + +template +inline std::vector parse_number_array( + std::function f, const ByteView::Slice & bytes) +{ + ssize_t type_size = sizeof(OutT); + if (bytes.size() % type_size != 0) { + throw std::runtime_error("Buffer length is not divisible by requested type's size"); + } + + size_t n_entries = bytes.size() / type_size; + std::vector result{}; + result.reserve(n_entries); + + for (auto it = bytes.cbegin(); it != bytes.cend(); it += type_size) { + result.emplace_back(f(&*it)); + } + + return result; +} + +inline std::string parse_string(const ByteView::Slice & bytes) +{ + return {bytes.cbegin(), bytes.cend()}; +} + +} // namespace telemetry_detail + +class TelemetryParser : public AevaParser +{ +public: + using callback_t = std::function; + + explicit TelemetryParser(std::shared_ptr incoming_byte_stream) + : AevaParser(std::move(incoming_byte_stream)) + { + } + + void register_callback(callback_t callback) { callback_ = std::move(callback); } + +protected: + void on_message(const MessageHeader & message_header, ByteView & payload_bytes) override + { + auto payload_size = pull_and_parse(payload_bytes); + + auto node_name_size = pull_and_parse(payload_bytes); + payload_bytes.consume(3).value_or_throw(); // reserved + + expect_eq( + payload_size, + message_header.message_length - sizeof(MessageHeader) - sizeof(payload_size) - + sizeof(node_name_size) - 3, + "Unexpected payload size field"); + + auto node_name_raw = payload_bytes.consume(node_name_size).value_or_throw(); + auto node_name = std::string(node_name_raw.cbegin(), node_name_raw.cend()); + + payload_size -= node_name_size; + + json entries = json::object(); + + while (payload_size > 0) { + auto type = pull_and_parse(payload_bytes); + payload_bytes.consume(8).value_or_throw(); + auto entry_key_size = pull_and_parse(payload_bytes); + auto entry_key_raw = payload_bytes.consume(entry_key_size).value_or_throw(); + auto key = std::string(entry_key_raw.cbegin(), entry_key_raw.cend()); + auto entry_data_size = pull_and_parse(payload_bytes); + auto entry_data_raw = payload_bytes.consume(entry_data_size).value_or_throw(); + + json value; + switch (type) { + case TelemetryDataType::kUInt8: + value = telemetry_detail::parse_number_array( + [](const auto * ref) { return *ref; }, entry_data_raw); + break; + case TelemetryDataType::kInt8: + value = telemetry_detail::parse_number_array( + [](const auto * ref) { return static_cast(*ref); }, entry_data_raw); + break; + case TelemetryDataType::kUInt16: + value = telemetry_detail::parse_number_array(&load_little_u16, entry_data_raw); + break; + case TelemetryDataType::kInt16: + value = telemetry_detail::parse_number_array(&load_little_s16, entry_data_raw); + break; + case TelemetryDataType::kUInt32: + value = telemetry_detail::parse_number_array(&load_little_u32, entry_data_raw); + break; + case TelemetryDataType::kInt32: + value = telemetry_detail::parse_number_array(&load_little_s32, entry_data_raw); + break; + case TelemetryDataType::kUInt64: + value = telemetry_detail::parse_number_array(&load_little_u64, entry_data_raw); + break; + case TelemetryDataType::kInt64: + value = telemetry_detail::parse_number_array(&load_little_s64, entry_data_raw); + break; + case TelemetryDataType::kFloat: + value = telemetry_detail::parse_number_array( + [](const uint8_t * ref) { + auto raw_bytes = load_little_u32(ref); + float result{}; + memcpy(&result, &raw_bytes, 4); + return result; + }, + entry_data_raw); + break; + case TelemetryDataType::kDouble: + value = telemetry_detail::parse_number_array( + [](const uint8_t * ref) { + auto raw_bytes = load_little_u64(ref); + double result{}; + memcpy(&result, &raw_bytes, 8); + return result; + }, + entry_data_raw); + break; + case TelemetryDataType::kChar: + auto overrides = telemetry_detail::g_type_overrides; + bool has_override = std::find(overrides.begin(), overrides.end(), key) != overrides.end(); + if (has_override) { + uint64_t raw_value = 0; + for (const uint8_t & it : entry_data_raw) { + raw_value = (raw_value << 8u) | it; + } + + value = static_cast(raw_value); + } else { + value = telemetry_detail::parse_string(entry_data_raw); + } + break; + } + + if (value.is_array() && value.size() == 1) { + value = value[0]; + } + + entries[key] = value; + payload_size -= 1 + 8 + 1 + entry_key_size + 4 + entry_data_size; + } + + expect_eq(payload_size, 0, "Payload and payload size mismatch"); + + json result = {{node_name, entries}}; + + if (callback_) { + callback_(result); + } + } + +private: + callback_t callback_; +}; + +} // namespace nebula::drivers::connections::aeva diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/connections/byte_stream.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/connections/byte_stream.hpp new file mode 100644 index 000000000..b72d8b0d1 --- /dev/null +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/connections/byte_stream.hpp @@ -0,0 +1,82 @@ +// 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 +#include +#include +#include + +namespace nebula::drivers::connections +{ + +/** + * @brief A stream of packets containing bytes. The given callback is called on each arriving + * packet. + */ +class ObservableByteStream +{ +public: + using callback_t = typename std::function & buffer)>; + + ObservableByteStream() = default; + + ObservableByteStream(const ObservableByteStream &) = default; + ObservableByteStream(ObservableByteStream &&) = default; + ObservableByteStream & operator=(const ObservableByteStream &) = default; + ObservableByteStream & operator=(ObservableByteStream &&) = default; + + virtual ~ObservableByteStream() = default; + + virtual void register_bytes_callback(callback_t callback) = 0; +}; + +/** + * @brief A stream of bytes that the user can read N bytes from synchronously. + */ +class PullableByteStream +{ +public: + PullableByteStream() = default; + + PullableByteStream(const PullableByteStream &) = default; + PullableByteStream(PullableByteStream &&) = default; + PullableByteStream & operator=(const PullableByteStream &) = default; + PullableByteStream & operator=(PullableByteStream &&) = default; + + virtual ~PullableByteStream() = default; + + virtual void read(std::vector & into, size_t n_bytes) = 0; +}; + +/** + * @brief A stream of bytes that can be written to by the user. + */ +class WritableByteStream +{ +public: + WritableByteStream() = default; + + WritableByteStream(const WritableByteStream &) = default; + WritableByteStream(WritableByteStream &&) = default; + WritableByteStream & operator=(const WritableByteStream &) = default; + WritableByteStream & operator=(WritableByteStream &&) = default; + + virtual ~WritableByteStream() = default; + + virtual void write(std::vector & data) = 0; +}; + +} // namespace nebula::drivers::connections diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/connections/byte_view.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/connections/byte_view.hpp new file mode 100644 index 000000000..0926547d4 --- /dev/null +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/connections/byte_view.hpp @@ -0,0 +1,121 @@ +// 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/util/expected.hpp" + +#include + +#include +#include +#include +#include +#include + +namespace nebula::drivers::connections +{ + +/** + * @brief A non-owning view onto the bytes contained in an underlying vector. + * Bytes can be consumed from the view, automatically shrinking it by that amount. + */ +class ByteView +{ +public: + class Slice; + +private: + using iter_t = std::vector::const_iterator; + using consume_result_t = nebula::util::expected; + +public: + explicit ByteView(const std::vector & underlying) + : cbegin_(underlying.cbegin()), cend_(underlying.cend()) + { + } + + explicit ByteView(std::vector &&) = delete; + explicit ByteView(const std::vector &&) = delete; + + /** + * @brief Consumes `n_bytes` bytes from the view's beginning, shrinking it by that amount and + * returning a slice making those bytes accessible. This function throws in case of out-of-bounds + * accesses. + * + * @param n_bytes The number of bytes to consume + * @return Slice The consumed bytes + */ + Slice consume_unsafe(size_t n_bytes) + { + auto n = static_cast(n_bytes); + if (n > size()) { + throw std::out_of_range("Index out of bounds"); + } + + auto new_cbegin = std::next(cbegin_, n); + auto result = Slice(cbegin_, new_cbegin); + cbegin_ = new_cbegin; + + return result; + } + + /** + * @brief Tries to consume `n_bytes`, returns a slice in case of success, or returns an exception + * in case of failure. Does not throw. + * + * @param n_bytes The number of bytes to consume + * @return consume_result_t Either the consumed bytes as a slice, or a caught exception + */ + [[nodiscard]] consume_result_t consume(size_t n_bytes) + { + try { + return consume_unsafe(n_bytes); + } catch (const std::out_of_range & e) { + return e; + } + } + + [[nodiscard]] ssize_t size() const { return std::distance(cbegin_, cend_); } + + /** + * @brief A non-owning slice of an underlying view. This slice's bytes can be iterated over and it + * cannot be split/consumed further. + */ + class Slice + { + friend ByteView; + + public: + [[nodiscard]] auto begin() const { return cbegin_; } + [[nodiscard]] auto cbegin() const { return cbegin_; } + + [[nodiscard]] auto end() const { return cend_; } + [[nodiscard]] auto cend() const { return cend_; } + + [[nodiscard]] ssize_t size() const { return std::distance(cbegin_, cend_); } + + private: + Slice(iter_t cbegin, iter_t cend) : cbegin_(cbegin), cend_(cend) {} + + iter_t cbegin_; + iter_t cend_; + }; + +private: + iter_t cbegin_; + iter_t cend_; +}; + +} // namespace nebula::drivers::connections diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/connections/stream_buffer.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/connections/stream_buffer.hpp new file mode 100644 index 000000000..6ff360651 --- /dev/null +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/connections/stream_buffer.hpp @@ -0,0 +1,116 @@ +// 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 "nebula_hw_interfaces/nebula_hw_interfaces_common/connections/byte_stream.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace nebula::drivers::connections +{ + +/** + * @brief A `PullableByteStream` that buffers incoming bytes from a given `ObservableByteStream` + * until they are read by the user. + */ +class StreamBuffer : public PullableByteStream +{ + using packet_loss_callback_t = std::function; + +public: + StreamBuffer( + std::shared_ptr underlying, size_t packets_buffered, + packet_loss_callback_t packet_loss_callback) + : underlying_(std::move(underlying)), + buffer_(packets_buffered), + packet_loss_callback_(std::move(packet_loss_callback)) + { + underlying_->register_bytes_callback( + [this](auto bytes) { on_bytes_from_underlying(std::move(bytes)); }); + } + + void read(std::vector & into, size_t n_bytes) override + { + into.clear(); + + // If there is a packet left from the last read that has not been read completely read + // (remainder_), then continue there. Otherwise, pop the next one off the queue. + auto bytes = remainder_ ? std::move(remainder_) : buffer_.pop(); + + // Sizes match perfectly, return popped vector + if (bytes->size() == n_bytes) { + into.swap(*bytes); + return; + } + + // Too many bytes popped off the queue, put remainder back + if (bytes->size() > n_bytes) { + // Cut the remaining bytes off of bytes (will become remainder_ later). `bytes` then contains + // exactly the `n_bytes` we want as the into, and `into` contains exactly the bytes we + // want to become the `remainder_`. Thus, we can just swap the two vectors. + into.reserve(bytes->size() - n_bytes); + into.insert(into.end(), bytes->end() - static_cast(n_bytes), bytes->end()); + bytes->resize(bytes->size() - n_bytes); + + into.swap(*bytes); + remainder_ = std::move(bytes); + return; + } + + // Too little bytes popped off the queue, fetch next part + if (bytes->size() < n_bytes) { + auto remaining_length = n_bytes - bytes->size(); + std::vector remaining_bytes; + read(remaining_bytes, remaining_length); + into.swap(*bytes); + into.reserve(n_bytes); + into.insert(into.end(), remaining_bytes.begin(), remaining_bytes.end()); + return; + } + + assert(false); // Unreachable + } + +private: + void on_bytes_from_underlying(std::vector bytes) + { + auto ptr = std::make_unique>(); + ptr->swap(bytes); + auto success = buffer_.tryPush(std::move(ptr)); + if (!success) { + packet_loss_callback_(); + } + } + + std::shared_ptr logger_; + std::shared_ptr underlying_; + std::unique_ptr> remainder_; + std::mutex mtx_buffer_; + MtQueue>> buffer_; + + packet_loss_callback_t packet_loss_callback_; +}; + +} // namespace nebula::drivers::connections diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/connections/tcp_receiver.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/connections/tcp_receiver.hpp new file mode 100644 index 000000000..6c3bbc548 --- /dev/null +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/connections/tcp_receiver.hpp @@ -0,0 +1,60 @@ +// 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_hw_interfaces/nebula_hw_interfaces_common/connections/byte_stream.hpp" + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace nebula::drivers::connections +{ + +class TcpStream : public PullableByteStream +{ +public: + TcpStream(const std::string & sensor_ip, uint16_t sensor_port) + { + boost::asio::ip::tcp::resolver resolver(io_service_); + boost::asio::ip::tcp::resolver::query query( + sensor_ip, std::to_string(static_cast(sensor_port))); + auto endpoint_iterator = resolver.resolve(query); + boost::asio::connect(socket_, endpoint_iterator); + } + + void read(std::vector & into, size_t n_bytes) override + { + into.clear(); + into.resize(n_bytes); + boost::asio::read(socket_, boost::asio::buffer(into), boost::asio::transfer_exactly(n_bytes)); + } + +private: + boost::asio::io_service io_service_{1}; + boost::asio::ip::tcp::socket socket_{io_service_}; +}; + +} // namespace nebula::drivers::connections diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/connections/tcp_sender.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/connections/tcp_sender.hpp new file mode 100644 index 000000000..5550fb67c --- /dev/null +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/connections/tcp_sender.hpp @@ -0,0 +1,58 @@ +// 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_hw_interfaces/nebula_hw_interfaces_common/connections/byte_stream.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace nebula::drivers::connections +{ + +class TcpSender : public WritableByteStream +{ +public: + TcpSender(const std::string & sensor_ip, uint16_t sensor_port) + { + boost::asio::ip::tcp::resolver resolver(io_service_); + boost::asio::ip::tcp::resolver::query query( + sensor_ip, std::to_string(static_cast(sensor_port))); + auto endpoint_iterator = resolver.resolve(query); + boost::asio::connect(socket_, endpoint_iterator); + } + + void write(std::vector & bytes) override + { + boost::asio::write(socket_, boost::asio::buffer(bytes), boost::asio::transfer_all()); + } + +private: + boost::asio::io_service io_service_{1}; + boost::asio::ip::tcp::socket socket_{io_service_}; +}; + +} // namespace nebula::drivers::connections diff --git a/nebula_hw_interfaces/src/nebula_aeva_hw_interfaces/aeva_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_aeva_hw_interfaces/aeva_hw_interface.cpp new file mode 100644 index 000000000..3a3f2fedd --- /dev/null +++ b/nebula_hw_interfaces/src/nebula_aeva_hw_interfaces/aeva_hw_interface.cpp @@ -0,0 +1,124 @@ +// 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_hw_interfaces/nebula_hw_interfaces_aeva/aeva_hw_interface.hpp" + +#include "nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/aeva_api.hpp" + +namespace nebula::drivers +{ + +void AevaHwInterface::on_config_change(const std::shared_ptr & config) +{ + if (!reconfig_api_ || !setup_sensor_) return; + + json manifest{}; + for (uint32_t i = 0; i < g_manifest_retries && manifest.is_null(); ++i) { + try { + if (i > 0) { + logger_->info("Re-trying to fetch manifest"); + } + manifest = reconfig_api_->get_manifest(); + if (i > 0) { + logger_->info("Manifest OK"); + } + } catch (const std::runtime_error & e) { + logger_->error(std::string("Could not fetch sensor manifest: ") + e.what()); + reconfig_api_ = make_reconfig_api(*config, logger_); + } + } + + if (manifest.is_null()) { + throw std::runtime_error("Reached maximum retries while trying to fetch manifest"); + } + + for (const auto & category : config->tree.items()) { + for (const auto & setting : category.value().items()) { + try_reconfig(manifest, category.key(), setting.key(), setting.value()); + } + } +} + +void AevaHwInterface::register_cloud_packet_callback(PointcloudParser::callback_t callback) +{ + pointcloud_api_->register_callback(std::move(callback)); +} + +void AevaHwInterface::register_raw_cloud_packet_callback( + connections::ObservableByteStream::callback_t callback) +{ + pointcloud_api_->register_bytes_callback(std::move(callback)); +} + +void AevaHwInterface::register_health_callback(HealthParser::callback_t callback) +{ + health_api_->register_callback(std::move(callback)); +} + +void AevaHwInterface::register_telemetry_callback(TelemetryParser::callback_t callback) +{ + telemetry_api_->register_callback(std::move(callback)); +} + +void AevaHwInterface::try_reconfig( + const json & manifest, const std::string & node_name, const std::string & key, const json & value) +{ + auto old_value_opt = util::get_if_exists(manifest, {node_name, key, "value"}); + if (old_value_opt && old_value_opt.value() == value) return; + + try { + reconfig_api_->set_parameter(node_name, key, value); + } catch (const std::runtime_error & e) { + throw std::runtime_error("Could not set " + node_name + "." + key + ": " + e.what()); + } + + // //////////////////////////////////////// + // Value was successfully updated + // //////////////////////////////////////// + + logger_->info("Set " + node_name + "." + key + " to " + value.dump()); +} + +std::shared_ptr AevaHwInterface::make_reconfig_api( + const aeva::Aeries2Config & config, const std::shared_ptr & logger) +{ + return std::make_shared( + std::make_shared(config.sensor_ip, connections::aeva::g_port_reconfig_response), + std::make_shared(config.sensor_ip, connections::aeva::g_port_reconfig_request), + logger->child("ReconfigApi")); +} + +std::shared_ptr AevaHwInterface::make_pointcloud_api( + const aeva::Aeries2Config & config) +{ + return std::make_shared( + std::make_shared(config.sensor_ip, connections::aeva::g_port_spherical_point_cloud)); +} + +std::shared_ptr AevaHwInterface::make_health_api(const aeva::Aeries2Config & config) +{ + return std::make_shared( + std::make_shared(config.sensor_ip, connections::aeva::g_port_health)); +} + +std::shared_ptr AevaHwInterface::make_telemetry_api( + const aeva::Aeries2Config & config) +{ + return std::make_shared( + std::make_shared(config.sensor_ip, connections::aeva::g_port_telemetry)); +} + +} // namespace nebula::drivers diff --git a/nebula_ros/CMakeLists.txt b/nebula_ros/CMakeLists.txt index 709de2025..c29a83996 100644 --- a/nebula_ros/CMakeLists.txt +++ b/nebula_ros/CMakeLists.txt @@ -215,11 +215,36 @@ rclcpp_components_register_node(continental_srr520_ros_wrapper EXECUTABLE continental_srr520_ros_wrapper_node ) +## Aeva +add_library(aeva_ros_wrapper SHARED + src/aeva/aeva_ros_wrapper.cpp + src/aeva/hw_monitor_wrapper.cpp + src/common/parameter_descriptors.cpp +) + +target_include_directories(aeva_ros_wrapper PUBLIC + ${diagnostic_updater_INCLUDE_DIRS} + ${nebula_decoders_INCLUDE_DIRS} + ${nebula_hw_interfaces_INCLUDE_DIRS} +) + +target_link_libraries(aeva_ros_wrapper PUBLIC + ${diagnostic_updater_TARGETS} + nebula_decoders::nebula_decoders_aeva + nebula_hw_interfaces::nebula_hw_interfaces_aeva +) + +rclcpp_components_register_node(aeva_ros_wrapper + PLUGIN "AevaRosWrapper" + EXECUTABLE aeva_ros_wrapper_node +) + install(TARGETS hesai_ros_wrapper EXPORT export_hesai_ros_wrapper) install(TARGETS velodyne_ros_wrapper EXPORT export_velodyne_ros_wrapper) install(TARGETS robosense_ros_wrapper EXPORT export_robosense_ros_wrapper) install(TARGETS continental_ars548_ros_wrapper EXPORT export_continental_ars548_ros_wrapper) install(TARGETS continental_srr520_ros_wrapper EXPORT export_continental_srr520_ros_wrapper) +install(TARGETS aeva_ros_wrapper EXPORT export_aeva_ros_wrapper) install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) if(BUILD_TESTING) @@ -232,6 +257,7 @@ ament_export_targets(export_hesai_ros_wrapper) ament_export_targets(export_velodyne_ros_wrapper) ament_export_targets(export_robosense_ros_wrapper) ament_export_targets(export_continental_ars548_ros_wrapper) +ament_export_targets(export_aeva_ros_wrapper) install( DIRECTORY config launch diff --git a/nebula_ros/config/lidar/aeva/Aeries2.param.yaml b/nebula_ros/config/lidar/aeva/Aeries2.param.yaml new file mode 100644 index 000000000..8f8fc694b --- /dev/null +++ b/nebula_ros/config/lidar/aeva/Aeries2.param.yaml @@ -0,0 +1,32 @@ +/**: + ros__parameters: + sensor_ip: 192.168.2.201 + launch_hw: true + frame_id: nebula + sensor_model: Aeries2 + setup_sensor: true + diag_span: 1000 + scanner.dithering_enable_ego_speed: 40.0 + scanner.dithering_pattern_option: dithering_pattern_1 + scanner.ele_offset_rad: 0.0 + scanner.enable_frame_dithering: false + scanner.enable_frame_sync: true + scanner.flip_pattern_vertically: false + scanner.frame_sync_offset_in_ms: 999 + scanner.frame_sync_type: nearest_second + scanner.frame_synchronization_on_rising_edge: false + scanner.hfov_adjustment_deg: 0.0 + scanner.hfov_rotation_deg: 0.0 + scanner.highlight_ROI: false + scanner.horizontal_fov_degrees: 110Degrees + scanner.roi_az_offset_rad: 0.0 + scanner.vertical_pattern: 40-14.4-ROI + system_config.range_modes: Normal Range + system_config.sensitivity_mode: normal + system_config.thermal_throttling_setting: Active + spc_converter.discard_points_in_ambiguity_region: false + spc_converter.display_all_points: false + spc_converter.enable_min_range_filter: true + dsp_control.second_peak_type: strongest + dsp_control.use_foveated_velocity_bias: false + dsp_control.velocity_bias_pattern_options: velocity_bias_pattern_1 diff --git a/nebula_ros/include/nebula_ros/aeva/aeva_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/aeva/aeva_ros_wrapper.hpp new file mode 100644 index 000000000..30d8f575d --- /dev/null +++ b/nebula_ros/include/nebula_ros/aeva/aeva_ros_wrapper.hpp @@ -0,0 +1,111 @@ +// 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_ros/aeva/hw_monitor_wrapper.hpp" +#include "nebula_ros/common/parameter_descriptors.hpp" +#include "nebula_ros/common/watchdog_timer.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace nebula::ros +{ + +using nlohmann::json; + +/// @brief Ros wrapper of hesai driver +class AevaRosWrapper final : public rclcpp::Node +{ +public: + explicit AevaRosWrapper(const rclcpp::NodeOptions & options); + +private: + Status declare_and_get_sensor_config_params(); + + template + void declare_json_param(const std::string & dot_delimited_path, json & inout_tree) + { + json param_value = declare_parameter(dot_delimited_path, param_read_write()); + json tree_patch = util::to_json_tree(param_value, util::to_json_path(dot_delimited_path)); + inout_tree.update(tree_patch, true); + } + + template + bool get_json_param( + const std::vector & p, const std::string & dot_delimited_path, + json & inout_tree) const + { + T value; + bool got_param = get_param(p, dot_delimited_path, value); + if (!got_param) return false; + + json json_value = value; + json tree_patch = util::to_json_tree(json_value, util::to_json_path(dot_delimited_path)); + inout_tree.update(tree_patch, true); + return true; + } + + Status validate_and_set_config( + const std::shared_ptr & new_config); + + rcl_interfaces::msg::SetParametersResult on_parameter_change( + const std::vector & p); + + void record_raw_packet(const std::vector & bytes); + + rclcpp::Publisher::SharedPtr packets_pub_; + std::mutex mtx_current_scan_msg_; + nebula_msgs::msg::NebulaPackets::UniquePtr current_scan_msg_; + + rclcpp::Publisher::SharedPtr cloud_pub_; + std::shared_ptr cloud_watchdog_; + + rclcpp::Subscription::SharedPtr packets_sub_; + + std::shared_ptr sensor_cfg_ptr_; + + std::optional hw_interface_; + std::optional hw_monitor_; + drivers::AevaAeries2Decoder decoder_; + + OnSetParametersCallbackHandle::SharedPtr parameter_event_cb_; +}; + +} // namespace nebula::ros diff --git a/nebula_ros/include/nebula_ros/aeva/hw_monitor_wrapper.hpp b/nebula_ros/include/nebula_ros/aeva/hw_monitor_wrapper.hpp new file mode 100644 index 000000000..8b1489820 --- /dev/null +++ b/nebula_ros/include/nebula_ros/aeva/hw_monitor_wrapper.hpp @@ -0,0 +1,92 @@ +// 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace nebula::ros +{ + +using drivers::aeva::HealthCode; +using nlohmann::json; + +class AevaHwMonitorWrapper +{ +public: + AevaHwMonitorWrapper( + rclcpp::Node * const parent_node, std::shared_ptr config); + + void onTelemetryFragment(const json & diff); + + void onHealthCodes(std::vector health_codes); + +private: + struct NodeTelemetry + { + json values; + rclcpp::Time last_update; + }; + + struct TelemetryState + { + std::map entries; + }; + + struct HealthState + { + std::vector codes; + rclcpp::Time last_update; + }; + + void publishDiagnostics(); + + rclcpp::Logger logger_; + rclcpp::Node * const parent_node_; + std::shared_ptr config_; + + rclcpp::Publisher::SharedPtr diagnostics_pub_{}; + rclcpp::TimerBase::SharedPtr diagnostics_pub_timer_{}; + uint16_t diag_span_; + + std::mutex mtx_hardware_id_; + std::optional hardware_id_; + + std::mutex mtx_telemetry_; + TelemetryState telemetry_; + + std::mutex mtx_health_; + std::optional health_; +}; +} // namespace nebula::ros diff --git a/nebula_ros/include/nebula_ros/common/nebula_packet_stream.hpp b/nebula_ros/include/nebula_ros/common/nebula_packet_stream.hpp new file mode 100644 index 000000000..410dd83e9 --- /dev/null +++ b/nebula_ros/include/nebula_ros/common/nebula_packet_stream.hpp @@ -0,0 +1,58 @@ +// 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_hw_interfaces/nebula_hw_interfaces_common/connections/byte_stream.hpp" + +#include +#include + +#include "nebula_msgs/msg/detail/nebula_packets__struct.hpp" + +#include +#include +#include +#include + +namespace nebula::ros +{ + +class NebulaPacketStream : public drivers::connections::ObservableByteStream +{ +public: + NebulaPacketStream() = default; + + void register_bytes_callback(callback_t callback) override + { + std::lock_guard lock(mtx_callback_); + callback_ = std::move(callback); + } + + void on_nebula_packets(std::unique_ptr packets) + { + std::lock_guard lock(mtx_callback_); + if (!callback_) return; + + for (auto & packet : packets->packets) { + callback_(packet.data); + } + } + +private: + std::mutex mtx_callback_; + callback_t callback_; +}; + +} // namespace nebula::ros diff --git a/nebula_ros/include/nebula_ros/common/rclcpp_logger.hpp b/nebula_ros/include/nebula_ros/common/rclcpp_logger.hpp new file mode 100644 index 000000000..333435288 --- /dev/null +++ b/nebula_ros/include/nebula_ros/common/rclcpp_logger.hpp @@ -0,0 +1,59 @@ +// 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 +#include +#include + +#include +#include + +namespace nebula::drivers::loggers +{ + +class RclcppLogger : public Logger +{ +public: + explicit RclcppLogger(const std::string & name) : underlying_logger_(rclcpp::get_logger(name)) {} + explicit RclcppLogger(const rclcpp::Logger & underlying) : underlying_logger_(underlying) {} + + void debug(const std::string & message) override + { + RCLCPP_DEBUG_STREAM(underlying_logger_, message); + } + void info(const std::string & message) override + { + RCLCPP_INFO_STREAM(underlying_logger_, message); + } + void warn(const std::string & message) override + { + RCLCPP_WARN_STREAM(underlying_logger_, message); + } + void error(const std::string & message) override + { + RCLCPP_ERROR_STREAM(underlying_logger_, message); + } + + std::shared_ptr child(const std::string & name) override + { + return std::make_shared(underlying_logger_.get_child(name)); + } + +private: + rclcpp::Logger underlying_logger_; +}; + +} // namespace nebula::drivers::loggers diff --git a/nebula_ros/include/nebula_ros/common/watchdog_timer.hpp b/nebula_ros/include/nebula_ros/common/watchdog_timer.hpp index cfca4fc50..945986ee2 100644 --- a/nebula_ros/include/nebula_ros/common/watchdog_timer.hpp +++ b/nebula_ros/include/nebula_ros/common/watchdog_timer.hpp @@ -20,20 +20,21 @@ #include #include #include +#include namespace nebula::ros { class WatchdogTimer { - using watchdog_cb_t = std::function; - public: + using callback_t = std::function; + WatchdogTimer( rclcpp::Node & node, const std::chrono::microseconds & expected_update_interval, - const watchdog_cb_t & callback) + callback_t callback) : node_(node), - callback_(callback), + callback_(std::move(callback)), expected_update_interval_ns_( std::chrono::duration_cast(expected_update_interval).count()) { @@ -59,8 +60,8 @@ class WatchdogTimer rclcpp::Node & node_; rclcpp::TimerBase::SharedPtr timer_; - std::atomic last_update_ns_; - const watchdog_cb_t callback_; + std::atomic last_update_ns_{}; + const callback_t callback_; const uint64_t expected_update_interval_ns_; }; diff --git a/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_wrapper.hpp index ea08cec6f..46ca94606 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_wrapper.hpp @@ -14,7 +14,6 @@ #pragma once -#include "nebula_ros/common/parameter_descriptors.hpp" #include "nebula_ros/common/watchdog_timer.hpp" #include diff --git a/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp index d3f92d556..7cce59f3b 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp @@ -14,8 +14,6 @@ #pragma once -#include "nebula_ros/common/parameter_descriptors.hpp" - #include #include #include diff --git a/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp index 1d755a3ef..a7b4b9f13 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp @@ -14,14 +14,13 @@ #pragma once -#include "nebula_ros/common/mt_queue.hpp" -#include "nebula_ros/common/parameter_descriptors.hpp" #include "nebula_ros/continental/continental_ars548_decoder_wrapper.hpp" #include "nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp" #include #include #include +#include #include #include #include diff --git a/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_wrapper.hpp index a01369209..d9d6c7046 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_wrapper.hpp @@ -14,7 +14,6 @@ #pragma once -#include "nebula_ros/common/parameter_descriptors.hpp" #include "nebula_ros/common/watchdog_timer.hpp" #include diff --git a/nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_wrapper.hpp index 1052981ec..aca56628f 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_wrapper.hpp @@ -14,8 +14,6 @@ #pragma once -#include "nebula_ros/common/parameter_descriptors.hpp" - #include #include #include diff --git a/nebula_ros/include/nebula_ros/continental/continental_srr520_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_srr520_ros_wrapper.hpp index dbef1c8ff..99c7885e0 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_srr520_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_srr520_ros_wrapper.hpp @@ -14,14 +14,13 @@ #pragma once -#include "nebula_ros/common/mt_queue.hpp" -#include "nebula_ros/common/parameter_descriptors.hpp" #include "nebula_ros/continental/continental_srr520_decoder_wrapper.hpp" #include "nebula_ros/continental/continental_srr520_hw_interface_wrapper.hpp" #include #include #include +#include #include #include #include diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp index f8d221c3f..15fd3664c 100644 --- a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp @@ -17,12 +17,14 @@ #include "nebula_common/hesai/hesai_common.hpp" #include "nebula_common/nebula_common.hpp" #include "nebula_common/nebula_status.hpp" -#include "nebula_ros/common/mt_queue.hpp" +#include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp" +#include "nebula_ros/common/parameter_descriptors.hpp" #include "nebula_ros/hesai/decoder_wrapper.hpp" #include "nebula_ros/hesai/hw_interface_wrapper.hpp" #include "nebula_ros/hesai/hw_monitor_wrapper.hpp" #include +#include #include #include diff --git a/nebula_ros/include/nebula_ros/robosense/robosense_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/robosense/robosense_ros_wrapper.hpp index 2c7e08d00..52fd8ebf7 100644 --- a/nebula_ros/include/nebula_ros/robosense/robosense_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/robosense/robosense_ros_wrapper.hpp @@ -14,7 +14,6 @@ #pragma once -#include "nebula_ros/common/mt_queue.hpp" #include "nebula_ros/robosense/decoder_wrapper.hpp" #include "nebula_ros/robosense/hw_interface_wrapper.hpp" #include "nebula_ros/robosense/hw_monitor_wrapper.hpp" @@ -24,6 +23,7 @@ #include #include #include +#include #include #include #include diff --git a/nebula_ros/include/nebula_ros/velodyne/velodyne_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/velodyne/velodyne_ros_wrapper.hpp index 9f7fe8758..0685eb321 100644 --- a/nebula_ros/include/nebula_ros/velodyne/velodyne_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/velodyne/velodyne_ros_wrapper.hpp @@ -14,7 +14,6 @@ #pragma once -#include "nebula_ros/common/mt_queue.hpp" #include "nebula_ros/common/parameter_descriptors.hpp" #include "nebula_ros/velodyne/decoder_wrapper.hpp" #include "nebula_ros/velodyne/hw_interface_wrapper.hpp" @@ -24,6 +23,7 @@ #include #include #include +#include #include #include #include diff --git a/nebula_ros/launch/aeva_launch_all_hw.xml b/nebula_ros/launch/aeva_launch_all_hw.xml new file mode 100644 index 000000000..9208be917 --- /dev/null +++ b/nebula_ros/launch/aeva_launch_all_hw.xml @@ -0,0 +1,14 @@ + + + + + + + + + + + + + diff --git a/nebula_ros/launch/nebula_launch.py b/nebula_ros/launch/nebula_launch.py index ca0eaaacb..51f024783 100644 --- a/nebula_ros/launch/nebula_launch.py +++ b/nebula_ros/launch/nebula_launch.py @@ -35,12 +35,14 @@ ] SENSOR_MODELS_ROBOSENSE = ["Bpearl", "Helios"] SENSOR_MODELS_CONTINENTAL = ["ARS548", "SRR520"] +SENSOR_MODELS_AEVA = ["Aeries2"] SENSOR_MODELS = ( SENSOR_MODELS_VELODYNE + SENSOR_MODELS_HESAI + SENSOR_MODELS_ROBOSENSE + SENSOR_MODELS_CONTINENTAL + + SENSOR_MODELS_AEVA ) @@ -58,6 +60,8 @@ def launch_setup(context: launch.LaunchContext, *args, **kwargs): vendor_launch_file = "robosense_launch_all_hw.xml" elif sensor_model in SENSOR_MODELS_CONTINENTAL: vendor_launch_file = "continental_launch_all_hw.xml" + elif sensor_model in SENSOR_MODELS_AEVA: + vendor_launch_file = "aeva_launch_all_hw.xml" else: raise KeyError(f"Sensor model {sensor_model} does not have an associated launch file") diff --git a/nebula_ros/schema/Aeries2.schema.json b/nebula_ros/schema/Aeries2.schema.json new file mode 100644 index 000000000..659254724 --- /dev/null +++ b/nebula_ros/schema/Aeries2.schema.json @@ -0,0 +1,280 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "LiDAR Aeva Aeries II parameters.", + "type": "object", + "definitions": { + "Aeries2": { + "type": "object", + "properties": { + "sensor_ip": { + "$ref": "sub/communication.json#/definitions/sensor_ip", + "default": "192.168.2.201" + }, + "launch_hw": { + "$ref": "sub/hardware.json#/definitions/launch_hw", + "default": true + }, + "frame_id": { + "$ref": "sub/topic.json#/definitions/frame_id", + "default": "nebula" + }, + "sensor_model": { + "$ref": "sub/lidar_aeva.json#/definitions/sensor_model", + "enum": [ + "Aeries2" + ], + "default": "Aeries2" + }, + "setup_sensor": { + "$ref": "sub/hardware.json#/definitions/setup_sensor" + }, + "diag_span": { + "$ref": "sub/topic.json#/definitions/diag_span" + }, + "scanner.dithering_enable_ego_speed": { + "type": "number", + "readOnly": false, + "description": "The ego speed in m/s from which pointcloud dithering (different patterns per frame) is enabled.", + "max": 200.0, + "min": 0.0, + "default": "40.0" + }, + "scanner.dithering_pattern_option": { + "type": "string", + "readOnly": false, + "description": "The dithering pattern to use if enabled.", + "enum": [ + "dithering_pattern_1", + "dithering_pattern_2" + ], + "default": "dithering_pattern_1" + }, + "scanner.ele_offset_rad": { + "type": "number", + "readOnly": false, + "description": "Elevation offset used to align ROI to ground level at a certain distance.", + "max": 0.35, + "min": -0.35, + "default": "0.0" + }, + "scanner.enable_frame_dithering": { + "type": "boolean", + "readOnly": false, + "description": "Whether dithering is enabled.", + "default": "false" + }, + "scanner.enable_frame_sync": { + "type": "boolean", + "readOnly": false, + "description": "Whether to sync pointcloud frame output to a given sub-second offset.", + "default": true + }, + "scanner.flip_pattern_vertically": { + "type": "boolean", + "readOnly": false, + "description": "Whether to flip the scan pattern vertically.", + "default": "false" + }, + "scanner.frame_sync_offset_in_ms": { + "type": "integer", + "readOnly": false, + "description": "The sub-second offset to sync pointcloud frame starts to. 0 for top-of-second alignment. In practice, 999 achieves results closer to top-of-second", + "min": 0, + "max": 999, + "default": 999 + }, + "scanner.frame_sync_type": { + "type": "string", + "readOnly": false, + "description": "Whether to start sync at an absolute timestamp in the future, or from the nearest second.", + "enum": [ + "absolute_time", + "nearest_second" + ], + "default": "nearest_second" + }, + "scanner.frame_synchronization_on_rising_edge": { + "type": "boolean", + "readOnly": false, + "description": "Contact vendor for info.", + "default": "false" + }, + "scanner.hfov_adjustment_deg": { + "type": "number", + "readOnly": false, + "description": "The number of degrees to expend or shrink the horizontal field of view by.", + "max": 100.0, + "min": -100.0, + "default": "0.0" + }, + "scanner.hfov_rotation_deg": { + "type": "number", + "readOnly": false, + "description": "Number of degrees to shift the horizontal field of view to the left (-) or right (+).", + "max": 100.0, + "min": -100.0, + "default": "0.0" + }, + "scanner.highlight_ROI": { + "type": "boolean", + "readOnly": false, + "description": "A debug option to visually highlight points in the configured region of interest.", + "default": "false" + }, + "scanner.horizontal_fov_degrees": { + "type": "string", + "readOnly": false, + "description": "The horizontal field of view of the sensor. Larger FoVs use more power.", + "enum": [ + "110Degrees", + "120Degrees" + ], + "default": "110Degrees" + }, + "scanner.roi_az_offset_rad": { + "type": "number", + "readOnly": false, + "description": "Shift the ROI (if configured) to the left (-) or right (+).", + "max": 0.2094, + "min": -0.2094, + "default": "0.0" + }, + "scanner.vertical_pattern": { + "type": "string", + "readOnly": false, + "description": "The scan pattern to use. Refer to the user manual for details.", + "enum": [ + "40-12-ROI-1m", + "40-14.4-ROI", + "40-14.4-ROI-1m", + "40-14.4-ROI-EGL", + "40-28.8-ROI", + "64-14.4-ROI-1m", + "64-16-ROI", + "64-16-ROI-1m", + "64-19.2-ROI-1m", + "64-19.2-ROI-2-EGL", + "64-28.8-ROI", + "80-19.2-Uniform", + "80-24-ROI-2", + "cal_192_384-19.2-Uniform-5f" + ], + "default": "40-14.4-ROI" + }, + "system_config.range_modes": { + "type": "string", + "enum": [ + "Long Range", + "Medium Range", + "Normal Range", + "Short Range", + "Ultra Long Range", + "Very Long Range" + ], + "default": "Normal Range" + }, + "system_config.sensitivity_mode": { + "type": "string", + "enum": [ + "higher", + "normal" + ], + "default": "normal" + }, + "system_config.thermal_throttling_setting": { + "type": "string", + "enum": [ + "Active", + "Inactive" + ], + "default": "Active" + }, + "spc_converter.discard_points_in_ambiguity_region": { + "type": "boolean", + "default": "false" + }, + "spc_converter.display_all_points": { + "type": "boolean", + "default": "false" + }, + "spc_converter.enable_min_range_filter": { + "type": "boolean", + "default": true + }, + "dsp_control.second_peak_type": { + "type": "string", + "enum": [ + "closest", + "farthest", + "strongest" + ], + "default": "strongest" + }, + "dsp_control.use_foveated_velocity_bias": { + "type": "boolean", + "default": "false" + }, + "dsp_control.velocity_bias_pattern_options": { + "type": "string", + "enum": [ + "velocity_bias_pattern_1", + "velocity_bias_pattern_2" + ], + "default": "velocity_bias_pattern_1" + } + }, + "required": [ + "sensor_ip", + "launch_hw", + "frame_id", + "sensor_model", + "setup_sensor", + "diag_span", + "scanner.dithering_enable_ego_speed", + "scanner.dithering_pattern_option", + "scanner.ele_offset_rad", + "scanner.elevation_auto_adjustment", + "scanner.enable_frame_dithering", + "scanner.enable_frame_sync", + "scanner.flip_pattern_vertically", + "scanner.frame_sync_offset_in_ms", + "scanner.frame_sync_type", + "scanner.frame_synchronization_on_rising_edge", + "scanner.hfov_adjustment_deg", + "scanner.hfov_rotation_deg", + "scanner.highlight_ROI", + "scanner.horizontal_fov_degrees", + "scanner.roi_az_offset_rad", + "scanner.vertical_pattern", + "system_config.range_modes", + "system_config.sensitivity_mode", + "system_config.thermal_throttling_setting", + "spc_converter.discard_points_in_ambiguity_region", + "spc_converter.display_all_points", + "spc_converter.enable_min_range_filter", + "dsp_control.second_peak_type", + "dsp_control.use_foveated_velocity_bias", + "dsp_control.velocity_bias_pattern_options" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/Aeries2" + } + }, + "required": [ + "ros__parameters" + ], + "additionalProperties": false + } + }, + "required": [ + "/**" + ], + "additionalProperties": false +} diff --git a/nebula_ros/schema/sub/lidar_aeva.json b/nebula_ros/schema/sub/lidar_aeva.json new file mode 100644 index 000000000..96458e843 --- /dev/null +++ b/nebula_ros/schema/sub/lidar_aeva.json @@ -0,0 +1,13 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Lidar Aeva parameters.", + "type": "object", + "definitions": { + "sensor_model": { + "$ref": "hardware.json#/definitions/sensor_model", + "enum": [ + "Aeries2" + ] + } + } +} diff --git a/nebula_ros/src/aeva/aeva_ros_wrapper.cpp b/nebula_ros/src/aeva/aeva_ros_wrapper.cpp new file mode 100644 index 000000000..328a1b894 --- /dev/null +++ b/nebula_ros/src/aeva/aeva_ros_wrapper.cpp @@ -0,0 +1,320 @@ +// Copyright 2024 TIER IV, Inc. + +#include "nebula_ros/aeva/aeva_ros_wrapper.hpp" + +#include "nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/pointcloud.hpp" +#include "nebula_ros/aeva/hw_monitor_wrapper.hpp" +#include "nebula_ros/common/nebula_packet_stream.hpp" +#include "nebula_ros/common/parameter_descriptors.hpp" +#include "nebula_ros/common/rclcpp_logger.hpp" +#include "nebula_ros/common/watchdog_timer.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +#pragma clang diagnostic ignored "-Wbitwise-instead-of-logical" + +namespace nebula::ros +{ +using drivers::AevaAeries2Decoder; +using drivers::PointcloudParser; +using drivers::aeva::Aeries2Config; +using nlohmann::json; +using namespace std::chrono_literals; // NOLINT +using AevaPointCloudUniquePtr = AevaAeries2Decoder::AevaPointCloudUniquePtr; + +AevaRosWrapper::AevaRosWrapper(const rclcpp::NodeOptions & options) +: rclcpp::Node("aeva_ros_wrapper", rclcpp::NodeOptions(options).use_intra_process_comms(true)) +{ + auto status = declare_and_get_sensor_config_params(); + + if (status != Status::OK) { + throw std::runtime_error( + (std::stringstream{} << "Sensor configuration invalid: " << status).str()); + } + + RCLCPP_INFO_STREAM(get_logger(), "SensorConfig: " << *sensor_cfg_ptr_); + + const auto & qos_profile = rmw_qos_profile_sensor_data; + auto pointcloud_qos = + rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), qos_profile); + + auto launch_hw = declare_parameter("launch_hw", param_read_only()); + auto setup_sensor = declare_parameter("setup_sensor", param_read_only()); + auto hw_interface_logger = drivers::loggers::RclcppLogger(get_logger()).child("HwInterface"); + + if (!launch_hw && setup_sensor) { + setup_sensor = false; + RCLCPP_WARN(get_logger(), "Ignoring setup_sensor:=true in offline mode."); + } + + if (launch_hw) { + // //////////////////////////////////////// + // If HW is connected, also publish packets + // //////////////////////////////////////// + hw_interface_.emplace(hw_interface_logger, setup_sensor, sensor_cfg_ptr_); + hw_monitor_.emplace(this, sensor_cfg_ptr_); + + packets_pub_ = + create_publisher("nebula_packets", pointcloud_qos); + + drivers::connections::ObservableByteStream::callback_t raw_packet_cb = [&](const auto & bytes) { + this->record_raw_packet(std::move(bytes)); + }; + + hw_interface_->register_raw_cloud_packet_callback(std::move(raw_packet_cb)); + + hw_interface_->register_telemetry_callback( + [this](const auto & msg) { hw_monitor_->onTelemetryFragment(msg); }); + hw_interface_->register_health_callback( + [this](auto codes) { hw_monitor_->onHealthCodes(codes); }); + } else { + // //////////////////////////////////////// + // If HW is disconnected, subscribe to + // packets topic + // //////////////////////////////////////// + auto packet_stream = std::make_shared(); + auto packet_buffer = + std::make_shared(packet_stream, 1000, [this]() { + RCLCPP_ERROR_THROTTLE( + get_logger(), *get_clock(), 1000, + "Packet stream buffer overflowed, packet loss occurred."); + }); + + packets_sub_ = create_subscription( + "nebula_packets", rclcpp::SensorDataQoS(), + [=](std::unique_ptr packets) { + packet_stream->on_nebula_packets(std::move(packets)); + }); + + auto pointcloud_parser = std::make_shared(packet_buffer); + hw_interface_.emplace( + hw_interface_logger, setup_sensor, sensor_cfg_ptr_, pointcloud_parser, nullptr, nullptr, + nullptr); + + RCLCPP_INFO_STREAM( + get_logger(), + "Hardware connection disabled, listening for packets on " << packets_sub_->get_topic_name()); + } + + RCLCPP_INFO(get_logger(), "Starting stream"); + + PointcloudParser::callback_t pointcloud_message_cb = [this](const auto & message) { + decoder_.process_pointcloud_message(message); + }; + + hw_interface_->register_cloud_packet_callback(std::move(pointcloud_message_cb)); + + cloud_pub_ = create_publisher("nebula_points", pointcloud_qos); + + cloud_watchdog_ = std::make_shared(*this, 110'000us, [this](bool ok) { + if (ok) return; + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "Missed pointcloud output deadline"); + }); + + AevaAeries2Decoder::callback_t pointcloud_cb = + [this](AevaPointCloudUniquePtr cloud_ptr, auto timestamp) { + auto now = this->now(); + cloud_watchdog_->update(); + + if ( + cloud_pub_->get_subscription_count() > 0 || + cloud_pub_->get_intra_process_subscription_count() > 0) { + auto ros_pc_msg_ptr = std::make_unique(); + pcl::toROSMsg(*cloud_ptr, *ros_pc_msg_ptr); + ros_pc_msg_ptr->header.frame_id = sensor_cfg_ptr_->frame_id; + ros_pc_msg_ptr->header.stamp = rclcpp::Time(static_cast(timestamp)); + cloud_pub_->publish(std::move(ros_pc_msg_ptr)); + } + + std::lock_guard lock(mtx_current_scan_msg_); + if (current_scan_msg_ && packets_pub_) { + packets_pub_->publish(std::move(current_scan_msg_)); + current_scan_msg_ = {}; + } + }; + + decoder_.register_point_cloud_callback(std::move(pointcloud_cb)); + + parameter_event_cb_ = + add_on_set_parameters_callback([this](const auto & p) { return on_parameter_change(p); }); +} + +Status AevaRosWrapper::declare_and_get_sensor_config_params() +{ + Aeries2Config config; + + std::string raw_sensor_model = declare_parameter("sensor_model", param_read_only()); + config.sensor_model = drivers::sensor_model_from_string(raw_sensor_model); + config.sensor_ip = declare_parameter("sensor_ip", param_read_only()); + config.frame_id = declare_parameter("frame_id", param_read_only()); + + declare_json_param("scanner.dithering_enable_ego_speed", config.tree); + declare_json_param("scanner.dithering_pattern_option", config.tree); + declare_json_param("scanner.ele_offset_rad", config.tree); + declare_json_param("scanner.enable_frame_dithering", config.tree); + declare_json_param("scanner.enable_frame_sync", config.tree); + declare_json_param("scanner.flip_pattern_vertically", config.tree); + declare_json_param("scanner.frame_sync_offset_in_ms", config.tree); + declare_json_param("scanner.frame_sync_type", config.tree); + declare_json_param("scanner.frame_synchronization_on_rising_edge", config.tree); + declare_json_param("scanner.hfov_adjustment_deg", config.tree); + declare_json_param("scanner.hfov_rotation_deg", config.tree); + declare_json_param("scanner.highlight_ROI", config.tree); + declare_json_param("scanner.horizontal_fov_degrees", config.tree); + declare_json_param("scanner.roi_az_offset_rad", config.tree); + declare_json_param("scanner.vertical_pattern", config.tree); + declare_json_param("system_config.range_modes", config.tree); + declare_json_param("system_config.sensitivity_mode", config.tree); + declare_json_param("system_config.thermal_throttling_setting", config.tree); + declare_json_param("spc_converter.discard_points_in_ambiguity_region", config.tree); + declare_json_param("spc_converter.display_all_points", config.tree); + declare_json_param("spc_converter.enable_min_range_filter", config.tree); + declare_json_param("dsp_control.second_peak_type", config.tree); + declare_json_param("dsp_control.use_foveated_velocity_bias", config.tree); + declare_json_param("dsp_control.velocity_bias_pattern_options", config.tree); + + auto new_cfg_ptr = std::make_shared(config); + return validate_and_set_config(new_cfg_ptr); +} + +Status AevaRosWrapper::validate_and_set_config( + const std::shared_ptr & new_config) +{ + if (!new_config) { + return Status::SENSOR_CONFIG_ERROR; + } + + if (new_config->sensor_model == nebula::drivers::SensorModel::UNKNOWN) { + return Status::INVALID_SENSOR_MODEL; + } + + if (new_config->frame_id.empty()) { + return Status::SENSOR_CONFIG_ERROR; + } + + if (hw_interface_) { + try { + hw_interface_->on_config_change(new_config); + } catch (const std::runtime_error & e) { + RCLCPP_ERROR_STREAM(get_logger(), "Sending configuration to sensor failed: " << e.what()); + return Status::SENSOR_CONFIG_ERROR; + } + } + + auto return_mode_opt = new_config->get_return_mode(); + + if (return_mode_opt && *return_mode_opt == drivers::ReturnMode::UNKNOWN) { + RCLCPP_ERROR_STREAM(get_logger(), "Invalid return mode"); + return Status::SENSOR_CONFIG_ERROR; + } + + if (return_mode_opt) { + decoder_.on_parameter_change(*return_mode_opt); + } + + sensor_cfg_ptr_ = new_config; + return Status::OK; +} + +rcl_interfaces::msg::SetParametersResult AevaRosWrapper::on_parameter_change( + const std::vector & p) +{ + using rcl_interfaces::msg::SetParametersResult; + Aeries2Config config = *sensor_cfg_ptr_; + + bool got_any = false; + got_any |= get_json_param(p, "scanner.dithering_enable_ego_speed", config.tree); + got_any |= get_json_param(p, "scanner.dithering_pattern_option", config.tree); + got_any |= get_json_param(p, "scanner.ele_offset_rad", config.tree); + got_any |= get_json_param(p, "scanner.enable_frame_dithering", config.tree); + got_any |= get_json_param(p, "scanner.enable_frame_sync", config.tree); + got_any |= get_json_param(p, "scanner.flip_pattern_vertically", config.tree); + got_any |= get_json_param(p, "scanner.frame_sync_offset_in_ms", config.tree); + got_any |= get_json_param(p, "scanner.frame_sync_type", config.tree); + got_any |= get_json_param(p, "scanner.frame_synchronization_on_rising_edge", config.tree); + got_any |= get_json_param(p, "scanner.hfov_adjustment_deg", config.tree); + got_any |= get_json_param(p, "scanner.hfov_rotation_deg", config.tree); + got_any |= get_json_param(p, "scanner.highlight_ROI", config.tree); + got_any |= get_json_param(p, "scanner.horizontal_fov_degrees", config.tree); + got_any |= get_json_param(p, "scanner.roi_az_offset_rad", config.tree); + got_any |= get_json_param(p, "scanner.vertical_pattern", config.tree); + got_any |= get_json_param(p, "system_config.range_modes", config.tree); + got_any |= get_json_param(p, "system_config.sensitivity_mode", config.tree); + got_any |= + get_json_param(p, "system_config.thermal_throttling_setting", config.tree); + got_any |= + get_json_param(p, "spc_converter.discard_points_in_ambiguity_region", config.tree); + got_any |= get_json_param(p, "spc_converter.display_all_points", config.tree); + got_any |= get_json_param(p, "spc_converter.enable_min_range_filter", config.tree); + got_any |= get_json_param(p, "dsp_control.second_peak_type", config.tree); + got_any |= get_json_param(p, "dsp_control.use_foveated_velocity_bias", config.tree); + got_any |= + get_json_param(p, "dsp_control.velocity_bias_pattern_options", config.tree); + + if (!got_any) { + return rcl_interfaces::build().successful(true).reason(""); + } + + auto new_cfg_ptr = std::make_shared(config); + auto status = validate_and_set_config(new_cfg_ptr); + + if (status != Status::OK) { + RCLCPP_WARN_STREAM(get_logger(), "OnParameterChange aborted: " << status); + auto result = SetParametersResult(); + result.successful = false; + result.reason = (std::stringstream() << "Invalid configuration: " << status).str(); + return result; + } + + return rcl_interfaces::build().successful(true).reason(""); +} + +void AevaRosWrapper::record_raw_packet(const std::vector & bytes) +{ + std::lock_guard lock(mtx_current_scan_msg_); + + if ( + !packets_pub_ || (packets_pub_->get_subscription_count() == 0 && + packets_pub_->get_intra_process_subscription_count() == 0)) { + return; + } + + auto packet_stamp = now(); + + if (!current_scan_msg_) { + current_scan_msg_ = std::make_unique(); + auto & header = current_scan_msg_->header; + header.frame_id = sensor_cfg_ptr_->frame_id; + header.stamp = packet_stamp; + } + + auto & packet = current_scan_msg_->packets.emplace_back(); + packet.stamp = packet_stamp; + packet.data = bytes; +} + +RCLCPP_COMPONENTS_REGISTER_NODE(AevaRosWrapper) +} // namespace nebula::ros diff --git a/nebula_ros/src/aeva/hw_monitor_wrapper.cpp b/nebula_ros/src/aeva/hw_monitor_wrapper.cpp new file mode 100644 index 000000000..1b7da1507 --- /dev/null +++ b/nebula_ros/src/aeva/hw_monitor_wrapper.cpp @@ -0,0 +1,176 @@ +// Copyright 2024 TIER IV, Inc. + +#include "nebula_ros/aeva/hw_monitor_wrapper.hpp" + +#include "nebula_ros/common/parameter_descriptors.hpp" + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace nebula::ros +{ + +using diagnostic_msgs::msg::DiagnosticStatus; +using drivers::aeva::HealthCode; +using nebula::util::get_if_exists; + +AevaHwMonitorWrapper::AevaHwMonitorWrapper( + rclcpp::Node * const parent_node, std::shared_ptr config) +: logger_(parent_node->get_logger().get_child("HwMonitor")), + parent_node_(parent_node), + config_(std::move(config)) +{ + diag_span_ = parent_node->declare_parameter("diag_span", 500, param_read_only()); + + diagnostics_pub_ = + parent_node->create_publisher("diagnostics", 10); + + diagnostics_pub_timer_ = parent_node->create_wall_timer( + std::chrono::milliseconds(diag_span_), [&]() { publishDiagnostics(); }); +} + +void AevaHwMonitorWrapper::onTelemetryFragment(const json & diff) +{ + std::scoped_lock lock(mtx_telemetry_, mtx_hardware_id_); + + // //////////////////////////////////////// + // Update aggregated telemetry tree + // //////////////////////////////////////// + + auto now = parent_node_->now(); + bool any_changed = false; + for (const auto & node : diff.items()) { + bool new_node = telemetry_.entries.count(node.key()) == 0; + bool node_changed = !new_node && telemetry_.entries.at(node.key()).values != diff[node.key()]; + + if (new_node) { + telemetry_.entries[node.key()] = {json::object(), now}; + } + + if (new_node || node_changed) { + any_changed = true; + auto & entry = telemetry_.entries[node.key()]; + entry.values.update(diff[node.key()]); + entry.last_update = now; + } + } + + if (!any_changed) { + return; + } + + // //////////////////////////////////////// + // Initialize hardware ID if unset + // //////////////////////////////////////// + + if (!hardware_id_) { + auto serial = get_if_exists(diff, {"sysinfo_main", "serial_number"}); + auto model = get_if_exists(diff, {"sysinfo_main", "platform"}); + + if (!serial || !model) return; + + hardware_id_.emplace(*model + ":" + *serial); + } +} + +void AevaHwMonitorWrapper::onHealthCodes(std::vector health_codes) +{ + std::scoped_lock lock(mtx_health_, mtx_hardware_id_); + + if (!hardware_id_) { + return; + } + + auto now = parent_node_->now(); + health_ = {std::move(health_codes), now}; +} + +void AevaHwMonitorWrapper::publishDiagnostics() +{ + std::scoped_lock lock(mtx_health_, mtx_telemetry_, mtx_hardware_id_); + + if (!hardware_id_) { + return; + } + + auto now = parent_node_->now(); + + diagnostic_msgs::msg::DiagnosticArray diagnostic_array_msg; + diagnostic_array_msg.header.stamp.sec = static_cast(now.seconds()); + diagnostic_array_msg.header.stamp.nanosec = now.nanoseconds() % 1'000'000'000; + diagnostic_array_msg.header.frame_id = config_->frame_id; + + auto diag_entry = [](const std::string & key, const std::string & value) { + diagnostic_msgs::msg::KeyValue entry{}; + entry.key = key; + entry.value = value; + return entry; + }; + + // //////////////////////////////////////// + // Add telemetry to diagnostics + // //////////////////////////////////////// + + for (const auto & entry : telemetry_.entries) { + auto & status = diagnostic_array_msg.status.emplace_back(); + status.level = DiagnosticStatus::OK; + status.hardware_id = *hardware_id_; + status.name = "aeva.telemetry." + entry.first; + + for (const auto & item : entry.second.values.items()) { + auto value = item.value(); + std::string stringified = + value.is_string() ? value.template get() : value.dump(); + status.values.emplace_back(diag_entry(item.key(), stringified)); + } + } + + // //////////////////////////////////////// + // Add health codes to diagnostics + // //////////////////////////////////////// + + auto & status = diagnostic_array_msg.status.emplace_back(); + status.level = DiagnosticStatus::OK; + status.hardware_id = *hardware_id_; + status.name = "aeva.health"; + + if (health_) { + json warning_codes = json::array(); + json error_codes = json::array(); + + for (const auto & code : health_->codes) { + if (code.is_error()) { + error_codes.push_back(code.get()); + } else { + warning_codes.push_back(code.get()); + } + } + + if (!error_codes.empty()) { + status.level = DiagnosticStatus::ERROR; + } else if (!warning_codes.empty()) { + status.level = DiagnosticStatus::WARN; + } + + status.values.emplace_back(diag_entry("warning_codes", warning_codes.dump())); + status.values.emplace_back(diag_entry("error_codes", error_codes.dump())); + } + + diagnostics_pub_->publish(diagnostic_array_msg); +} + +} // namespace nebula::ros diff --git a/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp index f459c208f..d47bdd90d 100644 --- a/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp @@ -14,6 +14,8 @@ #include "nebula_ros/continental/continental_ars548_ros_wrapper.hpp" +#include "nebula_ros/common/parameter_descriptors.hpp" + #pragma clang diagnostic ignored "-Wbitwise-instead-of-logical" namespace nebula::ros @@ -159,7 +161,7 @@ void ContinentalARS548RosWrapper::receive_packet_callback( return; } - if (!packet_queue_.try_push(std::move(msg_ptr))) { + if (!packet_queue_.tryPush(std::move(msg_ptr))) { RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 500, "Packet(s) dropped"); } } diff --git a/nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp b/nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp index 6ec611e42..7fdf9522d 100644 --- a/nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp @@ -14,6 +14,8 @@ #include "nebula_ros/continental/continental_srr520_ros_wrapper.hpp" +#include "nebula_ros/common/parameter_descriptors.hpp" + #pragma clang diagnostic ignored "-Wbitwise-instead-of-logical" namespace nebula::ros @@ -151,7 +153,7 @@ void ContinentalSRR520RosWrapper::receive_packet_callback( return; } - if (!packet_queue_.try_push(std::move(msg_ptr))) { + if (!packet_queue_.tryPush(std::move(msg_ptr))) { RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 500, "Packet(s) dropped"); } } diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp index 73869d1fd..261ba9fa2 100644 --- a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp @@ -428,7 +428,7 @@ void HesaiRosWrapper::receive_cloud_packet_callback(std::vector & packe msg_ptr->stamp.nanosec = static_cast(timestamp_ns % 1'000'000'000); msg_ptr->data.swap(packet); - if (!packet_queue_.try_push(std::move(msg_ptr))) { + if (!packet_queue_.tryPush(std::move(msg_ptr))) { RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 500, "Packet(s) dropped"); } } diff --git a/nebula_ros/src/robosense/robosense_ros_wrapper.cpp b/nebula_ros/src/robosense/robosense_ros_wrapper.cpp index 85d34bac9..235999694 100644 --- a/nebula_ros/src/robosense/robosense_ros_wrapper.cpp +++ b/nebula_ros/src/robosense/robosense_ros_wrapper.cpp @@ -307,7 +307,7 @@ void RobosenseRosWrapper::receive_cloud_packet_callback(std::vector & p msg_ptr->stamp.nanosec = static_cast(timestamp_ns % 1'000'000'000); msg_ptr->data.swap(packet); - if (!packet_queue_.try_push(std::move(msg_ptr))) { + if (!packet_queue_.tryPush(std::move(msg_ptr))) { RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 500, "Packet(s) dropped"); } } diff --git a/nebula_ros/src/velodyne/velodyne_ros_wrapper.cpp b/nebula_ros/src/velodyne/velodyne_ros_wrapper.cpp index e06535766..441a2d9f3 100644 --- a/nebula_ros/src/velodyne/velodyne_ros_wrapper.cpp +++ b/nebula_ros/src/velodyne/velodyne_ros_wrapper.cpp @@ -255,7 +255,7 @@ void VelodyneRosWrapper::receive_cloud_packet_callback(std::vector & pa msg_ptr->stamp.nanosec = static_cast(timestamp_ns % 1'000'000'000); msg_ptr->data.swap(packet); - if (!packet_queue_.try_push(std::move(msg_ptr))) { + if (!packet_queue_.tryPush(std::move(msg_ptr))) { RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 500, "Packet(s) dropped"); } } diff --git a/nebula_tests/CMakeLists.txt b/nebula_tests/CMakeLists.txt index da2481a2f..f50411055 100644 --- a/nebula_tests/CMakeLists.txt +++ b/nebula_tests/CMakeLists.txt @@ -24,6 +24,7 @@ endif() find_package(ament_cmake_auto REQUIRED) find_package(nebula_common REQUIRED) find_package(nebula_decoders REQUIRED) +find_package(nebula_hw_interfaces REQUIRED) find_package(PCL REQUIRED COMPONENTS common) find_package(rosbag2_cpp REQUIRED) find_package(diagnostic_updater REQUIRED) @@ -40,35 +41,45 @@ if(BUILD_TESTING) add_definitions(-D_SRC_CALIBRATION_DIR_PATH="${PROJECT_SOURCE_DIR}/../nebula_decoders/calibration/") set(NEBULA_TEST_INCLUDE_DIRS + ./ ${nebula_common_INCLUDE_DIRS} ${nebula_decoders_INCLUDE_DIRS} + ${nebula_hw_interfaces_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} ${rosbag2_cpp_INCLUDE_DIRS} ${diagnostic_updater_INCLUDE_DIRS} + ${nebula_tests_common_INCLUDE_DIRS} ) - set(NEBULA_TEST_LIBRARIES + set(COMMON_TEST_LIBRARIES ${nebula_common_TARGETS} ${PCL_LIBRARIES} ${rosbag2_cpp_TARGETS} ${diagnostic_updater_TARGETS} + ${nebula_tests_common} ) set(CONTINENTAL_TEST_LIBRARIES - ${NEBULA_TEST_LIBRARIES} + ${COMMON_TEST_LIBRARIES} nebula_decoders::nebula_decoders_continental ) set(HESAI_TEST_LIBRARIES - ${NEBULA_TEST_LIBRARIES} + ${COMMON_TEST_LIBRARIES} nebula_decoders::nebula_decoders_hesai ) set(VELODYNE_TEST_LIBRARIES - ${NEBULA_TEST_LIBRARIES} + ${COMMON_TEST_LIBRARIES} nebula_decoders::nebula_decoders_velodyne ) + set(AEVA_TEST_LIBRARIES + ${COMMON_TEST_LIBRARIES} + nebula_hw_interfaces::nebula_hw_interfaces_aeva + ) + + add_subdirectory(aeva) add_subdirectory(continental) add_subdirectory(hesai) add_subdirectory(velodyne) diff --git a/nebula_tests/aeva/CMakeLists.txt b/nebula_tests/aeva/CMakeLists.txt new file mode 100644 index 000000000..26d05fad6 --- /dev/null +++ b/nebula_tests/aeva/CMakeLists.txt @@ -0,0 +1,24 @@ +add_library(aeva_hw_interface_test SHARED + aeva_hw_interface_test.cpp) + +target_include_directories(aeva_hw_interface_test PUBLIC + ${NEBULA_TEST_INCLUDE_DIRS} +) + +target_link_libraries(aeva_hw_interface_test + ${AEVA_TEST_LIBRARIES} +) + +ament_add_gtest(aeva_test_main + aeva_test_main.cpp +) + +target_include_directories(aeva_test_main PUBLIC + ${PROJECT_SOURCE_DIR}/src/aeva + include + ${NEBULA_TEST_INCLUDE_DIRS} +) + +target_link_libraries(aeva_test_main +aeva_hw_interface_test +) diff --git a/nebula_tests/aeva/aeva_hw_interface_test.cpp b/nebula_tests/aeva/aeva_hw_interface_test.cpp new file mode 100644 index 000000000..c4d51d0cb --- /dev/null +++ b/nebula_tests/aeva/aeva_hw_interface_test.cpp @@ -0,0 +1,47 @@ +// Copyright 2024 TIER IV, Inc. + +#include "aeva_hw_interface_test.hpp" + +#include "../common/mock_byte_stream.hpp" +#include "../data/aeva/tcp_stream.hpp" +#include "nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/pointcloud.hpp" + +#include +#include + +#include + +#include +#include +#include + +using nebula::drivers::aeva::PointCloudMessage; +using nebula::drivers::connections::aeva::PointcloudParser; + +TEST(TestParsing, Pointcloud) // NOLINT +{ + using std::chrono_literals::operator""ms; + + auto mock_byte_stream = std::make_shared(STREAM); + PointcloudParser parser(mock_byte_stream); + std::atomic_bool done = false; + + PointcloudParser::callback_t callback = [&](const PointCloudMessage & arg) { + if (done) return; + + EXPECT_EQ(arg.header.aeva_marker, 0xAE5Au); + EXPECT_EQ(arg.header.platform, 2); + + EXPECT_GT(arg.points.size(), 0); + EXPECT_TRUE(mock_byte_stream->done()); + EXPECT_EQ(mock_byte_stream->get_read_count(), 2); + done = true; + }; + + parser.register_callback(std::move(callback)); + + mock_byte_stream->run(); + while (!done) { + std::this_thread::yield(); + } +} diff --git a/nebula_tests/aeva/aeva_hw_interface_test.hpp b/nebula_tests/aeva/aeva_hw_interface_test.hpp new file mode 100644 index 000000000..a31160f2e --- /dev/null +++ b/nebula_tests/aeva/aeva_hw_interface_test.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. diff --git a/nebula_tests/aeva/aeva_test_main.cpp b/nebula_tests/aeva/aeva_test_main.cpp new file mode 100644 index 000000000..2ceaec2d4 --- /dev/null +++ b/nebula_tests/aeva/aeva_test_main.cpp @@ -0,0 +1,21 @@ +// 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. + +#include + +int main(int argc, char * argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/nebula_tests/common/mock_byte_stream.hpp b/nebula_tests/common/mock_byte_stream.hpp new file mode 100644 index 000000000..c57cdbce1 --- /dev/null +++ b/nebula_tests/common/mock_byte_stream.hpp @@ -0,0 +1,63 @@ +// 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. + +#include + +#include + +#include +#include +#include +#include + +namespace nebula::test +{ + +class MockByteStream final : public drivers::connections::PullableByteStream +{ +public: + explicit MockByteStream(const std::vector> & stream) : stream_(stream) {} + + void read(std::vector & into, size_t n_bytes) override + { + while (!running_) { + std::this_thread::yield(); + } + read_count_++; + const auto & from = stream_[index_++]; + ASSERT_EQ(from.size(), n_bytes); + into.clear(); + into.insert(into.end(), from.cbegin(), from.cend()); + + if (index_ == stream_.size()) { + done_ = true; + index_ = 0; + } + } + + void run() { running_ = true; } + + [[nodiscard]] bool done() const { return done_; } + + [[nodiscard]] size_t get_read_count() const { return read_count_; } + +private: + const std::vector> & stream_; + std::atomic_size_t index_{0}; + std::atomic_size_t read_count_{0}; + std::atomic_bool running_{false}; + std::atomic_bool done_{false}; +}; + +} // namespace nebula::test diff --git a/nebula_tests/data/aeva/tcp_stream.hpp b/nebula_tests/data/aeva/tcp_stream.hpp new file mode 100644 index 000000000..c330416eb --- /dev/null +++ b/nebula_tests/data/aeva/tcp_stream.hpp @@ -0,0 +1,36 @@ +// 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 +#include + +const std::vector> STREAM = { + /* SomeIP Header. Payload size: bytes 4..8 (little endian, size of payload array + 12 bytes) */ + {0xfa, 0xae, 0x00, 0x00, 0x92, 0x00, 0x00, 0x00, 0xa2, 0xff, + 0x79, 0x12, 0x02, 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00}, + /* Payload. Message size: bytes 4..8 (little endian, size of payload array). + * Bytes 46..50: n_points (little endian) + */ + {0x03, 0x00, 0x78, 0x12, 0x86, 0x00, 0x00, 0x00, 0xe7, 0x35, 0x41, 0x12, 0x00, 0x84, 0xc5, + 0x15, 0x64, 0x35, 0x5b, 0x14, 0x00, 0x84, 0xc5, 0x15, 0x5a, 0xae, 0x02, 0x00, 0x00, 0x08, + 0xe7, 0x35, 0x41, 0x12, 0x00, 0x84, 0xc5, 0x15, 0xff, 0xff, 0xff, 0xff, 0x00, 0xf8, 0x7f, + 0x00, 0x04, 0x00, 0x00, 0x00, 0xf0, 0xff, 0x00, 0x00, 0x64, 0x0a, 0x01, 0x09, 0x50, 0x40, + 0x00, 0x00, 0xff, 0xd7, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0xba, 0x1f, 0xa4, 0xec, 0x3e, 0x01, 0xe9, 0xff, 0x5b, 0x45, 0x40, 0x06, + 0x21, 0x00, 0xba, 0x1f, 0xa4, 0xec, 0x91, 0x00, 0x18, 0xfd, 0x5b, 0x45, 0x88, 0x36, 0x23, + 0x00, 0x0e, 0x1e, 0x4c, 0xf3, 0x27, 0x30, 0x9e, 0x2b, 0x14, 0x16, 0xc1, 0x03, 0x01, 0x00, + 0x5e, 0x1c, 0x58, 0xe9, 0xc1, 0x3b, 0xe9, 0x08, 0x13, 0x17, 0x82, 0x32, 0x01, 0x00}, +}; diff --git a/nebula_tests/package.xml b/nebula_tests/package.xml index f593b2e01..2a762a6a2 100644 --- a/nebula_tests/package.xml +++ b/nebula_tests/package.xml @@ -15,6 +15,7 @@ diagnostic_updater nebula_common nebula_decoders + nebula_hw_interfaces rosbag2_cpp ament_cmake_gtest