feat: display robot footprint in local planning view

app/backend/node_manager.py

@@ -24,9 +24,9 @@
 import rclpy.time
 import tf2_ros
 from rclpy.qos import DurabilityPolicy, QoSProfile
-from geometry_msgs.msg import Twist
+from geometry_msgs.msg import Point32, Twist
 from nav_msgs.msg import OccupancyGrid, Odometry, Path
-from sensor_msgs.msg import CompressedImage, Image
+from sensor_msgs.msg import CompressedImage, Image, PointCloud
 from std_msgs.msg import Bool, Float32, String
 
 from tool.ros2_node_manager import Ros2NodeManager
@@ -83,6 +83,7 @@ def __init__(self, tinynav_db_path: str = '/tinynav/tinynav_db'):
         self._obstacle_bytes: bytes = b''
         self._trajectory: list = []
         self._global_path: list = []
+        self._footprint: list = []   # 4 corner points [{x,y},...] in world frame
         self._grid_info: dict | None = None
         self._nav_target_pose: dict | None = None
 
@@ -105,6 +106,9 @@ def __init__(self, tinynav_db_path: str = '/tinynav/tinynav_db'):
         self.create_subscription(
             Odometry, '/control/target_pose', self._on_nav_target_pose, 1
         )
+        self.create_subscription(
+            PointCloud, '/planning/footprint', self._on_footprint, 1
+        )
 
         self._tf_buffer = tf2_ros.Buffer()
         self._tf_listener = tf2_ros.TransformListener(self._tf_buffer, self)
@@ -250,6 +254,28 @@ def _on_global_plan(self, msg: Path):
         with self._lock:
             self._global_path = pts
 
+    def _on_footprint(self, msg: PointCloud):
+        """Store footprint corner points from PointCloud.
+
+        The planning node publishes 84 points (4 edges × 21 samples per edge).
+        We extract the 4 corner points (first of each edge group).
+        """
+        n = len(msg.points)
+        if n == 0:
+            return
+        # If 84 points (4 edges × 21), extract corners; otherwise store all unique points
+        if n >= 84 and n % 21 == 0:
+            edges = n // 21
+            corners = []
+            for i in range(edges):
+                p = msg.points[i * 21]
+                corners.append({'x': p.x, 'y': p.y})
+        else:
+            # Fallback: store all points
+            corners = [{'x': p.x, 'y': p.y} for p in msg.points]
+        with self._lock:
+            self._footprint = corners
+
     # ------------------------------------------------------------------ #
     # Helpers                                                              #
     # ------------------------------------------------------------------ #
@@ -441,6 +467,7 @@ def get_planning_snapshot(self) -> dict:
                 'map_global_path': path_snapshot,
                 'grid_info': self._grid_info,
                 'nav_target_pose': self._nav_target_pose,
+                'footprint': list(self._footprint),
             }
         snapshot['global_path'] = self._transform_path_via_tf(path_snapshot)
         return snapshot

app/frontend/lib/core/models.dart

@@ -157,6 +157,7 @@ class PlanningState {
   final List<TrajPoint> mapGlobalPath;
   final GridInfo? gridInfo;
   final TrajPoint? navTargetPose;
+  final List<TrajPoint> footprint;
 
   const PlanningState({
     required this.localized,
@@ -170,6 +171,7 @@ class PlanningState {
     this.mapGlobalPath = const [],
     this.gridInfo,
     this.navTargetPose,
+    this.footprint = const [],
   });
 
   factory PlanningState.fromJson(Map<String, dynamic> j) {
@@ -208,6 +210,10 @@ class PlanningState {
               (j['nav_target_pose']['y'] as num).toDouble(),
             )
           : null,
+      footprint: (j['footprint'] as List? ?? []).map((p) {
+        final m = p as Map<String, dynamic>;
+        return TrajPoint((m['x'] as num).toDouble(), (m['y'] as num).toDouble());
+      }).toList(),
     );
   }
 }

app/frontend/lib/pages/operate_tab.dart

@@ -32,6 +32,7 @@ class _OperateTabState extends ConsumerState<OperateTab> {
   bool _showEsdf = true;
   bool _showTrajectory = true;
   bool _showGlobalPath = true;
+  bool _showFootprint = true;
 
   @override
   void initState() {
@@ -106,6 +107,7 @@ class _OperateTabState extends ConsumerState<OperateTab> {
                   showEsdf: _showEsdf,
                   showTrajectory: _showTrajectory,
                   showGlobalPath: _showGlobalPath,
+                  showFootprint: _showFootprint,
                 ),
               ),
               if (planning != null)
@@ -122,11 +124,13 @@ class _OperateTabState extends ConsumerState<OperateTab> {
                   showEsdf: _showEsdf,
                   showTrajectory: _showTrajectory,
                   showGlobalPath: _showGlobalPath,
-                  onChanged: (obs, esdf, traj, gp) => setState(() {
+                  showFootprint: _showFootprint,
+                  onChanged: (obs, esdf, traj, gp, fp) => setState(() {
                     _showObstacle = obs;
                     _showEsdf = esdf;
                     _showTrajectory = traj;
                     _showGlobalPath = gp;
+                    _showFootprint = fp;
                   }),
                 ),
               ),
@@ -236,13 +240,15 @@ class _LocalPlanningView extends StatelessWidget {
   final bool showEsdf;
   final bool showTrajectory;
   final bool showGlobalPath;
+  final bool showFootprint;
 
   const _LocalPlanningView({
     this.planning,
     this.showObstacle = true,
     this.showEsdf = false,
     this.showTrajectory = false,
     this.showGlobalPath = true,
+    this.showFootprint = true,
   });
 
   @override
@@ -277,14 +283,16 @@ class _LocalPlanningView extends StatelessWidget {
                       painter: LocalPlanningPainter(
                         trajectory: p.trajectory,
                         globalPath: p.globalPath,
+                        footprint: p.footprint,
                         gridInfo: p.gridInfo,
                         odomPose: p.odomPose,
                         showTrajectory: showTrajectory,
                         showGlobalPath: showGlobalPath,
+                        showFootprint: showFootprint,
                         navTargetPose: p.navTargetPose,
                       ),
-                    )
-                  else
+                    ),
+                  if (p == null)
                     const Center(
                       child: Column(
                         mainAxisSize: MainAxisSize.min,
@@ -311,13 +319,15 @@ class _LayerTogglePanel extends StatefulWidget {
   final bool showEsdf;
   final bool showTrajectory;
   final bool showGlobalPath;
-  final void Function(bool obs, bool esdf, bool traj, bool gp) onChanged;
+  final bool showFootprint;
+  final void Function(bool obs, bool esdf, bool traj, bool gp, bool fp) onChanged;
 
   const _LayerTogglePanel({
     required this.showObstacle,
     required this.showEsdf,
     required this.showTrajectory,
     required this.showGlobalPath,
+    required this.showFootprint,
     required this.onChanged,
   });
 
@@ -368,13 +378,15 @@ class _LayerTogglePanelState extends State<_LayerTogglePanel> {
               mainAxisSize: MainAxisSize.min,
               children: [
                 _LayerRow('Obstacle', widget.showObstacle,
-                    (v) => widget.onChanged(v, widget.showEsdf, widget.showTrajectory, widget.showGlobalPath)),
+                    (v) => widget.onChanged(v, widget.showEsdf, widget.showTrajectory, widget.showGlobalPath, widget.showFootprint)),
                 _LayerRow('ESDF', widget.showEsdf,
-                    (v) => widget.onChanged(widget.showObstacle, v, widget.showTrajectory, widget.showGlobalPath)),
+                    (v) => widget.onChanged(widget.showObstacle, v, widget.showTrajectory, widget.showGlobalPath, widget.showFootprint)),
                 _LayerRow('Trajectory', widget.showTrajectory,
-                    (v) => widget.onChanged(widget.showObstacle, widget.showEsdf, v, widget.showGlobalPath)),
+                    (v) => widget.onChanged(widget.showObstacle, widget.showEsdf, v, widget.showGlobalPath, widget.showFootprint)),
                 _LayerRow('Global Path', widget.showGlobalPath,
-                    (v) => widget.onChanged(widget.showObstacle, widget.showEsdf, widget.showTrajectory, v)),
+                    (v) => widget.onChanged(widget.showObstacle, widget.showEsdf, widget.showTrajectory, v, widget.showFootprint)),
+                _LayerRow('Footprint', widget.showFootprint,
+                    (v) => widget.onChanged(widget.showObstacle, widget.showEsdf, widget.showTrajectory, widget.showGlobalPath, v)),
               ],
             ),
           ),

app/frontend/lib/pages/planning_painter.dart

@@ -10,19 +10,23 @@ import '../core/models.dart';
 class LocalPlanningPainter extends CustomPainter {
   final List<TrajPoint> trajectory;
   final List<TrajPoint> globalPath;
+  final List<TrajPoint> footprint;
   final GridInfo? gridInfo;
   final Pose? odomPose;
   final bool showTrajectory;
   final bool showGlobalPath;
+  final bool showFootprint;
   final TrajPoint? navTargetPose;
 
   const LocalPlanningPainter({
     required this.trajectory,
     this.globalPath = const [],
+    this.footprint = const [],
     this.gridInfo,
     this.odomPose,
     this.showTrajectory = true,
     this.showGlobalPath = true,
+    this.showFootprint = true,
     this.navTargetPose,
   });
 
@@ -47,6 +51,9 @@ class LocalPlanningPainter extends CustomPainter {
     if (navTargetPose != null && pose != null)
       _drawNavTarget(canvas, cx, cy, scaleX, scaleY, pose, navTargetPose!);
 
+    if (showFootprint) _drawFootprint(canvas, cx, cy, scaleX, scaleY, pose);
+
+    // Small arrow on top of everything
     _drawRobotArrow(canvas, Offset(cx, cy), pose?.yaw ?? 0.0);
   }
 
@@ -144,14 +151,47 @@ class LocalPlanningPainter extends CustomPainter {
     canvas.drawLine(Offset(px, py + r - arm), Offset(px, py + r + arm), cross);
   }
 
+  void _drawFootprint(Canvas canvas, double cx, double cy,
+      double scaleX, double scaleY, Pose? pose) {
+    if (footprint.isEmpty || pose == null) return;
+
+    final pts = <Offset>[];
+    for (int i = 0; i < footprint.length; i++) {
+      pts.add(Offset(
+        cx + (footprint[i].x - pose.x) * scaleX,
+        cy - (footprint[i].y - pose.y) * scaleY,
+      ));
+    }
+
+    final path = Path()..moveTo(pts[0].dx, pts[0].dy);
+    for (int i = 1; i < pts.length; i++) {
+      path.lineTo(pts[i].dx, pts[i].dy);
+    }
+    path.close();
+
+    // Semi-transparent fill
+    canvas.drawPath(path, Paint()..color = const Color(0xFF64B5F6).withOpacity(0.25));
+    // Bright solid border — thick enough to see at small scale
+    canvas.drawPath(path,
+        Paint()
+          ..color = const Color(0xFF29B6F6)
+          ..style = PaintingStyle.stroke
+          ..strokeWidth = 3.0
+          ..strokeJoin = StrokeJoin.miter);
+    // Corner markers
+    for (final p in pts) {
+      canvas.drawCircle(p, 4, Paint()..color = const Color(0xFF29B6F6));
+    }
+  }
+
   void _drawRobotArrow(Canvas canvas, Offset center, double yaw) {
     final cosY = math.cos(yaw);
     final sinY = math.sin(yaw);
 
-    final tip   = Offset(center.dx + cosY * 14, center.dy - sinY * 14);
-    final left  = Offset(center.dx - sinY *  6, center.dy - cosY *  6);
-    final right = Offset(center.dx + sinY *  6, center.dy + cosY *  6);
-    final base  = Offset(center.dx - cosY *  5, center.dy + sinY *  5);
+    final tip   = Offset(center.dx + cosY * 8, center.dy - sinY * 8);
+    final left  = Offset(center.dx - sinY * 3.5, center.dy - cosY * 3.5);
+    final right = Offset(center.dx + sinY * 3.5, center.dy + cosY * 3.5);
+    final base  = Offset(center.dx - cosY * 3, center.dy + sinY * 3);
 
     final path = Path()
       ..moveTo(tip.dx, tip.dy)
@@ -162,16 +202,18 @@ class LocalPlanningPainter extends CustomPainter {
 
     canvas.drawPath(path, Paint()..color = Colors.white);
     canvas.drawPath(path,
-        Paint()..color = Colors.black45..style = PaintingStyle.stroke..strokeWidth = 1.0);
+        Paint()..color = Colors.black45..style = PaintingStyle.stroke..strokeWidth = 0.8);
   }
 
   @override
   bool shouldRepaint(LocalPlanningPainter old) =>
       trajectory != old.trajectory ||
       globalPath != old.globalPath ||
+      footprint != old.footprint ||
       gridInfo != old.gridInfo ||
       odomPose != old.odomPose ||
       showTrajectory != old.showTrajectory ||
       showGlobalPath != old.showGlobalPath ||
+      showFootprint != old.showFootprint ||
       navTargetPose != old.navTargetPose;
 }