From c9be206a03c165db6e1a113d3900cec2f007bcd7 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 26 Jun 2024 19:41:45 +0900 Subject: [PATCH 01/57] refactor(mt_queue): move to nebula_common --- .../include/nebula_common/util}/mt_queue.hpp | 2 +- .../nebula_ros/continental/continental_ars548_ros_wrapper.hpp | 3 +-- .../nebula_ros/continental/continental_srr520_ros_wrapper.hpp | 3 +-- nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp | 1 + .../include/nebula_ros/robosense/robosense_ros_wrapper.hpp | 2 +- .../include/nebula_ros/velodyne/velodyne_ros_wrapper.hpp | 2 +- nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp | 2 +- nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp | 2 +- nebula_ros/src/robosense/robosense_ros_wrapper.cpp | 2 +- nebula_ros/src/velodyne/velodyne_ros_wrapper.cpp | 2 +- 10 files changed, 10 insertions(+), 11 deletions(-) rename {nebula_ros/include/nebula_ros/common => nebula_common/include/nebula_common/util}/mt_queue.hpp (98%) diff --git a/nebula_ros/include/nebula_ros/common/mt_queue.hpp b/nebula_common/include/nebula_common/util/mt_queue.hpp similarity index 98% rename from nebula_ros/include/nebula_ros/common/mt_queue.hpp rename to nebula_common/include/nebula_common/util/mt_queue.hpp index 40f26253d..beb2abf62 100644 --- a/nebula_ros/include/nebula_ros/common/mt_queue.hpp +++ b/nebula_common/include/nebula_common/util/mt_queue.hpp @@ -32,7 +32,7 @@ class MtQueue public: explicit MtQueue(size_t capacity) : capacity_(capacity) {} - bool try_push(T && value) + bool tryPush(T && value) { std::unique_lock lock(this->mutex_); bool can_push = queue_.size() < capacity_; diff --git a/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp index 1d755a3ef..a7b4b9f13 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp @@ -14,14 +14,13 @@ #pragma once -#include "nebula_ros/common/mt_queue.hpp" -#include "nebula_ros/common/parameter_descriptors.hpp" #include "nebula_ros/continental/continental_ars548_decoder_wrapper.hpp" #include "nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp" #include #include #include +#include #include #include #include diff --git a/nebula_ros/include/nebula_ros/continental/continental_srr520_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_srr520_ros_wrapper.hpp index dbef1c8ff..99c7885e0 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_srr520_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_srr520_ros_wrapper.hpp @@ -14,14 +14,13 @@ #pragma once -#include "nebula_ros/common/mt_queue.hpp" -#include "nebula_ros/common/parameter_descriptors.hpp" #include "nebula_ros/continental/continental_srr520_decoder_wrapper.hpp" #include "nebula_ros/continental/continental_srr520_hw_interface_wrapper.hpp" #include #include #include +#include #include #include #include diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp index 8bf0100ff..f05b47cc5 100644 --- a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp @@ -22,6 +22,7 @@ #include "nebula_ros/hesai/hw_monitor_wrapper.hpp" #include +#include #include #include diff --git a/nebula_ros/include/nebula_ros/robosense/robosense_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/robosense/robosense_ros_wrapper.hpp index 2c7e08d00..52fd8ebf7 100644 --- a/nebula_ros/include/nebula_ros/robosense/robosense_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/robosense/robosense_ros_wrapper.hpp @@ -14,7 +14,6 @@ #pragma once -#include "nebula_ros/common/mt_queue.hpp" #include "nebula_ros/robosense/decoder_wrapper.hpp" #include "nebula_ros/robosense/hw_interface_wrapper.hpp" #include "nebula_ros/robosense/hw_monitor_wrapper.hpp" @@ -24,6 +23,7 @@ #include #include #include +#include #include #include #include diff --git a/nebula_ros/include/nebula_ros/velodyne/velodyne_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/velodyne/velodyne_ros_wrapper.hpp index 9f7fe8758..0685eb321 100644 --- a/nebula_ros/include/nebula_ros/velodyne/velodyne_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/velodyne/velodyne_ros_wrapper.hpp @@ -14,7 +14,6 @@ #pragma once -#include "nebula_ros/common/mt_queue.hpp" #include "nebula_ros/common/parameter_descriptors.hpp" #include "nebula_ros/velodyne/decoder_wrapper.hpp" #include "nebula_ros/velodyne/hw_interface_wrapper.hpp" @@ -24,6 +23,7 @@ #include #include #include +#include #include #include #include diff --git a/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp index f459c208f..ab6fd0c29 100644 --- a/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp @@ -159,7 +159,7 @@ void ContinentalARS548RosWrapper::receive_packet_callback( return; } - if (!packet_queue_.try_push(std::move(msg_ptr))) { + if (!packet_queue_.tryPush(std::move(msg_ptr))) { RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 500, "Packet(s) dropped"); } } diff --git a/nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp b/nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp index 6ec611e42..b24e689c1 100644 --- a/nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp @@ -151,7 +151,7 @@ void ContinentalSRR520RosWrapper::receive_packet_callback( return; } - if (!packet_queue_.try_push(std::move(msg_ptr))) { + if (!packet_queue_.tryPush(std::move(msg_ptr))) { RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 500, "Packet(s) dropped"); } } diff --git a/nebula_ros/src/robosense/robosense_ros_wrapper.cpp b/nebula_ros/src/robosense/robosense_ros_wrapper.cpp index 85d34bac9..235999694 100644 --- a/nebula_ros/src/robosense/robosense_ros_wrapper.cpp +++ b/nebula_ros/src/robosense/robosense_ros_wrapper.cpp @@ -307,7 +307,7 @@ void RobosenseRosWrapper::receive_cloud_packet_callback(std::vector & p msg_ptr->stamp.nanosec = static_cast(timestamp_ns % 1'000'000'000); msg_ptr->data.swap(packet); - if (!packet_queue_.try_push(std::move(msg_ptr))) { + if (!packet_queue_.tryPush(std::move(msg_ptr))) { RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 500, "Packet(s) dropped"); } } diff --git a/nebula_ros/src/velodyne/velodyne_ros_wrapper.cpp b/nebula_ros/src/velodyne/velodyne_ros_wrapper.cpp index e06535766..441a2d9f3 100644 --- a/nebula_ros/src/velodyne/velodyne_ros_wrapper.cpp +++ b/nebula_ros/src/velodyne/velodyne_ros_wrapper.cpp @@ -255,7 +255,7 @@ void VelodyneRosWrapper::receive_cloud_packet_callback(std::vector & pa msg_ptr->stamp.nanosec = static_cast(timestamp_ns % 1'000'000'000); msg_ptr->data.swap(packet); - if (!packet_queue_.try_push(std::move(msg_ptr))) { + if (!packet_queue_.tryPush(std::move(msg_ptr))) { RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 500, "Packet(s) dropped"); } } From 438f986fbbd6ec40ba32431205060c43a3750fec Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 26 Jun 2024 19:42:24 +0900 Subject: [PATCH 02/57] refactor(expected): add shorthand value_or_throw function --- .../include/nebula_common/util/expected.hpp | 36 +++++++++++++------ 1 file changed, 25 insertions(+), 11 deletions(-) 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_; From ca35b242c1d2ed63e18062343e015aba00c408e1 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 26 Jun 2024 19:47:00 +0900 Subject: [PATCH 03/57] feat(hw_interfaces): light-weight TCP/UDP interfaces with abstractions --- .../connections/byte_stream.hpp | 82 ++++++++++++ .../connections/byte_view.hpp | 119 ++++++++++++++++++ .../connections/stream_buffer.hpp | 116 +++++++++++++++++ .../connections/tcp_receiver.hpp | 61 +++++++++ .../connections/tcp_sender.hpp | 66 ++++++++++ .../connections/udp_receiver.hpp | 68 ++++++++++ nebula_tests/CMakeLists.txt | 7 +- nebula_tests/common/mock_byte_stream.hpp | 40 ++++++ nebula_tests/package.xml | 1 + 9 files changed, 559 insertions(+), 1 deletion(-) 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/byte_view.hpp create mode 100644 nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/connections/stream_buffer.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/tcp_sender.hpp create mode 100644 nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/connections/udp_receiver.hpp create mode 100644 nebula_tests/common/mock_byte_stream.hpp 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..94c5efe4a --- /dev/null +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/connections/byte_stream.hpp @@ -0,0 +1,82 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include +#include +#include + +namespace nebula::drivers::connections +{ + +/** + * @brief A stream of packets containing bytes. The given callback is called on each arriving + * packet. + */ +class ObservableByteStream +{ +public: + using callback_t = typename std::function & buffer)>; + + ObservableByteStream() = default; + + ObservableByteStream(const ObservableByteStream &) = default; + ObservableByteStream(ObservableByteStream &&) = delete; + ObservableByteStream & operator=(const ObservableByteStream &) = default; + ObservableByteStream & operator=(ObservableByteStream &&) = delete; + + virtual ~ObservableByteStream() = default; + + virtual void registerBytesCallback(callback_t callback) = 0; +}; + +/** + * @brief A stream of bytes that the user can read N bytes from synchronously. + */ +class PullableByteStream +{ +public: + PullableByteStream() = default; + + PullableByteStream(const PullableByteStream &) = default; + PullableByteStream(PullableByteStream &&) = delete; + PullableByteStream & operator=(const PullableByteStream &) = default; + PullableByteStream & operator=(PullableByteStream &&) = delete; + + virtual ~PullableByteStream() = default; + + virtual void read(std::vector & into, size_t n_bytes) = 0; +}; + +/** + * @brief A stream of bytes that can be written to by the user. + */ +class WritableByteStream +{ +public: + WritableByteStream() = default; + + WritableByteStream(const WritableByteStream &) = default; + WritableByteStream(WritableByteStream &&) = delete; + WritableByteStream & operator=(const WritableByteStream &) = default; + WritableByteStream & operator=(WritableByteStream &&) = delete; + + virtual ~WritableByteStream() = default; + + virtual void write(std::vector & data) = 0; +}; + +} // namespace nebula::drivers::connections diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/connections/byte_view.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/connections/byte_view.hpp new file mode 100644 index 000000000..2ce1903a6 --- /dev/null +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/connections/byte_view.hpp @@ -0,0 +1,119 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include "nebula_common/util/expected.hpp" + +#include + +#include +#include +#include +#include +#include + +namespace nebula::drivers::connections +{ + +/** + * @brief A non-owning view onto the bytes contained in an underlying vector. + * Bytes can be consumed from the view, automatically shrinking it by that amount. + */ +class ByteView +{ +public: + class Slice; + +private: + using iter_t = std::vector::const_iterator; + using consume_result_t = nebula::util::expected; + +public: + explicit ByteView(const std::vector & underlying) + : cbegin_(underlying.cbegin()), cend_(underlying.cend()) + { + } + + explicit ByteView(std::vector &&) = delete; + explicit ByteView(const std::vector &&) = delete; + + /** + * @brief Consumes `n_bytes` bytes from the view's beginning, shrinking it by that amount and + * returning a slice making those bytes accessible. This function throws in case of out-of-bounds + * accesses. + * + * @param n_bytes The number of bytes to consume + * @return Slice The consumed bytes + */ + Slice consumeUnsafe(size_t n_bytes) + { + auto n = static_cast(n_bytes); + if (n > size()) { + throw std::runtime_error("Index out of bounds"); + } + + auto new_cbegin = std::next(cbegin_, n); + auto result = Slice(cbegin_, new_cbegin); + cbegin_ = new_cbegin; + + return result; + } + + /** + * @brief Tries to consume `n_bytes`, returns a slice in case of success, or returns an exception + * in case of failure. Does not throw. + * + * @param n_bytes The number of bytes to consume + * @return consume_result_t Either the consumed bytes as a slice, or a caught exception + */ + [[nodiscard]] consume_result_t consume(size_t n_bytes) + { + try { + return consumeUnsafe(n_bytes); + } catch (const std::runtime_error & e) { + return e; + } + } + + [[nodiscard]] ssize_t size() const { return std::distance(cbegin_, cend_); } + + /** + * @brief A non-owning slice of an underlying view. This slice's bytes can be iterated over and it + * cannot be split/consumed further. + */ + class Slice + { + friend ByteView; + + public: + [[nodiscard]] auto cbegin() const { return cbegin_; } + + [[nodiscard]] auto cend() const { return cend_; } + + [[nodiscard]] ssize_t size() const { return std::distance(cbegin_, cend_); } + + private: + Slice(iter_t cbegin, iter_t cend) : cbegin_(cbegin), cend_(cend) {} + + iter_t cbegin_; + iter_t cend_; + }; + +private: + iter_t cbegin_; + iter_t cend_; +}; + +} // namespace nebula::drivers::connections diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/connections/stream_buffer.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/connections/stream_buffer.hpp new file mode 100644 index 000000000..f5705a071 --- /dev/null +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/connections/stream_buffer.hpp @@ -0,0 +1,116 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include "nebula_common/loggers/logger.hpp" +#include "nebula_hw_interfaces/nebula_hw_interfaces_common/connections/byte_stream.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace nebula::drivers::connections +{ + +/** + * @brief A `PullableByteStream` that buffers incoming bytes from a given `ObservableByteStream` + * until they are read by the user. + */ +class StreamBuffer : public PullableByteStream +{ + using packet_loss_callback_t = std::function; + +public: + StreamBuffer( + std::shared_ptr underlying, size_t packets_buffered, + packet_loss_callback_t packet_loss_callback) + : underlying_(std::move(underlying)), + buffer_(packets_buffered), + packet_loss_callback_(std::move(packet_loss_callback)) + { + underlying_->registerBytesCallback( + [&](auto bytes) { onBytesFromUnderlying(std::move(bytes)); }); + } + + void read(std::vector & into, size_t n_bytes) override + { + into.clear(); + + // If there is a packet left from the last read that has not been read completely read + // (remainder_), then continue there. Otherwise, pop the next one off the queue. + auto bytes = remainder_ ? std::move(remainder_) : buffer_.pop(); + + // Sizes match perfectly, return popped vector + if (bytes->size() == n_bytes) { + into.swap(*bytes); + return; + } + + // Too many bytes popped off the queue, put remainder back + if (bytes->size() > n_bytes) { + // Cut the remaining bytes off of bytes (will become remainder_ later). `bytes` then contains + // exactly the `n_bytes` we want as the into, and `into` contains exactly the bytes we + // want to become the `remainder_`. Thus, we can just swap the two vectors. + into.reserve(bytes->size() - n_bytes); + into.insert(into.end(), bytes->end() - static_cast(n_bytes), bytes->end()); + bytes->resize(bytes->size() - n_bytes); + + into.swap(*bytes); + remainder_ = std::move(bytes); + return; + } + + // Too little bytes popped off the queue, fetch next part + if (bytes->size() < n_bytes) { + auto remaining_length = n_bytes - bytes->size(); + std::vector remaining_bytes; + read(remaining_bytes, remaining_length); + into.swap(*bytes); + into.reserve(n_bytes); + into.insert(into.end(), remaining_bytes.begin(), remaining_bytes.end()); + return; + } + + assert(false); // Unreachable + } + +private: + void onBytesFromUnderlying(std::vector bytes) + { + auto ptr = std::make_unique>(); + ptr->swap(bytes); + auto success = buffer_.tryPush(std::move(ptr)); + if (!success) { + packet_loss_callback_(); + } + } + + std::shared_ptr logger_; + std::shared_ptr underlying_; + std::unique_ptr> remainder_; + std::mutex mtx_buffer_; + MtQueue>> buffer_; + + packet_loss_callback_t packet_loss_callback_; +}; + +} // namespace nebula::drivers::connections diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/connections/tcp_receiver.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/connections/tcp_receiver.hpp new file mode 100644 index 000000000..9a5d0f444 --- /dev/null +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/connections/tcp_receiver.hpp @@ -0,0 +1,61 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include "nebula_hw_interfaces/nebula_hw_interfaces_common/connections/byte_stream.hpp" + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace nebula::drivers::connections +{ + +class TcpStream : public PullableByteStream +{ +public: + TcpStream(const std::string & sensor_ip, uint16_t sensor_port) + : 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); + } + + void read(std::vector & into, size_t n_bytes) override + { + into.clear(); + into.resize(n_bytes); + boost::asio::read(socket_, boost::asio::buffer(into), boost::asio::transfer_exactly(n_bytes)); + } + +private: + boost::asio::io_service io_service_; + 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/tcp_sender.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/connections/tcp_sender.hpp new file mode 100644 index 000000000..1d66be05b --- /dev/null +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/connections/tcp_sender.hpp @@ -0,0 +1,66 @@ +// 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 +#include + +namespace nebula::drivers::connections +{ + +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( + sensor_ip, std::to_string(static_cast(sensor_port))); + auto endpoint_iterator = resolver.resolve(query); + boost::asio::connect(socket_, endpoint_iterator); + } + + void write(std::vector & bytes) override + { + boost::asio::write(socket_, boost::asio::buffer(bytes), boost::asio::transfer_all()); + } + +private: + boost::asio::io_service io_service_; + 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..2a55db84a --- /dev/null +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/connections/udp_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 + +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_tests/CMakeLists.txt b/nebula_tests/CMakeLists.txt index da2481a2f..b2adaef13 100644 --- a/nebula_tests/CMakeLists.txt +++ b/nebula_tests/CMakeLists.txt @@ -24,6 +24,7 @@ endif() find_package(ament_cmake_auto REQUIRED) find_package(nebula_common REQUIRED) find_package(nebula_decoders REQUIRED) +find_package(nebula_hw_interfaces REQUIRED) find_package(PCL REQUIRED COMPONENTS common) find_package(rosbag2_cpp REQUIRED) find_package(diagnostic_updater REQUIRED) @@ -40,18 +41,22 @@ if(BUILD_TESTING) add_definitions(-D_SRC_CALIBRATION_DIR_PATH="${PROJECT_SOURCE_DIR}/../nebula_decoders/calibration/") set(NEBULA_TEST_INCLUDE_DIRS + ./ ${nebula_common_INCLUDE_DIRS} ${nebula_decoders_INCLUDE_DIRS} + ${nebula_hw_interfaces_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} ${rosbag2_cpp_INCLUDE_DIRS} ${diagnostic_updater_INCLUDE_DIRS} + ${nebula_tests_common_INCLUDE_DIRS} ) - set(NEBULA_TEST_LIBRARIES + set(COMMON_TEST_LIBRARIES ${nebula_common_TARGETS} ${PCL_LIBRARIES} ${rosbag2_cpp_TARGETS} ${diagnostic_updater_TARGETS} + ${nebula_tests_common} ) set(CONTINENTAL_TEST_LIBRARIES diff --git a/nebula_tests/common/mock_byte_stream.hpp b/nebula_tests/common/mock_byte_stream.hpp new file mode 100644 index 000000000..97e8845ab --- /dev/null +++ b/nebula_tests/common/mock_byte_stream.hpp @@ -0,0 +1,40 @@ +// 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 + +namespace nebula::test +{ + +class MockByteStream final : public drivers::connections::PullableByteStream +{ +public: + explicit MockByteStream(const std::vector> & stream) : stream_(stream) {} + + void read(std::vector & into, size_t /* n_bytes */) override + { + auto from = stream_[index_++]; + into.clear(); + into.insert(into.end(), from.cbegin(), from.cend()); + } + +private: + const std::vector> & stream_; + size_t index_{}; +}; + +} // namespace nebula::test diff --git a/nebula_tests/package.xml b/nebula_tests/package.xml index f593b2e01..2a762a6a2 100644 --- a/nebula_tests/package.xml +++ b/nebula_tests/package.xml @@ -15,6 +15,7 @@ diagnostic_updater nebula_common nebula_decoders + nebula_hw_interfaces rosbag2_cpp ament_cmake_gtest From e60b7e8e386e08bf7ceb2437d11261d0720780bb Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 26 Jun 2024 19:48:00 +0900 Subject: [PATCH 04/57] feat(nebula_common): add nlohmann_json and helper functions --- .../include/nebula_common/util/parsing.hpp | 52 +++++++++++++++++++ 1 file changed, 52 insertions(+) create mode 100644 nebula_common/include/nebula_common/util/parsing.hpp 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..f3458dd12 --- /dev/null +++ b/nebula_common/include/nebula_common/util/parsing.hpp @@ -0,0 +1,52 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include + +#include +#include +#include + +namespace nebula::util +{ + +using nlohmann::json; + +template +bool update_if_exists(const json & node, const std::vector & path, T & out_value) +{ + auto * current_node = &node; + for (const auto & current_key : path) { + if (!current_node->contains(current_key)) return false; + current_node = ¤t_node->at(current_key); + } + + out_value = current_node->template get(); + return true; +} + +template +std::optional get_if_exists(const json & node, const std::vector & path) +{ + T result; + if (update_if_exists(node, path, result)) { + return result; + } + + return {}; +} + +} // namespace nebula::util From d4d0e7c02a45cf31eb5b30896de9055c520edf3b Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 26 Jun 2024 19:48:40 +0900 Subject: [PATCH 05/57] chore(watchdog_timer): clean up watchdog timer --- .../include/nebula_ros/common/watchdog_timer.hpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/nebula_ros/include/nebula_ros/common/watchdog_timer.hpp b/nebula_ros/include/nebula_ros/common/watchdog_timer.hpp index cfca4fc50..945986ee2 100644 --- a/nebula_ros/include/nebula_ros/common/watchdog_timer.hpp +++ b/nebula_ros/include/nebula_ros/common/watchdog_timer.hpp @@ -20,20 +20,21 @@ #include #include #include +#include namespace nebula::ros { class WatchdogTimer { - using watchdog_cb_t = std::function; - public: + using callback_t = std::function; + WatchdogTimer( rclcpp::Node & node, const std::chrono::microseconds & expected_update_interval, - const watchdog_cb_t & callback) + callback_t callback) : node_(node), - callback_(callback), + callback_(std::move(callback)), expected_update_interval_ns_( std::chrono::duration_cast(expected_update_interval).count()) { @@ -59,8 +60,8 @@ class WatchdogTimer rclcpp::Node & node_; rclcpp::TimerBase::SharedPtr timer_; - std::atomic last_update_ns_; - const watchdog_cb_t callback_; + std::atomic last_update_ns_{}; + const callback_t callback_; const uint64_t expected_update_interval_ns_; }; From 2838e77b36bb62e647b07b9e5f78710c4371cce3 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 26 Jun 2024 19:50:41 +0900 Subject: [PATCH 06/57] chore: make merged changed comply with pre-commit --- .../continental/continental_ars548_decoder_wrapper.hpp | 1 - .../continental/continental_ars548_hw_interface_wrapper.hpp | 2 -- .../continental/continental_srr520_decoder_wrapper.hpp | 1 - .../continental/continental_srr520_hw_interface_wrapper.hpp | 2 -- nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp | 2 ++ nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp | 2 ++ 6 files changed, 4 insertions(+), 6 deletions(-) diff --git a/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_wrapper.hpp index ea08cec6f..46ca94606 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_wrapper.hpp @@ -14,7 +14,6 @@ #pragma once -#include "nebula_ros/common/parameter_descriptors.hpp" #include "nebula_ros/common/watchdog_timer.hpp" #include diff --git a/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp index d3f92d556..7cce59f3b 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp @@ -14,8 +14,6 @@ #pragma once -#include "nebula_ros/common/parameter_descriptors.hpp" - #include #include #include diff --git a/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_wrapper.hpp index a01369209..d9d6c7046 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_wrapper.hpp @@ -14,7 +14,6 @@ #pragma once -#include "nebula_ros/common/parameter_descriptors.hpp" #include "nebula_ros/common/watchdog_timer.hpp" #include diff --git a/nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_wrapper.hpp index 1052981ec..aca56628f 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_wrapper.hpp @@ -14,8 +14,6 @@ #pragma once -#include "nebula_ros/common/parameter_descriptors.hpp" - #include #include #include diff --git a/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp index ab6fd0c29..d47bdd90d 100644 --- a/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp @@ -14,6 +14,8 @@ #include "nebula_ros/continental/continental_ars548_ros_wrapper.hpp" +#include "nebula_ros/common/parameter_descriptors.hpp" + #pragma clang diagnostic ignored "-Wbitwise-instead-of-logical" namespace nebula::ros diff --git a/nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp b/nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp index b24e689c1..7fdf9522d 100644 --- a/nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp @@ -14,6 +14,8 @@ #include "nebula_ros/continental/continental_srr520_ros_wrapper.hpp" +#include "nebula_ros/common/parameter_descriptors.hpp" + #pragma clang diagnostic ignored "-Wbitwise-instead-of-logical" namespace nebula::ros From 26aeef447d64e513d5202076311977ee387e70d2 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 26 Jun 2024 19:52:01 +0900 Subject: [PATCH 07/57] feat(aeva): add sensor model and type support --- .../nebula_common/aeva/config_types.hpp | 73 ++++++++ .../nebula_common/aeva/packet_types.hpp | 169 ++++++++++++++++++ .../nebula_common/aeva/point_types.hpp | 45 +++++ .../include/nebula_common/nebula_common.hpp | 9 +- 4 files changed, 295 insertions(+), 1 deletion(-) create mode 100644 nebula_common/include/nebula_common/aeva/config_types.hpp create mode 100644 nebula_common/include/nebula_common/aeva/packet_types.hpp create mode 100644 nebula_common/include/nebula_common/aeva/point_types.hpp diff --git a/nebula_common/include/nebula_common/aeva/config_types.hpp b/nebula_common/include/nebula_common/aeva/config_types.hpp new file mode 100644 index 000000000..4d4fdf6ba --- /dev/null +++ b/nebula_common/include/nebula_common/aeva/config_types.hpp @@ -0,0 +1,73 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include "nebula_common/nebula_common.hpp" + +#include +#include +#include + +namespace nebula::drivers::aeva +{ + +struct Aeries2Config : public SensorConfigurationBase +{ + std::string sensor_ip; + float dithering_enable_ego_speed; + std::string dithering_pattern_option; + float ele_offset_rad; + bool elevation_auto_adjustment; + bool enable_frame_dithering; + bool enable_frame_sync; + bool flip_pattern_vertically; + uint16_t frame_sync_offset_in_ms; + std::string frame_sync_type; + bool frame_synchronization_on_rising_edge; + float hfov_adjustment_deg; + float hfov_rotation_deg; + bool highlight_ROI; + std::string horizontal_fov_degrees; + float roi_az_offset_rad; + std::string vertical_pattern; +}; + +inline std::ostream & operator<<(std::ostream & os, const Aeries2Config & arg) +{ + os << "Aeries2Config:\n"; + os << " sensor_model = " << arg.sensor_model << '\n'; + os << " frame_id = " << arg.frame_id << '\n'; + os << " sensor_ip = " << arg.sensor_ip << '\n'; + os << " dithering_enable_ego_speed = " << arg.dithering_enable_ego_speed << '\n'; + os << " dithering_pattern_option = " << arg.dithering_pattern_option << '\n'; + os << " ele_offset_rad = " << arg.ele_offset_rad << '\n'; + os << " elevation_auto_adjustment = " << arg.elevation_auto_adjustment << '\n'; + os << " enable_frame_dithering = " << arg.enable_frame_dithering << '\n'; + os << " enable_frame_sync = " << arg.enable_frame_sync << '\n'; + os << " flip_pattern_vertically = " << arg.flip_pattern_vertically << '\n'; + os << " frame_sync_offset_in_ms = " << static_cast(arg.frame_sync_offset_in_ms) << '\n'; + os << " frame_sync_type = " << arg.frame_sync_type << '\n'; + os << " frame_synchronization_on_rising_edge = " << arg.frame_synchronization_on_rising_edge + << '\n'; + os << " hfov_adjustment_deg = " << arg.hfov_adjustment_deg << '\n'; + os << " hfov_rotation_deg = " << arg.hfov_rotation_deg << '\n'; + os << " highlight_ROI = " << arg.highlight_ROI << '\n'; + os << " horizontal_fov_degrees = " << arg.horizontal_fov_degrees << '\n'; + os << " roi_az_offset_rad = " << arg.roi_az_offset_rad << '\n'; + os << " vertical_pattern = " << arg.vertical_pattern; + return os; +} + +} // namespace nebula::drivers::aeva diff --git a/nebula_common/include/nebula_common/aeva/packet_types.hpp b/nebula_common/include/nebula_common/aeva/packet_types.hpp new file mode 100644 index 000000000..c050cb3a9 --- /dev/null +++ b/nebula_common/include/nebula_common/aeva/packet_types.hpp @@ -0,0 +1,169 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include + +#include +#include +#include +#include + +namespace nebula::drivers::aeva +{ +using nlohmann::json; + +#pragma pack(push, 1) + +template +struct FixedPoint +{ + [[nodiscard]] float value() const { return value_ * std::pow(2., -1. * n_fractional_bits); } + +private: + storage_t value_; +}; + +template +struct Padding +{ + uint8_t padding[n_bytes]; // NOLINT +}; + +struct SomeIpHeader +{ + uint16_t service_id; + uint16_t method_id; + uint32_t message_length; + uint16_t client_id; + uint16_t session_id; + uint8_t protocol; + uint8_t interface_version; + uint8_t message_type; + uint8_t return_code; + uint32_t tp_header_offset : 28; + uint32_t tp_header : 4; +}; + +struct MessageHeader +{ + uint8_t major_version : 4; + uint8_t minor_version : 4; + uint8_t message_type; + uint16_t sequence_id; + uint32_t message_length; + int64_t acquisition_time_ns; + int64_t publish_time_ns; +}; + +struct PointcloudMsgSubheaderAndMetadata +{ + uint16_t aeva_marker; + uint8_t platform; + uint8_t reserved_1; + uint16_t ns_per_index; + uint64_t first_point_timestamp_ns; + int32_t frame_sync_index; + uint32_t period_ns; + uint32_t n_entries; + uint32_t capacity; + uint16_t num_beams : 4; + uint16_t num_peaks : 2; + uint16_t range_scale : 10; + uint8_t line_index; + uint8_t max_line_index; + uint32_t face_index : 4; + uint32_t n_faces : 4; + uint32_t reserved_2 : 3; + uint32_t sensitivity_mode : 3; + uint32_t frame_parity : 1; + uint32_t reserved_3 : 1; + uint32_t window_measurement : 1; + uint32_t reserved_5 : 1; + uint32_t discard_line : 1; + uint32_t ping_pong : 1; + uint32_t reserved_6 : 12; + FixedPoint mirror_rps; + uint16_t dither_step : 5; + uint16_t max_dither_step : 5; + uint16_t reserved_7 : 6; + uint8_t reserved_8[12]; +}; + +struct PointCloudPoint +{ + FixedPoint azimuth; + FixedPoint elevation; + FixedPoint range; + FixedPoint velocity; + uint8_t intensity; + uint8_t signal_quality; + uint32_t beam_id : 3; + uint32_t peak_id : 2; + uint32_t line_transition : 1; + uint32_t valid : 1; + uint32_t dynamic : 1; + uint32_t reserved_1 : 1; + uint32_t up_sweep : 1; + uint32_t in_ambiguity_region : 1; + uint32_t reserved_2 : 9; + uint32_t peak_width : 4; + uint32_t is_single_detection : 1; + uint32_t reserved_3 : 7; +}; + +struct PointCloudMessage +{ + PointcloudMsgSubheaderAndMetadata header; + std::vector points; +}; + +enum class TelemetryDataType : uint8_t { + kUInt8 = 0, + kInt8 = 1, + kUInt16 = 2, + kInt16 = 3, + kUInt32 = 4, + kInt32 = 5, + kUInt64 = 6, + kInt64 = 7, + kFloat = 8, + kDouble = 9, + kChar = 10 +}; + +enum class ReconfigRequestType : uint8_t { + kManifestRequest = 0, + kManifestResponse = 1, + kChangeRequest = 2, + kChangeApproved = 3, + kChangeIgnored = 4, + kInvalid = 5 +}; + +inline bool is_error_code(uint32_t health_code) +{ + return (health_code & (1u << 31u)) != 0; +} + +struct ReconfigMessage +{ + ReconfigRequestType type = ReconfigRequestType::kInvalid; + std::optional body; +}; + +#pragma pack(pop) + +} // namespace nebula::drivers::aeva diff --git a/nebula_common/include/nebula_common/aeva/point_types.hpp b/nebula_common/include/nebula_common/aeva/point_types.hpp new file mode 100644 index 000000000..8a78b82cc --- /dev/null +++ b/nebula_common/include/nebula_common/aeva/point_types.hpp @@ -0,0 +1,45 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include + +namespace nebula::drivers::aeva +{ + +struct EIGEN_ALIGN16 PointXYZVIRCAEDT +{ + float x; + float y; + float z; + float 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/nebula_common.hpp b/nebula_common/include/nebula_common/nebula_common.hpp index 36f7f60c3..e50ab016e 100644 --- a/nebula_common/include/nebula_common/nebula_common.hpp +++ b/nebula_common/include/nebula_common/nebula_common.hpp @@ -341,7 +341,8 @@ enum class SensorModel { ROBOSENSE_BPEARL_V3, ROBOSENSE_BPEARL_V4, CONTINENTAL_ARS548, - CONTINENTAL_SRR520 + CONTINENTAL_SRR520, + AEVA_AERIES2 }; /// @brief not used? @@ -441,6 +442,9 @@ inline std::ostream & operator<<(std::ostream & os, nebula::drivers::SensorModel case SensorModel::CONTINENTAL_SRR520: os << "SRR520"; break; + case SensorModel::AEVA_AERIES2: + os << "Aeries II"; + break; case SensorModel::UNKNOWN: os << "Sensor Unknown"; break; @@ -579,6 +583,7 @@ inline SensorModel sensor_model_from_string(const std::string & sensor_model) // Continental if (sensor_model == "ARS548") return SensorModel::CONTINENTAL_ARS548; if (sensor_model == "SRR520") return SensorModel::CONTINENTAL_SRR520; + if (sensor_model == "Aeries2") return SensorModel::AEVA_AERIES2; return SensorModel::UNKNOWN; } @@ -629,6 +634,8 @@ inline std::string sensor_model_to_string(const SensorModel & sensor_model) return "ARS548"; case SensorModel::CONTINENTAL_SRR520: return "SRR520"; + case SensorModel::AEVA_AERIES2: + return "Aeries II"; default: return "UNKNOWN"; } From fa5665c21a726a296ed9e6632447769c527ea17c Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 26 Jun 2024 19:52:34 +0900 Subject: [PATCH 08/57] feat(aeva): add low-level Aeva TCP protocol support --- .../connections/aeva_api.hpp | 267 ++++++++++++++++++ 1 file changed, 267 insertions(+) create mode 100644 nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/aeva_api.hpp 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..0a1f7c4ec --- /dev/null +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/aeva_api.hpp @@ -0,0 +1,267 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include "nebula_hw_interfaces/nebula_hw_interfaces_common/connections/byte_stream.hpp" +#include "nebula_hw_interfaces/nebula_hw_interfaces_common/connections/byte_view.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace nebula::drivers::connections +{ + +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); +} + +enum class AevaStreamType : uint16_t { + kSphericalPointCloud = 0, + kHealth = 1, + kConfig = 2, + kTelemetry = 3, + kVelocityEstimate = 4, + kCalibration = 5, + kImage = 6, + kReconfig = 7, + kVehicleStateEstimate = 8, + kLog = 9, + kImu = 10, + kObjectList = 12, + kEstimatedDetectionRange = 33, + kUnknown = 0xFFFFu +}; + +using nebula::drivers::aeva::MessageHeader; +using nebula::drivers::aeva::SomeIpHeader; + +template +T pull_and_parse( + const std::vector::const_iterator & cbegin, + const std::vector::const_iterator & cend) +{ + if (std::distance(cbegin, cend) != sizeof(T)) { + throw std::runtime_error("Number of bytes provided does not match type's size."); + } + + T result{}; + memcpy(&result, &*cbegin, sizeof(T)); + return result; +} + +template +T pull_and_parse(const std::vector & stream) +{ + return pull_and_parse(stream.cbegin(), stream.cend()); +} + +template +T pull_and_parse(PullableByteStream & stream) +{ + std::vector buffer; + stream.read(buffer, sizeof(T)); + return pull_and_parse(buffer); +} + +template +T pull_and_parse(ByteView & stream) +{ + auto consumed = stream.consume(sizeof(T)).value_or_throw(); + return pull_and_parse(consumed.cbegin(), consumed.cend()); +} + +template +void serialize(std::vector & out_bytes, const T & object) +{ + size_t old_size = out_bytes.size(); + size_t new_size = old_size + sizeof(T); + out_bytes.resize(new_size); + memcpy(&out_bytes.at(old_size), &object, sizeof(T)); +} + +/** + * @brief Handles a single datastream from an Aeva sensor. While this API can + * decode any supported stream type, routing several different streams through + * one API instance is not supported. Instead, create one instance per data + * stream. + */ +template +class AevaParser : public ObservableByteStream +{ +public: + explicit AevaParser(std::shared_ptr incoming_byte_stream) + : incoming_(std::move(incoming_byte_stream)) + { + if (!incoming_) { + throw std::runtime_error("Incoming byte stream cannot be null"); + } + + thread_ = std::thread([&]() { + while (true) onLowLevelMessage(); + }); + } + + void registerBytesCallback(callback_t callback) override + { + std::lock_guard lock(mtx_bytes_callback_); + 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() + { + std::lock_guard lock(mtx_bytes_callback_); + + std::vector some_ip_raw; + incoming_->read(some_ip_raw, sizeof(SomeIpHeader)); + if (bytes_callback_) { + bytes_callback_(some_ip_raw); + } + + auto some_ip = pull_and_parse(some_ip_raw); + expect_eq(some_ip.service_id, 0xAEFAu, "Aeva service header mismatch"); + expect_eq(some_ip.method_id, StreamId, "Unexpected method ID"); + auto payload_length = some_ip.message_length - 12; + + std::vector payload_raw; + incoming_->read(payload_raw, payload_length); + if (bytes_callback_) { + bytes_callback_(payload_raw); + } + + auto payload_view = ByteView(payload_raw); + + auto message_header = pull_and_parse(payload_view); + expect_eq(message_header.message_type, StreamId, "Unexpected message type"); + expect_eq(message_header.message_length, payload_length, "Payload size mismatch"); + + onMessage(message_header, payload_view); + } + + virtual void onMessage(const MessageHeader & message_header, ByteView & payload_bytes) = 0; + +private: + std::thread thread_; + + std::shared_ptr incoming_; + + std::mutex mtx_bytes_callback_; + callback_t bytes_callback_{}; +}; + +template +class AevaSender +{ +public: + explicit AevaSender(std::shared_ptr outgoing_byte_stream) + : outgoing_(std::move(outgoing_byte_stream)) + { + } + +protected: + /** + * @brief Sends the serialized message payload `bytes` over the outgoing byte stream, prepending + * the SomeIp and Message headers. + */ + void sendMessage(std::vector bytes) + { + sequence_number_++; + + std::vector out_bytes; + out_bytes.reserve(sizeof(SomeIpHeader) + sizeof(MessageHeader) + bytes.size()); + + auto some_ip_payload_size = static_cast(bytes.size() + sizeof(MessageHeader) + 12); + SomeIpHeader some_ip{ + 0xAEFA, + static_cast(StreamId), + some_ip_payload_size, + 0xFFF0, + sequence_number_, + 2, + 1, + 1, + 0, + 0, + 0}; + + serialize(out_bytes, some_ip); + + auto message_length = static_cast(sizeof(MessageHeader) + bytes.size()); + MessageHeader message_header{1, 1, static_cast(StreamId), 0, message_length, 0, 0}; + serialize(out_bytes, message_header); + out_bytes.insert(out_bytes.end(), bytes.begin(), bytes.end()); + + outgoing_->write(out_bytes); + } + +private: + std::shared_ptr outgoing_; + uint16_t sequence_number_{}; +}; + +} // namespace nebula::drivers::connections From 56bdf16bc1a8aed5b4a0e92fa4edbdf9e5ce4de5 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 26 Jun 2024 19:53:41 +0900 Subject: [PATCH 09/57] feat(aeva): add parsers for most important data streams --- .../connections/health.hpp | 75 ++++++ .../connections/pointcloud.hpp | 70 ++++++ .../connections/reconfig.hpp | 217 ++++++++++++++++++ .../connections/telemetry.hpp | 197 ++++++++++++++++ 4 files changed, 559 insertions(+) create mode 100644 nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/health.hpp create mode 100644 nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/pointcloud.hpp create mode 100644 nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/reconfig.hpp create mode 100644 nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/telemetry.hpp diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/health.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/health.hpp new file mode 100644 index 000000000..d2c652716 --- /dev/null +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/health.hpp @@ -0,0 +1,75 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include "nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/aeva_api.hpp" + +#include + +#include + +#include +#include +#include +#include +#include +#include + +namespace nebula::drivers::connections +{ + +using namespace boost::endian; // NOLINT + +class HealthParser : public AevaParser +{ +public: + using callback_t = std::function)>; + + explicit HealthParser(std::shared_ptr incoming_byte_stream) + : AevaParser(std::move(incoming_byte_stream)) + { + } + + void registerCallback(callback_t callback) { callback_ = std::move(callback); } + +protected: + void onMessage(const MessageHeader & message_header, ByteView & payload_bytes) override + { + auto n_entries = pull_and_parse(payload_bytes); + + expect_eq( + n_entries * sizeof(uint32_t), + message_header.message_length - sizeof(MessageHeader) - sizeof(n_entries), + "Unexpected size of health code list"); + + std::vector entries; + entries.reserve(n_entries); + + for (size_t i = 0; i < n_entries; ++i) { + auto pointer = &*payload_bytes.consumeUnsafe(sizeof(uint32_t)).cbegin(); + auto entry = load_little_u32(pointer); + entries.push_back(entry); + } + + if (callback_) { + callback_(entries); + } + } + +private: + callback_t callback_{}; +}; + +} // namespace nebula::drivers::connections diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/pointcloud.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/pointcloud.hpp new file mode 100644 index 000000000..4d4f51724 --- /dev/null +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/pointcloud.hpp @@ -0,0 +1,70 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include "nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/aeva_api.hpp" + +#include + +#include +#include + +namespace nebula::drivers::connections +{ + +using nebula::drivers::aeva::PointCloudMessage; +using nebula::drivers::aeva::PointcloudMsgSubheaderAndMetadata; +using nebula::drivers::aeva::PointCloudPoint; + +class PointcloudParser : public AevaParser +{ +public: + using callback_t = std::function; + + explicit PointcloudParser(std::shared_ptr incoming_byte_stream) + : AevaParser(std::move(incoming_byte_stream)) + { + } + + void registerCallback(callback_t callback) { callback_ = std::move(callback); } + +protected: + void onMessage(const MessageHeader & message_header, ByteView & payload_bytes) override + { + PointCloudMessage message{}; + message.header = pull_and_parse(payload_bytes); + expect_eq(message.header.aeva_marker, 0xAE5Au, "Pointcloud Aeva marker mismatch"); + expect_eq( + message.header.n_entries * sizeof(PointCloudPoint), + message_header.message_length - sizeof(MessageHeader) - + sizeof(PointcloudMsgSubheaderAndMetadata), + "Payload bytes and point bytes mismatch"); + + message.points.reserve(message.header.n_entries); + for (size_t i = 0; i < message.header.n_entries; ++i) { + auto point = pull_and_parse(payload_bytes); + message.points.emplace_back(point); + } + + if (callback_) { + callback_(message); + } + } + +private: + callback_t callback_{}; +}; + +} // namespace nebula::drivers::connections diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/reconfig.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/reconfig.hpp new file mode 100644 index 000000000..960f83eb9 --- /dev/null +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/reconfig.hpp @@ -0,0 +1,217 @@ +// 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 + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace nebula::drivers::connections +{ + +using aeva::ReconfigMessage; +using aeva::ReconfigRequestType; +using nlohmann::json; +using namespace std::chrono_literals; // NOLINT + +class ReconfigParser : public AevaParser, + public AevaSender +{ +public: + using callback_t = std::function; + + explicit ReconfigParser( + std::shared_ptr incoming_byte_stream, + std::shared_ptr outgoing_byte_stream) + : AevaParser(std::move(incoming_byte_stream)), + AevaSender(std::move(outgoing_byte_stream)) + { + } + + json getManifest() + { + ReconfigMessage request{ReconfigRequestType::kManifestRequest, {}}; + ReconfigMessage response = doRequest(request); + + if (!response.body) { + throw std::runtime_error("Expected manifest body but got empty response"); + } + + return *response.body; + } + + json setParameter(std::string node_name, std::string key, json value) + { + json request_body = {{node_name, {{key, {{"value", value}}}}}}; + std::cerr << "Sending request with body: \n" << request_body.dump(2) << std::endl; + ReconfigMessage request = {ReconfigRequestType::kChangeRequest, request_body}; + + ReconfigMessage response = doRequest(request); + if (response.type != aeva::ReconfigRequestType::kChangeApproved) { + std::stringstream ss{}; + ss << "change request failed"; + if (response.body) { + ss << " with body: " << *response.body << std::endl; + } + + throw std::runtime_error(ss.str()); + } + + if (!response.body) { + throw std::runtime_error("Expected change request response body but got empty response"); + } + + return *response.body; + } + +private: + ReconfigMessage doRequest(ReconfigMessage request) + { + std::lock_guard lock(mtx_inflight_); + + // //////////////////////////////////////// + // Build request headers and serialize + // //////////////////////////////////////// + + uint64_t request_id = std::rand(); + { + std::lock_guard lock(mtx_inflight_request_id_); + inflight_request_id_.emplace(request_id); + } + + ReconfigRequestType type = request.type; + uint8_t encoding_type = 0; + uint16_t reserved = 0; + + std::vector message_payload{}; + if (request.body) { + std::string body_string = nlohmann::to_string(*request.body); + message_payload = std::vector(body_string.begin(), body_string.end()); + } + + uint32_t data_size = message_payload.size(); + + std::vector bytes; + bytes.reserve( + sizeof(request_id) + sizeof(type) + sizeof(encoding_type) + sizeof(reserved) + + sizeof(data_size) + data_size); + serialize(bytes, request_id); + serialize(bytes, type); + serialize(bytes, encoding_type); + serialize(bytes, reserved); + serialize(bytes, data_size); + bytes.insert(bytes.end(), message_payload.begin(), message_payload.end()); + + // //////////////////////////////////////// + // Send and wait for response with timeout + // //////////////////////////////////////// + + std::timed_mutex tm; + tm.lock(); + + ReconfigMessage response{}; + + callback_ = [this, &tm, request_id, &response](const auto & message) { + std::lock_guard lock(mtx_inflight_request_id_); + if (!inflight_request_id_ || inflight_request_id_.value() != request_id) { + // Spurious message reached this callback, the references to `response` and `tm` might not + // be valid Exit without trying to access possibly invalid references and let request + // timeout + return; + } + + response = message; + tm.unlock(); + }; + + sendMessage(bytes); + auto request_timed_out = !tm.try_lock_for(3s); + + { + std::lock_guard lock(mtx_inflight_request_id_); + inflight_request_id_.reset(); + } + + if (request_timed_out) { + throw std::runtime_error("Request timed out"); + } + + // //////////////////////////////////////// + // Do validation and return response + // //////////////////////////////////////// + + bool response_type_valid = (request.type == aeva::ReconfigRequestType::kManifestRequest && + response.type == aeva::ReconfigRequestType::kManifestResponse) || + (request.type == aeva::ReconfigRequestType::kChangeRequest && + (response.type == aeva::ReconfigRequestType::kChangeApproved || + response.type == aeva::ReconfigRequestType::kChangeIgnored)); + + if (!response_type_valid) { + throw std::runtime_error("Invalid response type received"); + } + + return response; + } + +protected: + void onMessage(const MessageHeader & message_header, ByteView & payload_bytes) override + { + ReconfigMessage message{}; + + auto request_id = pull_and_parse(payload_bytes); + message.type = pull_and_parse(payload_bytes); + auto encoding_type = pull_and_parse(payload_bytes); + expect_eq(encoding_type, 0, "Unsupported encoding type"); + + payload_bytes.consume(2).value_or_throw(); + + auto data_size = pull_and_parse(payload_bytes); + auto data_raw = payload_bytes.consume(data_size).value_or_throw(); + + expect_eq( + data_size, + message_header.message_length - sizeof(MessageHeader) - sizeof(request_id) - + sizeof(message.type) - sizeof(encoding_type) - 2 - sizeof(data_size), + "Unexpected data size field"); + + message.body = nlohmann::json::parse(data_raw.cbegin(), data_raw.cend()); + + if (callback_) { + callback_(message); + } + } + +private: + callback_t callback_{}; + std::mutex mtx_inflight_{}; + std::mutex mtx_inflight_request_id_{}; + std::optional inflight_request_id_{}; +}; + +} // 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 new file mode 100644 index 000000000..f133ab5df --- /dev/null +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/telemetry.hpp @@ -0,0 +1,197 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include "nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/aeva_api.hpp" +#include "nebula_hw_interfaces/nebula_hw_interfaces_common/connections/byte_view.hpp" + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace nebula::drivers::connections +{ + +using nebula::drivers::aeva::TelemetryDataType; +using nlohmann::json; + +using namespace boost::endian; // NOLINT + +namespace telemetry_detail +{ + +const std::vector TYPE_OVERRIDES = { + "display_all_points", + "hfov_adjustment_deg", + "hfov_rotation_deg", + "PTP_master_offset_in_ns", + "target_below_or_near_min_range", +}; + +template +inline std::vector parse_number_array( + std::function f, const ByteView::Slice & bytes) +{ + ssize_t type_size = sizeof(OutT); + if (bytes.size() % type_size != 0) { + throw std::runtime_error("Buffer length is not divisible by requested type's size"); + } + + size_t n_entries = bytes.size() / type_size; + std::vector result{}; + result.reserve(n_entries); + + for (auto it = bytes.cbegin(); it != bytes.cend(); it += type_size) { + result.emplace_back(f(&*it)); + } + + return result; +} + +inline std::string parse_string(const ByteView::Slice & bytes) +{ + return {bytes.cbegin(), bytes.cend()}; +} + +} // namespace telemetry_detail + +class TelemetryParser : public AevaParser +{ +public: + using callback_t = std::function; + + explicit TelemetryParser(std::shared_ptr incoming_byte_stream) + : AevaParser(std::move(incoming_byte_stream)) + { + } + + void registerCallback(callback_t callback) { callback_ = std::move(callback); } + +protected: + void onMessage(const MessageHeader & message_header, ByteView & payload_bytes) override + { + auto payload_size = pull_and_parse(payload_bytes); + + auto node_name_size = pull_and_parse(payload_bytes); + payload_bytes.consume(3).value_or_throw(); // reserved + + expect_eq( + payload_size, + message_header.message_length - sizeof(MessageHeader) - sizeof(payload_size) - + sizeof(node_name_size) - 3, + "Unexpected payload size field"); + + auto node_name_raw = payload_bytes.consume(node_name_size).value_or_throw(); + auto node_name = std::string(node_name_raw.cbegin(), node_name_raw.cend()); + + payload_size -= node_name_size; + + json entries = json::object(); + + while (payload_size > 0) { + auto type = pull_and_parse(payload_bytes); + payload_bytes.consume(8).value_or_throw(); + auto entry_key_size = pull_and_parse(payload_bytes); + auto entry_key_raw = payload_bytes.consume(entry_key_size).value_or_throw(); + auto key = std::string(entry_key_raw.cbegin(), entry_key_raw.cend()); + auto entry_data_size = pull_and_parse(payload_bytes); + auto entry_data_raw = payload_bytes.consume(entry_data_size).value_or_throw(); + + json value; + switch (type) { + case aeva::TelemetryDataType::kUInt8: + value = telemetry_detail::parse_number_array( + [](const auto * ref) { return *ref; }, entry_data_raw); + case aeva::TelemetryDataType::kInt8: + value = telemetry_detail::parse_number_array( + [](const auto * ref) { return static_cast(*ref); }, entry_data_raw); + case aeva::TelemetryDataType::kUInt16: + value = telemetry_detail::parse_number_array(&load_little_u16, entry_data_raw); + case aeva::TelemetryDataType::kInt16: + value = telemetry_detail::parse_number_array(&load_little_s16, entry_data_raw); + case aeva::TelemetryDataType::kUInt32: + value = telemetry_detail::parse_number_array(&load_little_u32, entry_data_raw); + case aeva::TelemetryDataType::kInt32: + value = telemetry_detail::parse_number_array(&load_little_s32, entry_data_raw); + case aeva::TelemetryDataType::kUInt64: + value = telemetry_detail::parse_number_array(&load_little_u64, entry_data_raw); + case aeva::TelemetryDataType::kInt64: + value = telemetry_detail::parse_number_array(&load_little_s64, entry_data_raw); + case aeva::TelemetryDataType::kFloat: + value = telemetry_detail::parse_number_array( + [](const uint8_t * ref) { + auto raw_bytes = load_little_u32(ref); + float result{}; + memcpy(&result, &raw_bytes, 4); + return result; + }, + entry_data_raw); + case aeva::TelemetryDataType::kDouble: + value = telemetry_detail::parse_number_array( + [](const uint8_t * ref) { + auto raw_bytes = load_little_u64(ref); + double result{}; + memcpy(&result, &raw_bytes, 8); + return result; + }, + entry_data_raw); + case aeva::TelemetryDataType::kChar: + auto overrides = telemetry_detail::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; + } + + value = static_cast(raw_value); + } else { + value = telemetry_detail::parse_string(entry_data_raw); + } + break; + } + + if (value.is_array() && value.size() == 1) { + value = value[0]; + } + + entries[key] = value; + payload_size -= 1 + 8 + 1 + entry_key_size + 4 + entry_data_size; + } + + expect_eq(payload_size, 0, "Payload and payload size mismatch"); + + json result = {{node_name, entries}}; + + if (callback_) { + callback_(result); + } + } + +private: + callback_t callback_{}; +}; + +} // namespace nebula::drivers::connections From 15f00ca0b40ab78e9d8ccd6a3063862bd0163247 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 26 Jun 2024 19:54:19 +0900 Subject: [PATCH 10/57] feat(aeva): add hardware interface --- nebula_hw_interfaces/CMakeLists.txt | 14 ++ .../aeva_hw_interface.hpp | 212 ++++++++++++++++++ .../aeva_hw_interface.cpp | 24 ++ 3 files changed, 250 insertions(+) create mode 100644 nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/aeva_hw_interface.hpp create mode 100644 nebula_hw_interfaces/src/nebula_aeva_hw_interfaces/aeva_hw_interface.cpp diff --git a/nebula_hw_interfaces/CMakeLists.txt b/nebula_hw_interfaces/CMakeLists.txt index 2577ea9a5..59bb9cd8a 100644 --- a/nebula_hw_interfaces/CMakeLists.txt +++ b/nebula_hw_interfaces/CMakeLists.txt @@ -91,10 +91,23 @@ target_include_directories(nebula_hw_interfaces_continental PUBLIC ${ros2_socketcan_INCLUDE_DIRS} ) +add_library(nebula_hw_interfaces_aeva SHARED + src/nebula_aeva_hw_interfaces/aeva_hw_interface.cpp +) +target_link_libraries(nebula_hw_interfaces_aeva PUBLIC + ${boost_tcp_driver_LIBRARIES} + ${boost_udp_driver_LIBRARIES} +) +target_include_directories(nebula_hw_interfaces_aeva PUBLIC + ${boost_tcp_driver_INCLUDE_DIRS} + ${boost_udp_driver_INCLUDE_DIRS} +) + install(TARGETS nebula_hw_interfaces_hesai EXPORT export_nebula_hw_interfaces_hesai) install(TARGETS nebula_hw_interfaces_velodyne EXPORT export_nebula_hw_interfaces_velodyne) install(TARGETS nebula_hw_interfaces_robosense EXPORT export_nebula_hw_interfaces_robosense) install(TARGETS nebula_hw_interfaces_continental EXPORT export_nebula_hw_interfaces_continental) +install(TARGETS nebula_hw_interfaces_aeva EXPORT export_nebula_hw_interfaces_aeva) install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) if(BUILD_TESTING) @@ -107,6 +120,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..24bc21550 --- /dev/null +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/aeva_hw_interface.hpp @@ -0,0 +1,212 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include "nebula_common/util/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" +#include "nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/telemetry.hpp" +#include "nebula_hw_interfaces/nebula_hw_interfaces_common/connections/byte_stream.hpp" +#include "nebula_hw_interfaces/nebula_hw_interfaces_common/connections/tcp_receiver.hpp" +#include "nebula_hw_interfaces/nebula_hw_interfaces_common/connections/tcp_sender.hpp" + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace nebula::drivers +{ + +using connections::HealthParser; +using connections::PointcloudParser; +using connections::ReconfigParser; +using connections::TcpSender; +using connections::TcpStream; +using connections::TelemetryParser; +using nlohmann::json; + +class AevaHwInterface +{ +public: + /** + * @brief Construct a new AevaHwInterface which tries to connect to the sensor specified in + * `config`. Throws on connection failure. + * + * @param logger the logger the HW interface will log to + * @param config The configuration containing the sensor IP, whether to not to setup the sensor, + * and the sensor settings + */ + AevaHwInterface( + std::shared_ptr logger, bool setup_sensor, + const std::shared_ptr & config) + : AevaHwInterface( + std::move(logger), setup_sensor, config, makepointcloudApi(*config), + makeTelemetryApi(*config), makeReconfigApi(*config), makeHealthApi(*config)) + { + } + + /** + * @brief Construct a new AevaHwInterface which takes in given instances of the API endpoints it + * should use. This is useful for testing or offline playback. + * + * @param logger The logger the HW interface uses + * @param config The sensor configuration + * @param pointcloud_api The pointcloud endpoint. Can be null. + * @param telemetry_api The telemetry endpoint. Can be null. + * @param reconfig_api The reconfig endpoint. Can be null. + * @param health_api The health endpoint. Can be null. + */ + AevaHwInterface( + 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)), + pointcloud_api_(std::move(pointcloud_api)), + telemetry_api_(std::move(telemetry_api)), + reconfig_api_(std::move(reconfig_api)), + health_api_(std::move(health_api)) + { + if (setup_sensor_ && reconfig_api_) { + logger_->info("Configuring sensor..."); + onConfigChange(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); + } + } + + if (manifest.is_null()) { + throw std::runtime_error("Reached maximum retries while trying to fetch manifest"); + } + + tryReconfig(manifest, "scanner", "frame_sync_type", config->frame_sync_type); + tryReconfig(manifest, "scanner", "frame_sync_offset_in_ms", config->frame_sync_offset_in_ms); + tryReconfig( + manifest, "scanner", "frame_synchronization_on_rising_edge", + config->frame_synchronization_on_rising_edge); + tryReconfig(manifest, "scanner", "enable_frame_sync", config->enable_frame_sync); + } + + void registerCloudPacketCallback(PointcloudParser::callback_t callback) + { + pointcloud_api_->registerCallback(std::move(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)); + } + +private: + template + void tryReconfig( + const json & manifest, const std::string & node_name, const std::string & key, const T & 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 + // //////////////////////////////////////// + + std::string value_str; + if constexpr (std::is_same_v) { + value_str = value; + } else { + value_str = std::to_string(value); + } + + logger_->info("Set " + node_name + "." + key + " to " + value_str); + } + + static std::shared_ptr makeReconfigApi(const aeva::Aeries2Config & config) + { + return std::make_shared( + std::make_shared(config.sensor_ip, 41007), + std::make_shared(config.sensor_ip, 21901)); + } + + static std::shared_ptr makepointcloudApi(const aeva::Aeries2Config & config) + { + return std::make_shared(std::make_shared(config.sensor_ip, 41000)); + } + + static std::shared_ptr makeHealthApi(const aeva::Aeries2Config & config) + { + return std::make_shared(std::make_shared(config.sensor_ip, 41001)); + } + + static std::shared_ptr makeTelemetryApi(const aeva::Aeries2Config & config) + { + return std::make_shared(std::make_shared(config.sensor_ip, 41003)); + } + + const bool setup_sensor_; + std::shared_ptr logger_; + std::shared_ptr pointcloud_api_; + std::shared_ptr telemetry_api_; + std::shared_ptr reconfig_api_; + std::shared_ptr health_api_; + + // Fetching the sensor manifest sometimes times out. In those cases, retry the below number of + // times. + static const uint8_t MANIFEST_RETRIES = 3; +}; + +} // namespace nebula::drivers 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 From 6f96519858c70aec2719cf61b5876c9597088fb2 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 26 Jun 2024 19:56:32 +0900 Subject: [PATCH 11/57] test(aeva): add hw interface test --- nebula_tests/CMakeLists.txt | 12 +- nebula_tests/aeva/CMakeLists.txt | 24 +++ nebula_tests/aeva/aeva_hw_interface_test.cpp | 38 ++++ nebula_tests/aeva/aeva_hw_interface_test.hpp | 13 ++ nebula_tests/aeva/aeva_test_main.cpp | 21 ++ nebula_tests/data/aeva/tcp_stream.hpp | 204 +++++++++++++++++++ 6 files changed, 309 insertions(+), 3 deletions(-) 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/data/aeva/tcp_stream.hpp diff --git a/nebula_tests/CMakeLists.txt b/nebula_tests/CMakeLists.txt index b2adaef13..f50411055 100644 --- a/nebula_tests/CMakeLists.txt +++ b/nebula_tests/CMakeLists.txt @@ -60,20 +60,26 @@ if(BUILD_TESTING) ) 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..bfe1f4b41 --- /dev/null +++ b/nebula_tests/aeva/aeva_hw_interface_test.cpp @@ -0,0 +1,38 @@ +// Copyright 2024 TIER IV, Inc. + +#include "aeva_hw_interface_test.hpp" + +#include "../common/mock_byte_stream.hpp" +#include "../data/aeva/tcp_stream.hpp" +#include "nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/pointcloud.hpp" + +#include +#include + +#include + +using nebula::drivers::aeva::PointCloudMessage; +using nebula::drivers::connections::PointcloudParser; + +TEST(TestParsing, Pointcloud) // NOLINT +{ + size_t n_callback_invocations = 0; + + PointcloudParser::callback_t callback = [&](const PointCloudMessage & arg) { + EXPECT_EQ(n_callback_invocations, 0); + + EXPECT_EQ(arg.header.aeva_marker, 0xAE5Au); + EXPECT_EQ(arg.header.platform, 2); + + EXPECT_GT(arg.points.size(), 0); + + n_callback_invocations++; + }; + + auto mock_byte_stream = std::make_shared(STREAM); + + PointcloudParser parser(mock_byte_stream); + parser.registerCallback(std::move(callback)); + + EXPECT_EQ(n_callback_invocations, 1); +} 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/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}}; From 9ce7ba62f88c241e8e6df800428af5d57178f101 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 26 Jun 2024 19:57:00 +0900 Subject: [PATCH 12/57] feat(aeva): add decoder --- nebula_decoders/CMakeLists.txt | 22 ++++++ .../aeva_aries2_decoder.hpp | 69 ++++++++++++++++++ .../aeva_aries2_decoder.cpp | 73 +++++++++++++++++++ 3 files changed, 164 insertions(+) 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 diff --git a/nebula_decoders/CMakeLists.txt b/nebula_decoders/CMakeLists.txt index e94afcf2a..1043c706e 100644 --- a/nebula_decoders/CMakeLists.txt +++ b/nebula_decoders/CMakeLists.txt @@ -115,11 +115,32 @@ target_include_directories(nebula_decoders_continental PUBLIC ${nebula_msgs_INCLUDE_DIRS} ) +# Aeva +add_library(nebula_decoders_aeva SHARED +src/nebula_decoders_aeva/aeva_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) @@ -133,6 +154,7 @@ ament_export_targets(export_nebula_decoders_velodyne) ament_export_targets(export_nebula_decoders_robosense) ament_export_targets(export_nebula_decoders_robosense_info) ament_export_targets(export_nebula_decoders_continental) +ament_export_targets(export_nebula_decoders_aeva) install( DIRECTORY calibration diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_aeva/aeva_aries2_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_aeva/aeva_aries2_decoder.hpp new file mode 100644 index 000000000..16aff95b5 --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_aeva/aeva_aries2_decoder.hpp @@ -0,0 +1,69 @@ +// 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 + +namespace nebula::drivers +{ + +class AevaAries2Decoder +{ +public: + using AevaPoint = aeva::PointXYZVIRCAEDT; + using AevaPointCloud = pcl::PointCloud; + using AevaPointCloudUniquePtr = std::unique_ptr; + + using callback_t = std::function; + + AevaAries2Decoder() : cloud_state_({std::make_unique(), 0}) {} + + void processPointcloudMessage(const aeva::PointCloudMessage & message); + + void registerPointCloudCallback(callback_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; + }; + + callback_t callback_{}; + PointcloudState cloud_state_{}; + + std::mutex mtx_callback_; +}; +} // 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..b83889170 --- /dev/null +++ b/nebula_decoders/src/nebula_decoders_aeva/aeva_aries2_decoder.cpp @@ -0,0 +1,73 @@ +// Copyright 2024 TIER IV, Inc. + +#include "nebula_decoders/nebula_decoders_aeva/aeva_aries2_decoder.hpp" + +namespace nebula::drivers +{ + +void AevaAries2Decoder::processPointcloudMessage(const aeva::PointCloudMessage & message) +{ + DecoderState state{ + message.header.frame_sync_index, message.header.ns_per_index, message.header.line_index, 0, + message.header.first_point_timestamp_ns}; + + for (size_t i = 0; i < message.points.size(); ++i) { + const auto & raw_point = message.points[i]; + + // Point time increases every time a marker point (beam=0, peak=0) is encountered + if (raw_point.beam_id == 0 && raw_point.peak_id == 0) { + state.absolute_time_ns += state.time_per_marker_point_ns; + } + + if (static_cast(i) == state.new_frame_index) { + std::scoped_lock lock(mtx_callback_); + + if (callback_) { + callback_(std::move(cloud_state_.cloud), cloud_state_.timestamp); + } + // Cloud time gets reset below, on the first VALID point that will be + // put in the cloud. This guarantees that the earliest point(s) in the cloud + // have relative time 0 + cloud_state_ = {std::make_unique(), 0}; + } + + if (raw_point.line_transition) { + state.line_index++; + } + + if (!raw_point.valid || raw_point.signal_quality < 60) { + continue; + } + + if (cloud_state_.cloud->empty()) { + cloud_state_.timestamp = state.absolute_time_ns; + } + + AevaPoint point; + + point.distance = raw_point.range.value(); + point.azimuth = raw_point.azimuth.value() * M_PI_2f; + point.elevation = raw_point.elevation.value() * M_PI_4f; + + 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 = state.absolute_time_ns - cloud_state_.timestamp; + point.channel = 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 From 2db7929896c2efbc1122e17a751969e6e58e4b14 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 26 Jun 2024 19:57:41 +0900 Subject: [PATCH 13/57] feat(aeva): add ros wrapper --- nebula_ros/CMakeLists.txt | 24 ++ .../nebula_ros/aeva/aeva_ros_wrapper.hpp | 76 ++++++ .../nebula_ros/aeva/hw_monitor_wrapper.hpp | 90 +++++++ .../common/nebula_packet_stream.hpp | 58 +++++ nebula_ros/src/aeva/aeva_ros_wrapper.cpp | 246 ++++++++++++++++++ nebula_ros/src/aeva/hw_monitor_wrapper.cpp | 176 +++++++++++++ 6 files changed, 670 insertions(+) create mode 100644 nebula_ros/include/nebula_ros/aeva/aeva_ros_wrapper.hpp create mode 100644 nebula_ros/include/nebula_ros/aeva/hw_monitor_wrapper.hpp create mode 100644 nebula_ros/include/nebula_ros/common/nebula_packet_stream.hpp create mode 100644 nebula_ros/src/aeva/aeva_ros_wrapper.cpp create mode 100644 nebula_ros/src/aeva/hw_monitor_wrapper.cpp diff --git a/nebula_ros/CMakeLists.txt b/nebula_ros/CMakeLists.txt index 709de2025..19c4b085c 100644 --- a/nebula_ros/CMakeLists.txt +++ b/nebula_ros/CMakeLists.txt @@ -215,11 +215,34 @@ rclcpp_components_register_node(continental_srr520_ros_wrapper EXECUTABLE continental_srr520_ros_wrapper_node ) +## Aeva +add_library(aeva_ros_wrapper SHARED + src/aeva/aeva_ros_wrapper.cpp + src/aeva/hw_monitor_wrapper.cpp + src/common/parameter_descriptors.cpp +) + +target_include_directories(aeva_ros_wrapper PUBLIC + ${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 continental_srr520_ros_wrapper EXPORT export_continental_srr520_ros_wrapper) +install(TARGETS aeva_ros_wrapper EXPORT export_aeva_ros_wrapper) install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) if(BUILD_TESTING) @@ -232,6 +255,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..e8932970f --- /dev/null +++ b/nebula_ros/include/nebula_ros/aeva/aeva_ros_wrapper.hpp @@ -0,0 +1,76 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include "nebula_ros/aeva/hw_monitor_wrapper.hpp" +#include "nebula_ros/common/watchdog_timer.hpp" + +#include +#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: + Status declareAndGetSensorConfigParams(); + Status validateAndSetConfig(std::shared_ptr & new_config); + + void recordRawPacket(const std::vector & bytes); + + rclcpp::Publisher::SharedPtr packets_pub_{}; + std::mutex mtx_current_scan_msg_; + nebula_msgs::msg::NebulaPackets::UniquePtr current_scan_msg_{}; + + rclcpp::Publisher::SharedPtr cloud_pub_{}; + std::shared_ptr cloud_watchdog_; + + rclcpp::Subscription::SharedPtr packets_sub_{}; + + std::shared_ptr sensor_cfg_ptr_; + + std::optional hw_interface_; + std::optional hw_monitor_; + drivers::AevaAries2Decoder decoder_{}; +}; + +} // namespace nebula::ros diff --git a/nebula_ros/include/nebula_ros/aeva/hw_monitor_wrapper.hpp b/nebula_ros/include/nebula_ros/aeva/hw_monitor_wrapper.hpp new file mode 100644 index 000000000..1cd9bf9d5 --- /dev/null +++ b/nebula_ros/include/nebula_ros/aeva/hw_monitor_wrapper.hpp @@ -0,0 +1,90 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace nebula::ros +{ + +using nlohmann::json; + +class AevaHwMonitorWrapper +{ +public: + AevaHwMonitorWrapper( + rclcpp::Node * const parent_node, std::shared_ptr config); + + void onTelemetryFragment(const json & diff); + + void onHealthCodes(std::vector health_codes); + +private: + struct NodeTelemetry + { + json values; + rclcpp::Time last_update; + }; + + struct TelemetryState + { + std::map entries; + }; + + struct HealthState + { + std::vector codes; + rclcpp::Time last_update; + }; + + void publishDiagnostics(); + + rclcpp::Logger logger_; + rclcpp::Node * const parent_node_; + std::shared_ptr config_; + + rclcpp::Publisher::SharedPtr diagnostics_pub_{}; + rclcpp::TimerBase::SharedPtr diagnostics_pub_timer_{}; + uint16_t diag_span_; + + std::mutex mtx_hardware_id_; + std::optional hardware_id_; + + std::mutex mtx_telemetry_; + TelemetryState telemetry_; + + std::mutex mtx_health_; + std::optional health_; +}; +} // namespace nebula::ros diff --git a/nebula_ros/include/nebula_ros/common/nebula_packet_stream.hpp b/nebula_ros/include/nebula_ros/common/nebula_packet_stream.hpp new file mode 100644 index 000000000..ff9b6a53b --- /dev/null +++ b/nebula_ros/include/nebula_ros/common/nebula_packet_stream.hpp @@ -0,0 +1,58 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include "nebula_hw_interfaces/nebula_hw_interfaces_common/connections/byte_stream.hpp" + +#include +#include + +#include "nebula_msgs/msg/detail/nebula_packets__struct.hpp" + +#include +#include +#include +#include + +namespace nebula::ros +{ + +class NebulaPacketStream : public drivers::connections::ObservableByteStream +{ +public: + NebulaPacketStream() = default; + + void registerBytesCallback(callback_t callback) override + { + std::lock_guard lock(mtx_callback_); + callback_ = std::move(callback); + } + + void onNebulaPackets(std::unique_ptr packets) + { + std::lock_guard lock(mtx_callback_); + if (!callback_) return; + + for (auto & packet : packets->packets) { + callback_(packet.data); + } + } + +private: + std::mutex mtx_callback_{}; + callback_t callback_{}; +}; + +} // namespace nebula::ros diff --git a/nebula_ros/src/aeva/aeva_ros_wrapper.cpp b/nebula_ros/src/aeva/aeva_ros_wrapper.cpp new file mode 100644 index 000000000..c16001d60 --- /dev/null +++ b/nebula_ros/src/aeva/aeva_ros_wrapper.cpp @@ -0,0 +1,246 @@ +// Copyright 2024 TIER IV, Inc. + +#include "nebula_ros/aeva/aeva_ros_wrapper.hpp" + +#include "nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/pointcloud.hpp" +#include "nebula_ros/aeva/hw_monitor_wrapper.hpp" +#include "nebula_ros/common/nebula_packet_stream.hpp" +#include "nebula_ros/common/parameter_descriptors.hpp" +#include "nebula_ros/common/rclcpp_logger.hpp" +#include "nebula_ros/common/watchdog_timer.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#pragma clang diagnostic ignored "-Wbitwise-instead-of-logical" + +namespace nebula::ros +{ +using drivers::AevaAries2Decoder; +using drivers::aeva::Aeries2Config; +using drivers::connections::PointcloudParser; +using nlohmann::json; +using namespace std::chrono_literals; // NOLINT +using AevaPointCloudUniquePtr = AevaAries2Decoder::AevaPointCloudUniquePtr; + +AevaRosWrapper::AevaRosWrapper(const rclcpp::NodeOptions & options) +: rclcpp::Node("aeva_ros_wrapper", rclcpp::NodeOptions(options).use_intra_process_comms(true)) +{ + auto status = declareAndGetSensorConfigParams(); + + if (status != Status::OK) { + throw std::runtime_error( + (std::stringstream{} << "Sensor configuration invalid: " << status).str()); + } + + RCLCPP_INFO_STREAM(get_logger(), "SensorConfig: " << *sensor_cfg_ptr_); + + auto qos_profile = rmw_qos_profile_sensor_data; + auto pointcloud_qos = + rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), qos_profile); + + auto launch_hw = declare_parameter("launch_hw", param_read_only()); + auto setup_sensor = declare_parameter("setup_sensor", param_read_only()); + auto hw_interface_logger = drivers::loggers::RclcppLogger(get_logger()).child("HwInterface"); + + if (!launch_hw && setup_sensor) { + setup_sensor = false; + RCLCPP_WARN(get_logger(), "Ignoring setup_sensor:=true in offline mode."); + } + + if (launch_hw) { + // //////////////////////////////////////// + // If HW is connected, also publish packets + // //////////////////////////////////////// + hw_interface_.emplace(hw_interface_logger, setup_sensor, sensor_cfg_ptr_); + hw_monitor_.emplace(this, sensor_cfg_ptr_); + + packets_pub_ = + create_publisher("nebula_packets", pointcloud_qos); + + drivers::connections::ObservableByteStream::callback_t raw_packet_cb = [&](const auto & bytes) { + this->recordRawPacket(std::move(bytes)); + }; + + hw_interface_->registerRawCloudPacketCallback(std::move(raw_packet_cb)); + + hw_interface_->registerTelemetryCallback( + [&](const auto & msg) { hw_monitor_->onTelemetryFragment(msg); }); + hw_interface_->registerHealthCallback([&](auto codes) { hw_monitor_->onHealthCodes(codes); }); + } else { + // //////////////////////////////////////// + // If HW is disconnected, subscribe to + // packets topic + // //////////////////////////////////////// + auto packet_stream = std::make_shared(); + auto packet_buffer = + std::make_shared(packet_stream, 100, [&]() { + RCLCPP_ERROR_THROTTLE( + get_logger(), *get_clock(), 1000, + "Packet stream buffer overflowed, packet loss occurred."); + }); + + packets_sub_ = create_subscription( + "nebula_packets", rclcpp::SensorDataQoS(), + [=](std::unique_ptr packets) { + packet_stream->onNebulaPackets(std::move(packets)); + }); + + auto pointcloud_parser = std::make_shared(packet_buffer); + hw_interface_.emplace( + hw_interface_logger, setup_sensor, sensor_cfg_ptr_, pointcloud_parser, nullptr, nullptr, + nullptr); + + RCLCPP_INFO_STREAM( + get_logger(), + "Hardware connection disabled, listening for packets on " << packets_sub_->get_topic_name()); + } + + RCLCPP_DEBUG(get_logger(), "Starting stream"); + + PointcloudParser::callback_t pointcloud_message_cb = [this](const auto & message) { + decoder_.processPointcloudMessage(message); + }; + + hw_interface_->registerCloudPacketCallback(std::move(pointcloud_message_cb)); + + cloud_pub_ = create_publisher("/nebula_points", pointcloud_qos); + + cloud_watchdog_ = std::make_shared(*this, 100'000us, [&](bool ok) { + if (ok) return; + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "Missed pointcloud output deadline"); + }); + + { + AevaAries2Decoder::callback_t pointcloud_cb = + [&](AevaPointCloudUniquePtr cloud_ptr, auto timestamp) { + auto now = this->now(); + cloud_watchdog_->update(); + + if ( + cloud_pub_->get_subscription_count() > 0 || + cloud_pub_->get_intra_process_subscription_count() > 0) { + auto ros_pc_msg_ptr = std::make_unique(); + pcl::toROSMsg(*cloud_ptr, *ros_pc_msg_ptr); + ros_pc_msg_ptr->header.frame_id = sensor_cfg_ptr_->frame_id; + ros_pc_msg_ptr->header.stamp = rclcpp::Time(static_cast(timestamp)); + cloud_pub_->publish(std::move(ros_pc_msg_ptr)); + } + + std::lock_guard lock(mtx_current_scan_msg_); + if (current_scan_msg_ && packets_pub_) { + packets_pub_->publish(std::move(current_scan_msg_)); + current_scan_msg_ = {}; + } + }; + + decoder_.registerPointCloudCallback(std::move(pointcloud_cb)); + } +} + +Status AevaRosWrapper::declareAndGetSensorConfigParams() +{ + Aeries2Config config; + + std::string raw_sensor_model = declare_parameter("sensor_model", param_read_only()); + config.sensor_model = drivers::SensorModelFromString(raw_sensor_model); + config.sensor_ip = declare_parameter("sensor_ip", param_read_only()); + config.frame_id = declare_parameter("frame_id", param_read_only()); + config.dithering_enable_ego_speed = + declare_parameter("dithering_enable_ego_speed", param_read_only()); + config.dithering_pattern_option = + declare_parameter("dithering_pattern_option", param_read_only()); + config.ele_offset_rad = declare_parameter("ele_offset_rad", param_read_only()); + config.elevation_auto_adjustment = + declare_parameter("elevation_auto_adjustment", param_read_only()); + config.enable_frame_dithering = + declare_parameter("enable_frame_dithering", param_read_only()); + config.enable_frame_sync = declare_parameter("enable_frame_sync", param_read_only()); + config.flip_pattern_vertically = + declare_parameter("flip_pattern_vertically", param_read_only()); + config.frame_sync_offset_in_ms = + declare_parameter("frame_sync_offset_in_ms", param_read_only()); + config.frame_sync_type = declare_parameter("frame_sync_type", param_read_only()); + config.frame_synchronization_on_rising_edge = + declare_parameter("frame_synchronization_on_rising_edge", param_read_only()); + config.hfov_adjustment_deg = declare_parameter("hfov_adjustment_deg", param_read_only()); + config.hfov_rotation_deg = declare_parameter("hfov_rotation_deg", param_read_only()); + config.highlight_ROI = declare_parameter("highlight_ROI", param_read_only()); + config.horizontal_fov_degrees = + declare_parameter("horizontal_fov_degrees", param_read_only()); + config.roi_az_offset_rad = declare_parameter("roi_az_offset_rad", param_read_only()); + config.vertical_pattern = declare_parameter("vertical_pattern", param_read_only()); + + auto new_cfg_ptr = std::make_shared(config); + return validateAndSetConfig(new_cfg_ptr); +} + +Status AevaRosWrapper::validateAndSetConfig(std::shared_ptr & new_config) +{ + if (!new_config) { + return Status::SENSOR_CONFIG_ERROR; + } + + if (new_config->sensor_model == nebula::drivers::SensorModel::UNKNOWN) { + return Status::INVALID_SENSOR_MODEL; + } + + if (new_config->frame_id.empty()) { + return Status::SENSOR_CONFIG_ERROR; + } + + if (hw_interface_) { + hw_interface_->onConfigChange(new_config); + } + + sensor_cfg_ptr_ = new_config; + return Status::OK; +} + +void AevaRosWrapper::recordRawPacket(const std::vector & vector) +{ + std::lock_guard lock(mtx_current_scan_msg_); + + if ( + !packets_pub_ || (packets_pub_->get_subscription_count() == 0 && + packets_pub_->get_intra_process_subscription_count() == 0)) { + return; + } + + auto packet_stamp = now(); + + if (!current_scan_msg_) { + current_scan_msg_ = std::make_unique(); + auto & header = current_scan_msg_->header; + header.frame_id = sensor_cfg_ptr_->frame_id; + header.stamp = packet_stamp; + } + + auto & packet = current_scan_msg_->packets.emplace_back(); + packet.stamp = packet_stamp; + packet.data = vector; +} + +RCLCPP_COMPONENTS_REGISTER_NODE(AevaRosWrapper) +} // namespace nebula::ros diff --git a/nebula_ros/src/aeva/hw_monitor_wrapper.cpp b/nebula_ros/src/aeva/hw_monitor_wrapper.cpp new file mode 100644 index 000000000..90682ffe3 --- /dev/null +++ b/nebula_ros/src/aeva/hw_monitor_wrapper.cpp @@ -0,0 +1,176 @@ +// Copyright 2024 TIER IV, Inc. + +#include "nebula_ros/aeva/hw_monitor_wrapper.hpp" + +#include "nebula_ros/common/parameter_descriptors.hpp" + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace nebula::ros +{ + +using diagnostic_msgs::msg::DiagnosticStatus; +using nebula::util::get_if_exists; + +AevaHwMonitorWrapper::AevaHwMonitorWrapper( + rclcpp::Node * const parent_node, std::shared_ptr config) +: logger_(parent_node->get_logger().get_child("HwMonitor")), + parent_node_(parent_node), + config_(std::move(config)) +{ + diag_span_ = parent_node->declare_parameter("diag_span", 500, param_read_only()); + + diagnostics_pub_ = + parent_node->create_publisher("/diagnostics", 10); + + diagnostics_pub_timer_ = parent_node->create_wall_timer( + std::chrono::milliseconds(diag_span_), [&]() { publishDiagnostics(); }); +} + +void AevaHwMonitorWrapper::onTelemetryFragment(const json & diff) +{ + std::scoped_lock lock(mtx_telemetry_, mtx_hardware_id_); + + // //////////////////////////////////////// + // Update aggregated telemetry tree + // //////////////////////////////////////// + + auto now = parent_node_->now(); + bool any_changed = false; + for (const auto & node : diff.items()) { + bool new_node = telemetry_.entries.count(node.key()) == 0; + bool node_changed = !new_node && telemetry_.entries.at(node.key()).values != diff[node.key()]; + + if (new_node) { + telemetry_.entries[node.key()] = {json::object(), now}; + } + + if (new_node || node_changed) { + any_changed = true; + auto & entry = telemetry_.entries[node.key()]; + entry.values.update(diff[node.key()]); + entry.last_update = now; + } + } + + if (!any_changed) { + return; + } + + // //////////////////////////////////////// + // Initialize hardware ID if unset + // //////////////////////////////////////// + + if (!hardware_id_) { + auto serial = get_if_exists(diff, {"sysinfo_main", "serial_number"}); + auto model = get_if_exists(diff, {"sysinfo_main", "platform"}); + + if (!serial || !model) return; + + hardware_id_.emplace(*model + ":" + *serial); + } +} + +void AevaHwMonitorWrapper::onHealthCodes(std::vector health_codes) +{ + std::scoped_lock lock(mtx_health_, mtx_hardware_id_); + + if (!hardware_id_) { + return; + } + + auto now = parent_node_->now(); + health_ = {std::move(health_codes), now}; +} + +void AevaHwMonitorWrapper::publishDiagnostics() +{ + std::scoped_lock lock(mtx_health_, mtx_telemetry_, mtx_hardware_id_); + + if (!hardware_id_) { + return; + } + + auto now = parent_node_->now(); + + diagnostic_msgs::msg::DiagnosticArray diagnostic_array_msg; + diagnostic_array_msg.header.stamp.sec = static_cast(now.seconds()); + diagnostic_array_msg.header.stamp.nanosec = now.nanoseconds() % 1'000'000'000; + diagnostic_array_msg.header.frame_id = config_->frame_id; + + auto diag_entry = [](const std::string & key, const std::string & value) { + diagnostic_msgs::msg::KeyValue entry{}; + entry.key = key; + entry.value = value; + return entry; + }; + + // //////////////////////////////////////// + // Add telemetry to diagnostics + // //////////////////////////////////////// + + for (const auto & entry : telemetry_.entries) { + auto & status = diagnostic_array_msg.status.emplace_back(); + status.level = DiagnosticStatus::OK; + status.hardware_id = *hardware_id_; + status.name = "aeva.telemetry." + entry.first; + + for (const auto & item : entry.second.values.items()) { + auto value = item.value(); + std::string stringified = + value.is_string() ? value.template get() : value.dump(); + status.values.emplace_back(diag_entry(item.key(), stringified)); + } + } + + // //////////////////////////////////////// + // Add health codes to diagnostics + // //////////////////////////////////////// + + auto & status = diagnostic_array_msg.status.emplace_back(); + status.level = DiagnosticStatus::OK; + status.hardware_id = *hardware_id_; + status.name = "aeva.health"; + + if (health_) { + auto & codes = health_->codes; + + json warning_codes = json::array(); + json error_codes = json::array(); + + std::copy_if(codes.cbegin(), codes.cend(), std::back_inserter(warning_codes), [](auto code) { + return !drivers::aeva::is_error_code(code); + }); + + std::copy_if( + codes.cbegin(), codes.cend(), std::back_inserter(error_codes), drivers::aeva::is_error_code); + + if (!error_codes.empty()) { + status.level = DiagnosticStatus::ERROR; + } else if (!warning_codes.empty()) { + status.level = DiagnosticStatus::WARN; + } + + status.values.emplace_back(diag_entry("warning_codes", warning_codes.dump())); + status.values.emplace_back(diag_entry("error_codes", error_codes.dump())); + } + + diagnostics_pub_->publish(diagnostic_array_msg); +} + +} // namespace nebula::ros From d23c93d3b3a11dcf3819e0f9c1885a11e1880137 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 26 Jun 2024 19:58:14 +0900 Subject: [PATCH 14/57] feat(aeva): add launch and config files, fix cspell --- .cspell.json | 2 + .../config/lidar/aeva/Aeries2.param.yaml | 24 ++ nebula_ros/launch/aeva_launch_all_hw.xml | 10 + nebula_ros/schema/Aeries2.schema.json | 209 ++++++++++++++++++ nebula_ros/schema/sub/lidar_aeva.json | 13 ++ 5 files changed, 258 insertions(+) create mode 100644 nebula_ros/config/lidar/aeva/Aeries2.param.yaml create mode 100644 nebula_ros/launch/aeva_launch_all_hw.xml create mode 100644 nebula_ros/schema/Aeries2.schema.json create mode 100644 nebula_ros/schema/sub/lidar_aeva.json diff --git a/.cspell.json b/.cspell.json index 4c14cb9f3..3bce77d74 100644 --- a/.cspell.json +++ b/.cspell.json @@ -5,6 +5,7 @@ "words": [ "adctp", "Adctp", + "aeva", "applicate", "AT", "autosar", @@ -21,6 +22,7 @@ "gptp", "Helios", "Hesai", + "hfov", "horiz", "Idat", "ipaddr", diff --git a/nebula_ros/config/lidar/aeva/Aeries2.param.yaml b/nebula_ros/config/lidar/aeva/Aeries2.param.yaml new file mode 100644 index 000000000..33bcdd04f --- /dev/null +++ b/nebula_ros/config/lidar/aeva/Aeries2.param.yaml @@ -0,0 +1,24 @@ +/**: + ros__parameters: + sensor_ip: 192.168.2.201 + launch_hw: true + frame_id: aeva + sensor_model: Aeries2 + setup_sensor: true + diag_span: 1000 + dithering_enable_ego_speed: 40.0 + dithering_pattern_option: dithering_pattern_1 + ele_offset_rad: 0.0 + elevation_auto_adjustment: false + enable_frame_dithering: false + enable_frame_sync: true + flip_pattern_vertically: false + frame_sync_offset_in_ms: 999 + frame_sync_type: nearest_second + frame_synchronization_on_rising_edge: true + hfov_adjustment_deg: 0.0 + hfov_rotation_deg: 0.0 + highlight_ROI: false + horizontal_fov_degrees: 110Degrees + roi_az_offset_rad: 0.0 + vertical_pattern: 40-14.4-ROI diff --git a/nebula_ros/launch/aeva_launch_all_hw.xml b/nebula_ros/launch/aeva_launch_all_hw.xml new file mode 100644 index 000000000..20fcbbb7f --- /dev/null +++ b/nebula_ros/launch/aeva_launch_all_hw.xml @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/nebula_ros/schema/Aeries2.schema.json b/nebula_ros/schema/Aeries2.schema.json new file mode 100644 index 000000000..f1b0b3d1a --- /dev/null +++ b/nebula_ros/schema/Aeries2.schema.json @@ -0,0 +1,209 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "LiDAR Aeva Aeries II parameters.", + "type": "object", + "definitions": { + "Aeries2": { + "type": "object", + "properties": { + "sensor_ip": { + "$ref": "sub/communication.json#/definitions/sensor_ip" + }, + "launch_hw": { + "$ref": "sub/hardware.json#/definitions/launch_hw" + }, + "frame_id": { + "$ref": "sub/topic.json#/definitions/frame_id" + }, + "sensor_model": { + "$ref": "sub/lidar_aeva.json#/definitions/sensor_model" + }, + "setup_sensor": { + "$ref": "sub/hardware.json#/definitions/setup_sensor" + }, + "diag_span": { + "$ref": "sub/topic.json#/definitions/diag_span" + }, + "dithering_enable_ego_speed": { + "type": "number", + "readOnly": false, + "description": "The ego speed in m/s from which pointcloud dithering (different patterns per frame) is enabled.", + "max": 200.0, + "min": 0.0, + "default": 40.0 + }, + "dithering_pattern_option": { + "type": "string", + "readOnly": false, + "description": "The dithering pattern to use if enabled.", + "enum": [ + "dithering_pattern_1", + "dithering_pattern_2" + ], + "default": "dithering_pattern_1" + }, + "ele_offset_rad": { + "type": "number", + "readOnly": false, + "description": "Elevation offset used to align ROI to ground level at a certain distance.", + "max": 0.35, + "min": -0.35, + "default": 0.0 + }, + "elevation_auto_adjustment": { + "type": "boolean", + "readOnly": false, + "description": "Adjusts elevation to a long-range setting when the car is going sufficiently fast.", + "default": false + }, + "enable_frame_dithering": { + "type": "boolean", + "readOnly": false, + "description": "Whether dithering is enabled.", + "default": false + }, + "enable_frame_sync": { + "type": "boolean", + "readOnly": false, + "description": "Whether to sync pointcloud frame output to a given sub-second offset.", + "default": true + }, + "flip_pattern_vertically": { + "type": "boolean", + "readOnly": false, + "description": "Whether to flip the scan pattern vertically.", + "default": false + }, + "frame_sync_offset_in_ms": { + "type": "integer", + "readOnly": false, + "description": "The sub-second offset to sync pointcloud frame starts to. 0 for top-of-second alignment. In practice, 999 achieves results closer to top-of-second", + "min": 0, + "max": 999, + "default": 999 + }, + "frame_sync_type": { + "type": "string", + "readOnly": false, + "description": "Whether to start sync at an absolute timestamp in the future, or from the nearest second.", + "enum": [ + "absolute_time", + "nearest_second" + ], + "default": "nearest_second" + }, + "frame_synchronization_on_rising_edge": { + "type": "boolean", + "readOnly": false, + "description": "Contact vendor for info.", + "default": true + }, + "hfov_adjustment_deg": { + "type": "number", + "readOnly": false, + "description": "The number of degrees to expend or shrink the horizontal field of view by.", + "max": 100.0, + "min": -100.0, + "default": 0.0 + }, + "hfov_rotation_deg": { + "type": "number", + "readOnly": false, + "description": "Number of degrees to shift the horizontal field of view to the left (-) or right (+).", + "max": 100.0, + "min": -100.0, + "default": 0.0 + }, + "highlight_ROI": { + "type": "boolean", + "readOnly": false, + "description": "A debug option to visually highlight points in the configured region of interest.", + "default": false + }, + "horizontal_fov_degrees": { + "type": "string", + "readOnly": false, + "description": "The horizontal field of view of the sensor. Larger FoVs use more power.", + "enum": [ + "110Degrees", + "120Degrees" + ], + "default": "110Degrees" + }, + "roi_az_offset_rad": { + "type": "number", + "readOnly": false, + "description": "Shift the ROI (if configured) to the left (-) or right (+).", + "max": 0.2094, + "min": -0.2094, + "default": 0.0 + }, + "vertical_pattern": { + "type": "string", + "readOnly": false, + "description": "The scan pattern to use. Refer to the user manual for details.", + "enum": [ + "40-12-ROI-1m", + "40-14.4-ROI", + "40-14.4-ROI-1m", + "40-14.4-ROI-EGL", + "40-28.8-ROI", + "64-14.4-ROI-1m", + "64-16-ROI", + "64-16-ROI-1m", + "64-19.2-ROI-1m", + "64-19.2-ROI-2-EGL", + "64-28.8-ROI", + "80-19.2-Uniform", + "80-24-ROI-2", + "cal_192_384-19.2-Uniform-5f" + ], + "default": "40-14.4-ROI" + } + }, + "required": [ + "sensor_ip", + "launch_hw", + "frame_id", + "sensor_model", + "setup_sensor", + "diag_span", + "dithering_enable_ego_speed", + "dithering_pattern_option", + "ele_offset_rad", + "elevation_auto_adjustment", + "enable_frame_dithering", + "enable_frame_sync", + "flip_pattern_vertically", + "frame_sync_offset_in_ms", + "frame_sync_type", + "frame_synchronization_on_rising_edge", + "hfov_adjustment_deg", + "hfov_rotation_deg", + "highlight_ROI", + "horizontal_fov_degrees", + "roi_az_offset_rad", + "vertical_pattern" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/Aeries2" + } + }, + "required": [ + "ros__parameters" + ], + "additionalProperties": false + } + }, + "required": [ + "/**" + ], + "additionalProperties": false +} diff --git a/nebula_ros/schema/sub/lidar_aeva.json b/nebula_ros/schema/sub/lidar_aeva.json new file mode 100644 index 000000000..96458e843 --- /dev/null +++ b/nebula_ros/schema/sub/lidar_aeva.json @@ -0,0 +1,13 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Lidar Aeva parameters.", + "type": "object", + "definitions": { + "sensor_model": { + "$ref": "hardware.json#/definitions/sensor_model", + "enum": [ + "Aeries2" + ] + } + } +} From e00572c12dc97ea2898f041bd234fc035df29dfc Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Thu, 27 Jun 2024 15:38:15 +0900 Subject: [PATCH 15/57] docs(aeva): added to supported sensors --- docs/supported_sensors.md | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/docs/supported_sensors.md b/docs/supported_sensors.md index 407713e00..64b7e6adc 100644 --- a/docs/supported_sensors.md +++ b/docs/supported_sensors.md @@ -41,6 +41,12 @@ The `sensor_model` parameter below decides which sensor driver is launched. | Bpearl | Bpearl | Bpearl.param.yaml | ⚠️ | | Helios | Helios | Helios.param.yaml | ⚠️ | +## Aeva LiDARs + +| Model | `sensor_model` | Configuration file | Test status | +| --------- | -------------- | ------------------ | ----------- | +| Aeries II | Aeries2 | Aeries2.param.yaml | ⚠️ | + ## Continental radars | Model | `sensor_model` | Configuration file | Test status | From fdaac59c55f98e1c286a19153cf3623d2ea44beb Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Thu, 27 Jun 2024 16:09:07 +0900 Subject: [PATCH 16/57] docs(aeva): add XYZVIRCAEDT point type --- docs/point_types.md | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/docs/point_types.md b/docs/point_types.md index 16ce6e0ce..4780593fe 100644 --- a/docs/point_types.md +++ b/docs/point_types.md @@ -20,6 +20,22 @@ These definitions can be found in the `nebula_common/include/point_types.hpp`. | `distance` | `float` | `m` | Distance from the sensor origin. | | `timestamp` | `uint32` | `ns` | Time of detection relative to the pointcloud timestamp. | +## PointXYZVIRCAEDT + +| Field | Type | Units | Description | +| ------------- | -------- | ----- | --------------------------------------------------------------------------- | +| `x` | `float` | `m` | The point's cartesian x coordinate. | +| `y` | `float` | `m` | The point's cartesian y coordinate. | +| `z` | `float` | `m` | The point's cartesian z coordinate. | +| `v` | `float` | `m/s` | The point's velocity component in the direction of the sensor's origin. | +| `intensity` | `uint8` | | The intensity of the return as reported by the sensor. | +| `return type` | `uint8` | | Whether the point was the first, strongest, last, etc. of multiple returns. | +| `channel` | `uint16` | | The ID of the laser channel that produced the point. | +| `azimuth` | `float` | `rad` | The point's azimuth in polar coordinates. | +| `elevation` | `float` | `rad` | The point's elevation in polar coordinates. | +| `distance` | `float` | `m` | The point's distance from the sensor origin. | +| `timestamp` | `uint32` | `ns` | The time the point was detected relative to the pointcloud timestamp. | + ## [Deprecated] PointXYZIR | Field | Type | Units | Description | From 9441d9db75bb0716de141d8e0d46911161c8b718 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Fri, 28 Jun 2024 13:43:31 +0900 Subject: [PATCH 17/57] fix(aeries2_decoder): un-mirror the pointcloud --- .../src/nebula_decoders_aeva/aeva_aries2_decoder.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nebula_decoders/src/nebula_decoders_aeva/aeva_aries2_decoder.cpp b/nebula_decoders/src/nebula_decoders_aeva/aeva_aries2_decoder.cpp index b83889170..84bd30e84 100644 --- a/nebula_decoders/src/nebula_decoders_aeva/aeva_aries2_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_aeva/aeva_aries2_decoder.cpp @@ -46,7 +46,7 @@ void AevaAries2Decoder::processPointcloudMessage(const aeva::PointCloudMessage & AevaPoint point; point.distance = raw_point.range.value(); - point.azimuth = raw_point.azimuth.value() * M_PI_2f; + point.azimuth = -raw_point.azimuth.value() * M_PI_2f; point.elevation = raw_point.elevation.value() * M_PI_4f; float xy_distance = point.distance * std::cos(point.elevation); From 3b941662fbd5159361f2fdce8e50c98c8a54e2e1 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Fri, 28 Jun 2024 14:12:13 +0900 Subject: [PATCH 18/57] refactor(aeva)!: rename `v` point field to `distance_rate` --- docs/point_types.md | 26 +++++++++---------- .../nebula_common/aeva/point_types.hpp | 10 +++---- .../aeva_aries2_decoder.cpp | 2 +- 3 files changed, 19 insertions(+), 19 deletions(-) diff --git a/docs/point_types.md b/docs/point_types.md index 4780593fe..e01a9f24b 100644 --- a/docs/point_types.md +++ b/docs/point_types.md @@ -22,19 +22,19 @@ These definitions can be found in the `nebula_common/include/point_types.hpp`. ## PointXYZVIRCAEDT -| Field | Type | Units | Description | -| ------------- | -------- | ----- | --------------------------------------------------------------------------- | -| `x` | `float` | `m` | The point's cartesian x coordinate. | -| `y` | `float` | `m` | The point's cartesian y coordinate. | -| `z` | `float` | `m` | The point's cartesian z coordinate. | -| `v` | `float` | `m/s` | The point's velocity component in the direction of the sensor's origin. | -| `intensity` | `uint8` | | The intensity of the return as reported by the sensor. | -| `return type` | `uint8` | | Whether the point was the first, strongest, last, etc. of multiple returns. | -| `channel` | `uint16` | | The ID of the laser channel that produced the point. | -| `azimuth` | `float` | `rad` | The point's azimuth in polar coordinates. | -| `elevation` | `float` | `rad` | The point's elevation in polar coordinates. | -| `distance` | `float` | `m` | The point's distance from the sensor origin. | -| `timestamp` | `uint32` | `ns` | The time the point was detected relative to the pointcloud timestamp. | +| Field | Type | Units | Description | +| --------------- | -------- | ----- | --------------------------------------------------------------------------- | +| `x` | `float` | `m` | The point's cartesian x coordinate. | +| `y` | `float` | `m` | The point's cartesian y coordinate. | +| `z` | `float` | `m` | The point's cartesian z coordinate. | +| `distance_rate` | `float` | `m/s` | The point's velocity component in the direction of the sensor's origin. | +| `intensity` | `uint8` | | The intensity of the return as reported by the sensor. | +| `return type` | `uint8` | | Whether the point was the first, strongest, last, etc. of multiple returns. | +| `channel` | `uint16` | | The ID of the laser channel that produced the point. | +| `azimuth` | `float` | `rad` | The point's azimuth in polar coordinates. | +| `elevation` | `float` | `rad` | The point's elevation in polar coordinates. | +| `distance` | `float` | `m` | The point's distance from the sensor origin. | +| `timestamp` | `uint32` | `ns` | The time the point was detected relative to the pointcloud timestamp. | ## [Deprecated] PointXYZIR diff --git a/nebula_common/include/nebula_common/aeva/point_types.hpp b/nebula_common/include/nebula_common/aeva/point_types.hpp index 8a78b82cc..ac4834412 100644 --- a/nebula_common/include/nebula_common/aeva/point_types.hpp +++ b/nebula_common/include/nebula_common/aeva/point_types.hpp @@ -24,7 +24,7 @@ struct EIGEN_ALIGN16 PointXYZVIRCAEDT float x; float y; float z; - float v; + float distance_rate; uint8_t intensity; uint8_t return_type; uint16_t channel; @@ -39,7 +39,7 @@ struct EIGEN_ALIGN16 PointXYZVIRCAEDT 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)) + (float, x, x)(float, y, y)(float, z, z)(float, distance_rate, distance_rate)( + std::uint8_t, intensity, intensity)(std::uint8_t, return_type, return_type)( + std::uint16_t, channel, channel)(float, azimuth, azimuth)(float, elevation, elevation)( + float, distance, distance)(std::uint32_t, time_stamp, time_stamp)) diff --git a/nebula_decoders/src/nebula_decoders_aeva/aeva_aries2_decoder.cpp b/nebula_decoders/src/nebula_decoders_aeva/aeva_aries2_decoder.cpp index 84bd30e84..7e9ab3529 100644 --- a/nebula_decoders/src/nebula_decoders_aeva/aeva_aries2_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_aeva/aeva_aries2_decoder.cpp @@ -54,7 +54,7 @@ void AevaAries2Decoder::processPointcloudMessage(const aeva::PointCloudMessage & point.y = xy_distance * std::cos(point.azimuth); point.z = point.distance * std::sin(point.elevation); - point.v = raw_point.velocity.value(); + point.distance_rate = raw_point.velocity.value(); point.intensity = raw_point.intensity; point.time_stamp = state.absolute_time_ns - cloud_state_.timestamp; From 1ee0c92873a3d90eff281226a8fa6de264eedd8a Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Fri, 28 Jun 2024 14:59:12 +0900 Subject: [PATCH 19/57] fix(nebula_ros): remove erroneous header import --- nebula_ros/src/aeva/aeva_ros_wrapper.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/nebula_ros/src/aeva/aeva_ros_wrapper.cpp b/nebula_ros/src/aeva/aeva_ros_wrapper.cpp index c16001d60..2e43be3ba 100644 --- a/nebula_ros/src/aeva/aeva_ros_wrapper.cpp +++ b/nebula_ros/src/aeva/aeva_ros_wrapper.cpp @@ -23,7 +23,6 @@ #include #include -#include #include #include From 903e1b7fd22be4931c531d55cacdea8e888c1a5a Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Mon, 1 Jul 2024 16:29:32 +0900 Subject: [PATCH 20/57] fix(aeva): prevent fallthrough in telemetry parsing switch --- .../connections/telemetry.hpp | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) 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 f133ab5df..a700de3c6 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 @@ -124,21 +124,29 @@ class TelemetryParser : public AevaParser case aeva::TelemetryDataType::kUInt8: value = telemetry_detail::parse_number_array( [](const auto * ref) { return *ref; }, entry_data_raw); + break; case aeva::TelemetryDataType::kInt8: value = telemetry_detail::parse_number_array( [](const auto * ref) { return static_cast(*ref); }, entry_data_raw); + break; case aeva::TelemetryDataType::kUInt16: value = telemetry_detail::parse_number_array(&load_little_u16, entry_data_raw); + break; case aeva::TelemetryDataType::kInt16: value = telemetry_detail::parse_number_array(&load_little_s16, entry_data_raw); + break; case aeva::TelemetryDataType::kUInt32: value = telemetry_detail::parse_number_array(&load_little_u32, entry_data_raw); + break; case aeva::TelemetryDataType::kInt32: value = telemetry_detail::parse_number_array(&load_little_s32, entry_data_raw); + break; case aeva::TelemetryDataType::kUInt64: value = telemetry_detail::parse_number_array(&load_little_u64, entry_data_raw); + break; case aeva::TelemetryDataType::kInt64: value = telemetry_detail::parse_number_array(&load_little_s64, entry_data_raw); + break; case aeva::TelemetryDataType::kFloat: value = telemetry_detail::parse_number_array( [](const uint8_t * ref) { @@ -148,6 +156,7 @@ class TelemetryParser : public AevaParser return result; }, entry_data_raw); + break; case aeva::TelemetryDataType::kDouble: value = telemetry_detail::parse_number_array( [](const uint8_t * ref) { @@ -157,6 +166,7 @@ class TelemetryParser : public AevaParser return result; }, entry_data_raw); + break; case aeva::TelemetryDataType::kChar: auto overrides = telemetry_detail::TYPE_OVERRIDES; bool has_override = std::find(overrides.begin(), overrides.end(), key) != overrides.end(); @@ -191,7 +201,7 @@ class TelemetryParser : public AevaParser } private: - callback_t callback_{}; + callback_t callback_; }; } // namespace nebula::drivers::connections From cc353ceb01dbaca03e284282db0e0368d0e53435 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Thu, 4 Jul 2024 15:06:58 +0900 Subject: [PATCH 21/57] fix(aeva): fix starving when registering raw packet callback --- .../nebula_hw_interfaces_aeva/connections/aeva_api.hpp | 6 +----- .../nebula_hw_interfaces_aeva/connections/health.hpp | 2 +- .../nebula_hw_interfaces_aeva/connections/pointcloud.hpp | 2 +- 3 files changed, 3 insertions(+), 7 deletions(-) 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 0a1f7c4ec..4390a3beb 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 @@ -164,7 +164,6 @@ class AevaParser : public ObservableByteStream void registerBytesCallback(callback_t callback) override { - std::lock_guard lock(mtx_bytes_callback_); bytes_callback_ = std::move(callback); } @@ -175,8 +174,6 @@ class AevaParser : public ObservableByteStream */ void onLowLevelMessage() { - std::lock_guard lock(mtx_bytes_callback_); - std::vector some_ip_raw; incoming_->read(some_ip_raw, sizeof(SomeIpHeader)); if (bytes_callback_) { @@ -210,8 +207,7 @@ class AevaParser : public ObservableByteStream std::shared_ptr incoming_; - std::mutex mtx_bytes_callback_; - callback_t bytes_callback_{}; + callback_t bytes_callback_; }; template 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 d2c652716..694dc074f 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 @@ -69,7 +69,7 @@ class HealthParser : public AevaParser } private: - callback_t callback_{}; + callback_t callback_; }; } // namespace nebula::drivers::connections 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 4d4f51724..a983d42b3 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 @@ -64,7 +64,7 @@ class PointcloudParser : public AevaParser } private: - callback_t callback_{}; + callback_t callback_; }; } // namespace nebula::drivers::connections From 9394cb62e479c00323f42a8729f77c1c26b2885e Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Thu, 4 Jul 2024 15:08:00 +0900 Subject: [PATCH 22/57] chore(aeva): allow for slightly lower pointcloud frequency, more visible logging --- nebula_ros/src/aeva/aeva_ros_wrapper.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nebula_ros/src/aeva/aeva_ros_wrapper.cpp b/nebula_ros/src/aeva/aeva_ros_wrapper.cpp index 2e43be3ba..46b9b9a63 100644 --- a/nebula_ros/src/aeva/aeva_ros_wrapper.cpp +++ b/nebula_ros/src/aeva/aeva_ros_wrapper.cpp @@ -116,7 +116,7 @@ AevaRosWrapper::AevaRosWrapper(const rclcpp::NodeOptions & options) "Hardware connection disabled, listening for packets on " << packets_sub_->get_topic_name()); } - RCLCPP_DEBUG(get_logger(), "Starting stream"); + RCLCPP_INFO(get_logger(), "Starting stream"); PointcloudParser::callback_t pointcloud_message_cb = [this](const auto & message) { decoder_.processPointcloudMessage(message); @@ -126,7 +126,7 @@ AevaRosWrapper::AevaRosWrapper(const rclcpp::NodeOptions & options) cloud_pub_ = create_publisher("/nebula_points", pointcloud_qos); - cloud_watchdog_ = std::make_shared(*this, 100'000us, [&](bool ok) { + cloud_watchdog_ = std::make_shared(*this, 110'000us, [&](bool ok) { if (ok) return; RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "Missed pointcloud output deadline"); }); From b30950863962116ba635d583e327ebbb9da1c1cf Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Thu, 4 Jul 2024 15:21:27 +0900 Subject: [PATCH 23/57] chore(aeva)!: change `distance_rate` point field to `range_rate` --- .../include/nebula_common/aeva/point_types.hpp | 11 ++++++----- .../src/nebula_decoders_aeva/aeva_aries2_decoder.cpp | 2 +- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/nebula_common/include/nebula_common/aeva/point_types.hpp b/nebula_common/include/nebula_common/aeva/point_types.hpp index ac4834412..c16298eb9 100644 --- a/nebula_common/include/nebula_common/aeva/point_types.hpp +++ b/nebula_common/include/nebula_common/aeva/point_types.hpp @@ -24,7 +24,7 @@ struct EIGEN_ALIGN16 PointXYZVIRCAEDT float x; float y; float z; - float distance_rate; + float range_rate; uint8_t intensity; uint8_t return_type; uint16_t channel; @@ -39,7 +39,8 @@ struct EIGEN_ALIGN16 PointXYZVIRCAEDT POINT_CLOUD_REGISTER_POINT_STRUCT( // NOLINT nebula::drivers::aeva::PointXYZVIRCAEDT, - (float, x, x)(float, y, y)(float, z, z)(float, distance_rate, distance_rate)( - std::uint8_t, intensity, intensity)(std::uint8_t, return_type, return_type)( - std::uint16_t, channel, channel)(float, azimuth, azimuth)(float, elevation, elevation)( - float, distance, distance)(std::uint32_t, time_stamp, time_stamp)) + (float, x, + x)(float, y, y)(float, z, z)(float, range_rate, range_rate)(std::uint8_t, intensity, intensity)( + std::uint8_t, return_type, + return_type)(std::uint16_t, channel, channel)(float, azimuth, azimuth)( + float, elevation, elevation)(float, distance, distance)(std::uint32_t, time_stamp, time_stamp)) diff --git a/nebula_decoders/src/nebula_decoders_aeva/aeva_aries2_decoder.cpp b/nebula_decoders/src/nebula_decoders_aeva/aeva_aries2_decoder.cpp index 7e9ab3529..8e98edf58 100644 --- a/nebula_decoders/src/nebula_decoders_aeva/aeva_aries2_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_aeva/aeva_aries2_decoder.cpp @@ -54,7 +54,7 @@ void AevaAries2Decoder::processPointcloudMessage(const aeva::PointCloudMessage & point.y = xy_distance * std::cos(point.azimuth); point.z = point.distance * std::sin(point.elevation); - point.distance_rate = raw_point.velocity.value(); + point.range_rate = raw_point.velocity.value(); point.intensity = raw_point.intensity; point.time_stamp = state.absolute_time_ns - cloud_state_.timestamp; From 8964f6b17221b5e6c2b69f56d04a7ff562bd58e7 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Thu, 4 Jul 2024 15:44:49 +0900 Subject: [PATCH 24/57] fix(aeva): output return types --- .../aeva_aries2_decoder.hpp | 3 ++- .../aeva_aries2_decoder.cpp | 20 ++++++++++++++++++- 2 files changed, 21 insertions(+), 2 deletions(-) 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 index 16aff95b5..5c17e991d 100644 --- 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 @@ -16,6 +16,7 @@ #include #include +#include #include #include @@ -61,7 +62,7 @@ class AevaAries2Decoder uint64_t timestamp; }; - callback_t callback_{}; + callback_t callback_; PointcloudState cloud_state_{}; std::mutex mtx_callback_; diff --git a/nebula_decoders/src/nebula_decoders_aeva/aeva_aries2_decoder.cpp b/nebula_decoders/src/nebula_decoders_aeva/aeva_aries2_decoder.cpp index 8e98edf58..d4a288eca 100644 --- a/nebula_decoders/src/nebula_decoders_aeva/aeva_aries2_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_aeva/aeva_aries2_decoder.cpp @@ -2,6 +2,8 @@ #include "nebula_decoders/nebula_decoders_aeva/aeva_aries2_decoder.hpp" +#include + namespace nebula::drivers { @@ -35,7 +37,7 @@ void AevaAries2Decoder::processPointcloudMessage(const aeva::PointCloudMessage & state.line_index++; } - if (!raw_point.valid || raw_point.signal_quality < 60) { + if (!raw_point.valid) { continue; } @@ -49,6 +51,22 @@ void AevaAries2Decoder::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{ReturnType::UNKNOWN}; + // TODO(mojomex): Currently, there is no info published by the sensor on which return mode is + // active. Here, the default one is hardcoded for now. + switch (raw_point.peak_id) { + case 0: + return_type = ReturnType::STRONGEST; + break; + case 1: + return_type = ReturnType::SECONDSTRONGEST; + break; + default: + return_type = ReturnType::UNKNOWN; + } + + point.return_type = static_cast(return_type); + float xy_distance = point.distance * std::cos(point.elevation); point.x = xy_distance * std::sin(point.azimuth); point.y = xy_distance * std::cos(point.azimuth); From 7011769a056f067b9073ca562eb1ccac74fcd67d Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Thu, 4 Jul 2024 17:08:25 +0900 Subject: [PATCH 25/57] fix(aeva): display error codes correctly in monitor --- .../nebula_common/aeva/packet_types.hpp | 13 ++++++++++--- .../connections/health.hpp | 7 ++++--- .../nebula_ros/aeva/hw_monitor_wrapper.hpp | 6 ++++-- nebula_ros/src/aeva/hw_monitor_wrapper.cpp | 18 +++++++++--------- 4 files changed, 27 insertions(+), 17 deletions(-) diff --git a/nebula_common/include/nebula_common/aeva/packet_types.hpp b/nebula_common/include/nebula_common/aeva/packet_types.hpp index c050cb3a9..0ae4e63c0 100644 --- a/nebula_common/include/nebula_common/aeva/packet_types.hpp +++ b/nebula_common/include/nebula_common/aeva/packet_types.hpp @@ -153,10 +153,17 @@ enum class ReconfigRequestType : uint8_t { kInvalid = 5 }; -inline bool is_error_code(uint32_t health_code) +struct HealthCode { - return (health_code & (1u << 31u)) != 0; -} + 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; } + +private: + uint32_t value_; + static const uint32_t ERROR_MASK = 1u << 31u; +}; struct ReconfigMessage { 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 694dc074f..5088dd479 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 @@ -31,11 +31,12 @@ namespace nebula::drivers::connections { using namespace boost::endian; // NOLINT +using aeva::HealthCode; class HealthParser : public AevaParser { public: - using callback_t = std::function)>; + using callback_t = std::function)>; explicit HealthParser(std::shared_ptr incoming_byte_stream) : AevaParser(std::move(incoming_byte_stream)) @@ -54,13 +55,13 @@ class HealthParser : public AevaParser message_header.message_length - sizeof(MessageHeader) - sizeof(n_entries), "Unexpected size of health code list"); - std::vector entries; + std::vector entries; 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); - entries.push_back(entry); + entries.emplace_back(entry); } if (callback_) { diff --git a/nebula_ros/include/nebula_ros/aeva/hw_monitor_wrapper.hpp b/nebula_ros/include/nebula_ros/aeva/hw_monitor_wrapper.hpp index 1cd9bf9d5..8b1489820 100644 --- a/nebula_ros/include/nebula_ros/aeva/hw_monitor_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/aeva/hw_monitor_wrapper.hpp @@ -16,6 +16,7 @@ #include #include +#include #include #include #include @@ -38,6 +39,7 @@ namespace nebula::ros { +using drivers::aeva::HealthCode; using nlohmann::json; class AevaHwMonitorWrapper @@ -48,7 +50,7 @@ class AevaHwMonitorWrapper void onTelemetryFragment(const json & diff); - void onHealthCodes(std::vector health_codes); + void onHealthCodes(std::vector health_codes); private: struct NodeTelemetry @@ -64,7 +66,7 @@ class AevaHwMonitorWrapper struct HealthState { - std::vector codes; + std::vector codes; rclcpp::Time last_update; }; diff --git a/nebula_ros/src/aeva/hw_monitor_wrapper.cpp b/nebula_ros/src/aeva/hw_monitor_wrapper.cpp index 90682ffe3..41a606a81 100644 --- a/nebula_ros/src/aeva/hw_monitor_wrapper.cpp +++ b/nebula_ros/src/aeva/hw_monitor_wrapper.cpp @@ -25,6 +25,7 @@ namespace nebula::ros { using diagnostic_msgs::msg::DiagnosticStatus; +using drivers::aeva::HealthCode; using nebula::util::get_if_exists; AevaHwMonitorWrapper::AevaHwMonitorWrapper( @@ -86,7 +87,7 @@ void AevaHwMonitorWrapper::onTelemetryFragment(const json & diff) } } -void AevaHwMonitorWrapper::onHealthCodes(std::vector health_codes) +void AevaHwMonitorWrapper::onHealthCodes(std::vector health_codes) { std::scoped_lock lock(mtx_health_, mtx_hardware_id_); @@ -148,17 +149,16 @@ void AevaHwMonitorWrapper::publishDiagnostics() status.name = "aeva.health"; if (health_) { - auto & codes = health_->codes; - json warning_codes = json::array(); json error_codes = json::array(); - std::copy_if(codes.cbegin(), codes.cend(), std::back_inserter(warning_codes), [](auto code) { - return !drivers::aeva::is_error_code(code); - }); - - std::copy_if( - codes.cbegin(), codes.cend(), std::back_inserter(error_codes), drivers::aeva::is_error_code); + for (const auto & code : health_->codes) { + if (code.is_error()) { + error_codes.push_back(code.get()); + } else { + warning_codes.push_back(code.get()); + } + } if (!error_codes.empty()) { status.level = DiagnosticStatus::ERROR; From e309ad54c2b976debc2a9838db4873075916c701 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Fri, 5 Jul 2024 16:03:14 +0900 Subject: [PATCH 26/57] chore(aeva): align format with pretty-printing PR --- .../nebula_common/aeva/config_types.hpp | 41 +++++++++---------- 1 file changed, 20 insertions(+), 21 deletions(-) diff --git a/nebula_common/include/nebula_common/aeva/config_types.hpp b/nebula_common/include/nebula_common/aeva/config_types.hpp index 4d4fdf6ba..9ba412b1b 100644 --- a/nebula_common/include/nebula_common/aeva/config_types.hpp +++ b/nebula_common/include/nebula_common/aeva/config_types.hpp @@ -46,27 +46,26 @@ struct Aeries2Config : public SensorConfigurationBase inline std::ostream & operator<<(std::ostream & os, const Aeries2Config & arg) { - os << "Aeries2Config:\n"; - os << " sensor_model = " << arg.sensor_model << '\n'; - os << " frame_id = " << arg.frame_id << '\n'; - os << " sensor_ip = " << arg.sensor_ip << '\n'; - os << " dithering_enable_ego_speed = " << arg.dithering_enable_ego_speed << '\n'; - os << " dithering_pattern_option = " << arg.dithering_pattern_option << '\n'; - os << " ele_offset_rad = " << arg.ele_offset_rad << '\n'; - os << " elevation_auto_adjustment = " << arg.elevation_auto_adjustment << '\n'; - os << " enable_frame_dithering = " << arg.enable_frame_dithering << '\n'; - os << " enable_frame_sync = " << arg.enable_frame_sync << '\n'; - os << " flip_pattern_vertically = " << arg.flip_pattern_vertically << '\n'; - os << " frame_sync_offset_in_ms = " << static_cast(arg.frame_sync_offset_in_ms) << '\n'; - os << " frame_sync_type = " << arg.frame_sync_type << '\n'; - os << " frame_synchronization_on_rising_edge = " << arg.frame_synchronization_on_rising_edge - << '\n'; - os << " hfov_adjustment_deg = " << arg.hfov_adjustment_deg << '\n'; - os << " hfov_rotation_deg = " << arg.hfov_rotation_deg << '\n'; - os << " highlight_ROI = " << arg.highlight_ROI << '\n'; - os << " horizontal_fov_degrees = " << arg.horizontal_fov_degrees << '\n'; - os << " roi_az_offset_rad = " << arg.roi_az_offset_rad << '\n'; - os << " vertical_pattern = " << arg.vertical_pattern; + 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 << '\n'; + os << "Dithering Enable Ego Speed: " << arg.dithering_enable_ego_speed << '\n'; + os << "Dithering Pattern Option: " << arg.dithering_pattern_option << '\n'; + os << "Elevation Offset (rad): " << arg.ele_offset_rad << '\n'; + os << "Elevation Auto Adjustment: " << arg.elevation_auto_adjustment << '\n'; + os << "Enable Frame Dithering: " << arg.enable_frame_dithering << '\n'; + os << "Enable Frame Sync: " << arg.enable_frame_sync << '\n'; + os << "Flip Pattern Vertically: " << arg.flip_pattern_vertically << '\n'; + os << "Frame Sync Offset (ms): " << static_cast(arg.frame_sync_offset_in_ms) << '\n'; + os << "Frame Sync Type: " << arg.frame_sync_type << '\n'; + os << "Frame Sync on Rising Edge: " << arg.frame_synchronization_on_rising_edge << '\n'; + os << "hFoV Adjustment (deg): " << arg.hfov_adjustment_deg << '\n'; + os << "hFoV Rotation (deg): " << arg.hfov_rotation_deg << '\n'; + os << "Highlight ROI: " << arg.highlight_ROI << '\n'; + os << "Horizontal FoV (deg): " << arg.horizontal_fov_degrees << '\n'; + os << "ROI Azimuth Offset (rad): " << arg.roi_az_offset_rad << '\n'; + os << "Vertical Pattern: " << arg.vertical_pattern; return os; } From 32d80f36dd5ecec84d6ea48bbbc1c4436f4d3ec6 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Fri, 5 Jul 2024 16:18:24 +0900 Subject: [PATCH 27/57] feat(aeva): support runtime parameter changes --- .../aeva_hw_interface.hpp | 19 ++- .../connections/aeva_api.hpp | 3 - .../connections/reconfig.hpp | 1 - .../nebula_ros/aeva/aeva_ros_wrapper.hpp | 15 +- nebula_ros/src/aeva/aeva_ros_wrapper.cpp | 134 ++++++++++++------ 5 files changed, 119 insertions(+), 53 deletions(-) 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 24bc21550..da4e6ea1f 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 @@ -119,12 +119,27 @@ class AevaHwInterface throw std::runtime_error("Reached maximum retries while trying to fetch manifest"); } - tryReconfig(manifest, "scanner", "frame_sync_type", config->frame_sync_type); + tryReconfig(manifest, "scanner", "sensor_ip", config->sensor_ip); + tryReconfig( + manifest, "scanner", "dithering_enable_ego_speed", config->dithering_enable_ego_speed); + tryReconfig(manifest, "scanner", "dithering_pattern_option", config->dithering_pattern_option); + tryReconfig(manifest, "scanner", "ele_offset_rad", config->ele_offset_rad); + tryReconfig( + manifest, "scanner", "elevation_auto_adjustment", config->elevation_auto_adjustment); + tryReconfig(manifest, "scanner", "enable_frame_dithering", config->enable_frame_dithering); + tryReconfig(manifest, "scanner", "enable_frame_sync", config->enable_frame_sync); + tryReconfig(manifest, "scanner", "flip_pattern_vertically", config->flip_pattern_vertically); tryReconfig(manifest, "scanner", "frame_sync_offset_in_ms", config->frame_sync_offset_in_ms); + tryReconfig(manifest, "scanner", "frame_sync_type", config->frame_sync_type); tryReconfig( manifest, "scanner", "frame_synchronization_on_rising_edge", config->frame_synchronization_on_rising_edge); - tryReconfig(manifest, "scanner", "enable_frame_sync", config->enable_frame_sync); + tryReconfig(manifest, "scanner", "hfov_adjustment_deg", config->hfov_adjustment_deg); + tryReconfig(manifest, "scanner", "hfov_rotation_deg", config->hfov_rotation_deg); + tryReconfig(manifest, "scanner", "highlight_ROI", config->highlight_ROI); + tryReconfig(manifest, "scanner", "horizontal_fov_degrees", config->horizontal_fov_degrees); + tryReconfig(manifest, "scanner", "roi_az_offset_rad", config->roi_az_offset_rad); + tryReconfig(manifest, "scanner", "vertical_pattern", config->vertical_pattern); } void registerCloudPacketCallback(PointcloudParser::callback_t callback) 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 4390a3beb..f99d60e74 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 @@ -25,7 +25,6 @@ #include #include #include -#include #include #include #include @@ -204,9 +203,7 @@ class AevaParser : public ObservableByteStream private: std::thread thread_; - std::shared_ptr incoming_; - callback_t bytes_callback_; }; 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 960f83eb9..7702a74e4 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 @@ -69,7 +69,6 @@ class ReconfigParser : public AevaParser, json setParameter(std::string node_name, std::string key, json value) { json request_body = {{node_name, {{key, {{"value", value}}}}}}; - std::cerr << "Sending request with body: \n" << request_body.dump(2) << std::endl; ReconfigMessage request = {ReconfigRequestType::kChangeRequest, request_body}; ReconfigMessage response = doRequest(request); 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 e8932970f..717efdbf5 100644 --- a/nebula_ros/include/nebula_ros/aeva/aeva_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/aeva/aeva_ros_wrapper.hpp @@ -55,22 +55,27 @@ class AevaRosWrapper final : public rclcpp::Node Status declareAndGetSensorConfigParams(); Status validateAndSetConfig(std::shared_ptr & new_config); + rcl_interfaces::msg::SetParametersResult onParameterChange( + const std::vector & p); + void recordRawPacket(const std::vector & bytes); - rclcpp::Publisher::SharedPtr packets_pub_{}; + rclcpp::Publisher::SharedPtr packets_pub_; std::mutex mtx_current_scan_msg_; - nebula_msgs::msg::NebulaPackets::UniquePtr current_scan_msg_{}; + nebula_msgs::msg::NebulaPackets::UniquePtr current_scan_msg_; - rclcpp::Publisher::SharedPtr cloud_pub_{}; + rclcpp::Publisher::SharedPtr cloud_pub_; std::shared_ptr cloud_watchdog_; - rclcpp::Subscription::SharedPtr packets_sub_{}; + rclcpp::Subscription::SharedPtr packets_sub_; std::shared_ptr sensor_cfg_ptr_; std::optional hw_interface_; std::optional hw_monitor_; - drivers::AevaAries2Decoder decoder_{}; + drivers::AevaAries2Decoder decoder_; + + OnSetParametersCallbackHandle::SharedPtr parameter_event_cb_; }; } // namespace nebula::ros diff --git a/nebula_ros/src/aeva/aeva_ros_wrapper.cpp b/nebula_ros/src/aeva/aeva_ros_wrapper.cpp index 46b9b9a63..4509ef060 100644 --- a/nebula_ros/src/aeva/aeva_ros_wrapper.cpp +++ b/nebula_ros/src/aeva/aeva_ros_wrapper.cpp @@ -28,6 +28,7 @@ #include #include +#include #include #include #include @@ -131,31 +132,32 @@ AevaRosWrapper::AevaRosWrapper(const rclcpp::NodeOptions & options) RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "Missed pointcloud output deadline"); }); - { - AevaAries2Decoder::callback_t pointcloud_cb = - [&](AevaPointCloudUniquePtr cloud_ptr, auto timestamp) { - auto now = this->now(); - cloud_watchdog_->update(); - - if ( - cloud_pub_->get_subscription_count() > 0 || - cloud_pub_->get_intra_process_subscription_count() > 0) { - auto ros_pc_msg_ptr = std::make_unique(); - pcl::toROSMsg(*cloud_ptr, *ros_pc_msg_ptr); - ros_pc_msg_ptr->header.frame_id = sensor_cfg_ptr_->frame_id; - ros_pc_msg_ptr->header.stamp = rclcpp::Time(static_cast(timestamp)); - cloud_pub_->publish(std::move(ros_pc_msg_ptr)); - } - - std::lock_guard lock(mtx_current_scan_msg_); - if (current_scan_msg_ && packets_pub_) { - packets_pub_->publish(std::move(current_scan_msg_)); - current_scan_msg_ = {}; - } - }; - - decoder_.registerPointCloudCallback(std::move(pointcloud_cb)); - } + AevaAries2Decoder::callback_t pointcloud_cb = + [&](AevaPointCloudUniquePtr cloud_ptr, auto timestamp) { + auto now = this->now(); + cloud_watchdog_->update(); + + if ( + cloud_pub_->get_subscription_count() > 0 || + cloud_pub_->get_intra_process_subscription_count() > 0) { + auto ros_pc_msg_ptr = std::make_unique(); + pcl::toROSMsg(*cloud_ptr, *ros_pc_msg_ptr); + ros_pc_msg_ptr->header.frame_id = sensor_cfg_ptr_->frame_id; + ros_pc_msg_ptr->header.stamp = rclcpp::Time(static_cast(timestamp)); + cloud_pub_->publish(std::move(ros_pc_msg_ptr)); + } + + std::lock_guard lock(mtx_current_scan_msg_); + if (current_scan_msg_ && packets_pub_) { + packets_pub_->publish(std::move(current_scan_msg_)); + current_scan_msg_ = {}; + } + }; + + decoder_.registerPointCloudCallback(std::move(pointcloud_cb)); + + parameter_event_cb_ = + add_on_set_parameters_callback([this](const auto & p) { return onParameterChange(p); }); } Status AevaRosWrapper::declareAndGetSensorConfigParams() @@ -167,29 +169,29 @@ Status AevaRosWrapper::declareAndGetSensorConfigParams() config.sensor_ip = declare_parameter("sensor_ip", param_read_only()); config.frame_id = declare_parameter("frame_id", param_read_only()); config.dithering_enable_ego_speed = - declare_parameter("dithering_enable_ego_speed", param_read_only()); + declare_parameter("dithering_enable_ego_speed", param_read_write()); config.dithering_pattern_option = - declare_parameter("dithering_pattern_option", param_read_only()); - config.ele_offset_rad = declare_parameter("ele_offset_rad", param_read_only()); + declare_parameter("dithering_pattern_option", param_read_write()); + config.ele_offset_rad = declare_parameter("ele_offset_rad", param_read_write()); config.elevation_auto_adjustment = - declare_parameter("elevation_auto_adjustment", param_read_only()); + declare_parameter("elevation_auto_adjustment", param_read_write()); config.enable_frame_dithering = - declare_parameter("enable_frame_dithering", param_read_only()); - config.enable_frame_sync = declare_parameter("enable_frame_sync", param_read_only()); + declare_parameter("enable_frame_dithering", param_read_write()); + config.enable_frame_sync = declare_parameter("enable_frame_sync", param_read_write()); config.flip_pattern_vertically = - declare_parameter("flip_pattern_vertically", param_read_only()); + declare_parameter("flip_pattern_vertically", param_read_write()); config.frame_sync_offset_in_ms = - declare_parameter("frame_sync_offset_in_ms", param_read_only()); - config.frame_sync_type = declare_parameter("frame_sync_type", param_read_only()); + declare_parameter("frame_sync_offset_in_ms", param_read_write()); + config.frame_sync_type = declare_parameter("frame_sync_type", param_read_write()); config.frame_synchronization_on_rising_edge = - declare_parameter("frame_synchronization_on_rising_edge", param_read_only()); - config.hfov_adjustment_deg = declare_parameter("hfov_adjustment_deg", param_read_only()); - config.hfov_rotation_deg = declare_parameter("hfov_rotation_deg", param_read_only()); - config.highlight_ROI = declare_parameter("highlight_ROI", param_read_only()); + declare_parameter("frame_synchronization_on_rising_edge", param_read_write()); + config.hfov_adjustment_deg = declare_parameter("hfov_adjustment_deg", param_read_write()); + config.hfov_rotation_deg = declare_parameter("hfov_rotation_deg", param_read_write()); + config.highlight_ROI = declare_parameter("highlight_ROI", param_read_write()); config.horizontal_fov_degrees = - declare_parameter("horizontal_fov_degrees", param_read_only()); - config.roi_az_offset_rad = declare_parameter("roi_az_offset_rad", param_read_only()); - config.vertical_pattern = declare_parameter("vertical_pattern", param_read_only()); + declare_parameter("horizontal_fov_degrees", param_read_write()); + config.roi_az_offset_rad = declare_parameter("roi_az_offset_rad", param_read_write()); + config.vertical_pattern = declare_parameter("vertical_pattern", param_read_write()); auto new_cfg_ptr = std::make_shared(config); return validateAndSetConfig(new_cfg_ptr); @@ -210,13 +212,61 @@ Status AevaRosWrapper::validateAndSetConfig(std::shared_ptr } if (hw_interface_) { - hw_interface_->onConfigChange(new_config); + try { + hw_interface_->onConfigChange(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; + } } sensor_cfg_ptr_ = new_config; return Status::OK; } +rcl_interfaces::msg::SetParametersResult AevaRosWrapper::onParameterChange( + const std::vector & p) +{ + using rcl_interfaces::msg::SetParametersResult; + Aeries2Config config; + + bool got_any = + get_param(p, "dithering_enable_ego_speed", config.dithering_enable_ego_speed) | + get_param(p, "dithering_pattern_option", config.dithering_pattern_option) | + get_param(p, "ele_offset_rad", config.ele_offset_rad) | + get_param(p, "elevation_auto_adjustment", config.elevation_auto_adjustment) | + get_param(p, "enable_frame_dithering", config.enable_frame_dithering) | + get_param(p, "enable_frame_sync", config.enable_frame_sync) | + get_param(p, "flip_pattern_vertically", config.flip_pattern_vertically) | + get_param(p, "frame_sync_offset_in_ms", config.frame_sync_offset_in_ms) | + get_param(p, "frame_sync_type", config.frame_sync_type) | + get_param( + p, "frame_synchronization_on_rising_edge", config.frame_synchronization_on_rising_edge) | + get_param(p, "hfov_adjustment_deg", config.hfov_adjustment_deg) | + get_param(p, "hfov_rotation_deg", config.hfov_rotation_deg) | + get_param(p, "highlight_ROI", config.highlight_ROI) | + get_param(p, "horizontal_fov_degrees", config.horizontal_fov_degrees) | + get_param(p, "roi_az_offset_rad", config.roi_az_offset_rad) | + get_param(p, "vertical_pattern", config.vertical_pattern); + + if (!got_any) { + return rcl_interfaces::build().successful(true).reason(""); + } + + auto new_cfg_ptr = std::make_shared(config); + auto status = validateAndSetConfig(new_cfg_ptr); + + if (status != Status::OK) { + RCLCPP_WARN_STREAM(get_logger(), "OnParameterChange aborted: " << status); + auto result = SetParametersResult(); + result.successful = false; + result.reason = (std::stringstream() << "Invalid configuration: " << status).str(); + return result; + } + + return rcl_interfaces::build().successful(true).reason(""); +} + void AevaRosWrapper::recordRawPacket(const std::vector & vector) { std::lock_guard lock(mtx_current_scan_msg_); From 2fb591b2add84eb696283353aaaa70858c15172b Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Mon, 8 Jul 2024 13:54:48 +0900 Subject: [PATCH 28/57] chore(aeva): rename occurrences of `aries` to `aeries` --- nebula_decoders/CMakeLists.txt | 2 +- .../{aeva_aries2_decoder.hpp => aeva_aeries2_decoder.hpp} | 4 ++-- .../{aeva_aries2_decoder.cpp => aeva_aeries2_decoder.cpp} | 6 +++--- nebula_ros/include/nebula_ros/aeva/aeva_ros_wrapper.hpp | 4 ++-- nebula_ros/src/aeva/aeva_ros_wrapper.cpp | 8 ++++---- 5 files changed, 12 insertions(+), 12 deletions(-) rename nebula_decoders/include/nebula_decoders/nebula_decoders_aeva/{aeva_aries2_decoder.hpp => aeva_aeries2_decoder.hpp} (94%) rename nebula_decoders/src/nebula_decoders_aeva/{aeva_aries2_decoder.cpp => aeva_aeries2_decoder.cpp} (92%) diff --git a/nebula_decoders/CMakeLists.txt b/nebula_decoders/CMakeLists.txt index 1043c706e..89548f164 100644 --- a/nebula_decoders/CMakeLists.txt +++ b/nebula_decoders/CMakeLists.txt @@ -117,7 +117,7 @@ target_include_directories(nebula_decoders_continental PUBLIC # Aeva add_library(nebula_decoders_aeva SHARED -src/nebula_decoders_aeva/aeva_aries2_decoder.cpp +src/nebula_decoders_aeva/aeva_aeries2_decoder.cpp ) target_link_libraries(nebula_decoders_aeva PUBLIC 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_aeries2_decoder.hpp similarity index 94% rename from nebula_decoders/include/nebula_decoders/nebula_decoders_aeva/aeva_aries2_decoder.hpp rename to nebula_decoders/include/nebula_decoders/nebula_decoders_aeva/aeva_aeries2_decoder.hpp index 5c17e991d..cdd90cde8 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_aeva/aeva_aries2_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_aeva/aeva_aeries2_decoder.hpp @@ -31,7 +31,7 @@ namespace nebula::drivers { -class AevaAries2Decoder +class AevaAeries2Decoder { public: using AevaPoint = aeva::PointXYZVIRCAEDT; @@ -40,7 +40,7 @@ class AevaAries2Decoder using callback_t = std::function; - AevaAries2Decoder() : cloud_state_({std::make_unique(), 0}) {} + AevaAeries2Decoder() : cloud_state_({std::make_unique(), 0}) {} void processPointcloudMessage(const aeva::PointCloudMessage & message); diff --git a/nebula_decoders/src/nebula_decoders_aeva/aeva_aries2_decoder.cpp b/nebula_decoders/src/nebula_decoders_aeva/aeva_aeries2_decoder.cpp similarity index 92% rename from nebula_decoders/src/nebula_decoders_aeva/aeva_aries2_decoder.cpp rename to nebula_decoders/src/nebula_decoders_aeva/aeva_aeries2_decoder.cpp index d4a288eca..e324886c1 100644 --- a/nebula_decoders/src/nebula_decoders_aeva/aeva_aries2_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_aeva/aeva_aeries2_decoder.cpp @@ -1,13 +1,13 @@ // Copyright 2024 TIER IV, Inc. -#include "nebula_decoders/nebula_decoders_aeva/aeva_aries2_decoder.hpp" +#include "nebula_decoders/nebula_decoders_aeva/aeva_aeries2_decoder.hpp" #include namespace nebula::drivers { -void AevaAries2Decoder::processPointcloudMessage(const aeva::PointCloudMessage & message) +void AevaAeries2Decoder::processPointcloudMessage(const aeva::PointCloudMessage & message) { DecoderState state{ message.header.frame_sync_index, message.header.ns_per_index, message.header.line_index, 0, @@ -82,7 +82,7 @@ void AevaAries2Decoder::processPointcloudMessage(const aeva::PointCloudMessage & } } -void AevaAries2Decoder::registerPointCloudCallback( +void AevaAeries2Decoder::registerPointCloudCallback( std::function, uint64_t)> callback) { std::lock_guard lock(mtx_callback_); 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 717efdbf5..512e41d03 100644 --- a/nebula_ros/include/nebula_ros/aeva/aeva_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/aeva/aeva_ros_wrapper.hpp @@ -22,7 +22,7 @@ #include #include #include -#include +#include #include #include #include @@ -73,7 +73,7 @@ class AevaRosWrapper final : public rclcpp::Node std::optional hw_interface_; std::optional hw_monitor_; - drivers::AevaAries2Decoder decoder_; + drivers::AevaAeries2Decoder decoder_; OnSetParametersCallbackHandle::SharedPtr parameter_event_cb_; }; diff --git a/nebula_ros/src/aeva/aeva_ros_wrapper.cpp b/nebula_ros/src/aeva/aeva_ros_wrapper.cpp index 4509ef060..a147172c1 100644 --- a/nebula_ros/src/aeva/aeva_ros_wrapper.cpp +++ b/nebula_ros/src/aeva/aeva_ros_wrapper.cpp @@ -13,7 +13,7 @@ #include #include #include -#include +#include #include #include #include @@ -37,12 +37,12 @@ namespace nebula::ros { -using drivers::AevaAries2Decoder; +using drivers::AevaAeries2Decoder; using drivers::aeva::Aeries2Config; using drivers::connections::PointcloudParser; using nlohmann::json; using namespace std::chrono_literals; // NOLINT -using AevaPointCloudUniquePtr = AevaAries2Decoder::AevaPointCloudUniquePtr; +using AevaPointCloudUniquePtr = AevaAeries2Decoder::AevaPointCloudUniquePtr; AevaRosWrapper::AevaRosWrapper(const rclcpp::NodeOptions & options) : rclcpp::Node("aeva_ros_wrapper", rclcpp::NodeOptions(options).use_intra_process_comms(true)) @@ -132,7 +132,7 @@ AevaRosWrapper::AevaRosWrapper(const rclcpp::NodeOptions & options) RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "Missed pointcloud output deadline"); }); - AevaAries2Decoder::callback_t pointcloud_cb = + AevaAeries2Decoder::callback_t pointcloud_cb = [&](AevaPointCloudUniquePtr cloud_ptr, auto timestamp) { auto now = this->now(); cloud_watchdog_->update(); From 7896bd1dbc3bdf42c765c8739b3068e43aa92405 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Mon, 8 Jul 2024 15:27:17 +0900 Subject: [PATCH 29/57] fix(aeva_api): stop thread gracefully --- .../connections/aeva_api.hpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) 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 f99d60e74..5380d7737 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 @@ -19,6 +19,7 @@ #include +#include #include #include #include @@ -150,14 +151,14 @@ class AevaParser : public ObservableByteStream { public: explicit AevaParser(std::shared_ptr incoming_byte_stream) - : incoming_(std::move(incoming_byte_stream)) + : incoming_(std::move(incoming_byte_stream)), running_(true) { if (!incoming_) { throw std::runtime_error("Incoming byte stream cannot be null"); } thread_ = std::thread([&]() { - while (true) onLowLevelMessage(); + while (running_) onLowLevelMessage(); }); } @@ -166,6 +167,12 @@ class AevaParser : public ObservableByteStream bytes_callback_ = std::move(callback); } + ~AevaParser() override + { + running_ = false; + thread_.join(); + } + protected: /** * @brief Attempts to read one message from the stream, blocking. Parses the SomeIP and @@ -203,6 +210,8 @@ class AevaParser : public ObservableByteStream private: std::thread thread_; + std::atomic_bool running_; + std::shared_ptr incoming_; callback_t bytes_callback_; }; From 65258c3c47e650d04da2da2d567ff4e5774af14f Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Mon, 8 Jul 2024 16:02:29 +0900 Subject: [PATCH 30/57] test(aeva): fix tests --- nebula_tests/aeva/aeva_hw_interface_test.cpp | 23 ++- nebula_tests/common/mock_byte_stream.hpp | 27 ++- nebula_tests/data/aeva/tcp_stream.hpp | 200 ++----------------- 3 files changed, 55 insertions(+), 195 deletions(-) diff --git a/nebula_tests/aeva/aeva_hw_interface_test.cpp b/nebula_tests/aeva/aeva_hw_interface_test.cpp index bfe1f4b41..3c0219d10 100644 --- a/nebula_tests/aeva/aeva_hw_interface_test.cpp +++ b/nebula_tests/aeva/aeva_hw_interface_test.cpp @@ -11,28 +11,35 @@ #include +#include +#include + using nebula::drivers::aeva::PointCloudMessage; using nebula::drivers::connections::PointcloudParser; TEST(TestParsing, Pointcloud) // NOLINT { - size_t n_callback_invocations = 0; + using std::chrono_literals::operator""ms; + + auto mock_byte_stream = std::make_shared(STREAM); + PointcloudParser parser(mock_byte_stream); + std::atomic_bool done = false; PointcloudParser::callback_t callback = [&](const PointCloudMessage & arg) { - EXPECT_EQ(n_callback_invocations, 0); + if (done) return; EXPECT_EQ(arg.header.aeva_marker, 0xAE5Au); EXPECT_EQ(arg.header.platform, 2); EXPECT_GT(arg.points.size(), 0); - - n_callback_invocations++; + EXPECT_TRUE(mock_byte_stream->done()); + EXPECT_EQ(mock_byte_stream->getReadCount(), 2); + done = true; }; - auto mock_byte_stream = std::make_shared(STREAM); - - PointcloudParser parser(mock_byte_stream); parser.registerCallback(std::move(callback)); - EXPECT_EQ(n_callback_invocations, 1); + mock_byte_stream->run(); + while (!done) { + } } diff --git a/nebula_tests/common/mock_byte_stream.hpp b/nebula_tests/common/mock_byte_stream.hpp index 97e8845ab..f18cb82e8 100644 --- a/nebula_tests/common/mock_byte_stream.hpp +++ b/nebula_tests/common/mock_byte_stream.hpp @@ -14,6 +14,9 @@ #include +#include + +#include #include #include @@ -25,16 +28,34 @@ class MockByteStream final : public drivers::connections::PullableByteStream public: explicit MockByteStream(const std::vector> & stream) : stream_(stream) {} - void read(std::vector & into, size_t /* n_bytes */) override + void read(std::vector & into, size_t n_bytes) override { - auto from = stream_[index_++]; + while (!running_) { + } + read_count_++; + const auto & from = stream_[index_++]; + ASSERT_EQ(from.size(), n_bytes); into.clear(); into.insert(into.end(), from.cbegin(), from.cend()); + + if (index_ == stream_.size()) { + done_ = true; + index_ = 0; + } } + void run() { running_ = true; } + + bool done() { return done_; } + + size_t getReadCount() { return read_count_; } + private: const std::vector> & stream_; - size_t index_{}; + std::atomic_size_t index_{0}; + std::atomic_size_t read_count_{0}; + std::atomic_bool running_{false}; + std::atomic_bool done_{false}; }; } // namespace nebula::test diff --git a/nebula_tests/data/aeva/tcp_stream.hpp b/nebula_tests/data/aeva/tcp_stream.hpp index 50cdb1532..c330416eb 100644 --- a/nebula_tests/data/aeva/tcp_stream.hpp +++ b/nebula_tests/data/aeva/tcp_stream.hpp @@ -18,187 +18,19 @@ #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}}; + /* SomeIP Header. Payload size: bytes 4..8 (little endian, size of payload array + 12 bytes) */ + {0xfa, 0xae, 0x00, 0x00, 0x92, 0x00, 0x00, 0x00, 0xa2, 0xff, + 0x79, 0x12, 0x02, 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00}, + /* Payload. Message size: bytes 4..8 (little endian, size of payload array). + * Bytes 46..50: n_points (little endian) + */ + {0x03, 0x00, 0x78, 0x12, 0x86, 0x00, 0x00, 0x00, 0xe7, 0x35, 0x41, 0x12, 0x00, 0x84, 0xc5, + 0x15, 0x64, 0x35, 0x5b, 0x14, 0x00, 0x84, 0xc5, 0x15, 0x5a, 0xae, 0x02, 0x00, 0x00, 0x08, + 0xe7, 0x35, 0x41, 0x12, 0x00, 0x84, 0xc5, 0x15, 0xff, 0xff, 0xff, 0xff, 0x00, 0xf8, 0x7f, + 0x00, 0x04, 0x00, 0x00, 0x00, 0xf0, 0xff, 0x00, 0x00, 0x64, 0x0a, 0x01, 0x09, 0x50, 0x40, + 0x00, 0x00, 0xff, 0xd7, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0xba, 0x1f, 0xa4, 0xec, 0x3e, 0x01, 0xe9, 0xff, 0x5b, 0x45, 0x40, 0x06, + 0x21, 0x00, 0xba, 0x1f, 0xa4, 0xec, 0x91, 0x00, 0x18, 0xfd, 0x5b, 0x45, 0x88, 0x36, 0x23, + 0x00, 0x0e, 0x1e, 0x4c, 0xf3, 0x27, 0x30, 0x9e, 0x2b, 0x14, 0x16, 0xc1, 0x03, 0x01, 0x00, + 0x5e, 0x1c, 0x58, 0xe9, 0xc1, 0x3b, 0xe9, 0x08, 0x13, 0x17, 0x82, 0x32, 0x01, 0x00}, +}; From 185b0d8a9c238eb231ff921c521d34ff0e562238 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Mon, 8 Jul 2024 21:16:40 +0900 Subject: [PATCH 31/57] fix(aeva): allow multiple responses in reconfig stream, thread safety, higher timeouts --- .../nebula_common/aeva/packet_types.hpp | 20 +++ .../aeva_hw_interface.hpp | 12 +- .../connections/aeva_api.hpp | 6 +- .../connections/reconfig.hpp | 130 +++++++++++++----- nebula_ros/src/aeva/aeva_ros_wrapper.cpp | 3 +- 5 files changed, 125 insertions(+), 46 deletions(-) diff --git a/nebula_common/include/nebula_common/aeva/packet_types.hpp b/nebula_common/include/nebula_common/aeva/packet_types.hpp index 0ae4e63c0..cd2e5bd91 100644 --- a/nebula_common/include/nebula_common/aeva/packet_types.hpp +++ b/nebula_common/include/nebula_common/aeva/packet_types.hpp @@ -19,6 +19,8 @@ #include #include #include +#include +#include #include namespace nebula::drivers::aeva @@ -153,6 +155,24 @@ enum class ReconfigRequestType : uint8_t { kInvalid = 5 }; +inline std::ostream & operator<<(std::ostream & os, const ReconfigRequestType & arg) +{ + switch (arg) { + case ReconfigRequestType::kManifestRequest: + return os << "manifest request"; + case ReconfigRequestType::kManifestResponse: + return os << "manifest response"; + case ReconfigRequestType::kChangeRequest: + return os << "change request"; + case ReconfigRequestType::kChangeApproved: + return os << "change approved"; + case ReconfigRequestType::kChangeIgnored: + return os << "change ignored"; + default: + return os << "invalid: " + std::to_string(static_cast(arg)); + } +} + struct HealthCode { explicit HealthCode(uint32_t value) : value_(value) {} 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 da4e6ea1f..d184a1362 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 @@ -59,8 +59,8 @@ class AevaHwInterface std::shared_ptr logger, bool setup_sensor, const std::shared_ptr & config) : AevaHwInterface( - std::move(logger), setup_sensor, config, makepointcloudApi(*config), - makeTelemetryApi(*config), makeReconfigApi(*config), makeHealthApi(*config)) + logger, setup_sensor, config, makepointcloudApi(*config), makeTelemetryApi(*config), + makeReconfigApi(*config, logger), makeHealthApi(*config)) { } @@ -111,7 +111,7 @@ class AevaHwInterface } } catch (const std::runtime_error & e) { logger_->error(std::string("Could not fetch sensor manifest: ") + e.what()); - reconfig_api_ = makeReconfigApi(*config); + reconfig_api_ = makeReconfigApi(*config, logger_); } } @@ -119,7 +119,6 @@ class AevaHwInterface throw std::runtime_error("Reached maximum retries while trying to fetch manifest"); } - tryReconfig(manifest, "scanner", "sensor_ip", config->sensor_ip); tryReconfig( manifest, "scanner", "dithering_enable_ego_speed", config->dithering_enable_ego_speed); tryReconfig(manifest, "scanner", "dithering_pattern_option", config->dithering_pattern_option); @@ -190,11 +189,12 @@ class AevaHwInterface logger_->info("Set " + node_name + "." + key + " to " + value_str); } - static std::shared_ptr makeReconfigApi(const aeva::Aeries2Config & config) + 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)); + std::make_shared(config.sensor_ip, 21901), logger->child("ReconfigApi")); } static std::shared_ptr makepointcloudApi(const aeva::Aeries2Config & config) 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 5380d7737..7cdced8e7 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 @@ -151,7 +151,7 @@ class AevaParser : public ObservableByteStream { public: explicit AevaParser(std::shared_ptr incoming_byte_stream) - : incoming_(std::move(incoming_byte_stream)), running_(true) + : running_(true), incoming_(std::move(incoming_byte_stream)) { if (!incoming_) { throw std::runtime_error("Incoming byte stream cannot be null"); @@ -206,7 +206,9 @@ class AevaParser : public ObservableByteStream onMessage(message_header, payload_view); } - virtual void onMessage(const MessageHeader & message_header, ByteView & payload_bytes) = 0; + virtual void onMessage(const MessageHeader & /* message_header */, ByteView & /* payload_bytes */) + { + } private: std::thread thread_; 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 7702a74e4..1b2e3f59d 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 @@ -14,6 +14,7 @@ #pragma once +#include "nebula_common/loggers/logger.hpp" #include "nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/aeva_api.hpp" #include "nebula_hw_interfaces/nebula_hw_interfaces_common/connections/byte_stream.hpp" @@ -21,6 +22,10 @@ #include #include +#include + +#include +#include #include #include #include @@ -44,26 +49,34 @@ class ReconfigParser : public AevaParser, public AevaSender { public: - using callback_t = std::function; + using callback_t = std::function; explicit ReconfigParser( std::shared_ptr incoming_byte_stream, - std::shared_ptr outgoing_byte_stream) + std::shared_ptr outgoing_byte_stream, + std::shared_ptr logger) : AevaParser(std::move(incoming_byte_stream)), - AevaSender(std::move(outgoing_byte_stream)) + AevaSender(std::move(outgoing_byte_stream)), + logger_(std::move(logger)) { } json getManifest() { ReconfigMessage request{ReconfigRequestType::kManifestRequest, {}}; - ReconfigMessage response = doRequest(request); + auto responses = doRequest(request, N_MANIFEST_RESPONSES_EXPECTED); - if (!response.body) { - throw std::runtime_error("Expected manifest body but got empty response"); + json result{}; + + for (const auto & response : responses) { + if (!response.body) { + throw std::runtime_error("Expected manifest body but got empty response"); + } + + result.update(*response.body); } - return *response.body; + return result; } json setParameter(std::string node_name, std::string key, json value) @@ -71,7 +84,8 @@ class ReconfigParser : public AevaParser, json request_body = {{node_name, {{key, {{"value", value}}}}}}; ReconfigMessage request = {ReconfigRequestType::kChangeRequest, request_body}; - ReconfigMessage response = doRequest(request); + auto response = doRequest(request); + if (response.type != aeva::ReconfigRequestType::kChangeApproved) { std::stringstream ss{}; ss << "change request failed"; @@ -90,7 +104,14 @@ class ReconfigParser : public AevaParser, } private: - ReconfigMessage doRequest(ReconfigMessage request) + ReconfigMessage doRequest(const ReconfigMessage & request) + { + auto responses = doRequest(request, 1); + return responses.at(0); + } + + std::vector doRequest( + const ReconfigMessage & request, size_t n_responses_expected) { std::lock_guard lock(mtx_inflight_); @@ -99,10 +120,8 @@ class ReconfigParser : public AevaParser, // //////////////////////////////////////// uint64_t request_id = std::rand(); - { - std::lock_guard lock(mtx_inflight_request_id_); - inflight_request_id_.emplace(request_id); - } + + NEBULA_LOG_STREAM(logger_->debug, "Sent " << request.type << ", id=" << request_id); ReconfigRequestType type = request.type; uint8_t encoding_type = 0; @@ -131,30 +150,50 @@ class ReconfigParser : public AevaParser, // Send and wait for response with timeout // //////////////////////////////////////// - std::timed_mutex tm; - tm.lock(); + std::timed_mutex tm_callback_timeout; + tm_callback_timeout.lock(); + + std::mutex mtx_responses; + std::vector responses{}; + responses.reserve(n_responses_expected); + + auto request_valid = std::make_shared(true); - ReconfigMessage response{}; + callback_ = [this, request_valid, n_responses_expected, request_id, &responses, &mtx_responses, + &tm_callback_timeout](uint64_t response_id, const auto & message) { + if (!*request_valid) { + NEBULA_LOG_STREAM( + logger_->error, "Received " << message.type << ", id=" << response_id + << "for no longer valid request id=" << request_id); + return; + } - callback_ = [this, &tm, request_id, &response](const auto & message) { - std::lock_guard lock(mtx_inflight_request_id_); - if (!inflight_request_id_ || inflight_request_id_.value() != request_id) { + if (response_id != request_id) { // Spurious message reached this callback, the references to `response` and `tm` might not // be valid Exit without trying to access possibly invalid references and let request // timeout + NEBULA_LOG_STREAM( + logger_->error, "Spurious " << message.type << " received, id=" << response_id + << ". Expected id=" << request_id); return; } - response = message; - tm.unlock(); + { + std::lock_guard lock(mtx_responses); + responses.push_back(message); + + if (responses.size() == n_responses_expected) { + tm_callback_timeout.unlock(); + } + } }; sendMessage(bytes); - auto request_timed_out = !tm.try_lock_for(3s); + auto request_timed_out = !tm_callback_timeout.try_lock_for(20s); { - std::lock_guard lock(mtx_inflight_request_id_); - inflight_request_id_.reset(); + *request_valid = false; + callback_ = {}; } if (request_timed_out) { @@ -165,17 +204,32 @@ class ReconfigParser : public AevaParser, // Do validation and return response // //////////////////////////////////////// - bool response_type_valid = (request.type == aeva::ReconfigRequestType::kManifestRequest && - response.type == aeva::ReconfigRequestType::kManifestResponse) || - (request.type == aeva::ReconfigRequestType::kChangeRequest && - (response.type == aeva::ReconfigRequestType::kChangeApproved || - response.type == aeva::ReconfigRequestType::kChangeIgnored)); + auto response_type_valid = [&](const ReconfigMessage & response) { + return (request.type == aeva::ReconfigRequestType::kManifestRequest && + response.type == aeva::ReconfigRequestType::kManifestResponse) || + (request.type == aeva::ReconfigRequestType::kChangeRequest && + (response.type == aeva::ReconfigRequestType::kChangeApproved || + response.type == aeva::ReconfigRequestType::kChangeIgnored)); + }; + + std::vector response_types; + response_types.reserve(responses.size()); + for (const auto & response : responses) { + response_types.push_back((std::stringstream{} << response.type).str()); + } + + NEBULA_LOG_STREAM( + logger_->debug, "Got " + boost::join(response_types, ", ") + " for " + << request.type << ", id=" << request_id); - if (!response_type_valid) { - throw std::runtime_error("Invalid response type received"); + if (!std::all_of(responses.cbegin(), responses.cend(), response_type_valid)) { + std::stringstream msg{}; + msg << "Invalid responses for " << request.type << ", id=" << request_id << ": "; + msg << boost::join(response_types, "; "); + throw std::runtime_error(msg.str()); } - return response; + return responses; } protected: @@ -202,15 +256,17 @@ class ReconfigParser : public AevaParser, message.body = nlohmann::json::parse(data_raw.cbegin(), data_raw.cend()); if (callback_) { - callback_(message); + callback_(request_id, message); } } private: - callback_t callback_{}; - std::mutex mtx_inflight_{}; - std::mutex mtx_inflight_request_id_{}; - std::optional inflight_request_id_{}; + std::shared_ptr logger_; + callback_t callback_; + std::mutex mtx_inflight_; + + // scanner, calibration, system_config, spc_converter, dsp_control, self_test, window_measurement + static const size_t N_MANIFEST_RESPONSES_EXPECTED = 7; }; } // namespace nebula::drivers::connections diff --git a/nebula_ros/src/aeva/aeva_ros_wrapper.cpp b/nebula_ros/src/aeva/aeva_ros_wrapper.cpp index a147172c1..b4a8da84b 100644 --- a/nebula_ros/src/aeva/aeva_ros_wrapper.cpp +++ b/nebula_ros/src/aeva/aeva_ros_wrapper.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include #include @@ -228,7 +229,7 @@ rcl_interfaces::msg::SetParametersResult AevaRosWrapper::onParameterChange( const std::vector & p) { using rcl_interfaces::msg::SetParametersResult; - Aeries2Config config; + Aeries2Config config = *sensor_cfg_ptr_; bool got_any = get_param(p, "dithering_enable_ego_speed", config.dithering_enable_ego_speed) | From f18013817e5a87647b16d24505ec48f65b6b5f6e Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Mon, 8 Jul 2024 23:09:30 +0900 Subject: [PATCH 32/57] feat(aeva): switch to json config, add many more config options --- .../nebula_common/aeva/config_types.hpp | 48 ++--- .../include/nebula_common/util/parsing.hpp | 22 +++ .../aeva_hw_interface.hpp | 40 +---- .../config/lidar/aeva/Aeries2.param.yaml | 41 +++-- .../nebula_ros/aeva/aeva_ros_wrapper.hpp | 30 ++++ nebula_ros/schema/Aeries2.schema.json | 169 +++++++++++++----- nebula_ros/src/aeva/aeva_ros_wrapper.cpp | 92 +++++----- 7 files changed, 275 insertions(+), 167 deletions(-) diff --git a/nebula_common/include/nebula_common/aeva/config_types.hpp b/nebula_common/include/nebula_common/aeva/config_types.hpp index 9ba412b1b..8acef5cef 100644 --- a/nebula_common/include/nebula_common/aeva/config_types.hpp +++ b/nebula_common/include/nebula_common/aeva/config_types.hpp @@ -16,6 +16,8 @@ #include "nebula_common/nebula_common.hpp" +#include + #include #include #include @@ -23,25 +25,12 @@ namespace nebula::drivers::aeva { +using nlohmann::json; + struct Aeries2Config : public SensorConfigurationBase { std::string sensor_ip; - float dithering_enable_ego_speed; - std::string dithering_pattern_option; - float ele_offset_rad; - bool elevation_auto_adjustment; - bool enable_frame_dithering; - bool enable_frame_sync; - bool flip_pattern_vertically; - uint16_t frame_sync_offset_in_ms; - std::string frame_sync_type; - bool frame_synchronization_on_rising_edge; - float hfov_adjustment_deg; - float hfov_rotation_deg; - bool highlight_ROI; - std::string horizontal_fov_degrees; - float roi_az_offset_rad; - std::string vertical_pattern; + json tree; }; inline std::ostream & operator<<(std::ostream & os, const Aeries2Config & arg) @@ -49,23 +38,16 @@ 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 << '\n'; - os << "Dithering Enable Ego Speed: " << arg.dithering_enable_ego_speed << '\n'; - os << "Dithering Pattern Option: " << arg.dithering_pattern_option << '\n'; - os << "Elevation Offset (rad): " << arg.ele_offset_rad << '\n'; - os << "Elevation Auto Adjustment: " << arg.elevation_auto_adjustment << '\n'; - os << "Enable Frame Dithering: " << arg.enable_frame_dithering << '\n'; - os << "Enable Frame Sync: " << arg.enable_frame_sync << '\n'; - os << "Flip Pattern Vertically: " << arg.flip_pattern_vertically << '\n'; - os << "Frame Sync Offset (ms): " << static_cast(arg.frame_sync_offset_in_ms) << '\n'; - os << "Frame Sync Type: " << arg.frame_sync_type << '\n'; - os << "Frame Sync on Rising Edge: " << arg.frame_synchronization_on_rising_edge << '\n'; - os << "hFoV Adjustment (deg): " << arg.hfov_adjustment_deg << '\n'; - os << "hFoV Rotation (deg): " << arg.hfov_rotation_deg << '\n'; - os << "Highlight ROI: " << arg.highlight_ROI << '\n'; - os << "Horizontal FoV (deg): " << arg.horizontal_fov_degrees << '\n'; - os << "ROI Azimuth Offset (rad): " << arg.roi_az_offset_rad << '\n'; - os << "Vertical Pattern: " << arg.vertical_pattern; + os << "Sensor IP: " << arg.sensor_ip; + + for (const auto & category : arg.tree.items()) { + os << '\n' << category.key() << ":"; + auto category_settings = category.value(); + for (const auto & setting : category_settings.items()) { + os << "\n " << setting.key() << ": " << setting.value(); + } + } + return os; } diff --git a/nebula_common/include/nebula_common/util/parsing.hpp b/nebula_common/include/nebula_common/util/parsing.hpp index f3458dd12..c81c970cf 100644 --- a/nebula_common/include/nebula_common/util/parsing.hpp +++ b/nebula_common/include/nebula_common/util/parsing.hpp @@ -15,6 +15,10 @@ #pragma once #include +#include + +#include +#include #include #include @@ -25,6 +29,24 @@ namespace nebula::util using nlohmann::json; +inline std::vector to_json_path(const std::string & dot_delimited_path) +{ + std::vector result; + boost::split(result, dot_delimited_path, boost::is_any_of(".")); + return result; +} + +inline json to_json_tree(const json & node, const std::vector & path) +{ + json result = node; + for (auto it = path.crbegin(); it != path.crend(); ++it) { + auto current_key = *it; + result = {{current_key, result}}; + } + + return result; +} + template bool update_if_exists(const json & node, const std::vector & path, T & out_value) { 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 d184a1362..38bd0044d 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 @@ -119,26 +119,11 @@ class AevaHwInterface throw std::runtime_error("Reached maximum retries while trying to fetch manifest"); } - tryReconfig( - manifest, "scanner", "dithering_enable_ego_speed", config->dithering_enable_ego_speed); - tryReconfig(manifest, "scanner", "dithering_pattern_option", config->dithering_pattern_option); - tryReconfig(manifest, "scanner", "ele_offset_rad", config->ele_offset_rad); - tryReconfig( - manifest, "scanner", "elevation_auto_adjustment", config->elevation_auto_adjustment); - tryReconfig(manifest, "scanner", "enable_frame_dithering", config->enable_frame_dithering); - tryReconfig(manifest, "scanner", "enable_frame_sync", config->enable_frame_sync); - tryReconfig(manifest, "scanner", "flip_pattern_vertically", config->flip_pattern_vertically); - tryReconfig(manifest, "scanner", "frame_sync_offset_in_ms", config->frame_sync_offset_in_ms); - tryReconfig(manifest, "scanner", "frame_sync_type", config->frame_sync_type); - tryReconfig( - manifest, "scanner", "frame_synchronization_on_rising_edge", - config->frame_synchronization_on_rising_edge); - tryReconfig(manifest, "scanner", "hfov_adjustment_deg", config->hfov_adjustment_deg); - tryReconfig(manifest, "scanner", "hfov_rotation_deg", config->hfov_rotation_deg); - tryReconfig(manifest, "scanner", "highlight_ROI", config->highlight_ROI); - tryReconfig(manifest, "scanner", "horizontal_fov_degrees", config->horizontal_fov_degrees); - tryReconfig(manifest, "scanner", "roi_az_offset_rad", config->roi_az_offset_rad); - tryReconfig(manifest, "scanner", "vertical_pattern", config->vertical_pattern); + for (const auto & category : config->tree.items()) { + for (const auto & setting : category.value().items()) { + tryReconfig(manifest, category.key(), setting.key(), setting.value()); + } + } } void registerCloudPacketCallback(PointcloudParser::callback_t callback) @@ -162,11 +147,11 @@ class AevaHwInterface } private: - template void tryReconfig( - const json & manifest, const std::string & node_name, const std::string & key, const T & value) + 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"}); + auto old_value_opt = util::get_if_exists(manifest, {node_name, key, "value"}); if (old_value_opt && old_value_opt.value() == value) return; try { @@ -179,14 +164,7 @@ class AevaHwInterface // Value was successfully updated // //////////////////////////////////////// - std::string value_str; - if constexpr (std::is_same_v) { - value_str = value; - } else { - value_str = std::to_string(value); - } - - logger_->info("Set " + node_name + "." + key + " to " + value_str); + logger_->info("Set " + node_name + "." + key + " to " + value.dump()); } static std::shared_ptr makeReconfigApi( diff --git a/nebula_ros/config/lidar/aeva/Aeries2.param.yaml b/nebula_ros/config/lidar/aeva/Aeries2.param.yaml index 33bcdd04f..0fcd4b3cb 100644 --- a/nebula_ros/config/lidar/aeva/Aeries2.param.yaml +++ b/nebula_ros/config/lidar/aeva/Aeries2.param.yaml @@ -6,19 +6,28 @@ sensor_model: Aeries2 setup_sensor: true diag_span: 1000 - dithering_enable_ego_speed: 40.0 - dithering_pattern_option: dithering_pattern_1 - ele_offset_rad: 0.0 - elevation_auto_adjustment: false - enable_frame_dithering: false - enable_frame_sync: true - flip_pattern_vertically: false - frame_sync_offset_in_ms: 999 - frame_sync_type: nearest_second - frame_synchronization_on_rising_edge: true - hfov_adjustment_deg: 0.0 - hfov_rotation_deg: 0.0 - highlight_ROI: false - horizontal_fov_degrees: 110Degrees - roi_az_offset_rad: 0.0 - vertical_pattern: 40-14.4-ROI + scanner.dithering_enable_ego_speed: 40.0 + scanner.dithering_pattern_option: dithering_pattern_1 + scanner.ele_offset_rad: 0.0 + scanner.elevation_auto_adjustment: false + scanner.enable_frame_dithering: false + scanner.enable_frame_sync: true + scanner.flip_pattern_vertically: false + scanner.frame_sync_offset_in_ms: 999 + scanner.frame_sync_type: nearest_second + scanner.frame_synchronization_on_rising_edge: false + scanner.hfov_adjustment_deg: 0.0 + scanner.hfov_rotation_deg: 0.0 + scanner.highlight_ROI: false + scanner.horizontal_fov_degrees: 110Degrees + scanner.roi_az_offset_rad: 0.0 + scanner.vertical_pattern: 40-14.4-ROI + system_config.range_modes: Normal Range + system_config.sensitivity_mode: normal + system_config.thermal_throttling_setting: Active + spc_converter.discard_points_in_ambiguity_region: false + spc_converter.display_all_points: false + spc_converter.enable_min_range_filter: true + dsp_control.second_peak_type: strongest + dsp_control.use_foveated_velocity_bias: false + dsp_control.velocity_bias_pattern_options: velocity_bias_pattern_1 diff --git a/nebula_ros/include/nebula_ros/aeva/aeva_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/aeva/aeva_ros_wrapper.hpp index 512e41d03..118c7abf8 100644 --- a/nebula_ros/include/nebula_ros/aeva/aeva_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/aeva/aeva_ros_wrapper.hpp @@ -15,6 +15,7 @@ #pragma once #include "nebula_ros/aeva/hw_monitor_wrapper.hpp" +#include "nebula_ros/common/parameter_descriptors.hpp" #include "nebula_ros/common/watchdog_timer.hpp" #include @@ -22,9 +23,11 @@ #include #include #include +#include #include #include #include +#include #include #include #include @@ -40,11 +43,14 @@ #include #include #include +#include #include namespace nebula::ros { +using nlohmann::json; + /// @brief Ros wrapper of hesai driver class AevaRosWrapper final : public rclcpp::Node { @@ -53,6 +59,30 @@ class AevaRosWrapper final : public rclcpp::Node private: Status declareAndGetSensorConfigParams(); + + template + void declareJsonParam(const std::string & dot_delimited_path, json & inout_tree) + { + json param_value = declare_parameter(dot_delimited_path, param_read_write()); + json tree_patch = util::to_json_tree(param_value, util::to_json_path(dot_delimited_path)); + inout_tree.update(tree_patch, true); + } + + template + bool getJsonParam( + const std::vector & p, const std::string & dot_delimited_path, + json & inout_tree) + { + T value; + bool got_param = get_param(p, dot_delimited_path, value); + if (!got_param) return false; + + json json_value = value; + json tree_patch = util::to_json_tree(json_value, util::to_json_path(dot_delimited_path)); + inout_tree.update(tree_patch, true); + return true; + } + Status validateAndSetConfig(std::shared_ptr & new_config); rcl_interfaces::msg::SetParametersResult onParameterChange( diff --git a/nebula_ros/schema/Aeries2.schema.json b/nebula_ros/schema/Aeries2.schema.json index f1b0b3d1a..693970ae4 100644 --- a/nebula_ros/schema/Aeries2.schema.json +++ b/nebula_ros/schema/Aeries2.schema.json @@ -7,16 +7,23 @@ "type": "object", "properties": { "sensor_ip": { - "$ref": "sub/communication.json#/definitions/sensor_ip" + "$ref": "sub/communication.json#/definitions/sensor_ip", + "default": "192.168.2.201" }, "launch_hw": { - "$ref": "sub/hardware.json#/definitions/launch_hw" + "$ref": "sub/hardware.json#/definitions/launch_hw", + "default": true }, "frame_id": { - "$ref": "sub/topic.json#/definitions/frame_id" + "$ref": "sub/topic.json#/definitions/frame_id", + "default": "aeva" }, "sensor_model": { - "$ref": "sub/lidar_aeva.json#/definitions/sensor_model" + "$ref": "sub/lidar_aeva.json#/definitions/sensor_model", + "enum": [ + "Aeries2" + ], + "default": "Aeries2" }, "setup_sensor": { "$ref": "sub/hardware.json#/definitions/setup_sensor" @@ -24,15 +31,15 @@ "diag_span": { "$ref": "sub/topic.json#/definitions/diag_span" }, - "dithering_enable_ego_speed": { + "scanner.dithering_enable_ego_speed": { "type": "number", "readOnly": false, "description": "The ego speed in m/s from which pointcloud dithering (different patterns per frame) is enabled.", "max": 200.0, "min": 0.0, - "default": 40.0 + "default": "40.0" }, - "dithering_pattern_option": { + "scanner.dithering_pattern_option": { "type": "string", "readOnly": false, "description": "The dithering pattern to use if enabled.", @@ -42,39 +49,39 @@ ], "default": "dithering_pattern_1" }, - "ele_offset_rad": { + "scanner.ele_offset_rad": { "type": "number", "readOnly": false, "description": "Elevation offset used to align ROI to ground level at a certain distance.", "max": 0.35, "min": -0.35, - "default": 0.0 + "default": "0.0" }, - "elevation_auto_adjustment": { + "scanner.elevation_auto_adjustment": { "type": "boolean", "readOnly": false, "description": "Adjusts elevation to a long-range setting when the car is going sufficiently fast.", - "default": false + "default": "false" }, - "enable_frame_dithering": { + "scanner.enable_frame_dithering": { "type": "boolean", "readOnly": false, "description": "Whether dithering is enabled.", - "default": false + "default": "false" }, - "enable_frame_sync": { + "scanner.enable_frame_sync": { "type": "boolean", "readOnly": false, "description": "Whether to sync pointcloud frame output to a given sub-second offset.", "default": true }, - "flip_pattern_vertically": { + "scanner.flip_pattern_vertically": { "type": "boolean", "readOnly": false, "description": "Whether to flip the scan pattern vertically.", - "default": false + "default": "false" }, - "frame_sync_offset_in_ms": { + "scanner.frame_sync_offset_in_ms": { "type": "integer", "readOnly": false, "description": "The sub-second offset to sync pointcloud frame starts to. 0 for top-of-second alignment. In practice, 999 achieves results closer to top-of-second", @@ -82,7 +89,7 @@ "max": 999, "default": 999 }, - "frame_sync_type": { + "scanner.frame_sync_type": { "type": "string", "readOnly": false, "description": "Whether to start sync at an absolute timestamp in the future, or from the nearest second.", @@ -92,35 +99,35 @@ ], "default": "nearest_second" }, - "frame_synchronization_on_rising_edge": { + "scanner.frame_synchronization_on_rising_edge": { "type": "boolean", "readOnly": false, "description": "Contact vendor for info.", - "default": true + "default": "false" }, - "hfov_adjustment_deg": { + "scanner.hfov_adjustment_deg": { "type": "number", "readOnly": false, "description": "The number of degrees to expend or shrink the horizontal field of view by.", "max": 100.0, "min": -100.0, - "default": 0.0 + "default": "0.0" }, - "hfov_rotation_deg": { + "scanner.hfov_rotation_deg": { "type": "number", "readOnly": false, "description": "Number of degrees to shift the horizontal field of view to the left (-) or right (+).", "max": 100.0, "min": -100.0, - "default": 0.0 + "default": "0.0" }, - "highlight_ROI": { + "scanner.highlight_ROI": { "type": "boolean", "readOnly": false, "description": "A debug option to visually highlight points in the configured region of interest.", - "default": false + "default": "false" }, - "horizontal_fov_degrees": { + "scanner.horizontal_fov_degrees": { "type": "string", "readOnly": false, "description": "The horizontal field of view of the sensor. Larger FoVs use more power.", @@ -130,15 +137,15 @@ ], "default": "110Degrees" }, - "roi_az_offset_rad": { + "scanner.roi_az_offset_rad": { "type": "number", "readOnly": false, "description": "Shift the ROI (if configured) to the left (-) or right (+).", "max": 0.2094, "min": -0.2094, - "default": 0.0 + "default": "0.0" }, - "vertical_pattern": { + "scanner.vertical_pattern": { "type": "string", "readOnly": false, "description": "The scan pattern to use. Refer to the user manual for details.", @@ -159,6 +166,67 @@ "cal_192_384-19.2-Uniform-5f" ], "default": "40-14.4-ROI" + }, + "system_config.range_modes": { + "type": "string", + "enum": [ + "Long Range", + "Medium Range", + "Normal Range", + "Short Range", + "Ultra Long Range", + "Very Long Range" + ], + "default": "Normal Range" + }, + "system_config.sensitivity_mode": { + "type": "string", + "enum": [ + "higher", + "normal" + ], + "default": "normal" + }, + "system_config.thermal_throttling_setting": { + "type": "string", + "enum": [ + "Active", + "Inactive" + ], + "default": "Active" + }, + "spc_converter.discard_points_in_ambiguity_region": { + "type": "boolean", + "default": "false" + }, + "spc_converter.display_all_points": { + "type": "boolean", + "default": "false" + }, + "spc_converter.enable_min_range_filter": { + "type": "boolean", + "default": true + }, + "dsp_control.second_peak_type": { + "type": "string", + "enum": [ + "closest", + "farthest", + "strongest" + ], + "default": "strongest" + }, + "dsp_control.use_foveated_velocity_bias": { + "type": "boolean", + "default": "false" + }, + "dsp_control.velocity_bias_pattern_options": { + "type": "string", + "enum": [ + "velocity_bias_pattern_1", + "velocity_bias_pattern_2" + ], + "default": "velocity_bias_pattern_1" } }, "required": [ @@ -168,22 +236,31 @@ "sensor_model", "setup_sensor", "diag_span", - "dithering_enable_ego_speed", - "dithering_pattern_option", - "ele_offset_rad", - "elevation_auto_adjustment", - "enable_frame_dithering", - "enable_frame_sync", - "flip_pattern_vertically", - "frame_sync_offset_in_ms", - "frame_sync_type", - "frame_synchronization_on_rising_edge", - "hfov_adjustment_deg", - "hfov_rotation_deg", - "highlight_ROI", - "horizontal_fov_degrees", - "roi_az_offset_rad", - "vertical_pattern" + "scanner.dithering_enable_ego_speed", + "scanner.dithering_pattern_option", + "scanner.ele_offset_rad", + "scanner.elevation_auto_adjustment", + "scanner.enable_frame_dithering", + "scanner.enable_frame_sync", + "scanner.flip_pattern_vertically", + "scanner.frame_sync_offset_in_ms", + "scanner.frame_sync_type", + "scanner.frame_synchronization_on_rising_edge", + "scanner.hfov_adjustment_deg", + "scanner.hfov_rotation_deg", + "scanner.highlight_ROI", + "scanner.horizontal_fov_degrees", + "scanner.roi_az_offset_rad", + "scanner.vertical_pattern", + "system_config.range_modes", + "system_config.sensitivity_mode", + "system_config.thermal_throttling_setting", + "spc_converter.discard_points_in_ambiguity_region", + "spc_converter.display_all_points", + "spc_converter.enable_min_range_filter", + "dsp_control.second_peak_type", + "dsp_control.use_foveated_velocity_bias", + "dsp_control.velocity_bias_pattern_options" ], "additionalProperties": false } diff --git a/nebula_ros/src/aeva/aeva_ros_wrapper.cpp b/nebula_ros/src/aeva/aeva_ros_wrapper.cpp index b4a8da84b..ed6e5defd 100644 --- a/nebula_ros/src/aeva/aeva_ros_wrapper.cpp +++ b/nebula_ros/src/aeva/aeva_ros_wrapper.cpp @@ -169,30 +169,32 @@ Status AevaRosWrapper::declareAndGetSensorConfigParams() config.sensor_model = drivers::SensorModelFromString(raw_sensor_model); config.sensor_ip = declare_parameter("sensor_ip", param_read_only()); config.frame_id = declare_parameter("frame_id", param_read_only()); - config.dithering_enable_ego_speed = - declare_parameter("dithering_enable_ego_speed", param_read_write()); - config.dithering_pattern_option = - declare_parameter("dithering_pattern_option", param_read_write()); - config.ele_offset_rad = declare_parameter("ele_offset_rad", param_read_write()); - config.elevation_auto_adjustment = - declare_parameter("elevation_auto_adjustment", param_read_write()); - config.enable_frame_dithering = - declare_parameter("enable_frame_dithering", param_read_write()); - config.enable_frame_sync = declare_parameter("enable_frame_sync", param_read_write()); - config.flip_pattern_vertically = - declare_parameter("flip_pattern_vertically", param_read_write()); - config.frame_sync_offset_in_ms = - declare_parameter("frame_sync_offset_in_ms", param_read_write()); - config.frame_sync_type = declare_parameter("frame_sync_type", param_read_write()); - config.frame_synchronization_on_rising_edge = - declare_parameter("frame_synchronization_on_rising_edge", param_read_write()); - config.hfov_adjustment_deg = declare_parameter("hfov_adjustment_deg", param_read_write()); - config.hfov_rotation_deg = declare_parameter("hfov_rotation_deg", param_read_write()); - config.highlight_ROI = declare_parameter("highlight_ROI", param_read_write()); - config.horizontal_fov_degrees = - declare_parameter("horizontal_fov_degrees", param_read_write()); - config.roi_az_offset_rad = declare_parameter("roi_az_offset_rad", param_read_write()); - config.vertical_pattern = declare_parameter("vertical_pattern", param_read_write()); + + declareJsonParam("scanner.dithering_enable_ego_speed", config.tree); + declareJsonParam("scanner.dithering_pattern_option", config.tree); + declareJsonParam("scanner.ele_offset_rad", config.tree); + declareJsonParam("scanner.elevation_auto_adjustment", 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); auto new_cfg_ptr = std::make_shared(config); return validateAndSetConfig(new_cfg_ptr); @@ -232,23 +234,31 @@ rcl_interfaces::msg::SetParametersResult AevaRosWrapper::onParameterChange( Aeries2Config config = *sensor_cfg_ptr_; bool got_any = - get_param(p, "dithering_enable_ego_speed", config.dithering_enable_ego_speed) | - get_param(p, "dithering_pattern_option", config.dithering_pattern_option) | - get_param(p, "ele_offset_rad", config.ele_offset_rad) | - get_param(p, "elevation_auto_adjustment", config.elevation_auto_adjustment) | - get_param(p, "enable_frame_dithering", config.enable_frame_dithering) | - get_param(p, "enable_frame_sync", config.enable_frame_sync) | - get_param(p, "flip_pattern_vertically", config.flip_pattern_vertically) | - get_param(p, "frame_sync_offset_in_ms", config.frame_sync_offset_in_ms) | - get_param(p, "frame_sync_type", config.frame_sync_type) | - get_param( - p, "frame_synchronization_on_rising_edge", config.frame_synchronization_on_rising_edge) | - get_param(p, "hfov_adjustment_deg", config.hfov_adjustment_deg) | - get_param(p, "hfov_rotation_deg", config.hfov_rotation_deg) | - get_param(p, "highlight_ROI", config.highlight_ROI) | - get_param(p, "horizontal_fov_degrees", config.horizontal_fov_degrees) | - get_param(p, "roi_az_offset_rad", config.roi_az_offset_rad) | - get_param(p, "vertical_pattern", config.vertical_pattern); + 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.elevation_auto_adjustment", 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); if (!got_any) { return rcl_interfaces::build().successful(true).reason(""); From d6cd72f14a49a6d4a9ec349a8e90f813d512b1a8 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Mon, 8 Jul 2024 23:10:44 +0900 Subject: [PATCH 33/57] chore(aeva): set frame id to `nebula` --- nebula_ros/config/lidar/aeva/Aeries2.param.yaml | 2 +- nebula_ros/schema/Aeries2.schema.json | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/nebula_ros/config/lidar/aeva/Aeries2.param.yaml b/nebula_ros/config/lidar/aeva/Aeries2.param.yaml index 0fcd4b3cb..21aaba051 100644 --- a/nebula_ros/config/lidar/aeva/Aeries2.param.yaml +++ b/nebula_ros/config/lidar/aeva/Aeries2.param.yaml @@ -2,7 +2,7 @@ ros__parameters: sensor_ip: 192.168.2.201 launch_hw: true - frame_id: aeva + frame_id: nebula sensor_model: Aeries2 setup_sensor: true diag_span: 1000 diff --git a/nebula_ros/schema/Aeries2.schema.json b/nebula_ros/schema/Aeries2.schema.json index 693970ae4..9a5d7ec9d 100644 --- a/nebula_ros/schema/Aeries2.schema.json +++ b/nebula_ros/schema/Aeries2.schema.json @@ -16,7 +16,7 @@ }, "frame_id": { "$ref": "sub/topic.json#/definitions/frame_id", - "default": "aeva" + "default": "nebula" }, "sensor_model": { "$ref": "sub/lidar_aeva.json#/definitions/sensor_model", From 063d87b2480b102c1fd7a1801d8ae81e909f197d Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Mon, 8 Jul 2024 23:53:52 +0900 Subject: [PATCH 34/57] feat(aeva): correct handling of the currently active return mode --- .../nebula_common/aeva/config_types.hpp | 14 +++++++ .../include/nebula_common/nebula_common.hpp | 6 +++ .../aeva_aeries2_decoder.hpp | 8 +++- .../aeva_aeries2_decoder.cpp | 40 +++++++++++-------- nebula_ros/src/aeva/aeva_ros_wrapper.cpp | 11 +++++ 5 files changed, 61 insertions(+), 18 deletions(-) diff --git a/nebula_common/include/nebula_common/aeva/config_types.hpp b/nebula_common/include/nebula_common/aeva/config_types.hpp index 8acef5cef..7155dbb27 100644 --- a/nebula_common/include/nebula_common/aeva/config_types.hpp +++ b/nebula_common/include/nebula_common/aeva/config_types.hpp @@ -15,10 +15,12 @@ #pragma once #include "nebula_common/nebula_common.hpp" +#include "nebula_common/util/parsing.hpp" #include #include +#include #include #include @@ -31,6 +33,18 @@ struct Aeries2Config : public SensorConfigurationBase { std::string sensor_ip; json tree; + + [[nodiscard]] std::optional getReturnMode() const + { + auto mode_name = util::get_if_exists(tree, {"dsp_control", "second_peak_type"}); + + if (!mode_name) return {}; + if (mode_name == "strongest") return ReturnMode::DUAL_STRONGEST_SECONDSTRONGEST; + if (mode_name == "farthest") return ReturnMode::DUAL_STRONGEST_LAST; + if (mode_name == "closest") return ReturnMode::DUAL_STRONGEST_FIRST; + + return ReturnMode::UNKNOWN; + } }; inline std::ostream & operator<<(std::ostream & os, const Aeries2Config & arg) diff --git a/nebula_common/include/nebula_common/nebula_common.hpp b/nebula_common/include/nebula_common/nebula_common.hpp index e50ab016e..e8ba4e01b 100644 --- a/nebula_common/include/nebula_common/nebula_common.hpp +++ b/nebula_common/include/nebula_common/nebula_common.hpp @@ -64,6 +64,7 @@ enum class ReturnMode : uint8_t { FIRST, DUAL_LAST_FIRST, DUAL_FIRST_STRONGEST, + DUAL_STRONGEST_SECONDSTRONGEST, DUAL }; @@ -196,6 +197,8 @@ inline uint8_t return_mode_to_int(const ReturnMode & mode) case ReturnMode::DUAL: return 18; break; + case ReturnMode::DUAL_STRONGEST_SECONDSTRONGEST: + return 19; default: case ReturnMode::UNKNOWN: return 0; @@ -306,6 +309,9 @@ inline std::ostream & operator<<(std::ostream & os, nebula::drivers::ReturnMode case ReturnMode::DUAL_FIRST_STRONGEST: os << "FirstStrongest"; break; + case ReturnMode::DUAL_STRONGEST_SECONDSTRONGEST: + os << "StrongestSecondstrongest"; + break; case ReturnMode::DUAL: os << "Dual"; break; 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 cdd90cde8..f2900c1ef 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 @@ -21,6 +21,7 @@ #include #include +#include #include #include #include @@ -46,6 +47,8 @@ class AevaAeries2Decoder void registerPointCloudCallback(callback_t callback); + void onParameterChange(ReturnMode return_mode); + private: struct DecoderState { @@ -62,9 +65,10 @@ class AevaAeries2Decoder uint64_t timestamp; }; + ReturnType getReturnType(uint32_t peak_id); + callback_t callback_; + std::atomic return_mode_; PointcloudState cloud_state_{}; - - std::mutex mtx_callback_; }; } // 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 e324886c1..af34c69ec 100644 --- a/nebula_decoders/src/nebula_decoders_aeva/aeva_aeries2_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_aeva/aeva_aeries2_decoder.cpp @@ -22,8 +22,6 @@ void AevaAeries2Decoder::processPointcloudMessage(const aeva::PointCloudMessage } if (static_cast(i) == state.new_frame_index) { - std::scoped_lock lock(mtx_callback_); - if (callback_) { callback_(std::move(cloud_state_.cloud), cloud_state_.timestamp); } @@ -51,19 +49,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{ReturnType::UNKNOWN}; - // TODO(mojomex): Currently, there is no info published by the sensor on which return mode is - // active. Here, the default one is hardcoded for now. - switch (raw_point.peak_id) { - case 0: - return_type = ReturnType::STRONGEST; - break; - case 1: - return_type = ReturnType::SECONDSTRONGEST; - break; - default: - return_type = ReturnType::UNKNOWN; - } + ReturnType return_type = getReturnType(raw_point.peak_id); point.return_type = static_cast(return_type); @@ -82,10 +68,32 @@ void AevaAeries2Decoder::processPointcloudMessage(const aeva::PointCloudMessage } } +ReturnType AevaAeries2Decoder::getReturnType(uint32_t peak_id) +{ + if (peak_id == 0) return ReturnType::STRONGEST; + if (peak_id > 1) return ReturnType::UNKNOWN; + + switch (return_mode_.load()) { + case ReturnMode::DUAL_STRONGEST_FIRST: + return ReturnType::FIRST; + case ReturnMode::DUAL_STRONGEST_LAST: + return ReturnType::LAST; + case ReturnMode::DUAL_STRONGEST_SECONDSTRONGEST: + return ReturnType::SECONDSTRONGEST; + default: + return ReturnType::UNKNOWN; + } +} + +void AevaAeries2Decoder::onParameterChange(ReturnMode return_mode) +{ + return_mode_.store(return_mode); +} + void AevaAeries2Decoder::registerPointCloudCallback( std::function, uint64_t)> callback) { - std::lock_guard lock(mtx_callback_); callback_ = std::move(callback); } + } // namespace nebula::drivers diff --git a/nebula_ros/src/aeva/aeva_ros_wrapper.cpp b/nebula_ros/src/aeva/aeva_ros_wrapper.cpp index ed6e5defd..22ac7bdcf 100644 --- a/nebula_ros/src/aeva/aeva_ros_wrapper.cpp +++ b/nebula_ros/src/aeva/aeva_ros_wrapper.cpp @@ -223,6 +223,17 @@ Status AevaRosWrapper::validateAndSetConfig(std::shared_ptr } } + auto return_mode_opt = new_config->getReturnMode(); + + if (return_mode_opt && *return_mode_opt == drivers::ReturnMode::UNKNOWN) { + RCLCPP_ERROR_STREAM(get_logger(), "Invalid return mode"); + return Status::SENSOR_CONFIG_ERROR; + } + + if (return_mode_opt) { + decoder_.onParameterChange(*return_mode_opt); + } + sensor_cfg_ptr_ = new_config; return Status::OK; } From 1519e9979fa06730d3d8378264404e1a4236f5fc Mon Sep 17 00:00:00 2001 From: Max Schmeller <6088931+mojomex@users.noreply.github.com> Date: Mon, 22 Jul 2024 12:24:20 +0900 Subject: [PATCH 35/57] chore: accept suggestion to make pointcloud topic namespace local Co-authored-by: David Wong <33114676+drwnz@users.noreply.github.com> --- nebula_ros/src/aeva/aeva_ros_wrapper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nebula_ros/src/aeva/aeva_ros_wrapper.cpp b/nebula_ros/src/aeva/aeva_ros_wrapper.cpp index 22ac7bdcf..857bc1872 100644 --- a/nebula_ros/src/aeva/aeva_ros_wrapper.cpp +++ b/nebula_ros/src/aeva/aeva_ros_wrapper.cpp @@ -126,7 +126,7 @@ AevaRosWrapper::AevaRosWrapper(const rclcpp::NodeOptions & options) hw_interface_->registerCloudPacketCallback(std::move(pointcloud_message_cb)); - cloud_pub_ = create_publisher("/nebula_points", pointcloud_qos); + cloud_pub_ = create_publisher("nebula_points", pointcloud_qos); cloud_watchdog_ = std::make_shared(*this, 110'000us, [&](bool ok) { if (ok) return; From 6ac81841e4608c165fa1b5e79c5c7d4386f39379 Mon Sep 17 00:00:00 2001 From: Max Schmeller <6088931+mojomex@users.noreply.github.com> Date: Mon, 22 Jul 2024 19:53:11 +0900 Subject: [PATCH 36/57] chore: remove json_fwd header imports --- nebula_common/include/nebula_common/util/parsing.hpp | 1 - nebula_ros/include/nebula_ros/aeva/aeva_ros_wrapper.hpp | 1 - nebula_ros/src/aeva/aeva_ros_wrapper.cpp | 2 +- nebula_ros/src/aeva/hw_monitor_wrapper.cpp | 2 +- 4 files changed, 2 insertions(+), 4 deletions(-) diff --git a/nebula_common/include/nebula_common/util/parsing.hpp b/nebula_common/include/nebula_common/util/parsing.hpp index c81c970cf..94022fa4e 100644 --- a/nebula_common/include/nebula_common/util/parsing.hpp +++ b/nebula_common/include/nebula_common/util/parsing.hpp @@ -15,7 +15,6 @@ #pragma once #include -#include #include #include 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 118c7abf8..d4bae4e2b 100644 --- a/nebula_ros/include/nebula_ros/aeva/aeva_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/aeva/aeva_ros_wrapper.hpp @@ -27,7 +27,6 @@ #include #include #include -#include #include #include #include diff --git a/nebula_ros/src/aeva/aeva_ros_wrapper.cpp b/nebula_ros/src/aeva/aeva_ros_wrapper.cpp index 857bc1872..b3fb6dc17 100644 --- a/nebula_ros/src/aeva/aeva_ros_wrapper.cpp +++ b/nebula_ros/src/aeva/aeva_ros_wrapper.cpp @@ -16,7 +16,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/nebula_ros/src/aeva/hw_monitor_wrapper.cpp b/nebula_ros/src/aeva/hw_monitor_wrapper.cpp index 41a606a81..acdd9aef9 100644 --- a/nebula_ros/src/aeva/hw_monitor_wrapper.cpp +++ b/nebula_ros/src/aeva/hw_monitor_wrapper.cpp @@ -6,7 +6,7 @@ #include #include -#include +#include #include #include From e1be90cd5f04799e5b725a2f4162c9247b790dc2 Mon Sep 17 00:00:00 2001 From: Max Schmeller <6088931+mojomex@users.noreply.github.com> Date: Mon, 22 Jul 2024 19:54:35 +0900 Subject: [PATCH 37/57] chore: move diagnostics topic to local namespace --- nebula_ros/src/aeva/hw_monitor_wrapper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nebula_ros/src/aeva/hw_monitor_wrapper.cpp b/nebula_ros/src/aeva/hw_monitor_wrapper.cpp index acdd9aef9..1b7da1507 100644 --- a/nebula_ros/src/aeva/hw_monitor_wrapper.cpp +++ b/nebula_ros/src/aeva/hw_monitor_wrapper.cpp @@ -37,7 +37,7 @@ AevaHwMonitorWrapper::AevaHwMonitorWrapper( diag_span_ = parent_node->declare_parameter("diag_span", 500, param_read_only()); diagnostics_pub_ = - parent_node->create_publisher("/diagnostics", 10); + parent_node->create_publisher("diagnostics", 10); diagnostics_pub_timer_ = parent_node->create_wall_timer( std::chrono::milliseconds(diag_span_), [&]() { publishDiagnostics(); }); From 03622ad07968b9f1ff1c3df3da3f804a59e770a2 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Tue, 23 Jul 2024 16:48:43 +0900 Subject: [PATCH 38/57] fix(aeva): increase buffer size for packet replay as drops were observed --- nebula_ros/src/aeva/aeva_ros_wrapper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nebula_ros/src/aeva/aeva_ros_wrapper.cpp b/nebula_ros/src/aeva/aeva_ros_wrapper.cpp index b3fb6dc17..2f102d3df 100644 --- a/nebula_ros/src/aeva/aeva_ros_wrapper.cpp +++ b/nebula_ros/src/aeva/aeva_ros_wrapper.cpp @@ -96,7 +96,7 @@ AevaRosWrapper::AevaRosWrapper(const rclcpp::NodeOptions & options) // //////////////////////////////////////// auto packet_stream = std::make_shared(); auto packet_buffer = - std::make_shared(packet_stream, 100, [&]() { + std::make_shared(packet_stream, 1000, [&]() { RCLCPP_ERROR_THROTTLE( get_logger(), *get_clock(), 1000, "Packet stream buffer overflowed, packet loss occurred."); From a353f72e0bb10af1f9dc0bede4ad72b1bac4a90c Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Tue, 3 Sep 2024 10:57:15 +0900 Subject: [PATCH 39/57] fix(aeva): add missing diagnostic_updater dependency in CMakeLists.txt --- nebula_ros/CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/nebula_ros/CMakeLists.txt b/nebula_ros/CMakeLists.txt index 19c4b085c..c29a83996 100644 --- a/nebula_ros/CMakeLists.txt +++ b/nebula_ros/CMakeLists.txt @@ -223,11 +223,13 @@ add_library(aeva_ros_wrapper SHARED ) target_include_directories(aeva_ros_wrapper PUBLIC + ${diagnostic_updater_INCLUDE_DIRS} ${nebula_decoders_INCLUDE_DIRS} ${nebula_hw_interfaces_INCLUDE_DIRS} ) target_link_libraries(aeva_ros_wrapper PUBLIC + ${diagnostic_updater_TARGETS} nebula_decoders::nebula_decoders_aeva nebula_hw_interfaces::nebula_hw_interfaces_aeva ) From 7d156075d613aca595029c237c61025aa87229c8 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Tue, 12 Nov 2024 16:05:54 +0900 Subject: [PATCH 40/57] fix(aeva): delete elevation_auto_adjustment param that is not present in FW14 anymore Signed-off-by: Max SCHMELLER --- nebula_ros/config/lidar/aeva/Aeries2.param.yaml | 1 - nebula_ros/schema/Aeries2.schema.json | 6 ------ nebula_ros/src/aeva/aeva_ros_wrapper.cpp | 2 -- 3 files changed, 9 deletions(-) diff --git a/nebula_ros/config/lidar/aeva/Aeries2.param.yaml b/nebula_ros/config/lidar/aeva/Aeries2.param.yaml index 21aaba051..8f8fc694b 100644 --- a/nebula_ros/config/lidar/aeva/Aeries2.param.yaml +++ b/nebula_ros/config/lidar/aeva/Aeries2.param.yaml @@ -9,7 +9,6 @@ scanner.dithering_enable_ego_speed: 40.0 scanner.dithering_pattern_option: dithering_pattern_1 scanner.ele_offset_rad: 0.0 - scanner.elevation_auto_adjustment: false scanner.enable_frame_dithering: false scanner.enable_frame_sync: true scanner.flip_pattern_vertically: false diff --git a/nebula_ros/schema/Aeries2.schema.json b/nebula_ros/schema/Aeries2.schema.json index 9a5d7ec9d..659254724 100644 --- a/nebula_ros/schema/Aeries2.schema.json +++ b/nebula_ros/schema/Aeries2.schema.json @@ -57,12 +57,6 @@ "min": -0.35, "default": "0.0" }, - "scanner.elevation_auto_adjustment": { - "type": "boolean", - "readOnly": false, - "description": "Adjusts elevation to a long-range setting when the car is going sufficiently fast.", - "default": "false" - }, "scanner.enable_frame_dithering": { "type": "boolean", "readOnly": false, diff --git a/nebula_ros/src/aeva/aeva_ros_wrapper.cpp b/nebula_ros/src/aeva/aeva_ros_wrapper.cpp index 2f102d3df..cb8567568 100644 --- a/nebula_ros/src/aeva/aeva_ros_wrapper.cpp +++ b/nebula_ros/src/aeva/aeva_ros_wrapper.cpp @@ -173,7 +173,6 @@ Status AevaRosWrapper::declareAndGetSensorConfigParams() declareJsonParam("scanner.dithering_enable_ego_speed", config.tree); declareJsonParam("scanner.dithering_pattern_option", config.tree); declareJsonParam("scanner.ele_offset_rad", config.tree); - declareJsonParam("scanner.elevation_auto_adjustment", config.tree); declareJsonParam("scanner.enable_frame_dithering", config.tree); declareJsonParam("scanner.enable_frame_sync", config.tree); declareJsonParam("scanner.flip_pattern_vertically", config.tree); @@ -248,7 +247,6 @@ rcl_interfaces::msg::SetParametersResult AevaRosWrapper::onParameterChange( 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.elevation_auto_adjustment", 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) | From 0c94de3c32c04c4c9c9a6549b2d046b28adea7a5 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Tue, 12 Nov 2024 16:57:57 +0900 Subject: [PATCH 41/57] fix(aeva): change referenced function names after merge Signed-off-by: Max SCHMELLER --- nebula_ros/src/aeva/aeva_ros_wrapper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nebula_ros/src/aeva/aeva_ros_wrapper.cpp b/nebula_ros/src/aeva/aeva_ros_wrapper.cpp index cb8567568..bc55792f2 100644 --- a/nebula_ros/src/aeva/aeva_ros_wrapper.cpp +++ b/nebula_ros/src/aeva/aeva_ros_wrapper.cpp @@ -166,7 +166,7 @@ Status AevaRosWrapper::declareAndGetSensorConfigParams() Aeries2Config config; std::string raw_sensor_model = declare_parameter("sensor_model", param_read_only()); - config.sensor_model = drivers::SensorModelFromString(raw_sensor_model); + config.sensor_model = drivers::sensor_model_from_string(raw_sensor_model); config.sensor_ip = declare_parameter("sensor_ip", param_read_only()); config.frame_id = declare_parameter("frame_id", param_read_only()); From 6c5b183a6f4646160a81f499b7a6378405ab6b6c Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Tue, 12 Nov 2024 17:02:47 +0900 Subject: [PATCH 42/57] docs(aeva): add parameter table akin to #195 Signed-off-by: Max SCHMELLER --- docs/parameters/vendors/aeva/aeries2.md | 1 + mkdocs.yml | 2 ++ 2 files changed, 3 insertions(+) create mode 100644 docs/parameters/vendors/aeva/aeries2.md diff --git a/docs/parameters/vendors/aeva/aeries2.md b/docs/parameters/vendors/aeva/aeries2.md new file mode 100644 index 000000000..079f0079b --- /dev/null +++ b/docs/parameters/vendors/aeva/aeries2.md @@ -0,0 +1 @@ +{{ json_to_markdown("nebula_ros/schema/Aeries2.schema.json") }} diff --git a/mkdocs.yml b/mkdocs.yml index 4d002789f..c5aca2028 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -63,6 +63,8 @@ nav: - parameters/vendors/continental/common.md - ARS548: parameters/vendors/continental/ars548.md - SRR520: parameters/vendors/continental/srr520.md + - Aeva: + - Aeries II: parameters/vendors/aeva/aeries2.md - Point cloud types: point_types.md - Design: design.md - Tutorials: tutorials.md From 8f8171f793d32aaecf23c6e72bf78e834f210ae0 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Tue, 12 Nov 2024 17:10:01 +0900 Subject: [PATCH 43/57] docs(aeva): mention tested firmware version (14.0.0) Signed-off-by: Max SCHMELLER --- docs/supported_sensors.md | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/docs/supported_sensors.md b/docs/supported_sensors.md index 64b7e6adc..971232ce5 100644 --- a/docs/supported_sensors.md +++ b/docs/supported_sensors.md @@ -45,7 +45,9 @@ The `sensor_model` parameter below decides which sensor driver is launched. | Model | `sensor_model` | Configuration file | Test status | | --------- | -------------- | ------------------ | ----------- | -| Aeries II | Aeries2 | Aeries2.param.yaml | ⚠️ | +| Aeries II | Aeries2 | Aeries2.param.yaml | ⚠️\* | + +\*: The Aeries II has been tested with firmware version 14.0.0. ## Continental radars From 0af2516cc2971f41c667aee094df6fff2d0eb257 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Mon, 18 Nov 2024 17:18:12 +0900 Subject: [PATCH 44/57] refactor(aeva): implement clang-tidy changes Signed-off-by: Max SCHMELLER --- .../nebula_common/aeva/config_types.hpp | 33 ++-- .../nebula_common/aeva/packet_types.hpp | 6 +- .../include/nebula_common/util/parsing.hpp | 3 +- .../aeva_aeries2_decoder.hpp | 11 +- .../aeva_aeries2_decoder.cpp | 16 +- nebula_hw_interfaces/CMakeLists.txt | 10 +- .../aeva_hw_interface.hpp | 116 +++---------- .../connections/aeva_api.hpp | 38 +++-- .../connections/health.hpp | 9 +- .../connections/pointcloud.hpp | 4 +- .../connections/reconfig.hpp | 22 +-- .../connections/telemetry.hpp | 12 +- .../connections/byte_stream.hpp | 14 +- .../connections/byte_view.hpp | 12 +- .../connections/stream_buffer.hpp | 6 +- .../connections/tcp_receiver.hpp | 5 +- .../connections/tcp_sender.hpp | 12 +- .../connections/udp_receiver.hpp | 68 -------- .../aeva_hw_interface.cpp | 100 +++++++++++- .../nebula_ros/aeva/aeva_ros_wrapper.hpp | 15 +- .../common/nebula_packet_stream.hpp | 8 +- nebula_ros/src/aeva/aeva_ros_wrapper.cpp | 153 +++++++++--------- nebula_tests/aeva/aeva_hw_interface_test.cpp | 6 +- nebula_tests/common/mock_byte_stream.hpp | 6 +- 24 files changed, 322 insertions(+), 363 deletions(-) delete mode 100644 nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/connections/udp_receiver.hpp 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_; From c692aa245fbdaacda1ef4faa1dc0be626f8c39f7 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Mon, 18 Nov 2024 17:24:20 +0900 Subject: [PATCH 45/57] chore(aeva): rename Aeries2 to Aeries II Signed-off-by: Max SCHMELLER --- nebula_common/include/nebula_common/aeva/config_types.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nebula_common/include/nebula_common/aeva/config_types.hpp b/nebula_common/include/nebula_common/aeva/config_types.hpp index 589845977..b56948979 100644 --- a/nebula_common/include/nebula_common/aeva/config_types.hpp +++ b/nebula_common/include/nebula_common/aeva/config_types.hpp @@ -47,7 +47,7 @@ struct Aeries2Config : public SensorConfigurationBase friend std::ostream & operator<<(std::ostream & os, const Aeries2Config & arg) { - os << "Aeva Aeries2 Sensor Configuration:\n"; + os << "Aeva Aeries II Sensor Configuration:\n"; os << "Sensor Model: " << arg.sensor_model << '\n'; os << "Frame ID: " << arg.frame_id << '\n'; os << "Sensor IP: " << arg.sensor_ip; From 7bccfeca1c787982c6b332cc51c407a9fe5a55fd Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Mon, 18 Nov 2024 17:39:32 +0900 Subject: [PATCH 46/57] chore(aeva_api): put constants and types to the top of the file Signed-off-by: Max SCHMELLER --- .../connections/aeva_api.hpp | 44 +++++++++---------- 1 file changed, 22 insertions(+), 22 deletions(-) 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 a9e526386..6d2c4c481 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 @@ -35,6 +35,28 @@ namespace nebula::drivers::connections { +static const uint16_t g_aeva_header = 0xAEFA; + +enum class AevaStreamType : uint16_t { + kSphericalPointCloud = 0, + kHealth = 1, + kConfig = 2, + kTelemetry = 3, + kVelocityEstimate = 4, + kCalibration = 5, + kImage = 6, + kReconfig = 7, + kVehicleStateEstimate = 8, + kLog = 9, + kImu = 10, + kObjectList = 12, + kEstimatedDetectionRange = 33, + kUnknown = 0xFFFFu +}; + +using nebula::drivers::aeva::MessageHeader; +using nebula::drivers::aeva::SomeIpHeader; + class ParseError : public std::exception { }; @@ -58,8 +80,6 @@ class MismatchError : public ParseError const std::string message_; }; -static const uint16_t g_aeva_header = 0xAEFA; - template void expect_eq(A actual, E expected, const std::string & message) { @@ -76,26 +96,6 @@ void expect_geq(A actual, E expected, const std::string & message) if (cast_actual < cast_expected) throw MismatchError(message, cast_expected, cast_actual); } -enum class AevaStreamType : uint16_t { - kSphericalPointCloud = 0, - kHealth = 1, - kConfig = 2, - kTelemetry = 3, - kVelocityEstimate = 4, - kCalibration = 5, - kImage = 6, - kReconfig = 7, - kVehicleStateEstimate = 8, - kLog = 9, - kImu = 10, - kObjectList = 12, - kEstimatedDetectionRange = 33, - kUnknown = 0xFFFFu -}; - -using nebula::drivers::aeva::MessageHeader; -using nebula::drivers::aeva::SomeIpHeader; - template T pull_and_parse( const std::vector::const_iterator & cbegin, From 6c3bd3b95627408543f4240af655677b749813b4 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Mon, 18 Nov 2024 17:40:02 +0900 Subject: [PATCH 47/57] refactor(aeva): create aeva namespace inside connections namespace Signed-off-by: Max SCHMELLER --- .../aeva_hw_interface.hpp | 8 +++--- .../connections/aeva_api.hpp | 4 +-- .../connections/health.hpp | 6 ++--- .../connections/pointcloud.hpp | 4 +-- .../connections/reconfig.hpp | 8 +++--- .../connections/telemetry.hpp | 26 +++++++++---------- nebula_ros/src/aeva/aeva_ros_wrapper.cpp | 2 +- nebula_tests/aeva/aeva_hw_interface_test.cpp | 2 +- 8 files changed, 30 insertions(+), 30 deletions(-) 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 74c3a56e3..9185b2a30 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 @@ -33,12 +33,12 @@ namespace nebula::drivers { -using connections::HealthParser; -using connections::PointcloudParser; -using connections::ReconfigParser; using connections::TcpSender; using connections::TcpStream; -using connections::TelemetryParser; +using connections::aeva::HealthParser; +using connections::aeva::PointcloudParser; +using connections::aeva::ReconfigParser; +using connections::aeva::TelemetryParser; using nlohmann::json; class AevaHwInterface 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 6d2c4c481..9faf2e4c3 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 @@ -32,7 +32,7 @@ #include #include -namespace nebula::drivers::connections +namespace nebula::drivers::connections::aeva { static const uint16_t g_aeva_header = 0xAEFA; @@ -276,4 +276,4 @@ class AevaSender uint16_t sequence_number_{}; }; -} // namespace nebula::drivers::connections +} // namespace nebula::drivers::connections::aeva diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/health.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/health.hpp index cee931e83..9e11f74ce 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 @@ -27,10 +27,10 @@ #include #include -namespace nebula::drivers::connections +namespace nebula::drivers::connections::aeva { -using aeva::HealthCode; +using drivers::aeva::HealthCode; class HealthParser : public AevaParser { @@ -72,4 +72,4 @@ class HealthParser : public AevaParser callback_t callback_; }; -} // namespace nebula::drivers::connections +} // namespace nebula::drivers::connections::aeva diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/pointcloud.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/pointcloud.hpp index 73401c190..62c368b2f 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 @@ -21,7 +21,7 @@ #include #include -namespace nebula::drivers::connections +namespace nebula::drivers::connections::aeva { using nebula::drivers::aeva::PointCloudMessage; @@ -67,4 +67,4 @@ class PointcloudParser : public AevaParser callback_t callback_; }; -} // namespace nebula::drivers::connections +} // namespace nebula::drivers::connections::aeva diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/reconfig.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/reconfig.hpp index b3692b23e..46c45582a 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 @@ -37,11 +37,11 @@ #include #include -namespace nebula::drivers::connections +namespace nebula::drivers::connections::aeva { -using aeva::ReconfigMessage; -using aeva::ReconfigRequestType; +using drivers::aeva::ReconfigMessage; +using drivers::aeva::ReconfigRequestType; using nlohmann::json; using namespace std::chrono_literals; // NOLINT @@ -269,4 +269,4 @@ class ReconfigParser : public AevaParser, static const size_t g_n_manifest_responses_expected = 7; }; -} // namespace nebula::drivers::connections +} // namespace nebula::drivers::connections::aeva diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/telemetry.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/telemetry.hpp index 520be84ad..c3eb0e874 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 @@ -31,7 +31,7 @@ #include #include -namespace nebula::drivers::connections +namespace nebula::drivers::connections::aeva { using nebula::drivers::aeva::TelemetryDataType; @@ -121,33 +121,33 @@ class TelemetryParser : public AevaParser json value; switch (type) { - case aeva::TelemetryDataType::kUInt8: + case TelemetryDataType::kUInt8: value = telemetry_detail::parse_number_array( [](const auto * ref) { return *ref; }, entry_data_raw); break; - case aeva::TelemetryDataType::kInt8: + case TelemetryDataType::kInt8: value = telemetry_detail::parse_number_array( [](const auto * ref) { return static_cast(*ref); }, entry_data_raw); break; - case aeva::TelemetryDataType::kUInt16: + case TelemetryDataType::kUInt16: value = telemetry_detail::parse_number_array(&load_little_u16, entry_data_raw); break; - case aeva::TelemetryDataType::kInt16: + case TelemetryDataType::kInt16: value = telemetry_detail::parse_number_array(&load_little_s16, entry_data_raw); break; - case aeva::TelemetryDataType::kUInt32: + case TelemetryDataType::kUInt32: value = telemetry_detail::parse_number_array(&load_little_u32, entry_data_raw); break; - case aeva::TelemetryDataType::kInt32: + case TelemetryDataType::kInt32: value = telemetry_detail::parse_number_array(&load_little_s32, entry_data_raw); break; - case aeva::TelemetryDataType::kUInt64: + case TelemetryDataType::kUInt64: value = telemetry_detail::parse_number_array(&load_little_u64, entry_data_raw); break; - case aeva::TelemetryDataType::kInt64: + case TelemetryDataType::kInt64: value = telemetry_detail::parse_number_array(&load_little_s64, entry_data_raw); break; - case aeva::TelemetryDataType::kFloat: + case TelemetryDataType::kFloat: value = telemetry_detail::parse_number_array( [](const uint8_t * ref) { auto raw_bytes = load_little_u32(ref); @@ -157,7 +157,7 @@ class TelemetryParser : public AevaParser }, entry_data_raw); break; - case aeva::TelemetryDataType::kDouble: + case TelemetryDataType::kDouble: value = telemetry_detail::parse_number_array( [](const uint8_t * ref) { auto raw_bytes = load_little_u64(ref); @@ -167,7 +167,7 @@ class TelemetryParser : public AevaParser }, entry_data_raw); break; - case aeva::TelemetryDataType::kChar: + case TelemetryDataType::kChar: auto overrides = telemetry_detail::g_type_overrides; bool has_override = std::find(overrides.begin(), overrides.end(), key) != overrides.end(); if (has_override) { @@ -204,4 +204,4 @@ class TelemetryParser : public AevaParser callback_t callback_; }; -} // namespace nebula::drivers::connections +} // namespace nebula::drivers::connections::aeva diff --git a/nebula_ros/src/aeva/aeva_ros_wrapper.cpp b/nebula_ros/src/aeva/aeva_ros_wrapper.cpp index 3f8d70cbc..328a1b894 100644 --- a/nebula_ros/src/aeva/aeva_ros_wrapper.cpp +++ b/nebula_ros/src/aeva/aeva_ros_wrapper.cpp @@ -39,8 +39,8 @@ namespace nebula::ros { using drivers::AevaAeries2Decoder; +using drivers::PointcloudParser; using drivers::aeva::Aeries2Config; -using drivers::connections::PointcloudParser; using nlohmann::json; using namespace std::chrono_literals; // NOLINT using AevaPointCloudUniquePtr = AevaAeries2Decoder::AevaPointCloudUniquePtr; diff --git a/nebula_tests/aeva/aeva_hw_interface_test.cpp b/nebula_tests/aeva/aeva_hw_interface_test.cpp index 119f6f870..c4d51d0cb 100644 --- a/nebula_tests/aeva/aeva_hw_interface_test.cpp +++ b/nebula_tests/aeva/aeva_hw_interface_test.cpp @@ -16,7 +16,7 @@ #include using nebula::drivers::aeva::PointCloudMessage; -using nebula::drivers::connections::PointcloudParser; +using nebula::drivers::connections::aeva::PointcloudParser; TEST(TestParsing, Pointcloud) // NOLINT { From 24d135f8ecc5fc9b6626b379de5bd28116c53e91 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Mon, 18 Nov 2024 17:45:14 +0900 Subject: [PATCH 48/57] chore(aeva): move hardcoded port numbers to constants Signed-off-by: Max SCHMELLER --- .../connections/aeva_api.hpp | 11 +++++++++++ .../aeva_hw_interface.cpp | 16 +++++++++++----- 2 files changed, 22 insertions(+), 5 deletions(-) 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 9faf2e4c3..55999a904 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 @@ -37,6 +37,17 @@ namespace nebula::drivers::connections::aeva static const uint16_t g_aeva_header = 0xAEFA; +static const uint16_t g_port_spherical_point_cloud = 41000; +static const uint16_t g_port_health = 41001; +static const uint16_t g_port_config = 41002; +static const uint16_t g_port_telemetry = 41003; +static const uint16_t g_port_calibration = 41005; +static const uint16_t g_port_image = 41006; +static const uint16_t g_port_reconfig_request = 21901; +static const uint16_t g_port_reconfig_response = 41007; +static const uint16_t g_port_log = 41009; +static const uint16_t g_port_imu = 41010; + enum class AevaStreamType : uint16_t { kSphericalPointCloud = 0, kHealth = 1, 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 1245e6bff..3a3f2fedd 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 @@ -16,6 +16,8 @@ #include "nebula_hw_interfaces/nebula_hw_interfaces_aeva/aeva_hw_interface.hpp" +#include "nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/aeva_api.hpp" + namespace nebula::drivers { @@ -94,25 +96,29 @@ 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::make_shared(config.sensor_ip, connections::aeva::g_port_reconfig_response), + std::make_shared(config.sensor_ip, connections::aeva::g_port_reconfig_request), + logger->child("ReconfigApi")); } std::shared_ptr AevaHwInterface::make_pointcloud_api( const aeva::Aeries2Config & config) { - return std::make_shared(std::make_shared(config.sensor_ip, 41000)); + return std::make_shared( + std::make_shared(config.sensor_ip, connections::aeva::g_port_spherical_point_cloud)); } std::shared_ptr AevaHwInterface::make_health_api(const aeva::Aeries2Config & config) { - return std::make_shared(std::make_shared(config.sensor_ip, 41001)); + return std::make_shared( + std::make_shared(config.sensor_ip, connections::aeva::g_port_health)); } std::shared_ptr AevaHwInterface::make_telemetry_api( const aeva::Aeries2Config & config) { - return std::make_shared(std::make_shared(config.sensor_ip, 41003)); + return std::make_shared( + std::make_shared(config.sensor_ip, connections::aeva::g_port_telemetry)); } } // namespace nebula::drivers From 4fcd9ee93bbd1c10ceeb18395a49be8628195ef6 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Mon, 18 Nov 2024 17:47:54 +0900 Subject: [PATCH 49/57] chore(aeva): remove pointer hackery for cleaner code Signed-off-by: Max SCHMELLER --- .../nebula_hw_interfaces_aeva/connections/health.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 9e11f74ce..1b8f1e1d1 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 @@ -58,8 +58,8 @@ class HealthParser : public AevaParser entries.reserve(n_entries); for (size_t i = 0; i < n_entries; ++i) { - auto pointer = &*payload_bytes.consume_unsafe(sizeof(uint32_t)).cbegin(); - auto entry = boost::endian::load_little_u32(pointer); + auto health_code_raw = payload_bytes.consume_unsafe(sizeof(uint32_t)).cbegin(); + auto entry = boost::endian::load_little_u32(health_code_raw.base()); entries.emplace_back(entry); } From 2b2fe38fad8380bdd283dc17df97ddcb410d9f31 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Mon, 18 Nov 2024 18:04:26 +0900 Subject: [PATCH 50/57] chore(nebula_common): add version requirement for nlohmann_json Signed-off-by: Max SCHMELLER --- nebula_common/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nebula_common/CMakeLists.txt b/nebula_common/CMakeLists.txt index 9a17fb217..e3ce0c3bf 100644 --- a/nebula_common/CMakeLists.txt +++ b/nebula_common/CMakeLists.txt @@ -4,7 +4,7 @@ project(nebula_common) find_package(ament_cmake_auto REQUIRED) find_package(PCL REQUIRED COMPONENTS common) find_package(yaml-cpp REQUIRED) -find_package(nlohmann_json REQUIRED) +find_package(nlohmann_json 3.10.5 REQUIRED) # Default to C++17 if (NOT CMAKE_CXX_STANDARD) From f039c3677aaf8de079dbc631b12f86ca7b33874e Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Mon, 18 Nov 2024 18:07:44 +0900 Subject: [PATCH 51/57] chore(aeva): use arg with choices instead of textual descriptions for sensor_model in launch file Signed-off-by: Max SCHMELLER --- nebula_ros/launch/aeva_launch_all_hw.xml | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/nebula_ros/launch/aeva_launch_all_hw.xml b/nebula_ros/launch/aeva_launch_all_hw.xml index 20fcbbb7f..9208be917 100644 --- a/nebula_ros/launch/aeva_launch_all_hw.xml +++ b/nebula_ros/launch/aeva_launch_all_hw.xml @@ -1,10 +1,14 @@ - - + + + + + - + From 8139c6d0b7b2820ac7e05c3b0ff8d4b96bd17f26 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Mon, 18 Nov 2024 18:09:06 +0900 Subject: [PATCH 52/57] feat(nebula_launch): add Aeva sensors to the generic launch file Signed-off-by: Max SCHMELLER --- nebula_ros/launch/nebula_launch.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/nebula_ros/launch/nebula_launch.py b/nebula_ros/launch/nebula_launch.py index ca0eaaacb..51f024783 100644 --- a/nebula_ros/launch/nebula_launch.py +++ b/nebula_ros/launch/nebula_launch.py @@ -35,12 +35,14 @@ ] SENSOR_MODELS_ROBOSENSE = ["Bpearl", "Helios"] SENSOR_MODELS_CONTINENTAL = ["ARS548", "SRR520"] +SENSOR_MODELS_AEVA = ["Aeries2"] SENSOR_MODELS = ( SENSOR_MODELS_VELODYNE + SENSOR_MODELS_HESAI + SENSOR_MODELS_ROBOSENSE + SENSOR_MODELS_CONTINENTAL + + SENSOR_MODELS_AEVA ) @@ -58,6 +60,8 @@ def launch_setup(context: launch.LaunchContext, *args, **kwargs): vendor_launch_file = "robosense_launch_all_hw.xml" elif sensor_model in SENSOR_MODELS_CONTINENTAL: vendor_launch_file = "continental_launch_all_hw.xml" + elif sensor_model in SENSOR_MODELS_AEVA: + vendor_launch_file = "aeva_launch_all_hw.xml" else: raise KeyError(f"Sensor model {sensor_model} does not have an associated launch file") From 2dc01adff5967f298092945df8132e7a797e924f Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Mon, 16 Dec 2024 10:00:23 +0900 Subject: [PATCH 53/57] chore(hesai): remove spurious mt_queue import after Aeva rebase Signed-off-by: Max SCHMELLER --- nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp index f05b47cc5..8bf0100ff 100644 --- a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp @@ -22,7 +22,6 @@ #include "nebula_ros/hesai/hw_monitor_wrapper.hpp" #include -#include #include #include From a75e32f7ef7fabe2d47ab1562c8c51b7abacd5f0 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Mon, 16 Dec 2024 10:00:43 +0900 Subject: [PATCH 54/57] chore(aeva): remove pragma once in C file Signed-off-by: Max SCHMELLER --- .../src/nebula_aeva_hw_interfaces/aeva_hw_interface.cpp | 2 -- 1 file changed, 2 deletions(-) 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 3a3f2fedd..aa38833c7 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,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#pragma once - #include "nebula_hw_interfaces/nebula_hw_interfaces_aeva/aeva_hw_interface.hpp" #include "nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/aeva_api.hpp" From b248dee7f0d7112468a291e7f80a500a38e087c9 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Mon, 16 Dec 2024 10:12:39 +0900 Subject: [PATCH 55/57] test(aeva): fix wrongly compiledand thus skipped unit test Signed-off-by: Max SCHMELLER --- nebula_tests/aeva/CMakeLists.txt | 17 +----- nebula_tests/aeva/aeva_hw_interface_test.cpp | 47 ----------------- nebula_tests/aeva/aeva_hw_interface_test.hpp | 13 ----- nebula_tests/aeva/aeva_test_main.cpp | 55 +++++++++++++++----- 4 files changed, 45 insertions(+), 87 deletions(-) delete mode 100644 nebula_tests/aeva/aeva_hw_interface_test.cpp delete mode 100644 nebula_tests/aeva/aeva_hw_interface_test.hpp diff --git a/nebula_tests/aeva/CMakeLists.txt b/nebula_tests/aeva/CMakeLists.txt index 26d05fad6..f9e6bf38e 100644 --- a/nebula_tests/aeva/CMakeLists.txt +++ b/nebula_tests/aeva/CMakeLists.txt @@ -1,24 +1,11 @@ -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 -) + ${AEVA_TEST_LIBRARIES} +) \ No newline at end of file diff --git a/nebula_tests/aeva/aeva_hw_interface_test.cpp b/nebula_tests/aeva/aeva_hw_interface_test.cpp deleted file mode 100644 index c4d51d0cb..000000000 --- a/nebula_tests/aeva/aeva_hw_interface_test.cpp +++ /dev/null @@ -1,47 +0,0 @@ -// Copyright 2024 TIER IV, Inc. - -#include "aeva_hw_interface_test.hpp" - -#include "../common/mock_byte_stream.hpp" -#include "../data/aeva/tcp_stream.hpp" -#include "nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/pointcloud.hpp" - -#include -#include - -#include - -#include -#include -#include - -using nebula::drivers::aeva::PointCloudMessage; -using nebula::drivers::connections::aeva::PointcloudParser; - -TEST(TestParsing, Pointcloud) // NOLINT -{ - using std::chrono_literals::operator""ms; - - auto mock_byte_stream = std::make_shared(STREAM); - PointcloudParser parser(mock_byte_stream); - std::atomic_bool done = false; - - PointcloudParser::callback_t callback = [&](const PointCloudMessage & arg) { - if (done) return; - - EXPECT_EQ(arg.header.aeva_marker, 0xAE5Au); - EXPECT_EQ(arg.header.platform, 2); - - EXPECT_GT(arg.points.size(), 0); - EXPECT_TRUE(mock_byte_stream->done()); - EXPECT_EQ(mock_byte_stream->get_read_count(), 2); - done = true; - }; - - parser.register_callback(std::move(callback)); - - mock_byte_stream->run(); - while (!done) { - std::this_thread::yield(); - } -} diff --git a/nebula_tests/aeva/aeva_hw_interface_test.hpp b/nebula_tests/aeva/aeva_hw_interface_test.hpp deleted file mode 100644 index a31160f2e..000000000 --- a/nebula_tests/aeva/aeva_hw_interface_test.hpp +++ /dev/null @@ -1,13 +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. diff --git a/nebula_tests/aeva/aeva_test_main.cpp b/nebula_tests/aeva/aeva_test_main.cpp index 2ceaec2d4..1e54fa67f 100644 --- a/nebula_tests/aeva/aeva_test_main.cpp +++ b/nebula_tests/aeva/aeva_test_main.cpp @@ -1,19 +1,50 @@ // 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 "../common/mock_byte_stream.hpp" +#include "../data/aeva/tcp_stream.hpp" +#include "nebula_hw_interfaces/nebula_hw_interfaces_aeva/connections/pointcloud.hpp" + +#include +#include #include +#include +#include +#include + +using nebula::drivers::aeva::PointCloudMessage; +using nebula::drivers::connections::aeva::PointcloudParser; + +TEST(TestParsing, Pointcloud) // NOLINT +{ + using std::chrono_literals::operator""ms; + + auto mock_byte_stream = std::make_shared(STREAM); + PointcloudParser parser(mock_byte_stream); + std::atomic_bool done = false; + + PointcloudParser::callback_t callback = [&](const PointCloudMessage & arg) { + if (done) return; + + EXPECT_EQ(arg.header.aeva_marker, 0xAE5Au); + EXPECT_EQ(arg.header.platform, 2); + + EXPECT_GT(arg.points.size(), 0); + EXPECT_TRUE(mock_byte_stream->done()); + EXPECT_EQ(mock_byte_stream->get_read_count(), 2); + done = true; + }; + + parser.register_callback(std::move(callback)); + + mock_byte_stream->run(); + while (!done) { + std::this_thread::yield(); + } +} + + int main(int argc, char * argv[]) { ::testing::InitGoogleTest(&argc, argv); From 896943d437d36edffb53b4140c8b82872d0c5331 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 16 Dec 2024 01:13:31 +0000 Subject: [PATCH 56/57] ci(pre-commit): autofix --- nebula_tests/aeva/CMakeLists.txt | 2 +- nebula_tests/aeva/aeva_test_main.cpp | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/nebula_tests/aeva/CMakeLists.txt b/nebula_tests/aeva/CMakeLists.txt index f9e6bf38e..4bfe6626a 100644 --- a/nebula_tests/aeva/CMakeLists.txt +++ b/nebula_tests/aeva/CMakeLists.txt @@ -8,4 +8,4 @@ target_include_directories(aeva_test_main PUBLIC target_link_libraries(aeva_test_main ${AEVA_TEST_LIBRARIES} -) \ No newline at end of file +) diff --git a/nebula_tests/aeva/aeva_test_main.cpp b/nebula_tests/aeva/aeva_test_main.cpp index 1e54fa67f..ef502bd1f 100644 --- a/nebula_tests/aeva/aeva_test_main.cpp +++ b/nebula_tests/aeva/aeva_test_main.cpp @@ -44,7 +44,6 @@ TEST(TestParsing, Pointcloud) // NOLINT } } - int main(int argc, char * argv[]) { ::testing::InitGoogleTest(&argc, argv); From 0c16d01788c26c50c3de74f1b12dc1f1bd87268f Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Mon, 16 Dec 2024 10:16:18 +0900 Subject: [PATCH 57/57] fix(aeva): remove JSON schemarequirement of now-non-existent parameter Signed-off-by: Max SCHMELLER --- nebula_ros/schema/Aeries2.schema.json | 1 - 1 file changed, 1 deletion(-) diff --git a/nebula_ros/schema/Aeries2.schema.json b/nebula_ros/schema/Aeries2.schema.json index 659254724..ad643033a 100644 --- a/nebula_ros/schema/Aeries2.schema.json +++ b/nebula_ros/schema/Aeries2.schema.json @@ -233,7 +233,6 @@ "scanner.dithering_enable_ego_speed", "scanner.dithering_pattern_option", "scanner.ele_offset_rad", - "scanner.elevation_auto_adjustment", "scanner.enable_frame_dithering", "scanner.enable_frame_sync", "scanner.flip_pattern_vertically",