From 25e9e02c1b88086dcf315e8321ab54db17a21365 Mon Sep 17 00:00:00 2001 From: Maximilian Schmeller Date: Thu, 28 Sep 2023 18:47:18 +0900 Subject: [PATCH 1/3] Refactor test case for Pandar40P --- nebula_tests/hesai/hesai_common.hpp | 48 +++++++ ...est_40p.cpp => hesai_ros_decoder_test.cpp} | 130 +++++++----------- ...est_40p.hpp => hesai_ros_decoder_test.hpp} | 41 ++++-- ...0p.cpp => hesai_ros_decoder_test_main.cpp} | 15 +- 4 files changed, 142 insertions(+), 92 deletions(-) rename nebula_tests/hesai/{hesai_ros_decoder_test_40p.cpp => hesai_ros_decoder_test.cpp} (76%) rename nebula_tests/hesai/{hesai_ros_decoder_test_40p.hpp => hesai_ros_decoder_test.hpp} (83%) rename nebula_tests/hesai/{hesai_ros_decoder_test_main_40p.cpp => hesai_ros_decoder_test_main.cpp} (71%) diff --git a/nebula_tests/hesai/hesai_common.hpp b/nebula_tests/hesai/hesai_common.hpp index 3a622c19a..6cf085be0 100644 --- a/nebula_tests/hesai/hesai_common.hpp +++ b/nebula_tests/hesai/hesai_common.hpp @@ -3,6 +3,7 @@ #include "nebula_common/hesai/hesai_common.hpp" #include "nebula_common/nebula_common.hpp" #include "nebula_common/nebula_status.hpp" +#include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp" #include "nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp" #include "nebula_ros/common/nebula_driver_ros_wrapper_base.hpp" @@ -14,6 +15,9 @@ #include "pandar_msgs/msg/pandar_scan.hpp" #include +#include + +#include namespace nebula { @@ -39,5 +43,49 @@ void checkPCDs(nebula::drivers::NebulaPointCloudPtr pc, pcl::PointCloudpoints.size(), pp2->points.size()); + for (uint32_t i = 0; i < pp1->points.size(); i++) { + auto p1 = pp1->points[i]; + auto p2 = pp2->points[i]; + EXPECT_FLOAT_EQ(p1.x, p2.x); + EXPECT_FLOAT_EQ(p1.y, p2.y); + EXPECT_FLOAT_EQ(p1.z, p2.z); + EXPECT_FLOAT_EQ(p1.intensity, p2.intensity); + EXPECT_EQ(p1.channel, p2.channel); + EXPECT_FLOAT_EQ(p1.azimuth, p2.azimuth); + EXPECT_EQ(p1.return_type, p2.return_type); + EXPECT_DOUBLE_EQ(p1.time_stamp, p2.time_stamp); + } +} + +void printPCD(nebula::drivers::NebulaPointCloudPtr pp) +{ + for (auto p : pp->points) { + std::cout << "(" << p.x << ", " << p.y << "," << p.z << "): " << p.intensity << ", " + << p.channel << ", " << p.azimuth << ", " << p.return_type << ", " << p.time_stamp + << std::endl; + } +} + +void checkTimestamp( + pandar_msgs::msg::PandarScan & raw_scan, nebula::drivers::HesaiScanDecoder & decoder) +{ + // first: self-check + putenv("TZ=GMT"); + tzset(); + auto gmt = timezone; + putenv("TZ=JST"); + tzset(); + auto jst = timezone; + + EXPECT_NE(gmt, jst); + + for (auto packet : raw_scan.packets) { + decoder.unpack(packet); + } +} + } // namespace ros } // namespace nebula \ No newline at end of file diff --git a/nebula_tests/hesai/hesai_ros_decoder_test_40p.cpp b/nebula_tests/hesai/hesai_ros_decoder_test.cpp similarity index 76% rename from nebula_tests/hesai/hesai_ros_decoder_test_40p.cpp rename to nebula_tests/hesai/hesai_ros_decoder_test.cpp index 88a490aee..b37b83567 100644 --- a/nebula_tests/hesai/hesai_ros_decoder_test_40p.cpp +++ b/nebula_tests/hesai/hesai_ros_decoder_test.cpp @@ -1,5 +1,4 @@ -#include "hesai_ros_decoder_test_40p.hpp" - +#include "hesai_ros_decoder_test.hpp" #include "rclcpp/serialization.hpp" #include "rclcpp/serialized_message.hpp" #include "rcpputils/filesystem_helper.hpp" @@ -20,8 +19,9 @@ namespace nebula namespace ros { HesaiRosDecoderTest::HesaiRosDecoderTest( - const rclcpp::NodeOptions & options, const std::string & node_name) -: rclcpp::Node(node_name, options) + const rclcpp::NodeOptions & options, const std::string & node_name, + const HesaiRosDecoderTestParams & params) +: rclcpp::Node(node_name, options), params_(params) { drivers::HesaiCalibrationConfiguration calibration_configuration; drivers::HesaiSensorConfiguration sensor_configuration; @@ -81,118 +81,119 @@ Status HesaiRosDecoderTest::InitializeDriver( return driver_ptr_->GetStatus(); } -Status HesaiRosDecoderTest::GetStatus() { return wrapper_status_; } +Status HesaiRosDecoderTest::GetStatus() +{ + return wrapper_status_; +} Status HesaiRosDecoderTest::GetParameters( drivers::HesaiSensorConfiguration & sensor_configuration, drivers::HesaiCalibrationConfiguration & calibration_configuration, drivers::HesaiCorrection & correction_configuration) { - std::filesystem::path calib_dir = - _SRC_CALIBRATION_DIR_PATH; // variable defined in CMakeLists.txt; - calib_dir /= "hesai"; - std::filesystem::path bag_root_dir = - _SRC_RESOURCES_DIR_PATH; // variable defined in CMakeLists.txt; + std::filesystem::path calibration_dir = _SRC_CALIBRATION_DIR_PATH; + calibration_dir /= "hesai"; + std::filesystem::path bag_root_dir = _SRC_RESOURCES_DIR_PATH; bag_root_dir /= "hesai"; { rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; descriptor.read_only = true; descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; - this->declare_parameter("sensor_model", "Pandar40P"); + this->declare_parameter("sensor_model", params_.sensor_model, descriptor); sensor_configuration.sensor_model = nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); } { rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; descriptor.read_only = true; descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; - this->declare_parameter("return_mode", "Dual", descriptor); + this->declare_parameter("return_mode", params_.return_mode, descriptor); sensor_configuration.return_mode = nebula::drivers::ReturnModeFromStringHesai( this->get_parameter("return_mode").as_string(), sensor_configuration.sensor_model); } { rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; descriptor.read_only = true; descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", "hesai", descriptor); + this->declare_parameter("frame_id", params_.frame_id, descriptor); sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); } { rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 3; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; descriptor.read_only = true; descriptor.dynamic_typing = false; descriptor.additional_constraints = "Angle where scans begin (degrees, [0.,360.]"; rcl_interfaces::msg::FloatingPointRange range; range.set__from_value(0).set__to_value(360).set__step(0.01); descriptor.floating_point_range = {range}; - this->declare_parameter("scan_phase", 0., descriptor); + this->declare_parameter("scan_phase", params_.scan_phase, descriptor); sensor_configuration.scan_phase = this->get_parameter("scan_phase").as_double(); } { rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; descriptor.read_only = true; descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; this->declare_parameter( - "calibration_file", (calib_dir / "Pandar40P.csv").string(), descriptor); + "calibration_file", (calibration_dir / params_.calibration_file).string(), descriptor); calibration_configuration.calibration_file = this->get_parameter("calibration_file").as_string(); } if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; descriptor.read_only = true; descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; this->declare_parameter( - "correction_file", (calib_dir / "PandarAT128.dat").string(), descriptor); - correction_file_path = this->get_parameter("correction_file").as_string(); + "correction_file", (calibration_dir / params_.correction_file).string(), descriptor); + params_.correction_file = this->get_parameter("correction_file").as_string(); } { rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; descriptor.read_only = true; descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; this->declare_parameter( - "bag_path", (bag_root_dir / "40p" / "1673400149412331409").string(), descriptor); - bag_path = this->get_parameter("bag_path").as_string(); - std::cout << bag_path << std::endl; + "bag_path", (bag_root_dir / params_.bag_path).string(), descriptor); + params_.bag_path = this->get_parameter("bag_path").as_string(); + std::cout << params_.bag_path << std::endl; } { rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; descriptor.read_only = true; descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; - this->declare_parameter("storage_id", "sqlite3", descriptor); - storage_id = this->get_parameter("storage_id").as_string(); + this->declare_parameter("storage_id", params_.storage_id, descriptor); + params_.storage_id = this->get_parameter("storage_id").as_string(); } { rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; descriptor.read_only = true; descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; - this->declare_parameter("format", "cdr", descriptor); - format = this->get_parameter("format").as_string(); + this->declare_parameter("format", params_.format, descriptor); + params_.format = this->get_parameter("format").as_string(); } { rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; descriptor.read_only = true; descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; - this->declare_parameter("target_topic", "/pandar_packets", descriptor); - target_topic = this->get_parameter("target_topic").as_string(); + this->declare_parameter("target_topic", params_.target_topic, descriptor); + params_.target_topic = this->get_parameter("target_topic").as_string(); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -203,7 +204,8 @@ Status HesaiRosDecoderTest::GetParameters( rcl_interfaces::msg::FloatingPointRange range; range.set__from_value(0.00).set__to_value(0.5).set__step(0.01); descriptor.floating_point_range = {range}; - this->declare_parameter("dual_return_distance_threshold", 0.1, descriptor); + this->declare_parameter( + "dual_return_distance_threshold", params_.dual_return_distance_threshold, descriptor); sensor_configuration.dual_return_distance_threshold = this->get_parameter("dual_return_distance_threshold").as_double(); } @@ -230,13 +232,13 @@ Status HesaiRosDecoderTest::GetParameters( } } if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { - if (correction_file_path.empty()) { + if (params_.correction_file.empty()) { return Status::INVALID_CALIBRATION_FILE; } else { - auto cal_status = correction_configuration.LoadFromFile(correction_file_path); + auto cal_status = correction_configuration.LoadFromFile(params_.correction_file); if (cal_status != Status::OK) { RCLCPP_ERROR_STREAM( - this->get_logger(), "Given Correction File: '" << correction_file_path << "'"); + this->get_logger(), "Given Correction File: '" << params_.correction_file << "'"); return cal_status; } } @@ -246,45 +248,17 @@ Status HesaiRosDecoderTest::GetParameters( return Status::OK; } -void printPCD(nebula::drivers::NebulaPointCloudPtr pp) -{ - for (auto p : pp->points) { - std::cout << "(" << p.x << ", " << p.y << "," << p.z << "): " << p.intensity << ", " - << p.channel << ", " << p.azimuth << ", " << p.return_type << ", " << p.time_stamp - << std::endl; - } -} - -void checkPCDs(nebula::drivers::NebulaPointCloudPtr pp1, nebula::drivers::NebulaPointCloudPtr pp2) -{ - EXPECT_EQ(pp1->points.size(), pp2->points.size()); - for (uint32_t i = 0; i < pp1->points.size(); i++) { - auto p1 = pp1->points[i]; - auto p2 = pp2->points[i]; - EXPECT_FLOAT_EQ(p1.x, p2.x); - EXPECT_FLOAT_EQ(p1.y, p2.y); - EXPECT_FLOAT_EQ(p1.z, p2.z); - EXPECT_FLOAT_EQ(p1.intensity, p2.intensity); - EXPECT_EQ(p1.channel, p2.channel); - EXPECT_FLOAT_EQ(p1.azimuth, p2.azimuth); - EXPECT_EQ(p1.return_type, p2.return_type); - EXPECT_DOUBLE_EQ(p1.time_stamp, p2.time_stamp); - } -} - - - void HesaiRosDecoderTest::ReadBag() { rosbag2_storage::StorageOptions storage_options; rosbag2_cpp::ConverterOptions converter_options; - std::cout << bag_path << std::endl; - std::cout << storage_id << std::endl; - std::cout << format << std::endl; - std::cout << target_topic << std::endl; + std::cout << params_.bag_path << std::endl; + std::cout << params_.storage_id << std::endl; + std::cout << params_.format << std::endl; + std::cout << params_.target_topic << std::endl; - auto target_topic_name = target_topic; + auto target_topic_name = params_.target_topic; if (target_topic_name.substr(0, 1) == "/") { target_topic_name = target_topic_name.substr(1); } @@ -292,13 +266,13 @@ void HesaiRosDecoderTest::ReadBag() pcl::PCDReader pcd_reader; - rcpputils::fs::path bag_dir(bag_path); + rcpputils::fs::path bag_dir(params_.bag_path); rcpputils::fs::path pcd_dir = bag_dir.parent_path(); int check_cnt = 0; - storage_options.uri = bag_path; - storage_options.storage_id = storage_id; - converter_options.output_serialization_format = format; //"cdr"; + storage_options.uri = params_.bag_path; + storage_options.storage_id = params_.storage_id; + converter_options.output_serialization_format = params_.format; //"cdr"; rclcpp::Serialization serialization; nebula::drivers::NebulaPointCloudPtr pointcloud(new nebula::drivers::NebulaPointCloud); // nebula::drivers::NebulaPointCloudPtr ref_pointcloud(new nebula::drivers::NebulaPointCloud); @@ -311,7 +285,7 @@ void HesaiRosDecoderTest::ReadBag() std::cout << "Found topic name " << bag_message->topic_name << std::endl; - if (bag_message->topic_name == target_topic) { + if (bag_message->topic_name == params_.target_topic) { pandar_msgs::msg::PandarScan extracted_msg; rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data); serialization.deserialize_message(&extracted_serialized_msg, &extracted_msg); diff --git a/nebula_tests/hesai/hesai_ros_decoder_test_40p.hpp b/nebula_tests/hesai/hesai_ros_decoder_test.hpp similarity index 83% rename from nebula_tests/hesai/hesai_ros_decoder_test_40p.hpp rename to nebula_tests/hesai/hesai_ros_decoder_test.hpp index f961931e4..58ec80f40 100644 --- a/nebula_tests/hesai/hesai_ros_decoder_test_40p.hpp +++ b/nebula_tests/hesai/hesai_ros_decoder_test.hpp @@ -1,6 +1,6 @@ -#ifndef NEBULA_HesaiRosDecoderTest40p_H -#define NEBULA_HesaiRosDecoderTest40p_H +#pragma once +#include "hesai_common.hpp" #include "nebula_common/hesai/hesai_common.hpp" #include "nebula_common/nebula_common.hpp" #include "nebula_common/nebula_status.hpp" @@ -13,14 +13,37 @@ #include "pandar_msgs/msg/pandar_packet.hpp" #include "pandar_msgs/msg/pandar_scan.hpp" -#include "hesai_common.hpp" #include +#ifndef _SRC_CALIBRATION_DIR_PATH +#define _SRC_CALIBRATION_DIR_PATH "" +#endif + +#ifndef _SRC_RESOURCES_DIR_PATH +#define _SRC_RESOURCES_DIR_PATH "" +#endif + namespace nebula { namespace ros { + +struct HesaiRosDecoderTestParams +{ + std::string sensor_model; + std::string return_mode; + std::string frame_id = "hesai"; + double scan_phase = 0.; + std::string calibration_file = ""; + std::string correction_file = ""; + std::string bag_path; + std::string storage_id = "sqlite3"; + std::string format = "cdr"; + std::string target_topic = "/pandar_packets"; + double dual_return_distance_threshold = 0.1; +}; + /// @brief Testing decoder of pandar 40p (Keeps HesaiDriverRosWrapper structure as much as /// possible) class HesaiRosDecoderTest final : public rclcpp::Node, NebulaDriverRosWrapperBase //, testing::Test @@ -71,7 +94,9 @@ class HesaiRosDecoderTest final : public rclcpp::Node, NebulaDriverRosWrapperBas } public: - explicit HesaiRosDecoderTest(const rclcpp::NodeOptions & options, const std::string & node_name); + explicit HesaiRosDecoderTest( + const rclcpp::NodeOptions & options, const std::string & node_name, + const HesaiRosDecoderTestParams & params); // void ReceiveScanMsgCallback(const pandar_msgs::msg::PandarScan::SharedPtr scan_msg); @@ -94,14 +119,8 @@ class HesaiRosDecoderTest final : public rclcpp::Node, NebulaDriverRosWrapperBas } */ private: - std::string bag_path; - std::string storage_id; - std::string format; - std::string target_topic; - std::string correction_file_path; + HesaiRosDecoderTestParams params_; }; } // namespace ros } // namespace nebula - -#endif // NEBULA_HesaiRosDecoderTest40p_H diff --git a/nebula_tests/hesai/hesai_ros_decoder_test_main_40p.cpp b/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp similarity index 71% rename from nebula_tests/hesai/hesai_ros_decoder_test_main_40p.cpp rename to nebula_tests/hesai/hesai_ros_decoder_test_main.cpp index d3e681a17..429ad50d4 100644 --- a/nebula_tests/hesai/hesai_ros_decoder_test_main_40p.cpp +++ b/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp @@ -1,4 +1,4 @@ -#include "hesai_ros_decoder_test_40p.hpp" +#include "hesai_ros_decoder_test.hpp" #include @@ -14,9 +14,11 @@ TEST(TestDecoder, TestPcd) hesai_driver->ReadBag(); } + + int main(int argc, char * argv[]) { - std::cout << "hesai_ros_decoder_test_main_40p.cpp" << std::endl; + std::cout << "hesai_ros_decoder_test_main.cpp" << std::endl; setvbuf(stdout, NULL, _IONBF, BUFSIZ); std::string node_name = "nebula_hesai_decoder_test"; @@ -26,7 +28,14 @@ int main(int argc, char * argv[]) rclcpp::executors::SingleThreadedExecutor exec; rclcpp::NodeOptions options; - hesai_driver = std::make_shared(options, node_name); + nebula::ros::HesaiRosDecoderTestParams decoder_params; + decoder_params.sensor_model = "Pandar40P"; + decoder_params.return_mode = "Dual"; + decoder_params.calibration_file = "Pandar40P.csv"; + decoder_params.bag_path = "40p/1673400149412331409"; + + hesai_driver = + std::make_shared(options, node_name, decoder_params); exec.add_node(hesai_driver->get_node_base_interface()); RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Get Status"); From bdffdd8b11d3538b7c62eab99dfbb341c0723107 Mon Sep 17 00:00:00 2001 From: Maximilian Schmeller Date: Mon, 2 Oct 2023 14:20:24 +0900 Subject: [PATCH 2/3] Complete Hesai test refactoring into generic implementation The sensor-specific duplicates of the ..._test_main and ..._test files have been refactored into a single generic implementation that takes only the sensor parameters as input. The ReadBag function has been refactored such that it takes a callback function which is called on every point cloud scan. This allows for easy and concise implementation of new test cases. --- .cspell.json | 5 +- nebula_tests/hesai/CMakeLists.txt | 142 +------ nebula_tests/hesai/hesai_common.hpp | 20 +- nebula_tests/hesai/hesai_ros_decoder_test.cpp | 73 ++-- nebula_tests/hesai/hesai_ros_decoder_test.hpp | 15 +- .../hesai/hesai_ros_decoder_test_64.cpp | 353 ------------------ .../hesai/hesai_ros_decoder_test_64.hpp | 103 ----- .../hesai/hesai_ros_decoder_test_at128.cpp | 353 ------------------ .../hesai/hesai_ros_decoder_test_at128.hpp | 106 ------ .../hesai/hesai_ros_decoder_test_main.cpp | 144 +++++-- .../hesai/hesai_ros_decoder_test_main.hpp | 26 ++ .../hesai/hesai_ros_decoder_test_main_64.cpp | 47 --- .../hesai_ros_decoder_test_main_at128.cpp | 49 --- .../hesai_ros_decoder_test_main_qt64.cpp | 47 --- .../hesai_ros_decoder_test_main_xt32.cpp | 47 --- .../hesai_ros_decoder_test_main_xt32m.cpp | 47 --- .../hesai/hesai_ros_decoder_test_qt64.cpp | 353 ------------------ .../hesai/hesai_ros_decoder_test_qt64.hpp | 103 ----- .../hesai/hesai_ros_decoder_test_xt32.cpp | 350 ----------------- .../hesai/hesai_ros_decoder_test_xt32.hpp | 103 ----- .../hesai/hesai_ros_decoder_test_xt32m.cpp | 352 ----------------- .../hesai/hesai_ros_decoder_test_xt32m.hpp | 106 ------ 22 files changed, 185 insertions(+), 2759 deletions(-) delete mode 100644 nebula_tests/hesai/hesai_ros_decoder_test_64.cpp delete mode 100644 nebula_tests/hesai/hesai_ros_decoder_test_64.hpp delete mode 100644 nebula_tests/hesai/hesai_ros_decoder_test_at128.cpp delete mode 100644 nebula_tests/hesai/hesai_ros_decoder_test_at128.hpp create mode 100644 nebula_tests/hesai/hesai_ros_decoder_test_main.hpp delete mode 100644 nebula_tests/hesai/hesai_ros_decoder_test_main_64.cpp delete mode 100644 nebula_tests/hesai/hesai_ros_decoder_test_main_at128.cpp delete mode 100644 nebula_tests/hesai/hesai_ros_decoder_test_main_qt64.cpp delete mode 100644 nebula_tests/hesai/hesai_ros_decoder_test_main_xt32.cpp delete mode 100644 nebula_tests/hesai/hesai_ros_decoder_test_main_xt32m.cpp delete mode 100644 nebula_tests/hesai/hesai_ros_decoder_test_qt64.cpp delete mode 100644 nebula_tests/hesai/hesai_ros_decoder_test_qt64.hpp delete mode 100644 nebula_tests/hesai/hesai_ros_decoder_test_xt32.cpp delete mode 100644 nebula_tests/hesai/hesai_ros_decoder_test_xt32.hpp delete mode 100644 nebula_tests/hesai/hesai_ros_decoder_test_xt32m.cpp delete mode 100644 nebula_tests/hesai/hesai_ros_decoder_test_xt32m.hpp diff --git a/.cspell.json b/.cspell.json index fc437bdf8..6a372147a 100644 --- a/.cspell.json +++ b/.cspell.json @@ -10,6 +10,7 @@ "gprmc", "Hesai", "memcpy", + "mkdoxy", "nohup", "nproc", "pandar", @@ -19,6 +20,7 @@ "PANDARXT", "Pdelay", "QT", + "rclcpp", "schedutil", "STD_COUT", "stds", @@ -28,7 +30,6 @@ "vccint", "Vccint", "XT", - "XTM", - "mkdoxy" + "XTM" ] } diff --git a/nebula_tests/hesai/CMakeLists.txt b/nebula_tests/hesai/CMakeLists.txt index b239cd3a3..8a06fd07f 100644 --- a/nebula_tests/hesai/CMakeLists.txt +++ b/nebula_tests/hesai/CMakeLists.txt @@ -1,146 +1,22 @@ -# Pandar AT128 -ament_auto_add_library(hesai_ros_decoder_test_at128 SHARED - hesai_ros_decoder_test_at128.cpp +ament_auto_add_library(hesai_ros_decoder_test SHARED + hesai_ros_decoder_test.cpp ) -target_link_libraries(hesai_ros_decoder_test_at128 ${PCL_LIBRARIES} ${NEBULA_TEST_LIBRARIES}) +target_link_libraries(hesai_ros_decoder_test ${PCL_LIBRARIES} ${NEBULA_TEST_LIBRARIES}) -ament_add_gtest(hesai_ros_decoder_test_main_at128 - hesai_ros_decoder_test_main_at128.cpp +ament_add_gtest(hesai_ros_decoder_test_main + hesai_ros_decoder_test_main.cpp ) -ament_target_dependencies(hesai_ros_decoder_test_main_at128 +ament_target_dependencies(hesai_ros_decoder_test_main ${NEBULA_TEST_DEPENDENCIES} ) -target_include_directories(hesai_ros_decoder_test_main_at128 PUBLIC +target_include_directories(hesai_ros_decoder_test_main PUBLIC ${PROJECT_SOURCE_DIR}/src/hesai include ) -target_link_libraries(hesai_ros_decoder_test_main_at128 +target_link_libraries(hesai_ros_decoder_test_main ${PCL_LIBRARIES} - hesai_ros_decoder_test_at128 - ) - -# Pandar XT32M -ament_auto_add_library(hesai_ros_decoder_test_xt32m SHARED - hesai_ros_decoder_test_xt32m.cpp - ) -target_link_libraries(hesai_ros_decoder_test_xt32m ${PCL_LIBRARIES} ${NEBULA_TEST_LIBRARIES}) - -ament_add_gtest(hesai_ros_decoder_test_main_xt32m - hesai_ros_decoder_test_main_xt32m.cpp - ) - -ament_target_dependencies(hesai_ros_decoder_test_main_xt32m - ${NEBULA_TEST_DEPENDENCIES} - ) - -target_include_directories(hesai_ros_decoder_test_main_xt32m PUBLIC - ${PROJECT_SOURCE_DIR}/src/hesai - include - ) - -target_link_libraries(hesai_ros_decoder_test_main_xt32m - ${PCL_LIBRARIES} - hesai_ros_decoder_test_xt32m - ) - -# Pandar 40P -ament_auto_add_library(hesai_ros_decoder_test_40p SHARED - hesai_ros_decoder_test_40p.cpp - ) - -target_link_libraries(hesai_ros_decoder_test_40p ${PCL_LIBRARIES} ${NEBULA_TEST_LIBRARIES}) - -ament_add_gtest(hesai_ros_decoder_test_main_40p - hesai_ros_decoder_test_main_40p.cpp - ) - -ament_target_dependencies(hesai_ros_decoder_test_main_40p - ${NEBULA_TEST_DEPENDENCIES} - ) - -target_include_directories(hesai_ros_decoder_test_main_40p PUBLIC - ${PROJECT_SOURCE_DIR}/src/hesai - include - ) - -target_link_libraries(hesai_ros_decoder_test_main_40p - ${PCL_LIBRARIES} - hesai_ros_decoder_test_40p - ) - -# Pandar 64 -ament_auto_add_library(hesai_ros_decoder_test_64 SHARED - hesai_ros_decoder_test_64.cpp - ) - -target_link_libraries(hesai_ros_decoder_test_64 ${PCL_LIBRARIES} ${NEBULA_TEST_LIBRARIES}) - -ament_add_gtest(hesai_ros_decoder_test_main_64 - hesai_ros_decoder_test_main_64.cpp - ) - -ament_target_dependencies(hesai_ros_decoder_test_main_64 - ${NEBULA_TEST_DEPENDENCIES} - ) - -target_include_directories(hesai_ros_decoder_test_main_64 PUBLIC - ${PROJECT_SOURCE_DIR}/src/hesai - include - ) - -target_link_libraries(hesai_ros_decoder_test_main_64 - ${PCL_LIBRARIES} - hesai_ros_decoder_test_64 - ) - -# Pandar QT64 -ament_auto_add_library(hesai_ros_decoder_test_qt64 SHARED - hesai_ros_decoder_test_qt64.cpp - ) - -target_link_libraries(hesai_ros_decoder_test_qt64 ${PCL_LIBRARIES} ${NEBULA_TEST_LIBRARIES}) - -ament_add_gtest(hesai_ros_decoder_test_main_qt64 - hesai_ros_decoder_test_main_qt64.cpp - ) - -ament_target_dependencies(hesai_ros_decoder_test_main_qt64 - ${NEBULA_TEST_DEPENDENCIES} - ) -target_include_directories(hesai_ros_decoder_test_main_qt64 PUBLIC - ${PROJECT_SOURCE_DIR}/src/hesai - include - ) - -target_link_libraries(hesai_ros_decoder_test_main_qt64 - ${PCL_LIBRARIES} - hesai_ros_decoder_test_qt64 - ) - -# Pandar XT32 -ament_auto_add_library(hesai_ros_decoder_test_xt32 SHARED - hesai_ros_decoder_test_xt32.cpp - ) - -target_link_libraries(hesai_ros_decoder_test_xt32 ${PCL_LIBRARIES} ${NEBULA_TEST_LIBRARIES}) - -ament_add_gtest(hesai_ros_decoder_test_main_xt32 - hesai_ros_decoder_test_main_xt32.cpp - ) - -ament_target_dependencies(hesai_ros_decoder_test_main_xt32 - ${NEBULA_TEST_DEPENDENCIES} - ) - -target_include_directories(hesai_ros_decoder_test_main_xt32 PUBLIC - ${PROJECT_SOURCE_DIR}/src/hesai - include - ) - -target_link_libraries(hesai_ros_decoder_test_main_xt32 - ${PCL_LIBRARIES} - hesai_ros_decoder_test_xt32 + hesai_ros_decoder_test ) diff --git a/nebula_tests/hesai/hesai_common.hpp b/nebula_tests/hesai/hesai_common.hpp index 6cf085be0..784d8cd55 100644 --- a/nebula_tests/hesai/hesai_common.hpp +++ b/nebula_tests/hesai/hesai_common.hpp @@ -21,7 +21,7 @@ namespace nebula { -namespace ros +namespace test { void checkPCDs(nebula::drivers::NebulaPointCloudPtr pc, pcl::PointCloud::Ptr pc_ref) @@ -69,23 +69,5 @@ void printPCD(nebula::drivers::NebulaPointCloudPtr pp) } } -void checkTimestamp( - pandar_msgs::msg::PandarScan & raw_scan, nebula::drivers::HesaiScanDecoder & decoder) -{ - // first: self-check - putenv("TZ=GMT"); - tzset(); - auto gmt = timezone; - putenv("TZ=JST"); - tzset(); - auto jst = timezone; - - EXPECT_NE(gmt, jst); - - for (auto packet : raw_scan.packets) { - decoder.unpack(packet); - } -} - } // namespace ros } // namespace nebula \ No newline at end of file diff --git a/nebula_tests/hesai/hesai_ros_decoder_test.cpp b/nebula_tests/hesai/hesai_ros_decoder_test.cpp index b37b83567..bc6852d2d 100644 --- a/nebula_tests/hesai/hesai_ros_decoder_test.cpp +++ b/nebula_tests/hesai/hesai_ros_decoder_test.cpp @@ -1,4 +1,5 @@ #include "hesai_ros_decoder_test.hpp" + #include "rclcpp/serialization.hpp" #include "rclcpp/serialized_message.hpp" #include "rcpputils/filesystem_helper.hpp" @@ -166,7 +167,7 @@ Status HesaiRosDecoderTest::GetParameters( this->declare_parameter( "bag_path", (bag_root_dir / params_.bag_path).string(), descriptor); params_.bag_path = this->get_parameter("bag_path").as_string(); - std::cout << params_.bag_path << std::endl; + RCLCPP_DEBUG_STREAM(get_logger(), "Bag path relative to test root: " << params_.bag_path); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -248,15 +249,16 @@ Status HesaiRosDecoderTest::GetParameters( return Status::OK; } -void HesaiRosDecoderTest::ReadBag() +void HesaiRosDecoderTest::ReadBag( + std::function scan_callback) { rosbag2_storage::StorageOptions storage_options; rosbag2_cpp::ConverterOptions converter_options; - std::cout << params_.bag_path << std::endl; - std::cout << params_.storage_id << std::endl; - std::cout << params_.format << std::endl; - std::cout << params_.target_topic << std::endl; + RCLCPP_DEBUG_STREAM(get_logger(), params_.bag_path); + RCLCPP_DEBUG_STREAM(get_logger(), params_.storage_id); + RCLCPP_DEBUG_STREAM(get_logger(), params_.format); + RCLCPP_DEBUG_STREAM(get_logger(), params_.target_topic); auto target_topic_name = params_.target_topic; if (target_topic_name.substr(0, 1) == "/") { @@ -264,61 +266,38 @@ void HesaiRosDecoderTest::ReadBag() } target_topic_name = std::regex_replace(target_topic_name, std::regex("/"), "_"); - pcl::PCDReader pcd_reader; - rcpputils::fs::path bag_dir(params_.bag_path); - rcpputils::fs::path pcd_dir = bag_dir.parent_path(); - int check_cnt = 0; storage_options.uri = params_.bag_path; storage_options.storage_id = params_.storage_id; converter_options.output_serialization_format = params_.format; //"cdr"; rclcpp::Serialization serialization; nebula::drivers::NebulaPointCloudPtr pointcloud(new nebula::drivers::NebulaPointCloud); - // nebula::drivers::NebulaPointCloudPtr ref_pointcloud(new nebula::drivers::NebulaPointCloud); - pcl::PointCloud::Ptr ref_pointcloud(new pcl::PointCloud); - { - rosbag2_cpp::Reader bag_reader(std::make_unique()); - bag_reader.open(storage_options, converter_options); - while (bag_reader.has_next()) { - auto bag_message = bag_reader.read_next(); - std::cout << "Found topic name " << bag_message->topic_name << std::endl; + rosbag2_cpp::Reader bag_reader(std::make_unique()); + bag_reader.open(storage_options, converter_options); + while (bag_reader.has_next()) { + auto bag_message = bag_reader.read_next(); - if (bag_message->topic_name == params_.target_topic) { - pandar_msgs::msg::PandarScan extracted_msg; - rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data); - serialization.deserialize_message(&extracted_serialized_msg, &extracted_msg); + RCLCPP_DEBUG_STREAM(get_logger(), "Found topic name " << bag_message->topic_name); - std::cout << "Found data in topic " << bag_message->topic_name << ": " - << bag_message->time_stamp << std::endl; + if (bag_message->topic_name == params_.target_topic) { + pandar_msgs::msg::PandarScan extracted_msg; + rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data); + serialization.deserialize_message(&extracted_serialized_msg, &extracted_msg); - auto extracted_msg_ptr = std::make_shared(extracted_msg); - auto pointcloud_ts = driver_ptr_->ConvertScanToPointcloud(extracted_msg_ptr); - pointcloud = std::get<0>(pointcloud_ts); + RCLCPP_DEBUG_STREAM( + get_logger(), + "Found data in topic " << bag_message->topic_name << ": " << bag_message->time_stamp); - // There are very rare cases where has_scanned_ does not become true, but it is not known - // whether it is because of decoder or deserialize_message. - if (!pointcloud) continue; + auto extracted_msg_ptr = std::make_shared(extracted_msg); + auto pointcloud_ts = driver_ptr_->ConvertScanToPointcloud(extracted_msg_ptr); + auto scan_timestamp = std::get<1>(pointcloud_ts); + pointcloud = std::get<0>(pointcloud_ts); - auto fn = std::to_string(bag_message->time_stamp) + ".pcd"; - - auto target_pcd_path = (pcd_dir / fn); - std::cout << target_pcd_path << std::endl; - if (target_pcd_path.exists()) { - std::cout << "exists: " << target_pcd_path << std::endl; - auto rt = pcd_reader.read(target_pcd_path.string(), *ref_pointcloud); - std::cout << rt << " loaded: " << target_pcd_path << std::endl; - checkPCDs(pointcloud, ref_pointcloud); - check_cnt++; - // ref_pointcloud.reset(new nebula::drivers::NebulaPointCloud); - ref_pointcloud.reset(new pcl::PointCloud); - } - pointcloud.reset(new nebula::drivers::NebulaPointCloud); - } + scan_callback(bag_message->time_stamp, scan_timestamp, pointcloud); + pointcloud.reset(new nebula::drivers::NebulaPointCloud); } - EXPECT_GT(check_cnt, 0); - // close on scope exit } } diff --git a/nebula_tests/hesai/hesai_ros_decoder_test.hpp b/nebula_tests/hesai/hesai_ros_decoder_test.hpp index 58ec80f40..a055e1b2f 100644 --- a/nebula_tests/hesai/hesai_ros_decoder_test.hpp +++ b/nebula_tests/hesai/hesai_ros_decoder_test.hpp @@ -16,6 +16,8 @@ #include +#include + #ifndef _SRC_CALIBRATION_DIR_PATH #define _SRC_CALIBRATION_DIR_PATH "" #endif @@ -106,19 +108,8 @@ class HesaiRosDecoderTest final : public rclcpp::Node, NebulaDriverRosWrapperBas /// @brief Read the specified bag file and compare the constructed point clouds with the /// corresponding PCD files - void ReadBag(); - - /* - void SetUp() override { - // Setup things that should occur before every test instance should go here - RCLCPP_ERROR_STREAM(this->get_logger(), "DONE WITH SETUP!!"); - } + void ReadBag(std::function scan_callback); - void TearDown() override { - std::cout << "DONE WITH TEARDOWN" << std::endl; - } -*/ -private: HesaiRosDecoderTestParams params_; }; diff --git a/nebula_tests/hesai/hesai_ros_decoder_test_64.cpp b/nebula_tests/hesai/hesai_ros_decoder_test_64.cpp deleted file mode 100644 index 994dc2512..000000000 --- a/nebula_tests/hesai/hesai_ros_decoder_test_64.cpp +++ /dev/null @@ -1,353 +0,0 @@ -#include "hesai_ros_decoder_test_64.hpp" - -#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 - -#include -#include - -namespace nebula -{ -namespace ros -{ -HesaiRosDecoderTest::HesaiRosDecoderTest( - const rclcpp::NodeOptions & options, const std::string & node_name) -: rclcpp::Node(node_name, options) -{ - drivers::HesaiCalibrationConfiguration calibration_configuration; - drivers::HesaiSensorConfiguration sensor_configuration; - drivers::HesaiCorrection correction_configuration; - - 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_); - return; - } - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Starting..."); - - calibration_cfg_ptr_ = - std::make_shared(calibration_configuration); - - sensor_cfg_ptr_ = std::make_shared(sensor_configuration); - - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Driver "); - if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { - correction_cfg_ptr_ = std::make_shared(correction_configuration); - wrapper_status_ = InitializeDriver( - std::const_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_), - std::static_pointer_cast(correction_cfg_ptr_)); - } else { - wrapper_status_ = InitializeDriver( - std::const_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_)); - } - - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); -} - -Status HesaiRosDecoderTest::InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) -{ - // driver should be initialized here with proper decoder - driver_ptr_ = std::make_shared( - std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast(calibration_configuration)); - return driver_ptr_->GetStatus(); -} - -Status HesaiRosDecoderTest::InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration, - std::shared_ptr correction_configuration) -{ - // driver should be initialized here with proper decoder - driver_ptr_ = std::make_shared( - std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast( - calibration_configuration), //); - std::static_pointer_cast(correction_configuration)); - return driver_ptr_->GetStatus(); -} - -Status HesaiRosDecoderTest::GetStatus() -{ - return wrapper_status_; -} - -Status HesaiRosDecoderTest::GetParameters( - drivers::HesaiSensorConfiguration & sensor_configuration, - drivers::HesaiCalibrationConfiguration & calibration_configuration, - drivers::HesaiCorrection & correction_configuration) -{ - std::filesystem::path calib_dir = - _SRC_CALIBRATION_DIR_PATH; // variable defined in CMakeLists.txt; - calib_dir /= "hesai"; - std::filesystem::path bag_root_dir = - _SRC_RESOURCES_DIR_PATH; // variable defined in CMakeLists.txt; - bag_root_dir /= "hesai"; - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_model", "Pandar64"); - sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("return_mode", "Dual", descriptor); - sensor_configuration.return_mode = nebula::drivers::ReturnModeFromStringHesai( - this->get_parameter("return_mode").as_string(), sensor_configuration.sensor_model); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", "hesai", descriptor); - sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 3; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = "Angle where scans begin (degrees, [0.,360.]"; - rcl_interfaces::msg::FloatingPointRange range; - range.set__from_value(0).set__to_value(360).set__step(0.01); - descriptor.floating_point_range = {range}; - this->declare_parameter("scan_phase", 0., descriptor); - sensor_configuration.scan_phase = this->get_parameter("scan_phase").as_double(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter( - "calibration_file", (calib_dir / "Pandar64.csv").string(), descriptor); - calibration_configuration.calibration_file = - this->get_parameter("calibration_file").as_string(); - } - if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter( - "correction_file", (calib_dir / "PandarAT128.dat").string(), descriptor); - correction_file_path = this->get_parameter("correction_file").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter( - "bag_path", (bag_root_dir / "64" / "1673403880599376836").string(), descriptor); - bag_path = this->get_parameter("bag_path").as_string(); - std::cout << bag_path << std::endl; - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("storage_id", "sqlite3", descriptor); - storage_id = this->get_parameter("storage_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("format", "cdr", descriptor); - format = this->get_parameter("format").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("target_topic", "/pandar_packets", descriptor); - target_topic = this->get_parameter("target_topic").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = "Dual return distance threshold [0.01, 0.5]"; - rcl_interfaces::msg::FloatingPointRange range; - range.set__from_value(0.00).set__to_value(0.5).set__step(0.01); - descriptor.floating_point_range = {range}; - this->declare_parameter("dual_return_distance_threshold", 0.1, descriptor); - sensor_configuration.dual_return_distance_threshold = - this->get_parameter("dual_return_distance_threshold").as_double(); - } - - if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { - return Status::INVALID_SENSOR_MODEL; - } - if (sensor_configuration.return_mode == nebula::drivers::ReturnMode::UNKNOWN) { - return Status::INVALID_ECHO_MODE; - } - if (sensor_configuration.frame_id.empty() || sensor_configuration.scan_phase > 360) { - return Status::SENSOR_CONFIG_ERROR; - } - if (calibration_configuration.calibration_file.empty()) { - return Status::INVALID_CALIBRATION_FILE; - } else { - auto cal_status = - calibration_configuration.LoadFromFile(calibration_configuration.calibration_file); - if (cal_status != Status::OK) { - RCLCPP_ERROR_STREAM( - this->get_logger(), - "Given Calibration File: '" << calibration_configuration.calibration_file << "'"); - return cal_status; - } - } - if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { - if (correction_file_path.empty()) { - return Status::INVALID_CALIBRATION_FILE; - } else { - auto cal_status = correction_configuration.LoadFromFile(correction_file_path); - if (cal_status != Status::OK) { - RCLCPP_ERROR_STREAM( - this->get_logger(), "Given Correction File: '" << correction_file_path << "'"); - return cal_status; - } - } - } - - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); - return Status::OK; -} - -void printPCD(nebula::drivers::NebulaPointCloudPtr pp) -{ - for (auto p : pp->points) { - std::cout << "(" << p.x << ", " << p.y << "," << p.z << "): " << p.intensity << ", " - << p.channel << ", " << p.azimuth << ", " << p.return_type << ", " << p.time_stamp - << std::endl; - } -} - -void checkPCDs(nebula::drivers::NebulaPointCloudPtr pp1, nebula::drivers::NebulaPointCloudPtr pp2) -{ - EXPECT_EQ(pp1->points.size(), pp2->points.size()); - for (uint32_t i = 0; i < pp1->points.size(); i++) { - auto p1 = pp1->points[i]; - auto p2 = pp2->points[i]; - EXPECT_FLOAT_EQ(p1.x, p2.x); - EXPECT_FLOAT_EQ(p1.y, p2.y); - EXPECT_FLOAT_EQ(p1.z, p2.z); - EXPECT_FLOAT_EQ(p1.intensity, p2.intensity); - EXPECT_EQ(p1.channel, p2.channel); - EXPECT_FLOAT_EQ(p1.azimuth, p2.azimuth); - EXPECT_EQ(p1.return_type, p2.return_type); - EXPECT_DOUBLE_EQ(p1.time_stamp, p2.time_stamp); - } -} - -void HesaiRosDecoderTest::ReadBag() -{ - rosbag2_storage::StorageOptions storage_options; - rosbag2_cpp::ConverterOptions converter_options; - - std::cout << bag_path << std::endl; - std::cout << storage_id << std::endl; - std::cout << format << std::endl; - std::cout << target_topic << std::endl; - - auto target_topic_name = target_topic; - if (target_topic_name.substr(0, 1) == "/") { - target_topic_name = target_topic_name.substr(1); - } - target_topic_name = std::regex_replace(target_topic_name, std::regex("/"), "_"); - - pcl::PCDReader pcd_reader; - - rcpputils::fs::path bag_dir(bag_path); - rcpputils::fs::path pcd_dir = bag_dir.parent_path(); - int check_cnt = 0; - - storage_options.uri = bag_path; - storage_options.storage_id = storage_id; - converter_options.output_serialization_format = format; //"cdr"; - rclcpp::Serialization serialization; - nebula::drivers::NebulaPointCloudPtr pointcloud(new nebula::drivers::NebulaPointCloud); - // nebula::drivers::NebulaPointCloudPtr ref_pointcloud(new nebula::drivers::NebulaPointCloud); - pcl::PointCloud::Ptr ref_pointcloud(new pcl::PointCloud); - { - rosbag2_cpp::Reader bag_reader(std::make_unique()); - bag_reader.open(storage_options, converter_options); - while (bag_reader.has_next()) { - auto bag_message = bag_reader.read_next(); - - std::cout << "Found topic name " << bag_message->topic_name << std::endl; - - if (bag_message->topic_name == target_topic) { - pandar_msgs::msg::PandarScan extracted_msg; - rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data); - serialization.deserialize_message(&extracted_serialized_msg, &extracted_msg); - - std::cout << "Found data in topic " << bag_message->topic_name << ": " - << bag_message->time_stamp << std::endl; - - auto extracted_msg_ptr = std::make_shared(extracted_msg); - auto pointcloud_ts = driver_ptr_->ConvertScanToPointcloud(extracted_msg_ptr); - pointcloud = std::get<0>(pointcloud_ts); - - // There are very rare cases where has_scanned_ does not become true, but it is not known - // whether it is because of decoder or deserialize_message. - if (!pointcloud) continue; - - auto fn = std::to_string(bag_message->time_stamp) + ".pcd"; - - auto target_pcd_path = (pcd_dir / fn); - std::cout << target_pcd_path << std::endl; - if (target_pcd_path.exists()) { - std::cout << "exists: " << target_pcd_path << std::endl; - auto rt = pcd_reader.read(target_pcd_path.string(), *ref_pointcloud); - std::cout << rt << " loaded: " << target_pcd_path << std::endl; - checkPCDs(pointcloud, ref_pointcloud); - check_cnt++; - // ref_pointcloud.reset(new nebula::drivers::NebulaPointCloud); - ref_pointcloud.reset(new pcl::PointCloud); - } - pointcloud.reset(new nebula::drivers::NebulaPointCloud); - } - } - EXPECT_GT(check_cnt, 0); - // close on scope exit - } -} - -} // namespace ros -} // namespace nebula diff --git a/nebula_tests/hesai/hesai_ros_decoder_test_64.hpp b/nebula_tests/hesai/hesai_ros_decoder_test_64.hpp deleted file mode 100644 index 8ae9116d7..000000000 --- a/nebula_tests/hesai/hesai_ros_decoder_test_64.hpp +++ /dev/null @@ -1,103 +0,0 @@ -#ifndef NEBULA_HesaiRosDecoderTest64_H -#define NEBULA_HesaiRosDecoderTest64_H - -#include "nebula_common/hesai/hesai_common.hpp" -#include "nebula_common/nebula_common.hpp" -#include "nebula_common/nebula_status.hpp" -#include "nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp" -#include "nebula_ros/common/nebula_driver_ros_wrapper_base.hpp" - -#include -#include -#include - -#include "pandar_msgs/msg/pandar_packet.hpp" -#include "pandar_msgs/msg/pandar_scan.hpp" -#include "hesai_common.hpp" - -#include - -namespace nebula -{ -namespace ros -{ -/// @brief Testing decoder of pandar 64 (Keeps HesaiDriverRosWrapper structure as much as possible) -class HesaiRosDecoderTest final : public rclcpp::Node, NebulaDriverRosWrapperBase //, testing::Test -{ - std::shared_ptr driver_ptr_; - Status wrapper_status_; - - std::shared_ptr calibration_cfg_ptr_; - std::shared_ptr sensor_cfg_ptr_; - std::shared_ptr correction_cfg_ptr_; - - /// @brief Initializing ros wrapper - /// @param sensor_configuration SensorConfiguration for this driver - /// @param calibration_configuration CalibrationConfiguration for this driver - /// @return Resulting status - Status InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) override; - - /// @brief Initializing ros wrapper for AT - /// @param sensor_configuration SensorConfiguration for this driver - /// @param calibration_configuration CalibrationConfiguration for this driver - /// @param correction_configuration CorrectionConfiguration for this driver - /// @return Resulting status - Status InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration, - std::shared_ptr correction_configuration); - - /// @brief Get configurations (Magic numbers for Pandar64 is described, each function can be - /// integrated if the ros parameter can be passed to Google Test) - /// @param sensor_configuration Output of SensorConfiguration - /// @param calibration_configuration Output of CalibrationConfiguration - /// @param correction_configuration Output of CorrectionConfiguration (for AT) - /// @return Resulting status - Status GetParameters( - drivers::HesaiSensorConfiguration & sensor_configuration, - drivers::HesaiCalibrationConfiguration & calibration_configuration, - drivers::HesaiCorrection & correction_configuration); - - /// @brief Convert seconds to chrono::nanoseconds - /// @param seconds - /// @return chrono::nanoseconds - static inline std::chrono::nanoseconds SecondsToChronoNanoSeconds(const double seconds) - { - return std::chrono::duration_cast( - std::chrono::duration(seconds)); - } - -public: - explicit HesaiRosDecoderTest(const rclcpp::NodeOptions & options, const std::string & node_name); - - /// @brief Get current status of this driver - /// @return Current status - Status GetStatus(); - - /// @brief Read the specified bag file and compare the constructed point clouds with the - /// corresponding PCD files - void ReadBag(); - /* - void SetUp() override { - // Setup things that should occur before every test instance should go here - RCLCPP_ERROR_STREAM(this->get_logger(), "DONE WITH SETUP!!"); - } - - void TearDown() override { - std::cout << "DONE WITH TEARDOWN" << std::endl; - } -*/ -private: - std::string bag_path; - std::string storage_id; - std::string format; - std::string target_topic; - std::string correction_file_path; -}; - -} // namespace ros -} // namespace nebula - -#endif // NEBULA_HesaiRosDecoderTest64_H diff --git a/nebula_tests/hesai/hesai_ros_decoder_test_at128.cpp b/nebula_tests/hesai/hesai_ros_decoder_test_at128.cpp deleted file mode 100644 index 84660178a..000000000 --- a/nebula_tests/hesai/hesai_ros_decoder_test_at128.cpp +++ /dev/null @@ -1,353 +0,0 @@ -#include "hesai_ros_decoder_test_at128.hpp" - -#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 - -#include -#include - -namespace nebula -{ -namespace ros -{ -HesaiRosDecoderTest::HesaiRosDecoderTest( - const rclcpp::NodeOptions & options, const std::string & node_name) -: rclcpp::Node(node_name, options) -{ - drivers::HesaiCalibrationConfiguration calibration_configuration; - drivers::HesaiSensorConfiguration sensor_configuration; - drivers::HesaiCorrection correction_configuration; - - 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_); - return; - } - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Starting..."); - - calibration_cfg_ptr_ = - std::make_shared(calibration_configuration); - - sensor_cfg_ptr_ = std::make_shared(sensor_configuration); - - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Driver "); - if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { - correction_cfg_ptr_ = std::make_shared(correction_configuration); - wrapper_status_ = InitializeDriver( - std::const_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_), - std::static_pointer_cast(correction_cfg_ptr_)); - } else { - wrapper_status_ = InitializeDriver( - std::const_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_)); - } - - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); -} - -Status HesaiRosDecoderTest::InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) -{ - // driver should be initialized here with proper decoder - driver_ptr_ = std::make_shared( - std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast(calibration_configuration)); - return driver_ptr_->GetStatus(); -} - -Status HesaiRosDecoderTest::InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration, - std::shared_ptr correction_configuration) -{ - // driver should be initialized here with proper decoder - driver_ptr_ = std::make_shared( - std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast( - calibration_configuration), //); - std::static_pointer_cast(correction_configuration)); - return driver_ptr_->GetStatus(); -} - -Status HesaiRosDecoderTest::GetStatus() -{ - return wrapper_status_; -} - -Status HesaiRosDecoderTest::GetParameters( - drivers::HesaiSensorConfiguration & sensor_configuration, - drivers::HesaiCalibrationConfiguration & calibration_configuration, - drivers::HesaiCorrection & correction_configuration) -{ - std::filesystem::path calib_dir = - _SRC_CALIBRATION_DIR_PATH; // variable defined in CMakeLists.txt; - calib_dir /= "hesai"; - std::filesystem::path bag_root_dir = - _SRC_RESOURCES_DIR_PATH; // variable defined in CMakeLists.txt; - bag_root_dir /= "hesai"; - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_model", "PandarAT128"); - sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("return_mode", "LastStrongest", descriptor); - sensor_configuration.return_mode = nebula::drivers::ReturnModeFromStringHesai( - this->get_parameter("return_mode").as_string(), sensor_configuration.sensor_model); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", "hesai", descriptor); - sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 3; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = "Angle where scans begin (degrees, [0.,360.]"; - rcl_interfaces::msg::FloatingPointRange range; - range.set__from_value(0).set__to_value(360).set__step(0.01); - descriptor.floating_point_range = {range}; - this->declare_parameter("scan_phase", 0., descriptor); - sensor_configuration.scan_phase = this->get_parameter("scan_phase").as_double(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter( - "calibration_file", (calib_dir / "PandarAT128.csv").string(), descriptor); - calibration_configuration.calibration_file = - this->get_parameter("calibration_file").as_string(); - } - if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter( - "correction_file", (calib_dir / "PandarAT128.dat").string(), descriptor); - correction_file_path = this->get_parameter("correction_file").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter( - "bag_path", (bag_root_dir / "at128" / "1679653308406038376").string(), descriptor); - bag_path = this->get_parameter("bag_path").as_string(); - std::cout << bag_path << std::endl; - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("storage_id", "sqlite3", descriptor); - storage_id = this->get_parameter("storage_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("format", "cdr", descriptor); - format = this->get_parameter("format").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("target_topic", "/pandar_packets", descriptor); - target_topic = this->get_parameter("target_topic").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = "Dual return distance threshold [0.01, 0.5]"; - rcl_interfaces::msg::FloatingPointRange range; - range.set__from_value(0.00).set__to_value(0.5).set__step(0.01); - descriptor.floating_point_range = {range}; - this->declare_parameter("dual_return_distance_threshold", 0.1, descriptor); - sensor_configuration.dual_return_distance_threshold = - this->get_parameter("dual_return_distance_threshold").as_double(); - } - - if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { - return Status::INVALID_SENSOR_MODEL; - } - if (sensor_configuration.return_mode == nebula::drivers::ReturnMode::UNKNOWN) { - return Status::INVALID_ECHO_MODE; - } - if (sensor_configuration.frame_id.empty() || sensor_configuration.scan_phase > 360) { - return Status::SENSOR_CONFIG_ERROR; - } - if (calibration_configuration.calibration_file.empty()) { - return Status::INVALID_CALIBRATION_FILE; - } else { - auto cal_status = - calibration_configuration.LoadFromFile(calibration_configuration.calibration_file); - if (cal_status != Status::OK) { - RCLCPP_ERROR_STREAM( - this->get_logger(), - "Given Calibration File: '" << calibration_configuration.calibration_file << "'"); - return cal_status; - } - } - if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { - if (correction_file_path.empty()) { - return Status::INVALID_CALIBRATION_FILE; - } else { - auto cal_status = correction_configuration.LoadFromFile(correction_file_path); - if (cal_status != Status::OK) { - RCLCPP_ERROR_STREAM( - this->get_logger(), "Given Correction File: '" << correction_file_path << "'"); - return cal_status; - } - } - } - - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); - return Status::OK; -} - -void printPCD(nebula::drivers::NebulaPointCloudPtr pp) -{ - for (auto p : pp->points) { - std::cout << "(" << p.x << ", " << p.y << "," << p.z << "): " << p.intensity << ", " - << p.channel << ", " << p.azimuth << ", " << p.return_type << ", " << p.time_stamp - << std::endl; - } -} - -void checkPCDs(nebula::drivers::NebulaPointCloudPtr pp1, nebula::drivers::NebulaPointCloudPtr pp2) -{ - EXPECT_EQ(pp1->points.size(), pp2->points.size()); - for (uint32_t i = 0; i < pp1->points.size(); i++) { - auto p1 = pp1->points[i]; - auto p2 = pp2->points[i]; - EXPECT_FLOAT_EQ(p1.x, p2.x); - EXPECT_FLOAT_EQ(p1.y, p2.y); - EXPECT_FLOAT_EQ(p1.z, p2.z); - EXPECT_FLOAT_EQ(p1.intensity, p2.intensity); - EXPECT_EQ(p1.channel, p2.channel); - EXPECT_FLOAT_EQ(p1.azimuth, p2.azimuth); - EXPECT_EQ(p1.return_type, p2.return_type); - EXPECT_DOUBLE_EQ(p1.time_stamp, p2.time_stamp); - } -} - -void HesaiRosDecoderTest::ReadBag() -{ - rosbag2_storage::StorageOptions storage_options; - rosbag2_cpp::ConverterOptions converter_options; - - std::cout << bag_path << std::endl; - std::cout << storage_id << std::endl; - std::cout << format << std::endl; - std::cout << target_topic << std::endl; - - auto target_topic_name = target_topic; - if (target_topic_name.substr(0, 1) == "/") { - target_topic_name = target_topic_name.substr(1); - } - target_topic_name = std::regex_replace(target_topic_name, std::regex("/"), "_"); - - pcl::PCDReader pcd_reader; - - rcpputils::fs::path bag_dir(bag_path); - rcpputils::fs::path pcd_dir = bag_dir.parent_path(); - int check_cnt = 0; - - storage_options.uri = bag_path; - storage_options.storage_id = storage_id; - converter_options.output_serialization_format = format; //"cdr"; - rclcpp::Serialization serialization; - nebula::drivers::NebulaPointCloudPtr pointcloud(new nebula::drivers::NebulaPointCloud); - // nebula::drivers::NebulaPointCloudPtr ref_pointcloud(new nebula::drivers::NebulaPointCloud); - pcl::PointCloud::Ptr ref_pointcloud(new pcl::PointCloud); - { - rosbag2_cpp::Reader bag_reader(std::make_unique()); - bag_reader.open(storage_options, converter_options); - while (bag_reader.has_next()) { - auto bag_message = bag_reader.read_next(); - - std::cout << "Found topic name " << bag_message->topic_name << std::endl; - - if (bag_message->topic_name == target_topic) { - pandar_msgs::msg::PandarScan extracted_msg; - rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data); - serialization.deserialize_message(&extracted_serialized_msg, &extracted_msg); - - std::cout << "Found data in topic " << bag_message->topic_name << ": " - << bag_message->time_stamp << std::endl; - - auto extracted_msg_ptr = std::make_shared(extracted_msg); - auto pointcloud_ts = driver_ptr_->ConvertScanToPointcloud(extracted_msg_ptr); - pointcloud = std::get<0>(pointcloud_ts); - - // There are very rare cases where has_scanned_ does not become true, but it is not known - // whether it is because of decoder or deserialize_message. - if (!pointcloud) continue; - - auto fn = std::to_string(bag_message->time_stamp) + ".pcd"; - - auto target_pcd_path = (pcd_dir / fn); - std::cout << target_pcd_path << std::endl; - if (target_pcd_path.exists()) { - std::cout << "exists: " << target_pcd_path << std::endl; - auto rt = pcd_reader.read(target_pcd_path.string(), *ref_pointcloud); - std::cout << rt << " loaded: " << target_pcd_path << std::endl; - checkPCDs(pointcloud, ref_pointcloud); - check_cnt++; - // ref_pointcloud.reset(new nebula::drivers::NebulaPointCloud); - ref_pointcloud.reset(new pcl::PointCloud); - } - pointcloud.reset(new nebula::drivers::NebulaPointCloud); - } - } - EXPECT_GT(check_cnt, 0); - // close on scope exit - } -} - -} // namespace ros -} // namespace nebula diff --git a/nebula_tests/hesai/hesai_ros_decoder_test_at128.hpp b/nebula_tests/hesai/hesai_ros_decoder_test_at128.hpp deleted file mode 100644 index c7bfebb26..000000000 --- a/nebula_tests/hesai/hesai_ros_decoder_test_at128.hpp +++ /dev/null @@ -1,106 +0,0 @@ -//#ifndef NEBULA_HesaiRosDecoderTest_H -//#define NEBULA_HesaiRosDecoderTest_H -#ifndef NEBULA_HesaiRosDecoderTestAt128_H -#define NEBULA_HesaiRosDecoderTestAt128_H - -#include "nebula_common/hesai/hesai_common.hpp" -#include "nebula_common/nebula_common.hpp" -#include "nebula_common/nebula_status.hpp" -#include "nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp" -#include "nebula_ros/common/nebula_driver_ros_wrapper_base.hpp" - -#include -#include -#include - -#include "pandar_msgs/msg/pandar_packet.hpp" -#include "pandar_msgs/msg/pandar_scan.hpp" -#include "hesai_common.hpp" - -#include - -namespace nebula -{ -namespace ros -{ -/// @brief Testing decoder of AT128 (Keeps HesaiDriverRosWrapper structure as much as possible) -class HesaiRosDecoderTest final : public rclcpp::Node, NebulaDriverRosWrapperBase //, testing::Test -{ - std::shared_ptr driver_ptr_; - Status wrapper_status_; - - std::shared_ptr calibration_cfg_ptr_; - std::shared_ptr sensor_cfg_ptr_; - std::shared_ptr correction_cfg_ptr_; - - /// @brief Initializing ros wrapper - /// @param sensor_configuration SensorConfiguration for this driver - /// @param calibration_configuration CalibrationConfiguration for this driver - /// @return Resulting status - Status InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) override; - - /// @brief Initializing ros wrapper for AT - /// @param sensor_configuration SensorConfiguration for this driver - /// @param calibration_configuration CalibrationConfiguration for this driver - /// @param correction_configuration CorrectionConfiguration for this driver - /// @return Resulting status - Status InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration, - std::shared_ptr correction_configuration); - - /// @brief Get configurations (Magic numbers for AT128 is described, each function can be - /// integrated if the ros parameter can be passed to Google Test) - /// @param sensor_configuration Output of SensorConfiguration - /// @param calibration_configuration Output of CalibrationConfiguration - /// @param correction_configuration Output of CorrectionConfiguration (for AT) - /// @return Resulting status - Status GetParameters( - drivers::HesaiSensorConfiguration & sensor_configuration, - drivers::HesaiCalibrationConfiguration & calibration_configuration, - drivers::HesaiCorrection & correction_configuration); - - /// @brief Convert seconds to chrono::nanoseconds - /// @param seconds - /// @return chrono::nanoseconds - static inline std::chrono::nanoseconds SecondsToChronoNanoSeconds(const double seconds) - { - return std::chrono::duration_cast( - std::chrono::duration(seconds)); - } - -public: - explicit HesaiRosDecoderTest(const rclcpp::NodeOptions & options, const std::string & node_name); - - /// @brief Get current status of this driver - /// @return Current status - Status GetStatus(); - - /// @brief Read the specified bag file and compare the constructed point clouds with the - /// corresponding PCD files - void ReadBag(); - /* - void SetUp() override { - // Setup things that should occur before every test instance should go here - RCLCPP_ERROR_STREAM(this->get_logger(), "DONE WITH SETUP!!"); - } - - void TearDown() override { - std::cout << "DONE WITH TEARDOWN" << std::endl; - } -*/ -private: - std::string bag_path; - std::string storage_id; - std::string format; - std::string target_topic; - std::string correction_file_path; -}; - -} // namespace ros -} // namespace nebula - -#endif // NEBULA_HesaiRosDecoderTestAt128_H -//#endif // NEBULA_HesaiRosDecoderTest_H diff --git a/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp b/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp index 429ad50d4..a6913b4ca 100644 --- a/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp +++ b/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp @@ -1,17 +1,92 @@ +#include "hesai_ros_decoder_test_main.hpp" + +#include "hesai_common.hpp" #include "hesai_ros_decoder_test.hpp" #include #include +#include +#include +#include +#include #include -std::shared_ptr hesai_driver; +namespace nebula +{ +namespace test +{ + +const nebula::ros::HesaiRosDecoderTestParams TEST_CONFIGS[6] = { + {.sensor_model = "Pandar40P", + .return_mode = "Dual", + .calibration_file = "Pandar40P.csv", + .bag_path = "40p/1673400149412331409"}, + { + .sensor_model = "Pandar64", + .return_mode = "Dual", + .calibration_file = "Pandar64.csv", + .bag_path = "64/1673403880599376836", + }, + { + .sensor_model = "PandarAT128", + .return_mode = "LastStrongest", + .calibration_file = "PandarAT128.csv", + .correction_file = "PandarAT128.dat", + .bag_path = "at128/1679653308406038376", + }, + { + .sensor_model = "PandarQT64", + .return_mode = "Dual", + .calibration_file = "PandarQT64.csv", + .bag_path = "qt64/1673401195788312575", + }, + { + .sensor_model = "PandarXT32", + .return_mode = "Dual", + .calibration_file = "PandarXT32.csv", + .bag_path = "xt32/1673400677802009732", + }, + { + .sensor_model = "PandarXT32M", + .return_mode = "LastStrongest", + .calibration_file = "PandarXT32M.csv", + .bag_path = "xt32m/1660893203042895158", + }}; -TEST(TestDecoder, TestPcd) +// Compares geometrical output of decoder against pre-recorded reference pointcloud. +TEST_P(DecoderTest, TestPcd) { - std::cout << "TEST(TestDecoder, TestPcd)" << std::endl; - hesai_driver->ReadBag(); + pcl::PCDReader pcd_reader; + rcpputils::fs::path bag_dir(hesai_driver_->params_.bag_path); + rcpputils::fs::path pcd_dir = bag_dir.parent_path(); + + pcl::PointCloud::Ptr ref_pointcloud(new pcl::PointCloud); + int check_cnt = 0; + + auto scan_callback = [&]( + uint64_t msg_timestamp, uint64_t scan_timestamp, + nebula::drivers::NebulaPointCloudPtr pointcloud) { + if (!pointcloud) return; + + auto fn = std::to_string(msg_timestamp) + ".pcd"; + + auto target_pcd_path = (pcd_dir / fn); + RCLCPP_DEBUG_STREAM(*logger_, target_pcd_path); + if (target_pcd_path.exists()) { + RCLCPP_DEBUG_STREAM(*logger_, "exists: " << target_pcd_path); + auto rt = pcd_reader.read(target_pcd_path.string(), *ref_pointcloud); + RCLCPP_DEBUG_STREAM(*logger_, rt << " loaded: " << target_pcd_path); + checkPCDs(pointcloud, ref_pointcloud); + check_cnt++; + // ref_pointcloud.reset(new nebula::drivers::NebulaPointCloud); + ref_pointcloud.reset(new pcl::PointCloud); + } + }; + + hesai_driver_->ReadBag(scan_callback); + EXPECT_GT(check_cnt, 0); } @@ -19,38 +94,53 @@ TEST(TestDecoder, TestPcd) int main(int argc, char * argv[]) { std::cout << "hesai_ros_decoder_test_main.cpp" << std::endl; + +void DecoderTest::SetUp() +{ + auto decoder_params = GetParam(); + logger_ = std::make_shared(rclcpp::get_logger("UnitTest")); + logger_->set_level(rclcpp::Logger::Level::Info); + + RCLCPP_DEBUG_STREAM(*logger_, "Testing " << decoder_params.sensor_model); setvbuf(stdout, NULL, _IONBF, BUFSIZ); std::string node_name = "nebula_hesai_decoder_test"; - rclcpp::init(argc, argv); - ::testing::InitGoogleTest(&argc, argv); rclcpp::executors::SingleThreadedExecutor exec; rclcpp::NodeOptions options; - nebula::ros::HesaiRosDecoderTestParams decoder_params; - decoder_params.sensor_model = "Pandar40P"; - decoder_params.return_mode = "Dual"; - decoder_params.calibration_file = "Pandar40P.csv"; - decoder_params.bag_path = "40p/1673400149412331409"; - - hesai_driver = + hesai_driver_ = std::make_shared(options, node_name, decoder_params); - exec.add_node(hesai_driver->get_node_base_interface()); - - RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Get Status"); - nebula::Status driver_status = hesai_driver->GetStatus(); - int result = 0; - if (driver_status == nebula::Status::OK) { - RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Reading Started"); - // hesai_driver->ReadBag(); - result = RUN_ALL_TESTS(); - } else { - RCLCPP_ERROR_STREAM(rclcpp::get_logger(node_name), driver_status); + exec.add_node(hesai_driver_->get_node_base_interface()); + + nebula::Status driver_status = hesai_driver_->GetStatus(); + if (driver_status != nebula::Status::OK) { + throw std::runtime_error("Could not initialize driver"); } - RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Ending"); - hesai_driver.reset(); - rclcpp::shutdown(); +} + +void DecoderTest::TearDown() +{ + RCLCPP_INFO_STREAM(*logger_, "Tearing down"); + hesai_driver_.reset(); + logger_.reset(); +} + +INSTANTIATE_TEST_SUITE_P( + TestMain, DecoderTest, testing::ValuesIn(TEST_CONFIGS), + [](const testing::TestParamInfo & p) { + return p.param.sensor_model; + }); +} // namespace test +} // namespace nebula + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); return result; } diff --git a/nebula_tests/hesai/hesai_ros_decoder_test_main.hpp b/nebula_tests/hesai/hesai_ros_decoder_test_main.hpp new file mode 100644 index 000000000..dca0f05d7 --- /dev/null +++ b/nebula_tests/hesai/hesai_ros_decoder_test_main.hpp @@ -0,0 +1,26 @@ +#pragma once + +#include "hesai_ros_decoder_test.hpp" + +#include +#include + +namespace nebula +{ +namespace test +{ +class DecoderTest : public ::testing::TestWithParam +{ +protected: + /// @brief Instantiates the Hesai driver node with the given test parameters + void SetUp() override; + + /// @brief Destroys the Hesai driver node + void TearDown() override; + + std::shared_ptr hesai_driver_; + std::shared_ptr logger_; +}; + +} // namespace test +} // namespace nebula diff --git a/nebula_tests/hesai/hesai_ros_decoder_test_main_64.cpp b/nebula_tests/hesai/hesai_ros_decoder_test_main_64.cpp deleted file mode 100644 index 226cc8163..000000000 --- a/nebula_tests/hesai/hesai_ros_decoder_test_main_64.cpp +++ /dev/null @@ -1,47 +0,0 @@ -#include "hesai_ros_decoder_test_64.hpp" - -#include - -#include - -#include - -std::shared_ptr hesai_driver; - -TEST(TestDecoder, TestPcd) -{ - std::cout << "TEST(TestDecoder, TestPcd)" << std::endl; - hesai_driver->ReadBag(); -} - -int main(int argc, char * argv[]) -{ - std::cout << "hesai_ros_decoder_test_main_64.cpp" << std::endl; - setvbuf(stdout, NULL, _IONBF, BUFSIZ); - - std::string node_name = "nebula_hesai_decoder_test"; - - rclcpp::init(argc, argv); - ::testing::InitGoogleTest(&argc, argv); - rclcpp::executors::SingleThreadedExecutor exec; - rclcpp::NodeOptions options; - - hesai_driver = std::make_shared(options, node_name); - exec.add_node(hesai_driver->get_node_base_interface()); - - RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Get Status"); - nebula::Status driver_status = hesai_driver->GetStatus(); - int result = 0; - if (driver_status == nebula::Status::OK) { - RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Reading Started"); - // hesai_driver->ReadBag(); - result = RUN_ALL_TESTS(); - } else { - RCLCPP_ERROR_STREAM(rclcpp::get_logger(node_name), driver_status); - } - RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Ending"); - hesai_driver.reset(); - rclcpp::shutdown(); - - return result; -} diff --git a/nebula_tests/hesai/hesai_ros_decoder_test_main_at128.cpp b/nebula_tests/hesai/hesai_ros_decoder_test_main_at128.cpp deleted file mode 100644 index 0f3d14e55..000000000 --- a/nebula_tests/hesai/hesai_ros_decoder_test_main_at128.cpp +++ /dev/null @@ -1,49 +0,0 @@ -//#include "hesai/hesai_ros_decoder_test.hpp" -//#include "hesai_ros_decoder_test.hpp" -#include "hesai_ros_decoder_test_at128.hpp" - -#include - -#include - -#include - -std::shared_ptr hesai_driver; - -TEST(TestDecoder, TestPcd) -{ - std::cout << "TEST(TestDecoder, TestPcd)" << std::endl; - hesai_driver->ReadBag(); -} - -int main(int argc, char * argv[]) -{ - std::cout << "hesai_ros_decoder_test_main_at128.cpp" << std::endl; - setvbuf(stdout, NULL, _IONBF, BUFSIZ); - - std::string node_name = "nebula_hesai_decoder_test"; - - rclcpp::init(argc, argv); - ::testing::InitGoogleTest(&argc, argv); - rclcpp::executors::SingleThreadedExecutor exec; - rclcpp::NodeOptions options; - - hesai_driver = std::make_shared(options, node_name); - exec.add_node(hesai_driver->get_node_base_interface()); - - RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Get Status"); - nebula::Status driver_status = hesai_driver->GetStatus(); - int result = 0; - if (driver_status == nebula::Status::OK) { - RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Reading Started"); - // hesai_driver->ReadBag(); - result = RUN_ALL_TESTS(); - } else { - RCLCPP_ERROR_STREAM(rclcpp::get_logger(node_name), driver_status); - } - RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Ending"); - hesai_driver.reset(); - rclcpp::shutdown(); - - return result; -} diff --git a/nebula_tests/hesai/hesai_ros_decoder_test_main_qt64.cpp b/nebula_tests/hesai/hesai_ros_decoder_test_main_qt64.cpp deleted file mode 100644 index f403c5ef1..000000000 --- a/nebula_tests/hesai/hesai_ros_decoder_test_main_qt64.cpp +++ /dev/null @@ -1,47 +0,0 @@ -#include "hesai_ros_decoder_test_qt64.hpp" - -#include - -#include - -#include - -std::shared_ptr hesai_driver; - -TEST(TestDecoder, TestPcd) -{ - std::cout << "TEST(TestDecoder, TestPcd)" << std::endl; - hesai_driver->ReadBag(); -} - -int main(int argc, char * argv[]) -{ - std::cout << "hesai_ros_decoder_test_main_qt64.cpp" << std::endl; - setvbuf(stdout, NULL, _IONBF, BUFSIZ); - - std::string node_name = "nebula_hesai_decoder_test"; - - rclcpp::init(argc, argv); - ::testing::InitGoogleTest(&argc, argv); - rclcpp::executors::SingleThreadedExecutor exec; - rclcpp::NodeOptions options; - - hesai_driver = std::make_shared(options, node_name); - exec.add_node(hesai_driver->get_node_base_interface()); - - RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Get Status"); - nebula::Status driver_status = hesai_driver->GetStatus(); - int result = 0; - if (driver_status == nebula::Status::OK) { - RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Reading Started"); - // hesai_driver->ReadBag(); - result = RUN_ALL_TESTS(); - } else { - RCLCPP_ERROR_STREAM(rclcpp::get_logger(node_name), driver_status); - } - RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Ending"); - hesai_driver.reset(); - rclcpp::shutdown(); - - return result; -} diff --git a/nebula_tests/hesai/hesai_ros_decoder_test_main_xt32.cpp b/nebula_tests/hesai/hesai_ros_decoder_test_main_xt32.cpp deleted file mode 100644 index e9e8df16e..000000000 --- a/nebula_tests/hesai/hesai_ros_decoder_test_main_xt32.cpp +++ /dev/null @@ -1,47 +0,0 @@ -#include "hesai_ros_decoder_test_xt32.hpp" - -#include - -#include - -#include - -std::shared_ptr hesai_driver; - -TEST(TestDecoder, TestPcd) -{ - std::cout << "TEST(TestDecoder, TestPcd)" << std::endl; - hesai_driver->ReadBag(); -} - -int main(int argc, char * argv[]) -{ - std::cout << "hesai_ros_decoder_test_main_xt32.cpp" << std::endl; - setvbuf(stdout, NULL, _IONBF, BUFSIZ); - - std::string node_name = "nebula_hesai_decoder_test"; - - rclcpp::init(argc, argv); - ::testing::InitGoogleTest(&argc, argv); - rclcpp::executors::SingleThreadedExecutor exec; - rclcpp::NodeOptions options; - - hesai_driver = std::make_shared(options, node_name); - exec.add_node(hesai_driver->get_node_base_interface()); - - RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Get Status"); - nebula::Status driver_status = hesai_driver->GetStatus(); - int result = 0; - if (driver_status == nebula::Status::OK) { - RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Reading Started"); - // hesai_driver->ReadBag(); - result = RUN_ALL_TESTS(); - } else { - RCLCPP_ERROR_STREAM(rclcpp::get_logger(node_name), driver_status); - } - RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Ending"); - hesai_driver.reset(); - rclcpp::shutdown(); - - return result; -} diff --git a/nebula_tests/hesai/hesai_ros_decoder_test_main_xt32m.cpp b/nebula_tests/hesai/hesai_ros_decoder_test_main_xt32m.cpp deleted file mode 100644 index e1583038a..000000000 --- a/nebula_tests/hesai/hesai_ros_decoder_test_main_xt32m.cpp +++ /dev/null @@ -1,47 +0,0 @@ -#include "hesai_ros_decoder_test_xt32m.hpp" - -#include - -#include - -#include - -std::shared_ptr hesai_driver; - -TEST(TestDecoder, TestPcd) -{ - std::cout << "TEST(TestDecoder, TestPcd)" << std::endl; - hesai_driver->ReadBag(); -} - -int main(int argc, char * argv[]) -{ - std::cout << "hesai_ros_decoder_test_main_xt32m.cpp" << std::endl; - setvbuf(stdout, NULL, _IONBF, BUFSIZ); - - std::string node_name = "nebula_hesai_decoder_test"; - - rclcpp::init(argc, argv); - ::testing::InitGoogleTest(&argc, argv); - rclcpp::executors::SingleThreadedExecutor exec; - rclcpp::NodeOptions options; - - hesai_driver = std::make_shared(options, node_name); - exec.add_node(hesai_driver->get_node_base_interface()); - - RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Get Status"); - nebula::Status driver_status = hesai_driver->GetStatus(); - int result = 0; - if (driver_status == nebula::Status::OK) { - RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Reading Started"); - // hesai_driver->ReadBag(); - result = RUN_ALL_TESTS(); - } else { - RCLCPP_ERROR_STREAM(rclcpp::get_logger(node_name), driver_status); - } - RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Ending"); - hesai_driver.reset(); - rclcpp::shutdown(); - - return result; -} diff --git a/nebula_tests/hesai/hesai_ros_decoder_test_qt64.cpp b/nebula_tests/hesai/hesai_ros_decoder_test_qt64.cpp deleted file mode 100644 index bd3da6c1f..000000000 --- a/nebula_tests/hesai/hesai_ros_decoder_test_qt64.cpp +++ /dev/null @@ -1,353 +0,0 @@ -#include "hesai_ros_decoder_test_qt64.hpp" - -#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 - -#include -#include - -namespace nebula -{ -namespace ros -{ -HesaiRosDecoderTest::HesaiRosDecoderTest( - const rclcpp::NodeOptions & options, const std::string & node_name) -: rclcpp::Node(node_name, options) -{ - drivers::HesaiCalibrationConfiguration calibration_configuration; - drivers::HesaiSensorConfiguration sensor_configuration; - drivers::HesaiCorrection correction_configuration; - - 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_); - return; - } - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Starting..."); - - calibration_cfg_ptr_ = - std::make_shared(calibration_configuration); - - sensor_cfg_ptr_ = std::make_shared(sensor_configuration); - - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Driver "); - if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { - correction_cfg_ptr_ = std::make_shared(correction_configuration); - wrapper_status_ = InitializeDriver( - std::const_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_), - std::static_pointer_cast(correction_cfg_ptr_)); - } else { - wrapper_status_ = InitializeDriver( - std::const_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_)); - } - - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); -} - -Status HesaiRosDecoderTest::InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) -{ - // driver should be initialized here with proper decoder - driver_ptr_ = std::make_shared( - std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast(calibration_configuration)); - return driver_ptr_->GetStatus(); -} - -Status HesaiRosDecoderTest::InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration, - std::shared_ptr correction_configuration) -{ - // driver should be initialized here with proper decoder - driver_ptr_ = std::make_shared( - std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast( - calibration_configuration), //); - std::static_pointer_cast(correction_configuration)); - return driver_ptr_->GetStatus(); -} - -Status HesaiRosDecoderTest::GetStatus() -{ - return wrapper_status_; -} - -Status HesaiRosDecoderTest::GetParameters( - drivers::HesaiSensorConfiguration & sensor_configuration, - drivers::HesaiCalibrationConfiguration & calibration_configuration, - drivers::HesaiCorrection & correction_configuration) -{ - std::filesystem::path calib_dir = - _SRC_CALIBRATION_DIR_PATH; // variable defined in CMakeLists.txt; - calib_dir /= "hesai"; - std::filesystem::path bag_root_dir = - _SRC_RESOURCES_DIR_PATH; // variable defined in CMakeLists.txt; - bag_root_dir /= "hesai"; - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_model", "PandarQT64"); - sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("return_mode", "Dual", descriptor); - sensor_configuration.return_mode = nebula::drivers::ReturnModeFromStringHesai( - this->get_parameter("return_mode").as_string(), sensor_configuration.sensor_model); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", "hesai", descriptor); - sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 3; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = "Angle where scans begin (degrees, [0.,360.]"; - rcl_interfaces::msg::FloatingPointRange range; - range.set__from_value(0).set__to_value(360).set__step(0.01); - descriptor.floating_point_range = {range}; - this->declare_parameter("scan_phase", 0., descriptor); - sensor_configuration.scan_phase = this->get_parameter("scan_phase").as_double(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter( - "calibration_file", (calib_dir / "PandarQT64.csv").string(), descriptor); - calibration_configuration.calibration_file = - this->get_parameter("calibration_file").as_string(); - } - if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter( - "correction_file", (calib_dir / "PandarAT128.dat").string(), descriptor); - correction_file_path = this->get_parameter("correction_file").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter( - "bag_path", (bag_root_dir / "qt64" / "1673401195788312575").string(), descriptor); - bag_path = this->get_parameter("bag_path").as_string(); - std::cout << bag_path << std::endl; - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("storage_id", "sqlite3", descriptor); - storage_id = this->get_parameter("storage_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("format", "cdr", descriptor); - format = this->get_parameter("format").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("target_topic", "/pandar_packets", descriptor); - target_topic = this->get_parameter("target_topic").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = "Dual return distance threshold [0.01, 0.5]"; - rcl_interfaces::msg::FloatingPointRange range; - range.set__from_value(0.00).set__to_value(0.5).set__step(0.01); - descriptor.floating_point_range = {range}; - this->declare_parameter("dual_return_distance_threshold", 0.1, descriptor); - sensor_configuration.dual_return_distance_threshold = - this->get_parameter("dual_return_distance_threshold").as_double(); - } - - if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { - return Status::INVALID_SENSOR_MODEL; - } - if (sensor_configuration.return_mode == nebula::drivers::ReturnMode::UNKNOWN) { - return Status::INVALID_ECHO_MODE; - } - if (sensor_configuration.frame_id.empty() || sensor_configuration.scan_phase > 360) { - return Status::SENSOR_CONFIG_ERROR; - } - if (calibration_configuration.calibration_file.empty()) { - return Status::INVALID_CALIBRATION_FILE; - } else { - auto cal_status = - calibration_configuration.LoadFromFile(calibration_configuration.calibration_file); - if (cal_status != Status::OK) { - RCLCPP_ERROR_STREAM( - this->get_logger(), - "Given Calibration File: '" << calibration_configuration.calibration_file << "'"); - return cal_status; - } - } - if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { - if (correction_file_path.empty()) { - return Status::INVALID_CALIBRATION_FILE; - } else { - auto cal_status = correction_configuration.LoadFromFile(correction_file_path); - if (cal_status != Status::OK) { - RCLCPP_ERROR_STREAM( - this->get_logger(), "Given Correction File: '" << correction_file_path << "'"); - return cal_status; - } - } - } - - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); - return Status::OK; -} - -void printPCD(nebula::drivers::NebulaPointCloudPtr pp) -{ - for (auto p : pp->points) { - std::cout << "(" << p.x << ", " << p.y << "," << p.z << "): " << p.intensity << ", " - << p.channel << ", " << p.azimuth << ", " << p.return_type << ", " << p.time_stamp - << std::endl; - } -} - -void checkPCDs(nebula::drivers::NebulaPointCloudPtr pp1, nebula::drivers::NebulaPointCloudPtr pp2) -{ - EXPECT_EQ(pp1->points.size(), pp2->points.size()); - for (uint32_t i = 0; i < pp1->points.size(); i++) { - auto p1 = pp1->points[i]; - auto p2 = pp2->points[i]; - EXPECT_FLOAT_EQ(p1.x, p2.x); - EXPECT_FLOAT_EQ(p1.y, p2.y); - EXPECT_FLOAT_EQ(p1.z, p2.z); - EXPECT_FLOAT_EQ(p1.intensity, p2.intensity); - EXPECT_EQ(p1.channel, p2.channel); - EXPECT_FLOAT_EQ(p1.azimuth, p2.azimuth); - EXPECT_EQ(p1.return_type, p2.return_type); - EXPECT_DOUBLE_EQ(p1.time_stamp, p2.time_stamp); - } -} - -void HesaiRosDecoderTest::ReadBag() -{ - rosbag2_storage::StorageOptions storage_options; - rosbag2_cpp::ConverterOptions converter_options; - - std::cout << bag_path << std::endl; - std::cout << storage_id << std::endl; - std::cout << format << std::endl; - std::cout << target_topic << std::endl; - - auto target_topic_name = target_topic; - if (target_topic_name.substr(0, 1) == "/") { - target_topic_name = target_topic_name.substr(1); - } - target_topic_name = std::regex_replace(target_topic_name, std::regex("/"), "_"); - - pcl::PCDReader pcd_reader; - - rcpputils::fs::path bag_dir(bag_path); - rcpputils::fs::path pcd_dir = bag_dir.parent_path(); - int check_cnt = 0; - - storage_options.uri = bag_path; - storage_options.storage_id = storage_id; - converter_options.output_serialization_format = format; //"cdr"; - rclcpp::Serialization serialization; - nebula::drivers::NebulaPointCloudPtr pointcloud(new nebula::drivers::NebulaPointCloud); - // nebula::drivers::NebulaPointCloudPtr ref_pointcloud(new nebula::drivers::NebulaPointCloud); - pcl::PointCloud::Ptr ref_pointcloud(new pcl::PointCloud); - { - rosbag2_cpp::Reader bag_reader(std::make_unique()); - bag_reader.open(storage_options, converter_options); - while (bag_reader.has_next()) { - auto bag_message = bag_reader.read_next(); - - std::cout << "Found topic name " << bag_message->topic_name << std::endl; - - if (bag_message->topic_name == target_topic) { - pandar_msgs::msg::PandarScan extracted_msg; - rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data); - serialization.deserialize_message(&extracted_serialized_msg, &extracted_msg); - - std::cout << "Found data in topic " << bag_message->topic_name << ": " - << bag_message->time_stamp << std::endl; - - auto extracted_msg_ptr = std::make_shared(extracted_msg); - auto pointcloud_ts = driver_ptr_->ConvertScanToPointcloud(extracted_msg_ptr); - pointcloud = std::get<0>(pointcloud_ts); - - // There are very rare cases where has_scanned_ does not become true, but it is not known - // whether it is because of decoder or deserialize_message. - if (!pointcloud) continue; - - auto fn = std::to_string(bag_message->time_stamp) + ".pcd"; - - auto target_pcd_path = (pcd_dir / fn); - std::cout << target_pcd_path << std::endl; - if (target_pcd_path.exists()) { - std::cout << "exists: " << target_pcd_path << std::endl; - auto rt = pcd_reader.read(target_pcd_path.string(), *ref_pointcloud); - std::cout << rt << " loaded: " << target_pcd_path << std::endl; - checkPCDs(pointcloud, ref_pointcloud); - check_cnt++; - // ref_pointcloud.reset(new nebula::drivers::NebulaPointCloud); - ref_pointcloud.reset(new pcl::PointCloud); - } - pointcloud.reset(new nebula::drivers::NebulaPointCloud); - } - } - EXPECT_GT(check_cnt, 0); - // close on scope exit - } -} - -} // namespace ros -} // namespace nebula diff --git a/nebula_tests/hesai/hesai_ros_decoder_test_qt64.hpp b/nebula_tests/hesai/hesai_ros_decoder_test_qt64.hpp deleted file mode 100644 index f0ddffeed..000000000 --- a/nebula_tests/hesai/hesai_ros_decoder_test_qt64.hpp +++ /dev/null @@ -1,103 +0,0 @@ -#ifndef NEBULA_HesaiRosDecoderTestQt64_H -#define NEBULA_HesaiRosDecoderTestQt64_H - -#include "nebula_common/hesai/hesai_common.hpp" -#include "nebula_common/nebula_common.hpp" -#include "nebula_common/nebula_status.hpp" -#include "nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp" -#include "nebula_ros/common/nebula_driver_ros_wrapper_base.hpp" - -#include -#include -#include - -#include "pandar_msgs/msg/pandar_packet.hpp" -#include "pandar_msgs/msg/pandar_scan.hpp" -#include "hesai_common.hpp" - -#include - -namespace nebula -{ -namespace ros -{ -/// @brief Testing decoder of QT64 (Keeps HesaiDriverRosWrapper structure as much as possible) -class HesaiRosDecoderTest final : public rclcpp::Node, NebulaDriverRosWrapperBase //, testing::Test -{ - std::shared_ptr driver_ptr_; - Status wrapper_status_; - - std::shared_ptr calibration_cfg_ptr_; - std::shared_ptr sensor_cfg_ptr_; - std::shared_ptr correction_cfg_ptr_; - - /// @brief Initializing ros wrapper - /// @param sensor_configuration SensorConfiguration for this driver - /// @param calibration_configuration CalibrationConfiguration for this driver - /// @return Resulting status - Status InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) override; - - /// @brief Initializing ros wrapper for AT - /// @param sensor_configuration SensorConfiguration for this driver - /// @param calibration_configuration CalibrationConfiguration for this driver - /// @param correction_configuration CorrectionConfiguration for this driver - /// @return Resulting status - Status InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration, - std::shared_ptr correction_configuration); - - /// @brief Get configurations (Magic numbers for QT64 is described, each function can be - /// integrated if the ros parameter can be passed to Google Test) - /// @param sensor_configuration Output of SensorConfiguration - /// @param calibration_configuration Output of CalibrationConfiguration - /// @param correction_configuration Output of CorrectionConfiguration (for AT) - /// @return Resulting status - Status GetParameters( - drivers::HesaiSensorConfiguration & sensor_configuration, - drivers::HesaiCalibrationConfiguration & calibration_configuration, - drivers::HesaiCorrection & correction_configuration); - - /// @brief Convert seconds to chrono::nanoseconds - /// @param seconds - /// @return chrono::nanoseconds - static inline std::chrono::nanoseconds SecondsToChronoNanoSeconds(const double seconds) - { - return std::chrono::duration_cast( - std::chrono::duration(seconds)); - } - -public: - explicit HesaiRosDecoderTest(const rclcpp::NodeOptions & options, const std::string & node_name); - - /// @brief Get current status of this driver - /// @return Current status - Status GetStatus(); - - /// @brief Read the specified bag file and compare the constructed point clouds with the - /// corresponding PCD files - void ReadBag(); - /* - void SetUp() override { - // Setup things that should occur before every test instance should go here - RCLCPP_ERROR_STREAM(this->get_logger(), "DONE WITH SETUP!!"); - } - - void TearDown() override { - std::cout << "DONE WITH TEARDOWN" << std::endl; - } -*/ -private: - std::string bag_path; - std::string storage_id; - std::string format; - std::string target_topic; - std::string correction_file_path; -}; - -} // namespace ros -} // namespace nebula - -#endif // NEBULA_HesaiRosDecoderTestQt64_H diff --git a/nebula_tests/hesai/hesai_ros_decoder_test_xt32.cpp b/nebula_tests/hesai/hesai_ros_decoder_test_xt32.cpp deleted file mode 100644 index 8e671a203..000000000 --- a/nebula_tests/hesai/hesai_ros_decoder_test_xt32.cpp +++ /dev/null @@ -1,350 +0,0 @@ -#include "hesai_ros_decoder_test_xt32.hpp" - -#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 - -#include -#include - -namespace nebula -{ -namespace ros -{ -HesaiRosDecoderTest::HesaiRosDecoderTest( - const rclcpp::NodeOptions & options, const std::string & node_name) -: rclcpp::Node(node_name, options) -{ - drivers::HesaiCalibrationConfiguration calibration_configuration; - drivers::HesaiSensorConfiguration sensor_configuration; - drivers::HesaiCorrection correction_configuration; - - 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_); - return; - } - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Starting..."); - - calibration_cfg_ptr_ = - std::make_shared(calibration_configuration); - - sensor_cfg_ptr_ = std::make_shared(sensor_configuration); - - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Driver "); - if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { - correction_cfg_ptr_ = std::make_shared(correction_configuration); - wrapper_status_ = InitializeDriver( - std::const_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_), - std::static_pointer_cast(correction_cfg_ptr_)); - } else { - wrapper_status_ = InitializeDriver( - std::const_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_)); - } - - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); -} - -Status HesaiRosDecoderTest::InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) -{ - // driver should be initialized here with proper decoder - driver_ptr_ = std::make_shared( - std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast(calibration_configuration)); - return driver_ptr_->GetStatus(); -} - -Status HesaiRosDecoderTest::InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration, - std::shared_ptr correction_configuration) -{ - // driver should be initialized here with proper decoder - driver_ptr_ = std::make_shared( - std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast( - calibration_configuration), //); - std::static_pointer_cast(correction_configuration)); - return driver_ptr_->GetStatus(); -} - -Status HesaiRosDecoderTest::GetStatus() { return wrapper_status_; } - -Status HesaiRosDecoderTest::GetParameters( - drivers::HesaiSensorConfiguration & sensor_configuration, - drivers::HesaiCalibrationConfiguration & calibration_configuration, - drivers::HesaiCorrection & correction_configuration) -{ - std::filesystem::path calib_dir = - _SRC_CALIBRATION_DIR_PATH; // variable defined in CMakeLists.txt; - calib_dir /= "hesai"; - std::filesystem::path bag_root_dir = - _SRC_RESOURCES_DIR_PATH; // variable defined in CMakeLists.txt; - bag_root_dir /= "hesai"; - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_model", "PandarXT32"); - sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("return_mode", "Dual", descriptor); - sensor_configuration.return_mode = nebula::drivers::ReturnModeFromStringHesai( - this->get_parameter("return_mode").as_string(), sensor_configuration.sensor_model); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", "hesai", descriptor); - sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 3; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = "Angle where scans begin (degrees, [0.,360.]"; - rcl_interfaces::msg::FloatingPointRange range; - range.set__from_value(0).set__to_value(360).set__step(0.01); - descriptor.floating_point_range = {range}; - this->declare_parameter("scan_phase", 0., descriptor); - sensor_configuration.scan_phase = this->get_parameter("scan_phase").as_double(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter( - "calibration_file", (calib_dir / "PandarXT32.csv").string(), descriptor); - calibration_configuration.calibration_file = - this->get_parameter("calibration_file").as_string(); - } - if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter( - "correction_file", (calib_dir / "PandarAT128.dat").string(), descriptor); - correction_file_path = this->get_parameter("correction_file").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter( - "bag_path", (bag_root_dir / "xt32" / "1673400677802009732").string(), descriptor); - bag_path = this->get_parameter("bag_path").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("storage_id", "sqlite3", descriptor); - storage_id = this->get_parameter("storage_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("format", "cdr", descriptor); - format = this->get_parameter("format").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("target_topic", "/pandar_packets", descriptor); - target_topic = this->get_parameter("target_topic").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = "Dual return distance threshold [0.01, 0.5]"; - rcl_interfaces::msg::FloatingPointRange range; - range.set__from_value(0.00).set__to_value(0.5).set__step(0.01); - descriptor.floating_point_range = {range}; - this->declare_parameter("dual_return_distance_threshold", 0.1, descriptor); - sensor_configuration.dual_return_distance_threshold = - this->get_parameter("dual_return_distance_threshold").as_double(); - } - if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { - return Status::INVALID_SENSOR_MODEL; - } - if (sensor_configuration.return_mode == nebula::drivers::ReturnMode::UNKNOWN) { - return Status::INVALID_ECHO_MODE; - } - if (sensor_configuration.frame_id.empty() || sensor_configuration.scan_phase > 360) { - return Status::SENSOR_CONFIG_ERROR; - } - if (calibration_configuration.calibration_file.empty()) { - return Status::INVALID_CALIBRATION_FILE; - } else { - auto cal_status = - calibration_configuration.LoadFromFile(calibration_configuration.calibration_file); - if (cal_status != Status::OK) { - RCLCPP_ERROR_STREAM( - this->get_logger(), - "Given Calibration File: '" << calibration_configuration.calibration_file << "'"); - return cal_status; - } - } - if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { - if (correction_file_path.empty()) { - return Status::INVALID_CALIBRATION_FILE; - } else { - auto cal_status = correction_configuration.LoadFromFile(correction_file_path); - if (cal_status != Status::OK) { - RCLCPP_ERROR_STREAM( - this->get_logger(), "Given Correction File: '" << correction_file_path << "'"); - return cal_status; - } - } - } - - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); - return Status::OK; -} - -void printPCD(nebula::drivers::NebulaPointCloudPtr pp) -{ - for (auto p : pp->points) { - std::cout << "(" << p.x << ", " << p.y << "," << p.z << "): " << p.intensity << ", " - << p.channel << ", " << p.azimuth << ", " << p.return_type << ", " << p.time_stamp - << std::endl; - } -} - -void checkPCDs(nebula::drivers::NebulaPointCloudPtr pp1, nebula::drivers::NebulaPointCloudPtr pp2) -{ - EXPECT_EQ(pp1->points.size(), pp2->points.size()); - for (uint32_t i = 0; i < pp1->points.size(); i++) { - auto p1 = pp1->points[i]; - auto p2 = pp2->points[i]; - EXPECT_FLOAT_EQ(p1.x, p2.x); - EXPECT_FLOAT_EQ(p1.y, p2.y); - EXPECT_FLOAT_EQ(p1.z, p2.z); - EXPECT_FLOAT_EQ(p1.intensity, p2.intensity); - EXPECT_EQ(p1.channel, p2.channel); - EXPECT_FLOAT_EQ(p1.azimuth, p2.azimuth); - EXPECT_EQ(p1.return_type, p2.return_type); - EXPECT_DOUBLE_EQ(p1.time_stamp, p2.time_stamp); - } -} - - - -void HesaiRosDecoderTest::ReadBag() -{ - rosbag2_storage::StorageOptions storage_options; - rosbag2_cpp::ConverterOptions converter_options; - - std::cout << bag_path << std::endl; - std::cout << storage_id << std::endl; - std::cout << format << std::endl; - std::cout << target_topic << std::endl; - - auto target_topic_name = target_topic; - if (target_topic_name.substr(0, 1) == "/") { - target_topic_name = target_topic_name.substr(1); - } - target_topic_name = std::regex_replace(target_topic_name, std::regex("/"), "_"); - - pcl::PCDReader pcd_reader; - - rcpputils::fs::path bag_dir(bag_path); - rcpputils::fs::path pcd_dir = bag_dir.parent_path(); - int check_cnt = 0; - - storage_options.uri = bag_path; - storage_options.storage_id = storage_id; - converter_options.output_serialization_format = format; //"cdr"; - rclcpp::Serialization serialization; - nebula::drivers::NebulaPointCloudPtr pointcloud(new nebula::drivers::NebulaPointCloud); - // nebula::drivers::NebulaPointCloudPtr ref_pointcloud(new nebula::drivers::NebulaPointCloud); - pcl::PointCloud::Ptr ref_pointcloud(new pcl::PointCloud); - { - rosbag2_cpp::Reader bag_reader(std::make_unique()); - bag_reader.open(storage_options, converter_options); - while (bag_reader.has_next()) { - auto bag_message = bag_reader.read_next(); - - std::cout << "Found topic name " << bag_message->topic_name << std::endl; - - if (bag_message->topic_name == target_topic) { - pandar_msgs::msg::PandarScan extracted_msg; - rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data); - serialization.deserialize_message(&extracted_serialized_msg, &extracted_msg); - - std::cout << "Found data in topic " << bag_message->topic_name << ": " - << bag_message->time_stamp << std::endl; - - auto extracted_msg_ptr = std::make_shared(extracted_msg); - auto pointcloud_ts = driver_ptr_->ConvertScanToPointcloud(extracted_msg_ptr); - pointcloud = std::get<0>(pointcloud_ts); - - // There are very rare cases where has_scanned_ does not become true, but it is not known - // whether it is because of decoder or deserialize_message. - if (!pointcloud) continue; - - auto fn = std::to_string(bag_message->time_stamp) + ".pcd"; - - auto target_pcd_path = (pcd_dir / fn); - std::cout << target_pcd_path << std::endl; - if (target_pcd_path.exists()) { - std::cout << "exists: " << target_pcd_path << std::endl; - auto rt = pcd_reader.read(target_pcd_path.string(), *ref_pointcloud); - std::cout << rt << " loaded: " << target_pcd_path << std::endl; - checkPCDs(pointcloud, ref_pointcloud); - check_cnt++; - // ref_pointcloud.reset(new nebula::drivers::NebulaPointCloud); - ref_pointcloud.reset(new pcl::PointCloud); - } - pointcloud.reset(new nebula::drivers::NebulaPointCloud); - } - } - EXPECT_GT(check_cnt, 0); - // close on scope exit - } -} - -} // namespace ros -} // namespace nebula diff --git a/nebula_tests/hesai/hesai_ros_decoder_test_xt32.hpp b/nebula_tests/hesai/hesai_ros_decoder_test_xt32.hpp deleted file mode 100644 index 9092c61ce..000000000 --- a/nebula_tests/hesai/hesai_ros_decoder_test_xt32.hpp +++ /dev/null @@ -1,103 +0,0 @@ -#ifndef NEBULA_HesaiRosDecoderTestXt32_H -#define NEBULA_HesaiRosDecoderTestXt32_H - -#include "nebula_common/hesai/hesai_common.hpp" -#include "nebula_common/nebula_common.hpp" -#include "nebula_common/nebula_status.hpp" -#include "nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp" -#include "nebula_ros/common/nebula_driver_ros_wrapper_base.hpp" - -#include -#include -#include - -#include "pandar_msgs/msg/pandar_packet.hpp" -#include "pandar_msgs/msg/pandar_scan.hpp" -#include "hesai_common.hpp" - -#include - -namespace nebula -{ -namespace ros -{ -/// @brief Testing decoder of XT32 (Keeps HesaiDriverRosWrapper structure as much as possible) -class HesaiRosDecoderTest final : public rclcpp::Node, NebulaDriverRosWrapperBase //, testing::Test -{ - std::shared_ptr driver_ptr_; - Status wrapper_status_; - - std::shared_ptr calibration_cfg_ptr_; - std::shared_ptr sensor_cfg_ptr_; - std::shared_ptr correction_cfg_ptr_; - - /// @brief Initializing ros wrapper - /// @param sensor_configuration SensorConfiguration for this driver - /// @param calibration_configuration CalibrationConfiguration for this driver - /// @return Resulting status - Status InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) override; - - /// @brief Initializing ros wrapper for AT - /// @param sensor_configuration SensorConfiguration for this driver - /// @param calibration_configuration CalibrationConfiguration for this driver - /// @param correction_configuration CorrectionConfiguration for this driver - /// @return Resulting status - Status InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration, - std::shared_ptr correction_configuration); - - /// @brief Get configurations (Magic numbers for XT32 is described, each function can be - /// integrated if the ros parameter can be passed to Google Test) - /// @param sensor_configuration Output of SensorConfiguration - /// @param calibration_configuration Output of CalibrationConfiguration - /// @param correction_configuration Output of CorrectionConfiguration (for AT) - /// @return Resulting status - Status GetParameters( - drivers::HesaiSensorConfiguration & sensor_configuration, - drivers::HesaiCalibrationConfiguration & calibration_configuration, - drivers::HesaiCorrection & correction_configuration); - - /// @brief Convert seconds to chrono::nanoseconds - /// @param seconds - /// @return chrono::nanoseconds - static inline std::chrono::nanoseconds SecondsToChronoNanoSeconds(const double seconds) - { - return std::chrono::duration_cast( - std::chrono::duration(seconds)); - } - -public: - explicit HesaiRosDecoderTest(const rclcpp::NodeOptions & options, const std::string & node_name); - - /// @brief Get current status of this driver - /// @return Current status - Status GetStatus(); - - /// @brief Read the specified bag file and compare the constructed point clouds with the - /// corresponding PCD files - void ReadBag(); - /* - void SetUp() override { - // Setup things that should occur before every test instance should go here - RCLCPP_ERROR_STREAM(this->get_logger(), "DONE WITH SETUP!!"); - } - - void TearDown() override { - std::cout << "DONE WITH TEARDOWN" << std::endl; - } -*/ -private: - std::string bag_path; - std::string storage_id; - std::string format; - std::string target_topic; - std::string correction_file_path; -}; - -} // namespace ros -} // namespace nebula - -#endif // NEBULA_HesaiRosDecoderTestXt32_H diff --git a/nebula_tests/hesai/hesai_ros_decoder_test_xt32m.cpp b/nebula_tests/hesai/hesai_ros_decoder_test_xt32m.cpp deleted file mode 100644 index 9b8ea9f91..000000000 --- a/nebula_tests/hesai/hesai_ros_decoder_test_xt32m.cpp +++ /dev/null @@ -1,352 +0,0 @@ -#include "hesai_ros_decoder_test_xt32m.hpp" - -#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 - -#include -#include - -namespace nebula -{ -namespace ros -{ -HesaiRosDecoderTest::HesaiRosDecoderTest( - const rclcpp::NodeOptions & options, const std::string & node_name) -: rclcpp::Node(node_name, options) -{ - drivers::HesaiCalibrationConfiguration calibration_configuration; - drivers::HesaiSensorConfiguration sensor_configuration; - drivers::HesaiCorrection correction_configuration; - - 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_); - return; - } - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Starting..."); - - calibration_cfg_ptr_ = - std::make_shared(calibration_configuration); - - sensor_cfg_ptr_ = std::make_shared(sensor_configuration); - - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Driver "); - if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { - correction_cfg_ptr_ = std::make_shared(correction_configuration); - wrapper_status_ = InitializeDriver( - std::const_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_), - std::static_pointer_cast(correction_cfg_ptr_)); - } else { - wrapper_status_ = InitializeDriver( - std::const_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_)); - } - - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); -} - -Status HesaiRosDecoderTest::InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) -{ - // driver should be initialized here with proper decoder - driver_ptr_ = std::make_shared( - std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast(calibration_configuration)); - return driver_ptr_->GetStatus(); -} - -Status HesaiRosDecoderTest::InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration, - std::shared_ptr correction_configuration) -{ - // driver should be initialized here with proper decoder - driver_ptr_ = std::make_shared( - std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast( - calibration_configuration), //); - std::static_pointer_cast(correction_configuration)); - return driver_ptr_->GetStatus(); -} - -Status HesaiRosDecoderTest::GetStatus() -{ - return wrapper_status_; -} - -Status HesaiRosDecoderTest::GetParameters( - drivers::HesaiSensorConfiguration & sensor_configuration, - drivers::HesaiCalibrationConfiguration & calibration_configuration, - drivers::HesaiCorrection & correction_configuration) -{ - std::filesystem::path calib_dir = - _SRC_CALIBRATION_DIR_PATH; // variable defined in CMakeLists.txt; - calib_dir /= "hesai"; - std::filesystem::path bag_root_dir = - _SRC_RESOURCES_DIR_PATH; // variable defined in CMakeLists.txt; - bag_root_dir /= "hesai"; - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_model", "PandarXT32M"); - sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("return_mode", "LastStrongest", descriptor); - sensor_configuration.return_mode = nebula::drivers::ReturnModeFromStringHesai( - this->get_parameter("return_mode").as_string(), sensor_configuration.sensor_model); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", "hesai", descriptor); - sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 3; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = "Angle where scans begin (degrees, [0.,360.]"; - rcl_interfaces::msg::FloatingPointRange range; - range.set__from_value(0).set__to_value(360).set__step(0.01); - descriptor.floating_point_range = {range}; - this->declare_parameter("scan_phase", 0., descriptor); - sensor_configuration.scan_phase = this->get_parameter("scan_phase").as_double(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter( - "calibration_file", (calib_dir / "PandarXT32M.csv").string(), descriptor); - calibration_configuration.calibration_file = - this->get_parameter("calibration_file").as_string(); - } - if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter( - "correction_file", (calib_dir / "PandarAT128.dat").string(), descriptor); - correction_file_path = this->get_parameter("correction_file").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter( - "bag_path", (bag_root_dir / "xt32m" / "1660893203042895158").string(), descriptor); - bag_path = this->get_parameter("bag_path").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("storage_id", "sqlite3", descriptor); - storage_id = this->get_parameter("storage_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("format", "cdr", descriptor); - format = this->get_parameter("format").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("target_topic", "/pandar_packets", descriptor); - target_topic = this->get_parameter("target_topic").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = "Dual return distance threshold [0.01, 0.5]"; - rcl_interfaces::msg::FloatingPointRange range; - range.set__from_value(0.00).set__to_value(0.5).set__step(0.01); - descriptor.floating_point_range = {range}; - this->declare_parameter("dual_return_distance_threshold", 0.1, descriptor); - sensor_configuration.dual_return_distance_threshold = - this->get_parameter("dual_return_distance_threshold").as_double(); - } - - if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { - return Status::INVALID_SENSOR_MODEL; - } - if (sensor_configuration.return_mode == nebula::drivers::ReturnMode::UNKNOWN) { - return Status::INVALID_ECHO_MODE; - } - if (sensor_configuration.frame_id.empty() || sensor_configuration.scan_phase > 360) { - return Status::SENSOR_CONFIG_ERROR; - } - if (calibration_configuration.calibration_file.empty()) { - return Status::INVALID_CALIBRATION_FILE; - } else { - auto cal_status = - calibration_configuration.LoadFromFile(calibration_configuration.calibration_file); - if (cal_status != Status::OK) { - RCLCPP_ERROR_STREAM( - this->get_logger(), - "Given Calibration File: '" << calibration_configuration.calibration_file << "'"); - return cal_status; - } - } - if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { - if (correction_file_path.empty()) { - return Status::INVALID_CALIBRATION_FILE; - } else { - auto cal_status = correction_configuration.LoadFromFile(correction_file_path); - if (cal_status != Status::OK) { - RCLCPP_ERROR_STREAM( - this->get_logger(), "Given Correction File: '" << correction_file_path << "'"); - return cal_status; - } - } - } - - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); - return Status::OK; -} - -void printPCD(nebula::drivers::NebulaPointCloudPtr pp) -{ - for (auto p : pp->points) { - std::cout << "(" << p.x << ", " << p.y << "," << p.z << "): " << p.intensity << ", " - << p.channel << ", " << p.azimuth << ", " << p.return_type << ", " << p.time_stamp - << std::endl; - } -} - -void checkPCDs(nebula::drivers::NebulaPointCloudPtr pp1, nebula::drivers::NebulaPointCloudPtr pp2) -{ - EXPECT_EQ(pp1->points.size(), pp2->points.size()); - for (uint32_t i = 0; i < pp1->points.size(); i++) { - auto p1 = pp1->points[i]; - auto p2 = pp2->points[i]; - EXPECT_FLOAT_EQ(p1.x, p2.x); - EXPECT_FLOAT_EQ(p1.y, p2.y); - EXPECT_FLOAT_EQ(p1.z, p2.z); - EXPECT_FLOAT_EQ(p1.intensity, p2.intensity); - EXPECT_EQ(p1.channel, p2.channel); - EXPECT_FLOAT_EQ(p1.azimuth, p2.azimuth); - EXPECT_EQ(p1.return_type, p2.return_type); - EXPECT_DOUBLE_EQ(p1.time_stamp, p2.time_stamp); - } -} - -void HesaiRosDecoderTest::ReadBag() -{ - rosbag2_storage::StorageOptions storage_options; - rosbag2_cpp::ConverterOptions converter_options; - - std::cout << bag_path << std::endl; - std::cout << storage_id << std::endl; - std::cout << format << std::endl; - std::cout << target_topic << std::endl; - - auto target_topic_name = target_topic; - if (target_topic_name.substr(0, 1) == "/") { - target_topic_name = target_topic_name.substr(1); - } - target_topic_name = std::regex_replace(target_topic_name, std::regex("/"), "_"); - - pcl::PCDReader pcd_reader; - - rcpputils::fs::path bag_dir(bag_path); - rcpputils::fs::path pcd_dir = bag_dir.parent_path(); - int check_cnt = 0; - - storage_options.uri = bag_path; - storage_options.storage_id = storage_id; - converter_options.output_serialization_format = format; //"cdr"; - rclcpp::Serialization serialization; - nebula::drivers::NebulaPointCloudPtr pointcloud(new nebula::drivers::NebulaPointCloud); - // nebula::drivers::NebulaPointCloudPtr ref_pointcloud(new nebula::drivers::NebulaPointCloud); - pcl::PointCloud::Ptr ref_pointcloud(new pcl::PointCloud); - { - rosbag2_cpp::Reader bag_reader(std::make_unique()); - bag_reader.open(storage_options, converter_options); - while (bag_reader.has_next()) { - auto bag_message = bag_reader.read_next(); - - std::cout << "Found topic name " << bag_message->topic_name << std::endl; - - if (bag_message->topic_name == target_topic) { - pandar_msgs::msg::PandarScan extracted_msg; - rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data); - serialization.deserialize_message(&extracted_serialized_msg, &extracted_msg); - - std::cout << "Found data in topic " << bag_message->topic_name << ": " - << bag_message->time_stamp << std::endl; - - auto extracted_msg_ptr = std::make_shared(extracted_msg); - auto pointcloud_ts = driver_ptr_->ConvertScanToPointcloud(extracted_msg_ptr); - pointcloud = std::get<0>(pointcloud_ts); - - // There are very rare cases where has_scanned_ does not become true, but it is not known - // whether it is because of decoder or deserialize_message. - if (!pointcloud) continue; - - auto fn = std::to_string(bag_message->time_stamp) + ".pcd"; - - auto target_pcd_path = (pcd_dir / fn); - std::cout << target_pcd_path << std::endl; - if (target_pcd_path.exists()) { - std::cout << "exists: " << target_pcd_path << std::endl; - auto rt = pcd_reader.read(target_pcd_path.string(), *ref_pointcloud); - std::cout << rt << " loaded: " << target_pcd_path << std::endl; - checkPCDs(pointcloud, ref_pointcloud); - check_cnt++; - // ref_pointcloud.reset(new nebula::drivers::NebulaPointCloud); - ref_pointcloud.reset(new pcl::PointCloud); - } - pointcloud.reset(new nebula::drivers::NebulaPointCloud); - } - } - EXPECT_GT(check_cnt, 0); - // close on scope exit - } -} - -} // namespace ros -} // namespace nebula diff --git a/nebula_tests/hesai/hesai_ros_decoder_test_xt32m.hpp b/nebula_tests/hesai/hesai_ros_decoder_test_xt32m.hpp deleted file mode 100644 index b386dbd00..000000000 --- a/nebula_tests/hesai/hesai_ros_decoder_test_xt32m.hpp +++ /dev/null @@ -1,106 +0,0 @@ -//#ifndef NEBULA_HesaiRosDecoderTest_H -//#define NEBULA_HesaiRosDecoderTest_H -#ifndef NEBULA_HesaiRosDecoderTestXt32m_H -#define NEBULA_HesaiRosDecoderTestXt32m_H - -#include "nebula_common/hesai/hesai_common.hpp" -#include "nebula_common/nebula_common.hpp" -#include "nebula_common/nebula_status.hpp" -#include "nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp" -#include "nebula_ros/common/nebula_driver_ros_wrapper_base.hpp" - -#include -#include -#include - -#include "pandar_msgs/msg/pandar_packet.hpp" -#include "pandar_msgs/msg/pandar_scan.hpp" -#include "hesai_common.hpp" - -#include - -namespace nebula -{ -namespace ros -{ -/// @brief Testing decoder of XT32M (Keeps HesaiDriverRosWrapper structure as much as possible) -class HesaiRosDecoderTest final : public rclcpp::Node, NebulaDriverRosWrapperBase //, testing::Test -{ - std::shared_ptr driver_ptr_; - Status wrapper_status_; - - std::shared_ptr calibration_cfg_ptr_; - std::shared_ptr sensor_cfg_ptr_; - std::shared_ptr correction_cfg_ptr_; - - /// @brief Initializing ros wrapper - /// @param sensor_configuration SensorConfiguration for this driver - /// @param calibration_configuration CalibrationConfiguration for this driver - /// @return Resulting status - Status InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) override; - - /// @brief Initializing ros wrapper for AT - /// @param sensor_configuration SensorConfiguration for this driver - /// @param calibration_configuration CalibrationConfiguration for this driver - /// @param correction_configuration CorrectionConfiguration for this driver - /// @return Resulting status - Status InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration, - std::shared_ptr correction_configuration); - - /// @brief Get configurations (Magic numbers for XT32 is described, each function can be - /// integrated if the ros parameter can be passed to Google Test) - /// @param sensor_configuration Output of SensorConfiguration - /// @param calibration_configuration Output of CalibrationConfiguration - /// @param correction_configuration Output of CorrectionConfiguration (for AT) - /// @return Resulting status - Status GetParameters( - drivers::HesaiSensorConfiguration & sensor_configuration, - drivers::HesaiCalibrationConfiguration & calibration_configuration, - drivers::HesaiCorrection & correction_configuration); - - /// @brief Convert seconds to chrono::nanoseconds - /// @param seconds - /// @return chrono::nanoseconds - static inline std::chrono::nanoseconds SecondsToChronoNanoSeconds(const double seconds) - { - return std::chrono::duration_cast( - std::chrono::duration(seconds)); - } - -public: - explicit HesaiRosDecoderTest(const rclcpp::NodeOptions & options, const std::string & node_name); - - /// @brief Get current status of this driver - /// @return Current status - Status GetStatus(); - - /// @brief Read the specified bag file and compare the constructed point clouds with the - /// corresponding PCD files - void ReadBag(); - /* - void SetUp() override { - // Setup things that should occur before every test instance should go here - RCLCPP_ERROR_STREAM(this->get_logger(), "DONE WITH SETUP!!"); - } - - void TearDown() override { - std::cout << "DONE WITH TEARDOWN" << std::endl; - } -*/ -private: - std::string bag_path; - std::string storage_id; - std::string format; - std::string target_topic; - std::string correction_file_path; -}; - -} // namespace ros -} // namespace nebula - -#endif // NEBULA_HesaiRosDecoderTestXt32m_H -//#endif // NEBULA_HesaiRosDecoderTest_H From dd98b26757a2fe929eda772fa1c589ea7bcb9524 Mon Sep 17 00:00:00 2001 From: Maximilian Schmeller Date: Mon, 2 Oct 2023 14:39:15 +0900 Subject: [PATCH 3/3] Fix syntax error --- nebula_tests/hesai/hesai_ros_decoder_test_main.cpp | 6 ------ 1 file changed, 6 deletions(-) diff --git a/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp b/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp index a6913b4ca..af9bdd857 100644 --- a/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp +++ b/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp @@ -89,12 +89,6 @@ TEST_P(DecoderTest, TestPcd) EXPECT_GT(check_cnt, 0); } - - -int main(int argc, char * argv[]) -{ - std::cout << "hesai_ros_decoder_test_main.cpp" << std::endl; - void DecoderTest::SetUp() { auto decoder_params = GetParam();