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) {