From 0e28cf1987a2889ce3db94319e275be71b79acaa Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Mon, 17 Jun 2024 23:01:41 +0900 Subject: [PATCH] Prototype (outputs pointclouds but nothing more) --- .clang-tidy | 461 ++++++++++++++++++ .cspell.json | 1 + .../include/nebula_common/aeva/types.hpp | 151 ++++++ .../nebula_common/loggers/console_logger.hpp | 57 +++ .../include/nebula_common/loggers/logger.hpp | 41 ++ .../include/nebula_common/util/expected.hpp | 36 +- .../include/nebula_common/util/parsing.hpp | 13 + .../include/nebula_common/util/stream.hpp | 136 ++++++ nebula_decoders/CMakeLists.txt | 22 + .../aeva_aries2_decoder.hpp | 92 ++++ .../aeva_aries2_decoder.cpp | 88 ++++ nebula_hw_interfaces/CMakeLists.txt | 14 + .../aeva_hw_interface.hpp | 43 ++ .../connections/aeva_api.hpp | 258 ++++++++++ .../connections/byte_stream.hpp | 59 +++ .../connections/tcp_receiver.hpp | 68 +++ .../connections/udp_receiver.hpp | 63 +++ .../aeva_hw_interface.cpp | 24 + nebula_ros/CMakeLists.txt | 22 + .../nebula_ros/aeva/aeva_ros_wrapper.hpp | 60 +++ nebula_ros/src/aeva/aeva_ros_wrapper.cpp | 64 +++ nebula_tests/CMakeLists.txt | 19 +- nebula_tests/aeva/CMakeLists.txt | 24 + nebula_tests/aeva/aeva_hw_interface_test.cpp | 51 ++ nebula_tests/aeva/aeva_hw_interface_test.hpp | 13 + nebula_tests/aeva/aeva_test_main.cpp | 21 + nebula_tests/common/mock_byte_stream.hpp | 36 ++ nebula_tests/data/aeva/tcp_stream.hpp | 204 ++++++++ nebula_tests/package.xml | 1 + 29 files changed, 2127 insertions(+), 15 deletions(-) create mode 100644 .clang-tidy create mode 100644 nebula_common/include/nebula_common/aeva/types.hpp create mode 100644 nebula_common/include/nebula_common/loggers/console_logger.hpp create mode 100644 nebula_common/include/nebula_common/loggers/logger.hpp create mode 100644 nebula_common/include/nebula_common/util/parsing.hpp create mode 100644 nebula_common/include/nebula_common/util/stream.hpp create mode 100644 nebula_decoders/include/nebula_decoders/nebula_decoders_aeva/aeva_aries2_decoder.hpp create mode 100644 nebula_decoders/src/nebula_decoders_aeva/aeva_aries2_decoder.cpp create mode 100644 nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/aeva_hw_interface.hpp create mode 100644 nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/aeva_api.hpp create mode 100644 nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/connections/byte_stream.hpp create mode 100644 nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/connections/tcp_receiver.hpp create mode 100644 nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/connections/udp_receiver.hpp create mode 100644 nebula_hw_interfaces/src/nebula_aeva_hw_interfaces/aeva_hw_interface.cpp create mode 100644 nebula_ros/include/nebula_ros/aeva/aeva_ros_wrapper.hpp create mode 100644 nebula_ros/src/aeva/aeva_ros_wrapper.cpp create mode 100644 nebula_tests/aeva/CMakeLists.txt create mode 100644 nebula_tests/aeva/aeva_hw_interface_test.cpp create mode 100644 nebula_tests/aeva/aeva_hw_interface_test.hpp create mode 100644 nebula_tests/aeva/aeva_test_main.cpp create mode 100644 nebula_tests/common/mock_byte_stream.hpp create mode 100644 nebula_tests/data/aeva/tcp_stream.hpp diff --git a/.clang-tidy b/.clang-tidy new file mode 100644 index 000000000..3957aa28a --- /dev/null +++ b/.clang-tidy @@ -0,0 +1,461 @@ +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, + 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, + 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, + 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, + 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, + readability-static-definition-in-anonymous-namespace, + 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" + +HeaderFilterRegex: ^(?!\/usr)(?!\/opt) + +AnalyzeTemporaryDtors: false + +FormatStyle: none + +CheckOptions: + - key: bugprone-argument-comment.CommentBoolLiterals + value: "0" + - key: bugprone-argument-comment.CommentCharacterLiterals + value: "0" + - key: bugprone-argument-comment.CommentFloatLiterals + value: "0" + - key: bugprone-argument-comment.CommentIntegerLiterals + value: "0" + - key: bugprone-argument-comment.CommentNullPtrs + value: "0" + - key: bugprone-argument-comment.CommentStringLiterals + value: "0" + - key: bugprone-argument-comment.CommentUserDefinedLiterals + value: "0" + - key: bugprone-argument-comment.IgnoreSingleArgument + value: "0" + - key: bugprone-argument-comment.StrictMode + value: "0" + - key: bugprone-assert-side-effect.AssertMacros + value: assert + - key: bugprone-assert-side-effect.CheckFunctionCalls + value: "0" + - key: bugprone-dangling-handle.HandleClasses + value: std::basic_string_view;std::experimental::basic_string_view + - key: bugprone-dynamic-static-initializers.HeaderFileExtensions + value: ",h,hh,hpp,hxx" + - key: bugprone-exception-escape.FunctionsThatShouldNotThrow + value: "" + - key: bugprone-exception-escape.IgnoredExceptions + value: "" + - key: bugprone-misplaced-widening-cast.CheckImplicitCasts + value: "0" + - key: bugprone-not-null-terminated-result.WantToUseSafeFunctions + value: "1" + - key: bugprone-signed-char-misuse.CharTypdefsToIgnore + value: "" + - key: bugprone-sizeof-expression.WarnOnSizeOfCompareToConstant + value: "1" + - key: bugprone-sizeof-expression.WarnOnSizeOfConstant + value: "1" + - key: bugprone-sizeof-expression.WarnOnSizeOfIntegerExpression + value: "0" + - key: bugprone-sizeof-expression.WarnOnSizeOfThis + value: "1" + - key: bugprone-string-constructor.LargeLengthThreshold + value: "8388608" + - key: bugprone-string-constructor.WarnOnLargeLength + value: "1" + - key: bugprone-suspicious-enum-usage.StrictMode + value: "0" + - key: bugprone-suspicious-missing-comma.MaxConcatenatedTokens + value: "5" + - key: bugprone-suspicious-missing-comma.RatioThreshold + value: "0.200000" + - key: bugprone-suspicious-missing-comma.SizeThreshold + value: "5" + - key: bugprone-suspicious-string-compare.StringCompareLikeFunctions + value: "" + - key: bugprone-suspicious-string-compare.WarnOnImplicitComparison + value: "1" + - key: bugprone-suspicious-string-compare.WarnOnLogicalNotComparison + value: "0" + - key: bugprone-too-small-loop-variable.MagnitudeBitsUpperLimit + value: "16" + - key: bugprone-unhandled-self-assignment.WarnOnlyIfThisHasSuspiciousField + value: "1" + - key: bugprone-unused-return-value.CheckedFunctions + value: ::std::async;::std::launder;::std::remove;::std::remove_if;::std::unique;::std::unique_ptr::release;::std::basic_string::empty;::std::vector::empty + - key: cert-dcl16-c.NewSuffixes + value: L;LL;LU;LLU + - key: cert-oop54-cpp.WarnOnlyIfThisHasSuspiciousField + value: "0" + - key: cppcoreguidelines-explicit-virtual-functions.IgnoreDestructors + value: "1" + - key: cppcoreguidelines-macro-usage.AllowedRegexp + value: ^DEBUG_* + - key: cppcoreguidelines-macro-usage.CheckCapsOnly + value: "0" + - key: cppcoreguidelines-macro-usage.IgnoreCommandLineMacros + value: "1" + - key: cppcoreguidelines-no-malloc.Allocations + value: ::malloc;::calloc + - key: cppcoreguidelines-no-malloc.Deallocations + value: ::free + - key: cppcoreguidelines-no-malloc.Reallocations + value: ::realloc + - key: cppcoreguidelines-non-private-member-variables-in-classes.IgnoreClassesWithAllMemberVariablesBeingPublic + value: "1" + - key: cppcoreguidelines-pro-type-member-init.IgnoreArrays + value: "0" + - key: cppcoreguidelines-pro-type-member-init.UseAssignment + value: "0" + - key: cppcoreguidelines-special-member-functions.AllowMissingMoveFunctions + value: "0" + - key: cppcoreguidelines-special-member-functions.AllowSoleDefaultDtor + value: "0" + - key: google-readability-braces-around-statements.ShortStatementLines + value: "1" + - key: google-readability-function-size.StatementThreshold + value: "800" + - key: google-readability-namespace-comments.ShortNamespaceLines + value: "10" + - key: google-readability-namespace-comments.SpacesBeforeComments + value: "2" + - key: hicpp-multiway-paths-covered.WarnOnMissingElse + value: "0" + - key: hicpp-signed-bitwise.IgnorePositiveIntegerLiterals + value: "0" + - key: misc-definitions-in-headers.HeaderFileExtensions + value: ",h,hh,hpp,hxx" + - key: misc-definitions-in-headers.UseHeaderFileExtension + value: "1" + - key: misc-throw-by-value-catch-by-reference.CheckThrowTemporaries + value: "1" + - key: misc-unused-parameters.StrictMode + value: "0" + - key: modernize-loop-convert.MaxCopySize + value: "16" + - key: modernize-loop-convert.MinConfidence + value: reasonable + - key: modernize-loop-convert.NamingStyle + value: CamelCase + - key: modernize-make-shared.IgnoreMacros + value: "1" + - key: modernize-make-shared.IncludeStyle + value: google + - key: modernize-make-shared.MakeSmartPtrFunction + value: std::make_shared + - key: modernize-make-shared.MakeSmartPtrFunctionHeader + value: memory + - key: modernize-make-unique.IgnoreMacros + value: "1" + - key: modernize-make-unique.IncludeStyle + value: google + - key: modernize-make-unique.MakeSmartPtrFunction + value: std::make_unique + - key: modernize-make-unique.MakeSmartPtrFunctionHeader + value: memory + - key: modernize-pass-by-value.IncludeStyle + value: google + - key: modernize-pass-by-value.ValuesOnly + value: "0" + - key: modernize-raw-string-literal.ReplaceShorterLiterals + value: "0" + - key: modernize-replace-auto-ptr.IncludeStyle + value: google + - key: modernize-replace-random-shuffle.IncludeStyle + value: google + - key: modernize-use-auto.MinTypeNameLength + value: "5" + - key: modernize-use-auto.RemoveStars + value: "0" + - key: modernize-use-default-member-init.IgnoreMacros + value: "1" + - key: modernize-use-default-member-init.UseAssignment + value: "0" + - key: modernize-use-emplace.ContainersWithPushBack + value: ::std::vector;::std::list;::std::deque + - key: modernize-use-emplace.SmartPointers + value: ::std::shared_ptr;::std::unique_ptr;::std::auto_ptr;::std::weak_ptr + - key: modernize-use-emplace.TupleMakeFunctions + value: ::std::make_pair;::std::make_tuple + - key: modernize-use-emplace.TupleTypes + value: ::std::pair;::std::tuple + - key: modernize-use-equals-default.IgnoreMacros + value: "1" + - key: modernize-use-equals-delete.IgnoreMacros + value: "1" + - key: modernize-use-nodiscard.ReplacementString + value: "[[nodiscard]]" + - key: modernize-use-noexcept.ReplacementString + value: "" + - key: modernize-use-noexcept.UseNoexceptFalse + value: "1" + - key: modernize-use-nullptr.NullMacros + value: "NULL" + - key: modernize-use-override.AllowOverrideAndFinal + value: "0" + - key: modernize-use-override.FinalSpelling + value: final + - key: modernize-use-override.IgnoreDestructors + value: "0" + - key: modernize-use-override.OverrideSpelling + value: override + - key: modernize-use-transparent-functors.SafeMode + value: "0" + - key: modernize-use-using.IgnoreMacros + value: "1" + - key: performance-faster-string-find.StringLikeClasses + value: std::basic_string + - key: performance-for-range-copy.AllowedTypes + value: "" + - key: performance-for-range-copy.WarnOnAllAutoCopies + value: "0" + - key: performance-inefficient-string-concatenation.StrictMode + value: "0" + - key: performance-inefficient-vector-operation.EnableProto + value: "0" + - key: performance-inefficient-vector-operation.VectorLikeClasses + value: ::std::vector + - key: performance-move-const-arg.CheckTriviallyCopyableMove + value: "1" + - key: performance-move-constructor-init.IncludeStyle + value: google + - key: performance-no-automatic-move.AllowedTypes + value: "" + - key: performance-type-promotion-in-math-fn.IncludeStyle + value: google + - key: performance-unnecessary-copy-initialization.AllowedTypes + value: "" + - key: performance-unnecessary-value-param.AllowedTypes + value: .*Ptr;.*SharedFuture + - key: performance-unnecessary-value-param.IncludeStyle + value: google + - key: portability-simd-intrinsics.Std + value: "" + - key: portability-simd-intrinsics.Suggest + value: "0" + - key: readability-function-cognitive-complexity.IgnoreMacros + value: "1" + - key: readability-else-after-return.WarnOnUnfixable + value: "1" + - key: readability-identifier-naming.NamespaceCase + value: lower_case + - key: readability-identifier-naming.ClassCase + value: CamelCase + - key: readability-identifier-naming.PrivateMemberSuffix + value: _ + - key: readability-identifier-naming.StructCase + value: CamelCase + - key: readability-identifier-naming.FunctionCase + value: lower_case + - key: readability-identifier-naming.MethodCase + value: camelBack + - key: readability-identifier-naming.VariableCase + value: lower_case + - key: readability-identifier-naming.GlobalConstantCase + value: UPPER_CASE + - key: readability-identifier-naming.ConstexprVariableCase + value: lower_case + - key: readability-inconsistent-declaration-parameter-name.IgnoreMacros + value: "1" + - key: readability-inconsistent-declaration-parameter-name.Strict + value: "0" + - key: readability-redundant-smartptr-get.IgnoreMacros + value: "1" + - key: readability-redundant-string-init.StringNames + value: ::std::basic_string + - key: readability-simplify-subscript-expr.Types + value: ::std::basic_string;::std::basic_string_view;::std::vector;::std::array + - key: readability-static-accessed-through-instance.NameSpecifierNestingThreshold + value: "3" diff --git a/.cspell.json b/.cspell.json index 17db0b0c0..bbeb1367c 100644 --- a/.cspell.json +++ b/.cspell.json @@ -5,6 +5,7 @@ "words": [ "adctp", "Adctp", + "aeva", "applicate", "AT", "autosar", diff --git a/nebula_common/include/nebula_common/aeva/types.hpp b/nebula_common/include/nebula_common/aeva/types.hpp new file mode 100644 index 000000000..9ec48e402 --- /dev/null +++ b/nebula_common/include/nebula_common/aeva/types.hpp @@ -0,0 +1,151 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +namespace nebula::drivers::aeva +{ + +#pragma pack(push, 1) + +template +struct FixedPoint +{ + [[nodiscard]] float value() const { return value_ * std::pow(2., -1. * n_fractional_bits); } + +private: + storage_t value_; +}; + +struct SomeIpHeader +{ + uint16_t service_id; + uint16_t method_id; + uint32_t message_length; + uint16_t client_id; + uint16_t session_id; + uint8_t protocol; + uint8_t interface_version; + uint8_t message_type; + uint8_t return_code; + uint32_t tp_header_offset : 28; + uint32_t tp_header : 4; +}; + +struct MessageHeader +{ + uint8_t major_version : 4; + uint8_t minor_version : 4; + uint8_t message_type; + uint16_t sequence_id; + uint32_t message_length; + uint64_t acquisition_time_ns; + uint64_t publish_time_ns; +}; + +struct PointcloudMsgSubheaderAndMetadata +{ + uint16_t aeva_marker; + uint8_t platform; + uint8_t reserved_1; + uint16_t ns_per_index; + uint64_t first_point_timestamp_ns; + int32_t frame_sync_index; + uint32_t period_ns; + uint32_t n_entries; + uint32_t capacity; + uint16_t num_beams : 4; + uint16_t num_peaks : 2; + uint16_t range_scale : 10; + uint8_t line_index; + uint8_t max_line_index; + uint32_t face_index : 4; + uint32_t n_faces : 4; + uint32_t reserved_2 : 3; + uint32_t sensitivity_mode : 3; + uint32_t frame_parity : 1; + uint32_t reserved_3 : 1; + uint32_t window_measurement : 1; + uint32_t reserved_5 : 1; + uint32_t discard_line : 1; + uint32_t ping_pong : 1; + uint32_t reserved_6 : 12; + FixedPoint 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 PointCloudMessageFragment +{ + std::vector points; +}; + +#pragma pack(pop) + +struct EIGEN_ALIGN16 PointXYZVIRCAEDT +{ + float x; + float y; + float z; + float v; + uint8_t intensity; + uint8_t return_type; + uint16_t channel; + float azimuth; + float elevation; + float distance; + uint32_t time_stamp; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +} // namespace nebula::drivers::aeva + +POINT_CLOUD_REGISTER_POINT_STRUCT( // NOLINT + nebula::drivers::aeva::PointXYZVIRCAEDT, + (float, x, x)(float, y, y)(float, z, z)(float, v, v)(std::uint8_t, intensity, intensity)( + std::uint8_t, return_type, + return_type)(std::uint16_t, channel, channel)(float, azimuth, azimuth)( + float, elevation, elevation)(float, distance, distance)(std::uint32_t, time_stamp, time_stamp)) 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..d3a3faba3 --- /dev/null +++ b/nebula_common/include/nebula_common/loggers/logger.hpp @@ -0,0 +1,41 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include + +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/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_common/include/nebula_common/util/parsing.hpp b/nebula_common/include/nebula_common/util/parsing.hpp new file mode 100644 index 000000000..a31160f2e --- /dev/null +++ b/nebula_common/include/nebula_common/util/parsing.hpp @@ -0,0 +1,13 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. diff --git a/nebula_common/include/nebula_common/util/stream.hpp b/nebula_common/include/nebula_common/util/stream.hpp new file mode 100644 index 000000000..64c636e14 --- /dev/null +++ b/nebula_common/include/nebula_common/util/stream.hpp @@ -0,0 +1,136 @@ +// 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 std::placeholders::_1; + +class StreamElement +{ +}; + +template +class Source; + +template +class Stream; + +template +class Sink : public StreamElement +{ + friend class Source; + template + friend class Stream; + +protected: + Sink(std::function action, StreamElement & previous) + : callback_(action), previous_element_(previous) + { + } + + void call(InputT && input) { callback_(std::forward(input)); } + +private: + std::function callback_; + StreamElement & previous_element_; +}; + +template +class Stream : public StreamElement +{ + friend class Source; + +public: + template + Stream then(std::function transformation) + { + auto stream = Stream(transformation, *this); + next_callback_ = [&](OutputT && input) { stream.call(std::forward(input)); }; + return stream; + } + + Sink then(std::function action) + { + auto sink = Sink(action, *this); + next_callback_ = [&](OutputT && input) { sink.call(std::forward(input)); }; + return sink; + } + +protected: + Stream(std::function transformation, StreamElement & previous) + : callback_(transformation), + next_callback_(&Stream::noCallback), + previous_element_(previous) + { + } + + void call(InputT && input) + { + auto output = callback_(std::forward(input)); + next_callback_(std::forward(output)); + } + +private: + std::function callback_; + std::function next_callback_; + StreamElement & previous_element_; + + static void noCallback(OutputT &&) { throw std::runtime_error("Stream ended without sink"); } +}; + +template +class Source : public StreamElement +{ +public: + explicit Source(std::function)> register_callback) + : next_callback_(&Source::noCallback) + { + register_callback(std::bind(&Source::call, this, _1)); + } + + template + Stream then(std::function transformation) + { + auto stream = Stream(transformation, *this); + next_callback_ = [&](T && input) { stream.call(std::forward(input)); }; + return stream; + } + + Sink then(std::function action) + { + auto sink = Sink(action, *this); + next_callback_ = [&](T && input) { sink.call(std::forward(input)); }; + return sink; + } + +protected: + void call(T && input) { next_callback_(std::forward(input)); } + +private: + std::function next_callback_; + + static void noCallback(T &&) { throw std::runtime_error("Stream ended without sink"); } +}; + +} // namespace nebula::util diff --git a/nebula_decoders/CMakeLists.txt b/nebula_decoders/CMakeLists.txt index 7238e0d67..1bacfcb75 100644 --- a/nebula_decoders/CMakeLists.txt +++ b/nebula_decoders/CMakeLists.txt @@ -114,11 +114,32 @@ target_include_directories(nebula_decoders_continental PUBLIC ${nebula_msgs_INCLUDE_DIRS} ) +# Aeva +add_library(nebula_decoders_aeva SHARED +src/nebula_decoders_aeva/aeva_aries2_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) @@ -132,6 +153,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_aries2_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_aeva/aeva_aries2_decoder.hpp new file mode 100644 index 000000000..14d794430 --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_aeva/aeva_aries2_decoder.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 + +namespace nebula::drivers +{ +struct MinMax +{ + void update(double value) + { + min_ = std::min(min_, value); + max_ = std::max(max_, value); + } + + [[nodiscard]] std::string toString() const + { + return "min=" + std::to_string(min_) + ", max=" + std::to_string(max_); + } + +private: + double min_{1e6}, max_{-1e6}; +}; + +class AevaAries2Decoder +{ +public: + using AevaPoint = aeva::PointXYZVIRCAEDT; + using AevaPointCloud = pcl::PointCloud; + + AevaAries2Decoder() : cloud_state_({std::make_unique(), 0}) {} + + void processPointMessageHeader(const aeva::PointcloudMsgSubheaderAndMetadata & header); + + void processPointMessageFragment(const aeva::PointCloudMessageFragment & fragment); + + void registerPointCloudCallback( + std::function, uint64_t)> callback); + +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; + }; + + std::function, uint64_t)> callback_{}; + PointcloudState cloud_state_{}; + DecoderState decoder_state_{}; + + std::mutex mtx_callback_; + std::mutex mtx_decode_; +}; +} // namespace nebula::drivers diff --git a/nebula_decoders/src/nebula_decoders_aeva/aeva_aries2_decoder.cpp b/nebula_decoders/src/nebula_decoders_aeva/aeva_aries2_decoder.cpp new file mode 100644 index 000000000..e559381f3 --- /dev/null +++ b/nebula_decoders/src/nebula_decoders_aeva/aeva_aries2_decoder.cpp @@ -0,0 +1,88 @@ +// Copyright 2024 TIER IV, Inc. + +#include "nebula_decoders/nebula_decoders_aeva/aeva_aries2_decoder.hpp" + +namespace nebula::drivers +{ +void AevaAries2Decoder::processPointMessageHeader( + const aeva::PointcloudMsgSubheaderAndMetadata & header) +{ + std::lock_guard lock(mtx_decode_); + + decoder_state_ = { + header.frame_sync_index, header.ns_per_index, header.line_index, 0, + header.first_point_timestamp_ns}; +} + +void AevaAries2Decoder::processPointMessageFragment( + const aeva::PointCloudMessageFragment & fragment) +{ + std::scoped_lock lock(mtx_decode_, mtx_callback_); + + for (size_t i = 0; i < fragment.points.size(); ++i) { + const auto & raw_point = fragment.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) { + decoder_state_.absolute_time_ns += decoder_state_.time_per_marker_point_ns; + } + + if (static_cast(i) == decoder_state_.new_frame_index) { + std::cout << "Cloud: " << cloud_state_.cloud->size() << std::endl; + 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) { + decoder_state_.line_index++; + } + + if (!raw_point.valid || raw_point.signal_quality < 60) { + continue; + } + + if (cloud_state_.cloud->empty()) { + cloud_state_.timestamp = decoder_state_.absolute_time_ns; + } + + AevaPoint point; + + // static MinMax d; + // static MinMax a; + // static MinMax e; + + point.distance = raw_point.range.value(); + point.azimuth = raw_point.azimuth.value() * M_PI_2f; + point.elevation = raw_point.elevation.value() * M_PI_4f; + + // d.update(point.distance); + // a.update(point.azimuth * 180 / M_PIf); + // e.update(point.elevation * 180 / M_PIf); + + // std::cout << "D."<< d.toString() <<" A."<< a.toString() <<" E."<< e.toString() << std::endl; + + 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.v = raw_point.velocity.value(); + point.intensity = raw_point.intensity; + + point.time_stamp = decoder_state_.absolute_time_ns - cloud_state_.timestamp; + point.channel = decoder_state_.line_index; + + cloud_state_.cloud->emplace_back(point); + } +} + +void AevaAries2Decoder::registerPointCloudCallback( + std::function, uint64_t)> callback) +{ + std::lock_guard lock(mtx_callback_); + callback_ = std::move(callback); +} +} // namespace nebula::drivers diff --git a/nebula_hw_interfaces/CMakeLists.txt b/nebula_hw_interfaces/CMakeLists.txt index 631d345dd..e17a7ec67 100644 --- a/nebula_hw_interfaces/CMakeLists.txt +++ b/nebula_hw_interfaces/CMakeLists.txt @@ -88,10 +88,23 @@ target_include_directories(nebula_hw_interfaces_continental PUBLIC ${nebula_msgs_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) @@ -104,6 +117,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..9ef8a4ff6 --- /dev/null +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/aeva_hw_interface.hpp @@ -0,0 +1,43 @@ +// 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_stream.hpp" + +#include + +#include +#include + +namespace nebula::drivers +{ + +class AevaHwInterface +{ +public: + AevaHwInterface( + std::shared_ptr logger, + std::unique_ptr pointcloud_stream) + : logger_(std::move(logger)), pointcloud_api_(std::move(pointcloud_stream)) + { + } + +private: + std::shared_ptr logger_; + connections::AevaParser pointcloud_api_; +}; + +} // 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..91311b3df --- /dev/null +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/aeva_api.hpp @@ -0,0 +1,258 @@ +// 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/aeva/types.hpp" +#include "nebula_common/util/expected.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 +#include +#include +#include + +namespace nebula::drivers::connections +{ + +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_; +}; + +static const uint16_t AEVA_HEADER = 0xAEFA; + +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); +} + +struct ByteView +{ +private: + using iter_t = std::vector::const_iterator; + std::deque> buffers_; + iter_t begin_; + +public: + template + using consume_result_t = nebula::util::expected; + + [[nodiscard]] size_t size() const + { + if (buffers_.empty()) { + return 0; + } + + size_t size = 0; + for (const auto & buf : buffers_) { + size += buf.size(); + } + + size -= std::distance(begin_, buffers_.front().cbegin()); + + return size; + } + + void enqueue(std::vector buffer) + { + auto & enqueued_buffer = buffers_.emplace_back(); + enqueued_buffer.swap(buffer); + + if (buffers_.size() == 1) { + begin_ = enqueued_buffer.cbegin(); + } + } + + template + [[nodiscard]] consume_result_t tryConsume() + { + size_t bytes_to_copy = sizeof(T); + if (bytes_to_copy > size()) { + return MismatchError( + "Tried to consume more bytes than available", static_cast(size()), + static_cast(bytes_to_copy)); + } + + T result{}; + auto * result_ptr = static_cast(static_cast(&result)); + + size_t bytes_copied = 0; + while (bytes_to_copy > 0) { + size_t bytes_in_current_buffer = std::distance(begin_, buffers_.front().cend()); + size_t bytes_to_copy_from_current_buffer = std::min(bytes_in_current_buffer, bytes_to_copy); + memcpy(result_ptr + bytes_copied, &*begin_, bytes_to_copy_from_current_buffer); + + std::advance(begin_, bytes_to_copy_from_current_buffer); + bytes_copied += bytes_to_copy_from_current_buffer; + bytes_to_copy -= bytes_to_copy_from_current_buffer; + if (begin_ == buffers_.front().cend()) { + buffers_.pop_front(); + begin_ = buffers_.front().cbegin(); + } + } + + return result; + } +}; + +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::PointCloudMessageFragment; +using nebula::drivers::aeva::PointcloudMsgSubheaderAndMetadata; +using nebula::drivers::aeva::PointCloudPoint; +using nebula::drivers::aeva::SomeIpHeader; + +template +T pull_and_parse(PullableByteStream & stream) +{ + auto buffer = stream.read(sizeof(T)); + T result{}; + + memcpy(&result, buffer.data(), sizeof(T)); + return result; +} + +/** + * @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. + */ +class AevaParser +{ +public: + using header_callback_t = std::function; + using fragment_callback_t = std::function; + + explicit AevaParser(std::shared_ptr incoming_byte_stream) + : incoming_(std::move(incoming_byte_stream)) + { + thread_ = std::thread([&]() { + while (true) receiveApiMessage(); + }); + } + + void registerCallbacks(header_callback_t header_callback, fragment_callback_t fragment_callback) + { + cb_header_ = std::move(header_callback); + cb_fragment_ = std::move(fragment_callback); + } + +private: + void receiveApiMessage() + { + auto some_ip = pull_and_parse(*incoming_); + expect_eq(some_ip.service_id, 0xAEFAu, "Aeva service header mismatch"); + expect_eq( + some_ip.method_id, AevaStreamType::kSphericalPointCloud, "Service is not pointcloud service"); + auto payload_bytes = some_ip.message_length - 12; + + auto message_header = pull_and_parse(*incoming_); + expect_eq( + message_header.message_type, AevaStreamType::kSphericalPointCloud, + "Message type is not pointcloud type"); + expect_eq(message_header.message_length, payload_bytes, "Payload size mismatch"); + + auto pointcloud_header = pull_and_parse(*incoming_); + expect_eq(pointcloud_header.aeva_marker, 0xAE5Au, "Pointcloud Aeva marker mismatch"); + expect_eq( + pointcloud_header.n_entries * sizeof(PointCloudPoint), + payload_bytes - sizeof(MessageHeader) - sizeof(PointcloudMsgSubheaderAndMetadata), + "Payload bytes and point bytes mismatch"); + + if (cb_header_) { + cb_header_(pointcloud_header); + } + + PointCloudMessageFragment fragment{}; + fragment.points.reserve(pointcloud_header.n_entries); + for (size_t i = 0; i < pointcloud_header.n_entries; ++i) { + auto point = pull_and_parse(*incoming_); + fragment.points.emplace_back(point); + } + + if (cb_fragment_) { + cb_fragment_(fragment); + } + } + + std::shared_ptr incoming_; + std::thread thread_; + + header_callback_t cb_header_{}; + fragment_callback_t cb_fragment_{}; +}; + +} // namespace nebula::drivers::connections 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..f870d0806 --- /dev/null +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/connections/byte_stream.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 +#include + +namespace nebula::drivers::connections +{ + +class ObservableByteStream +{ +public: + using callback_t = typename std::function & buffer)>; + + ObservableByteStream() = default; + + ObservableByteStream(const ObservableByteStream &) = default; + ObservableByteStream(ObservableByteStream &&) = delete; + ObservableByteStream & operator=(const ObservableByteStream &) = default; + ObservableByteStream & operator=(ObservableByteStream &&) = delete; + + virtual ~ObservableByteStream() = default; + + virtual void registerCallback(callback_t callback) = 0; +}; + +class PullableByteStream +{ +public: + PullableByteStream() = default; + + PullableByteStream(const PullableByteStream &) = default; + PullableByteStream(PullableByteStream &&) = delete; + PullableByteStream & operator=(const PullableByteStream &) = default; + PullableByteStream & operator=(PullableByteStream &&) = delete; + + virtual ~PullableByteStream() = default; + + virtual std::vector read(size_t n_bytes) = 0; +}; + +} // 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..48d5d84eb --- /dev/null +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/connections/tcp_receiver.hpp @@ -0,0 +1,68 @@ +// 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 +#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) + : io_service_(1), socket_(io_service_) + { + 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); + } + + std::vector read(size_t n_bytes) override + { + std::vector buffer; + buffer.resize(n_bytes); + boost::asio::read(socket_, boost::asio::buffer(buffer), boost::asio::transfer_exactly(n_bytes)); + return buffer; + } + +private: + boost::asio::io_service io_service_; + boost::asio::ip::tcp::socket socket_; +}; + +} // namespace nebula::drivers::connections diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/connections/udp_receiver.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/connections/udp_receiver.hpp new file mode 100644 index 000000000..51f74040e --- /dev/null +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/connections/udp_receiver.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. + +#pragma once + +#include "byte_stream.hpp" + +#include + +#include +#include +#include +#include +#include + +namespace nebula::drivers::connections +{ + +class UdpReceiver : public ObservableByteStream +{ +public: + /// @brief Create a receiving UDP connection and forward received packets to the registered + /// callback (if any) + UdpReceiver(const std::string & host_ip, uint16_t host_port) : ctx_(1), udp_driver_(ctx_) + { + try { + udp_driver_.init_receiver(host_ip, host_port); + udp_driver_.receiver()->open(); + udp_driver_.receiver()->bind(); + + udp_driver_.receiver()->asyncReceive([&](const auto & bytes) { onReceive(bytes); }); + } catch (const std::exception & ex) { + throw std::runtime_error(std::string("Could not open UDP socket: ") + ex.what()); + } + } + + void registerCallback(callback_t callback) override { callback_ = std::move(callback); } + +private: + void onReceive(const std::vector & bytes) + { + if (callback_) { + callback_(bytes); + } + } + + ::drivers::common::IoContext ctx_; + ::drivers::udp_driver::UdpDriver udp_driver_; + callback_t callback_; +}; + +} // 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..d5fd215e0 --- /dev/null +++ b/nebula_hw_interfaces/src/nebula_aeva_hw_interfaces/aeva_hw_interface.cpp @@ -0,0 +1,24 @@ +// 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 "nebula_hw_interfaces/nebula_hw_interfaces_aeva/aeva_hw_interface.hpp" + +#include + +#include + +namespace nebula::drivers +{ + +} // namespace nebula::drivers diff --git a/nebula_ros/CMakeLists.txt b/nebula_ros/CMakeLists.txt index 45cd6262b..b763d1b33 100644 --- a/nebula_ros/CMakeLists.txt +++ b/nebula_ros/CMakeLists.txt @@ -173,10 +173,31 @@ rclcpp_components_register_node(continental_ars548_ros_wrapper EXECUTABLE continental_ars548_ros_wrapper_node ) +## Aeva +add_library(aeva_ros_wrapper SHARED + src/aeva/aeva_ros_wrapper.cpp +) + +target_include_directories(aeva_ros_wrapper PUBLIC + ${nebula_decoders_INCLUDE_DIRS} + ${nebula_hw_interfaces_INCLUDE_DIRS} +) + +target_link_libraries(aeva_ros_wrapper PUBLIC + 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 aeva_ros_wrapper EXPORT export_aeva_ros_wrapper) install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) if(BUILD_TESTING) @@ -189,6 +210,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/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..42f21deb3 --- /dev/null +++ b/nebula_ros/include/nebula_ros/aeva/aeva_ros_wrapper.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_ros/common/mt_queue.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace nebula::ros +{ + +/// @brief Ros wrapper of hesai driver +class AevaRosWrapper final : public rclcpp::Node +{ +public: + explicit AevaRosWrapper(const rclcpp::NodeOptions & options); + +private: + rclcpp::Publisher::SharedPtr cloud_pub_{}; + + std::shared_ptr receiver_; + drivers::connections::AevaParser parser_; + drivers::AevaAries2Decoder decoder_; +}; + +} // namespace nebula::ros 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..3ddec1dcf --- /dev/null +++ b/nebula_ros/src/aeva/aeva_ros_wrapper.cpp @@ -0,0 +1,64 @@ +// Copyright 2024 TIER IV, Inc. + +#include "nebula_ros/aeva/aeva_ros_wrapper.hpp" + +#include "nebula_decoders/nebula_decoders_aeva/aeva_aries2_decoder.hpp" +#include "nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/aeva_api.hpp" +#include "nebula_hw_interfaces/nebula_hw_interfaces_common/connections/tcp_receiver.hpp" + +#include +#include + +#include + +#include +#include + +#include +#include +#include +#include + +#pragma clang diagnostic ignored "-Wbitwise-instead-of-logical" + +namespace nebula::ros +{ +AevaRosWrapper::AevaRosWrapper(const rclcpp::NodeOptions & options) +: rclcpp::Node("velodyne_ros_wrapper", rclcpp::NodeOptions(options).use_intra_process_comms(true)), + receiver_(new drivers::connections::TcpStream("192.168.2.201", 41000)), + parser_(receiver_) +{ + auto qos_profile = rmw_qos_profile_sensor_data; + auto pointcloud_qos = + rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), qos_profile); + + cloud_pub_ = create_publisher("/aeva_points", pointcloud_qos); + + drivers::connections::AevaParser::header_callback_t header_cb = [this](const auto & header) { + decoder_.processPointMessageHeader(header); + }; + + drivers::connections::AevaParser::fragment_callback_t fragment_cb = + [this](const auto & fragment) { decoder_.processPointMessageFragment(fragment); }; + + std::function, uint64_t)> + pointcloud_cb = + [this]( + std::unique_ptr cloud_ptr, uint64_t timestamp) { + 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 = "aeva"; + ros_pc_msg_ptr->header.stamp = rclcpp::Time(static_cast(timestamp)); + cloud_pub_->publish(std::move(ros_pc_msg_ptr)); + } + }; + + decoder_.registerPointCloudCallback(pointcloud_cb); + parser_.registerCallbacks(std::move(header_cb), std::move(fragment_cb)); +} + +RCLCPP_COMPONENTS_REGISTER_NODE(AevaRosWrapper) +} // namespace nebula::ros diff --git a/nebula_tests/CMakeLists.txt b/nebula_tests/CMakeLists.txt index 4753586da..2430a6b69 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) @@ -39,33 +40,43 @@ 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} + ${nebula_tests_common_INCLUDE_DIRS} ) - set(NEBULA_TEST_LIBRARIES + set(COMMON_TEST_LIBRARIES ${nebula_common_TARGETS} ${PCL_LIBRARIES} ${rosbag2_cpp_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..fc50e769a --- /dev/null +++ b/nebula_tests/aeva/aeva_hw_interface_test.cpp @@ -0,0 +1,51 @@ +// 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_common/connections/byte_stream.hpp" + +#include +#include + +#include + +using nebula::drivers::aeva::PointCloudMessageFragment; +using nebula::drivers::aeva::PointcloudMsgSubheaderAndMetadata; +using nebula::drivers::connections::AevaParser; + +TEST(TestParsing, Pointcloud) // NOLINT +{ + size_t n_calls_cb_header = 0; + size_t n_calls_cb_fragment = 0; + + AevaParser::header_callback_t cb_header = [&](const PointcloudMsgSubheaderAndMetadata & arg) { + EXPECT_EQ(n_calls_cb_header, 0); + EXPECT_EQ(n_calls_cb_fragment, 0); + EXPECT_EQ(arg.aeva_marker, 0xAE5Au); + EXPECT_EQ(arg.platform, 2); + + n_calls_cb_header++; + }; + + AevaParser::fragment_callback_t cb_fragment = [&](const PointCloudMessageFragment & arg) { + EXPECT_EQ(n_calls_cb_header, 1); + EXPECT_EQ(n_calls_cb_fragment, 0); + + EXPECT_GT(arg.points.size(), 0); + for (const auto & p : arg.points) { + std::cout << p.azimuth.value() * 90 << "\t" << p.elevation.value() * 45 << std::endl; + } + + n_calls_cb_fragment++; + }; + + auto mock_byte_stream = std::make_shared(STREAM); + + nebula::drivers::connections::AevaParser parser(mock_byte_stream); + parser.registerCallbacks(std::move(cb_header), std::move(cb_fragment)); + + EXPECT_EQ(n_calls_cb_header, 1); + EXPECT_GT(n_calls_cb_fragment, 0); +} 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..ad4034b2c --- /dev/null +++ b/nebula_tests/common/mock_byte_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. + +#include + +#include +#include +#include + +namespace nebula::test +{ + +class MockByteStream final : public drivers::connections::PullableByteStream +{ +public: + explicit MockByteStream(const std::vector> & stream) : stream_(stream) {} + + std::vector read(size_t /* n_bytes */) override { return stream_[index_++]; } + +private: + const std::vector> & stream_; + size_t index_{}; +}; + +} // 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..50cdb1532 --- /dev/null +++ b/nebula_tests/data/aeva/tcp_stream.hpp @@ -0,0 +1,204 @@ +// 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 = { + {/* Packet 70 */ + 0xfa, 0xae, 0x00, 0x00, 0x6c, 0xd0, 0x02, 0x00, 0xa2, 0xff, 0x79, 0x12, 0x02, 0x01, 0x01, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x03, 0x00, 0x78, 0x12, 0x60, 0xd0, 0x02, 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, 0x6f, 0x33, 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, 0x12, 0x00, 0xaa, 0x1a, 0xf8, 0xef, 0x00, 0x00, + 0x8e, 0x02, 0x46, 0x13, 0x83, 0x36, 0x02, 0x00, 0xaa, 0x1a, 0xf8, 0xef, 0x00, 0x00, 0x8c, 0x02, + 0x46, 0x13, 0x8b, 0x36, 0x04, 0x00, 0xc8, 0x1f, 0xa4, 0xec, 0x45, 0x01, 0x05, 0x00, 0x5d, 0x4a, + 0x40, 0x06, 0x21, 0x00, 0xc8, 0x1f, 0xa4, 0xec, 0x8c, 0x00, 0x02, 0xfd, 0x5d, 0x4a, 0x88, 0x36, + 0x23, 0x00, 0xb8, 0x1a, 0xf8, 0xef, 0x00, 0x00, 0x92, 0x02, 0x4a, 0x19, 0x83, 0x36, 0x02, 0x00, + 0xb8, 0x1a, 0xf8, 0xef, 0x00, 0x00, 0x90, 0x02, 0x4a, 0x19, 0x8b, 0x36, 0x04, 0x00, 0xd0, 0x1f, + 0xa4, 0xec, 0x4b, 0x01, 0x1a, 0x00, 0x49, 0x31, 0x00, 0x37, 0x12, 0x00, 0xd0, 0x1f, 0xa4, 0xec, + 0x85, 0x00, 0xe1, 0xfc, 0x49, 0x31, 0x88, 0x37, 0x14, 0x00, 0x24, 0x1e, 0x4c, 0xf3, 0x01, 0x00, + 0x1a, 0x02, 0x50, 0x25, 0x81, 0x36, 0x12, 0x00, 0x24, 0x1e, 0x4c, 0xf3, 0x01, 0x00, 0x1a, 0x02, + 0x50, 0x25, 0x89, 0x36, 0x14, 0x00, 0xc0, 0x1a, 0xf8, 0xef, 0x91, 0x00, 0xe6, 0xfd, 0x43, 0x13, + 0x03, 0x03, 0x00, 0x01, 0xdc, 0x1f, 0xa4, 0xec, 0x44, 0x01, 0xe6, 0xff, 0x47, 0x2b, 0x40, 0x05, + 0x11, 0x00, 0xdc, 0x1f, 0xa4, 0xec, 0x97, 0x00, 0x09, 0xfd, 0x47, 0x2b, 0x88, 0x35, 0x13, 0x00, + 0x30, 0x1e, 0x4c, 0xf3, 0x03, 0x00, 0x24, 0x02, 0x49, 0x1b, 0x81, 0x35, 0x12, 0x00, 0x30, 0x1e, + 0x4c, 0xf3, 0x03, 0x00, 0x24, 0x02, 0x49, 0x1b, 0x89, 0x35, 0x14, 0x00, 0x80, 0x1c, 0x58, 0xe9, + 0xb0, 0x00, 0x1a, 0x02, 0x3a, 0x1e, 0x02, 0x01, 0x00, 0x01, 0xcc, 0x1a, 0xf8, 0xef, 0xa2, 0x00, + 0xe6, 0xfd, 0x40, 0x0b, 0x03, 0x03, 0x00, 0x01, 0xe6, 0x1f, 0xa8, 0xec, 0x43, 0x01, 0xe6, 0xff, + 0x57, 0x3f, 0x40, 0x05, 0x21, 0x00, 0xe6, 0x1f, 0xa8, 0xec, 0x96, 0x00, 0x06, 0xfd, 0x57, 0x3f, + 0x88, 0x35, 0x23, 0x00, 0x3a, 0x1e, 0x50, 0xf3, 0x07, 0x00, 0x12, 0x02, 0x4a, 0x1c, 0x81, 0x35, + 0x02, 0x00, 0x3a, 0x1e, 0x50, 0xf3, 0x00, 0x00, 0x2c, 0x02, 0x4a, 0x1c, 0x89, 0x35, 0x04, 0x00, + 0x8a, 0x1c, 0x5c, 0xe9, 0x8d, 0x00, 0xe6, 0xfd, 0x56, 0x25, 0x02, 0x03, 0x10, 0x01, 0xd6, 0x1a, + 0xfc, 0xef, 0xc9, 0x00, 0x1a, 0x02, 0x42, 0x2b, 0x03, 0x01, 0x20, 0x01, 0xf0, 0x1f, 0xa8, 0xec, + 0x41, 0x01, 0xef, 0xff, 0x59, 0x40, 0x40, 0x04, 0x21, 0x00, 0xf0, 0x1f, 0xa8, 0xec, 0x97, 0x00, + 0x01, 0xfd, 0x57, 0x40, 0x88, 0x34, 0x23, 0x00, 0x44, 0x1e, 0x50, 0xf3, 0x06, 0x00, 0x14, 0x02, + 0x4c, 0x1f, 0x81, 0x35, 0x02, 0x00, 0x44, 0x1e, 0x50, 0xf3, 0x01, 0x00, 0x2b, 0x02, 0x4c, 0x1f, + 0x89, 0x35, 0x04, 0x00, 0x94, 0x1c, 0x5c, 0xe9, 0xb3, 0x00, 0x1a, 0x02, 0x3b, 0x20, 0x02, 0x01, + 0x00, 0x01, 0xe0, 0x1a, 0xfc, 0xef, 0xc4, 0x00, 0x1a, 0x02, 0x42, 0x2a, 0x03, 0x00, 0x00, 0x01, + 0xfc, 0x1f, 0xa8, 0xec, 0x41, 0x01, 0xee, 0xff, 0x59, 0x3e, 0x40, 0x04, 0x21, 0x00, 0xfc, 0x1f, + 0xa8, 0xec, 0x92, 0x00, 0x0e, 0xfd, 0x56, 0x3e, 0x88, 0x34, 0x23, 0x00, 0x50, 0x1e, 0x50, 0xf3, + 0x07, 0x00, 0x15, 0x02, 0x4a, 0x1c, 0x81, 0x35, 0x02, 0x00, 0x50, 0x1e, 0x50, 0xf3, 0x00, 0x00, + 0x2f, 0x02, 0x4a, 0x1c, 0x89, 0x35, 0x04, 0x00, 0xa0, 0x1c, 0x5c, 0xe9, 0x8e, 0x00, 0xe6, 0xfd, + 0x56, 0x25, 0x02, 0x03, 0x00, 0x01, 0xec, 0x1a, 0xfc, 0xef, 0xc4, 0x00, 0x1a, 0x02, 0x47, 0x31, + 0x03, 0x01, 0x20, 0x01, 0x08, 0x20, 0xa8, 0xec, 0x40, 0x01, 0xea, 0xff, 0x59, 0x40, 0x40, 0x04, + 0x21, 0x00, 0x08, 0x20, 0xa8, 0xec, 0x97, 0x00, 0x19, 0xfd, 0x59, 0x40, 0x88, 0x34, 0x23, 0x00, + 0x5c, 0x1e, 0x50, 0xf3, 0x14, 0x3d, 0x1a, 0x02, 0x0d, 0x19, 0x01, 0x00, 0x20, 0x01, 0xac, 0x1c, + 0x5c, 0xe9, 0xc1, 0x00, 0x1a, 0x02, 0x3d, 0x22, 0x02, 0x00, 0x20, 0x01, 0xf8, 0x1a, 0xfc, 0xef, + 0xc1, 0x00, 0x1a, 0x02, 0x4c, 0x37, 0x03, 0x01, 0x20, 0x01, 0x10, 0x20, 0xa8, 0xec, 0x40, 0x01, + 0xea, 0xff, 0x59, 0x44, 0x40, 0x04, 0x21, 0x00, 0x10, 0x20, 0xa8, 0xec, 0x95, 0x00, 0x12, 0xfd, + 0x5b, 0x44, 0x88, 0x34, 0x23, 0x00, 0x64, 0x1e, 0x50, 0xf3, 0x8d, 0x35, 0x36, 0xe1, 0x13, 0x1a, + 0x81, 0x31, 0x12, 0x00, 0xb4, 0x1c, 0x5c, 0xe9, 0xc3, 0x00, 0x1a, 0x02, 0x3d, 0x22, 0x02, 0x00, + 0x20, 0x01, 0x00, 0x1b, 0xfc, 0xef, 0xc2, 0x00, 0x1a, 0x02, 0x55, 0x43, 0x03, 0x00, 0x20, 0x01, + 0x1c, 0x20, 0xa8, 0xec, 0x40, 0x01, 0xed, 0xff, 0x58, 0x45, 0x40, 0x04, 0x21, 0x00, 0x1c, 0x20, + 0xa8, 0xec, 0x95, 0x00, 0x20, 0xfd, 0x5c, 0x45, 0x88, 0x34, 0x23, 0x00, 0x70, 0x1e, 0x50, 0xf3, + 0x91, 0x35, 0x2a, 0xe1, 0x13, 0x1a, 0x81, 0x30, 0x32, 0x00, 0xc0, 0x1c, 0x5c, 0xe9, 0xc9, 0x00, + 0x1a, 0x02, 0x3d, 0x23, 0x02, 0x00, 0x20, 0x01, 0x0c, 0x1b, 0xfc, 0xef, 0xbf, 0x00, 0x1a, 0x02, + 0x57, 0x45, 0x03, 0x00, 0x20, 0x01, 0x26, 0x20, 0xa8, 0xec, 0x40, 0x01, 0xee, 0xff, 0x59, 0x43, + 0x40, 0x04, 0x21, 0x00, 0x26, 0x20, 0xa8, 0xec, 0x97, 0x00, 0x29, 0xfd, 0x5b, 0x43, 0x88, 0x34, + 0x23, 0x00, 0x7a, 0x1e, 0x50, 0xf3, 0x19, 0x35, 0x1a, 0xe3, 0x13, 0x18, 0xc1, 0x01, 0x11, 0x00, + 0xca, 0x1c, 0x5c, 0xe9, 0xc1, 0x00, 0x1a, 0x02, 0x3c, 0x21, 0x02, 0x00, 0x20, 0x01, 0x16, 0x1b, + 0xfc, 0xef, 0xbd, 0x00, 0x1a, 0x02, 0x55, 0x42, 0x03, 0x00, 0x20, 0x01, 0x32, 0x20, 0xa8, 0xec, + 0x3d, 0x01, 0x09, 0x00, 0x44, 0x27, 0x00, 0x34, 0x12, 0x00, 0x32, 0x20, 0xa8, 0xec, 0x8e, 0x00, + 0x23, 0xfd, 0x44, 0x27, 0x88, 0x34, 0x14, 0x00, 0x86, 0x1e, 0x50, 0xf3, 0xa4, 0x3c, 0x18, 0x03, + 0x0f, 0x17, 0x81, 0x31, 0x02, 0x00, 0x22, 0x1b, 0xfc, 0xef, 0xc6, 0x00, 0x1a, 0x02, 0x3d, 0x25, + 0x03, 0x00, 0x20, 0x01, 0x3c, 0x20, 0xa8, 0xec, 0x43, 0x01, 0xe6, 0xff, 0x55, 0x28, 0x40, 0x07, + 0x21, 0x00, 0x3c, 0x20, 0xa8, 0xec, 0x98, 0x00, 0xef, 0xfc, 0x44, 0x28, 0x88, 0x37, 0x23, 0x00, + 0x2c, 0x1b, 0xfc, 0xef, 0xbd, 0x00, 0x1a, 0x02, 0x39, 0x22, 0x03, 0x01, 0x20, 0x01, 0x46, 0x20, + 0xa8, 0xec, 0x42, 0x01, 0xe8, 0xff, 0x5a, 0x43, 0x40, 0x07, 0x21, 0x00, 0x46, 0x20, 0xa8, 0xec, + 0x94, 0x00, 0xf8, 0xfc, 0x58, 0x43, 0x88, 0x37, 0x23, 0x00, 0x9a, 0x1e, 0x50, 0xf3, 0x15, 0x3e, + 0xe6, 0xfd, 0x0a, 0x15, 0x01, 0x03, 0x00, 0x01, 0x36, 0x1b, 0xfc, 0xef, 0xcc, 0x00, 0x1a, 0x02, + 0x45, 0x34, 0x03, 0x01, 0x20, 0x01, 0x52, 0x20, 0xa8, 0xec, 0x42, 0x01, 0xe8, 0xff, 0x5b, 0x40, + 0x40, 0x06, 0x21, 0x00, 0x52, 0x20, 0xa8, 0xec, 0x8f, 0x00, 0x1b, 0xfd, 0x56, 0x40, 0x88, 0x36, + 0x23, 0x00, 0xa6, 0x1e, 0x50, 0xf3, 0x17, 0x3e, 0xe6, 0xfd, 0x0a, 0x15, 0x01, 0x02, 0x10, 0x01, + 0x5c, 0x20, 0xa8, 0xec, 0x41, 0x01, 0xe7, 0xff, 0x5a, 0x46, 0x40, 0x06, 0x21, 0x00, 0x5c, 0x20, + 0xa8, 0xec, 0x93, 0x00, 0x02, 0xfd, 0x5b, 0x46, 0x88, 0x36, 0x23, 0x00, 0x64, 0x20, 0xa8, 0xec, + 0x42, 0x01, 0xe8, 0xff, 0x5b, 0x47, 0x40, 0x06, 0x21, 0x00, 0x64, 0x20, 0xa8, 0xec, 0x92, 0x00, + 0x01, 0xfd, 0x5b, 0x47, 0x88, 0x36, 0x23, 0x00, 0x54, 0x1b, 0xfc, 0xef, 0xc8, 0x00, 0x1a, 0x02, + 0x42, 0x2f, 0x03, 0x01, 0x20, 0x01, 0x70, 0x20, 0xa8, 0xec, 0x43, 0x01, 0xec, 0xff, 0x59, 0x4c, + 0x40, 0x06, 0x21, 0x00, 0x70, 0x20, 0xa8, 0xec, 0x94, 0x00, 0xfd, 0xfc, 0x5f, 0x4c, 0x88, 0x36, + 0x23, 0x00, 0x60, 0x1b, 0xfc, 0xef, 0xc3, 0x00, 0x1a, 0x02, 0x40, 0x2d, 0x03, 0x00, 0x20, 0x01, + 0x78, 0x20, 0xa8, 0xec, 0x42, 0x01, 0xeb, 0xff, 0x59, 0x47, 0x40, 0x06, 0x21, 0x00, 0x78, 0x20, + 0xa8, 0xec, 0x93, 0x00, 0xfb, 0xfc, 0x5c, 0x47, 0x88, 0x36, 0x23, 0x00, 0x68, 0x1b, 0xfc, 0xef, + 0xcf, 0x00, 0x1a, 0x02, 0x50, 0x42, 0x03, 0x00, 0x20, 0x01, 0x86, 0x20, 0xa8, 0xec, 0x42, 0x01, + 0xe8, 0xff, 0x58, 0x45, 0x40, 0x06, 0x20, 0x00, 0xda, 0x1e, 0x50, 0xf3, 0xff, 0xff, 0x67, 0x02, + 0x4c, 0x20, 0x81, 0x37, 0x02, 0x00, 0xda, 0x1e, 0x50, 0xf3, 0xff, 0xff, 0x67, 0x02, 0x4c, 0x20, + 0x89, 0x37, 0x04, 0x00, 0x76, 0x1b, 0xfc, 0xef, 0xcb, 0x00, 0x1a, 0x02, 0x51, 0x44, 0x03, 0x00, + 0x20, 0x01, 0x90, 0x20, 0xa8, 0xec, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, + 0xe4, 0x1e, 0x50, 0xf3, 0x04, 0x00, 0x1a, 0x02, 0x50, 0x25, 0x81, 0x36, 0x12, 0x00, 0xe4, 0x1e, + 0x50, 0xf3, 0x01, 0x00, 0x29, 0x02, 0x50, 0x25, 0x89, 0x36, 0x14, 0x00, 0x80, 0x1b, 0xfc, 0xef, + 0x02, 0x00, 0x87, 0x02, 0x53, 0x25, 0x83, 0x36, 0x12, 0x00, 0x80, 0x1b, 0xfc, 0xef, 0x03, 0x00, + 0x89, 0x02, 0x53, 0x25, 0x8b, 0x36, 0x14, 0x00, 0x9c, 0x20, 0xa8, 0xec, 0x00, 0x00, 0x1a, 0x02, + 0x46, 0x1b, 0x00, 0x01, 0x10, 0x01, 0xf0, 0x1e, 0x50, 0xf3, 0x06, 0x00, 0x19, 0x02, 0x4a, 0x1c, + 0x81, 0x35, 0x12, 0x00, 0xf0, 0x1e, 0x50, 0xf3, 0x00, 0x00, 0x30, 0x02, 0x4a, 0x1c, 0x89, 0x35, + 0x14, 0x00, 0x40, 0x1d, 0x5c, 0xe9, 0x01, 0x00}, + {/* Packet 72 */ + 0x4a, 0x02, 0x4d, 0x19, 0x82, 0x35, 0x02, 0x00, 0x40, 0x1d, 0x5c, 0xe9, 0x02, 0x00, 0x4d, 0x02, + 0x4d, 0x19, 0x8a, 0x35, 0x04, 0x00, 0x8c, 0x1b, 0xfc, 0xef, 0x00, 0x00, 0x85, 0x02, 0x4c, 0x1d, + 0x83, 0x35, 0x12, 0x00, 0x8c, 0x1b, 0xfc, 0xef, 0x01, 0x00, 0x87, 0x02, 0x4c, 0x1d, 0x8b, 0x35, + 0x14, 0x00, 0xa6, 0x20, 0xa8, 0xec, 0x41, 0x01, 0xeb, 0xff, 0x57, 0x39, 0x40, 0x05, 0x20, 0x00, + 0xfa, 0x1e, 0x50, 0xf3, 0x06, 0x00, 0x16, 0x02, 0x4a, 0x1d, 0x81, 0x35, 0x02, 0x00, 0xfa, 0x1e, + 0x50, 0xf3, 0x00, 0x00, 0x33, 0x02, 0x4a, 0x1d, 0x89, 0x35, 0x04, 0x00, 0x4a, 0x1d, 0x5c, 0xe9, + 0x01, 0x00, 0x4a, 0x02, 0x54, 0x21, 0x82, 0x35, 0x12, 0x00, 0x4a, 0x1d, 0x5c, 0xe9, 0x02, 0x00, + 0x4d, 0x02, 0x54, 0x21, 0x8a, 0x35, 0x14, 0x00, 0x96, 0x1b, 0xfc, 0xef, 0xc9, 0x00, 0x1a, 0x02, + 0x46, 0x30, 0x03, 0x01, 0x20, 0x01, 0xb0, 0x20, 0xa8, 0xec, 0x40, 0x01, 0xe8, 0xff, 0x57, 0x3a, + 0x40, 0x04, 0x20, 0x00, 0x04, 0x1f, 0x50, 0xf3, 0x06, 0x00, 0x18, 0x02, 0x4c, 0x20, 0x81, 0x35, + 0x02, 0x00, 0x04, 0x1f, 0x50, 0xf3, 0x00, 0x00, 0x31, 0x02, 0x4c, 0x20, 0x89, 0x35, 0x04, 0x00, + 0x54, 0x1d, 0x5c, 0xe9, 0x01, 0x00, 0x4a, 0x02, 0x54, 0x21, 0x82, 0x34, 0x12, 0x00, 0x54, 0x1d, + 0x5c, 0xe9, 0x02, 0x00, 0x4d, 0x02, 0x54, 0x21, 0x8a, 0x34, 0x14, 0x00, 0xa0, 0x1b, 0xfc, 0xef, + 0xbe, 0x00, 0x1a, 0x02, 0x47, 0x30, 0x03, 0x00, 0x20, 0x01, 0xbe, 0x20, 0xa8, 0xec, 0x3f, 0x01, + 0xe7, 0xff, 0x57, 0x39, 0x40, 0x04, 0x20, 0x00, 0x12, 0x1f, 0x50, 0xf3, 0x07, 0x00, 0x1d, 0x02, + 0x4a, 0x1d, 0x81, 0x35, 0x02, 0x00, 0x12, 0x1f, 0x50, 0xf3, 0x00, 0x00, 0x39, 0x02, 0x4a, 0x1d, + 0x89, 0x35, 0x04, 0x00, 0xae, 0x1b, 0xfc, 0xef, 0xc4, 0x00, 0x1a, 0x02, 0x49, 0x33, 0x03, 0x01, + 0x20, 0x01, 0xc4, 0x20, 0xa8, 0xec, 0x3d, 0x01, 0xe9, 0xff, 0x55, 0x3c, 0x40, 0x04, 0x20, 0x00, + 0x18, 0x1f, 0x50, 0xf3, 0xe0, 0x39, 0x1a, 0x02, 0x13, 0x18, 0x01, 0x00, 0x20, 0x01, 0xb4, 0x1b, + 0xfc, 0xef, 0xc0, 0x00, 0x1a, 0x02, 0x4c, 0x37, 0x03, 0x01, 0x20, 0x01, 0xd2, 0x20, 0xa8, 0xec, + 0x3d, 0x01, 0xe9, 0xff, 0x54, 0x3d, 0x40, 0x04, 0x20, 0x00, 0x26, 0x1f, 0x50, 0xf3, 0xcc, 0x38, + 0x1a, 0x02, 0x16, 0x1b, 0x01, 0x00, 0x10, 0x01, 0xc2, 0x1b, 0xfc, 0xef, 0xc2, 0x00, 0x1a, 0x02, + 0x51, 0x3d, 0x03, 0x00, 0x20, 0x01, 0xda, 0x20, 0xa8, 0xec, 0x3e, 0x01, 0xe9, 0xff, 0x55, 0x3f, + 0x40, 0x04, 0x20, 0x00, 0x2e, 0x1f, 0x50, 0xf3, 0x7d, 0x3b, 0x1a, 0x02, 0x16, 0x1e, 0x01, 0x00, + 0x20, 0x01, 0xca, 0x1b, 0xfc, 0xef, 0xbd, 0x00, 0x1a, 0x02, 0x51, 0x3d, 0x03, 0x00, 0x20, 0x01, + 0xe6, 0x20, 0xa8, 0xec, 0x3e, 0x01, 0xe7, 0xff, 0x56, 0x45, 0x40, 0x04, 0x20, 0x00, 0x3a, 0x1f, + 0x50, 0xf3, 0xf7, 0x3c, 0x1a, 0x02, 0x0d, 0x18, 0x01, 0x00, 0x10, 0x01, 0x8a, 0x1d, 0x5c, 0xe9, + 0x1d, 0x19, 0xe6, 0xfd, 0x1c, 0x16, 0x02, 0x02, 0x10, 0x01, 0xd6, 0x1b, 0xfc, 0xef, 0xc6, 0x00, + 0x1a, 0x02, 0x56, 0x44, 0x03, 0x00, 0x20, 0x01, 0xf2, 0x20, 0xa8, 0xec, 0x3e, 0x01, 0xe8, 0xff, + 0x57, 0x31, 0x40, 0x04, 0x10, 0x00, 0xe2, 0x1b, 0xfc, 0xef, 0xc5, 0x00, 0x1a, 0x02, 0x55, 0x45, + 0x03, 0x00, 0x20, 0x01, 0xfc, 0x20, 0xa8, 0xec, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, + 0x00, 0x00, 0xec, 0x1b, 0xfc, 0xef, 0xc8, 0x00, 0x1a, 0x02, 0x51, 0x42, 0x03, 0x01, 0x20, 0x01, + 0x06, 0x21, 0xa8, 0xec, 0x40, 0x01, 0xe8, 0xff, 0x57, 0x43, 0x40, 0x07, 0x21, 0x00, 0x06, 0x21, + 0xa8, 0xec, 0x91, 0x00, 0xf2, 0xfc, 0x57, 0x43, 0x88, 0x37, 0x23, 0x00, 0x5a, 0x1f, 0x50, 0xf3, + 0xf5, 0x02, 0x0f, 0x00, 0x37, 0x2f, 0x41, 0x03, 0x10, 0x00, 0xaa, 0x1d, 0x5c, 0xe9, 0x46, 0x3c, + 0xe6, 0xfd, 0x16, 0x16, 0x02, 0x03, 0x10, 0x01, 0xf6, 0x1b, 0xfc, 0xef, 0xcb, 0x00, 0x1a, 0x02, + 0x47, 0x37, 0x03, 0x01, 0x20, 0x01, 0x12, 0x21, 0xa8, 0xec, 0x3f, 0x01, 0xe6, 0xff, 0x58, 0x42, + 0x40, 0x06, 0x21, 0x00, 0x12, 0x21, 0xa8, 0xec, 0x94, 0x00, 0x08, 0xfd, 0x57, 0x42, 0x88, 0x36, + 0x23, 0x00, 0xb6, 0x1d, 0x5c, 0xe9, 0xd6, 0x3b, 0xa5, 0x01, 0x12, 0x12, 0x82, 0x33, 0x02, 0x00, + 0x02, 0x1c, 0xfc, 0xef, 0xc8, 0x00, 0x1a, 0x02, 0x48, 0x38, 0x03, 0x00, 0x20, 0x01, 0x1c, 0x21, + 0xa8, 0xec, 0x3f, 0x01, 0xe3, 0xff, 0x58, 0x45, 0x40, 0x06, 0x21, 0x00, 0x1c, 0x21, 0xa8, 0xec, + 0x90, 0x00, 0x02, 0xfd, 0x5a, 0x45, 0x88, 0x36, 0x23, 0x00, 0x70, 0x1f, 0x50, 0xf3, 0xf4, 0x02, + 0x0f, 0x00, 0x3a, 0x34, 0x41, 0x02, 0x10, 0x00, 0xc0, 0x1d, 0x5c, 0xe9, 0xd6, 0x3b, 0x9b, 0x01, + 0x12, 0x12, 0x82, 0x32, 0x02, 0x00, 0x0c, 0x1c, 0xfc, 0xef, 0xc4, 0x00, 0x1a, 0x02, 0x44, 0x32, + 0x03, 0x00, 0x20, 0x01, 0x26, 0x21, 0xa8, 0xec, 0x40, 0x01, 0xe4, 0xff, 0x58, 0x46, 0x40, 0x06, + 0x21, 0x00, 0x26, 0x21, 0xa8, 0xec, 0x93, 0x00, 0x19, 0xfd, 0x5b, 0x46, 0x88, 0x36, 0x23, 0x00, + 0x7a, 0x1f, 0x50, 0xf3, 0xf8, 0x02, 0x00, 0x00, 0x3e, 0x3e, 0x41, 0x02, 0x20, 0x00, 0xca, 0x1d, + 0x5c, 0xe9, 0xc3, 0x3c, 0x7c, 0x05, 0x0f, 0x13, 0x82, 0x33, 0x02, 0x00, 0x16, 0x1c, 0xfc, 0xef, + 0xc7, 0x00, 0x1a, 0x02, 0x45, 0x34, 0x03, 0x00, 0x20, 0x01, 0x30, 0x21, 0xa8, 0xec, 0x41, 0x01, + 0xe3, 0xff, 0x5a, 0x40, 0x40, 0x06, 0x21, 0x00, 0x30, 0x21, 0xa8, 0xec, 0x94, 0x00, 0x13, 0xfd, + 0x57, 0x40, 0x88, 0x36, 0x23, 0x00, 0x20, 0x1c, 0xfc, 0xef, 0xc3, 0x00, 0x1a, 0x02, 0x48, 0x37, + 0x03, 0x01, 0x20, 0x01, 0x3a, 0x21, 0xa8, 0xec, 0x41, 0x01, 0xe0, 0xff, 0x5a, 0x45, 0x40, 0x06, + 0x31, 0x00, 0x3a, 0x21, 0xa8, 0xec, 0x96, 0x00, 0x1f, 0xfd, 0x5b, 0x45, 0x88, 0x36, 0x33, 0x00, + 0x8e, 0x1f, 0x50, 0xf3, 0xf8, 0x02, 0x04, 0x00, 0x40, 0x3f, 0x41, 0x03, 0x20, 0x00, 0x2a, 0x1c, + 0xfc, 0xef, 0x00, 0x00, 0x1a, 0x02, 0x4e, 0x1b, 0x03, 0x00, 0x10, 0x01, 0x46, 0x21, 0xa8, 0xec, + 0x41, 0x01, 0xdd, 0xff, 0x5a, 0x48, 0x40, 0x06, 0x21, 0x00, 0x46, 0x21, 0xa8, 0xec, 0x9a, 0x00, + 0x26, 0xfd, 0x5e, 0x48, 0x88, 0x36, 0x23, 0x00, 0x9a, 0x1f, 0x50, 0xf3, 0xbb, 0x01, 0xde, 0xfa, + 0x47, 0x30, 0x81, 0x56, 0x12, 0x00, 0x9a, 0x1f, 0x50, 0xf3, 0xbb, 0x01, 0xde, 0xfa, 0x47, 0x30, + 0x89, 0x06, 0x14, 0x00, 0xea, 0x1d, 0x5c, 0xe9, 0x98, 0x3b, 0x4e, 0x0a, 0x13, 0x16, 0x82, 0x33, + 0x02, 0x00, 0x36, 0x1c, 0xfc, 0xef, 0xc7, 0x00, 0x1a, 0x02, 0x4f, 0x42, 0x03, 0x00, 0x20, 0x01, + 0x52, 0x21, 0xa8, 0xec, 0x42, 0x01, 0xe1, 0xff, 0x48, 0x2c, 0x00, 0x36, 0x12, 0x00, 0x52, 0x21, + 0xa8, 0xec, 0x95, 0x00, 0x10, 0xfd, 0x48, 0x2c, 0x88, 0x36, 0x14, 0x00, 0xa6, 0x1f, 0x50, 0xf3, + 0x01, 0x00, 0x25, 0x02, 0x4a, 0x1e, 0x81, 0x36, 0x12, 0x00, 0xa6, 0x1f, 0x50, 0xf3, 0x01, 0x00, + 0x25, 0x02, 0x4a, 0x1e, 0x89, 0x36, 0x14, 0x00, 0xf6, 0x1d, 0x5c, 0xe9, 0x90, 0x00, 0xe6, 0xfd, + 0x55, 0x28, 0x02, 0x02, 0x10, 0x01, 0x42, 0x1c, 0xfc, 0xef, 0x02, 0x00, 0xa0, 0x02, 0x50, 0x21, + 0x83, 0x36, 0x12, 0x00, 0x42, 0x1c, 0xfc, 0xef, 0x03, 0x00, 0x9e, 0x02, 0x50, 0x21, 0x8b, 0x36, + 0x14, 0x00, 0x5a, 0x21, 0xa8, 0xec, 0x3f, 0x01, 0xd9, 0xff, 0x59, 0x33, 0x40, 0x05, 0x11, 0x00, + 0x5a, 0x21, 0xa8, 0xec, 0x9c, 0x00, 0x10, 0xfd, 0x4e, 0x33, 0x88, 0x35, 0x13, 0x00, 0xae, 0x1f, + 0x50, 0xf3, 0xf9, 0x02, 0x04, 0x00, 0x41, 0x40, 0x41, 0x01, 0x20, 0x00, 0x4a, 0x1c, 0xfc, 0xef, + 0xc7, 0x00, 0x1a, 0x02, 0x4e, 0x3b, 0x03, 0x01, 0x20, 0x01, 0x66, 0x21, 0xa8, 0xec, 0x3e, 0x01, + 0xdc, 0xff, 0x5c, 0x46, 0x40, 0x05, 0x21, 0x00, 0x66, 0x21, 0xa8, 0xec, 0x97, 0x00, 0x36, 0xfd, + 0x5e, 0x46, 0x88, 0x35, 0x23, 0x00, 0xba, 0x1f, 0x50, 0xf3, 0x39, 0x01, 0x28, 0x07, 0x4b, 0x31, + 0x81, 0x55, 0x12, 0x00, 0xba, 0x1f, 0x50, 0xf3, 0x2c, 0x01, 0x5d, 0x07, 0x4b, 0x31, 0x89, 0x05, + 0x14, 0x00, 0x56, 0x1c, 0xfc, 0xef, 0xc4, 0x00, 0x1a, 0x02, 0x44, 0x2c, 0x03, 0x01, 0x20, 0x01, + 0x70, 0x21, 0xa8, 0xec, 0x3c, 0x01, 0xdc, 0xff, 0x5c, 0x45, 0x40, 0x04, 0x31, 0x00, 0x70, 0x21, + 0xa8, 0xec, 0x9d, 0x00, 0x19, 0xfd, 0x5d, 0x45, 0x88, 0x34, 0x33, 0x00, 0xc4, 0x1f, 0x50, 0xf3, + 0x3a, 0x01, 0x2d, 0x07, 0x4e, 0x34, 0x81, 0x54, 0x22, 0x00, 0xc4, 0x1f, 0x50, 0xf3, 0x2e, 0x01, + 0x5f, 0x07, 0x4e, 0x34, 0x89, 0x04, 0x24, 0x00, 0x60, 0x1c, 0xfc, 0xef, 0xc2, 0x00, 0x1a, 0x02, + 0x48, 0x32, 0x03, 0x00, 0x20, 0x01, 0x7a, 0x21, 0xa8, 0xec, 0x3c, 0x01, 0xdb, 0xff, 0x5c, 0x41, + 0x40, 0x04, 0x21, 0x00, 0x7a, 0x21, 0xa8, 0xec, 0x9c, 0x00, 0x17, 0xfd, 0x59, 0x41, 0x88, 0x34, + 0x23, 0x00, 0xce, 0x1f, 0x50, 0xf3, 0x38, 0x01, 0x24, 0x07, 0x4b, 0x31, 0x81, 0x55, 0x12, 0x00, + 0xce, 0x1f, 0x50, 0xf3, 0x2b, 0x01, 0x59, 0x07, 0x4b, 0x31, 0x89, 0x05, 0x14, 0x00, 0x6a, 0x1c, + 0xfc, 0xef, 0xca, 0x00, 0x1a, 0x02, 0x46, 0x30, 0x03, 0x01, 0x20, 0x01, 0x86, 0x21, 0xa8, 0xec, + 0x3c, 0x01, 0xda, 0xff, 0x5b, 0x44, 0x40, 0x04, 0x21, 0x00, 0x86, 0x21, 0xa8, 0xec, 0x9a, 0x00, + 0x3a, 0xfd, 0x5d, 0x44, 0x88, 0x34, 0x23, 0x00, 0xda, 0x1f, 0x50, 0xf3, 0xf8, 0x02, 0x02, 0x00, + 0x41, 0x3a, 0x41, 0x01, 0x20, 0x00, 0x2a, 0x1e, 0x5c, 0xe9, 0x3f, 0x3b, 0x6d, 0x04, 0x16, 0x19, + 0x82, 0x30, 0x22, 0x00, 0x76, 0x1c, 0xfc, 0xef, 0xc4, 0x00, 0x1a, 0x02, 0x46, 0x2f, 0x03, 0x01, + 0x20, 0x01, 0x92, 0x21, 0xa8, 0xec, 0x3c, 0x01, 0xda, 0xff, 0x5b, 0x47, 0x40, 0x04, 0x21, 0x00, + 0x92, 0x21, 0xa8, 0xec, 0x98, 0x00, 0x44, 0xfd, 0x5f, 0x47, 0x88, 0x34, 0x23, 0x00, 0xe6, 0x1f, + 0x50, 0xf3, 0xf9, 0x02, 0x03, 0x00, 0x43, 0x3a, 0x41, 0x00, 0x20, 0x00, 0x36, 0x1e, 0x5c, 0xe9, + 0x0d, 0x3c, 0x01, 0x01, 0x16, 0x1a, 0x02, 0x30}}; diff --git a/nebula_tests/package.xml b/nebula_tests/package.xml index 375dd0e3d..7e0db3449 100644 --- a/nebula_tests/package.xml +++ b/nebula_tests/package.xml @@ -14,6 +14,7 @@ nebula_common nebula_decoders + nebula_hw_interfaces rosbag2_cpp ament_cmake_gtest