From 284ef205700b8dd8b8f63e579040df71def412b4 Mon Sep 17 00:00:00 2001 From: Timo Date: Sat, 16 Aug 2025 11:09:17 +0200 Subject: [PATCH] wip: save pcb --- scripts/save_pointcloud.py | 77 +++++++++++++++++++ src/sherpa/config/fastlio2_mid40.yaml | 48 ++++++++++++ src/sherpa/launch/odometry_mid40.launch.py | 53 +++++++++++++ ...try.launch.py => odometry_vlp16.launch.py} | 0 src/sherpa/launch/pointcloud-mid40.py | 23 ++++++ src/sherpa/launch/pointcloud-vlp16.py | 30 ++++++++ 6 files changed, 231 insertions(+) create mode 100644 scripts/save_pointcloud.py create mode 100644 src/sherpa/config/fastlio2_mid40.yaml create mode 100644 src/sherpa/launch/odometry_mid40.launch.py rename src/sherpa/launch/{odometry.launch.py => odometry_vlp16.launch.py} (100%) create mode 100755 src/sherpa/launch/pointcloud-mid40.py create mode 100755 src/sherpa/launch/pointcloud-vlp16.py diff --git a/scripts/save_pointcloud.py b/scripts/save_pointcloud.py new file mode 100644 index 0000000..c91eb8b --- /dev/null +++ b/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() + diff --git a/src/sherpa/config/fastlio2_mid40.yaml b/src/sherpa/config/fastlio2_mid40.yaml new file mode 100644 index 0000000..d61bb6f --- /dev/null +++ b/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. diff --git a/src/sherpa/launch/odometry_mid40.launch.py b/src/sherpa/launch/odometry_mid40.launch.py new file mode 100644 index 0000000..f55b3c1 --- /dev/null +++ b/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 diff --git a/src/sherpa/launch/odometry.launch.py b/src/sherpa/launch/odometry_vlp16.launch.py similarity index 100% rename from src/sherpa/launch/odometry.launch.py rename to src/sherpa/launch/odometry_vlp16.launch.py diff --git a/src/sherpa/launch/pointcloud-mid40.py b/src/sherpa/launch/pointcloud-mid40.py new file mode 100755 index 0000000..7931b8f --- /dev/null +++ b/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 diff --git a/src/sherpa/launch/pointcloud-vlp16.py b/src/sherpa/launch/pointcloud-vlp16.py new file mode 100755 index 0000000..e0d5bea --- /dev/null +++ b/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