Skip to content
Closed
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
}

Expand All @@ -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;
}
}

Expand All @@ -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;
}
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,10 @@ class RobosenseHwInterface
info_reception_callback_; /**This function pointer is called when DIFOP packet is received*/
std::shared_ptr<rclcpp::Logger> 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);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
}
}

Expand All @@ -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;
}
}

Expand All @@ -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;
}
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -60,7 +60,9 @@ Status ContinentalSRR520HwInterface::sensor_interface_start()
std::make_unique<std::thread>(&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;
Expand Down Expand Up @@ -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;
}
}

Expand All @@ -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;
}
}

Expand All @@ -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;
}
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -48,8 +48,9 @@
&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;
Expand All @@ -58,7 +59,6 @@
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));
Expand All @@ -71,8 +71,9 @@
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;
}

Expand Down Expand Up @@ -112,7 +113,7 @@
if (parent_node_logger_) {
RCLCPP_DEBUG_STREAM((*parent_node_logger_), debug);
} else {
std::cout << debug << std::endl;
std::cout << debug << std::endl << std::flush;
}
}

Expand All @@ -121,7 +122,16 @@
if (parent_node_logger_) {
RCLCPP_INFO_STREAM((*parent_node_logger_), info);
} else {
std::cout << info << std::endl;
std::cout << info << std::endl << std::flush;
}
}

Check warning on line 127 in nebula_hw_interfaces/src/nebula_robosense_hw_interfaces/robosense_hw_interface.cpp

View check run for this annotation

Codecov / codecov/patch

nebula_hw_interfaces/src/nebula_robosense_hw_interfaces/robosense_hw_interface.cpp#L127

Added line #L127 was not covered by tests

void RobosenseHwInterface::print_error(std::string info)

Check warning on line 129 in nebula_hw_interfaces/src/nebula_robosense_hw_interfaces/robosense_hw_interface.cpp

View check run for this annotation

Codecov / codecov/patch

nebula_hw_interfaces/src/nebula_robosense_hw_interfaces/robosense_hw_interface.cpp#L129

Added line #L129 was not covered by tests
{
if (parent_node_logger_) {
RCLCPP_ERROR_STREAM((*parent_node_logger_), info);
} else {
std::cerr << info << std::endl << std::flush;
}
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
}
Expand Down Expand Up @@ -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;
}
}

Expand All @@ -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;
}
}

Expand All @@ -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;
}
}

Expand Down