Skip to content

Commit

Permalink
style: pretty-printing sensor configs
Browse files Browse the repository at this point in the history
  • Loading branch information
mojomex committed Jun 27, 2024
1 parent 10725e4 commit 0f4570f
Show file tree
Hide file tree
Showing 27 changed files with 99 additions and 91 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,6 @@
#pragma once

#include "nebula_common/nebula_common.hpp"
#include "nebula_common/nebula_status.hpp"

#include <boost/algorithm/string/join.hpp>
#include <boost/endian/buffers.hpp>
Expand All @@ -24,9 +23,7 @@
#include <pcl/point_types.h>

#include <iostream>
#include <sstream>
#include <string>
#include <vector>

namespace nebula
{
Expand Down Expand Up @@ -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;
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -15,21 +15,16 @@
#pragma once

#include "nebula_common/nebula_common.hpp"
#include "nebula_common/nebula_status.hpp"

#include <boost/endian/buffers.hpp>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

#include <bitset>
#include <cmath>
#include <cstddef>
#include <cstdint>
#include <ctime>
#include <fstream>
#include <iostream>
#include <sstream>
#include <string>

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

Expand Down
26 changes: 16 additions & 10 deletions nebula_common/include/nebula_common/hesai/hesai_common.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;
}

Expand Down Expand Up @@ -106,7 +112,7 @@ struct HesaiCalibrationConfiguration : public HesaiCalibrationConfigurationBase

std::vector<std::string> 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;
}

Expand Down Expand Up @@ -318,10 +324,10 @@ struct HesaiCorrection : public HesaiCalibrationConfigurationBase
inline nebula::Status SaveToFileFromBytes(
const std::string & correction_file, const std::vector<uint8_t> & 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";
Expand Down
28 changes: 17 additions & 11 deletions nebula_common/include/nebula_common/nebula_common.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,6 @@
#include <boost/tokenizer.hpp>

#include <algorithm>
#include <map>
#include <ostream>
#include <string>
#include <vector>
Expand Down Expand Up @@ -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;
}

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

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

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

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

Expand Down
10 changes: 7 additions & 3 deletions nebula_common/include/nebula_common/velodyne/velodyne_common.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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))) {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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))) {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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"

Expand All @@ -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...");
Expand Down Expand Up @@ -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;
}

Expand Down
5 changes: 2 additions & 3 deletions nebula_examples/src/hesai/hesai_ros_offline_extract_pcd.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand All @@ -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...");
Expand Down Expand Up @@ -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;
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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...");
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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<bool>("launch_hw", param_read_only());

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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<bool>("launch_hw", param_read_only());

Expand Down
2 changes: 1 addition & 1 deletion nebula_ros/src/hesai/hesai_ros_wrapper.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<bool>("launch_hw", param_read_only());

Expand Down
4 changes: 2 additions & 2 deletions nebula_ros/src/hesai/hw_monitor_wrapper.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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();
}

Expand Down
Loading

0 comments on commit 0f4570f

Please sign in to comment.