feat(slam): add lidar SLAM and pointcloud processing packages

2026-01-13 19:12:25 +08:00
parent dcb52622eb
commit ddef31d11a
140 changed files with 29546 additions and 0 deletions
@@ -0,0 +1,43 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    return LaunchDescription([
+
+        DeclareLaunchArgument(
+            name='scanner', default_value='unilidar',
+            description='Namespace for Unitree LiDAR topics, used to organize topic names'
+        ),
+
+        # Make sure the Unitree LiDAR node is already running
+        # and publishing point cloud data to the /unilidar/cloud topic
+
+        # ========== Unitree PointCloud to LaserScan Converter ==========
+        # Convert Unitree LiDAR 3D point cloud data into 2D LaserScan data
+        Node(
+            package='pointcloud_to_laserscan', 
+            executable='pointcloud_to_laserscan_node',
+
+            remappings=[
+                ('cloud_in', '/unilidar/cloud'),  # Input: Unitree LiDAR point cloud topic
+                ('scan', '/scan')                 # Output: standard LaserScan topic
+            ],
+            parameters=[{
+                'min_height': 0.35,               # Minimum height: filter points below this height
+                'max_height': 0.45,               # Maximum height: keep obstacles, filter ceiling points
+                'angle_min': -3.14159,            # Minimum scan angle: -π radians (-180 degrees)
+                'angle_max': 3.14159,             # Maximum scan angle:  π radians (180 degrees)
+                'angle_increment': 0.00436,       # Angular resolution: π/720 radians (~0.25 degrees)
+                'scan_time': 0.1,                 # Scan time: 0.1s (10 Hz publishing rate, suitable for navigation)
+                'range_min': 0.1,                 # Minimum valid range: avoid self-reflections
+                'range_max': 100.0,               # Maximum valid range: Unitree LiDAR effective range
+                'use_inf': True,                  # Use infinity for missing readings (LaserScan standard)
+                'inf_epsilon': 1.0                # Epsilon value for infinity handling
+            }],
+            name='unitree_pointcloud_to_laserscan'  # Node name
+        )
+    ])
+
@@ -0,0 +1,47 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.actions import ExecuteProcess
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+import yaml
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            name='scanner', default_value='scanner',
+            description='Namespace for sample topics'
+        ),
+        ExecuteProcess(
+            cmd=[
+                'ros2', 'topic', 'pub', '-r', '10',
+                '--qos-profile', 'sensor_data',
+                [LaunchConfiguration(variable_name='scanner'), '/scan'],
+                'sensor_msgs/msg/LaserScan', yaml.dump({
+                    'header': {'frame_id': 'scan'}, 'angle_min': -1.0,
+                    'angle_max': 1.0, 'angle_increment': 0.1, 'range_max': 10.0,
+                    'ranges': [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]
+                })
+            ],
+            name='scan_publisher'
+        ),
+        Node(
+            package='tf2_ros',
+            executable='static_transform_publisher',
+            name='static_transform_publisher',
+            arguments=[
+                '--x', '0', '--y', '0', '--z', '0',
+                '--qx', '0', '--qy', '0', '--qz', '0', '--qw', '1',
+                '--frame-id', 'map', '--child-frame-id', 'scan'
+            ]
+        ),
+        Node(
+            package='pointcloud_to_laserscan',
+            executable='laserscan_to_pointcloud_node',
+            name='laserscan_to_pointcloud',
+            remappings=[('scan_in', [LaunchConfiguration(variable_name='scanner'), '/scan']),
+                        ('cloud', [LaunchConfiguration(variable_name='scanner'), '/cloud'])],
+            parameters=[{'target_frame': 'scan', 'transform_tolerance': 0.01}]
+        ),
+    ])
@@ -0,0 +1,49 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            name='scanner', default_value='scanner',
+            description='Namespace for sample topics'
+        ),
+        Node(
+            package='pointcloud_to_laserscan', executable='dummy_pointcloud_publisher',
+            remappings=[('cloud', [LaunchConfiguration(variable_name='scanner'), '/cloud'])],
+            parameters=[{'cloud_frame_id': 'cloud', 'cloud_extent': 2.0, 'cloud_size': 500}],
+            name='cloud_publisher'
+        ),
+        Node(
+            package='tf2_ros',
+            executable='static_transform_publisher',
+            name='static_transform_publisher',
+            arguments=[
+                '--x', '0', '--y', '0', '--z', '0',
+                '--qx', '0', '--qy', '0', '--qz', '0', '--qw', '1',
+                '--frame-id', 'map', '--child-frame-id', 'cloud'
+            ]
+        ),
+        Node(
+            package='pointcloud_to_laserscan', executable='pointcloud_to_laserscan_node',
+            remappings=[('cloud_in', [LaunchConfiguration(variable_name='scanner'), '/cloud']),
+                        ('scan', [LaunchConfiguration(variable_name='scanner'), '/scan'])],
+            parameters=[{
+                'target_frame': 'cloud',
+                'transform_tolerance': 0.01,
+                'min_height': 0.0,
+                'max_height': 1.0,
+                'angle_min': -1.5708,  # -M_PI/2
+                'angle_max': 1.5708,  # M_PI/2
+                'angle_increment': 0.0087,  # M_PI/360.0
+                'scan_time': 0.3333,
+                'range_min': 0.45,
+                'range_max': 4.0,
+                'use_inf': True,
+                'inf_epsilon': 1.0
+            }],
+            name='pointcloud_to_laserscan'
+        )
+    ])