From 8f8952f38a7b49d42633b1f6a67ea86103773d74 Mon Sep 17 00:00:00 2001 From: xiaolefang-dm Date: Fri, 1 May 2026 11:48:06 +0800 Subject: [PATCH] feat: display robot footprint in local planning view - Backend: subscribe /planning/footprint PointCloud, extract corner points - Backend: include footprint in planning WS snapshot - Frontend: add footprint field to PlanningState model - Frontend: draw footprint as blue rectangle in LocalPlanningPainter - Frontend: shrink robot arrow to fit inside footprint - Frontend: add Footprint layer toggle in operate tab --- app/backend/node_manager.py | 31 +++++++++++- app/frontend/lib/core/models.dart | 6 +++ app/frontend/lib/pages/operate_tab.dart | 28 ++++++++--- app/frontend/lib/pages/planning_painter.dart | 52 ++++++++++++++++++-- 4 files changed, 102 insertions(+), 15 deletions(-) diff --git a/app/backend/node_manager.py b/app/backend/node_manager.py index aa36b650..2cc5a7bb 100644 --- a/app/backend/node_manager.py +++ b/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 diff --git a/app/frontend/lib/core/models.dart b/app/frontend/lib/core/models.dart index 0372a0a2..6248764d 100644 --- a/app/frontend/lib/core/models.dart +++ b/app/frontend/lib/core/models.dart @@ -157,6 +157,7 @@ class PlanningState { final List mapGlobalPath; final GridInfo? gridInfo; final TrajPoint? navTargetPose; + final List 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 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; + return TrajPoint((m['x'] as num).toDouble(), (m['y'] as num).toDouble()); + }).toList(), ); } } diff --git a/app/frontend/lib/pages/operate_tab.dart b/app/frontend/lib/pages/operate_tab.dart index b1e9d85c..3f3f6919 100644 --- a/app/frontend/lib/pages/operate_tab.dart +++ b/app/frontend/lib/pages/operate_tab.dart @@ -32,6 +32,7 @@ class _OperateTabState extends ConsumerState { bool _showEsdf = true; bool _showTrajectory = true; bool _showGlobalPath = true; + bool _showFootprint = true; @override void initState() { @@ -106,6 +107,7 @@ class _OperateTabState extends ConsumerState { showEsdf: _showEsdf, showTrajectory: _showTrajectory, showGlobalPath: _showGlobalPath, + showFootprint: _showFootprint, ), ), if (planning != null) @@ -122,11 +124,13 @@ class _OperateTabState extends ConsumerState { 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,6 +240,7 @@ class _LocalPlanningView extends StatelessWidget { final bool showEsdf; final bool showTrajectory; final bool showGlobalPath; + final bool showFootprint; const _LocalPlanningView({ this.planning, @@ -243,6 +248,7 @@ class _LocalPlanningView extends StatelessWidget { 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)), ], ), ), diff --git a/app/frontend/lib/pages/planning_painter.dart b/app/frontend/lib/pages/planning_painter.dart index a5b1a2c7..a9469908 100644 --- a/app/frontend/lib/pages/planning_painter.dart +++ b/app/frontend/lib/pages/planning_painter.dart @@ -10,19 +10,23 @@ import '../core/models.dart'; class LocalPlanningPainter extends CustomPainter { final List trajectory; final List globalPath; + final List 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 = []; + 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; }