From ca6112ba8783ff0220ea1c3af9e95d04ece634ed Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Tue, 26 Nov 2024 19:09:05 +0900 Subject: [PATCH] temp: includes #231, #232, #233, #234, #235 Signed-off-by: Max SCHMELLER --- .cspell.json | 1 + .../nebula_common/loggers/console_logger.hpp | 14 +- .../include/nebula_common/loggers/logger.hpp | 9 +- nebula_hw_interfaces/CMakeLists.txt | 38 +- .../connections/udp.hpp | 383 ++++++++++++++++++ .../connections/tcp.hpp | 103 +++++ .../hesai_hw_interface.hpp | 58 +-- nebula_hw_interfaces/package.xml | 1 + .../hesai_hw_interface.cpp | 87 ++-- nebula_hw_interfaces/test/common/test_udp.cpp | 198 +++++++++ .../test/common/test_udp/utils.hpp | 78 ++++ nebula_hw_interfaces/test/hesai/test_ptc.cpp | 115 ++++++ .../test/hesai/test_ptc/ptc_test.hpp | 50 +++ .../test/hesai/test_ptc/tcp_mock.hpp | 47 +++ .../nebula_ros/hesai/hesai_ros_wrapper.hpp | 8 +- .../nebula_ros/hesai/hw_interface_wrapper.hpp | 1 + nebula_ros/src/hesai/hesai_ros_wrapper.cpp | 19 +- nebula_ros/src/hesai/hw_interface_wrapper.cpp | 16 +- 18 files changed, 1110 insertions(+), 116 deletions(-) create mode 100644 nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/connections/udp.hpp create mode 100644 nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/connections/tcp.hpp create mode 100644 nebula_hw_interfaces/test/common/test_udp.cpp create mode 100644 nebula_hw_interfaces/test/common/test_udp/utils.hpp create mode 100644 nebula_hw_interfaces/test/hesai/test_ptc.cpp create mode 100644 nebula_hw_interfaces/test/hesai/test_ptc/ptc_test.hpp create mode 100644 nebula_hw_interfaces/test/hesai/test_ptc/tcp_mock.hpp diff --git a/.cspell.json b/.cspell.json index 77bf328b3..879dcb363 100644 --- a/.cspell.json +++ b/.cspell.json @@ -33,6 +33,7 @@ "nproc", "nsec", "ntoa", + "OVFL", "pandar", "PANDAR", "PANDARAT", diff --git a/nebula_common/include/nebula_common/loggers/console_logger.hpp b/nebula_common/include/nebula_common/loggers/console_logger.hpp index 45d0ee465..fb2a61276 100644 --- a/nebula_common/include/nebula_common/loggers/console_logger.hpp +++ b/nebula_common/include/nebula_common/loggers/console_logger.hpp @@ -18,10 +18,8 @@ #include #include -#include #include #include -#include #include #include @@ -33,10 +31,10 @@ class ConsoleLogger : public Logger public: explicit ConsoleLogger(std::string name) : name_(std::move(name)) {} - void debug(const std::string & message) override { printTagged(std::cout, "DEBUG", message); } - void info(const std::string & message) override { printTagged(std::cout, "INFO", message); } - void warn(const std::string & message) override { printTagged(std::cerr, "WARN", message); } - void error(const std::string & message) override { printTagged(std::cerr, "ERROR", message); } + void debug(const std::string & message) override { print_tagged(std::cout, "DEBUG", message); } + void info(const std::string & message) override { print_tagged(std::cout, "INFO", message); } + void warn(const std::string & message) override { print_tagged(std::cerr, "WARN", message); } + void error(const std::string & message) override { print_tagged(std::cerr, "ERROR", message); } std::shared_ptr child(const std::string & name) override { @@ -44,9 +42,9 @@ class ConsoleLogger : public Logger } private: - const std::string name_; + std::string name_; - void printTagged(std::ostream & os, const std::string & severity, const std::string & message) + void print_tagged(std::ostream & os, const std::string & severity, const std::string & message) { // In multithreaded logging, building the string first (... + ...) and then shifting to the // stream will ensure that no other logger outputs between string fragments diff --git a/nebula_common/include/nebula_common/loggers/logger.hpp b/nebula_common/include/nebula_common/loggers/logger.hpp index 3807994a3..b416e2523 100644 --- a/nebula_common/include/nebula_common/loggers/logger.hpp +++ b/nebula_common/include/nebula_common/loggers/logger.hpp @@ -18,10 +18,11 @@ #include #include -#define NEBULA_LOG_STREAM(log_func, stream_args) \ - { \ - auto msg = (std::stringstream{} << stream_args).str(); \ - log_func(msg); \ +#define NEBULA_LOG_STREAM(log_func, stream_args) \ + { \ + std::stringstream ss{}; \ + ss << stream_args; \ + log_func(ss.str()); \ } namespace nebula::drivers::loggers diff --git a/nebula_hw_interfaces/CMakeLists.txt b/nebula_hw_interfaces/CMakeLists.txt index 59bb9cd8a..8d65de8f1 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} @@ -113,6 +111,34 @@ install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_cmake_gmock REQUIRED) + + ament_add_gtest(test_udp + test/common/test_udp.cpp + ) + + target_include_directories(test_udp PUBLIC + ${nebula_common_INCLUDE_DIRS} + include + test) + + ament_add_gmock(hesai_test_ptc + test/hesai/test_ptc.cpp + ) + + target_include_directories(hesai_test_ptc PUBLIC + ${nebula_common_INCLUDE_DIRS} + ${nebula_hw_interfaces_hesai_INCLUDE_DIRS} + ${boost_tcp_driver_INCLUDE_DIRS} + ${boost_udp_driver_INCLUDE_DIRS} + include + test) + + target_link_libraries(hesai_test_ptc + nebula_hw_interfaces_hesai + ) endif() ament_export_include_directories("include/${PROJECT_NAME}") diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/connections/udp.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/connections/udp.hpp new file mode 100644 index 000000000..23cb4b928 --- /dev/null +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/connections/udp.hpp @@ -0,0 +1,383 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace nebula::drivers::connections +{ + +class SocketError : public std::exception +{ +public: + explicit SocketError(int err_no) : what_(strerror(err_no)) {} + + explicit SocketError(const char * msg) : what_(msg) {} + + const char * what() const noexcept override { return what_.c_str(); } + +private: + std::string what_; +}; + +class UsageError : public std::runtime_error +{ +public: + explicit UsageError(const std::string & msg) : std::runtime_error(msg) {} +}; + +class UdpSocket +{ +private: + struct Endpoint + { + std::string ip; + uint16_t port; + }; + + struct MsgBuffers + { + msghdr msg{}; + iovec iov{}; + std::array control; + sockaddr_in sender_addr; + }; + + class DropMonitor + { + uint32_t last_drop_counter_{0}; + + public: + uint32_t get_drops_since_last_receive(uint32_t current_drop_counter) + { + uint32_t last = last_drop_counter_; + last_drop_counter_ = current_drop_counter; + + bool counter_did_wrap = current_drop_counter < last; + if (counter_did_wrap) { + return (UINT32_MAX - last) + current_drop_counter; + } + + return current_drop_counter - last; + } + }; + + enum class State { UNINITIALIZED, INITIALIZED, BOUND, ACTIVE }; + + static const int g_poll_timeout_ms = 10; + +public: + struct ReceiveMetadata + { + std::optional timestamp_ns; + uint64_t drops_since_last_receive{0}; + bool truncated; + }; + + using callback_t = std::function &, const ReceiveMetadata &)>; + + /** + * @brief Construct a UDP socket with timestamp measuring enabled. The minimal way to start + * receiving on the socket is `UdpSocket().init(...).bind().subscribe(...);`. + */ + UdpSocket() + { + int sock_fd = socket(AF_INET, SOCK_DGRAM, 0); + if (sock_fd == -1) throw SocketError(errno); + + int enable = 1; + int result = setsockopt(sock_fd, SOL_SOCKET, SO_REUSEADDR, &enable, sizeof(enable)); + if (result < 0) throw SocketError(errno); + + // Enable kernel-space receive time measurement + result = setsockopt(sock_fd, SOL_SOCKET, SO_TIMESTAMP, &enable, sizeof(enable)); + if (result < 0) throw SocketError(errno); + + // Enable reporting on packets dropped due to full UDP receive buffer + result = setsockopt(sock_fd, SOL_SOCKET, SO_RXQ_OVFL, &enable, sizeof(enable)); + if (result < 0) throw SocketError(errno); + + poll_fd_ = {sock_fd, POLLIN, 0}; + sock_fd_ = sock_fd; + } + + /** + * @brief Specify the host address and port for this socket to be bound to. To bind the socket, + * call the `bind()` function. + * + * @param host_ip The address to bind to. + * @param host_port The port to bind to. + */ + UdpSocket & init(const std::string & host_ip, uint16_t host_port) + { + if (state_ > State::INITIALIZED) throw UsageError("Socket must be initialized before binding"); + + host_ = {host_ip, host_port}; + state_ = State::INITIALIZED; + return *this; + } + + /** + * @brief Set the socket to drop all packets not coming from `sender_ip` and `sender_port`. + * + * @param sender_ip The only allowed sender IP. Cannot be a multicast or broadcast address. + * @param sender_port The only allowed sender port. + */ + UdpSocket & limit_to_sender(const std::string & sender_ip, uint16_t sender_port) + { + if (state_ >= State::ACTIVE) throw UsageError("Sender has to be set before subscribing"); + + sender_.emplace(Endpoint{sender_ip, sender_port}); + return *this; + } + + /** + * @brief Set the MTU this socket supports. While this can be set arbitrarily, it is best set to + * the MTU of the network interface, or to the maximum expected packet length. + * + * @param bytes The MTU size. The default value is 1500. + */ + UdpSocket & set_mtu(size_t bytes) + { + if (state_ >= State::ACTIVE) throw UsageError("MTU size has to be set before subscribing"); + + buffer_size_ = bytes; + return *this; + } + + UdpSocket & set_socket_buffer_size(size_t bytes) + { + if (state_ > State::INITIALIZED) throw UsageError("Buffer size has to be set before binding"); + if (bytes > static_cast(INT32_MAX)) + throw UsageError("The maximum value supported (0x7FFFFFF) has been exceeded"); + + auto buf_size = static_cast(bytes); + int result = setsockopt(sock_fd_, SOL_SOCKET, SO_RCVBUF, &buf_size, sizeof(buf_size)); + if (result < 0) throw SocketError(errno); + return *this; + } + + /** + * @brief Join an IP multicast group. Only one group can be joined by the socket. + * + * @param group_ip The multicast IP. It has to be in the multicast range `224.0.0.0/4` (between + * `224.0.0.0` and `239.255.255.255`). + */ + UdpSocket & join_multicast_group(const std::string & group_ip) + { + if (state_ < State::INITIALIZED) throw UsageError("Socket has to be initialized first"); + if (state_ >= State::BOUND) + throw UsageError("Multicast groups have to be joined before binding"); + + ip_mreq mreq; + mreq.imr_multiaddr.s_addr = inet_addr(group_ip.c_str()); // Multicast group address + mreq.imr_interface.s_addr = inet_addr(host_.ip.c_str()); + + int result = setsockopt(sock_fd_, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq)); + if (result < 0) throw SocketError(errno); + + multicast_ip_.emplace(group_ip); + return *this; + } + + /** + * @brief Bind the socket to host IP and port given in `init()`. If `join_multicast_group()` was + * called before this function, the socket will be bound to `group_ip` instead. At least `init()` + * has to have been called before. + */ + UdpSocket & bind() + { + if (state_ < State::INITIALIZED) throw UsageError("Socket has to be initialized first"); + if (state_ >= State::BOUND) throw UsageError("Re-binding already bound socket"); + + sockaddr_in addr{}; + addr.sin_family = AF_INET; + addr.sin_port = htons(host_.port); + addr.sin_addr.s_addr = multicast_ip_ ? inet_addr(multicast_ip_->c_str()) : INADDR_ANY; + + int result = ::bind(sock_fd_, (struct sockaddr *)&addr, sizeof(addr)); + if (result == -1) throw SocketError(errno); + + state_ = State::BOUND; + return *this; + } + + /** + * @brief Register a callback for processing received packets and start the receiver thread. The + * callback will be called for each received packet, and will be executed in the receive thread. + * Has to be called on a bound socket (`bind()` has to have been called before). + * + * @param callback The function to be executed for each received packet. + */ + UdpSocket & subscribe(callback_t && callback) + { + if (state_ < State::BOUND) throw UsageError("Socket has to be bound first"); + if (state_ > State::BOUND) throw UsageError("Cannot re-subscribe to socket"); + + callback_ = std::move(callback); + launch_receiver(); + return *this; + } + + /** + * @brief Gracefully stops the active receiver thread (if any) but keeps the socket alive. The + * same socket can later be subscribed again. + */ + UdpSocket & unsubscribe() + { + if (state_ == State::ACTIVE) state_ = State::BOUND; + if (receive_thread_.joinable()) receive_thread_.join(); + return *this; + } + + ~UdpSocket() + { + unsubscribe(); + close(sock_fd_); + } + +private: + void launch_receiver() + { + assert(state_ == State::BOUND); + assert(callback_); + + state_ = State::ACTIVE; + receive_thread_ = std::thread([this]() { + std::vector buffer; + while (state_ == State::ACTIVE) { + auto data_available = is_data_available(); + if (!data_available.has_value()) throw SocketError(data_available.error()); + if (!data_available.value()) continue; + + buffer.resize(buffer_size_); + auto msg_header = make_msg_header(buffer); + + ssize_t recv_result = recvmsg(sock_fd_, &msg_header.msg, MSG_TRUNC); + if (recv_result < 0) throw SocketError(errno); + size_t untruncated_packet_length = recv_result; + + if (!is_accepted_sender(msg_header.sender_addr)) continue; + + ReceiveMetadata metadata; + get_receive_metadata(msg_header.msg, metadata); + metadata.truncated = untruncated_packet_length > buffer_size_; + + buffer.resize(std::min(buffer_size_, untruncated_packet_length)); + callback_(buffer, metadata); + } + }); + } + + void get_receive_metadata(msghdr & msg, ReceiveMetadata & inout_metadata) + { + for (cmsghdr * cmsg = CMSG_FIRSTHDR(&msg); cmsg != nullptr; cmsg = CMSG_NXTHDR(&msg, cmsg)) { + if (cmsg->cmsg_level != SOL_SOCKET) continue; + + switch (cmsg->cmsg_type) { + case SO_TIMESTAMP: { + auto tv = (timeval const *)CMSG_DATA(cmsg); + uint64_t timestamp_ns = tv->tv_sec * 1'000'000'000 + tv->tv_usec * 1000; + inout_metadata.timestamp_ns.emplace(timestamp_ns); + break; + } + case SO_RXQ_OVFL: { + auto drops = (uint32_t const *)CMSG_DATA(cmsg); + inout_metadata.drops_since_last_receive = + drop_monitor_.get_drops_since_last_receive(*drops); + break; + } + default: + continue; + } + } + } + + util::expected is_data_available() + { + int status = poll(&poll_fd_, 1, g_poll_timeout_ms); + if (status < 0) return errno; + return (poll_fd_.revents & POLLIN) && (status > 0); + } + + bool is_accepted_sender(const sockaddr_in & sender_addr) + { + if (!sender_) return true; + + std::array sender_name; + inet_ntop(AF_INET, &sender_addr.sin_addr, sender_name.data(), INET_ADDRSTRLEN); + return std::strncmp(sender_->ip.c_str(), sender_name.data(), INET_ADDRSTRLEN) == 0; + } + + MsgBuffers make_msg_header(std::vector & receive_buffer) const + { + msghdr msg{}; + iovec iov{}; + std::array control; + + sockaddr_in sender_addr; + socklen_t sender_addr_len = sizeof(sender_addr); + + iov.iov_base = receive_buffer.data(); + iov.iov_len = receive_buffer.size(); + + msg.msg_iov = &iov; + msg.msg_iovlen = 1; + msg.msg_control = control.data(); + msg.msg_controllen = control.size(); + msg.msg_name = &sender_addr; + msg.msg_namelen = sender_addr_len; + + return MsgBuffers{msg, iov, control, sender_addr}; + } + + std::atomic state_{State::UNINITIALIZED}; + + int sock_fd_; + pollfd poll_fd_; + + size_t buffer_size_ = 1500; + Endpoint host_; + std::optional multicast_ip_; + std::optional sender_; + std::thread receive_thread_; + callback_t callback_; + + DropMonitor drop_monitor_; +}; + +} // namespace nebula::drivers::connections diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/connections/tcp.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/connections/tcp.hpp new file mode 100644 index 000000000..aa5f45986 --- /dev/null +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/connections/tcp.hpp @@ -0,0 +1,103 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include + +#include + +#include +#include +#include +#include +#include + +namespace nebula::drivers::connections +{ + +class TcpError : public std::runtime_error +{ +public: + explicit TcpError(const std::string & msg) : std::runtime_error(msg) {} +}; + +class AbstractTcpSocket +{ +public: + using header_callback_t = std::function &)>; + using payload_callback_t = std::function &)>; + using completion_callback_t = std::function; + + virtual ~AbstractTcpSocket() = default; + + virtual void init( + const std::string & host_ip, uint16_t host_port, const std::string & remote_ip, + uint16_t remote_port) = 0; + + virtual void bind() = 0; + + virtual void close() = 0; + + virtual void async_ptc_request( + std::vector & ptc_packet, header_callback_t cb_header, payload_callback_t cb_payload, + completion_callback_t cb_completion) = 0; +}; + +class TcpSocket : public AbstractTcpSocket +{ +public: + using callback_t = std::function &)>; + + void init( + const std::string & host_ip, uint16_t host_port, const std::string & remote_ip, + uint16_t remote_port) override + { + tcp_driver_.init_socket(remote_ip, remote_port, host_ip, host_port); + } + + void bind() override + { + if (!tcp_driver_.isOpen() && !tcp_driver_.open()) { + throw TcpError("could not open TCP socket for an unknown reason"); + } + } + + void close() override + { + if (tcp_driver_.isOpen()) { + tcp_driver_.close(); + } + } + + void async_ptc_request( + std::vector & ptc_packet, header_callback_t cb_header, payload_callback_t cb_payload, + completion_callback_t cb_completion) override + { + if (tcp_driver_.GetIOContext()->stopped()) { + tcp_driver_.GetIOContext()->restart(); + } + bool success = + tcp_driver_.asyncSendReceiveHeaderPayload(ptc_packet, cb_header, cb_payload, cb_completion); + if (!success) { + throw TcpError("sending the PTC command failed for an unknown reason"); + } + tcp_driver_.GetIOContext()->run(); + } + +private: + ::drivers::tcp_driver::TcpDriver tcp_driver_{std::make_shared(1)}; +}; + +} // namespace nebula::drivers::connections diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp index 78174df6d..162ce7d1e 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp @@ -17,7 +17,10 @@ // Have to define macros to silence warnings about deprecated headers being used by // boost/property_tree/ in some versions of boost. // See: https://github.com/boostorg/property_tree/issues/51 -#include "nebula_common/nebula_status.hpp" +#include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/connections/tcp.hpp" +#include "nebula_hw_interfaces/nebula_hw_interfaces_common/connections/udp.hpp" + +#include #include @@ -28,15 +31,13 @@ #if (BOOST_VERSION / 100 == 1074) // Boost 1.74 #define BOOST_ALLOW_DEPRECATED_HEADERS #endif -#include "boost_tcp_driver/http_client_driver.hpp" -#include "boost_tcp_driver/tcp_driver.hpp" -#include "boost_udp_driver/udp_driver.hpp" -#include "nebula_common/hesai/hesai_common.hpp" -#include "nebula_common/hesai/hesai_status.hpp" -#include "nebula_common/util/expected.hpp" #include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp" -#include +#include +#include +#include +#include +#include #include #include @@ -138,12 +139,11 @@ class HesaiHwInterface using ptc_cmd_result_t = nebula::util::expected, ptc_error_t>; - std::unique_ptr<::drivers::common::IoContext> cloud_io_context_; - std::shared_ptr m_owned_ctx; - std::unique_ptr<::drivers::udp_driver::UdpDriver> cloud_udp_driver_; - std::shared_ptr<::drivers::tcp_driver::TcpDriver> tcp_driver_; + std::shared_ptr logger_; + connections::UdpSocket udp_socket_{}; + std::shared_ptr tcp_socket_; std::shared_ptr sensor_configuration_; - std::function & buffer)> + std::function & buffer)> cloud_packet_callback_; /**This function pointer is called when the scan is complete*/ std::mutex mtx_inflight_tcp_request_; @@ -167,20 +167,6 @@ class HesaiHwInterface /// @param str Received string void str_cb(const std::string & str); - std::shared_ptr parent_node_logger; - /// @brief Printing the string to RCLCPP_INFO_STREAM - /// @param info Target string - void PrintInfo(std::string info); - /// @brief Printing the string to RCLCPP_ERROR_STREAM - /// @param error Target string - void PrintError(std::string error); - /// @brief Printing the string to RCLCPP_DEBUG_STREAM - /// @param debug Target string - void PrintDebug(std::string debug); - /// @brief Printing the bytes to RCLCPP_DEBUG_STREAM - /// @param bytes Target byte vector - void PrintDebug(const std::vector & bytes); - /// @brief Convert an error code to a human-readable string /// @param error_code The error code, containing the sensor's error code (if any), along with /// flags such as TCP_ERROR_UNRELATED_RESPONSE etc. @@ -204,7 +190,9 @@ class HesaiHwInterface public: /// @brief Constructor - HesaiHwInterface(); + HesaiHwInterface( + std::shared_ptr logger, + std::shared_ptr tcp_socket); /// @brief Destructor ~HesaiHwInterface(); /// @brief Initializing tcp_driver for TCP communication @@ -221,7 +209,7 @@ class HesaiHwInterface /// @brief Callback function to receive the Cloud Packet data from the UDP Driver /// @param buffer Buffer containing the data received from the UDP socket - void ReceiveSensorPacketCallback(std::vector & buffer); + void ReceiveSensorPacketCallback(const std::vector & buffer); /// @brief Starting the interface that handles UDP streams /// @return Resulting status Status SensorInterfaceStart(); @@ -244,7 +232,7 @@ class HesaiHwInterface /// @brief Registering callback for PandarScan /// @param scan_callback Callback function /// @return Resulting status - Status RegisterScanCallback(std::function &)> scan_callback); + Status RegisterScanCallback(std::function &)> scan_callback); /// @brief Getting data with PTC_COMMAND_GET_LIDAR_CALIBRATION /// @return Resulting status std::string GetLidarCalibrationString(); @@ -377,12 +365,6 @@ class HesaiHwInterface /// @return Resulting status HesaiLidarMonitor GetLidarMonitor(); - /// @brief Call run() of IO Context - void IOContextRun(); - /// @brief GetIO Context - /// @return IO Context - std::shared_ptr GetIOContext(); - /// @brief Setting spin_speed via HTTP API /// @param ctx IO Context used /// @param rpm spin_speed (300, 600, 1200) @@ -462,10 +444,6 @@ class HesaiHwInterface /// @brief Whether to use HTTP for getting LidarMonitor /// @return Use HTTP bool UseHttpGetLidarMonitor(); - - /// @brief Setting rclcpp::Logger - /// @param node Logger - void SetLogger(std::shared_ptr node); }; } // namespace drivers } // namespace nebula diff --git a/nebula_hw_interfaces/package.xml b/nebula_hw_interfaces/package.xml index fed686b8a..407a54ed0 100644 --- a/nebula_hw_interfaces/package.xml +++ b/nebula_hw_interfaces/package.xml @@ -20,6 +20,7 @@ ros2_socketcan velodyne_msgs + ament_cmake_gmock ament_cmake_gtest ament_lint_auto diff --git a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp index 436023aef..a3dc3d277 100644 --- a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp @@ -6,6 +6,7 @@ #include "nebula_common/hesai/hesai_status.hpp" #include "nebula_common/nebula_common.hpp" #include "nebula_common/nebula_status.hpp" +#include "nebula_hw_interfaces/nebula_hw_interfaces_common/connections/udp.hpp" #include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp" #include @@ -14,6 +15,7 @@ #include #include +#include #include #include #include @@ -36,11 +38,12 @@ namespace nebula::drivers using std::string_literals::operator""s; using nlohmann::json; -HesaiHwInterface::HesaiHwInterface() -: cloud_io_context_{new ::drivers::common::IoContext(1)}, - m_owned_ctx{new boost::asio::io_context(1)}, - cloud_udp_driver_{new ::drivers::udp_driver::UdpDriver(*cloud_io_context_)}, - tcp_driver_{new ::drivers::tcp_driver::TcpDriver(m_owned_ctx)} +HesaiHwInterface::HesaiHwInterface( + std::shared_ptr logger, + std::shared_ptr tcp_socket) +: logger_(std::move(logger)), + tcp_socket_{std::move(tcp_socket)}, + target_model_no(NebulaModelToHesaiModelNo(SensorModel::UNKNOWN)) { } @@ -156,49 +159,28 @@ Status HesaiHwInterface::SetSensorConfiguration( Status HesaiHwInterface::SensorInterfaceStart() { - try { - PrintInfo("Starting UDP receiver"); - if (sensor_configuration_->multicast_ip.empty()) { - cloud_udp_driver_->init_receiver( - sensor_configuration_->host_ip, sensor_configuration_->data_port); - } else { - cloud_udp_driver_->init_receiver( - sensor_configuration_->multicast_ip, sensor_configuration_->data_port, - sensor_configuration_->host_ip, sensor_configuration_->data_port); - cloud_udp_driver_->receiver()->setMulticast(true); - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - PrintError("init ok"); -#endif - cloud_udp_driver_->receiver()->open(); -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - PrintError("open ok"); -#endif - - bool success = cloud_udp_driver_->receiver()->setKernelBufferSize(UDP_SOCKET_BUFFER_SIZE); - if (!success) { - PrintError( - "Could not set receive buffer size. Try increasing net.core.rmem_max to " + - std::to_string(UDP_SOCKET_BUFFER_SIZE) + " B."); - return Status::ERROR_1; - } + udp_socket_.init(sensor_configuration_->host_ip, sensor_configuration_->data_port); + if (!sensor_configuration_->multicast_ip.empty()) { + udp_socket_.join_multicast_group(sensor_configuration_->multicast_ip); + } - cloud_udp_driver_->receiver()->bind(); -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - PrintError("bind ok"); -#endif + udp_socket_.set_mtu(MTU_SIZE); - cloud_udp_driver_->receiver()->asyncReceive( - std::bind(&HesaiHwInterface::ReceiveSensorPacketCallback, this, std::placeholders::_1)); -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - PrintError("async receive set"); -#endif - } catch (const std::exception & ex) { - Status status = Status::UDP_CONNECTION_ERROR; - std::cerr << status << " for " << sensor_configuration_->sensor_ip << ":" - << sensor_configuration_->data_port << " - " << ex.what() << std::endl; - return status; + try { + udp_socket_.set_socket_buffer_size(UDP_SOCKET_BUFFER_SIZE); + } catch (const connections::SocketError & e) { + throw std::runtime_error( + "Could not set socket receive buffer size to " + std::to_string(UDP_SOCKET_BUFFER_SIZE) + + ". Try increasing net.core.rmem_max."); } + + udp_socket_.bind().subscribe([&]( + const std::vector & packet, + const connections::UdpSocket::ReceiveMetadata & metadata) { + if (metadata.drops_since_last_receive) std::cout << metadata.drops_since_last_receive << '\n'; + ReceiveSensorPacketCallback(packet); + }); + return Status::OK; } @@ -741,6 +723,7 @@ std::pair HesaiHwInterface::unwrap_http_response( } return {Status::ERROR_1, message}; + logger_->info(str); } std::pair HesaiHwInterface::unwrap_http_response( @@ -1088,7 +1071,7 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( logger_->info( "current lidar range.start: " + std::to_string(static_cast(current_cloud_min_angle_ddeg.value()))); - PrintInfo( + logger_->info( "current configuration cloud_min_angle: " + std::to_string(sensor_configuration->cloud_min_angle)); } @@ -1101,7 +1084,7 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( logger_->info( "current lidar range.end: " + std::to_string(static_cast(current_cloud_max_angle_ddeg.value()))); - PrintInfo( + logger_->info( "current configuration cloud_max_angle: " + std::to_string(sensor_configuration->cloud_max_angle)); } @@ -1376,9 +1359,13 @@ T HesaiHwInterface::CheckSizeAndParse(const std::vector & data) } if (data.size() > sizeof(T)) { - RCLCPP_WARN_ONCE( - *parent_node_logger, - "Sensor returned longer payload than expected. Truncating and parsing anyway."); + // TODO(mojomex): having a static variable for this is not optimal, but the loggers::Logger + // class does not support things like _ONCE macros yet + static bool already_warned_for_this_type = false; + if (!already_warned_for_this_type) { + logger_->warn("Sensor returned longer payload than expected. Truncating and parsing anyway."); + already_warned_for_this_type = true; + } } T parsed; diff --git a/nebula_hw_interfaces/test/common/test_udp.cpp b/nebula_hw_interfaces/test/common/test_udp.cpp new file mode 100644 index 000000000..674077243 --- /dev/null +++ b/nebula_hw_interfaces/test/common/test_udp.cpp @@ -0,0 +1,198 @@ +// Copyright 2024 TIER IV, Inc. + +#include "common/test_udp/utils.hpp" +#include "nebula_common/util/expected.hpp" +#include "nebula_hw_interfaces/nebula_hw_interfaces_common/connections/udp.hpp" + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace nebula::drivers::connections +{ + +using std::chrono_literals::operator""ms; + +static const char localhost_ip[] = "127.0.0.1"; +static const char broadcast_ip[] = "255.255.255.255"; +static const char any_ip[] = "0.0.0.0"; +static const char multicast_group[] = "230.1.2.3"; + +static const char sender_ip[] = "192.168.201"; +static const uint16_t sender_port = 7373; +static const uint16_t host_port = 6262; + +static const std::chrono::duration send_receive_timeout = 100ms; + +UdpSocket::callback_t empty_cb() +{ + return [](const auto &, const auto &) {}; +} + +util::expected read_sys_param(const std::string & param_fqn) +{ + std::string path = "/proc/sys/" + param_fqn; + std::replace(path.begin(), path.end(), '.', '/'); + std::ifstream ifs{path}; + if (!ifs) return "could not read " + param_fqn; + + size_t param{}; + if (!(ifs >> param)) return param_fqn + " has unrecognized format"; + return param; +} + +TEST(test_udp, test_basic_lifecycle) +{ + ASSERT_NO_THROW( + UdpSocket().init(localhost_ip, host_port).bind().subscribe(empty_cb()).unsubscribe()); +} + +TEST(test_udp, test_special_addresses_bind) +{ + ASSERT_NO_THROW(UdpSocket().init(broadcast_ip, host_port).bind()); + ASSERT_NO_THROW(UdpSocket().init(any_ip, host_port).bind()); +} + +TEST(test_udp, test_wildcard_multicast_join_error) +{ + ASSERT_THROW( + UdpSocket().init(broadcast_ip, host_port).join_multicast_group(multicast_group).bind(), + SocketError); +} + +TEST(test_udp, test_buffer_resize) +{ + auto rmem_max_maybe = read_sys_param("net.core.rmem_max"); + if (!rmem_max_maybe.has_value()) GTEST_SKIP() << rmem_max_maybe.error(); + size_t rmem_max = rmem_max_maybe.value(); + + // Setting buffer sizes up to and including rmem_max shall succeed + ASSERT_NO_THROW( + UdpSocket().init(localhost_ip, host_port).set_socket_buffer_size(rmem_max).bind()); + + // Linux only supports sizes up to INT32_MAX + ASSERT_THROW( + UdpSocket() + .init(localhost_ip, host_port) + .set_socket_buffer_size(static_cast(INT32_MAX) + 1) + .bind(), + UsageError); +} + +TEST(test_udp, test_correct_usage_is_enforced) +{ + // These functions require other functions (e.g. `init()`) to be called beforehand + ASSERT_THROW(UdpSocket().bind(), UsageError); + ASSERT_THROW(UdpSocket().join_multicast_group(multicast_group), UsageError); + ASSERT_THROW(UdpSocket().subscribe(empty_cb()), UsageError); + + // The following functions can be called in any order, any number of times + ASSERT_NO_THROW(UdpSocket().limit_to_sender(sender_ip, sender_port)); + ASSERT_NO_THROW(UdpSocket().init(localhost_ip, host_port)); + ASSERT_NO_THROW(UdpSocket() + .limit_to_sender(sender_ip, sender_port) + .init(localhost_ip, host_port) + .limit_to_sender(sender_ip, sender_port) + .init(localhost_ip, host_port) + .bind()); + + // Sockets cannot be re-bound + ASSERT_THROW(UdpSocket().init(localhost_ip, host_port).bind().bind(), UsageError); + + ASSERT_THROW( + UdpSocket().init(localhost_ip, host_port).bind().init(localhost_ip, host_port), UsageError); + ASSERT_NO_THROW( + UdpSocket().init(localhost_ip, host_port).bind().limit_to_sender(sender_ip, sender_port)); + + // Only bound sockets can be subscribed + ASSERT_THROW(UdpSocket().init(localhost_ip, host_port).subscribe(empty_cb()), UsageError); + ASSERT_NO_THROW(UdpSocket().init(localhost_ip, host_port).bind().subscribe(empty_cb())); + + // Only one callback can exist + ASSERT_THROW( + UdpSocket().init(localhost_ip, host_port).bind().subscribe(empty_cb()).subscribe(empty_cb()), + UsageError); + + // But un- and re-subscribing shall be supported + ASSERT_NO_THROW(UdpSocket() + .init(localhost_ip, host_port) + .bind() + .subscribe(empty_cb()) + .unsubscribe() + .subscribe(empty_cb())); + + // Unsubscribing on a non-subscribed socket shall also be supported + ASSERT_NO_THROW(UdpSocket().unsubscribe()); +} + +TEST(test_udp, test_receiving) +{ + const std::vector payload{1, 2, 3}; + UdpSocket sock{}; + sock.init(localhost_ip, host_port).bind(); + + auto err_no_opt = udp_send(localhost_ip, host_port, payload); + if (err_no_opt.has_value()) GTEST_SKIP() << strerror(err_no_opt.value()); + + auto result_opt = receive_once(sock, send_receive_timeout); + + ASSERT_TRUE(result_opt.has_value()); + auto const & [recv_payload, metadata] = result_opt.value(); + ASSERT_EQ(recv_payload, payload); + ASSERT_FALSE(metadata.truncated); + ASSERT_EQ(metadata.drops_since_last_receive, 0); + + // TODO(mojomex): currently cannot test timestamping on loopback interface (no timestamp produced) +} + +TEST(test_udp, test_receiving_oversized) +{ + const size_t mtu = 1500; + std::vector payload; + payload.resize(mtu + 1, 0x42); + UdpSocket sock{}; + sock.init(localhost_ip, host_port).set_mtu(mtu).bind(); + + auto err_no_opt = udp_send(localhost_ip, host_port, payload); + if (err_no_opt.has_value()) GTEST_SKIP() << strerror(err_no_opt.value()); + + auto result_opt = receive_once(sock, send_receive_timeout); + + ASSERT_TRUE(result_opt.has_value()); + auto const & [recv_payload, metadata] = result_opt.value(); + ASSERT_EQ(recv_payload.size(), mtu); + ASSERT_TRUE(std::equal(recv_payload.begin(), recv_payload.end(), payload.begin())); + ASSERT_TRUE(metadata.truncated); + ASSERT_EQ(metadata.drops_since_last_receive, 0); +} + +TEST(test_udp, test_filtering_sender) +{ + std::vector payload{1, 2, 3}; + UdpSocket sock{}; + sock.init(localhost_ip, host_port).bind().limit_to_sender(sender_ip, sender_port); + + auto err_no_opt = udp_send(localhost_ip, host_port, payload); + if (err_no_opt.has_value()) GTEST_SKIP() << strerror(err_no_opt.value()); + + auto result_opt = receive_once(sock, send_receive_timeout); + ASSERT_FALSE(result_opt.has_value()); +} + +} // namespace nebula::drivers::connections + +int main(int argc, char * argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}; diff --git a/nebula_hw_interfaces/test/common/test_udp/utils.hpp b/nebula_hw_interfaces/test/common/test_udp/utils.hpp new file mode 100644 index 000000000..dc9a08c0e --- /dev/null +++ b/nebula_hw_interfaces/test/common/test_udp/utils.hpp @@ -0,0 +1,78 @@ +// 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/udp.hpp" + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +std::optional udp_send( + const char * to_ip, uint16_t to_port, const std::vector & bytes) +{ + int sock_fd = socket(AF_INET, SOCK_DGRAM, 0); + if (sock_fd < 0) return errno; + + int enable = 1; + int result = setsockopt(sock_fd, SOL_SOCKET, SO_REUSEADDR, &enable, sizeof(enable)); + if (result < 0) return errno; + + sockaddr_in receiver_addr{}; + memset(&receiver_addr, 0, sizeof(receiver_addr)); + receiver_addr.sin_family = AF_INET; + receiver_addr.sin_port = htons(to_port); + receiver_addr.sin_addr.s_addr = inet_addr(to_ip); + + result = sendto( + sock_fd, bytes.data(), bytes.size(), 0, reinterpret_cast(&receiver_addr), + sizeof(receiver_addr)); + if (result < 0) return errno; + result = close(sock_fd); + if (result < 0) return errno; + return {}; +} + +template +std::optional< + std::pair, nebula::drivers::connections::UdpSocket::ReceiveMetadata>> +receive_once(nebula::drivers::connections::UdpSocket & sock, std::chrono::duration<_T, _R> timeout) +{ + std::condition_variable cv_received_result; + std::mutex mtx_result; + std::optional< + std::pair, nebula::drivers::connections::UdpSocket::ReceiveMetadata>> + result; + + sock.subscribe([&](const auto & data, const auto & metadata) { + std::lock_guard lock(mtx_result); + result.emplace(data, metadata); + cv_received_result.notify_one(); + }); + + std::unique_lock lock(mtx_result); + cv_received_result.wait_for(lock, timeout, [&result]() { return result.has_value(); }); + sock.unsubscribe(); + return result; +} diff --git a/nebula_hw_interfaces/test/hesai/test_ptc.cpp b/nebula_hw_interfaces/test/hesai/test_ptc.cpp new file mode 100644 index 000000000..34e3544e3 --- /dev/null +++ b/nebula_hw_interfaces/test/hesai/test_ptc.cpp @@ -0,0 +1,115 @@ +// Copyright 2024 TIER IV, Inc. + +#include "hesai/test_ptc/ptc_test.hpp" +#include "nebula_common/hesai/hesai_common.hpp" +#include "nebula_common/nebula_common.hpp" + +#include +#include +#include +#include + +#include + +namespace nebula::drivers +{ + +using testing::_; +using testing::AtLeast; +using testing::Exactly; +using testing::InSequence; + +const SensorModel g_models_under_test[] = { + SensorModel::HESAI_PANDAR64, SensorModel::HESAI_PANDAR40P, SensorModel::HESAI_PANDARQT64, + SensorModel::HESAI_PANDARQT128, SensorModel::HESAI_PANDARXT32, SensorModel::HESAI_PANDARAT128, + SensorModel::HESAI_PANDAR128_E4X, +}; + +const uint16_t g_u16_invalid = 0x4242; +const uint16_t g_ptc_port = 9347; +const char g_host_ip[] = "192.168.42.42"; +const char g_sensor_ip[] = "192.168.84.84"; + +auto make_sensor_config(SensorModel model) +{ + uint16_t rotation_speed = 600; + uint16_t sync_angle = 0; + double cut_angle = 0.0; + uint16_t cloud_min_angle = 0; + uint16_t cloud_max_angle = 360; + + if (model == SensorModel::HESAI_PANDARAT128) { + rotation_speed = 200; + sync_angle = 30; + cut_angle = 150.0; + cloud_min_angle = 30; + cloud_max_angle = 150; + } + + HesaiSensorConfiguration config{ + LidarConfigurationBase{ + EthernetSensorConfigurationBase{ + SensorConfigurationBase{model, "test"}, g_host_ip, g_sensor_ip, g_u16_invalid}, + ReturnMode::UNKNOWN, + g_u16_invalid, + g_u16_invalid, + CoordinateMode::UNKNOWN, + NAN, + NAN, + false, + {}, + false}, + "", + g_u16_invalid, + sync_angle, + cut_angle, + 0.1, + "", + rotation_speed, + cloud_min_angle, + cloud_max_angle, + PtpProfile::IEEE_802_1AS_AUTO, + 0, + PtpTransportType::L2, + PtpSwitchType::NON_TSN}; + + return std::make_shared(config); +} + +TEST_P(PtcTest, ConnectionLifecycle) +{ + /* Constructor does not immediately connect, destructor closes socket */ { + auto tcp_sock_ptr = make_mock_tcp_socket(); + auto & tcp_sock = *tcp_sock_ptr; + + EXPECT_CALL(tcp_sock, close()).Times(AtLeast(1)); + auto hw_interface = make_hw_interface(tcp_sock_ptr); + } + + /* Full lifecycle without sending/receiving */ { + auto tcp_sock_ptr = make_mock_tcp_socket(); + auto & tcp_sock = *tcp_sock_ptr; + + InSequence seq; + EXPECT_CALL(tcp_sock, init(g_host_ip, _, g_sensor_ip, g_ptc_port)).Times(Exactly(1)); + EXPECT_CALL(tcp_sock, bind()).Times(Exactly(1)); + EXPECT_CALL(tcp_sock, close()).Times(AtLeast(1)); + + auto cfg = make_sensor_config(GetParam()); + + auto hw_interface = make_hw_interface(tcp_sock_ptr); + hw_interface->SetSensorConfiguration(cfg); + hw_interface->InitializeTcpDriver(); + hw_interface->FinalizeTcpDriver(); + } +} + +INSTANTIATE_TEST_SUITE_P(TestMain, PtcTest, testing::ValuesIn(g_models_under_test)); + +} // namespace nebula::drivers + +int main(int argc, char * argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}; diff --git a/nebula_hw_interfaces/test/hesai/test_ptc/ptc_test.hpp b/nebula_hw_interfaces/test/hesai/test_ptc/ptc_test.hpp new file mode 100644 index 000000000..0cbe4f960 --- /dev/null +++ b/nebula_hw_interfaces/test/hesai/test_ptc/ptc_test.hpp @@ -0,0 +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. + +#pragma once + +#include "hesai/test_ptc/tcp_mock.hpp" +#include "nebula_common/loggers/console_logger.hpp" +#include "nebula_common/nebula_common.hpp" +#include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp" + +#include + +#include +#include + +namespace nebula::drivers +{ +class PtcTest : public ::testing::TestWithParam +{ +protected: + void SetUp() override {} + + void TearDown() override {} + + static auto make_mock_tcp_socket() { return std::make_shared(); } + + static auto make_hw_interface(std::shared_ptr tcp_socket) + { + auto model = GetParam(); + + auto logger = std::make_shared("HwInterface"); + + auto hw_interface = std::make_unique(logger, std::move(tcp_socket)); + hw_interface->SetTargetModel(hw_interface->NebulaModelToHesaiModelNo(model)); + return hw_interface; + } +}; + +} // namespace nebula::drivers diff --git a/nebula_hw_interfaces/test/hesai/test_ptc/tcp_mock.hpp b/nebula_hw_interfaces/test/hesai/test_ptc/tcp_mock.hpp new file mode 100644 index 000000000..3916c13f2 --- /dev/null +++ b/nebula_hw_interfaces/test/hesai/test_ptc/tcp_mock.hpp @@ -0,0 +1,47 @@ +// 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_hesai/connections/tcp.hpp" + +#include + +#include +#include + +namespace nebula::drivers::connections +{ + +class MockTcpSocket : public AbstractTcpSocket +{ +public: + MOCK_METHOD( + void, init, + (const std::string & host_ip, uint16_t host_port, const std::string & remote_ip, + uint16_t remote_port), + (override)); + + MOCK_METHOD(void, bind, (), (override)); + + MOCK_METHOD(void, close, (), (override)); + + MOCK_METHOD( + void, async_ptc_request, + (std::vector & ptc_packet, header_callback_t cb_header, payload_callback_t cb_payload, + completion_callback_t cb_completion), + (override)); +}; + +} // namespace nebula::drivers::connections 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 26dbd751b..c21841b20 100644 --- a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp @@ -54,7 +54,11 @@ class HesaiRosWrapper final : public rclcpp::Node public: explicit HesaiRosWrapper(const rclcpp::NodeOptions & options); - ~HesaiRosWrapper() noexcept override = default; + ~HesaiRosWrapper() noexcept override + { + if (!hw_interface_wrapper_) return; + hw_interface_wrapper_->hw_interface()->SensorInterfaceStop(); + }; /// @brief Get current status of this driver /// @return Current status @@ -65,7 +69,7 @@ class HesaiRosWrapper final : public rclcpp::Node Status StreamStart(); private: - void ReceiveCloudPacketCallback(std::vector & packet); + void receive_cloud_packet_callback(const std::vector & packet); void ReceiveScanMessageCallback(std::unique_ptr scan_msg); diff --git a/nebula_ros/include/nebula_ros/hesai/hw_interface_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hw_interface_wrapper.hpp index addc4f478..7070ebd54 100644 --- a/nebula_ros/include/nebula_ros/hesai/hw_interface_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hw_interface_wrapper.hpp @@ -15,6 +15,7 @@ #pragma once #include +#include #include #include diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp index cd1444606..6c7fc1019 100644 --- a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp @@ -9,6 +9,7 @@ #include #include +#include #include #include #include @@ -43,6 +44,14 @@ HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) RCLCPP_INFO_STREAM(get_logger(), "Sensor Configuration: " << *sensor_cfg_ptr_); launch_hw_ = declare_parameter("launch_hw", param_read_only()); + bool use_udp_only = declare_parameter("udp_only", false, param_read_only()); + + if (use_udp_only) { + RCLCPP_INFO_STREAM( + get_logger(), + "UDP-only mode is enabled. Settings checks, synchronization, and diagnostics publishing are " + "disabled."); + } if (launch_hw_) { hw_interface_wrapper_.emplace(this, sensor_cfg_ptr_); @@ -222,6 +231,13 @@ Status HesaiRosWrapper::ValidateAndSetConfig( if (new_config->frame_id.empty()) { return Status::SENSOR_CONFIG_ERROR; } + if (!new_config->multicast_ip.empty() && new_config->host_ip == "255.255.255.255") { + RCLCPP_ERROR( + get_logger(), + "A concrete host IP must be given when multicast is enabled, otherwise the correct network " + "interface cannot be determined."); + return Status::SENSOR_CONFIG_ERROR; + } if (new_config->ptp_profile == nebula::drivers::PtpProfile::UNKNOWN_PROFILE) { RCLCPP_ERROR( get_logger(), "Invalid PTP Profile Provided. Please use '1588v2', '802.1as' or 'automotive'"); @@ -417,6 +433,7 @@ rcl_interfaces::msg::SetParametersResult HesaiRosWrapper::OnParameterChange( } void HesaiRosWrapper::ReceiveCloudPacketCallback(std::vector & packet) +void HesaiRosWrapper::receive_cloud_packet_callback(const std::vector & packet) { if (!decoder_wrapper_ || decoder_wrapper_->Status() != Status::OK) { return; @@ -429,7 +446,7 @@ void HesaiRosWrapper::ReceiveCloudPacketCallback(std::vector & packet) auto msg_ptr = std::make_unique(); msg_ptr->stamp.sec = static_cast(timestamp_ns / 1'000'000'000); msg_ptr->stamp.nanosec = static_cast(timestamp_ns % 1'000'000'000); - msg_ptr->data.swap(packet); + msg_ptr->data = packet; if (!packet_queue_.tryPush(std::move(msg_ptr))) { RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 500, "Packet(s) dropped"); diff --git a/nebula_ros/src/hesai/hw_interface_wrapper.cpp b/nebula_ros/src/hesai/hw_interface_wrapper.cpp index 4a1ad5c91..c8e10ac88 100644 --- a/nebula_ros/src/hesai/hw_interface_wrapper.cpp +++ b/nebula_ros/src/hesai/hw_interface_wrapper.cpp @@ -2,7 +2,11 @@ #include "nebula_ros/hesai/hw_interface_wrapper.hpp" +#include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp" #include "nebula_ros/common/parameter_descriptors.hpp" +#include "nebula_ros/common/rclcpp_logger.hpp" + +#include namespace nebula { @@ -11,10 +15,13 @@ namespace ros HesaiHwInterfaceWrapper::HesaiHwInterfaceWrapper( rclcpp::Node * const parent_node, - std::shared_ptr & config) -: hw_interface_(new nebula::drivers::HesaiHwInterface()), - logger_(parent_node->get_logger().get_child("HwInterface")), - status_(Status::NOT_INITIALIZED) + std::shared_ptr & config, bool use_udp_only) +: hw_interface_(std::make_shared( + drivers::loggers::RclcppLogger(parent_node->get_logger()).child("HwInterface"), + std::move(std::make_unique()))), + logger_(parent_node->get_logger().get_child("HwInterfaceWrapper")), + status_(Status::NOT_INITIALIZED), + use_udp_only_(use_udp_only) { setup_sensor_ = parent_node->declare_parameter("setup_sensor", param_read_only()); bool retry_connect = parent_node->declare_parameter("retry_hw", param_read_only()); @@ -27,7 +34,6 @@ HesaiHwInterfaceWrapper::HesaiHwInterfaceWrapper( (std::stringstream{} << "Could not initialize HW interface: " << status_).str()); } - hw_interface_->SetLogger(std::make_shared(parent_node->get_logger())); hw_interface_->SetTargetModel(config->sensor_model); int retry_count = 0;