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 3a622c19a..784d8cd55 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,10 +15,13 @@ #include "pandar_msgs/msg/pandar_scan.hpp" #include +#include + +#include namespace nebula { -namespace ros +namespace test { void checkPCDs(nebula::drivers::NebulaPointCloudPtr pc, pcl::PointCloud::Ptr pc_ref) @@ -39,5 +43,31 @@ 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; + } +} + } // namespace ros } // namespace nebula \ No newline at end of file diff --git a/nebula_tests/hesai/hesai_ros_decoder_test_xt32m.cpp b/nebula_tests/hesai/hesai_ros_decoder_test.cpp similarity index 61% rename from nebula_tests/hesai/hesai_ros_decoder_test_xt32m.cpp rename to nebula_tests/hesai/hesai_ros_decoder_test.cpp index 9b8ea9f91..bc6852d2d 100644 --- a/nebula_tests/hesai/hesai_ros_decoder_test_xt32m.cpp +++ b/nebula_tests/hesai/hesai_ros_decoder_test.cpp @@ -1,4 +1,4 @@ -#include "hesai_ros_decoder_test_xt32m.hpp" +#include "hesai_ros_decoder_test.hpp" #include "rclcpp/serialization.hpp" #include "rclcpp/serialized_message.hpp" @@ -20,8 +20,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; @@ -91,110 +92,109 @@ Status HesaiRosDecoderTest::GetParameters( 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", "PandarXT32M"); + 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", "LastStrongest", 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 / "PandarXT32M.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 / "xt32m" / "1660893203042895158").string(), descriptor); - bag_path = this->get_parameter("bag_path").as_string(); + "bag_path", (bag_root_dir / params_.bag_path).string(), descriptor); + params_.bag_path = this->get_parameter("bag_path").as_string(); + RCLCPP_DEBUG_STREAM(get_logger(), "Bag path relative to test root: " << params_.bag_path); } { 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; @@ -205,7 +205,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(); } @@ -232,13 +233,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; } } @@ -248,103 +249,55 @@ 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() +void HesaiRosDecoderTest::ReadBag( + std::function scan_callback) { 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; + 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 = target_topic; + auto target_topic_name = params_.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; + rcpputils::fs::path bag_dir(params_.bag_path); - 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); - 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); + 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 data in topic " << bag_message->topic_name << ": " - << bag_message->time_stamp << std::endl; + RCLCPP_DEBUG_STREAM(get_logger(), "Found topic name " << bag_message->topic_name); - auto extracted_msg_ptr = std::make_shared(extracted_msg); - auto pointcloud_ts = driver_ptr_->ConvertScanToPointcloud(extracted_msg_ptr); - pointcloud = std::get<0>(pointcloud_ts); + 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); - // 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; + RCLCPP_DEBUG_STREAM( + get_logger(), + "Found data in topic " << bag_message->topic_name << ": " << bag_message->time_stamp); - auto fn = std::to_string(bag_message->time_stamp) + ".pcd"; + 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 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_40p.hpp b/nebula_tests/hesai/hesai_ros_decoder_test.hpp similarity index 79% rename from nebula_tests/hesai/hesai_ros_decoder_test_40p.hpp rename to nebula_tests/hesai/hesai_ros_decoder_test.hpp index f961931e4..a055e1b2f 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,39 @@ #include "pandar_msgs/msg/pandar_packet.hpp" #include "pandar_msgs/msg/pandar_scan.hpp" -#include "hesai_common.hpp" #include +#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 +96,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); @@ -81,27 +108,10 @@ 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: - 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_40p.cpp b/nebula_tests/hesai/hesai_ros_decoder_test_40p.cpp deleted file mode 100644 index 88a490aee..000000000 --- a/nebula_tests/hesai/hesai_ros_decoder_test_40p.cpp +++ /dev/null @@ -1,352 +0,0 @@ -#include "hesai_ros_decoder_test_40p.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", "Pandar40P"); - 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 / "Pandar40P.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 / "40p" / "1673400149412331409").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.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 new file mode 100644 index 000000000..af9bdd857 --- /dev/null +++ b/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp @@ -0,0 +1,140 @@ +#include "hesai_ros_decoder_test_main.hpp" + +#include "hesai_common.hpp" +#include "hesai_ros_decoder_test.hpp" + +#include + +#include +#include +#include + +#include +#include +#include + +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", + }}; + +// Compares geometrical output of decoder against pre-recorded reference pointcloud. +TEST_P(DecoderTest, TestPcd) +{ + 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); +} + +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::executors::SingleThreadedExecutor exec; + rclcpp::NodeOptions options; + + hesai_driver_ = + std::make_shared(options, node_name, decoder_params); + 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"); + } +} + +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_40p.cpp b/nebula_tests/hesai/hesai_ros_decoder_test_main_40p.cpp deleted file mode 100644 index d3e681a17..000000000 --- a/nebula_tests/hesai/hesai_ros_decoder_test_main_40p.cpp +++ /dev/null @@ -1,47 +0,0 @@ -#include "hesai_ros_decoder_test_40p.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_40p.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_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.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