Test foot to plane

3D_tracking_Re-ID/launch/full_pipeline.launch.py

@@ -0,0 +1,58 @@
+"""Launch the full 3D tracking + ground-plane estimation pipeline.
+
+Nodes started:
+  1. single_person_loc_node  -- headless 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)
+
+Visualise in RViz with Fixed Frame = 'left', then add:
+  MarkerArray  /keypoint_markers      (3D skeleton + keypoints)
+  MarkerArray  /ground_plane_markers  (plane disc, normal arrow, ground samples)
+"""
+
+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([
+
+        # ── 1. 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},
+        ),
+
+        # ── 2. Ground-plane estimator ────────────────────────────────────
+        ExecuteProcess(
+            cmd=[
+                python_exe, '-m', '3D_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},
+        ),
+
+    ])

3D_tracking_Re-ID/launch/ground_plane.launch.py

@@ -0,0 +1,53 @@
+"""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},
+        ),
+
+    ])

3D_tracking_Re-ID/launch/single_person_headless.launch.py

@@ -0,0 +1,33 @@
+"""Launch single_person_loc_node in headless mode (no display windows).
+
+Publishes 3D keypoint markers to /keypoint_markers without opening any
+OpenCV windows.  Useful for running on a server or as part of a larger
+pipeline where visualisation is handled elsewhere.
+"""
+
+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'
+    )
+
+    node_module = '3D_tracking_Re-ID.single_person_loc_node'
+
+    return LaunchDescription([
+        ExecuteProcess(
+            cmd=[
+                python_exe, '-m', node_module,
+                '--ros-args',
+                '-p', 'threshold:=0.3',
+                '-p', 'device:=cuda:0',
+                '-p', 'max_residual:=0.10',
+                '-p', 'headless:=true',
+            ],
+            output='screen',
+            env={**os.environ},
+        ),
+    ])