diff --git a/nebula_examples/CMakeLists.txt b/nebula_examples/CMakeLists.txt index 40519e4b..a8f27928 100644 --- a/nebula_examples/CMakeLists.txt +++ b/nebula_examples/CMakeLists.txt @@ -71,23 +71,6 @@ list(APPEND ${PROJECT_NAME}_EXECUTABLES hesai_ros_offline_extract_bag_pcd_node) target_link_libraries(hesai_ros_offline_extract_bag_pcd_node PUBLIC hesai_ros_offline_extract_bag_pcd ) -# Extraction for Hesai ROSBag -add_library(hesai_ros_offline_extract_bag_decode SHARED - ${CMAKE_CURRENT_SOURCE_DIR}/src/hesai/hesai_ros_offline_extract_bag_decode.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/src/common/parameter_descriptors.cpp -) -target_link_libraries(hesai_ros_offline_extract_bag_decode PUBLIC - nebula_decoders::nebula_decoders_hesai -) - -add_executable(hesai_ros_offline_extract_bag_decode_node - ${CMAKE_CURRENT_SOURCE_DIR}/src/hesai/hesai_ros_offline_extract_bag_decode_main.cpp -) -list(APPEND ${PROJECT_NAME}_EXECUTABLES hesai_ros_offline_extract_bag_decode_node) - -target_link_libraries(hesai_ros_offline_extract_bag_decode_node PUBLIC - hesai_ros_offline_extract_bag_decode -) ## Velodyne # Extraction for TEST Lib diff --git a/nebula_examples/include/hesai/hesai_ros_offline_extract_bag_decode.hpp b/nebula_examples/include/hesai/hesai_ros_offline_extract_bag_decode.hpp deleted file mode 100644 index c2ad0dd1..00000000 --- a/nebula_examples/include/hesai/hesai_ros_offline_extract_bag_decode.hpp +++ /dev/null @@ -1,83 +0,0 @@ -// Copyright 2023 Map IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef NEBULA_HesaiRosOfflineExtractBagDecode_H -#define NEBULA_HesaiRosOfflineExtractBagDecode_H - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include - -namespace nebula::ros -{ -class HesaiRosOfflineExtractBagDecode final : public rclcpp::Node -{ - 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_; - - Status initialize_driver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration); - - Status get_parameters( - drivers::HesaiSensorConfiguration & sensor_configuration, - drivers::HesaiCalibrationConfiguration & calibration_configuration, - drivers::HesaiCorrection & correction_configuration); - - static inline std::chrono::nanoseconds seconds_to_chrono_nano_seconds(const double seconds) - { - return std::chrono::duration_cast( - std::chrono::duration(seconds)); - } - -public: - explicit HesaiRosOfflineExtractBagDecode( - const rclcpp::NodeOptions & options, const std::string & node_name); - - Status get_status(); - Status read_bag(); - -private: - std::string bag_path_; - std::string storage_id_; - std::string out_path_; - std::string format_; - std::string target_topic_; - std::string correction_file_path_; - std::string frame_id_; - std::string output_topic_; - int out_num_; - int skip_num_; - bool only_xyz_; -}; - -} // namespace nebula::ros - -#endif // NEBULA_HesaiRosOfflineExtractBagDecode_H diff --git a/nebula_examples/include/hesai/hesai_ros_offline_extract_bag_pcd.hpp b/nebula_examples/include/hesai/hesai_ros_offline_extract_bag_pcd.hpp index 8b3e0520..b990c804 100644 --- a/nebula_examples/include/hesai/hesai_ros_offline_extract_bag_pcd.hpp +++ b/nebula_examples/include/hesai/hesai_ros_offline_extract_bag_pcd.hpp @@ -71,9 +71,15 @@ class HesaiRosOfflineExtractBag final : public rclcpp::Node std::string format_; std::string target_topic_; std::string correction_file_path_; + std::string frame_id_; + std::string output_pointcloud_topic_; int out_num_; int skip_num_; bool only_xyz_; + + bool write_pcd_flag_; + bool write_rosbag_pointcloud_flag_; + bool write_rosbag_packets_flag_; }; } // namespace nebula::ros diff --git a/nebula_examples/launch/hesai_offline_bag_decode.xml b/nebula_examples/launch/hesai_offline_bag_decode.xml deleted file mode 100644 index f26ce57b..00000000 --- a/nebula_examples/launch/hesai_offline_bag_decode.xml +++ /dev/null @@ -1,44 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/nebula_examples/launch/hesai_offline_bag_pcd.xml b/nebula_examples/launch/hesai_offline_bag_pcd.xml index acb60432..effb6058 100644 --- a/nebula_examples/launch/hesai_offline_bag_pcd.xml +++ b/nebula_examples/launch/hesai_offline_bag_pcd.xml @@ -1,25 +1,38 @@ - - - + + + + + + sensor related configuration + + + + + + + + + + + + + + - - - - - + @@ -31,8 +44,8 @@ + - diff --git a/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_decode.cpp b/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_decode.cpp deleted file mode 100644 index fca1f1bc..00000000 --- a/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_decode.cpp +++ /dev/null @@ -1,298 +0,0 @@ -// Copyright 2023 Map IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "hesai/hesai_ros_offline_extract_bag_decode.hpp" - -#include "rclcpp/serialization.hpp" -#include "rclcpp/serialized_message.hpp" -#include "rcpputils/filesystem_helper.hpp" -#include "rosbag2_cpp/reader.hpp" -#include "rosbag2_cpp/readers/sequential_reader.hpp" -#include "rosbag2_cpp/writers/sequential_writer.hpp" -#include "rosbag2_storage/storage_options.hpp" - -#include - -#include - -#include - -#include - -namespace nebula::ros -{ -HesaiRosOfflineExtractBagDecode::HesaiRosOfflineExtractBagDecode( - 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_ = - get_parameters(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_ = initialize_driver( - std::const_pointer_cast(sensor_cfg_ptr_), - correction_cfg_ptr_); - } else { - wrapper_status_ = initialize_driver( - std::const_pointer_cast(sensor_cfg_ptr_), - calibration_cfg_ptr_); - } - - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); -} - -Status HesaiRosOfflineExtractBagDecode::initialize_driver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) -{ - driver_ptr_ = std::make_shared( - std::static_pointer_cast(sensor_configuration), - calibration_configuration); - return driver_ptr_->get_status(); -} - -Status HesaiRosOfflineExtractBagDecode::get_status() -{ - return wrapper_status_; -} - -Status HesaiRosOfflineExtractBagDecode::get_parameters( - drivers::HesaiSensorConfiguration & sensor_configuration, - drivers::HesaiCalibrationConfiguration & calibration_configuration, - drivers::HesaiCorrection & correction_configuration) -{ - auto sensor_model_ = this->declare_parameter("sensor_model", "", param_read_only()); - sensor_configuration.sensor_model = nebula::drivers::sensor_model_from_string(sensor_model_); - auto return_mode_ = this->declare_parameter("return_mode", "", param_read_only()); - sensor_configuration.return_mode = - nebula::drivers::return_mode_from_string_hesai(return_mode_, sensor_configuration.sensor_model); - this->declare_parameter("frame_id", "pandar", param_read_only()); - sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); - frame_id_ = sensor_configuration.frame_id; - - { - rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); - descriptor.additional_constraints = "Angle where scans begin (degrees, [0.,360.]"; - descriptor.integer_range = int_range(0, 360, 1); - sensor_configuration.sync_angle = - declare_parameter("sync_angle", 0., param_read_only()); - } - - { - rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); - descriptor.additional_constraints = "Angle where scans begin (degrees, [0.,360.]"; - descriptor.floating_point_range = float_range(0, 360, 0.01); - sensor_configuration.cut_angle = declare_parameter("cut_angle", 0., param_read_only()); - sensor_configuration.cloud_min_angle = - declare_parameter("cloud_min_angle", 0, param_read_only()); - sensor_configuration.cloud_max_angle = - declare_parameter("cloud_max_angle", 360, param_read_only()); - sensor_configuration.rotation_speed = - declare_parameter("rotation_speed", 600, param_read_only()); - sensor_configuration.dual_return_distance_threshold = - declare_parameter("dual_return_distance_threshold", 0.1, param_read_only()); - sensor_configuration.min_range = declare_parameter("min_range", 0.3, param_read_only()); - sensor_configuration.max_range = - declare_parameter("max_range", 300.0, param_read_only()); - sensor_configuration.packet_mtu_size = - declare_parameter("packet_mtu_size", 1500, param_read_only()); - } - - calibration_configuration.calibration_file = - this->declare_parameter("calibration_file", "", param_read_only()); - if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { - correction_file_path_ = - this->declare_parameter("correction_file", "", param_read_only()); - } - - bag_path_ = this->declare_parameter("bag_path", "", param_read_only()); - storage_id_ = this->declare_parameter("storage_id", "sqlite3", param_read_only()); - out_path_ = this->declare_parameter("out_path", "", param_read_only()); - format_ = this->declare_parameter("format", "cdr", param_read_only()); - out_num_ = this->declare_parameter("out_num", 3, param_read_only()); - skip_num_ = this->declare_parameter("skip_num", 3, param_read_only()); - target_topic_ = this->declare_parameter("target_topic", "", param_read_only()); - output_topic_ = this->declare_parameter("output_topic", "", param_read_only()); - - 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()) { - return Status::SENSOR_CONFIG_ERROR; - } - if (calibration_configuration.calibration_file.empty()) { - return Status::INVALID_CALIBRATION_FILE; - } else { - auto cal_status = - calibration_configuration.load_from_file(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.load_from_file(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(), "Sensor Configuration: " << sensor_configuration); - return Status::OK; -} - -Status HesaiRosOfflineExtractBagDecode::read_bag() -{ - rosbag2_storage::StorageOptions storage_options; - rosbag2_cpp::ConverterOptions converter_options; - - std::cout << bag_path_ << std::endl; - std::cout << storage_id_ << std::endl; - std::cout << out_path_ << std::endl; - std::cout << format_ << std::endl; - std::cout << target_topic_ << std::endl; - std::cout << out_num_ << std::endl; - std::cout << skip_num_ << std::endl; - - rcpputils::fs::path o_dir(out_path_); - 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("/"), "_"); - o_dir = o_dir / rcpputils::fs::path(target_topic_name); - if (rcpputils::fs::create_directories(o_dir)) { - std::cout << "created: " << o_dir << std::endl; - } - - std::unique_ptr bag_writer{}; - storage_options.uri = bag_path_; - storage_options.storage_id = storage_id_; - converter_options.output_serialization_format = format_; - - rosbag2_cpp::Reader reader(std::make_unique()); - reader.open(storage_options, converter_options); - int cnt = 0; - int out_cnt = 0; - while (reader.has_next()) { - auto bag_message = reader.read_next(); - - // std::cout << "Found topic name " << bag_message->topic_name << std::endl; - - if (bag_message->topic_name != target_topic_) { - continue; - } - - std::cout << (cnt + 1) << ", " << (out_cnt + 1) << "/" << out_num_ << std::endl; - pandar_msgs::msg::PandarScan extracted_msg; - rclcpp::Serialization serialization; - 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; - - nebula_msgs::msg::NebulaPackets nebula_msg; - nebula_msg.header = extracted_msg.header; - for (auto & pkt : extracted_msg.packets) { - std::vector pkt_data(pkt.data.begin(), std::next(pkt.data.begin(), pkt.size)); - auto pointcloud_ts = driver_ptr_->parse_cloud_packet(pkt_data); - auto pointcloud = std::get<0>(pointcloud_ts); // This is a pcl::PointCloud; - - nebula_msgs::msg::NebulaPacket nebula_pkt; - nebula_pkt.stamp = pkt.stamp; - nebula_pkt.data.swap(pkt_data); // move storage from `pkt_data` to `data` - nebula_msg.packets.push_back(nebula_pkt); - - if (!pointcloud) { - continue; - } - - if (!bag_writer) { - const rosbag2_storage::StorageOptions storage_options_w( - {(o_dir / std::to_string(bag_message->time_stamp)).string(), "sqlite3"}); - const rosbag2_cpp::ConverterOptions converter_options_w( - {rmw_get_serialization_format(), rmw_get_serialization_format()}); - bag_writer = std::make_unique(); - bag_writer->open(storage_options_w, converter_options_w); - bag_writer->create_topic( - {output_topic_, "sensor_msgs/msg/PointCloud2", rmw_get_serialization_format(), ""}); - } - - cnt++; - if (cnt < skip_num_) { - continue; - } - out_cnt++; - // Create ROS Pointcloud and Write to ROSBag - { - // Create ROS Pointcloud from PCL pointcloud - sensor_msgs::msg::PointCloud2 cloud_msg; - pcl::toROSMsg(*pointcloud, cloud_msg); - cloud_msg.header = extracted_msg.header; - cloud_msg.header.frame_id = frame_id_; - - // Create a serialized message for the pointcloud - rclcpp::SerializedMessage cloud_serialized_msg; - rclcpp::Serialization cloud_serialization; - cloud_serialization.serialize_message(&cloud_msg, &cloud_serialized_msg); - - // Create a bag message for the pointcloud - auto cloud_bag_msg = std::make_shared(); - cloud_bag_msg->topic_name = output_topic_; - cloud_bag_msg->time_stamp = bag_message->time_stamp; - - // Create a new shared_ptr for the serialized data - cloud_bag_msg->serialized_data = std::make_shared( - cloud_serialized_msg.get_rcl_serialized_message()); - - // Write both messages to the bag - bag_writer->write(cloud_bag_msg); - } - } - if (out_num_ <= out_cnt) { - break; - } - } - return Status::OK; -} - -} // namespace nebula::ros diff --git a/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_decode_main.cpp b/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_decode_main.cpp deleted file mode 100644 index 6279ae93..00000000 --- a/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_decode_main.cpp +++ /dev/null @@ -1,64 +0,0 @@ -// Copyright 2023-2024 Map IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "hesai/hesai_ros_offline_extract_bag_decode.hpp" - -#include - -#include - -/** - * @file hesai_ros_offline_extract_bag_decode.cpp - * @brief Main script to decode Hesai LiDAR packets from a ROS bag and write to a new bag - * - * This script reads a ROS bag containing Hesai LiDAR packets, decodes them into standard - * sensor_msgs::pointcloud point cloud data, and writes the decoded data to a new ROS bag. It uses - * the HesaiRosOfflineExtractBagDecode class to handle the packet decoding and bag read/write - * operations. - * - * The script performs the following steps: - * 1. Initializes ROS node and executor - * 2. Creates an instance of HesaiRosOfflineExtractBagDecode - * 3. Checks driver status - * 4. If status is OK, reads and processes the input bag - * 5. Shuts down ROS node after processing completes - */ - -int main(int argc, char * argv[]) -{ - setvbuf(stdout, NULL, _IONBF, BUFSIZ); - - std::string node_name = "nebula_hesai_offline_extraction_with_bag"; - - rclcpp::init(argc, argv); - rclcpp::executors::SingleThreadedExecutor exec; - rclcpp::NodeOptions options; - - auto 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->get_status(); - if (driver_status == nebula::Status::OK) { - RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Reading Started"); - driver_status = hesai_driver->read_bag(); - } else { - RCLCPP_ERROR_STREAM(rclcpp::get_logger(node_name), driver_status); - } - RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Ending"); - rclcpp::shutdown(); - - return 0; -} diff --git a/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp b/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp index 576a7f8b..2ddb0dab 100644 --- a/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp +++ b/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp @@ -24,6 +24,8 @@ #include +#include + #include namespace nebula::ros @@ -91,6 +93,7 @@ Status HesaiRosOfflineExtractBag::get_parameters( nebula::drivers::return_mode_from_string_hesai(return_mode_, sensor_configuration.sensor_model); this->declare_parameter("frame_id", "pandar", param_read_only()); sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); + frame_id_ = sensor_configuration.frame_id; { rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); @@ -105,6 +108,19 @@ Status HesaiRosOfflineExtractBag::get_parameters( descriptor.additional_constraints = "Angle where scans begin (degrees, [0.,360.]"; descriptor.floating_point_range = float_range(0, 360, 0.01); sensor_configuration.cut_angle = declare_parameter("cut_angle", 0., param_read_only()); + sensor_configuration.cloud_min_angle = + declare_parameter("cloud_min_angle", 0, param_read_only()); + sensor_configuration.cloud_max_angle = + declare_parameter("cloud_max_angle", 360, param_read_only()); + sensor_configuration.rotation_speed = + declare_parameter("rotation_speed", 600, param_read_only()); + sensor_configuration.dual_return_distance_threshold = + declare_parameter("dual_return_distance_threshold", 0.1, param_read_only()); + sensor_configuration.min_range = declare_parameter("min_range", 0.3, param_read_only()); + sensor_configuration.max_range = + declare_parameter("max_range", 300.0, param_read_only()); + sensor_configuration.packet_mtu_size = + declare_parameter("packet_mtu_size", 1500, param_read_only()); } calibration_configuration.calibration_file = @@ -122,6 +138,13 @@ Status HesaiRosOfflineExtractBag::get_parameters( skip_num_ = this->declare_parameter("skip_num", 3, param_read_only()); only_xyz_ = this->declare_parameter("only_xyz", false, param_read_only()); target_topic_ = this->declare_parameter("target_topic", "", param_read_only()); + output_pointcloud_topic_ = + this->declare_parameter("output_topic", "", param_read_only()); + write_pcd_flag_ = this->declare_parameter("is_write_to_pcd", false, param_read_only()); + write_rosbag_pointcloud_flag_ = + this->declare_parameter("is_write_to_rosbag", true, param_read_only()); + write_rosbag_packets_flag_ = + this->declare_parameter("is_write_packets_to_rosbag", false, param_read_only()); if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { return Status::INVALID_SENSOR_MODEL; @@ -197,6 +220,7 @@ Status HesaiRosOfflineExtractBag::read_bag() reader.open(storage_options, converter_options); int cnt = 0; int out_cnt = 0; + bool pointcloud_enough = false; while (reader.has_next()) { auto bag_message = reader.read_next(); @@ -240,28 +264,71 @@ Status HesaiRosOfflineExtractBag::read_bag() {rmw_get_serialization_format(), rmw_get_serialization_format()}); bag_writer = std::make_unique(); bag_writer->open(storage_options_w, converter_options_w); - bag_writer->create_topic( - {bag_message->topic_name, "nebula_msgs/msg/NebulaPackets", rmw_get_serialization_format(), - ""}); + if (write_rosbag_packets_flag_) { + bag_writer->create_topic( + {bag_message->topic_name, "nebula_msgs/msg/NebulaPackets", + rmw_get_serialization_format(), ""}); + } + if (write_rosbag_pointcloud_flag_) { + bag_writer->create_topic( + {output_pointcloud_topic_, "sensor_msgs/msg/PointCloud2", + rmw_get_serialization_format(), ""}); + } + } + + if (write_rosbag_packets_flag_) { + bag_writer->write(bag_message); } - bag_writer->write(bag_message); cnt++; if (skip_num_ < cnt) { out_cnt++; - if (only_xyz_) { - pcl::PointCloud cloud_xyz; - pcl::copyPointCloud(*pointcloud, cloud_xyz); - pcd_writer.writeBinary((o_dir / fn).string(), cloud_xyz); - } else { - pcd_writer.writeBinary((o_dir / fn).string(), *pointcloud); + + if (write_pcd_flag_) { + if (only_xyz_) { + pcl::PointCloud cloud_xyz; + pcl::copyPointCloud(*pointcloud, cloud_xyz); + pcd_writer.writeBinary((o_dir / fn).string(), cloud_xyz); + } else { + pcd_writer.writeBinary((o_dir / fn).string(), *pointcloud); + } + } + + if (write_rosbag_packets_flag_) { + // Create ROS Pointcloud from PCL pointcloud + sensor_msgs::msg::PointCloud2 cloud_msg; + pcl::toROSMsg(*pointcloud, cloud_msg); + cloud_msg.header = extracted_msg.header; + cloud_msg.header.frame_id = frame_id_; + + // Create a serialized message for the pointcloud + rclcpp::SerializedMessage cloud_serialized_msg; + rclcpp::Serialization cloud_serialization; + cloud_serialization.serialize_message(&cloud_msg, &cloud_serialized_msg); + + // Create a bag message for the pointcloud + auto cloud_bag_msg = std::make_shared(); + cloud_bag_msg->topic_name = output_pointcloud_topic_; + cloud_bag_msg->time_stamp = bag_message->time_stamp; + + // Create a new shared_ptr for the serialized data + cloud_bag_msg->serialized_data = std::make_shared( + cloud_serialized_msg.get_rcl_serialized_message()); + + // Write both messages to the bag + bag_writer->write(cloud_bag_msg); } } if (out_num_ <= out_cnt) { + pointcloud_enough = true; break; } } + + if (pointcloud_enough) { + break; + } } return Status::OK; }