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",
            ),
        ]
    )