diff --git a/nebula_common/include/nebula_common/continental/continental_ars548.hpp b/nebula_common/include/nebula_common/continental/continental_ars548.hpp index 06bdb3789..09cf8ef6b 100644 --- a/nebula_common/include/nebula_common/continental/continental_ars548.hpp +++ b/nebula_common/include/nebula_common/continental/continental_ars548.hpp @@ -15,7 +15,6 @@ #pragma once #include "nebula_common/nebula_common.hpp" -#include "nebula_common/nebula_status.hpp" #include #include @@ -24,9 +23,7 @@ #include #include -#include #include -#include namespace nebula { @@ -58,15 +55,18 @@ struct ContinentalARS548SensorConfiguration : EthernetSensorConfigurationBase inline std::ostream & operator<<( std::ostream & os, ContinentalARS548SensorConfiguration const & arg) { - os << (EthernetSensorConfigurationBase)(arg) << ", MulticastIP: " << arg.multicast_ip - << ", BaseFrame: " << arg.base_frame << ", ObjectFrame: " << arg.object_frame - << ", ConfigurationHostPort: " << arg.configuration_host_port - << ", ConfigurationSensorPort: " << arg.configuration_sensor_port - << ", UseSensorTime: " << arg.use_sensor_time - << ", ConfigurationVehicleLength: " << arg.configuration_vehicle_length - << ", ConfigurationVehicleWidth: " << arg.configuration_vehicle_width - << ", ConfigurationVehicleHeight: " << arg.configuration_vehicle_height - << ", ConfigurationVehicleWheelbase: " << arg.configuration_vehicle_wheelbase; + os << "ContinentalARS548SensorConfiguration:" << '\n'; + os << (EthernetSensorConfigurationBase)(arg) << '\n'; + os << "MulticastIP: " << arg.multicast_ip << '\n'; + os << "BaseFrame: " << arg.base_frame << '\n'; + os << "ObjectFrame: " << arg.object_frame << '\n'; + os << "ConfigurationHostPort: " << arg.configuration_host_port << '\n'; + os << "ConfigurationSensorPort: " << arg.configuration_sensor_port << '\n'; + os << "UseSensorTime: " << arg.use_sensor_time << '\n'; + os << "ConfigurationVehicleLength: " << arg.configuration_vehicle_length << '\n'; + os << "ConfigurationVehicleWidth: " << arg.configuration_vehicle_width << '\n'; + os << "ConfigurationVehicleHeight: " << arg.configuration_vehicle_height << '\n'; + os << "ConfigurationVehicleWheelbase: " << arg.configuration_vehicle_wheelbase; return os; } diff --git a/nebula_common/include/nebula_common/continental/continental_srr520.hpp b/nebula_common/include/nebula_common/continental/continental_srr520.hpp index ca75e6944..cab7e9178 100644 --- a/nebula_common/include/nebula_common/continental/continental_srr520.hpp +++ b/nebula_common/include/nebula_common/continental/continental_srr520.hpp @@ -15,21 +15,16 @@ #pragma once #include "nebula_common/nebula_common.hpp" -#include "nebula_common/nebula_status.hpp" #include #include #include -#include #include -#include #include #include -#include #include -#include #include namespace nebula @@ -55,9 +50,11 @@ struct ContinentalSRR520SensorConfiguration : CANSensorConfigurationBase inline std::ostream & operator<<( std::ostream & os, ContinentalSRR520SensorConfiguration const & arg) { - os << (CANSensorConfigurationBase)(arg) << ", BaseFrame: " << arg.base_frame - << ", SyncUseBusTime: " << arg.sync_use_bus_time - << ", ConfigurationVehicleWheelbase: " << arg.configuration_vehicle_wheelbase; + os << "ContinentalSRR520SensorConfiguration:" << '\n'; + os << (CANSensorConfigurationBase)(arg) << '\n'; + os << "BaseFrame: " << arg.base_frame << '\n'; + os << "SyncUseBusTime: " << arg.sync_use_bus_time << '\n'; + os << "ConfigurationVehicleWheelbase: " << arg.configuration_vehicle_wheelbase; return os; } diff --git a/nebula_common/include/nebula_common/hesai/hesai_common.hpp b/nebula_common/include/nebula_common/hesai/hesai_common.hpp index 2017bbf4f..7f8a081b8 100644 --- a/nebula_common/include/nebula_common/hesai/hesai_common.hpp +++ b/nebula_common/include/nebula_common/hesai/hesai_common.hpp @@ -35,6 +35,7 @@ struct HesaiSensorConfiguration : public LidarConfigurationBase uint16_t gnss_port{}; double scan_phase{}; double dual_return_distance_threshold{}; + std::string calibration_path{}; uint16_t rotation_speed; uint16_t cloud_min_angle; uint16_t cloud_max_angle; @@ -49,13 +50,18 @@ struct HesaiSensorConfiguration : public LidarConfigurationBase /// @return stream inline std::ostream & operator<<(std::ostream & os, HesaiSensorConfiguration const & arg) { - os << (LidarConfigurationBase)(arg) << ", GnssPort: " << arg.gnss_port - << ", ScanPhase:" << arg.scan_phase << ", RotationSpeed:" << arg.rotation_speed - << ", FOV(Start):" << arg.cloud_min_angle << ", FOV(End):" << arg.cloud_max_angle - << ", DualReturnDistanceThreshold:" << arg.dual_return_distance_threshold - << ", PtpProfile:" << arg.ptp_profile << ", PtpDomain:" << std::to_string(arg.ptp_domain) - << ", PtpTransportType:" << arg.ptp_transport_type - << ", PtpSwitchType:" << arg.ptp_switch_type; + os << "HesaiSensorConfiguration:" << '\n'; + os << (LidarConfigurationBase)(arg) << '\n'; + os << "GnssPort: " << arg.gnss_port << '\n'; + os << "ScanPhase: " << arg.scan_phase << '\n'; + os << "RotationSpeed: " << arg.rotation_speed << '\n'; + os << "FOV(Start): " << arg.cloud_min_angle << '\n'; + os << "FOV(End): " << arg.cloud_max_angle << '\n'; + os << "DualReturnDistanceThreshold: " << arg.dual_return_distance_threshold << '\n'; + os << "PtpProfile: " << arg.ptp_profile << '\n'; + os << "PtpDomain: " << std::to_string(arg.ptp_domain) << '\n'; + os << "PtpTransportType: " << arg.ptp_transport_type << '\n'; + os << "PtpSwitchType: " << arg.ptp_switch_type; return os; } @@ -106,7 +112,7 @@ struct HesaiCalibrationConfiguration : public HesaiCalibrationConfigurationBase std::vector actual_tokens(tok.begin(), tok.end()); if (actual_tokens.size() < expected_cols || actual_tokens.size() > expected_cols) { - std::cerr << "Ignoring line with unexpected data:" << line << std::endl; + std::cerr << "Ignoring line with unexpected data: " << line << std::endl; continue; } @@ -318,10 +324,10 @@ struct HesaiCorrection : public HesaiCalibrationConfigurationBase inline nebula::Status SaveToFileFromBytes( const std::string & correction_file, const std::vector & buf) override { - std::cerr << "Saving in:" << correction_file << "\n"; + std::cerr << "Saving in: " << correction_file << "\n"; std::ofstream ofs(correction_file, std::ios::trunc | std::ios::binary); if (!ofs) { - std::cerr << "Could not create file:" << correction_file << "\n"; + std::cerr << "Could not create file: " << correction_file << "\n"; return Status::CANNOT_SAVE_FILE; } std::cerr << "Writing start...." << buf.size() << "\n"; diff --git a/nebula_common/include/nebula_common/nebula_common.hpp b/nebula_common/include/nebula_common/nebula_common.hpp index 747946255..aa8ba175a 100644 --- a/nebula_common/include/nebula_common/nebula_common.hpp +++ b/nebula_common/include/nebula_common/nebula_common.hpp @@ -20,7 +20,6 @@ #include #include -#include #include #include #include @@ -498,7 +497,8 @@ struct LidarConfigurationBase : EthernetSensorConfigurationBase /// @return stream inline std::ostream & operator<<(std::ostream & os, SensorConfigurationBase const & arg) { - os << "SensorModel: " << arg.sensor_model << ", FrameID: " << arg.frame_id; + os << "SensorModel: " << arg.sensor_model << '\n'; + os << "FrameID: " << arg.frame_id; return os; } @@ -508,8 +508,10 @@ inline std::ostream & operator<<(std::ostream & os, SensorConfigurationBase cons /// @return stream inline std::ostream & operator<<(std::ostream & os, EthernetSensorConfigurationBase const & arg) { - os << (SensorConfigurationBase)(arg) << ", HostIP: " << arg.host_ip - << ", SensorIP: " << arg.sensor_ip << ", DataPort: " << arg.data_port; + os << (SensorConfigurationBase)(arg) << '\n'; + os << "HostIP: " << arg.host_ip << '\n'; + os << "SensorIP: " << arg.sensor_ip << '\n'; + os << "DataPort: " << arg.data_port; return os; } @@ -519,10 +521,12 @@ inline std::ostream & operator<<(std::ostream & os, EthernetSensorConfigurationB /// @return stream inline std::ostream & operator<<(std::ostream & os, CANSensorConfigurationBase const & arg) { - os << (SensorConfigurationBase)(arg) - << ", Interface: " << arg.interface << ", ReceiverTimeoutSec: " << arg.receiver_timeout_sec - << ", SenderTimeoutSec: " << arg.sender_timeout_sec << ", Filters: " << arg.filters - << ", UseBusTime: " << arg.use_bus_time; + os << (SensorConfigurationBase)(arg) << '\n'; + os << "Interface: " << arg.interface << '\n'; + os << "ReceiverTimeoutSec: " << arg.receiver_timeout_sec << '\n'; + os << "SenderTimeoutSec: " << arg.sender_timeout_sec << '\n'; + os << "Filters: " << arg.filters << '\n'; + os << "UseBusTime: " << arg.use_bus_time; return os; } @@ -533,9 +537,11 @@ inline std::ostream & operator<<(std::ostream & os, CANSensorConfigurationBase c inline std::ostream & operator<<( std::ostream & os, nebula::drivers::LidarConfigurationBase const & arg) { - os << (EthernetSensorConfigurationBase)(arg) << ", ReturnMode: " << arg.return_mode - << ", Frequency: " << arg.frequency_ms << ", MTU: " << arg.packet_mtu_size - << ", Use sensor time: " << arg.use_sensor_time; + os << (EthernetSensorConfigurationBase)(arg) << '\n'; + os << "ReturnMode: " << arg.return_mode << '\n'; + os << "Frequency: " << arg.frequency_ms << '\n'; + os << "MTU: " << arg.packet_mtu_size << '\n'; + os << "Use sensor time: " << arg.use_sensor_time; return os; } diff --git a/nebula_common/include/nebula_common/robosense/robosense_common.hpp b/nebula_common/include/nebula_common/robosense/robosense_common.hpp index 8541f8f90..575a7d700 100644 --- a/nebula_common/include/nebula_common/robosense/robosense_common.hpp +++ b/nebula_common/include/nebula_common/robosense/robosense_common.hpp @@ -46,8 +46,10 @@ struct RobosenseSensorConfiguration : LidarConfigurationBase /// @return stream inline std::ostream & operator<<(std::ostream & os, RobosenseSensorConfiguration const & arg) { - os << (LidarConfigurationBase)(arg) << ", GnssPort: " << arg.gnss_port - << ", ScanPhase:" << arg.scan_phase; + os << "RobosenseSensorConfiguration:" << '\n'; + os << (LidarConfigurationBase)(arg) << '\n'; + os << "GnssPort: " << arg.gnss_port << '\n'; + os << "ScanPhase: " << arg.scan_phase; return os; } diff --git a/nebula_common/include/nebula_common/velodyne/velodyne_common.hpp b/nebula_common/include/nebula_common/velodyne/velodyne_common.hpp index 22265780b..dc9c1e453 100644 --- a/nebula_common/include/nebula_common/velodyne/velodyne_common.hpp +++ b/nebula_common/include/nebula_common/velodyne/velodyne_common.hpp @@ -39,9 +39,13 @@ struct VelodyneSensorConfiguration : LidarConfigurationBase /// @return stream inline std::ostream & operator<<(std::ostream & os, VelodyneSensorConfiguration const & arg) { - os << (LidarConfigurationBase)(arg) << ", GnssPort: " << arg.gnss_port - << ", ScanPhase:" << arg.scan_phase << ", RotationSpeed:" << arg.rotation_speed - << ", FOV(Start):" << arg.cloud_min_angle << ", FOV(End):" << arg.cloud_max_angle; + os << "VelodyneSensorConfiguration:" << '\n'; + os << (LidarConfigurationBase)(arg) << '\n'; + os << "GnssPort: " << arg.gnss_port << '\n'; + os << "ScanPhase: " << arg.scan_phase << '\n'; + os << "RotationSpeed: " << arg.rotation_speed << '\n'; + os << "FOV(Start): " << arg.cloud_min_angle << '\n'; + os << "FOV(End): " << arg.cloud_max_angle; return os; } diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp index a8caf43f5..aafe9d155 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp @@ -77,8 +77,8 @@ class HesaiDecoder : public HesaiScanDecoder { if (packet.size() < sizeof(typename SensorT::packet_t)) { RCLCPP_ERROR_STREAM( - logger_, "Packet size mismatch:" << packet.size() << " | Expected at least:" - << sizeof(typename SensorT::packet_t)); + logger_, "Packet size mismatch: " << packet.size() << " | Expected at least: " + << sizeof(typename SensorT::packet_t)); return false; } if (std::memcpy(&packet_, packet.data(), sizeof(typename SensorT::packet_t))) { diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_decoder.hpp index ab00b9af5..48bf77eb2 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_decoder.hpp @@ -67,8 +67,8 @@ class RobosenseDecoder : public RobosenseScanDecoder { if (msop_packet.size() < sizeof(typename SensorT::packet_t)) { RCLCPP_ERROR_STREAM( - logger_, "Packet size mismatch:" << msop_packet.size() << " | Expected at least:" - << sizeof(typename SensorT::packet_t)); + logger_, "Packet size mismatch: " << msop_packet.size() << " | Expected at least: " + << sizeof(typename SensorT::packet_t)); return false; } if (std::memcpy(&packet_, msop_packet.data(), sizeof(typename SensorT::packet_t))) { diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_info_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_info_decoder.hpp index 8e6f4d4bd..02a00792c 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_info_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_info_decoder.hpp @@ -50,8 +50,8 @@ class RobosenseInfoDecoder : public RobosenseInfoDecoderBase const auto packet_size = raw_packet.size(); if (packet_size < sizeof(typename SensorT::info_t)) { RCLCPP_ERROR_STREAM( - logger_, "Packet size mismatch:" << packet_size << " | Expected at least:" - << sizeof(typename SensorT::info_t)); + logger_, "Packet size mismatch: " << packet_size << " | Expected at least: " + << sizeof(typename SensorT::info_t)); return false; } try { diff --git a/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp b/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp index 649085018..fe4b61844 100644 --- a/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp +++ b/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp @@ -17,10 +17,8 @@ #include "rclcpp/serialization.hpp" #include "rclcpp/serialized_message.hpp" #include "rcpputils/filesystem_helper.hpp" -#include "rcutils/time.h" #include "rosbag2_cpp/reader.hpp" #include "rosbag2_cpp/readers/sequential_reader.hpp" -#include "rosbag2_cpp/writer.hpp" #include "rosbag2_cpp/writers/sequential_writer.hpp" #include "rosbag2_storage/storage_options.hpp" @@ -41,7 +39,7 @@ HesaiRosOfflineExtractBag::HesaiRosOfflineExtractBag( wrapper_status_ = GetParameters(sensor_configuration, calibration_configuration, correction_configuration); if (Status::OK != wrapper_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << wrapper_status_); + RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error: " << wrapper_status_); return; } RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Starting..."); @@ -152,7 +150,7 @@ Status HesaiRosOfflineExtractBag::GetParameters( } } - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); + RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig: " << sensor_configuration); return Status::OK; } diff --git a/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd.cpp b/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd.cpp index 720236446..c21d37df7 100644 --- a/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd.cpp +++ b/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd.cpp @@ -17,7 +17,6 @@ #include "rclcpp/serialization.hpp" #include "rclcpp/serialized_message.hpp" #include "rcpputils/filesystem_helper.hpp" -#include "rcutils/time.h" #include "rosbag2_cpp/reader.hpp" #include "rosbag2_cpp/readers/sequential_reader.hpp" #include "rosbag2_storage/storage_options.hpp" @@ -41,7 +40,7 @@ HesaiRosOfflineExtractSample::HesaiRosOfflineExtractSample( wrapper_status_ = GetParameters(sensor_configuration, calibration_configuration, correction_configuration); if (Status::OK != wrapper_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << wrapper_status_); + RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error: " << wrapper_status_); return; } RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Starting..."); @@ -149,7 +148,7 @@ Status HesaiRosOfflineExtractSample::GetParameters( } } - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); + RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig: " << sensor_configuration); return Status::OK; } diff --git a/nebula_examples/src/velodyne/velodyne_ros_offline_extract_bag_pcd.cpp b/nebula_examples/src/velodyne/velodyne_ros_offline_extract_bag_pcd.cpp index fca1b5d7c..5c9c67e39 100644 --- a/nebula_examples/src/velodyne/velodyne_ros_offline_extract_bag_pcd.cpp +++ b/nebula_examples/src/velodyne/velodyne_ros_offline_extract_bag_pcd.cpp @@ -32,7 +32,7 @@ VelodyneRosOfflineExtractBag::VelodyneRosOfflineExtractBag( wrapper_status_ = GetParameters(sensor_configuration, calibration_configuration); if (Status::OK != wrapper_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << wrapper_status_); + RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error: " << wrapper_status_); return; } RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Starting..."); 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 3ce4858fc..d8d972488 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 @@ -56,7 +56,7 @@ Status RobosenseHwInterface::InfoInterfaceStart() try { std::cout << "Starting UDP server for info packets on: " << *sensor_configuration_ << std::endl; PrintInfo( - "Starting UDP server for info packets on: " + sensor_configuration_->sensor_ip + ":" + + "Starting UDP server for info packets on: " + sensor_configuration_->sensor_ip + ": " + std::to_string(sensor_configuration_->gnss_port)); info_udp_driver_->init_receiver( sensor_configuration_->host_ip, sensor_configuration_->gnss_port); 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 23b01d2b4..40ba0f754 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 @@ -129,7 +129,7 @@ boost::property_tree::ptree VelodyneHwInterface::ParseJson(const std::string & s 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::cerr << "Error on ParseJson: " << e.what() << std::endl; } return tree; } diff --git a/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp index a1db3cc2d..a07fdde19 100644 --- a/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp @@ -37,7 +37,7 @@ ContinentalARS548RosWrapper::ContinentalARS548RosWrapper(const rclcpp::NodeOptio (std::stringstream{} << "Sensor configuration invalid: " << wrapper_status_).str()); } - RCLCPP_INFO_STREAM(get_logger(), "SensorConfig:" << *config_ptr_); + RCLCPP_INFO_STREAM(get_logger(), "SensorConfig: " << *config_ptr_); launch_hw_ = declare_parameter("launch_hw", param_read_only()); diff --git a/nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp b/nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp index 8fff84139..24438746b 100644 --- a/nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp @@ -14,6 +14,8 @@ #include "nebula_ros/continental/continental_srr520_ros_wrapper.hpp" +#pragma clang diagnostic ignored "-Wbitwise-instead-of-logical" + namespace nebula { namespace ros @@ -35,7 +37,7 @@ ContinentalSRR520RosWrapper::ContinentalSRR520RosWrapper(const rclcpp::NodeOptio (std::stringstream{} << "Sensor configuration invalid: " << wrapper_status_).str()); } - RCLCPP_INFO_STREAM(get_logger(), "SensorConfig:" << *config_ptr_); + RCLCPP_INFO_STREAM(get_logger(), "SensorConfig: " << *config_ptr_); launch_hw_ = declare_parameter("launch_hw", param_read_only()); diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp index f7b93cb81..f45eb1231 100644 --- a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp @@ -26,7 +26,7 @@ HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) (std::stringstream{} << "Sensor configuration invalid: " << wrapper_status_).str()); } - RCLCPP_INFO_STREAM(get_logger(), "SensorConfig:" << *sensor_cfg_ptr_); + RCLCPP_INFO_STREAM(get_logger(), "SensorConfig: " << *sensor_cfg_ptr_); launch_hw_ = declare_parameter("launch_hw", param_read_only()); diff --git a/nebula_ros/src/hesai/hw_monitor_wrapper.cpp b/nebula_ros/src/hesai/hw_monitor_wrapper.cpp index 239c57d3f..4f3384659 100644 --- a/nebula_ros/src/hesai/hw_monitor_wrapper.cpp +++ b/nebula_ros/src/hesai/hw_monitor_wrapper.cpp @@ -59,8 +59,8 @@ HesaiHwMonitorWrapper::HesaiHwMonitorWrapper( std::cout << result << std::endl; info_model_ = result.get_str_model(); info_serial_ = std::string(std::begin(result.sn), std::end(result.sn)); - RCLCPP_INFO_STREAM(logger_, "Model:" << info_model_); - RCLCPP_INFO_STREAM(logger_, "Serial:" << info_serial_); + RCLCPP_INFO_STREAM(logger_, "Model: " << info_model_); + RCLCPP_INFO_STREAM(logger_, "Serial: " << info_serial_); InitializeHesaiDiagnostics(); } diff --git a/nebula_ros/src/robosense/robosense_ros_wrapper.cpp b/nebula_ros/src/robosense/robosense_ros_wrapper.cpp index 194a49398..72d837022 100644 --- a/nebula_ros/src/robosense/robosense_ros_wrapper.cpp +++ b/nebula_ros/src/robosense/robosense_ros_wrapper.cpp @@ -28,7 +28,7 @@ RobosenseRosWrapper::RobosenseRosWrapper(const rclcpp::NodeOptions & options) (std::stringstream{} << "Sensor configuration invalid: " << wrapper_status_).str()); } - RCLCPP_INFO_STREAM(get_logger(), "SensorConfig:" << *sensor_cfg_ptr_); + RCLCPP_INFO_STREAM(get_logger(), "SensorConfig: " << *sensor_cfg_ptr_); launch_hw_ = declare_parameter("launch_hw", param_read_only()); diff --git a/nebula_ros/src/velodyne/hw_monitor_wrapper.cpp b/nebula_ros/src/velodyne/hw_monitor_wrapper.cpp index d8aae2bc6..44c56528d 100644 --- a/nebula_ros/src/velodyne/hw_monitor_wrapper.cpp +++ b/nebula_ros/src/velodyne/hw_monitor_wrapper.cpp @@ -35,8 +35,8 @@ VelodyneHwMonitorWrapper::VelodyneHwMonitorWrapper( try { info_model_ = GetPtreeValue(current_snapshot_tree, mtx_snapshot_, key_info_model); info_serial_ = GetPtreeValue(current_snapshot_tree, mtx_snapshot_, key_info_serial); - RCLCPP_INFO_STREAM(logger_, "Model:" << info_model_); - RCLCPP_INFO_STREAM(logger_, "Serial:" << info_serial_); + RCLCPP_INFO_STREAM(logger_, "Model: " << info_model_); + RCLCPP_INFO_STREAM(logger_, "Serial: " << info_serial_); } catch (boost::bad_lexical_cast & ex) { RCLCPP_ERROR(logger_, " Error: Can't get model and serial"); return; @@ -737,8 +737,8 @@ std::tuple VelodyneHwMonitorWrapper::Ve std::ostringstream os; for (auto v = child->begin(); v != child->end(); ++v) { os << "("; - os << "mean:" << v->second.get("mean") << ", "; - os << "stddev:" << v->second.get("stddev") << ", "; + os << "mean: " << v->second.get("mean") << ", "; + os << "stddev: " << v->second.get("stddev") << ", "; os << "), "; } mes = os.str(); diff --git a/nebula_ros/src/velodyne/velodyne_ros_wrapper.cpp b/nebula_ros/src/velodyne/velodyne_ros_wrapper.cpp index 49c93b43a..5edce1953 100644 --- a/nebula_ros/src/velodyne/velodyne_ros_wrapper.cpp +++ b/nebula_ros/src/velodyne/velodyne_ros_wrapper.cpp @@ -26,7 +26,7 @@ VelodyneRosWrapper::VelodyneRosWrapper(const rclcpp::NodeOptions & options) (std::stringstream{} << "Sensor configuration invalid: " << wrapper_status_).str()); } - RCLCPP_INFO_STREAM(get_logger(), "SensorConfig:" << *sensor_cfg_ptr_); + RCLCPP_INFO_STREAM(get_logger(), "SensorConfig: " << *sensor_cfg_ptr_); launch_hw_ = declare_parameter("launch_hw", param_read_only()); diff --git a/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp b/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp index 727b9db62..9315bbe50 100644 --- a/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp +++ b/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp @@ -46,7 +46,7 @@ ContinentalRosDecoderTest::ContinentalRosDecoderTest( wrapper_status_ = GetParameters(sensor_configuration); if (Status::OK != wrapper_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << wrapper_status_); + RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error: " << wrapper_status_); return; } RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Starting..."); @@ -111,7 +111,7 @@ Status ContinentalRosDecoderTest::GetParameters( return Status::INVALID_SENSOR_MODEL; } - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); + RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig: " << sensor_configuration); return Status::OK; } diff --git a/nebula_tests/continental/continental_ros_decoder_test_srr520.cpp b/nebula_tests/continental/continental_ros_decoder_test_srr520.cpp index 7991fada1..31cbf4db1 100644 --- a/nebula_tests/continental/continental_ros_decoder_test_srr520.cpp +++ b/nebula_tests/continental/continental_ros_decoder_test_srr520.cpp @@ -46,7 +46,7 @@ ContinentalRosDecoderTest::ContinentalRosDecoderTest( wrapper_status_ = GetParameters(sensor_configuration); if (Status::OK != wrapper_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << wrapper_status_); + RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error: " << wrapper_status_); return; } RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Starting..."); @@ -113,7 +113,7 @@ Status ContinentalRosDecoderTest::GetParameters( return Status::INVALID_SENSOR_MODEL; } - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); + RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig: " << sensor_configuration); return Status::OK; } diff --git a/nebula_tests/hesai/hesai_ros_decoder_test.cpp b/nebula_tests/hesai/hesai_ros_decoder_test.cpp index 7968f5ce9..ccf1348e5 100644 --- a/nebula_tests/hesai/hesai_ros_decoder_test.cpp +++ b/nebula_tests/hesai/hesai_ros_decoder_test.cpp @@ -5,11 +5,8 @@ #include "rclcpp/serialization.hpp" #include "rclcpp/serialized_message.hpp" #include "rcpputils/filesystem_helper.hpp" -#include "rcutils/time.h" #include "rosbag2_cpp/reader.hpp" #include "rosbag2_cpp/readers/sequential_reader.hpp" -#include "rosbag2_cpp/writer.hpp" -#include "rosbag2_cpp/writers/sequential_writer.hpp" #include "rosbag2_storage/storage_options.hpp" #include @@ -33,7 +30,7 @@ HesaiRosDecoderTest::HesaiRosDecoderTest( wrapper_status_ = GetParameters(sensor_configuration, calibration_configuration, correction_configuration); if (Status::OK != wrapper_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << wrapper_status_); + RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error: " << wrapper_status_); return; } RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Starting..."); @@ -249,7 +246,7 @@ Status HesaiRosDecoderTest::GetParameters( } } - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); + RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig: " << sensor_configuration); return Status::OK; } diff --git a/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp16.cpp b/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp16.cpp index b4bb04236..828794e75 100644 --- a/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp16.cpp +++ b/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp16.cpp @@ -5,11 +5,8 @@ #include "rclcpp/serialization.hpp" #include "rclcpp/serialized_message.hpp" #include "rcpputils/filesystem_helper.hpp" -#include "rcutils/time.h" #include "rosbag2_cpp/reader.hpp" #include "rosbag2_cpp/readers/sequential_reader.hpp" -#include "rosbag2_cpp/writer.hpp" -#include "rosbag2_cpp/writers/sequential_writer.hpp" #include "rosbag2_storage/storage_options.hpp" #include @@ -33,7 +30,7 @@ VelodyneRosDecoderTest::VelodyneRosDecoderTest( wrapper_status_ = GetParameters(sensor_configuration, calibration_configuration); if (Status::OK != wrapper_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << wrapper_status_); + RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error: " << wrapper_status_); return; } RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Starting..."); @@ -254,7 +251,7 @@ Status VelodyneRosDecoderTest::GetParameters( } } - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); + RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig: " << sensor_configuration); return Status::OK; } diff --git a/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp32.cpp b/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp32.cpp index bfa6a490e..a10c4c81a 100644 --- a/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp32.cpp +++ b/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp32.cpp @@ -28,7 +28,7 @@ VelodyneRosDecoderTest::VelodyneRosDecoderTest( wrapper_status_ = GetParameters(sensor_configuration, calibration_configuration); if (Status::OK != wrapper_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << wrapper_status_); + RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error: " << wrapper_status_); return; } RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Starting..."); @@ -250,7 +250,7 @@ Status VelodyneRosDecoderTest::GetParameters( } } - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); + RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig: " << sensor_configuration); return Status::OK; } diff --git a/nebula_tests/velodyne/velodyne_ros_decoder_test_vls128.cpp b/nebula_tests/velodyne/velodyne_ros_decoder_test_vls128.cpp index c6980daef..68201d6a4 100644 --- a/nebula_tests/velodyne/velodyne_ros_decoder_test_vls128.cpp +++ b/nebula_tests/velodyne/velodyne_ros_decoder_test_vls128.cpp @@ -33,7 +33,7 @@ VelodyneRosDecoderTest::VelodyneRosDecoderTest( wrapper_status_ = GetParameters(sensor_configuration, calibration_configuration); if (Status::OK != wrapper_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << wrapper_status_); + RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error: " << wrapper_status_); return; } RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Starting..."); @@ -254,7 +254,7 @@ Status VelodyneRosDecoderTest::GetParameters( } } - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); + RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig: " << sensor_configuration); return Status::OK; }