"""Launch the KeyRe-ID re-identification pipeline alongside the existing
stereo triangulation pipeline.

Nodes started
─────────────
  1. single_person_loc_node  (unchanged – stereo 3-D triangulation)
       publishes: /keypoint_markers  (MarkerArray)
                  /keypoints_3d      (PointCloud2)

  2. reid_node  (self-contained – left-camera MMPose + KeyRe-ID)
       publishes: /reid/annotated      (Image)
                  /reid/track_markers  (MarkerArray)

The two nodes are independent: reid_node runs its own MMPose instance on
the left camera only and does not depend on single_person_loc_node output.
Run them together to get both 3-D triangulation and persistent person IDs,
or launch reid_node on its own if only re-identification is needed.

Viewing the output
──────────────────
  ros2 run rqt_image_view rqt_image_view  →  /reid/annotated
  rviz2  →  add MarkerArray /reid/track_markers and /keypoint_markers
"""

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')
    keyreID_path = os.path.expanduser('~/KeyRe-ID')

    return LaunchDescription([

        # # ── 1. Stereo keypoint triangulator (3-D, unchanged) ─────────────────
        # 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. KeyRe-ID re-identification (self-contained) ───────────────────
        ExecuteProcess(
            cmd=[
                python_exe, '-m', 'tracking_re_id.reid_node',
                '--ros-args',
                '-p', f'keyreID_path:={keyreID_path}',
                '-p', 'num_classes:=150',
                '-p', 'camera_num:=2',
                '-p', 'device:=cuda:0',
                '-p', 'seq_len:=4',
                '-p', 'kp_threshold:=0.3',
                '-p', 'match_threshold:=0.65',
                '-p', 'track_dist_px:=120.0',
                '-p', 'track_timeout:=3.0',
                '-p', 'headless:=false',
            ],
            output='screen',
            env={**os.environ},
        ),

    ])