diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp index f22c3c447..3cbeb83be 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp @@ -19,7 +19,7 @@ #include #include -#include +#include #include @@ -34,7 +34,7 @@ class ContinentalARS548HwInterface { public: /// @brief Constructor - ContinentalARS548HwInterface(); + explicit ContinentalARS548HwInterface(const std::shared_ptr & logger); /// @brief Starting the interface that handles UDP streams /// @return Resulting status @@ -130,10 +130,6 @@ class ContinentalARS548HwInterface /// @return Resulting status Status set_yaw_rate(float yaw_rate); - /// @brief Setting rclcpp::Logger - /// @param node Logger - void set_logger(std::shared_ptr node); - private: /// @brief Printing the string to RCLCPP_INFO_STREAM /// @param info Target string @@ -160,8 +156,7 @@ class ContinentalARS548HwInterface std::unique_ptr<::drivers::udp_driver::UdpDriver> sensor_udp_driver_ptr_; std::shared_ptr config_ptr_; std::function)> packet_callback_; - - std::shared_ptr parent_node_logger_ptr_; + std::shared_ptr logger_; }; } // namespace nebula::drivers::continental_ars548 diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp index 05e954f1f..d571953d9 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp @@ -18,7 +18,7 @@ #include "nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp" #include -#include +#include #include #include @@ -28,6 +28,7 @@ #include #include #include +#include #include namespace nebula::drivers::continental_srr520 @@ -37,7 +38,7 @@ class ContinentalSRR520HwInterface { public: /// @brief Constructor - ContinentalSRR520HwInterface(); + explicit ContinentalSRR520HwInterface(const std::shared_ptr & logger); /// @brief Starting the interface that handles UDP streams /// @return Resulting status @@ -95,10 +96,6 @@ class ContinentalSRR520HwInterface float longitudinal_acceleration, float lateral_acceleration, float yaw_rate, float longitudinal_velocity, bool standstill); - /// @brief Setting rclcpp::Logger - /// @param node Logger - void set_logger(std::shared_ptr node); - private: /// @brief Send a Fd frame /// @param data a buffer containing the data to send @@ -135,7 +132,7 @@ class ContinentalSRR520HwInterface bool sync_follow_up_sent_{true}; builtin_interfaces::msg::Time last_sync_stamp_; - std::shared_ptr parent_node_logger_ptr_; + std::shared_ptr logger_; }; } // namespace nebula::drivers::continental_srr520 diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_robosense/robosense_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_robosense/robosense_hw_interface.hpp index 94e6eb256..4f69c2ae2 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_robosense/robosense_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_robosense/robosense_hw_interface.hpp @@ -26,8 +26,8 @@ #endif #include +#include #include -#include #include #include @@ -54,19 +54,11 @@ class RobosenseHwInterface scan_reception_callback_; /**This function pointer is called when the scan is complete*/ std::function & buffer)> info_reception_callback_; /**This function pointer is called when DIFOP packet is received*/ - std::shared_ptr parent_node_logger_; - - /// @brief Printing the string to RCLCPP_INFO_STREAM - /// @param info Target string - void print_info(std::string info); - - /// @brief Printing the string to RCLCPP_DEBUG_STREAM - /// @param debug Target string - void print_debug(std::string debug); + std::shared_ptr logger_; public: /// @brief Constructor - RobosenseHwInterface(); + explicit RobosenseHwInterface(const std::shared_ptr & logger); /// @brief Callback function to receive the Cloud Packet data from the UDP Driver /// @param buffer Buffer containing the data received from the UDP socket @@ -99,10 +91,6 @@ class RobosenseHwInterface /// @param scan_callback Callback function /// @return Resulting status Status register_info_callback(std::function &)> info_callback); - - /// @brief Setting rclcpp::Logger - /// @param node Logger - void set_logger(std::shared_ptr logger); }; } // namespace nebula::drivers diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_velodyne/velodyne_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_velodyne/velodyne_hw_interface.hpp index 6d4771b15..ff2319565 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_velodyne/velodyne_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_velodyne/velodyne_hw_interface.hpp @@ -31,15 +31,17 @@ #include #include +#include #include #include -#include #include #include +#include #include #include +#include #include namespace nebula::drivers @@ -136,20 +138,13 @@ class VelodyneHwInterface std::shared_ptr sensor_configuration, boost::property_tree::ptree tree); - std::shared_ptr parent_node_logger_; - /// @brief Printing the string to RCLCPP_INFO_STREAM - /// @param info Target string - void print_info(std::string info); - /// @brief Printing the string to RCLCPP_ERROR_STREAM - /// @param error Target string - void print_error(std::string error); - /// @brief Printing the string to RCLCPP_DEBUG_STREAM - /// @param debug Target string - void print_debug(std::string debug); + std::shared_ptr logger_; public: /// @brief Constructor - VelodyneHwInterface(); + explicit VelodyneHwInterface(const std::shared_ptr & logger); + + virtual ~VelodyneHwInterface() = default; /// @brief Callback function to receive the Cloud Packet data from the UDP Driver /// @param buffer Buffer containing the data received from the UDP socket @@ -261,12 +256,7 @@ class VelodyneHwInterface /// @param use_dhcp DHCP on /// @return Resulting status VelodyneStatus set_net_dhcp(bool use_dhcp); - - /// @brief Setting rclcpp::Logger - /// @param node Logger - void set_logger(std::shared_ptr node); }; - } // namespace nebula::drivers #endif // NEBULA_VELODYNE_HW_INTERFACE_H diff --git a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp index 240a6a35b..518d0a08a 100644 --- a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp @@ -25,9 +25,11 @@ namespace nebula::drivers::continental_ars548 { -ContinentalARS548HwInterface::ContinentalARS548HwInterface() +ContinentalARS548HwInterface::ContinentalARS548HwInterface( + const std::shared_ptr & logger) : sensor_io_context_ptr_{new ::drivers::common::IoContext(1)}, - sensor_udp_driver_ptr_{new ::drivers::udp_driver::UdpDriver(*sensor_io_context_ptr_)} + sensor_udp_driver_ptr_{new ::drivers::udp_driver::UdpDriver(*sensor_io_context_ptr_)}, + logger_(logger) { } @@ -64,7 +66,7 @@ Status ContinentalARS548HwInterface::sensor_interface_start() } } catch (const std::exception & ex) { Status status = Status::UDP_CONNECTION_ERROR; - std::cerr << status << config_ptr_->sensor_ip << "," << config_ptr_->data_port << std::endl; + logger_->error("Setting sensor configuration failed"); return status; } return Status::OK; @@ -87,7 +89,7 @@ void ContinentalARS548HwInterface::receive_sensor_packet_callback_with_sender( void ContinentalARS548HwInterface::receive_sensor_packet_callback(std::vector & buffer) { if (buffer.size() < sizeof(HeaderPacket)) { - print_error("Unrecognized packet. Too short"); + logger_->error("Unrecognized packet. Too short"); return; } @@ -513,36 +515,4 @@ Status ContinentalARS548HwInterface::set_yaw_rate(float yaw_rate) return Status::OK; } -void ContinentalARS548HwInterface::set_logger(std::shared_ptr logger) -{ - parent_node_logger_ptr_ = logger; -} - -void ContinentalARS548HwInterface::print_info(std::string info) -{ - if (parent_node_logger_ptr_) { - RCLCPP_INFO_STREAM((*parent_node_logger_ptr_), info); - } else { - std::cout << info << std::endl; - } -} - -void ContinentalARS548HwInterface::print_error(std::string error) -{ - if (parent_node_logger_ptr_) { - RCLCPP_ERROR_STREAM((*parent_node_logger_ptr_), error); - } else { - std::cerr << error << std::endl; - } -} - -void ContinentalARS548HwInterface::print_debug(std::string debug) -{ - if (parent_node_logger_ptr_) { - RCLCPP_DEBUG_STREAM((*parent_node_logger_ptr_), debug); - } else { - std::cout << debug << std::endl; - } -} - } // namespace nebula::drivers::continental_ars548 diff --git a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_srr520_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_srr520_hw_interface.cpp index c4b6f3de8..9e6ec2b0d 100644 --- a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_srr520_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_srr520_hw_interface.cpp @@ -17,6 +17,7 @@ #include #include +#include #include #include @@ -27,7 +28,9 @@ namespace nebula::drivers::continental_srr520 { -ContinentalSRR520HwInterface::ContinentalSRR520HwInterface() +ContinentalSRR520HwInterface::ContinentalSRR520HwInterface( + const std::shared_ptr & logger) +: logger_(logger) { } @@ -53,14 +56,14 @@ Status ContinentalSRR520HwInterface::sensor_interface_start() can_receiver_ptr_->SetCanFilters( ::drivers::socketcan::SocketCanReceiver::CanFilterList(config_ptr_->filters)); - print_info(std::string("applied filters: ") + config_ptr_->filters); + logger_->info(std::string("applied filters: ") + config_ptr_->filters); sensor_interface_active_ = true; receiver_thread_ptr_ = std::make_unique(&ContinentalSRR520HwInterface::receive_loop, this); } catch (const std::exception & ex) { Status status = Status::CAN_CONNECTION_ERROR; - std::cerr << status << config_ptr_->interface << std::endl; + logger_->error(std::string("Error connecting to CAN interface: ") + ex.what()); return status; } return Status::OK; @@ -79,7 +82,7 @@ bool ContinentalSRR520HwInterface::send_frame(const std::array & dat std::chrono::duration(config_ptr_->sender_timeout_sec))); return true; } catch (const std::exception & ex) { - print_error(std::string("Error sending CAN message: ") + ex.what()); + logger_->error(std::string("Error sending CAN message: ") + ex.what()); return false; } } @@ -88,7 +91,7 @@ void ContinentalSRR520HwInterface::receive_loop() { ::drivers::socketcan::CanId receive_id{}; std::chrono::nanoseconds receiver_timeout_nsec; - bool use_bus_time; + bool use_bus_time = false; while (true) { auto packet_msg_ptr = std::make_unique(); @@ -109,7 +112,7 @@ void ContinentalSRR520HwInterface::receive_loop() receive_id = can_receiver_ptr_->receive_fd( packet_msg_ptr->data.data() + 4 * sizeof(uint8_t), receiver_timeout_nsec); } catch (const std::exception & ex) { - print_error(std::string("Error receiving CAN FD message: ") + ex.what()); + logger_->error(std::string("Error receiving CAN FD message: ") + ex.what()); continue; } @@ -131,7 +134,7 @@ void ContinentalSRR520HwInterface::receive_loop() packet_msg_ptr->stamp.nanosec = stamp % 1'000'000'000; if (receive_id.frame_type() == ::drivers::socketcan::FrameType::ERROR) { - print_error("CAN FD message is an error frame"); + logger_->error("CAN FD message is an error frame"); continue; } @@ -149,7 +152,7 @@ Status ContinentalSRR520HwInterface::register_packet_callback( void ContinentalSRR520HwInterface::sensor_sync_follow_up(builtin_interfaces::msg::Time stamp) { if (!can_sender_ptr_) { - print_error("Can sender is invalid so can not do follow up"); + logger_->error("Can sender is invalid so can not do follow up"); } if (!config_ptr_->sync_use_bus_time || sync_follow_up_sent_) { @@ -188,12 +191,12 @@ void ContinentalSRR520HwInterface::sensor_sync_follow_up(builtin_interfaces::msg void ContinentalSRR520HwInterface::sensor_sync() { if (!can_sender_ptr_) { - print_error("Can sender is invalid so can not do sync up"); + logger_->error("Can sender is invalid so can not do sync up"); return; } if (!sync_follow_up_sent_) { - print_error("We will send a SYNC message without having sent a FollowUp message first!"); + logger_->error("We will send a SYNC message without having sent a FollowUp message first!"); } auto now = std::chrono::system_clock::now(); @@ -263,14 +266,14 @@ Status ContinentalSRR520HwInterface::configure_sensor( float yaw_autosar, float longitudinal_cog, float wheelbase, float cover_damping, bool plug_bottom, bool reset) { - print_info("longitudinal_autosar=" + std::to_string(longitudinal_autosar)); - print_info("lateral_autosar=" + std::to_string(lateral_autosar)); - print_info("vertical_autosar=" + std::to_string(vertical_autosar)); - print_info("longitudinal_cog=" + std::to_string(longitudinal_cog)); - print_info("wheelbase=" + std::to_string(wheelbase)); - print_info("yaw_autosar=" + std::to_string(yaw_autosar)); - print_info("sensor_id=" + std::to_string(static_cast(sensor_id))); - print_info("plug_bottom=" + std::to_string(plug_bottom)); + logger_->info("longitudinal_autosar=" + std::to_string(longitudinal_autosar)); + logger_->info("lateral_autosar=" + std::to_string(lateral_autosar)); + logger_->info("vertical_autosar=" + std::to_string(vertical_autosar)); + logger_->info("longitudinal_cog=" + std::to_string(longitudinal_cog)); + logger_->info("wheelbase=" + std::to_string(wheelbase)); + logger_->info("yaw_autosar=" + std::to_string(yaw_autosar)); + logger_->info("sensor_id=" + std::to_string(static_cast(sensor_id))); + logger_->info("plug_bottom=" + std::to_string(plug_bottom)); if ( longitudinal_autosar < -32.767f || longitudinal_autosar > 32.767f || @@ -278,7 +281,7 @@ Status ContinentalSRR520HwInterface::configure_sensor( vertical_autosar > 32.767f || longitudinal_cog < -32.767f || longitudinal_cog > 32.767f || wheelbase < 0.f || wheelbase > 65.534f || yaw_autosar < -3.14159f || yaw_autosar > 3.14159f || cover_damping < -32.767f || cover_damping > 32.767f) { - print_error("Sensor configuration values out of range!"); + logger_->error("Sensor configuration values out of range!"); return Status::SENSOR_CONFIG_ERROR; } @@ -332,7 +335,7 @@ Status ContinentalSRR520HwInterface::set_vehicle_dynamics( longitudinal_acceleration < -12.7 || longitudinal_acceleration > 12.7 || lateral_acceleration < -12.7 || lateral_acceleration > 12.7 || yaw_rate < -3.14159 || yaw_rate > 3.14159 || abs(longitudinal_velocity) > 100.0) { - print_error("Vehicle dynamics out of range!"); + logger_->error("Vehicle dynamics out of range!"); return Status::SENSOR_CONFIG_ERROR; } @@ -368,36 +371,4 @@ Status ContinentalSRR520HwInterface::set_vehicle_dynamics( } } -void ContinentalSRR520HwInterface::set_logger(std::shared_ptr logger) -{ - parent_node_logger_ptr_ = logger; -} - -void ContinentalSRR520HwInterface::print_info(std::string info) -{ - if (parent_node_logger_ptr_) { - RCLCPP_INFO_STREAM((*parent_node_logger_ptr_), info); - } else { - std::cout << info << std::endl; - } -} - -void ContinentalSRR520HwInterface::print_error(std::string error) -{ - if (parent_node_logger_ptr_) { - RCLCPP_ERROR_STREAM((*parent_node_logger_ptr_), error); - } else { - std::cerr << error << std::endl; - } -} - -void ContinentalSRR520HwInterface::print_debug(std::string debug) -{ - if (parent_node_logger_ptr_) { - RCLCPP_DEBUG_STREAM((*parent_node_logger_ptr_), debug); - } else { - std::cout << debug << std::endl; - } -} - } // namespace nebula::drivers::continental_srr520 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 17e17a183..1f9ed9bad 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 @@ -234,21 +234,21 @@ Status HesaiHwInterface::get_calibration_configuration( Status HesaiHwInterface::initialize_tcp_driver() { #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "HesaiHwInterface::InitializeTcpDriver" << std::endl; - std::cout << "st: tcp_driver_->init_socket" << std::endl; - std::cout << "sensor_configuration_->sensor_ip=" << sensor_configuration_->sensor_ip << std::endl; - std::cout << "sensor_configuration_->host_ip=" << sensor_configuration_->host_ip << std::endl; - std::cout << "PandarTcpCommandPort=" << PandarTcpCommandPort << std::endl; + logger_->debug("HesaiHwInterface::InitializeTcpDriver"); + logger_->debug("st: tcp_driver_->init_socket"); + logger_->debug("sensor_configuration_->sensor_ip=" + sensor_configuration_->sensor_ip); + logger_->debug("sensor_configuration_->host_ip=" + sensor_configuration_->host_ip); + logger_->debug("PandarTcpCommandPort=" + std::to_string(g_pandar_tcp_command_port)); #endif tcp_driver_->init_socket( sensor_configuration_->sensor_ip, g_pandar_tcp_command_port, sensor_configuration_->host_ip, g_pandar_tcp_command_port); #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "ed: tcp_driver_->init_socket" << std::endl; + logger_->debug("ed: tcp_driver_->init_socket"); #endif if (!tcp_driver_->open()) { #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "!tcp_driver_->open()" << std::endl; + logger_->debug("!tcp_driver_->open()"); #endif // tcp_driver_->close(); tcp_driver_->closeSync(); @@ -278,7 +278,7 @@ boost::property_tree::ptree HesaiHwInterface::parse_json(const std::string & str ss << str; boost::property_tree::read_json(ss, tree); } catch (boost::property_tree::json_parser_error & e) { - std::cerr << e.what() << std::endl; + logger_->error(e.what()); } return tree; } @@ -959,7 +959,7 @@ HesaiStatus HesaiHwInterface::check_and_set_config( { using namespace std::chrono_literals; // NOLINT(build/namespaces) #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "Start CheckAndSetConfig(HesaiConfig)!!" << std::endl; + logger_->debug("Start CheckAndSetConfig(HesaiConfig)!"); #endif const auto hesai_config = hesai_config_ptr->get(); auto current_return_mode = nebula::drivers::return_mode_from_int_hesai( @@ -1155,7 +1155,7 @@ HesaiStatus HesaiHwInterface::check_and_set_config( } #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "End CheckAndSetConfig(HesaiConfig)!!" << std::endl; + logger_->debug("End CheckAndSetConfig(HesaiConfig)!"); #endif logger_->debug("GetAndCheckConfig(HesaiConfig) finished"); @@ -1167,15 +1167,15 @@ HesaiStatus HesaiHwInterface::check_and_set_config( HesaiLidarRangeAll hesai_lidar_range_all) { #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "Start CheckAndSetConfig(HesaiLidarRangeAll)!!" << std::endl; + logger_->debug("Start CheckAndSetConfig(HesaiLidarRangeAll)!"); #endif //* // g_ptc_command_set_lidar_range bool set_flg = false; if (hesai_lidar_range_all.method != 0) { #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "current hesai_lidar_range_all.method: " << hesai_lidar_range_all.method - << std::endl; + logger_->debug( + "current hesai_lidar_range_all.method: " + std::to_string(hesai_lidar_range_all.method)); #endif set_flg = true; } else { @@ -1216,7 +1216,7 @@ HesaiStatus HesaiHwInterface::check_and_set_config( } #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "End CheckAndSetConfig(HesaiLidarRangeAll)!!" << std::endl; + logger_->debug("End CheckAndSetConfig(HesaiLidarRangeAll)!"); #endif return Status::WAITING_FOR_SENSOR_RESPONSE; } @@ -1224,7 +1224,7 @@ HesaiStatus HesaiHwInterface::check_and_set_config( HesaiStatus HesaiHwInterface::check_and_set_config() { #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "Start CheckAndSetConfig!!" << std::endl; + logger_->debug("Start CheckAndSetConfig!"); #endif std::thread t([this] { auto result = get_config(); @@ -1246,7 +1246,7 @@ HesaiStatus HesaiHwInterface::check_and_set_config() }); t2.join(); #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "End CheckAndSetConfig!!" << std::endl; + logger_->debug("End CheckAndSetConfig!"); #endif return Status::OK; } diff --git a/nebula_hw_interfaces/src/nebula_robosense_hw_interfaces/robosense_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_robosense_hw_interfaces/robosense_hw_interface.cpp index c56114278..3b4198fcb 100644 --- a/nebula_hw_interfaces/src/nebula_robosense_hw_interfaces/robosense_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_robosense_hw_interfaces/robosense_hw_interface.cpp @@ -2,17 +2,20 @@ #include "nebula_hw_interfaces/nebula_hw_interfaces_robosense/robosense_hw_interface.hpp" +#include "nebula_common/util/string_conversions.hpp" + #include #include #include #include namespace nebula::drivers { -RobosenseHwInterface::RobosenseHwInterface() +RobosenseHwInterface::RobosenseHwInterface(const std::shared_ptr & logger) : cloud_io_context_(new ::drivers::common::IoContext(1)), info_io_context_(new ::drivers::common::IoContext(1)), cloud_udp_driver_(new ::drivers::udp_driver::UdpDriver(*cloud_io_context_)), - info_udp_driver_(new ::drivers::udp_driver::UdpDriver(*info_io_context_)) + info_udp_driver_(new ::drivers::udp_driver::UdpDriver(*info_io_context_)), + logger_(logger) { } @@ -37,7 +40,9 @@ void RobosenseHwInterface::receive_info_packet_callback(std::vector & b Status RobosenseHwInterface::sensor_interface_start() { try { - std::cout << "Starting UDP server for data packets on: " << *sensor_configuration_ << std::endl; + logger_->info( + "Starting UDP server for data packets on: " + sensor_configuration_->sensor_ip + ": " + + std::to_string(sensor_configuration_->data_port)); cloud_udp_driver_->init_receiver( sensor_configuration_->host_ip, sensor_configuration_->data_port); cloud_udp_driver_->receiver()->open(); @@ -48,8 +53,9 @@ Status RobosenseHwInterface::sensor_interface_start() &RobosenseHwInterface::receive_sensor_packet_callback, this, std::placeholders::_1)); } catch (const std::exception & ex) { Status status = Status::UDP_CONNECTION_ERROR; - std::cerr << status << sensor_configuration_->sensor_ip << "," - << sensor_configuration_->data_port << std::endl; + logger_->error( + util::to_string(status) + " " + sensor_configuration_->sensor_ip + "," + + std::to_string(sensor_configuration_->data_port)); return status; } return Status::OK; @@ -58,8 +64,7 @@ Status RobosenseHwInterface::sensor_interface_start() Status RobosenseHwInterface::info_interface_start() { try { - std::cout << "Starting UDP server for info packets on: " << *sensor_configuration_ << std::endl; - print_info( + logger_->info( "Starting UDP server for info packets on: " + sensor_configuration_->sensor_ip + ": " + std::to_string(sensor_configuration_->gnss_port)); info_udp_driver_->init_receiver( @@ -71,8 +76,9 @@ Status RobosenseHwInterface::info_interface_start() std::bind(&RobosenseHwInterface::receive_info_packet_callback, this, std::placeholders::_1)); } catch (const std::exception & ex) { Status status = Status::UDP_CONNECTION_ERROR; - std::cerr << status << sensor_configuration_->sensor_ip << "," - << sensor_configuration_->gnss_port << std::endl; + logger_->error( + util::to_string(status) + " " + sensor_configuration_->sensor_ip + "," + + std::to_string(sensor_configuration_->gnss_port)); return status; } @@ -106,28 +112,4 @@ Status RobosenseHwInterface::register_info_callback( info_reception_callback_ = std::move(info_callback); return Status::OK; } - -void RobosenseHwInterface::print_debug(std::string debug) -{ - if (parent_node_logger_) { - RCLCPP_DEBUG_STREAM((*parent_node_logger_), debug); - } else { - std::cout << debug << std::endl; - } -} - -void RobosenseHwInterface::print_info(std::string info) -{ - if (parent_node_logger_) { - RCLCPP_INFO_STREAM((*parent_node_logger_), info); - } else { - std::cout << info << std::endl; - } -} - -void RobosenseHwInterface::set_logger(std::shared_ptr logger) -{ - parent_node_logger_ = logger; -} - } // namespace nebula::drivers diff --git a/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp index 1b6276f1b..50455c3bb 100644 --- a/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp @@ -2,6 +2,8 @@ #include "nebula_hw_interfaces/nebula_hw_interfaces_velodyne/velodyne_hw_interface.hpp" +#include "nebula_common/util/string_conversions.hpp" + #include #include #include @@ -9,11 +11,14 @@ namespace nebula::drivers { -VelodyneHwInterface::VelodyneHwInterface() +using std::string_literals::operator""s; + +VelodyneHwInterface::VelodyneHwInterface(const std::shared_ptr & logger) : cloud_io_context_{new ::drivers::common::IoContext(1)}, cloud_udp_driver_{new ::drivers::udp_driver::UdpDriver(*cloud_io_context_)}, boost_ctx_{new boost::asio::io_context()}, - http_client_driver_{new ::drivers::tcp_driver::HttpClientDriver(boost_ctx_)} + http_client_driver_{new ::drivers::tcp_driver::HttpClientDriver(boost_ctx_)}, + logger_{logger} { } @@ -66,8 +71,9 @@ Status VelodyneHwInterface::sensor_interface_start() std::bind(&VelodyneHwInterface::receive_sensor_packet_callback, this, std::placeholders::_1)); } catch (const std::exception & ex) { Status status = Status::UDP_CONNECTION_ERROR; - std::cerr << status << sensor_configuration_->sensor_ip << "," - << sensor_configuration_->data_port << std::endl; + logger_->error( + util::to_string(status) + " " + sensor_configuration_->sensor_ip + "," + + std::to_string(sensor_configuration_->data_port)); return status; } return Status::OK; @@ -97,7 +103,7 @@ Status VelodyneHwInterface::get_sensor_configuration(SensorConfigurationBase & s { std::stringstream ss; ss << sensor_configuration; - print_debug(ss.str()); + logger_->debug(ss.str()); return Status::ERROR_1; } @@ -117,7 +123,7 @@ VelodyneStatus VelodyneHwInterface::init_http_client() void VelodyneHwInterface::string_callback(const std::string & str) { - std::cout << "VelodyneHwInterface::string_callback: " << str << std::endl; + logger_->debug("VelodyneHwInterface::string_callback: " + str); } boost::property_tree::ptree VelodyneHwInterface::parse_json(const std::string & str) @@ -128,7 +134,7 @@ boost::property_tree::ptree VelodyneHwInterface::parse_json(const std::string & ss << str; boost::property_tree::read_json(ss, tree); } catch (boost::property_tree::json_parser_error & e) { - std::cerr << "Error on ParseJson: " << e.what() << std::endl; + logger_->error("Error on ParseJson: "s + e.what()); } return tree; } @@ -148,11 +154,11 @@ VelodyneStatus VelodyneHwInterface::check_and_set_config( status = set_return_type(sensor_configuration->return_mode); if (status != ok) return status; - std::cout << "VelodyneHwInterface::parse_json(" << target_key - << "): " << current_return_mode_str << std::endl; - std::cout << "current_return_mode: " << current_return_mode << std::endl; - std::cout << "sensor_configuration->return_mode: " << sensor_configuration->return_mode - << std::endl; + logger_->debug( + "VelodyneHwInterface::parse_json(" + target_key + "): " + current_return_mode_str); + logger_->debug("current_return_mode: " + util::to_string(current_return_mode)); + logger_->debug( + "sensor_configuration->return_mode: " + util::to_string(sensor_configuration->return_mode)); } target_key = "config.rpm"; @@ -161,10 +167,12 @@ VelodyneStatus VelodyneHwInterface::check_and_set_config( status = set_rpm(sensor_configuration->rotation_speed); if (status != ok) return status; - std::cout << "VelodyneHwInterface::parse_json(" << target_key << "): " << current_rotation_speed - << std::endl; - std::cout << "sensor_configuration->rotation_speed: " << sensor_configuration->rotation_speed - << std::endl; + logger_->debug( + "VelodyneHwInterface::parse_json(" + target_key + + "): " + std::to_string(current_rotation_speed)); + logger_->debug( + "sensor_configuration->rotation_speed: " + + std::to_string(sensor_configuration->rotation_speed)); } target_key = "config.fov.start"; @@ -178,9 +186,11 @@ VelodyneStatus VelodyneHwInterface::check_and_set_config( status = set_fov_start(setting_cloud_min_angle); if (status != ok) return status; - std::cout << "VelodyneHwInterface::parse_json(" << target_key - << "): " << current_cloud_min_angle << std::endl; - std::cout << "sensor_configuration->cloud_min_angle: " << setting_cloud_min_angle << std::endl; + logger_->debug( + "VelodyneHwInterface::parse_json(" + target_key + + "): " + std::to_string(current_cloud_min_angle)); + logger_->debug( + "sensor_configuration->cloud_min_angle: " + std::to_string(setting_cloud_min_angle)); } target_key = "config.fov.end"; @@ -194,9 +204,11 @@ VelodyneStatus VelodyneHwInterface::check_and_set_config( status = set_fov_end(setting_cloud_max_angle); if (status != ok) return status; - std::cout << "VelodyneHwInterface::parse_json(" << target_key - << "): " << current_cloud_max_angle << std::endl; - std::cout << "sensor_configuration->cloud_max_angle: " << setting_cloud_max_angle << std::endl; + logger_->debug( + "VelodyneHwInterface::parse_json(" + target_key + + "): " + std::to_string(current_cloud_max_angle)); + logger_->debug( + "sensor_configuration->cloud_max_angle: " + std::to_string(setting_cloud_max_angle)); } target_key = "config.host.addr"; @@ -205,9 +217,8 @@ VelodyneStatus VelodyneHwInterface::check_and_set_config( status = set_host_addr(sensor_configuration->host_ip); if (status != ok) return status; - std::cout << "VelodyneHwInterface::parse_json(" << target_key << "): " << current_host_addr - << std::endl; - std::cout << "sensor_configuration->host_ip: " << sensor_configuration->host_ip << std::endl; + logger_->debug("VelodyneHwInterface::parse_json(" + target_key + "): " + current_host_addr); + logger_->debug("sensor_configuration->host_ip: " + sensor_configuration->host_ip); } target_key = "config.host.dport"; @@ -216,10 +227,10 @@ VelodyneStatus VelodyneHwInterface::check_and_set_config( status = set_host_dport(sensor_configuration->data_port); if (status != ok) return status; - std::cout << "VelodyneHwInterface::parse_json(" << target_key << "): " << current_host_dport - << std::endl; - std::cout << "sensor_configuration->data_port: " << sensor_configuration->data_port - << std::endl; + logger_->debug( + "VelodyneHwInterface::parse_json(" + target_key + "): " + std::to_string(current_host_dport)); + logger_->debug( + "sensor_configuration->data_port: " + std::to_string(sensor_configuration->data_port)); } target_key = "config.host.tport"; @@ -228,10 +239,10 @@ VelodyneStatus VelodyneHwInterface::check_and_set_config( status = set_host_tport(sensor_configuration->gnss_port); if (status != ok) return status; - std::cout << "VelodyneHwInterface::parse_json(" << target_key << "): " << current_host_tport - << std::endl; - std::cout << "sensor_configuration->gnss_port: " << sensor_configuration->gnss_port - << std::endl; + logger_->debug( + "VelodyneHwInterface::parse_json(" + target_key + "): " + std::to_string(current_host_tport)); + logger_->debug( + "sensor_configuration->gnss_port: " + std::to_string(sensor_configuration->gnss_port)); } return ok; @@ -248,7 +259,7 @@ nebula::util::expected VelodyneHwInterface::get_dia { auto response = http_get_request(target_diag_); if (response.has_value()) { - std::cout << "read_response: " << response.value() << std::endl; + logger_->debug("read_response: " + response.value()); } return response; } @@ -447,36 +458,4 @@ VelodyneStatus VelodyneHwInterface::set_net_dhcp(bool use_dhcp) return Status::OK; } -void VelodyneHwInterface::set_logger(std::shared_ptr logger) -{ - parent_node_logger_ = logger; -} - -void VelodyneHwInterface::print_info(std::string info) -{ - if (parent_node_logger_) { - RCLCPP_INFO_STREAM((*parent_node_logger_), info); - } else { - std::cout << info << std::endl; - } -} - -void VelodyneHwInterface::print_error(std::string error) -{ - if (parent_node_logger_) { - RCLCPP_ERROR_STREAM((*parent_node_logger_), error); - } else { - std::cerr << error << std::endl; - } -} - -void VelodyneHwInterface::print_debug(std::string debug) -{ - if (parent_node_logger_) { - RCLCPP_DEBUG_STREAM((*parent_node_logger_), debug); - } else { - std::cout << debug << std::endl; - } -} - } // namespace nebula::drivers diff --git a/nebula_ros/src/continental/continental_ars548_hw_interface_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_hw_interface_wrapper.cpp index 312e3f222..d2e1f1554 100644 --- a/nebula_ros/src/continental/continental_ars548_hw_interface_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_hw_interface_wrapper.cpp @@ -14,6 +14,8 @@ #include "nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp" +#include "nebula_ros/common/rclcpp_logger.hpp" + #include #include @@ -30,7 +32,8 @@ ContinentalARS548HwInterfaceWrapper::ContinentalARS548HwInterfaceWrapper( config_ptr) : parent_node_(parent_node), hw_interface_( - std::make_shared()), + std::make_shared( + drivers::loggers::RclcppLogger(parent_node->get_logger()).child("HwInterface"))), logger_(parent_node->get_logger().get_child("HwInterfaceWrapper")), status_(Status::NOT_INITIALIZED), config_ptr_(config_ptr), @@ -44,8 +47,6 @@ ContinentalARS548HwInterfaceWrapper::ContinentalARS548HwInterfaceWrapper( nebula::drivers::continental_ars548::min_odometry_hz, nebula::drivers::continental_ars548::max_odometry_hz, 100) { - hw_interface_->set_logger( - std::make_shared(parent_node->get_logger().get_child("HwInterface"))); status_ = hw_interface_->set_sensor_configuration(config_ptr); if (status_ != Status::OK) { diff --git a/nebula_ros/src/continental/continental_srr520_hw_interface_wrapper.cpp b/nebula_ros/src/continental/continental_srr520_hw_interface_wrapper.cpp index b38a1f7dc..75bcf4354 100644 --- a/nebula_ros/src/continental/continental_srr520_hw_interface_wrapper.cpp +++ b/nebula_ros/src/continental/continental_srr520_hw_interface_wrapper.cpp @@ -14,6 +14,8 @@ #include "nebula_ros/continental/continental_srr520_hw_interface_wrapper.hpp" +#include "nebula_ros/common/rclcpp_logger.hpp" + #include #include @@ -31,13 +33,12 @@ ContinentalSRR520HwInterfaceWrapper::ContinentalSRR520HwInterfaceWrapper( config_ptr) : parent_node_(parent_node), hw_interface_( - std::make_shared()), + std::make_shared( + drivers::loggers::RclcppLogger(parent_node->get_logger()).child("HwInterface"))), logger_(parent_node->get_logger().get_child("HwInterfaceWrapper")), status_(Status::NOT_INITIALIZED), config_ptr_(config_ptr) { - hw_interface_->set_logger( - std::make_shared(parent_node->get_logger().get_child("HwInterface"))); status_ = hw_interface_->set_sensor_configuration(config_ptr_); if (status_ != Status::OK) { diff --git a/nebula_ros/src/robosense/hw_interface_wrapper.cpp b/nebula_ros/src/robosense/hw_interface_wrapper.cpp index 26e6e1e15..60a658858 100644 --- a/nebula_ros/src/robosense/hw_interface_wrapper.cpp +++ b/nebula_ros/src/robosense/hw_interface_wrapper.cpp @@ -2,6 +2,8 @@ #include "nebula_ros/robosense/hw_interface_wrapper.hpp" +#include "nebula_ros/common/rclcpp_logger.hpp" + #include #include @@ -11,12 +13,12 @@ namespace nebula::ros RobosenseHwInterfaceWrapper::RobosenseHwInterfaceWrapper( rclcpp::Node * const parent_node, std::shared_ptr & config) -: hw_interface_(new nebula::drivers::RobosenseHwInterface()), +: hw_interface_( + std::make_shared( + drivers::loggers::RclcppLogger(parent_node->get_logger()).child("HwInterface"))), logger_(parent_node->get_logger().get_child("HwInterfaceWrapper")), status_(nebula::Status::NOT_INITIALIZED) { - hw_interface_->set_logger( - std::make_shared(parent_node->get_logger().get_child("HwInterface"))); status_ = hw_interface_->set_sensor_configuration(config); if (Status::OK != status_) { diff --git a/nebula_ros/src/velodyne/hw_interface_wrapper.cpp b/nebula_ros/src/velodyne/hw_interface_wrapper.cpp index 4e2b7202d..b31f61463 100644 --- a/nebula_ros/src/velodyne/hw_interface_wrapper.cpp +++ b/nebula_ros/src/velodyne/hw_interface_wrapper.cpp @@ -2,6 +2,8 @@ #include "nebula_ros/velodyne/hw_interface_wrapper.hpp" +#include "nebula_ros/common/rclcpp_logger.hpp" + #include #include @@ -12,15 +14,15 @@ namespace nebula::ros VelodyneHwInterfaceWrapper::VelodyneHwInterfaceWrapper( rclcpp::Node * const parent_node, std::shared_ptr & config, bool use_udp_only) -: hw_interface_(new nebula::drivers::VelodyneHwInterface()), +: 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) { setup_sensor_ = parent_node->declare_parameter("setup_sensor", param_read_only()); - hw_interface_->set_logger( - std::make_shared(parent_node->get_logger().get_child("HwInterface"))); status_ = hw_interface_->initialize_sensor_configuration(config); if (status_ != Status::OK) {