feat: improve script for decoding packets into a new rosbag (#236)

* feat: create a new script for decoding packets into a new rosbag Signed-off-by: YuxuanLiuTier4Desktop <[email protected]> * do not use ament_auto instead Signed-off-by: YuxuanLiuTier4Desktop <[email protected]> * restore Signed-off-by: YuxuanLiuTier4Desktop <[email protected]> * fix spell check Signed-off-by: YuxuanLiuTier4Desktop <[email protected]> * merge modifications Signed-off-by: YuxuanLiuTier4Desktop <[email protected]> * fix bug Signed-off-by: YuxuanLiuTier4Desktop <[email protected]> * fix bug Signed-off-by: YuxuanLiuTier4Desktop <[email protected]> * reset output number to a small one so that we do not break things Signed-off-by: YuxuanLiuTier4Desktop <[email protected]> * update variable names Signed-off-by: YuxuanLiuTier4Desktop <[email protected]> * clean up default variables Signed-off-by: YuxuanLiuTier4Desktop <[email protected]> * ci(pre-commit): autofix * fix typo Signed-off-by: YuxuanLiuTier4Desktop <[email protected]> * update configuration file Signed-off-by: YuxuanLiuTier4Desktop <[email protected]> * fix CMakeList Signed-off-by: YuxuanLiuTier4Desktop <[email protected]> * remove extra empty line Signed-off-by: YuxuanLiuTier4Desktop <[email protected]> * 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 <[email protected]> * Update nebula_examples/launch/hesai_offline_bag_pcd.xml update grammar for the descriptions. Co-authored-by: Max Schmeller <[email protected]> * update default values and description Signed-off-by: YuxuanLiuTier4Desktop <[email protected]> --------- Signed-off-by: YuxuanLiuTier4Desktop <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Max Schmeller <[email protected]>
tier4 · Dec 2, 2024 · c53b4fd · c53b4fd
1 parent 06be1f7
commit c53b4fd
Show file tree

Hide file tree

Showing 4 changed files with 132 additions and 49 deletions.
diff --git a/nebula_examples/CMakeLists.txt b/nebula_examples/CMakeLists.txt
@@ -92,6 +92,12 @@ 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

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
@@ -69,11 +69,17 @@ 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_;
   int out_num_;
   int skip_num_;
   bool only_xyz_;
+
+  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
@@ -1,38 +1,41 @@
 <?xml version="1.0"?>
 <launch>
-    <arg name="sensor_model" description="Pandar64|Pandar40P|PandarXT32|PandarXT32M|PandarAT128|PandarQT64"/>
-    <arg name="return_mode" default="Dual" description="See readme for supported return modes"/>
-    <arg name="frame_id"  default="hesai"/>
-    <arg name="scan_phase" default="0.0" />
+    <!-- action selection -->
+    <arg name="output_pcd" default="true" description="Whether to output each decoded pointclouds to a PCD file"/>
+    <arg name="output_rosbag" default="true" description="Whether to output the decoded pointclouds to a new RosBag"/>
+    <arg name="forward_packets_to_rosbag" default="false" description="Whether to copy the input packets to the output RosBag"/>
+
+    <!-- sensor related configuration -->
+    <arg name="sensor_model"  description="Pandar64|Pandar40P|PandarXT32|PandarXT32M|PandarAT128|PandarQT64|Pandar128E4X"/>
+    <arg name="lidar_parameter_file" default="$(find-pkg-share nebula_ros)/config/lidar/hesai/$(var sensor_model).param.yaml" description="Path: LiDAR configuration yaml"/>
+
+    <!-- output sample number configuration -->
+    <arg name="out_num" default="0" description="The maximum number of pointclouds to output. 0 to output all pointclouds"/>
+    <arg name="skip_num" default="0" description="The number of pointclouds to discard at the beginning"/>
+    <arg name="bag_path" description="Path of the input RosBag"/>
+    <arg name="input_topic" default="/pandar_packets" description="Name of the input packets topic, e.g. /pandar_packets"/>
+    <arg name="output_topic" default="/pandar_points" description="Name of the output pointcloud topic"/>
+    <arg name="out_path" description="Directory path to which the output RosBag and PCDs will be saved"/>
 
-    <arg name="calibration_file" default="$(find-pkg-share nebula_decoders)/calibration/hesai/$(var sensor_model).csv"/>
-    <arg name="correction_file" default="$(find-pkg-share nebula_decoders)/calibration/hesai/$(var sensor_model).dat"/>
 
-    <arg name="bag_path" default="path to bag dir"/>
     <arg name="storage_id" default="sqlite3"/>
-    <arg name="out_path" default="path to output dir"/>
     <arg name="format" default="cdr"/>
-    <arg name="target_topic" default="/pandar_packets"/>
-    <arg name="out_num" default="1"/>
-    <arg name="skip_num" default="3"/>
     <arg name="only_xyz" default="true"/>
 
 
     <node pkg="nebula_examples" exec="hesai_ros_offline_extract_bag_pcd_node"
           name="hesai_cloud" output="screen">
-        <param name="sensor_model" value="$(var sensor_model)"/>
-        <param name="return_mode" value="$(var return_mode)"/>
-        <param name="frame_id" value="$(var frame_id)"/>
-        <param name="scan_phase" value="$(var scan_phase)"/>
-        <param name="calibration_file" value="$(var calibration_file)"/>
-        <param name="correction_file" value="$(var correction_file)"/>
+        <param from="$(var lidar_parameter_file)" allow_substs="true" />
+        <param name="output_pcd" value="$(var output_pcd)"/>
+        <param name="output_rosbag" value="$(var output_rosbag)"/>
+        <param name="forward_packets_to_rosbag" value="$(var forward_packets_to_rosbag)"/>
         <param name="bag_path" value="$(var bag_path)"/>
-        <param name="storage_id" value="$(var storage_id)"/>
         <param name="out_path" value="$(var out_path)"/>
+        <param name="input_topic" value="$(var input_topic)"/>
+        <param name="output_topic" value="$(var output_topic)"/>
+        <param name="storage_id" value="$(var storage_id)"/>
         <param name="format" value="$(var format)"/>
-        <param name="target_topic" value="$(var target_topic)"/>
         <param name="out_num" value="$(var out_num)"/>
         <param name="skip_num" value="$(var skip_num)"/>
-        <param name="only_xyz" value="$(var only_xyz)"/>
     </node>
 </launch>
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
@@ -24,6 +24,8 @@
 
 #include <nebula_common/hesai/hesai_common.hpp>
 
+#include <pcl_conversions/pcl_conversions.h>
+
 #include <regex>
 
 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<std::string>("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<double>("cut_angle", 0., param_read_only());
+    sensor_configuration.cloud_min_angle =
+      declare_parameter<uint16_t>("cloud_min_angle", 0, param_read_only());
+    sensor_configuration.cloud_max_angle =
+      declare_parameter<uint16_t>("cloud_max_angle", 360, param_read_only());
+    sensor_configuration.rotation_speed =
+      declare_parameter<uint16_t>("rotation_speed", 600, param_read_only());
+    sensor_configuration.dual_return_distance_threshold =
+      declare_parameter<double>("dual_return_distance_threshold", 0.1, param_read_only());
+    sensor_configuration.min_range = declare_parameter<double>("min_range", 0.3, param_read_only());
+    sensor_configuration.max_range =
+      declare_parameter<double>("max_range", 300.0, param_read_only());
+    sensor_configuration.packet_mtu_size =
+      declare_parameter<uint16_t>("packet_mtu_size", 1500, param_read_only());
   }
 
   calibration_configuration.calibration_file =
@@ -121,7 +137,13 @@ Status HesaiRosOfflineExtractBag::get_parameters(
   out_num_ = this->declare_parameter<uint16_t>("out_num", 3, param_read_only());
   skip_num_ = this->declare_parameter<uint16_t>("skip_num", 3, param_read_only());
   only_xyz_ = this->declare_parameter<bool>("only_xyz", false, param_read_only());
-  target_topic_ = this->declare_parameter<std::string>("target_topic", "", param_read_only());
+  input_topic_ = this->declare_parameter<std::string>("input_topic", "", param_read_only());
+  output_pointcloud_topic_ =
+    this->declare_parameter<std::string>("output_topic", "", param_read_only());
+  output_pcd_ = this->declare_parameter<bool>("output_pcd", false, param_read_only());
+  output_rosbag_ = this->declare_parameter<bool>("output_rosbag", true, param_read_only());
+  forward_packets_to_rosbag_ =
+    this->declare_parameter<bool>("forward_packets_to_rosbag", false, param_read_only());
 
   if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) {
     return Status::INVALID_SENSOR_MODEL;
@@ -170,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;
   }
@@ -197,12 +219,13 @@ Status HesaiRosOfflineExtractBag::read_bag()
   reader.open(storage_options, converter_options);
   int cnt = 0;
   int out_cnt = 0;
+  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;
     }
 
@@ -215,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<rosbag2_cpp::writers::SequentialWriter>();
+      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) {
@@ -233,35 +281,55 @@ 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<rosbag2_cpp::writers::SequentialWriter>();
-        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(),
-           ""});
-      }
-
-      bag_writer->write(bag_message);
       cnt++;
       if (skip_num_ < cnt) {
         out_cnt++;
-        if (only_xyz_) {
-          pcl::PointCloud<pcl::PointXYZ> 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 (output_pcd_) {
+          if (only_xyz_) {
+            pcl::PointCloud<pcl::PointXYZ> 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 (output_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<sensor_msgs::msg::PointCloud2> 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<rosbag2_storage::SerializedBagMessage>();
+          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<rcutils_uint8_array_t>(
+            cloud_serialized_msg.get_rcl_serialized_message());
+
+          // Write both messages to the bag
+          bag_writer->write(cloud_bag_msg);
         }
       }
 
-      if (out_num_ <= out_cnt) {
+      if (out_num_ != 0 && out_num_ <= out_cnt) {
+        output_limit_reached = true;
         break;
       }
     }
+
+    if (output_limit_reached) {
+      break;
+    }
   }
   return Status::OK;
 }