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/nebula_common/include/nebula_common/aeva/config_types.hpp b/nebula_common/include/nebula_common/aeva/config_types.hpp index 7155dbb27..589845977 100644 --- a/nebula_common/include/nebula_common/aeva/config_types.hpp +++ b/nebula_common/include/nebula_common/aeva/config_types.hpp @@ -19,7 +19,6 @@ #include -#include #include #include #include @@ -34,7 +33,7 @@ struct Aeries2Config : public SensorConfigurationBase std::string sensor_ip; json tree; - [[nodiscard]] std::optional getReturnMode() const + [[nodiscard]] std::optional get_return_mode() const { auto mode_name = util::get_if_exists(tree, {"dsp_control", "second_peak_type"}); @@ -45,24 +44,24 @@ struct Aeries2Config : public SensorConfigurationBase return ReturnMode::UNKNOWN; } -}; -inline std::ostream & operator<<(std::ostream & os, const Aeries2Config & arg) -{ - os << "Aeva Aeries2 Sensor Configuration:\n"; - os << "Sensor Model: " << arg.sensor_model << '\n'; - os << "Frame ID: " << arg.frame_id << '\n'; - os << "Sensor IP: " << arg.sensor_ip; + friend std::ostream & operator<<(std::ostream & os, const Aeries2Config & arg) + { + os << "Aeva Aeries2 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(); + 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; -} + 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 index cd2e5bd91..d6936cf8f 100644 --- a/nebula_common/include/nebula_common/aeva/packet_types.hpp +++ b/nebula_common/include/nebula_common/aeva/packet_types.hpp @@ -177,12 +177,12 @@ struct HealthCode { explicit HealthCode(uint32_t value) : value_(value) {} - [[nodiscard]] bool is_error() const { return (value_ & ERROR_MASK) != 0; } - [[nodiscard]] uint32_t get() const { return value_ & ~ERROR_MASK; } + [[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 ERROR_MASK = 1u << 31u; + static const uint32_t g_error_mask = 1u << 31u; }; struct ReconfigMessage diff --git a/nebula_common/include/nebula_common/util/parsing.hpp b/nebula_common/include/nebula_common/util/parsing.hpp index 94022fa4e..425902506 100644 --- a/nebula_common/include/nebula_common/util/parsing.hpp +++ b/nebula_common/include/nebula_common/util/parsing.hpp @@ -62,8 +62,7 @@ bool update_if_exists(const json & node, const std::vector & path, template std::optional get_if_exists(const json & node, const std::vector & path) { - T result; - if (update_if_exists(node, path, result)) { + if (T result; update_if_exists(node, path, result)) { return result; } 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 index f2900c1ef..708653dd3 100644 --- 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 @@ -27,7 +27,6 @@ #include #include #include -#include namespace nebula::drivers { @@ -43,11 +42,11 @@ class AevaAeries2Decoder AevaAeries2Decoder() : cloud_state_({std::make_unique(), 0}) {} - void processPointcloudMessage(const aeva::PointCloudMessage & message); + void process_pointcloud_message(const aeva::PointCloudMessage & message); - void registerPointCloudCallback(callback_t callback); + void register_point_cloud_callback(callback_t callback); - void onParameterChange(ReturnMode return_mode); + void on_parameter_change(ReturnMode return_mode); private: struct DecoderState @@ -65,10 +64,10 @@ class AevaAeries2Decoder uint64_t timestamp; }; - ReturnType getReturnType(uint32_t peak_id); + [[nodiscard]] ReturnType get_return_type(uint32_t peak_id) const; callback_t callback_; - std::atomic return_mode_; + 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 index af34c69ec..96fc09c68 100644 --- a/nebula_decoders/src/nebula_decoders_aeva/aeva_aeries2_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_aeva/aeva_aeries2_decoder.cpp @@ -4,10 +4,12 @@ #include +#include + namespace nebula::drivers { -void AevaAeries2Decoder::processPointcloudMessage(const aeva::PointCloudMessage & message) +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, @@ -49,7 +51,7 @@ void AevaAeries2Decoder::processPointcloudMessage(const aeva::PointCloudMessage point.azimuth = -raw_point.azimuth.value() * M_PI_2f; point.elevation = raw_point.elevation.value() * M_PI_4f; - ReturnType return_type = getReturnType(raw_point.peak_id); + ReturnType return_type = get_return_type(raw_point.peak_id); point.return_type = static_cast(return_type); @@ -61,14 +63,14 @@ void AevaAeries2Decoder::processPointcloudMessage(const aeva::PointCloudMessage point.range_rate = raw_point.velocity.value(); point.intensity = raw_point.intensity; - point.time_stamp = state.absolute_time_ns - cloud_state_.timestamp; - point.channel = state.line_index; + 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::getReturnType(uint32_t peak_id) +ReturnType AevaAeries2Decoder::get_return_type(uint32_t peak_id) const { if (peak_id == 0) return ReturnType::STRONGEST; if (peak_id > 1) return ReturnType::UNKNOWN; @@ -85,12 +87,12 @@ ReturnType AevaAeries2Decoder::getReturnType(uint32_t peak_id) } } -void AevaAeries2Decoder::onParameterChange(ReturnMode return_mode) +void AevaAeries2Decoder::on_parameter_change(ReturnMode return_mode) { return_mode_.store(return_mode); } -void AevaAeries2Decoder::registerPointCloudCallback( +void AevaAeries2Decoder::register_point_cloud_callback( std::function, uint64_t)> callback) { callback_ = std::move(callback); diff --git a/nebula_hw_interfaces/CMakeLists.txt b/nebula_hw_interfaces/CMakeLists.txt index 59bb9cd8a..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} 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 index 38bd0044d..74c3a56e3 100644 --- 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 @@ -14,7 +14,6 @@ #pragma once -#include "nebula_common/util/parsing.hpp" #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" @@ -28,9 +27,7 @@ #include #include -#include #include -#include #include namespace nebula::drivers @@ -56,11 +53,11 @@ class AevaHwInterface * and the sensor settings */ AevaHwInterface( - std::shared_ptr logger, bool setup_sensor, + const std::shared_ptr & logger, bool setup_sensor, const std::shared_ptr & config) : AevaHwInterface( - logger, setup_sensor, config, makepointcloudApi(*config), makeTelemetryApi(*config), - makeReconfigApi(*config, logger), makeHealthApi(*config)) + logger, setup_sensor, config, make_pointcloud_api(*config), make_telemetry_api(*config), + make_reconfig_api(*config, logger), make_health_api(*config)) { } @@ -76,13 +73,13 @@ class AevaHwInterface * @param health_api The health endpoint. Can be null. */ AevaHwInterface( - std::shared_ptr logger, bool setup_sensor, + 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_(std::move(logger)), + logger_(logger), pointcloud_api_(std::move(pointcloud_api)), telemetry_api_(std::move(telemetry_api)), reconfig_api_(std::move(reconfig_api)), @@ -90,105 +87,34 @@ class AevaHwInterface { if (setup_sensor_ && reconfig_api_) { logger_->info("Configuring sensor..."); - onConfigChange(config); + on_config_change(config); logger_->info("Config OK"); } } - void onConfigChange(const std::shared_ptr & config) - { - if (!reconfig_api_ || !setup_sensor_) return; - - json manifest{}; - for (uint32_t i = 0; i < MANIFEST_RETRIES && manifest.is_null(); ++i) { - try { - if (i > 0) { - logger_->info("Re-trying to fetch manifest"); - } - manifest = reconfig_api_->getManifest(); - 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_ = makeReconfigApi(*config, logger_); - } - } + void on_config_change(const std::shared_ptr & config); - if (manifest.is_null()) { - throw std::runtime_error("Reached maximum retries while trying to fetch manifest"); - } + void register_cloud_packet_callback(PointcloudParser::callback_t callback); - for (const auto & category : config->tree.items()) { - for (const auto & setting : category.value().items()) { - tryReconfig(manifest, category.key(), setting.key(), setting.value()); - } - } - } + void register_raw_cloud_packet_callback(connections::ObservableByteStream::callback_t callback); - void registerCloudPacketCallback(PointcloudParser::callback_t callback) - { - pointcloud_api_->registerCallback(std::move(callback)); - } + void register_health_callback(HealthParser::callback_t callback); - void registerRawCloudPacketCallback(connections::ObservableByteStream::callback_t callback) - { - pointcloud_api_->registerBytesCallback(std::move(callback)); - } - - void registerHealthCallback(HealthParser::callback_t callback) - { - health_api_->registerCallback(std::move(callback)); - } - - void registerTelemetryCallback(TelemetryParser::callback_t callback) - { - telemetry_api_->registerCallback(std::move(callback)); - } + void register_telemetry_callback(TelemetryParser::callback_t callback); private: - void tryReconfig( + void 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_->setParameter(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 - // //////////////////////////////////////// + const json & value); - logger_->info("Set " + node_name + "." + key + " to " + value.dump()); - } - - static std::shared_ptr makeReconfigApi( - const aeva::Aeries2Config & config, const std::shared_ptr & logger) - { - return std::make_shared( - std::make_shared(config.sensor_ip, 41007), - std::make_shared(config.sensor_ip, 21901), logger->child("ReconfigApi")); - } + static std::shared_ptr make_reconfig_api( + const aeva::Aeries2Config & config, const std::shared_ptr & logger); - static std::shared_ptr makepointcloudApi(const aeva::Aeries2Config & config) - { - return std::make_shared(std::make_shared(config.sensor_ip, 41000)); - } + static std::shared_ptr make_pointcloud_api(const aeva::Aeries2Config & config); - static std::shared_ptr makeHealthApi(const aeva::Aeries2Config & config) - { - return std::make_shared(std::make_shared(config.sensor_ip, 41001)); - } + static std::shared_ptr make_health_api(const aeva::Aeries2Config & config); - static std::shared_ptr makeTelemetryApi(const aeva::Aeries2Config & config) - { - return std::make_shared(std::make_shared(config.sensor_ip, 41003)); - } + static std::shared_ptr make_telemetry_api(const aeva::Aeries2Config & config); const bool setup_sensor_; std::shared_ptr logger_; @@ -197,9 +123,9 @@ class AevaHwInterface 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 MANIFEST_RETRIES = 3; + /// 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 index 7cdced8e7..a9e526386 100644 --- 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 @@ -58,7 +58,7 @@ class MismatchError : public ParseError const std::string message_; }; -static const uint16_t AEVA_HEADER = 0xAEFA; +static const uint16_t g_aeva_header = 0xAEFA; template void expect_eq(A actual, E expected, const std::string & message) @@ -102,7 +102,7 @@ T pull_and_parse( const std::vector::const_iterator & cend) { if (std::distance(cbegin, cend) != sizeof(T)) { - throw std::runtime_error("Number of bytes provided does not match type's size."); + throw std::out_of_range("Number of bytes provided does not match type's size."); } T result{}; @@ -151,21 +151,21 @@ class AevaParser : public ObservableByteStream { public: explicit AevaParser(std::shared_ptr incoming_byte_stream) - : running_(true), incoming_(std::move(incoming_byte_stream)) + : incoming_(std::move(incoming_byte_stream)) { if (!incoming_) { - throw std::runtime_error("Incoming byte stream cannot be null"); + throw std::invalid_argument("Incoming byte stream cannot be null"); } - thread_ = std::thread([&]() { - while (running_) onLowLevelMessage(); + thread_ = std::thread([this]() { + while (running_) on_low_level_message(); }); } - void registerBytesCallback(callback_t callback) override - { - bytes_callback_ = std::move(callback); - } + AevaParser(const AevaParser &) = default; + AevaParser(AevaParser &&) noexcept = default; + AevaParser & operator=(const AevaParser &) = default; + AevaParser & operator=(AevaParser &&) noexcept = default; ~AevaParser() override { @@ -173,12 +173,17 @@ class AevaParser : public ObservableByteStream 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 onLowLevelMessage() + void on_low_level_message() { std::vector some_ip_raw; incoming_->read(some_ip_raw, sizeof(SomeIpHeader)); @@ -203,16 +208,19 @@ class AevaParser : public ObservableByteStream expect_eq(message_header.message_type, StreamId, "Unexpected message type"); expect_eq(message_header.message_length, payload_length, "Payload size mismatch"); - onMessage(message_header, payload_view); + on_message(message_header, payload_view); } - virtual void onMessage(const MessageHeader & /* message_header */, ByteView & /* payload_bytes */) + 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_; + std::atomic_bool running_{true}; std::shared_ptr incoming_; callback_t bytes_callback_; @@ -232,7 +240,7 @@ class AevaSender * @brief Sends the serialized message payload `bytes` over the outgoing byte stream, prepending * the SomeIp and Message headers. */ - void sendMessage(std::vector bytes) + void send_message(std::vector bytes) { sequence_number_++; 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 index 5088dd479..cee931e83 100644 --- 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 @@ -30,7 +30,6 @@ namespace nebula::drivers::connections { -using namespace boost::endian; // NOLINT using aeva::HealthCode; class HealthParser : public AevaParser @@ -43,10 +42,10 @@ class HealthParser : public AevaParser { } - void registerCallback(callback_t callback) { callback_ = std::move(callback); } + void register_callback(callback_t callback) { callback_ = std::move(callback); } protected: - void onMessage(const MessageHeader & message_header, ByteView & payload_bytes) override + void on_message(const MessageHeader & message_header, ByteView & payload_bytes) override { auto n_entries = pull_and_parse(payload_bytes); @@ -59,8 +58,8 @@ class HealthParser : public AevaParser entries.reserve(n_entries); for (size_t i = 0; i < n_entries; ++i) { - auto pointer = &*payload_bytes.consumeUnsafe(sizeof(uint32_t)).cbegin(); - auto entry = load_little_u32(pointer); + auto pointer = &*payload_bytes.consume_unsafe(sizeof(uint32_t)).cbegin(); + auto entry = boost::endian::load_little_u32(pointer); entries.emplace_back(entry); } 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 index a983d42b3..73401c190 100644 --- 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 @@ -38,10 +38,10 @@ class PointcloudParser : public AevaParser { } - void registerCallback(callback_t callback) { callback_ = std::move(callback); } + void register_callback(callback_t callback) { callback_ = std::move(callback); } protected: - void onMessage(const MessageHeader & message_header, ByteView & payload_bytes) override + void on_message(const MessageHeader & message_header, ByteView & payload_bytes) override { PointCloudMessage message{}; message.header = pull_and_parse(payload_bytes); 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 index 1b2e3f59d..b3692b23e 100644 --- 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 @@ -61,10 +61,10 @@ class ReconfigParser : public AevaParser, { } - json getManifest() + json get_manifest() { ReconfigMessage request{ReconfigRequestType::kManifestRequest, {}}; - auto responses = doRequest(request, N_MANIFEST_RESPONSES_EXPECTED); + auto responses = do_request(request, g_n_manifest_responses_expected); json result{}; @@ -79,12 +79,12 @@ class ReconfigParser : public AevaParser, return result; } - json setParameter(std::string node_name, std::string key, json value) + 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 = doRequest(request); + auto response = do_request(request); if (response.type != aeva::ReconfigRequestType::kChangeApproved) { std::stringstream ss{}; @@ -104,13 +104,13 @@ class ReconfigParser : public AevaParser, } private: - ReconfigMessage doRequest(const ReconfigMessage & request) + ReconfigMessage do_request(const ReconfigMessage & request) { - auto responses = doRequest(request, 1); + auto responses = do_request(request, 1); return responses.at(0); } - std::vector doRequest( + std::vector do_request( const ReconfigMessage & request, size_t n_responses_expected) { std::lock_guard lock(mtx_inflight_); @@ -133,7 +133,7 @@ class ReconfigParser : public AevaParser, message_payload = std::vector(body_string.begin(), body_string.end()); } - uint32_t data_size = message_payload.size(); + auto data_size = static_cast(message_payload.size()); std::vector bytes; bytes.reserve( @@ -188,7 +188,7 @@ class ReconfigParser : public AevaParser, } }; - sendMessage(bytes); + send_message(bytes); auto request_timed_out = !tm_callback_timeout.try_lock_for(20s); { @@ -233,7 +233,7 @@ class ReconfigParser : public AevaParser, } protected: - void onMessage(const MessageHeader & message_header, ByteView & payload_bytes) override + void on_message(const MessageHeader & message_header, ByteView & payload_bytes) override { ReconfigMessage message{}; @@ -266,7 +266,7 @@ class ReconfigParser : public AevaParser, std::mutex mtx_inflight_; // scanner, calibration, system_config, spc_converter, dsp_control, self_test, window_measurement - static const size_t N_MANIFEST_RESPONSES_EXPECTED = 7; + static const size_t g_n_manifest_responses_expected = 7; }; } // namespace nebula::drivers::connections 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 index a700de3c6..520be84ad 100644 --- 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 @@ -42,7 +42,7 @@ using namespace boost::endian; // NOLINT namespace telemetry_detail { -const std::vector TYPE_OVERRIDES = { +const std::vector g_type_overrides = { "display_all_points", "hfov_adjustment_deg", "hfov_rotation_deg", @@ -87,10 +87,10 @@ class TelemetryParser : public AevaParser { } - void registerCallback(callback_t callback) { callback_ = std::move(callback); } + void register_callback(callback_t callback) { callback_ = std::move(callback); } protected: - void onMessage(const MessageHeader & message_header, ByteView & payload_bytes) override + void on_message(const MessageHeader & message_header, ByteView & payload_bytes) override { auto payload_size = pull_and_parse(payload_bytes); @@ -168,12 +168,12 @@ class TelemetryParser : public AevaParser entry_data_raw); break; case aeva::TelemetryDataType::kChar: - auto overrides = telemetry_detail::TYPE_OVERRIDES; + 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 (auto it = entry_data_raw.cbegin(); it != entry_data_raw.cend(); ++it) { - raw_value = (raw_value << 8u) | *it; + for (const uint8_t & it : entry_data_raw) { + raw_value = (raw_value << 8u) | it; } value = static_cast(raw_value); 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 index 94c5efe4a..b72d8b0d1 100644 --- 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 @@ -34,13 +34,13 @@ class ObservableByteStream ObservableByteStream() = default; ObservableByteStream(const ObservableByteStream &) = default; - ObservableByteStream(ObservableByteStream &&) = delete; + ObservableByteStream(ObservableByteStream &&) = default; ObservableByteStream & operator=(const ObservableByteStream &) = default; - ObservableByteStream & operator=(ObservableByteStream &&) = delete; + ObservableByteStream & operator=(ObservableByteStream &&) = default; virtual ~ObservableByteStream() = default; - virtual void registerBytesCallback(callback_t callback) = 0; + virtual void register_bytes_callback(callback_t callback) = 0; }; /** @@ -52,9 +52,9 @@ class PullableByteStream PullableByteStream() = default; PullableByteStream(const PullableByteStream &) = default; - PullableByteStream(PullableByteStream &&) = delete; + PullableByteStream(PullableByteStream &&) = default; PullableByteStream & operator=(const PullableByteStream &) = default; - PullableByteStream & operator=(PullableByteStream &&) = delete; + PullableByteStream & operator=(PullableByteStream &&) = default; virtual ~PullableByteStream() = default; @@ -70,9 +70,9 @@ class WritableByteStream WritableByteStream() = default; WritableByteStream(const WritableByteStream &) = default; - WritableByteStream(WritableByteStream &&) = delete; + WritableByteStream(WritableByteStream &&) = default; WritableByteStream & operator=(const WritableByteStream &) = default; - WritableByteStream & operator=(WritableByteStream &&) = delete; + WritableByteStream & operator=(WritableByteStream &&) = default; virtual ~WritableByteStream() = default; 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 index 2ce1903a6..0926547d4 100644 --- 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 @@ -38,7 +38,7 @@ class ByteView private: using iter_t = std::vector::const_iterator; - using consume_result_t = nebula::util::expected; + using consume_result_t = nebula::util::expected; public: explicit ByteView(const std::vector & underlying) @@ -57,11 +57,11 @@ class ByteView * @param n_bytes The number of bytes to consume * @return Slice The consumed bytes */ - Slice consumeUnsafe(size_t n_bytes) + Slice consume_unsafe(size_t n_bytes) { auto n = static_cast(n_bytes); if (n > size()) { - throw std::runtime_error("Index out of bounds"); + throw std::out_of_range("Index out of bounds"); } auto new_cbegin = std::next(cbegin_, n); @@ -81,8 +81,8 @@ class ByteView [[nodiscard]] consume_result_t consume(size_t n_bytes) { try { - return consumeUnsafe(n_bytes); - } catch (const std::runtime_error & e) { + return consume_unsafe(n_bytes); + } catch (const std::out_of_range & e) { return e; } } @@ -98,8 +98,10 @@ class ByteView 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_); } 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 index f5705a071..6ff360651 100644 --- 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 @@ -47,8 +47,8 @@ class StreamBuffer : public PullableByteStream buffer_(packets_buffered), packet_loss_callback_(std::move(packet_loss_callback)) { - underlying_->registerBytesCallback( - [&](auto bytes) { onBytesFromUnderlying(std::move(bytes)); }); + underlying_->register_bytes_callback( + [this](auto bytes) { on_bytes_from_underlying(std::move(bytes)); }); } void read(std::vector & into, size_t n_bytes) override @@ -94,7 +94,7 @@ class StreamBuffer : public PullableByteStream } private: - void onBytesFromUnderlying(std::vector bytes) + void on_bytes_from_underlying(std::vector bytes) { auto ptr = std::make_unique>(); ptr->swap(bytes); 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 index 9a5d0f444..6c3bbc548 100644 --- 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 @@ -37,7 +37,6 @@ 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( @@ -54,8 +53,8 @@ class TcpStream : public PullableByteStream } private: - boost::asio::io_service io_service_; - boost::asio::ip::tcp::socket socket_; + 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 index 1d66be05b..5550fb67c 100644 --- 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 @@ -26,15 +26,8 @@ #include #include -#include #include -#include -#include -#include -#include -#include #include -#include #include namespace nebula::drivers::connections @@ -44,7 +37,6 @@ class TcpSender : public WritableByteStream { public: TcpSender(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( @@ -59,8 +51,8 @@ class TcpSender : public WritableByteStream } private: - boost::asio::io_service io_service_; - boost::asio::ip::tcp::socket socket_; + 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/udp_receiver.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/connections/udp_receiver.hpp deleted file mode 100644 index 2a55db84a..000000000 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/connections/udp_receiver.hpp +++ /dev/null @@ -1,68 +0,0 @@ -// 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 - -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 registerBytesCallback(callback_t callback) override - { - std::lock_guard lock(mtx_callback_); - callback_ = std::move(callback); - } - -private: - void onReceive(const std::vector & bytes) - { - std::lock_guard lock(mtx_callback_); - if (!callback_) return; - callback_(bytes); - } - - ::drivers::common::IoContext ctx_; - ::drivers::udp_driver::UdpDriver udp_driver_; - std::mutex mtx_callback_; - 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 index d5fd215e0..1245e6bff 100644 --- 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 @@ -12,13 +12,107 @@ // 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 +namespace nebula::drivers +{ -#include +void AevaHwInterface::on_config_change(const std::shared_ptr & config) +{ + if (!reconfig_api_ || !setup_sensor_) return; -namespace nebula::drivers + 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, 41007), + std::make_shared(config.sensor_ip, 21901), logger->child("ReconfigApi")); +} + +std::shared_ptr AevaHwInterface::make_pointcloud_api( + const aeva::Aeries2Config & config) +{ + return std::make_shared(std::make_shared(config.sensor_ip, 41000)); +} + +std::shared_ptr AevaHwInterface::make_health_api(const aeva::Aeries2Config & config) +{ + return std::make_shared(std::make_shared(config.sensor_ip, 41001)); +} + +std::shared_ptr AevaHwInterface::make_telemetry_api( + const aeva::Aeries2Config & config) { + return std::make_shared(std::make_shared(config.sensor_ip, 41003)); +} } // namespace nebula::drivers diff --git a/nebula_ros/include/nebula_ros/aeva/aeva_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/aeva/aeva_ros_wrapper.hpp index d4bae4e2b..30d8f575d 100644 --- a/nebula_ros/include/nebula_ros/aeva/aeva_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/aeva/aeva_ros_wrapper.hpp @@ -57,10 +57,10 @@ class AevaRosWrapper final : public rclcpp::Node explicit AevaRosWrapper(const rclcpp::NodeOptions & options); private: - Status declareAndGetSensorConfigParams(); + Status declare_and_get_sensor_config_params(); template - void declareJsonParam(const std::string & dot_delimited_path, json & inout_tree) + 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)); @@ -68,9 +68,9 @@ class AevaRosWrapper final : public rclcpp::Node } template - bool getJsonParam( + bool get_json_param( const std::vector & p, const std::string & dot_delimited_path, - json & inout_tree) + json & inout_tree) const { T value; bool got_param = get_param(p, dot_delimited_path, value); @@ -82,12 +82,13 @@ class AevaRosWrapper final : public rclcpp::Node return true; } - Status validateAndSetConfig(std::shared_ptr & new_config); + Status validate_and_set_config( + const std::shared_ptr & new_config); - rcl_interfaces::msg::SetParametersResult onParameterChange( + rcl_interfaces::msg::SetParametersResult on_parameter_change( const std::vector & p); - void recordRawPacket(const std::vector & bytes); + void record_raw_packet(const std::vector & bytes); rclcpp::Publisher::SharedPtr packets_pub_; std::mutex mtx_current_scan_msg_; diff --git a/nebula_ros/include/nebula_ros/common/nebula_packet_stream.hpp b/nebula_ros/include/nebula_ros/common/nebula_packet_stream.hpp index ff9b6a53b..410dd83e9 100644 --- a/nebula_ros/include/nebula_ros/common/nebula_packet_stream.hpp +++ b/nebula_ros/include/nebula_ros/common/nebula_packet_stream.hpp @@ -34,13 +34,13 @@ class NebulaPacketStream : public drivers::connections::ObservableByteStream public: NebulaPacketStream() = default; - void registerBytesCallback(callback_t callback) override + void register_bytes_callback(callback_t callback) override { std::lock_guard lock(mtx_callback_); callback_ = std::move(callback); } - void onNebulaPackets(std::unique_ptr packets) + void on_nebula_packets(std::unique_ptr packets) { std::lock_guard lock(mtx_callback_); if (!callback_) return; @@ -51,8 +51,8 @@ class NebulaPacketStream : public drivers::connections::ObservableByteStream } private: - std::mutex mtx_callback_{}; - callback_t callback_{}; + std::mutex mtx_callback_; + callback_t callback_; }; } // namespace nebula::ros diff --git a/nebula_ros/src/aeva/aeva_ros_wrapper.cpp b/nebula_ros/src/aeva/aeva_ros_wrapper.cpp index bc55792f2..3f8d70cbc 100644 --- a/nebula_ros/src/aeva/aeva_ros_wrapper.cpp +++ b/nebula_ros/src/aeva/aeva_ros_wrapper.cpp @@ -48,7 +48,7 @@ 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 = declareAndGetSensorConfigParams(); + auto status = declare_and_get_sensor_config_params(); if (status != Status::OK) { throw std::runtime_error( @@ -57,7 +57,7 @@ AevaRosWrapper::AevaRosWrapper(const rclcpp::NodeOptions & options) RCLCPP_INFO_STREAM(get_logger(), "SensorConfig: " << *sensor_cfg_ptr_); - auto qos_profile = rmw_qos_profile_sensor_data; + const auto & qos_profile = rmw_qos_profile_sensor_data; auto pointcloud_qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), qos_profile); @@ -81,14 +81,15 @@ AevaRosWrapper::AevaRosWrapper(const rclcpp::NodeOptions & options) create_publisher("nebula_packets", pointcloud_qos); drivers::connections::ObservableByteStream::callback_t raw_packet_cb = [&](const auto & bytes) { - this->recordRawPacket(std::move(bytes)); + this->record_raw_packet(std::move(bytes)); }; - hw_interface_->registerRawCloudPacketCallback(std::move(raw_packet_cb)); + hw_interface_->register_raw_cloud_packet_callback(std::move(raw_packet_cb)); - hw_interface_->registerTelemetryCallback( - [&](const auto & msg) { hw_monitor_->onTelemetryFragment(msg); }); - hw_interface_->registerHealthCallback([&](auto codes) { hw_monitor_->onHealthCodes(codes); }); + 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 @@ -96,7 +97,7 @@ AevaRosWrapper::AevaRosWrapper(const rclcpp::NodeOptions & options) // //////////////////////////////////////// auto packet_stream = std::make_shared(); auto packet_buffer = - std::make_shared(packet_stream, 1000, [&]() { + std::make_shared(packet_stream, 1000, [this]() { RCLCPP_ERROR_THROTTLE( get_logger(), *get_clock(), 1000, "Packet stream buffer overflowed, packet loss occurred."); @@ -105,7 +106,7 @@ AevaRosWrapper::AevaRosWrapper(const rclcpp::NodeOptions & options) packets_sub_ = create_subscription( "nebula_packets", rclcpp::SensorDataQoS(), [=](std::unique_ptr packets) { - packet_stream->onNebulaPackets(std::move(packets)); + packet_stream->on_nebula_packets(std::move(packets)); }); auto pointcloud_parser = std::make_shared(packet_buffer); @@ -121,20 +122,20 @@ AevaRosWrapper::AevaRosWrapper(const rclcpp::NodeOptions & options) RCLCPP_INFO(get_logger(), "Starting stream"); PointcloudParser::callback_t pointcloud_message_cb = [this](const auto & message) { - decoder_.processPointcloudMessage(message); + decoder_.process_pointcloud_message(message); }; - hw_interface_->registerCloudPacketCallback(std::move(pointcloud_message_cb)); + 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, [&](bool ok) { + 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 = - [&](AevaPointCloudUniquePtr cloud_ptr, auto timestamp) { + [this](AevaPointCloudUniquePtr cloud_ptr, auto timestamp) { auto now = this->now(); cloud_watchdog_->update(); @@ -155,13 +156,13 @@ AevaRosWrapper::AevaRosWrapper(const rclcpp::NodeOptions & options) } }; - decoder_.registerPointCloudCallback(std::move(pointcloud_cb)); + decoder_.register_point_cloud_callback(std::move(pointcloud_cb)); parameter_event_cb_ = - add_on_set_parameters_callback([this](const auto & p) { return onParameterChange(p); }); + add_on_set_parameters_callback([this](const auto & p) { return on_parameter_change(p); }); } -Status AevaRosWrapper::declareAndGetSensorConfigParams() +Status AevaRosWrapper::declare_and_get_sensor_config_params() { Aeries2Config config; @@ -170,36 +171,37 @@ Status AevaRosWrapper::declareAndGetSensorConfigParams() config.sensor_ip = declare_parameter("sensor_ip", param_read_only()); config.frame_id = declare_parameter("frame_id", param_read_only()); - declareJsonParam("scanner.dithering_enable_ego_speed", config.tree); - declareJsonParam("scanner.dithering_pattern_option", config.tree); - declareJsonParam("scanner.ele_offset_rad", config.tree); - declareJsonParam("scanner.enable_frame_dithering", config.tree); - declareJsonParam("scanner.enable_frame_sync", config.tree); - declareJsonParam("scanner.flip_pattern_vertically", config.tree); - declareJsonParam("scanner.frame_sync_offset_in_ms", config.tree); - declareJsonParam("scanner.frame_sync_type", config.tree); - declareJsonParam("scanner.frame_synchronization_on_rising_edge", config.tree); - declareJsonParam("scanner.hfov_adjustment_deg", config.tree); - declareJsonParam("scanner.hfov_rotation_deg", config.tree); - declareJsonParam("scanner.highlight_ROI", config.tree); - declareJsonParam("scanner.horizontal_fov_degrees", config.tree); - declareJsonParam("scanner.roi_az_offset_rad", config.tree); - declareJsonParam("scanner.vertical_pattern", config.tree); - declareJsonParam("system_config.range_modes", config.tree); - declareJsonParam("system_config.sensitivity_mode", config.tree); - declareJsonParam("system_config.thermal_throttling_setting", config.tree); - declareJsonParam("spc_converter.discard_points_in_ambiguity_region", config.tree); - declareJsonParam("spc_converter.display_all_points", config.tree); - declareJsonParam("spc_converter.enable_min_range_filter", config.tree); - declareJsonParam("dsp_control.second_peak_type", config.tree); - declareJsonParam("dsp_control.use_foveated_velocity_bias", config.tree); - declareJsonParam("dsp_control.velocity_bias_pattern_options", config.tree); + 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 validateAndSetConfig(new_cfg_ptr); + return validate_and_set_config(new_cfg_ptr); } -Status AevaRosWrapper::validateAndSetConfig(std::shared_ptr & new_config) +Status AevaRosWrapper::validate_and_set_config( + const std::shared_ptr & new_config) { if (!new_config) { return Status::SENSOR_CONFIG_ERROR; @@ -215,14 +217,14 @@ Status AevaRosWrapper::validateAndSetConfig(std::shared_ptr if (hw_interface_) { try { - hw_interface_->onConfigChange(new_config); + 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->getReturnMode(); + 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"); @@ -230,51 +232,54 @@ Status AevaRosWrapper::validateAndSetConfig(std::shared_ptr } if (return_mode_opt) { - decoder_.onParameterChange(*return_mode_opt); + decoder_.on_parameter_change(*return_mode_opt); } sensor_cfg_ptr_ = new_config; return Status::OK; } -rcl_interfaces::msg::SetParametersResult AevaRosWrapper::onParameterChange( +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 = - getJsonParam(p, "scanner.dithering_enable_ego_speed", config.tree) | - getJsonParam(p, "scanner.dithering_pattern_option", config.tree) | - getJsonParam(p, "scanner.ele_offset_rad", config.tree) | - getJsonParam(p, "scanner.enable_frame_dithering", config.tree) | - getJsonParam(p, "scanner.enable_frame_sync", config.tree) | - getJsonParam(p, "scanner.flip_pattern_vertically", config.tree) | - getJsonParam(p, "scanner.frame_sync_offset_in_ms", config.tree) | - getJsonParam(p, "scanner.frame_sync_type", config.tree) | - getJsonParam(p, "scanner.frame_synchronization_on_rising_edge", config.tree) | - getJsonParam(p, "scanner.hfov_adjustment_deg", config.tree) | - getJsonParam(p, "scanner.hfov_rotation_deg", config.tree) | - getJsonParam(p, "scanner.highlight_ROI", config.tree) | - getJsonParam(p, "scanner.horizontal_fov_degrees", config.tree) | - getJsonParam(p, "scanner.roi_az_offset_rad", config.tree) | - getJsonParam(p, "scanner.vertical_pattern", config.tree) | - getJsonParam(p, "system_config.range_modes", config.tree) | - getJsonParam(p, "system_config.sensitivity_mode", config.tree) | - getJsonParam(p, "system_config.thermal_throttling_setting", config.tree) | - getJsonParam(p, "spc_converter.discard_points_in_ambiguity_region", config.tree) | - getJsonParam(p, "spc_converter.display_all_points", config.tree) | - getJsonParam(p, "spc_converter.enable_min_range_filter", config.tree) | - getJsonParam(p, "dsp_control.second_peak_type", config.tree) | - getJsonParam(p, "dsp_control.use_foveated_velocity_bias", config.tree) | - getJsonParam(p, "dsp_control.velocity_bias_pattern_options", config.tree); + 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 = validateAndSetConfig(new_cfg_ptr); + auto status = validate_and_set_config(new_cfg_ptr); if (status != Status::OK) { RCLCPP_WARN_STREAM(get_logger(), "OnParameterChange aborted: " << status); @@ -287,7 +292,7 @@ rcl_interfaces::msg::SetParametersResult AevaRosWrapper::onParameterChange( return rcl_interfaces::build().successful(true).reason(""); } -void AevaRosWrapper::recordRawPacket(const std::vector & vector) +void AevaRosWrapper::record_raw_packet(const std::vector & bytes) { std::lock_guard lock(mtx_current_scan_msg_); @@ -308,7 +313,7 @@ void AevaRosWrapper::recordRawPacket(const std::vector & vector) auto & packet = current_scan_msg_->packets.emplace_back(); packet.stamp = packet_stamp; - packet.data = vector; + packet.data = bytes; } RCLCPP_COMPONENTS_REGISTER_NODE(AevaRosWrapper) diff --git a/nebula_tests/aeva/aeva_hw_interface_test.cpp b/nebula_tests/aeva/aeva_hw_interface_test.cpp index 3c0219d10..119f6f870 100644 --- a/nebula_tests/aeva/aeva_hw_interface_test.cpp +++ b/nebula_tests/aeva/aeva_hw_interface_test.cpp @@ -13,6 +13,7 @@ #include #include +#include using nebula::drivers::aeva::PointCloudMessage; using nebula::drivers::connections::PointcloudParser; @@ -33,13 +34,14 @@ TEST(TestParsing, Pointcloud) // NOLINT EXPECT_GT(arg.points.size(), 0); EXPECT_TRUE(mock_byte_stream->done()); - EXPECT_EQ(mock_byte_stream->getReadCount(), 2); + EXPECT_EQ(mock_byte_stream->get_read_count(), 2); done = true; }; - parser.registerCallback(std::move(callback)); + parser.register_callback(std::move(callback)); mock_byte_stream->run(); while (!done) { + std::this_thread::yield(); } } diff --git a/nebula_tests/common/mock_byte_stream.hpp b/nebula_tests/common/mock_byte_stream.hpp index f18cb82e8..c57cdbce1 100644 --- a/nebula_tests/common/mock_byte_stream.hpp +++ b/nebula_tests/common/mock_byte_stream.hpp @@ -18,6 +18,7 @@ #include #include +#include #include namespace nebula::test @@ -31,6 +32,7 @@ class MockByteStream final : public drivers::connections::PullableByteStream void read(std::vector & into, size_t n_bytes) override { while (!running_) { + std::this_thread::yield(); } read_count_++; const auto & from = stream_[index_++]; @@ -46,9 +48,9 @@ class MockByteStream final : public drivers::connections::PullableByteStream void run() { running_ = true; } - bool done() { return done_; } + [[nodiscard]] bool done() const { return done_; } - size_t getReadCount() { return read_count_; } + [[nodiscard]] size_t get_read_count() const { return read_count_; } private: const std::vector> & stream_;