wip: save pcb

scripts/save_pointcloud.py

@@ -0,0 +1,77 @@
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import PointCloud2
+import sensor_msgs_py.point_cloud2 as pc2
+import numpy as np
+import matplotlib.pyplot as plt
+import struct
+from io import BytesIO
+
+class PointCloudSaver(Node):
+    def __init__(self, node_name: str, pointcloud_topic: str, buffer, timeout_ms: int):
+        super().__init__(node_name)
+        self.subscription = self.create_subscription(
+            PointCloud2,
+            pointcloud_topic,
+            self.callback,
+            10
+        )
+        self.buffer = buffer
+        self.points = []
+        self.end_time = self.get_clock().now().nanoseconds + (timeout_ms * 1_000_000)
+        self.cmap = plt.get_cmap('jet') 
+
+    def callback(self, msg):
+        now = self.get_clock().now().nanoseconds
+        for p in pc2.read_points(msg, field_names=("x", "y", "z", "intensity"), skip_nans=True):
+            self.points.append([p[0], p[1], p[2], p[3]])
+
+        if now > self.end_time:
+            if not self.points:
+                self.get_logger().warn("No points received!")
+                rclpy.shutdown()
+                return
+
+            np_points = np.array(self.points, dtype=np.float32)
+            intensities = np_points[:, 3]
+            norm_int = (intensities - intensities.min()) / (intensities.ptp() + 1e-8)
+
+            # Map normalized intensity to RGB colormap
+            colors = self.cmap(norm_int)[:, :3]  # RGB 0-1
+            colors = (colors * 255).astype(np.uint8)
+            rgb_int = np.left_shift(colors[:,0].astype(np.uint32), 16) | \
+                      np.left_shift(colors[:,1].astype(np.uint32), 8)  | \
+                      colors[:,2].astype(np.uint32)
+
+            filename = "pointcloud.pcd"
+            self.write_pcd_with_intensity_rgb(filename, np_points, rgb_int)
+            self.get_logger().info(f"Saved {filename} with intensity (grayscale) and colored RGB")
+            rclpy.shutdown()
+
+    def write_pcd_with_intensity_rgb(self, filename, points, rgb_int):
+        header = f"""# .PCD v0.7 - Point Cloud Data file format
+VERSION 0.7
+FIELDS x y z intensity rgb
+SIZE 4 4 4 4 4
+TYPE F F F F U
+COUNT 1 1 1 1 1
+WIDTH {points.shape[0]}
+HEIGHT 1
+VIEWPOINT 0 0 0 1 0 0 0
+POINTS {points.shape[0]}
+DATA binary
+"""
+        self.buffer.write(header.encode('ascii'))
+        for i in range(points.shape[0]):
+            # x, y, z, intensity as float32, rgb as uint32
+            f.write(struct.pack('ffffI', points[i,0], points[i,1], points[i,2], points[i,3], rgb_int[i]))
+
+def main():
+    rclpy.init()
+    with open('/root/velodyne.pcd', "w+") as f:
+        node = PointCloudSaver('velodyne_pcd_saver', '/velodyne_points', f, 5000)
+    rclpy.spin(node)
+
+if __name__ == "__main__":
+    main()
+

src/sherpa/config/fastlio2_mid40.yaml

@@ -0,0 +1,48 @@
+/**:
+    ros__parameters:
+        feature_extract_enable: false
+        point_filter_num: 1
+        max_iteration: 25
+        filter_size_surf: 0.5
+        filter_size_map: 0.5
+        cube_side_length: 1000.0
+        runtime_pos_log_enable: false
+        map_file_path: "/home/timo/Downloads/scan.pcd"
+
+        common:
+            lid_topic:  "/livox/lidar"
+            imu_topic:  "/imu"
+            time_sync_en: false         # ONLY turn on when external time synchronization is really not possible
+            time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README).
+                                        # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0
+
+        preprocess:
+            lidar_type: 0                # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 
+            scan_line: 1
+            timestamp_unit: 3            # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
+            blind: 0.35
+
+        mapping:
+            acc_cov: 0.01
+            gyr_cov: 0.025
+            b_acc_cov: 0.001
+            b_gyr_cov: 0.00025
+            fov_degree:    38.4
+            det_range:     260.0
+            extrinsic_est_en:  false      # true: enable the online estimation of IMU-LiDAR extrinsic,
+            extrinsic_T: [ 0., 0., 0.05]
+            extrinsic_R: [ 1., 0., 0., 
+                        0., 1., 0., 
+                        0., 0., -1.]
+
+        publish:
+            path_en:  true
+            map_en: true
+            scan_publish_en:  true       # false: close all the point cloud output
+            dense_publish_en: true       # false: low down the points number in a global-frame point clouds scan.
+            scan_bodyframe_pub_en: true  # true: output the point cloud scans in IMU-body-frame
+
+        pcd_save:
+            pcd_save_en: true
+            interval: -1                 # how many LiDAR frames saved in each pcd file; 
+                                        # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.

src/sherpa/launch/odometry_mid40.launch.py

@@ -0,0 +1,53 @@
+import os.path
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.substitutions import FindPackageShare
+from launch.conditions import IfCondition
+
+def generate_launch_description():
+
+    ld = LaunchDescription()   
+
+    livox_driver = Node(
+        package='livox_ros2_driver',
+        executable='livox_ros2_driver_node',
+        arguments=[]
+    )
+
+    imu = Node(
+        package = 'witmotion_ros',
+        executable = 'witmotion_ros_node',
+        parameters = [os.path.join(get_package_share_directory('sherpa'), 'config', 'wt931.yml')]
+    )
+
+    fast_lio = Node(
+        package='fast_lio',
+        executable='fastlio_mapping',
+        output='screen',
+        parameters=[PathJoinSubstitution([os.path.join(get_package_share_directory('sherpa'), 'config'), 'fastlio2_mid40.yaml'])]
+    )
+
+    tf_laserscan = Node(
+        package='tf2_ros',
+        executable='static_transform_publisher',
+        arguments=['0.0', '0', '0.0', '0', '0', '0', '1', 'body', 'frame_default']
+    )
+
+    tf_map = Node(
+        package='tf2_ros',
+        executable='static_transform_publisher',
+        arguments=['0.0', '0', '0.0', '0', '0', '0', '1', 'camera_init', 'map']
+    )
+
+
+    ld.add_action(livox_driver)
+    ld.add_action(imu)
+    ld.add_action(fast_lio)
+    ld.add_action(tf_laserscan)
+    ld.add_action(tf_map)
+
+    return ld

src/sherpa/launch/pointcloud-mid40.py

@@ -0,0 +1,23 @@
+import os.path
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.substitutions import FindPackageShare
+from launch.conditions import IfCondition
+
+def generate_launch_description():
+
+    ld = LaunchDescription()   
+
+    livox_driver = Node(
+        package='livox_ros2_driver',
+        executable='livox_ros2_driver_node',
+        arguments=[]
+    )
+
+    ld.add_action(livox_driver)
+
+    return ld

src/sherpa/launch/pointcloud-vlp16.py

@@ -0,0 +1,30 @@
+import os.path
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.substitutions import FindPackageShare
+from launch.conditions import IfCondition
+
+def generate_launch_description():
+
+    ld = LaunchDescription()   
+
+    velodyne_raw = Node(
+        package='velodyne_driver',
+        executable='velodyne_driver_node',
+        arguments=["--ros-args", "-p", "model:=VLP16", "-p", "rpm:=600.0", "-p", "device_ip:=10.42.30.200"]
+    )
+
+    velodyne_pointcloud = Node(
+        package='velodyne_pointcloud',
+        executable='velodyne_transform_node',
+        arguments=["--ros-args", "-p", f"calibration:={os.path.join(get_package_share_directory('sherpa'), 'config', 'vlp-16-pointcloud.yaml')}", "-p", "model:=VLP16"]
+    )
+
+    ld.add_action(velodyne_raw)
+    ld.add_action(velodyne_pointcloud)
+
+    return ld