From 702ae97ae145d03c2194800aa9dadae467297825 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Fri, 4 Apr 2025 12:57:14 +0900 Subject: [PATCH] chore: changed cerrs to loggers and forced flush to couts Signed-off-by: Kenzo Lobos-Tsunekawa --- .../decoders/continental_srr520_decoder.cpp | 6 ++--- .../robosense_hw_interface.hpp | 4 ++++ .../continental_ars548_hw_interface.cpp | 10 ++++---- .../continental_srr520_hw_interface.cpp | 10 ++++---- .../hesai_hw_interface.cpp | 4 +++- .../robosense_hw_interface.cpp | 24 +++++++++++++------ .../velodyne_hw_interface.cpp | 16 ++++++++----- 7 files changed, 49 insertions(+), 25 deletions(-) diff --git a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp index a5362f6f6..fe0363c88 100644 --- a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp @@ -1183,7 +1183,7 @@ void ContinentalSRR520Decoder::print_info(std::string info) if (parent_node_logger_ptr_) { RCLCPP_INFO_STREAM((*parent_node_logger_ptr_), info); } else { - std::cout << info << std::endl; + std::cout << info << std::endl << std::flush; } } @@ -1192,7 +1192,7 @@ void ContinentalSRR520Decoder::print_error(std::string error) if (parent_node_logger_ptr_) { RCLCPP_ERROR_STREAM((*parent_node_logger_ptr_), error); } else { - std::cerr << error << std::endl; + std::cerr << error << std::endl << std::flush; } } @@ -1201,7 +1201,7 @@ void ContinentalSRR520Decoder::print_debug(std::string debug) if (parent_node_logger_ptr_) { RCLCPP_DEBUG_STREAM((*parent_node_logger_ptr_), debug); } else { - std::cout << debug << std::endl; + std::cout << debug << std::endl << std::flush; } } 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..82cc7850d 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 @@ -56,6 +56,10 @@ class RobosenseHwInterface 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_ERROR_STREAM + /// @param info Target string + void print_error(std::string info); + /// @brief Printing the string to RCLCPP_INFO_STREAM /// @param info Target string void print_info(std::string info); 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..24266e2da 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 @@ -64,7 +64,9 @@ 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; + std::stringstream ss; + ss << status << config_ptr_->sensor_ip << "," << config_ptr_->data_port; + print_error(ss.str()); return status; } return Status::OK; @@ -523,7 +525,7 @@ 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; + std::cout << info << std::endl << std::flush; } } @@ -532,7 +534,7 @@ 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; + std::cerr << error << std::endl << std::flush; } } @@ -541,7 +543,7 @@ 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; + std::cout << debug << std::endl << std::flush; } } 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..8b1f28e25 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 @@ -60,7 +60,9 @@ Status ContinentalSRR520HwInterface::sensor_interface_start() 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; + std::stringstream ss; + ss << status << config_ptr_->interface; + print_error(ss.str()); return status; } return Status::OK; @@ -378,7 +380,7 @@ 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; + std::cout << info << std::endl << std::flush; } } @@ -387,7 +389,7 @@ 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; + std::cerr << error << std::endl << std::flush; } } @@ -396,7 +398,7 @@ 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; + std::cout << debug << std::endl << std::flush; } } 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..55ecbac31 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 @@ -278,7 +278,9 @@ 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; + std::stringstream ss; + ss << e.what(); + logger_->error(ss.str()); } return tree; } 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..7c30264b7 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 @@ -48,8 +48,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; + std::stringstream ss; + ss << status << sensor_configuration_->sensor_ip << "," << sensor_configuration_->data_port; + print_error(ss.str()); return status; } return Status::OK; @@ -58,7 +59,6 @@ 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( "Starting UDP server for info packets on: " + sensor_configuration_->sensor_ip + ": " + std::to_string(sensor_configuration_->gnss_port)); @@ -71,8 +71,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; + std::stringstream ss; + ss << status << sensor_configuration_->sensor_ip << "," << sensor_configuration_->gnss_port; + print_error(ss.str()); return status; } @@ -112,7 +113,7 @@ void RobosenseHwInterface::print_debug(std::string debug) if (parent_node_logger_) { RCLCPP_DEBUG_STREAM((*parent_node_logger_), debug); } else { - std::cout << debug << std::endl; + std::cout << debug << std::endl << std::flush; } } @@ -121,7 +122,16 @@ void RobosenseHwInterface::print_info(std::string info) if (parent_node_logger_) { RCLCPP_INFO_STREAM((*parent_node_logger_), info); } else { - std::cout << info << std::endl; + std::cout << info << std::endl << std::flush; + } +} + +void RobosenseHwInterface::print_error(std::string info) +{ + if (parent_node_logger_) { + RCLCPP_ERROR_STREAM((*parent_node_logger_), info); + } else { + std::cerr << info << std::endl << std::flush; } } 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..44085f7fc 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 @@ -66,8 +66,10 @@ 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; + std::stringstream ss; + ss << status << sensor_configuration_->sensor_ip << "," << sensor_configuration_->data_port; + + print_error(ss.str()); return status; } return Status::OK; @@ -128,7 +130,9 @@ 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; + std::stringstream ss; + ss << "Error on ParseJson: " << e.what(); + print_error(ss.str()); } return tree; } @@ -457,7 +461,7 @@ void VelodyneHwInterface::print_info(std::string info) if (parent_node_logger_) { RCLCPP_INFO_STREAM((*parent_node_logger_), info); } else { - std::cout << info << std::endl; + std::cout << info << std::endl << std::flush; } } @@ -466,7 +470,7 @@ void VelodyneHwInterface::print_error(std::string error) if (parent_node_logger_) { RCLCPP_ERROR_STREAM((*parent_node_logger_), error); } else { - std::cerr << error << std::endl; + std::cerr << error << std::endl << std::flush; } } @@ -475,7 +479,7 @@ void VelodyneHwInterface::print_debug(std::string debug) if (parent_node_logger_) { RCLCPP_DEBUG_STREAM((*parent_node_logger_), debug); } else { - std::cout << debug << std::endl; + std::cout << debug << std::endl << std::flush; } }