94 lines
4.1 KiB
Python
94 lines
4.1 KiB
Python
from launch import LaunchDescription
|
||
from launch_ros.actions import Node
|
||
from ament_index_python.packages import get_package_share_directory
|
||
from pathlib import Path
|
||
|
||
|
||
def generate_launch_description():
|
||
pkg_share = get_package_share_directory("rslidar_pointcloud_merger")
|
||
|
||
# front_lidar（右前角） -> rslidar（车辆中心）
|
||
# [x, y, z, yaw, pitch, roll]
|
||
tf_front = [
|
||
0.9, # x 前方 0.9 m
|
||
0.67, # y 右侧 0.67 m
|
||
0.0, # z 高度 1.0 m
|
||
0.7854, # yaw π/4 ≈ 0.785 rad (水平右前 45°)
|
||
0.0, # pitch
|
||
0.0, # roll
|
||
]
|
||
|
||
# rear_lidar（左后角） -> rslidar（车辆中心）
|
||
# [x, y, z, yaw, pitch, roll]
|
||
tf_rear = [
|
||
-0.9, # x 后方 0.9 m
|
||
-0.67, # y 左侧 0.67 m
|
||
0.0, # z 高度 1.0 m
|
||
3.9270, # yaw +5π/4 ≈ +3.927rad (水平后左 135°)
|
||
0.0, # pitch
|
||
0.0, # roll
|
||
]
|
||
|
||
return LaunchDescription(
|
||
[
|
||
# ---------- 静态 TF (front) ----------
|
||
Node(
|
||
package="tf2_ros",
|
||
executable="static_transform_publisher",
|
||
name="static_tf_front",
|
||
arguments=[*map(str, tf_front), "rslidar", "front_lidar"],
|
||
# arguments=[*map(str, tf_front), "rslidar", "front_lidar", "100"],
|
||
output="log",
|
||
),
|
||
# ---------- 静态 TF (rear) ----------
|
||
Node(
|
||
package="tf2_ros",
|
||
executable="static_transform_publisher",
|
||
name="static_tf_rear",
|
||
arguments=[*map(str, tf_rear), "rslidar", "rear_lidar"],
|
||
# arguments=[*map(str, tf_rear), "rslidar", "rear_lidar", "100"],
|
||
output="log",
|
||
),
|
||
# ---------- 点云合并节点 ----------
|
||
Node(
|
||
package="rslidar_pointcloud_merger",
|
||
executable="merge_two_lidars",
|
||
name="lidar_merger",
|
||
parameters=[
|
||
{
|
||
"front_topic": "/rslidar_points/front_lidar",
|
||
"rear_topic": "/rslidar_points/rear_lidar",
|
||
"output_topic": "/rslidar_points",
|
||
"frame_id": "rslidar",
|
||
"queue_size": 3,
|
||
"cache_size": 10,
|
||
"time_tolerance": 0.1,
|
||
"max_wait_time": 1.0,
|
||
"enable_debug": False,
|
||
# 点云处理参数
|
||
"filter_min_x": -5.0, # X轴最小坐标（米）
|
||
"filter_max_x": 10.0, # X轴最大坐标（米）
|
||
"filter_min_y": -5.0, # Y轴最小坐标（米）
|
||
"filter_max_y": 5.0, # Y轴最大坐标（米）
|
||
"filter_min_z": 0.0, # Z轴最小坐标（米）
|
||
"filter_max_z": 1.0, # Z轴最大坐标（米）1.0
|
||
"voxel_size": 0.1, # 体素网格大小（米） 0.1~0.3m
|
||
"stat_mean_k": 30, # 计算点的平均距离时考虑的邻居数
|
||
"stat_std_thresh": 1.5, # 标准差倍数阈值 噪声较多 ：0.5~1.0。 噪声较少 ：1.0~2.0。
|
||
"grid_size": 50, # 栅格矩阵的边长（单元格数）
|
||
"grid_range": 15.0, # 栅格覆盖的实际空间范围（米）
|
||
# 单元格尺寸：由 grid_range / grid_size 决定
|
||
"enable_print": False, # 是否打印栅格
|
||
# 车身过滤参数
|
||
"filter_car": True, # 是否启用车身过滤
|
||
"car_length": 1.8, # 车长（米）
|
||
"car_width": 1.34, # 车宽（米）
|
||
"car_lidar_offset_x": 0.0, # LiDAR在x轴的安装偏移
|
||
"car_lidar_offset_y": 0.0, # LiDAR在y轴的安装偏移
|
||
}
|
||
],
|
||
output="screen",
|
||
),
|
||
]
|
||
)
|