From 658aab16546a673d6875364addcafeb9a6319d9c Mon Sep 17 00:00:00 2001 From: Max Schmeller <6088931+mojomex@users.noreply.github.com> Date: Mon, 25 Nov 2024 18:52:18 +0900 Subject: [PATCH 1/3] perf(hesai): remove decoder thread and MtQueue, make everything single-threaded (#226) Signed-off-by: Max SCHMELLER --- .../include/nebula_ros/hesai/hesai_ros_wrapper.hpp | 6 ------ nebula_ros/src/hesai/hesai_ros_wrapper.cpp | 13 ++----------- 2 files changed, 2 insertions(+), 17 deletions(-) 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 f8d221c3..8bf0100f 100644 --- a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp @@ -17,7 +17,6 @@ #include "nebula_common/hesai/hesai_common.hpp" #include "nebula_common/nebula_common.hpp" #include "nebula_common/nebula_status.hpp" -#include "nebula_ros/common/mt_queue.hpp" #include "nebula_ros/hesai/decoder_wrapper.hpp" #include "nebula_ros/hesai/hw_interface_wrapper.hpp" #include "nebula_ros/hesai/hw_monitor_wrapper.hpp" @@ -98,11 +97,6 @@ class HesaiRosWrapper final : public rclcpp::Node std::shared_ptr sensor_cfg_ptr_{}; - /// @brief Stores received packets that have not been processed yet by the decoder thread - MtQueue> packet_queue_; - /// @brief Thread to isolate decoding from receiving - std::thread decoder_thread_; - rclcpp::Subscription::SharedPtr packets_sub_{}; bool launch_hw_; diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp index 73869d1f..dbf5fe09 100644 --- a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp @@ -23,7 +23,6 @@ HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) : rclcpp::Node("hesai_ros_wrapper", rclcpp::NodeOptions(options).use_intra_process_comms(true)), wrapper_status_(Status::NOT_INITIALIZED), sensor_cfg_ptr_(nullptr), - packet_queue_(3000), hw_interface_wrapper_(), hw_monitor_wrapper_(), decoder_wrapper_() @@ -82,12 +81,6 @@ HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) RCLCPP_DEBUG(get_logger(), "Starting stream"); - decoder_thread_ = std::thread([this]() { - while (true) { - decoder_wrapper_->process_cloud_packet(packet_queue_.pop()); - } - }); - if (launch_hw_) { hw_interface_wrapper_->hw_interface()->RegisterScanCallback( std::bind(&HesaiRosWrapper::receive_cloud_packet_callback, this, std::placeholders::_1)); @@ -289,7 +282,7 @@ void HesaiRosWrapper::receive_scan_message_callback( nebula_pkt_ptr->stamp = pkt.stamp; std::copy(pkt.data.begin(), pkt.data.end(), std::back_inserter(nebula_pkt_ptr->data)); - packet_queue_.push(std::move(nebula_pkt_ptr)); + decoder_wrapper_->process_cloud_packet(std::move(nebula_pkt_ptr)); } } @@ -428,9 +421,7 @@ void HesaiRosWrapper::receive_cloud_packet_callback(std::vector & packe msg_ptr->stamp.nanosec = static_cast(timestamp_ns % 1'000'000'000); msg_ptr->data.swap(packet); - if (!packet_queue_.try_push(std::move(msg_ptr))) { - RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 500, "Packet(s) dropped"); - } + decoder_wrapper_->process_cloud_packet(std::move(msg_ptr)); } std::string HesaiRosWrapper::get_calibration_parameter_name(drivers::SensorModel model) const From 77b4c6c5b562c37e88babdf84914efdc8eb9f22c Mon Sep 17 00:00:00 2001 From: Max Schmeller <6088931+mojomex@users.noreply.github.com> Date: Mon, 25 Nov 2024 18:52:47 +0900 Subject: [PATCH 2/3] feat: add a logging interface, dependency injectable loggers (#233) * feat(loggers): add a new generic dependency-injectable logger class Signed-off-by: Max SCHMELLER * feat(loggers): add a console logger implementation Signed-off-by: Max SCHMELLER * feat(loggers): add an rclcpp logger implementation Signed-off-by: Max SCHMELLER * chore(logger): remove useless rule-of-five boilerplate Signed-off-by: Max SCHMELLER --------- Signed-off-by: Max SCHMELLER --- .../nebula_common/loggers/console_logger.hpp | 55 +++++++++++++++++ .../include/nebula_common/loggers/logger.hpp | 44 ++++++++++++++ .../nebula_ros/common/rclcpp_logger.hpp | 59 +++++++++++++++++++ 3 files changed, 158 insertions(+) create mode 100644 nebula_common/include/nebula_common/loggers/console_logger.hpp create mode 100644 nebula_common/include/nebula_common/loggers/logger.hpp create mode 100644 nebula_ros/include/nebula_ros/common/rclcpp_logger.hpp diff --git a/nebula_common/include/nebula_common/loggers/console_logger.hpp b/nebula_common/include/nebula_common/loggers/console_logger.hpp new file mode 100644 index 00000000..fb2a6127 --- /dev/null +++ b/nebula_common/include/nebula_common/loggers/console_logger.hpp @@ -0,0 +1,55 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include "nebula_common/loggers/logger.hpp" + +#include +#include +#include +#include +#include +#include + +namespace nebula::drivers::loggers +{ + +class ConsoleLogger : public Logger +{ +public: + explicit ConsoleLogger(std::string name) : name_(std::move(name)) {} + + void debug(const std::string & message) override { 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 + { + return std::make_shared(name_ + "." + name); + } + +private: + std::string name_; + + 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 + os << ("[" + name_ + "][" + severity + "] " + message) << std::endl; + } +}; + +} // namespace nebula::drivers::loggers diff --git a/nebula_common/include/nebula_common/loggers/logger.hpp b/nebula_common/include/nebula_common/loggers/logger.hpp new file mode 100644 index 00000000..9bf40715 --- /dev/null +++ b/nebula_common/include/nebula_common/loggers/logger.hpp @@ -0,0 +1,44 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include +#include + +#define NEBULA_LOG_STREAM(log_func, stream_args) \ + { \ + std::stringstream ss{}; \ + ss << stream_args; \ + log_func(ss.str()); \ + } + +namespace nebula::drivers::loggers +{ + +class Logger +{ +public: + virtual ~Logger() = default; + + virtual void debug(const std::string & message) = 0; + virtual void info(const std::string & message) = 0; + virtual void warn(const std::string & message) = 0; + virtual void error(const std::string & message) = 0; + + virtual std::shared_ptr child(const std::string & name) = 0; +}; + +} // namespace nebula::drivers::loggers diff --git a/nebula_ros/include/nebula_ros/common/rclcpp_logger.hpp b/nebula_ros/include/nebula_ros/common/rclcpp_logger.hpp new file mode 100644 index 00000000..33343528 --- /dev/null +++ b/nebula_ros/include/nebula_ros/common/rclcpp_logger.hpp @@ -0,0 +1,59 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include +#include + +#include +#include + +namespace nebula::drivers::loggers +{ + +class RclcppLogger : public Logger +{ +public: + explicit RclcppLogger(const std::string & name) : underlying_logger_(rclcpp::get_logger(name)) {} + explicit RclcppLogger(const rclcpp::Logger & underlying) : underlying_logger_(underlying) {} + + void debug(const std::string & message) override + { + RCLCPP_DEBUG_STREAM(underlying_logger_, message); + } + void info(const std::string & message) override + { + RCLCPP_INFO_STREAM(underlying_logger_, message); + } + void warn(const std::string & message) override + { + RCLCPP_WARN_STREAM(underlying_logger_, message); + } + void error(const std::string & message) override + { + RCLCPP_ERROR_STREAM(underlying_logger_, message); + } + + std::shared_ptr child(const std::string & name) override + { + return std::make_shared(underlying_logger_.get_child(name)); + } + +private: + rclcpp::Logger underlying_logger_; +}; + +} // namespace nebula::drivers::loggers From 3d42cd0879c501056331d5a841c5c058f1400f30 Mon Sep 17 00:00:00 2001 From: Max Schmeller <6088931+mojomex@users.noreply.github.com> Date: Tue, 26 Nov 2024 19:48:31 +0900 Subject: [PATCH 3/3] feat(hesai_hw_interface): use dependency-injected logger, get rid of RCLCPP dependency (#232) * chore(hesai_hw_interfaces): switch to the new loggers::Logger class, remove rclcpp dependency :tada: Signed-off-by: Max SCHMELLER * chore(hesai): use angle brackets for out-of-package import Signed-off-by: Max SCHMELLER * ci(pre-commit): autofix * chore(hesai_hw_interface): pass logger shared_ptr by constref Signed-off-by: Max SCHMELLER --------- Signed-off-by: Max SCHMELLER Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../hesai_hw_interface.hpp | 37 ++-- .../hesai_hw_interface.cpp | 165 +++++++----------- nebula_ros/src/hesai/hw_interface_wrapper.cpp | 11 +- 3 files changed, 84 insertions(+), 129 deletions(-) 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 d37a33e0..dc5103ab 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,7 @@ // 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 #include @@ -28,15 +28,15 @@ #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 #include #include @@ -136,6 +136,7 @@ class HesaiHwInterface using ptc_cmd_result_t = nebula::util::expected, ptc_error_t>; + std::shared_ptr logger_; 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_; @@ -165,20 +166,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. @@ -202,7 +189,7 @@ class HesaiHwInterface public: /// @brief Constructor - HesaiHwInterface(); + explicit HesaiHwInterface(const std::shared_ptr & logger); /// @brief Destructor ~HesaiHwInterface(); /// @brief Initializing tcp_driver for TCP communication @@ -460,10 +447,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 nebula::drivers 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 0a08dd93..29302bbc 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 @@ -17,6 +17,7 @@ #include #include #include +#include // #define WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE @@ -27,17 +28,21 @@ #include +#include + namespace nebula::drivers { using std::string_literals::operator""s; using nlohmann::json; -HesaiHwInterface::HesaiHwInterface() -: cloud_io_context_{new ::drivers::common::IoContext(1)}, +HesaiHwInterface::HesaiHwInterface(const std::shared_ptr & logger) +: logger_(logger), + 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)} + tcp_driver_{new ::drivers::tcp_driver::TcpDriver(m_owned_ctx)}, + target_model_no(NebulaModelToHesaiModelNo(SensorModel::UNKNOWN)) { } @@ -76,17 +81,17 @@ HesaiHwInterface::ptc_cmd_result_t HesaiHwInterface::SendReceive( << " (" << len << ") "; std::string log_tag = ss.str(); - PrintDebug(log_tag + "Entering lock"); + logger_->debug(log_tag + "Entering lock"); std::timed_mutex tm; tm.lock(); if (tcp_driver_->GetIOContext()->stopped()) { - PrintDebug(log_tag + "IOContext was stopped"); + logger_->debug(log_tag + "IOContext was stopped"); tcp_driver_->GetIOContext()->restart(); } - PrintDebug(log_tag + "Sending payload"); + logger_->debug(log_tag + "Sending payload"); tcp_driver_->asyncSendReceiveHeaderPayload( send_buf, [this, log_tag, command_id, response_complete, @@ -95,7 +100,7 @@ HesaiHwInterface::ptc_cmd_result_t HesaiHwInterface::SendReceive( size_t payload_len = (header_bytes[4] << 24) | (header_bytes[5] << 16) | (header_bytes[6] << 8) | header_bytes[7]; - PrintDebug( + logger_->debug( log_tag + "Received header (expecting " + std::to_string(payload_len) + "B payload)"); // If command_id in the response does not match, we got a response for another command (or // rubbish), probably as a result of too many simultaneous TCP connections to the sensor (e.g. @@ -109,7 +114,7 @@ HesaiHwInterface::ptc_cmd_result_t HesaiHwInterface::SendReceive( }, [this, log_tag, recv_buf, response_complete, error_code](const std::vector & payload_bytes) { - PrintDebug(log_tag + "Received payload"); + logger_->debug(log_tag + "Received payload"); // Header had payload length 0 (thus, header callback processed request successfully already), // but we still received a payload: invalid state @@ -123,19 +128,19 @@ HesaiHwInterface::ptc_cmd_result_t HesaiHwInterface::SendReceive( *response_complete = true; }, [this, log_tag, &tm]() { - PrintDebug(log_tag + "Unlocking mutex"); + logger_->debug(log_tag + "Unlocking mutex"); tm.unlock(); - PrintDebug(log_tag + "Unlocked mutex"); + logger_->debug(log_tag + "Unlocked mutex"); }); this->IOContextRun(); if (!tm.try_lock_for(std::chrono::seconds(1))) { - PrintError(log_tag + "Request did not finish within 1s"); + logger_->error(log_tag + "Request did not finish within 1s"); error_code->error_flags |= TCP_ERROR_TIMEOUT; return *error_code; } if (!response_complete) { - PrintError(log_tag + "Did not receive response"); + logger_->error(log_tag + "Did not receive response"); error_code->error_flags |= TCP_ERROR_INCOMPLETE_RESPONSE; return *error_code; } @@ -144,7 +149,7 @@ HesaiHwInterface::ptc_cmd_result_t HesaiHwInterface::SendReceive( return *error_code; } - PrintDebug(log_tag + "Received response"); + logger_->debug(log_tag + "Received response"); return *recv_buf; } @@ -160,7 +165,7 @@ Status HesaiHwInterface::SetSensorConfiguration( Status HesaiHwInterface::SensorInterfaceStart() { try { - PrintInfo("Starting UDP receiver"); + logger_->info("Starting UDP receiver"); if (sensor_configuration_->multicast_ip.empty()) { cloud_udp_driver_->init_receiver( sensor_configuration_->host_ip, sensor_configuration_->data_port); @@ -171,16 +176,16 @@ Status HesaiHwInterface::SensorInterfaceStart() cloud_udp_driver_->receiver()->setMulticast(true); } #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - PrintError("init ok"); + logger_->error("init ok"); #endif cloud_udp_driver_->receiver()->open(); #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - PrintError("open ok"); + logger_->error("open ok"); #endif bool success = cloud_udp_driver_->receiver()->setKernelBufferSize(UDP_SOCKET_BUFFER_SIZE); if (!success) { - PrintError( + logger_->error( "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; @@ -188,13 +193,13 @@ Status HesaiHwInterface::SensorInterfaceStart() cloud_udp_driver_->receiver()->bind(); #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - PrintError("bind ok"); + logger_->error("bind ok"); #endif cloud_udp_driver_->receiver()->asyncReceive( std::bind(&HesaiHwInterface::ReceiveSensorPacketCallback, this, std::placeholders::_1)); #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - PrintError("async receive set"); + logger_->error("async receive set"); #endif } catch (const std::exception & ex) { Status status = Status::UDP_CONNECTION_ERROR; @@ -226,14 +231,14 @@ Status HesaiHwInterface::GetSensorConfiguration( { std::stringstream ss; ss << sensor_configuration; - PrintDebug(ss.str()); + logger_->debug(ss.str()); return Status::ERROR_1; } Status HesaiHwInterface::GetCalibrationConfiguration( CalibrationConfigurationBase & calibration_configuration) { - PrintDebug(calibration_configuration.calibration_file); + logger_->debug(calibration_configuration.calibration_file); return Status::ERROR_1; } @@ -270,7 +275,7 @@ Status HesaiHwInterface::FinalizeTcpDriver() tcp_driver_->close(); } } catch (std::exception & e) { - PrintError("Error while finalizing the TcpDriver"); + logger_->error("Error while finalizing the TcpDriver"); return Status::UDP_CONNECTION_ERROR; } return Status::OK; @@ -672,7 +677,7 @@ HesaiPtpConfig HesaiHwInterface::GetPtpConfig() if (response.size() < sizeof(HesaiPtpConfig)) { throw std::runtime_error("HesaiPtpConfig has unexpected payload size"); } else if (response.size() > sizeof(HesaiPtpConfig)) { - PrintError("HesaiPtpConfig from Sensor has unknown format. Will parse anyway."); + logger_->error("HesaiPtpConfig from Sensor has unknown format. Will parse anyway."); } HesaiPtpConfig hesai_ptp_config; @@ -735,7 +740,7 @@ HesaiStatus HesaiHwInterface::GetHttpClientDriverOnce( std::stringstream ss; ss << "HesaiHwInterface::GetHttpClientDriverOnce: " << status << sensor_configuration_->sensor_ip << "," << 80 << std::endl; - PrintError(ss.str()); + logger_->error(ss.str()); return Status::HTTP_CONNECTION_ERROR; } return Status::OK; @@ -752,7 +757,7 @@ HesaiStatus HesaiHwInterface::GetHttpClientDriverOnce( void HesaiHwInterface::str_cb(const std::string & str) { - PrintInfo(str); + logger_->info(str); } std::pair HesaiHwInterface::unwrap_http_response( @@ -881,7 +886,7 @@ HesaiStatus HesaiHwInterface::GetLidarMonitorAsyncHttp( std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; auto st = GetHttpClientDriverOnce(ctx, hcd); if (st != Status::OK) { - PrintError("HesaiHwInterface::GetLidarMonitorAsyncHttp: cannot GetHttpClientDriverOnce"); + logger_->error("HesaiHwInterface::GetLidarMonitorAsyncHttp: cannot GetHttpClientDriverOnce"); return st; } @@ -891,7 +896,7 @@ HesaiStatus HesaiHwInterface::GetLidarMonitorAsyncHttp( boost::system::error_code ec; ctx->run(ec); if (ec) { - PrintError("HesaiHwInterface::GetLidarMonitorAsyncHttp: " + ec.message()); + logger_->error("HesaiHwInterface::GetLidarMonitorAsyncHttp: " + ec.message()); } return Status::WAITING_FOR_SENSOR_RESPONSE; } @@ -918,15 +923,15 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( if (sensor_configuration->return_mode != current_return_mode) { std::stringstream ss; ss << current_return_mode; - PrintInfo("Current LiDAR return_mode: " + ss.str()); + logger_->info("Current LiDAR return_mode: " + ss.str()); std::stringstream ss2; ss2 << sensor_configuration->return_mode; - PrintInfo("Current Configuration return_mode: " + ss2.str()); + logger_->info("Current Configuration return_mode: " + ss2.str()); std::thread t([this, sensor_configuration] { auto return_mode_int = nebula::drivers::int_from_return_mode_hesai( sensor_configuration->return_mode, sensor_configuration->sensor_model); if (return_mode_int < 0) { - PrintError( + logger_->error( "Invalid Return Mode for this sensor. Please check your settings. Falling back to Dual " "mode."); return_mode_int = 2; @@ -939,16 +944,16 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( auto current_rotation_speed = hesai_config.spin_rate; if (sensor_configuration->rotation_speed != current_rotation_speed.value()) { - PrintInfo( + logger_->info( "current lidar rotation_speed: " + std::to_string(static_cast(current_rotation_speed.value()))); - PrintInfo( + logger_->info( "current configuration rotation_speed: " + std::to_string(sensor_configuration->rotation_speed)); if (UseHttpSetSpinRate()) { SetSpinSpeedAsyncHttp(sensor_configuration->rotation_speed); } else { - PrintInfo( + logger_->info( "Setting up spin rate via TCP." + std::to_string(sensor_configuration->rotation_speed)); std::thread t( [this, sensor_configuration] { SetSpinRate(sensor_configuration->rotation_speed); }); @@ -969,27 +974,27 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( : sensor_configuration->multicast_ip; if (desired_host_addr != current_host_addr) { set_flg = true; - PrintInfo("current lidar dest_ipaddr: " + current_host_addr); - PrintInfo("current configuration host_ip: " + desired_host_addr); + logger_->info("current lidar dest_ipaddr: " + current_host_addr); + logger_->info("current configuration host_ip: " + desired_host_addr); } auto current_host_dport = hesai_config.dest_LiDAR_udp_port; if (sensor_configuration->data_port != current_host_dport.value()) { set_flg = true; - PrintInfo( + logger_->info( "current lidar dest_LiDAR_udp_port: " + std::to_string(static_cast(current_host_dport.value()))); - PrintInfo( + logger_->info( "current configuration data_port: " + std::to_string(sensor_configuration->data_port)); } auto current_host_tport = hesai_config.dest_gps_udp_port; if (sensor_configuration->gnss_port != current_host_tport.value()) { set_flg = true; - PrintInfo( + logger_->info( "current lidar dest_gps_udp_port: " + std::to_string(static_cast(current_host_tport.value()))); - PrintInfo( + logger_->info( "current configuration gnss_port: " + std::to_string(sensor_configuration->gnss_port)); } @@ -1015,9 +1020,9 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( set_flg = true; } if (sync_flg && set_flg) { - PrintInfo("current lidar sync: " + std::to_string(hesai_config.sync)); - PrintInfo("current lidar sync_angle: " + std::to_string(sensor_sync_angle)); - PrintInfo("current configuration sync_angle: " + std::to_string(config_sync_angle)); + logger_->info("current lidar sync: " + std::to_string(hesai_config.sync)); + logger_->info("current lidar sync_angle: " + std::to_string(sensor_sync_angle)); + logger_->info("current configuration sync_angle: " + std::to_string(config_sync_angle)); std::thread t( [this, sync_flg, config_sync_angle] { SetSyncAngle(sync_flg, config_sync_angle); }); t.join(); @@ -1031,7 +1036,7 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( sensor_configuration->sensor_model == SensorModel::HESAI_PANDARQT64 || sensor_configuration->sensor_model == SensorModel::HESAI_PANDARXT32 || sensor_configuration->sensor_model == SensorModel::HESAI_PANDARXT32M) { - PrintInfo("Trying to set Clock source to PTP"); + logger_->info("Trying to set Clock source to PTP"); SetClockSource(HESAI_LIDAR_PTP_CLOCK_SOURCE); } std::ostringstream tmp_ostringstream; @@ -1039,28 +1044,28 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( << ", Domain: " << std::to_string(sensor_configuration->ptp_domain) << ", Transport: " << sensor_configuration->ptp_transport_type << ", Switch Type: " << sensor_configuration->ptp_switch_type << " via TCP"; - PrintInfo(tmp_ostringstream.str()); + logger_->info(tmp_ostringstream.str()); SetPtpConfig( static_cast(sensor_configuration->ptp_profile), sensor_configuration->ptp_domain, static_cast(sensor_configuration->ptp_transport_type), static_cast(sensor_configuration->ptp_switch_type), PTP_LOG_ANNOUNCE_INTERVAL, PTP_SYNC_INTERVAL, PTP_LOG_MIN_DELAY_INTERVAL); - PrintDebug("Setting properties done"); + logger_->debug("Setting properties done"); }); - PrintDebug("Waiting for thread to finish"); + logger_->debug("Waiting for thread to finish"); t.join(); - PrintDebug("Thread finished"); + logger_->debug("Thread finished"); std::this_thread::sleep_for(wait_time); } else { // AT128 only supports PTP setup via HTTP - PrintInfo("Trying to set SyncAngle via HTTP"); + logger_->info("Trying to set SyncAngle via HTTP"); SetSyncAngleSyncHttp(1, sensor_configuration->sync_angle); std::ostringstream tmp_ostringstream; tmp_ostringstream << "Trying to set PTP Config: " << sensor_configuration->ptp_profile << ", Domain: " << sensor_configuration->ptp_domain << ", Transport: " << sensor_configuration->ptp_transport_type << " via HTTP"; - PrintInfo(tmp_ostringstream.str()); + logger_->info(tmp_ostringstream.str()); SetPtpConfigSyncHttp( static_cast(sensor_configuration->ptp_profile), sensor_configuration->ptp_domain, static_cast(sensor_configuration->ptp_transport_type), PTP_LOG_ANNOUNCE_INTERVAL, @@ -1070,7 +1075,7 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE std::cout << "End CheckAndSetConfig(HesaiConfig)!!" << std::endl; #endif - PrintDebug("GetAndCheckConfig(HesaiConfig) finished"); + logger_->debug("GetAndCheckConfig(HesaiConfig) finished"); return Status::OK; } @@ -1097,10 +1102,10 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( static_cast(sensor_configuration->cloud_min_angle * 10) != current_cloud_min_angle_ddeg.value()) { set_flg = true; - PrintInfo( + 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)); } @@ -1110,10 +1115,10 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( static_cast(sensor_configuration->cloud_max_angle * 10) != current_cloud_max_angle_ddeg.value()) { set_flg = true; - PrintInfo( + 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)); } @@ -1307,48 +1312,6 @@ bool HesaiHwInterface::UseHttpGetLidarMonitor() return UseHttpGetLidarMonitor(target_model_no); } -void HesaiHwInterface::SetLogger(std::shared_ptr logger) -{ - parent_node_logger = logger; -} - -void HesaiHwInterface::PrintInfo(std::string info) -{ - if (parent_node_logger) { - RCLCPP_INFO_STREAM((*parent_node_logger), info); - } else { - std::cout << info << std::endl; - } -} - -void HesaiHwInterface::PrintError(std::string error) -{ - if (parent_node_logger) { - RCLCPP_ERROR_STREAM((*parent_node_logger), error); - } else { - std::cerr << error << std::endl; - } -} - -void HesaiHwInterface::PrintDebug(std::string debug) -{ - if (parent_node_logger) { - RCLCPP_DEBUG_STREAM((*parent_node_logger), debug); - } else { - std::cout << debug << std::endl; - } -} - -void HesaiHwInterface::PrintDebug(const std::vector & bytes) -{ - std::stringstream ss; - for (const auto & b : bytes) { - ss << static_cast(b) << ", "; - } - ss << std::endl; - PrintDebug(ss.str()); -} - std::string HesaiHwInterface::PrettyPrintPTCError(ptc_error_t error_code) { if (error_code.ok()) { @@ -1430,9 +1393,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_ros/src/hesai/hw_interface_wrapper.cpp b/nebula_ros/src/hesai/hw_interface_wrapper.cpp index b5f99109..22765bf7 100644 --- a/nebula_ros/src/hesai/hw_interface_wrapper.cpp +++ b/nebula_ros/src/hesai/hw_interface_wrapper.cpp @@ -3,6 +3,11 @@ #include "nebula_ros/hesai/hw_interface_wrapper.hpp" #include "nebula_ros/common/parameter_descriptors.hpp" +#include "nebula_ros/common/rclcpp_logger.hpp" + +#include + +#include namespace nebula::ros { @@ -10,8 +15,9 @@ namespace nebula::ros HesaiHwInterfaceWrapper::HesaiHwInterfaceWrapper( rclcpp::Node * const parent_node, std::shared_ptr & config, bool use_udp_only) -: hw_interface_(new nebula::drivers::HesaiHwInterface()), - logger_(parent_node->get_logger().get_child("HwInterface")), +: hw_interface_(std::make_shared( + drivers::loggers::RclcppLogger(parent_node->get_logger()).child("HwInterface"))), + logger_(parent_node->get_logger().get_child("HwInterfaceWrapper")), status_(Status::NOT_INITIALIZED), use_udp_only_(use_udp_only) { @@ -26,7 +32,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); if (use_udp_only) {