"""Launch the full 3D tracking + ground-plane estimation + top-down view pipeline.

Nodes started:
  1. single_person_loc_node  -- headless stereo keypoint triangulator
       publishes: /keypoint_markers  (MarkerArray)
                  /keypoints_3d      (PointCloud2)
  2. ground_plane_node       -- ground-plane estimator
       publishes: /ground_plane_markers  (MarkerArray)
                  /ground_plane_pose     (PoseStamped)
  3. top_down_node           -- synthetic bird's-eye view of the ground plane
       publishes: /top_down_image        (Image)

To view the top-down image:
  ros2 run rqt_image_view rqt_image_view   →   select /top_down_image
"""

import os
import sys

sys.path.insert(0, os.path.dirname(__file__))
from _conda_utils import find_conda_python  # noqa: E402

from launch import LaunchDescription
from launch.actions import ExecuteProcess


def generate_launch_description():
    python_exe = find_conda_python('mmpose')

    return LaunchDescription([

        # ── 1. Keypoint triangulator (headless) ──────────────────────────────
        ExecuteProcess(
            cmd=[
                python_exe, '-m', '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},
        ),

        # ── 2. Ground-plane estimator ─────────────────────────────────────────
        ExecuteProcess(
            cmd=[
                python_exe, '-m', 'tracking_re_id.ground_plane_node',
                '--ros-args',
                '-p', 'stable_frames:=5',
                '-p', 'stable_radius:=0.05',
                '-p', 'duplicate_radius:=0',
                '-p', 'collinearity_threshold:=0.25',
                '-p', 'max_ground_points:=100',
                '-p', 'min_plane_points:=5',
            ],
            output='screen',
            env={**os.environ},
        ),

        # ── 3. Top-down bird's-eye visualiser ────────────────────────────────
        ExecuteProcess(
            cmd=[
                python_exe, '-m', 'tracking_re_id.top_down_node',
                '--ros-args',
            ],
            output='screen',
            env={**os.environ},
        ),

    ])