"""Launch ground_plane_node alongside single_person_loc_node (headless).

Runs the keypoint triangulator in headless mode and pipes its output
into the ground-plane estimator.  Both nodes share the mmpose conda
Python environment.
"""

import os
from launch import LaunchDescription
from launch.actions import ExecuteProcess


def generate_launch_description():
    python_exe = os.path.expanduser(
        '~/miniconda3/envs/mmpose/bin/python3'
    )

    return LaunchDescription([

        # ── Keypoint triangulator (headless) ────────────────────────────
        ExecuteProcess(
            cmd=[
                python_exe, '-m', '3D_tracking_Re-ID.single_person_loc_node',
                '--ros-args',
                '-p', 'threshold:=0.3',
                '-p', 'device:=cuda:0',
                '-p', 'max_residual:=0.10',
                '-p', 'headless:=true',
            ],
            output='screen',
            env={**os.environ},
        ),

        # ── Ground-plane estimator ───────────────────────────────────────
        ExecuteProcess(
            cmd=[
                python_exe, '-m', '3D_tracking_Re-ID.ground_plane_node',
                '--ros-args',
                # Foot must stay within 15 cm for 5 consecutive frames
                '-p', 'stable_frames:=5',
                '-p', 'stable_radius:=0.15',
                # Suppress ground points closer than 12 cm to an existing one
                '-p', 'duplicate_radius:=0.12',
                # Points are collinear if none deviates > 5 cm from best-fit line
                '-p', 'collinearity_threshold:=0.05',
                '-p', 'max_ground_points:=100',
                '-p', 'min_plane_points:=3',
            ],
            output='screen',
            env={**os.environ},
        ),

    ])