feat: create a new script for decoding packets into a new rosbag

Signed-off-by: YuxuanLiuTier4Desktop <[email protected]>
tier4 · Nov 26, 2024 · 8828069 · 8828069
1 parent 77b4c6c
commit 8828069
Show file tree

Hide file tree

Showing 5 changed files with 508 additions and 3 deletions.
diff --git 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
@@ -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 <diagnostic_updater/diagnostic_updater.hpp>
+#include <nebula_common/hesai/hesai_common.hpp>
+#include <nebula_common/nebula_common.hpp>
+#include <nebula_common/nebula_status.hpp>
+#include <nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp>
+#include <nebula_ros/common/parameter_descriptors.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <nebula_msgs/msg/nebula_packet.hpp>
+#include <nebula_msgs/msg/nebula_packets.hpp>
+#include <pandar_msgs/msg/pandar_packet.hpp>
+#include <pandar_msgs/msg/pandar_scan.hpp>
+
+#include <memory>
+#include <string>
+
+namespace nebula::ros
+{
+class HesaiRosOfflineExtractBagDecode final : public rclcpp::Node
+{
+  std::shared_ptr<drivers::HesaiDriver> driver_ptr_;
+  Status wrapper_status_;
+
+  std::shared_ptr<drivers::HesaiCalibrationConfiguration> calibration_cfg_ptr_;
+  std::shared_ptr<drivers::SensorConfigurationBase> sensor_cfg_ptr_;
+  std::shared_ptr<drivers::HesaiCorrection> correction_cfg_ptr_;
+
+  Status initialize_driver(
+    std::shared_ptr<drivers::SensorConfigurationBase> sensor_configuration,
+    std::shared_ptr<drivers::HesaiCalibrationConfigurationBase> 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::nanoseconds>(
+      std::chrono::duration<double>(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
@@ -0,0 +1,44 @@
+<?xml version="1.0"?>
+<launch>
+    <!-- io configuration -->
+    <arg name="sensor_model" default="Pandar128E4X" description="Pandar64|Pandar40P|PandarXT32|PandarXT32M|PandarAT128|PandarQT64"/>
+    <arg name="lidar_parameter_file" default="$(find-pkg-share nebula_ros)/config/lidar/hesai/Pandar128E4X.params.yaml"/>
+    <arg name="bag_path" default="rosbags/default_2024-10-24-10-50-48_p0900"/>
+    <arg name="target_topic" default="/sensing/lidar/top/pandar_packets"/>
+    <arg name="output_topic" default="/sensing/lidar/top/pointcloud"/>
+    <arg name="frame_id"  default="lidar_top"/>
+    <arg name="out_path" default="output_rosbag"/>
+
+    <!-- output sample number configuration -->
+    <arg name="out_num" default="3999"/>
+    <arg name="skip_num" default="0"/>
+
+    <!-- LiDAR configuration overriding parameter file -->
+    <arg name="scan_phase" default="0.0" />
+    <arg name="return_mode" default="Dual" description="See readme for supported return modes"/>
+
+    <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="storage_id" default="sqlite3"/>
+    <arg name="format" default="cdr"/>
+
+    <node pkg="nebula_examples" exec="hesai_ros_offline_extract_bag_decode_node"
+          name="hesai_cloud" output="screen">
+        <param from="$(var lidar_parameter_file)"/>
+        <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 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="format" value="$(var format)"/>
+        <param name="target_topic" value="$(var target_topic)"/>
+        <param name="output_topic" value="$(var output_topic)"/>
+        <param name="out_num" value="$(var out_num)"/>
+        <param name="skip_num" value="$(var skip_num)"/>
+    </node>
+</launch>