From 882806933ed5c216fb77acb68910bcf7e2fdb03a Mon Sep 17 00:00:00 2001 From: YuxuanLiuTier4Desktop <619684051@qq.com> Date: Tue, 26 Nov 2024 15:16:11 +0900 Subject: [PATCH 01/18] feat: create a new script for decoding packets into a new rosbag Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --- nebula_examples/CMakeLists.txt | 22 +- .../hesai_ros_offline_extract_bag_decode.hpp | 83 +++++ .../launch/hesai_offline_bag_decode.xml | 44 +++ .../hesai_ros_offline_extract_bag_decode.cpp | 298 ++++++++++++++++++ ...ai_ros_offline_extract_bag_decode_main.cpp | 64 ++++ 5 files changed, 508 insertions(+), 3 deletions(-) create mode 100644 nebula_examples/include/hesai/hesai_ros_offline_extract_bag_decode.hpp create mode 100644 nebula_examples/launch/hesai_offline_bag_decode.xml create mode 100644 nebula_examples/src/hesai/hesai_ros_offline_extract_bag_decode.cpp create mode 100644 nebula_examples/src/hesai/hesai_ros_offline_extract_bag_decode_main.cpp diff --git a/nebula_examples/CMakeLists.txt b/nebula_examples/CMakeLists.txt index 311105e30..5d194977d 100644 --- a/nebula_examples/CMakeLists.txt +++ b/nebula_examples/CMakeLists.txt @@ -45,7 +45,7 @@ target_link_libraries(hesai_ros_offline_extract_pcd PUBLIC nebula_decoders::nebula_decoders_hesai ) -add_executable(hesai_ros_offline_extract_pcd_node +ament_auto_add_executable(hesai_ros_offline_extract_pcd_node ${CMAKE_CURRENT_SOURCE_DIR}/src/hesai/hesai_ros_offline_extract_pcd_main.cpp ) @@ -62,13 +62,29 @@ target_link_libraries(hesai_ros_offline_extract_bag_pcd PUBLIC nebula_decoders::nebula_decoders_hesai ) -add_executable(hesai_ros_offline_extract_bag_pcd_node +ament_auto_add_executable(hesai_ros_offline_extract_bag_pcd_node ${CMAKE_CURRENT_SOURCE_DIR}/src/hesai/hesai_ros_offline_extract_bag_pcd_main.cpp ) 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 +) + +ament_auto_add_executable(hesai_ros_offline_extract_bag_decode_node + ${CMAKE_CURRENT_SOURCE_DIR}/src/hesai/hesai_ros_offline_extract_bag_decode_main.cpp +) + +target_link_libraries(hesai_ros_offline_extract_bag_decode_node PUBLIC + hesai_ros_offline_extract_bag_decode +) ## Velodyne # Extraction for TEST Lib @@ -79,7 +95,7 @@ target_link_libraries(velodyne_ros_offline_extract_bag_pcd PUBLIC nebula_decoders::nebula_decoders_velodyne ) -add_executable(velodyne_ros_offline_extract_bag_pcd_node +ament_auto_add_executable(velodyne_ros_offline_extract_bag_pcd_node ${CMAKE_CURRENT_SOURCE_DIR}/src/velodyne/velodyne_ros_offline_extract_bag_pcd_main.cpp ) 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 new file mode 100644 index 000000000..c2ad0dd1b --- /dev/null +++ b/nebula_examples/include/hesai/hesai_ros_offline_extract_bag_decode.hpp @@ -0,0 +1,83 @@ +// 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/launch/hesai_offline_bag_decode.xml b/nebula_examples/launch/hesai_offline_bag_decode.xml new file mode 100644 index 000000000..f26ce57b6 --- /dev/null +++ b/nebula_examples/launch/hesai_offline_bag_decode.xml @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 new file mode 100644 index 000000000..f3288f6df --- /dev/null +++ b/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_decode.cpp @@ -0,0 +1,298 @@ +// 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 ROS2 Pointcloud and Write to ROSBag + { + // Create ROS2 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 new file mode 100644 index 000000000..d73b78c03 --- /dev/null +++ b/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_decode_main.cpp @@ -0,0 +1,64 @@ +// 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 ROS2 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 ROS2 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; +} From cb633d22d73deecd30237019413f89bfca7a7d29 Mon Sep 17 00:00:00 2001 From: YuxuanLiuTier4Desktop <619684051@qq.com> Date: Tue, 26 Nov 2024 16:27:22 +0900 Subject: [PATCH 02/18] do not use ament_auto instead Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --- nebula_examples/CMakeLists.txt | 20 ++++++++++++-------- 1 file changed, 12 insertions(+), 8 deletions(-) diff --git a/nebula_examples/CMakeLists.txt b/nebula_examples/CMakeLists.txt index 5d194977d..5c22f55a2 100644 --- a/nebula_examples/CMakeLists.txt +++ b/nebula_examples/CMakeLists.txt @@ -45,9 +45,10 @@ target_link_libraries(hesai_ros_offline_extract_pcd PUBLIC nebula_decoders::nebula_decoders_hesai ) -ament_auto_add_executable(hesai_ros_offline_extract_pcd_node - ${CMAKE_CURRENT_SOURCE_DIR}/src/hesai/hesai_ros_offline_extract_pcd_main.cpp +add_executable(hesai_ros_offline_extract_pcd_node + src/hesai/hesai_ros_offline_extract_pcd_main.cpp ) +list(APPEND ${PROJECT_NAME}_EXECUTABLES hesai_ros_offline_extract_pcd_node) target_link_libraries(hesai_ros_offline_extract_pcd_node PUBLIC hesai_ros_offline_extract_pcd @@ -62,9 +63,10 @@ target_link_libraries(hesai_ros_offline_extract_bag_pcd PUBLIC nebula_decoders::nebula_decoders_hesai ) -ament_auto_add_executable(hesai_ros_offline_extract_bag_pcd_node - ${CMAKE_CURRENT_SOURCE_DIR}/src/hesai/hesai_ros_offline_extract_bag_pcd_main.cpp +add_executable(hesai_ros_offline_extract_bag_pcd_node + src/hesai/hesai_ros_offline_extract_bag_pcd_main.cpp ) +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 @@ -78,9 +80,10 @@ target_link_libraries(hesai_ros_offline_extract_bag_decode PUBLIC nebula_decoders::nebula_decoders_hesai ) -ament_auto_add_executable(hesai_ros_offline_extract_bag_decode_node - ${CMAKE_CURRENT_SOURCE_DIR}/src/hesai/hesai_ros_offline_extract_bag_decode_main.cpp +add_executable(hesai_ros_offline_extract_bag_decode_node + 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 @@ -95,9 +98,10 @@ target_link_libraries(velodyne_ros_offline_extract_bag_pcd PUBLIC nebula_decoders::nebula_decoders_velodyne ) -ament_auto_add_executable(velodyne_ros_offline_extract_bag_pcd_node - ${CMAKE_CURRENT_SOURCE_DIR}/src/velodyne/velodyne_ros_offline_extract_bag_pcd_main.cpp +add_executable(velodyne_ros_offline_extract_bag_pcd_node + src/velodyne/velodyne_ros_offline_extract_bag_pcd_main.cpp ) +list(APPEND ${PROJECT_NAME}_EXECUTABLES velodyne_ros_offline_extract_bag_pcd_node) target_link_libraries(velodyne_ros_offline_extract_bag_pcd_node PUBLIC velodyne_ros_offline_extract_bag_pcd From fe3df92ee46a4252818e1e46eb2b108673bcd9ac Mon Sep 17 00:00:00 2001 From: YuxuanLiuTier4Desktop <619684051@qq.com> Date: Tue, 26 Nov 2024 16:31:09 +0900 Subject: [PATCH 03/18] restore Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --- nebula_examples/CMakeLists.txt | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/nebula_examples/CMakeLists.txt b/nebula_examples/CMakeLists.txt index 5c22f55a2..40519e4bc 100644 --- a/nebula_examples/CMakeLists.txt +++ b/nebula_examples/CMakeLists.txt @@ -46,7 +46,7 @@ target_link_libraries(hesai_ros_offline_extract_pcd PUBLIC ) add_executable(hesai_ros_offline_extract_pcd_node - src/hesai/hesai_ros_offline_extract_pcd_main.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/hesai/hesai_ros_offline_extract_pcd_main.cpp ) list(APPEND ${PROJECT_NAME}_EXECUTABLES hesai_ros_offline_extract_pcd_node) @@ -64,7 +64,7 @@ target_link_libraries(hesai_ros_offline_extract_bag_pcd PUBLIC ) add_executable(hesai_ros_offline_extract_bag_pcd_node - src/hesai/hesai_ros_offline_extract_bag_pcd_main.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/hesai/hesai_ros_offline_extract_bag_pcd_main.cpp ) list(APPEND ${PROJECT_NAME}_EXECUTABLES hesai_ros_offline_extract_bag_pcd_node) @@ -81,7 +81,7 @@ target_link_libraries(hesai_ros_offline_extract_bag_decode PUBLIC ) add_executable(hesai_ros_offline_extract_bag_decode_node - src/hesai/hesai_ros_offline_extract_bag_decode_main.cpp + ${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) @@ -99,7 +99,7 @@ target_link_libraries(velodyne_ros_offline_extract_bag_pcd PUBLIC ) add_executable(velodyne_ros_offline_extract_bag_pcd_node - src/velodyne/velodyne_ros_offline_extract_bag_pcd_main.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/velodyne/velodyne_ros_offline_extract_bag_pcd_main.cpp ) list(APPEND ${PROJECT_NAME}_EXECUTABLES velodyne_ros_offline_extract_bag_pcd_node) From 80f7b26b002f10db3ce973944257a42856c92b9d Mon Sep 17 00:00:00 2001 From: YuxuanLiuTier4Desktop <619684051@qq.com> Date: Tue, 26 Nov 2024 17:00:17 +0900 Subject: [PATCH 04/18] fix spell check Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --- .../src/hesai/hesai_ros_offline_extract_bag_decode.cpp | 4 ++-- .../src/hesai/hesai_ros_offline_extract_bag_decode_main.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) 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 index f3288f6df..fca1f1bcc 100644 --- a/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_decode.cpp +++ b/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_decode.cpp @@ -262,9 +262,9 @@ Status HesaiRosOfflineExtractBagDecode::read_bag() continue; } out_cnt++; - // Create ROS2 Pointcloud and Write to ROSBag + // Create ROS Pointcloud and Write to ROSBag { - // Create ROS2 Pointcloud from PCL pointcloud + // Create ROS Pointcloud from PCL pointcloud sensor_msgs::msg::PointCloud2 cloud_msg; pcl::toROSMsg(*pointcloud, cloud_msg); cloud_msg.header = extracted_msg.header; 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 index d73b78c03..6279ae93d 100644 --- 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 @@ -28,11 +28,11 @@ * operations. * * The script performs the following steps: - * 1. Initializes ROS2 node and executor + * 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 ROS2 node after processing completes + * 5. Shuts down ROS node after processing completes */ int main(int argc, char * argv[]) From 322fe29242599d0e08666c0401e2ace640f0aa7c Mon Sep 17 00:00:00 2001 From: YuxuanLiuTier4Desktop <619684051@qq.com> Date: Tue, 26 Nov 2024 18:34:33 +0900 Subject: [PATCH 05/18] merge modifications Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --- nebula_examples/CMakeLists.txt | 17 - .../hesai_ros_offline_extract_bag_decode.hpp | 83 ----- .../hesai_ros_offline_extract_bag_pcd.hpp | 6 + .../launch/hesai_offline_bag_decode.xml | 44 --- .../launch/hesai_offline_bag_pcd.xml | 31 +- .../hesai_ros_offline_extract_bag_decode.cpp | 298 ------------------ ...ai_ros_offline_extract_bag_decode_main.cpp | 64 ---- .../hesai_ros_offline_extract_bag_pcd.cpp | 87 ++++- 8 files changed, 105 insertions(+), 525 deletions(-) delete mode 100644 nebula_examples/include/hesai/hesai_ros_offline_extract_bag_decode.hpp delete mode 100644 nebula_examples/launch/hesai_offline_bag_decode.xml delete mode 100644 nebula_examples/src/hesai/hesai_ros_offline_extract_bag_decode.cpp delete mode 100644 nebula_examples/src/hesai/hesai_ros_offline_extract_bag_decode_main.cpp diff --git a/nebula_examples/CMakeLists.txt b/nebula_examples/CMakeLists.txt index 40519e4bc..a8f279284 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 c2ad0dd1b..000000000 --- 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 8b3e0520c..b990c8045 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 f26ce57b6..000000000 --- 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 acb604321..effb60583 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 fca1f1bcc..000000000 --- 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 6279ae93d..000000000 --- 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 576a7f8bb..2ddb0dab6 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; } From af77670a7f50add59f1c4fb7a9c4fc39c0441de8 Mon Sep 17 00:00:00 2001 From: YuxuanLiuTier4Desktop <619684051@qq.com> Date: Tue, 26 Nov 2024 18:36:31 +0900 Subject: [PATCH 06/18] fix bug Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --- nebula_examples/launch/hesai_offline_bag_pcd.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nebula_examples/launch/hesai_offline_bag_pcd.xml b/nebula_examples/launch/hesai_offline_bag_pcd.xml index effb60583..d499b3112 100644 --- a/nebula_examples/launch/hesai_offline_bag_pcd.xml +++ b/nebula_examples/launch/hesai_offline_bag_pcd.xml @@ -5,7 +5,7 @@ - sensor related configuration + From 04f2e7b82a76baaae440a5ebd3913cd97c9f61e4 Mon Sep 17 00:00:00 2001 From: YuxuanLiuTier4Desktop <619684051@qq.com> Date: Tue, 26 Nov 2024 18:37:36 +0900 Subject: [PATCH 07/18] fix bug Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --- nebula_examples/launch/hesai_offline_bag_pcd.xml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/nebula_examples/launch/hesai_offline_bag_pcd.xml b/nebula_examples/launch/hesai_offline_bag_pcd.xml index d499b3112..161e692a4 100644 --- a/nebula_examples/launch/hesai_offline_bag_pcd.xml +++ b/nebula_examples/launch/hesai_offline_bag_pcd.xml @@ -33,6 +33,9 @@ + + + From 645052fbe87bd6930fc444ff78b28fd7d62844e1 Mon Sep 17 00:00:00 2001 From: YuxuanLiuTier4Desktop <619684051@qq.com> Date: Tue, 26 Nov 2024 18:41:02 +0900 Subject: [PATCH 08/18] reset output number to a small one so that we do not break things Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --- nebula_examples/launch/hesai_offline_bag_pcd.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nebula_examples/launch/hesai_offline_bag_pcd.xml b/nebula_examples/launch/hesai_offline_bag_pcd.xml index 161e692a4..d61967a7a 100644 --- a/nebula_examples/launch/hesai_offline_bag_pcd.xml +++ b/nebula_examples/launch/hesai_offline_bag_pcd.xml @@ -15,8 +15,8 @@ - - + + From f5280adcb33f2f107a7978730ae503243521df02 Mon Sep 17 00:00:00 2001 From: YuxuanLiuTier4Desktop <619684051@qq.com> Date: Wed, 27 Nov 2024 17:34:20 +0900 Subject: [PATCH 09/18] update variable names Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --- .../hesai_ros_offline_extract_bag_pcd.hpp | 8 +- .../launch/hesai_offline_bag_pcd.xml | 16 ++-- .../hesai_ros_offline_extract_bag_pcd.cpp | 83 ++++++++++--------- 3 files changed, 54 insertions(+), 53 deletions(-) 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 b990c8045..9427ef7fd 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 @@ -69,7 +69,7 @@ class HesaiRosOfflineExtractBag final : public rclcpp::Node std::string storage_id_; std::string out_path_; std::string format_; - std::string target_topic_; + std::string input_topic_; std::string correction_file_path_; std::string frame_id_; std::string output_pointcloud_topic_; @@ -77,9 +77,9 @@ class HesaiRosOfflineExtractBag final : public rclcpp::Node int skip_num_; bool only_xyz_; - bool write_pcd_flag_; - bool write_rosbag_pointcloud_flag_; - bool write_rosbag_packets_flag_; + bool output_pcd_; + bool output_rosbag_; + bool forward_packets_to_rosbag_; }; } // namespace nebula::ros diff --git a/nebula_examples/launch/hesai_offline_bag_pcd.xml b/nebula_examples/launch/hesai_offline_bag_pcd.xml index d61967a7a..610e12ca8 100644 --- a/nebula_examples/launch/hesai_offline_bag_pcd.xml +++ b/nebula_examples/launch/hesai_offline_bag_pcd.xml @@ -1,15 +1,15 @@ - - - + + + - + @@ -33,9 +33,9 @@ - - - + + + @@ -46,7 +46,7 @@ - + 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 2ddb0dab6..2dcfbc42e 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 @@ -137,14 +137,13 @@ Status HesaiRosOfflineExtractBag::get_parameters( out_num_ = this->declare_parameter("out_num", 3, param_read_only()); 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()); + input_topic_ = this->declare_parameter("input_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()); + output_pcd_ = this->declare_parameter("output_pcd", false, param_read_only()); + output_rosbag_ = this->declare_parameter("output_rosbag", true, param_read_only()); + forward_packets_to_rosbag_ = + this->declare_parameter("forward_packets_to_rosbag", false, param_read_only()); if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { return Status::INVALID_SENSOR_MODEL; @@ -193,18 +192,18 @@ Status HesaiRosOfflineExtractBag::read_bag() 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 << input_topic_ << std::endl; std::cout << out_num_ << std::endl; std::cout << skip_num_ << std::endl; std::cout << only_xyz_ << 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); + auto input_topic_name = input_topic_; + if (input_topic_name.substr(0, 1) == "/") { + input_topic_name = input_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); + input_topic_name = std::regex_replace(input_topic_name, std::regex("/"), "_"); + o_dir = o_dir / rcpputils::fs::path(input_topic_name); if (rcpputils::fs::create_directories(o_dir)) { std::cout << "created: " << o_dir << std::endl; } @@ -220,13 +219,13 @@ Status HesaiRosOfflineExtractBag::read_bag() reader.open(storage_options, converter_options); int cnt = 0; int out_cnt = 0; - bool pointcloud_enough = false; + bool output_limit_reached = false; 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_) { + if (bag_message->topic_name != input_topic_) { continue; } @@ -239,6 +238,31 @@ Status HesaiRosOfflineExtractBag::read_bag() std::cout << "Found data in topic " << bag_message->topic_name << ": " << bag_message->time_stamp << std::endl; + // Initialize the bag writer if it is not initialized + 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); + if (forward_packets_to_rosbag_) { + bag_writer->create_topic( + {bag_message->topic_name, "nebula_msgs/msg/NebulaPackets", rmw_get_serialization_format(), + ""}); + } + if (output_rosbag_) { + bag_writer->create_topic( + {output_pointcloud_topic_, "sensor_msgs/msg/PointCloud2", rmw_get_serialization_format(), + ""}); + } + } + + // Forward the bag_message + if (forward_packets_to_rosbag_) { + bag_writer->write(bag_message); + } + nebula_msgs::msg::NebulaPackets nebula_msg; nebula_msg.header = extracted_msg.header; for (auto & pkt : extracted_msg.packets) { @@ -257,34 +281,11 @@ Status HesaiRosOfflineExtractBag::read_bag() auto fn = std::to_string(bag_message->time_stamp) + ".pcd"; - 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); - 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); - } - cnt++; if (skip_num_ < cnt) { out_cnt++; - if (write_pcd_flag_) { + if (output_pcd_) { if (only_xyz_) { pcl::PointCloud cloud_xyz; pcl::copyPointCloud(*pointcloud, cloud_xyz); @@ -294,7 +295,7 @@ Status HesaiRosOfflineExtractBag::read_bag() } } - if (write_rosbag_packets_flag_) { + if (output_rosbag_) { // Create ROS Pointcloud from PCL pointcloud sensor_msgs::msg::PointCloud2 cloud_msg; pcl::toROSMsg(*pointcloud, cloud_msg); @@ -321,12 +322,12 @@ Status HesaiRosOfflineExtractBag::read_bag() } if (out_num_ <= out_cnt) { - pointcloud_enough = true; + output_limit_reached = true; break; } } - if (pointcloud_enough) { + if (output_limit_reached) { break; } } From 8c1fa5bdf24a6109e142abd2e30554a7f020df0f Mon Sep 17 00:00:00 2001 From: YuxuanLiuTier4Desktop <619684051@qq.com> Date: Wed, 27 Nov 2024 17:35:00 +0900 Subject: [PATCH 10/18] clean up default variables Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --- .../launch/hesai_offline_bag_pcd.xml | 39 +++++++------------ 1 file changed, 13 insertions(+), 26 deletions(-) diff --git a/nebula_examples/launch/hesai_offline_bag_pcd.xml b/nebula_examples/launch/hesai_offline_bag_pcd.xml index 610e12ca8..d1b1f5e99 100644 --- a/nebula_examples/launch/hesai_offline_bag_pcd.xml +++ b/nebula_examples/launch/hesai_offline_bag_pcd.xml @@ -1,29 +1,22 @@ - - - + + + - - - - - - - + + + + + + - - - - - - @@ -32,23 +25,17 @@ - + - - - - - - - - + + - + \ No newline at end of file From c18f4caefd0eb6f3e06e921a58d96e28fa793412 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 27 Nov 2024 08:35:29 +0000 Subject: [PATCH 11/18] ci(pre-commit): autofix --- nebula_examples/launch/hesai_offline_bag_pcd.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nebula_examples/launch/hesai_offline_bag_pcd.xml b/nebula_examples/launch/hesai_offline_bag_pcd.xml index d1b1f5e99..e1777e9a1 100644 --- a/nebula_examples/launch/hesai_offline_bag_pcd.xml +++ b/nebula_examples/launch/hesai_offline_bag_pcd.xml @@ -38,4 +38,4 @@ - \ No newline at end of file + From f52eb416d5e64ae25695a7027fa62a4a61efd0c4 Mon Sep 17 00:00:00 2001 From: YuxuanLiuTier4Desktop <619684051@qq.com> Date: Wed, 27 Nov 2024 17:44:24 +0900 Subject: [PATCH 12/18] fix typo Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --- nebula_examples/launch/hesai_offline_bag_pcd.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nebula_examples/launch/hesai_offline_bag_pcd.xml b/nebula_examples/launch/hesai_offline_bag_pcd.xml index d1b1f5e99..f6d30aa60 100644 --- a/nebula_examples/launch/hesai_offline_bag_pcd.xml +++ b/nebula_examples/launch/hesai_offline_bag_pcd.xml @@ -2,7 +2,7 @@ - + From 580b4178452d1b856cb79350adf62b5c152d1c96 Mon Sep 17 00:00:00 2001 From: YuxuanLiuTier4Desktop <619684051@qq.com> Date: Wed, 27 Nov 2024 18:14:04 +0900 Subject: [PATCH 13/18] update configuration file Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --- nebula_examples/launch/hesai_offline_bag_pcd.xml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/nebula_examples/launch/hesai_offline_bag_pcd.xml b/nebula_examples/launch/hesai_offline_bag_pcd.xml index 597e864c6..38650eb58 100644 --- a/nebula_examples/launch/hesai_offline_bag_pcd.xml +++ b/nebula_examples/launch/hesai_offline_bag_pcd.xml @@ -7,15 +7,15 @@ - + - - + + From 6835e0f091be132706a78adec5fbf7e9e878461e Mon Sep 17 00:00:00 2001 From: YuxuanLiuTier4Desktop <619684051@qq.com> Date: Thu, 28 Nov 2024 09:29:55 +0900 Subject: [PATCH 14/18] fix CMakeList Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --- nebula_examples/CMakeLists.txt | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/nebula_examples/CMakeLists.txt b/nebula_examples/CMakeLists.txt index a8f279284..6bcde8934 100644 --- a/nebula_examples/CMakeLists.txt +++ b/nebula_examples/CMakeLists.txt @@ -48,7 +48,6 @@ target_link_libraries(hesai_ros_offline_extract_pcd PUBLIC add_executable(hesai_ros_offline_extract_pcd_node ${CMAKE_CURRENT_SOURCE_DIR}/src/hesai/hesai_ros_offline_extract_pcd_main.cpp ) -list(APPEND ${PROJECT_NAME}_EXECUTABLES hesai_ros_offline_extract_pcd_node) target_link_libraries(hesai_ros_offline_extract_pcd_node PUBLIC hesai_ros_offline_extract_pcd @@ -66,7 +65,6 @@ target_link_libraries(hesai_ros_offline_extract_bag_pcd PUBLIC add_executable(hesai_ros_offline_extract_bag_pcd_node ${CMAKE_CURRENT_SOURCE_DIR}/src/hesai/hesai_ros_offline_extract_bag_pcd_main.cpp ) -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 @@ -84,7 +82,6 @@ target_link_libraries(velodyne_ros_offline_extract_bag_pcd PUBLIC add_executable(velodyne_ros_offline_extract_bag_pcd_node ${CMAKE_CURRENT_SOURCE_DIR}/src/velodyne/velodyne_ros_offline_extract_bag_pcd_main.cpp ) -list(APPEND ${PROJECT_NAME}_EXECUTABLES velodyne_ros_offline_extract_bag_pcd_node) target_link_libraries(velodyne_ros_offline_extract_bag_pcd_node PUBLIC velodyne_ros_offline_extract_bag_pcd @@ -95,6 +92,13 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() +install(TARGETS hesai_ros_offline_extract_pcd_node + hesai_ros_offline_extract_bag_pcd_node + velodyne_ros_offline_extract_bag_pcd_node + DESTINATION lib/${PROJECT_NAME} +) + + ament_auto_package( INSTALL_TO_SHARE launch From 662275bca1776779ecb55b35c8caf9e61ac5141d Mon Sep 17 00:00:00 2001 From: YuxuanLiuTier4Desktop <619684051@qq.com> Date: Thu, 28 Nov 2024 09:30:37 +0900 Subject: [PATCH 15/18] remove extra empty line Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --- nebula_examples/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/nebula_examples/CMakeLists.txt b/nebula_examples/CMakeLists.txt index 6bcde8934..779987293 100644 --- a/nebula_examples/CMakeLists.txt +++ b/nebula_examples/CMakeLists.txt @@ -98,7 +98,6 @@ install(TARGETS hesai_ros_offline_extract_pcd_node DESTINATION lib/${PROJECT_NAME} ) - ament_auto_package( INSTALL_TO_SHARE launch From b37c24ffbe33f88d2a8279af62b5f82bab06abe1 Mon Sep 17 00:00:00 2001 From: Yuxuan Liu <619684051@qq.com> Date: Mon, 2 Dec 2024 16:30:58 +0900 Subject: [PATCH 16/18] Update nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp output all point clouds when out_num_ = 0 Co-authored-by: Max Schmeller <6088931+mojomex@users.noreply.github.com> --- nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 2dcfbc42e..06e586aaa 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 @@ -321,7 +321,7 @@ Status HesaiRosOfflineExtractBag::read_bag() } } - if (out_num_ <= out_cnt) { + if (out_num_ != 0 && out_num_ <= out_cnt) { output_limit_reached = true; break; } From 71a8cd0c2bdc41ef0dad5abd9056f522d9d66d40 Mon Sep 17 00:00:00 2001 From: Yuxuan Liu <619684051@qq.com> Date: Mon, 2 Dec 2024 16:31:28 +0900 Subject: [PATCH 17/18] Update nebula_examples/launch/hesai_offline_bag_pcd.xml update grammar for the descriptions. Co-authored-by: Max Schmeller <6088931+mojomex@users.noreply.github.com> --- nebula_examples/launch/hesai_offline_bag_pcd.xml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/nebula_examples/launch/hesai_offline_bag_pcd.xml b/nebula_examples/launch/hesai_offline_bag_pcd.xml index 38650eb58..2632c08b1 100644 --- a/nebula_examples/launch/hesai_offline_bag_pcd.xml +++ b/nebula_examples/launch/hesai_offline_bag_pcd.xml @@ -1,9 +1,9 @@ - - - + + + From 0eddb9a475335b84cf9c3eae2bca31684875ff63 Mon Sep 17 00:00:00 2001 From: YuxuanLiuTier4Desktop <619684051@qq.com> Date: Mon, 2 Dec 2024 16:33:03 +0900 Subject: [PATCH 18/18] update default values and description Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --- nebula_examples/launch/hesai_offline_bag_pcd.xml | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/nebula_examples/launch/hesai_offline_bag_pcd.xml b/nebula_examples/launch/hesai_offline_bag_pcd.xml index 2632c08b1..b9e308e89 100644 --- a/nebula_examples/launch/hesai_offline_bag_pcd.xml +++ b/nebula_examples/launch/hesai_offline_bag_pcd.xml @@ -8,14 +8,14 @@ - - - - - - + + + + + +