From 705560a069349a92aa286f354c2423339322343f Mon Sep 17 00:00:00 2001 From: Xiaole Fang Date: Sat, 16 May 2026 09:26:50 +0800 Subject: [PATCH 01/11] feat(frontend): add local map fit fill toggle --- app/frontend/lib/pages/operate_tab.dart | 115 ++++++++++++++++++------ 1 file changed, 89 insertions(+), 26 deletions(-) diff --git a/app/frontend/lib/pages/operate_tab.dart b/app/frontend/lib/pages/operate_tab.dart index 91f7cfbe..0373dd90 100644 --- a/app/frontend/lib/pages/operate_tab.dart +++ b/app/frontend/lib/pages/operate_tab.dart @@ -39,6 +39,7 @@ class _OperateTabState extends ConsumerState { bool _showGlobalMap = false; bool _navArrived = false; bool _showFootprint = true; + bool _localMapFill = false; @override void initState() { @@ -154,6 +155,7 @@ class _OperateTabState extends ConsumerState { showTrajectory: _showTrajectory, showGlobalPath: _showGlobalPath, showFootprint: _showFootprint, + fillViewport: _localMapFill, ), ), if (planning != null) @@ -178,19 +180,30 @@ class _OperateTabState extends ConsumerState { Positioned( top: 8, right: 8, - child: _LayerTogglePanel( - showObstacle: _showObstacle, - showEsdf: _showEsdf, - showTrajectory: _showTrajectory, - showGlobalPath: _showGlobalPath, - showFootprint: _showFootprint, - onChanged: (obs, esdf, traj, gp, fp) => setState(() { - _showObstacle = obs; - _showEsdf = esdf; - _showTrajectory = traj; - _showGlobalPath = gp; - _showFootprint = fp; - }), + child: Column( + crossAxisAlignment: CrossAxisAlignment.end, + mainAxisSize: MainAxisSize.min, + children: [ + _LocalMapScaleButton( + fillViewport: _localMapFill, + onTap: () => setState(() => _localMapFill = !_localMapFill), + ), + const SizedBox(height: 6), + _LayerTogglePanel( + showObstacle: _showObstacle, + showEsdf: _showEsdf, + showTrajectory: _showTrajectory, + showGlobalPath: _showGlobalPath, + showFootprint: _showFootprint, + onChanged: (obs, esdf, traj, gp, fp) => setState(() { + _showObstacle = obs; + _showEsdf = esdf; + _showTrajectory = traj; + _showGlobalPath = gp; + _showFootprint = fp; + }), + ), + ], ), ), if (isNavigating || _navArrived) @@ -306,6 +319,7 @@ class _LocalPlanningView extends StatelessWidget { final bool showTrajectory; final bool showGlobalPath; final bool showFootprint; + final bool fillViewport; const _LocalPlanningView({ this.planning, @@ -314,6 +328,7 @@ class _LocalPlanningView extends StatelessWidget { this.showTrajectory = false, this.showGlobalPath = true, this.showFootprint = true, + this.fillViewport = false, }); @override @@ -323,16 +338,21 @@ class _LocalPlanningView extends StatelessWidget { fit: StackFit.expand, children: [ Container(color: const Color(0xFF0D1117)), - Center( - child: AspectRatio( - aspectRatio: 1.0, - child: InteractiveViewer( - minScale: 0.5, - maxScale: 8.0, - boundaryMargin: const EdgeInsets.all(double.infinity), - child: Stack( - fit: StackFit.expand, - children: [ + LayoutBuilder( + builder: (context, constraints) { + final side = fillViewport + ? max(constraints.maxWidth, constraints.maxHeight) + : min(constraints.maxWidth, constraints.maxHeight); + return Center( + child: SizedBox.square( + dimension: side, + child: InteractiveViewer( + minScale: 0.5, + maxScale: 8.0, + boundaryMargin: const EdgeInsets.all(double.infinity), + child: Stack( + fit: StackFit.expand, + children: [ if (showEsdf && p?.esdfImage != null) Opacity( opacity: 0.85, @@ -369,16 +389,59 @@ class _LocalPlanningView extends StatelessWidget { ], ), ), - ], + ], + ), + ), ), - ), - ), + ); + }, ), ], ); } } +class _LocalMapScaleButton extends StatelessWidget { + final bool fillViewport; + final VoidCallback onTap; + + const _LocalMapScaleButton({required this.fillViewport, required this.onTap}); + + @override + Widget build(BuildContext context) { + return GestureDetector( + onTap: onTap, + child: Container( + padding: const EdgeInsets.symmetric(horizontal: 10, vertical: 6), + decoration: BoxDecoration( + color: Colors.black54, + borderRadius: BorderRadius.circular(20), + border: Border.all(color: Colors.white12), + ), + child: Row( + mainAxisSize: MainAxisSize.min, + children: [ + Icon( + fillViewport ? Icons.fullscreen_exit_rounded : Icons.fullscreen_rounded, + size: 15, + color: Colors.white70, + ), + const SizedBox(width: 5), + Text( + fillViewport ? 'Fill' : 'Fit', + style: const TextStyle( + color: Colors.white70, + fontSize: 11, + fontWeight: FontWeight.w600, + ), + ), + ], + ), + ), + ); + } +} + class _LayerTogglePanel extends StatefulWidget { final bool showObstacle; final bool showEsdf; From 5fa5b4d4eb2d083f7a7d546bc84fcb8daf1b3192 Mon Sep 17 00:00:00 2001 From: Xiaole Fang Date: Sat, 16 May 2026 10:19:00 +0800 Subject: [PATCH 02/11] fix(frontend): clip local map fill mode --- app/frontend/lib/pages/operate_tab.dart | 40 +++++++++++++------------ 1 file changed, 21 insertions(+), 19 deletions(-) diff --git a/app/frontend/lib/pages/operate_tab.dart b/app/frontend/lib/pages/operate_tab.dart index 0373dd90..dbef5591 100644 --- a/app/frontend/lib/pages/operate_tab.dart +++ b/app/frontend/lib/pages/operate_tab.dart @@ -338,21 +338,22 @@ class _LocalPlanningView extends StatelessWidget { fit: StackFit.expand, children: [ Container(color: const Color(0xFF0D1117)), - LayoutBuilder( - builder: (context, constraints) { - final side = fillViewport - ? max(constraints.maxWidth, constraints.maxHeight) - : min(constraints.maxWidth, constraints.maxHeight); - return Center( - child: SizedBox.square( - dimension: side, - child: InteractiveViewer( - minScale: 0.5, - maxScale: 8.0, - boundaryMargin: const EdgeInsets.all(double.infinity), - child: Stack( - fit: StackFit.expand, - children: [ + ClipRect( + child: LayoutBuilder( + builder: (context, constraints) { + final side = fillViewport + ? max(constraints.maxWidth, constraints.maxHeight) + : min(constraints.maxWidth, constraints.maxHeight); + return Center( + child: SizedBox.square( + dimension: side, + child: InteractiveViewer( + minScale: 0.5, + maxScale: 8.0, + boundaryMargin: const EdgeInsets.all(double.infinity), + child: Stack( + fit: StackFit.expand, + children: [ if (showEsdf && p?.esdfImage != null) Opacity( opacity: 0.85, @@ -389,12 +390,13 @@ class _LocalPlanningView extends StatelessWidget { ], ), ), - ], + ], + ), ), ), - ), - ); - }, + ); + }, + ), ), ], ); From 7ab3ba4c84bd7c12a40b6a1f55330cdb4f1f75bf Mon Sep 17 00:00:00 2001 From: Xiaole Fang Date: Sat, 16 May 2026 12:19:16 +0800 Subject: [PATCH 03/11] feat(frontend): add local 3d voxel view --- app/backend/node_manager.py | 24 +++++- app/frontend/lib/core/models.dart | 17 ++++ .../lib/pages/local_voxel_painter.dart | 78 +++++++++++++++++++ app/frontend/lib/pages/operate_tab.dart | 64 +++++++++++++-- 4 files changed, 176 insertions(+), 7 deletions(-) create mode 100644 app/frontend/lib/pages/local_voxel_painter.dart diff --git a/app/backend/node_manager.py b/app/backend/node_manager.py index 61d2255b..e254a789 100644 --- a/app/backend/node_manager.py +++ b/app/backend/node_manager.py @@ -26,7 +26,7 @@ from rclpy.qos import DurabilityPolicy, QoSProfile from geometry_msgs.msg import Point32, Twist from nav_msgs.msg import OccupancyGrid, Odometry, Path -from sensor_msgs.msg import CompressedImage, Image, PointCloud +from sensor_msgs.msg import CompressedImage, Image, PointCloud, PointCloud2 from std_msgs.msg import Bool, Float32, String from tool.ros2_node_manager import Ros2NodeManager @@ -84,6 +84,7 @@ def __init__(self, tinynav_db_path: str = '/tinynav/tinynav_db'): self._trajectory: list = [] self._global_path: list = [] self._footprint: list = [] # 4 corner points [{x,y},...] in world frame + self._voxel_points: list = [] self._grid_info: dict | None = None self._nav_target_pose: dict | None = None @@ -109,6 +110,9 @@ def __init__(self, tinynav_db_path: str = '/tinynav/tinynav_db'): self.create_subscription( PointCloud, '/planning/footprint', self._on_footprint, 1 ) + self.create_subscription( + PointCloud2, '/planning/occupied_voxels', self._on_occupied_voxels, 1 + ) self._tf_buffer = tf2_ros.Buffer() self._tf_listener = tf2_ros.TransformListener(self._tf_buffer, self) @@ -298,6 +302,23 @@ def _on_footprint(self, msg: PointCloud): with self._lock: self._footprint = corners + def _on_occupied_voxels(self, msg: PointCloud2): + """Store a downsampled local 3D occupied voxel cloud for the web UI.""" + try: + step = max(1, len(msg.data) // max(1, msg.point_step) // 2500) + points = [] + import sensor_msgs_py.point_cloud2 as pc2 + for i, p in enumerate(pc2.read_points(msg, field_names=('x', 'y', 'z'), skip_nans=True)): + if i % step != 0: + continue + points.append({'x': float(p[0]), 'y': float(p[1]), 'z': float(p[2])}) + if len(points) >= 2500: + break + with self._lock: + self._voxel_points = points + except Exception: + pass + # ------------------------------------------------------------------ # # Helpers # # ------------------------------------------------------------------ # @@ -490,6 +511,7 @@ def get_planning_snapshot(self) -> dict: 'grid_info': self._grid_info, 'nav_target_pose': self._nav_target_pose, 'footprint': list(self._footprint), + 'voxel_points': list(self._voxel_points), } 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 76aef824..93c9c0ea 100644 --- a/app/frontend/lib/core/models.dart +++ b/app/frontend/lib/core/models.dart @@ -145,6 +145,13 @@ class TrajPoint { const TrajPoint(this.x, this.y); } +class VoxelPoint { + final double x; + final double y; + final double z; + const VoxelPoint(this.x, this.y, this.z); +} + class GridInfo { final double originX; final double originY; @@ -182,6 +189,7 @@ class PlanningState { final GridInfo? gridInfo; final TrajPoint? navTargetPose; final List footprint; + final List voxelPoints; const PlanningState({ required this.localized, @@ -196,6 +204,7 @@ class PlanningState { this.gridInfo, this.navTargetPose, this.footprint = const [], + this.voxelPoints = const [], }); factory PlanningState.fromJson(Map j) { @@ -238,6 +247,14 @@ class PlanningState { final m = p as Map; return TrajPoint((m['x'] as num).toDouble(), (m['y'] as num).toDouble()); }).toList(), + voxelPoints: (j['voxel_points'] as List? ?? []).map((p) { + final m = p as Map; + return VoxelPoint( + (m['x'] as num).toDouble(), + (m['y'] as num).toDouble(), + (m['z'] as num).toDouble(), + ); + }).toList(), ); } } diff --git a/app/frontend/lib/pages/local_voxel_painter.dart b/app/frontend/lib/pages/local_voxel_painter.dart new file mode 100644 index 00000000..ffcebbac --- /dev/null +++ b/app/frontend/lib/pages/local_voxel_painter.dart @@ -0,0 +1,78 @@ +import 'dart:math' as math; + +import 'package:flutter/material.dart'; + +import '../core/models.dart'; + +class LocalVoxelPainter extends CustomPainter { + final List points; + final Pose? odomPose; + + const LocalVoxelPainter({required this.points, this.odomPose}); + + @override + void paint(Canvas canvas, Size size) { + final bg = Paint()..color = const Color(0xFF0F1621); + canvas.drawRect(Offset.zero & size, bg); + + final gridPaint = Paint() + ..color = Colors.white.withOpacity(0.07) + ..strokeWidth = 1; + for (var i = 1; i < 4; i++) { + final x = size.width * i / 4; + final y = size.height * i / 4; + canvas.drawLine(Offset(x, 0), Offset(x, size.height), gridPaint); + canvas.drawLine(Offset(0, y), Offset(size.width, y), gridPaint); + } + + final pose = odomPose; + if (points.isEmpty || pose == null) { + _drawEmpty(canvas, size); + return; + } + + final cosYaw = math.cos(-pose.yaw); + final sinYaw = math.sin(-pose.yaw); + const rangeM = 3.0; + final scale = math.min(size.width, size.height) / (rangeM * 2.0); + final center = Offset(size.width / 2, size.height * 0.58); + + final sorted = [...points]..sort((a, b) => a.z.compareTo(b.z)); + for (final p in sorted) { + final dx = p.x - pose.x; + final dy = p.y - pose.y; + final localX = dx * cosYaw - dy * sinYaw; + final localY = dx * sinYaw + dy * cosYaw; + if (localX.abs() > rangeM || localY.abs() > rangeM) continue; + + final sx = center.dx + localX * scale; + final sy = center.dy - localY * scale - (p.z * scale * 0.22); + final zNorm = ((p.z + 0.4) / 1.2).clamp(0.0, 1.0); + final color = Color.lerp(const Color(0xFF25D0FF), const Color(0xFFFFB020), zNorm)!; + canvas.drawCircle(Offset(sx, sy), 2.0, Paint()..color = color.withOpacity(0.82)); + } + + final robotPaint = Paint()..color = Colors.white; + final path = Path() + ..moveTo(center.dx, center.dy - 12) + ..lineTo(center.dx - 8, center.dy + 10) + ..lineTo(center.dx + 8, center.dy + 10) + ..close(); + canvas.drawPath(path, robotPaint); + } + + void _drawEmpty(Canvas canvas, Size size) { + final tp = TextPainter( + text: const TextSpan( + text: 'Waiting for 3D voxel data…', + style: TextStyle(color: Colors.white54, fontSize: 13), + ), + textDirection: TextDirection.ltr, + )..layout(maxWidth: size.width); + tp.paint(canvas, Offset((size.width - tp.width) / 2, (size.height - tp.height) / 2)); + } + + @override + bool shouldRepaint(covariant LocalVoxelPainter oldDelegate) => + oldDelegate.points != points || oldDelegate.odomPose != odomPose; +} diff --git a/app/frontend/lib/pages/operate_tab.dart b/app/frontend/lib/pages/operate_tab.dart index dbef5591..12dd2350 100644 --- a/app/frontend/lib/pages/operate_tab.dart +++ b/app/frontend/lib/pages/operate_tab.dart @@ -10,6 +10,7 @@ import 'package:web_socket_channel/web_socket_channel.dart'; import '../core/models.dart'; import '../core/providers.dart'; +import 'local_voxel_painter.dart'; import 'map_painter.dart'; import 'planning_painter.dart'; @@ -40,6 +41,7 @@ class _OperateTabState extends ConsumerState { bool _navArrived = false; bool _showFootprint = true; bool _localMapFill = false; + bool _showLocal3d = false; @override void initState() { @@ -156,6 +158,7 @@ class _OperateTabState extends ConsumerState { showGlobalPath: _showGlobalPath, showFootprint: _showFootprint, fillViewport: _localMapFill, + show3d: _showLocal3d, ), ), if (planning != null) @@ -184,9 +187,19 @@ class _OperateTabState extends ConsumerState { crossAxisAlignment: CrossAxisAlignment.end, mainAxisSize: MainAxisSize.min, children: [ - _LocalMapScaleButton( - fillViewport: _localMapFill, - onTap: () => setState(() => _localMapFill = !_localMapFill), + Row( + mainAxisSize: MainAxisSize.min, + children: [ + _LocalViewModeButton( + show3d: _showLocal3d, + onTap: () => setState(() => _showLocal3d = !_showLocal3d), + ), + const SizedBox(width: 6), + _LocalMapScaleButton( + fillViewport: _localMapFill, + onTap: () => setState(() => _localMapFill = !_localMapFill), + ), + ], ), const SizedBox(height: 6), _LayerTogglePanel( @@ -320,6 +333,7 @@ class _LocalPlanningView extends StatelessWidget { final bool showGlobalPath; final bool showFootprint; final bool fillViewport; + final bool show3d; const _LocalPlanningView({ this.planning, @@ -329,6 +343,7 @@ class _LocalPlanningView extends StatelessWidget { this.showGlobalPath = true, this.showFootprint = true, this.fillViewport = false, + this.show3d = false, }); @override @@ -351,9 +366,16 @@ class _LocalPlanningView extends StatelessWidget { minScale: 0.5, maxScale: 8.0, boundaryMargin: const EdgeInsets.all(double.infinity), - child: Stack( - fit: StackFit.expand, - children: [ + child: show3d + ? CustomPaint( + painter: LocalVoxelPainter( + points: p?.voxelPoints ?? const [], + odomPose: p?.odomPose, + ), + ) + : Stack( + fit: StackFit.expand, + children: [ if (showEsdf && p?.esdfImage != null) Opacity( opacity: 0.85, @@ -403,6 +425,36 @@ class _LocalPlanningView extends StatelessWidget { } } +class _LocalViewModeButton extends StatelessWidget { + final bool show3d; + final VoidCallback onTap; + + const _LocalViewModeButton({required this.show3d, required this.onTap}); + + @override + Widget build(BuildContext context) { + return GestureDetector( + onTap: onTap, + child: Container( + padding: const EdgeInsets.symmetric(horizontal: 10, vertical: 6), + decoration: BoxDecoration( + color: Colors.black54, + borderRadius: BorderRadius.circular(20), + border: Border.all(color: Colors.white12), + ), + child: Text( + show3d ? '3D' : '2D', + style: const TextStyle( + color: Colors.white70, + fontSize: 11, + fontWeight: FontWeight.w700, + ), + ), + ), + ); + } +} + class _LocalMapScaleButton extends StatelessWidget { final bool fillViewport; final VoidCallback onTap; From e461499bf5372d05cb3a369b983927266c16ec78 Mon Sep 17 00:00:00 2001 From: Xiaole Fang Date: Sat, 16 May 2026 12:42:33 +0800 Subject: [PATCH 04/11] fix(frontend): align local 3d view with 2d map axes --- .../lib/pages/local_voxel_painter.dart | 56 ++++++++++++------- 1 file changed, 36 insertions(+), 20 deletions(-) diff --git a/app/frontend/lib/pages/local_voxel_painter.dart b/app/frontend/lib/pages/local_voxel_painter.dart index ffcebbac..6d86f601 100644 --- a/app/frontend/lib/pages/local_voxel_painter.dart +++ b/app/frontend/lib/pages/local_voxel_painter.dart @@ -31,34 +31,24 @@ class LocalVoxelPainter extends CustomPainter { return; } - final cosYaw = math.cos(-pose.yaw); - final sinYaw = math.sin(-pose.yaw); - const rangeM = 3.0; - final scale = math.min(size.width, size.height) / (rangeM * 2.0); - final center = Offset(size.width / 2, size.height * 0.58); + const worldW = 10.0; + const worldH = 10.0; + final scaleX = size.width / worldW; + final scaleY = size.height / worldH; + final center = Offset(size.width / 2, size.height / 2); final sorted = [...points]..sort((a, b) => a.z.compareTo(b.z)); for (final p in sorted) { - final dx = p.x - pose.x; - final dy = p.y - pose.y; - final localX = dx * cosYaw - dy * sinYaw; - final localY = dx * sinYaw + dy * cosYaw; - if (localX.abs() > rangeM || localY.abs() > rangeM) continue; - - final sx = center.dx + localX * scale; - final sy = center.dy - localY * scale - (p.z * scale * 0.22); + final sx = center.dx + (p.x - pose.x) * scaleX; + final sy = center.dy - (p.y - pose.y) * scaleY - (p.z * math.min(scaleX, scaleY) * 0.08); + if (sx < -8 || sx > size.width + 8 || sy < -8 || sy > size.height + 8) continue; + final zNorm = ((p.z + 0.4) / 1.2).clamp(0.0, 1.0); final color = Color.lerp(const Color(0xFF25D0FF), const Color(0xFFFFB020), zNorm)!; canvas.drawCircle(Offset(sx, sy), 2.0, Paint()..color = color.withOpacity(0.82)); } - final robotPaint = Paint()..color = Colors.white; - final path = Path() - ..moveTo(center.dx, center.dy - 12) - ..lineTo(center.dx - 8, center.dy + 10) - ..lineTo(center.dx + 8, center.dy + 10) - ..close(); - canvas.drawPath(path, robotPaint); + _drawRobotArrow(canvas, center, pose.yaw); } void _drawEmpty(Canvas canvas, Size size) { @@ -72,6 +62,32 @@ class LocalVoxelPainter extends CustomPainter { tp.paint(canvas, Offset((size.width - tp.width) / 2, (size.height - tp.height) / 2)); } + void _drawRobotArrow(Canvas canvas, Offset center, double yaw) { + final cosY = math.cos(yaw); + final sinY = math.sin(yaw); + + 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) + ..lineTo(left.dx, left.dy) + ..lineTo(base.dx, base.dy) + ..lineTo(right.dx, right.dy) + ..close(); + + canvas.drawPath(path, Paint()..color = Colors.white); + canvas.drawPath( + path, + Paint() + ..color = Colors.black45 + ..style = PaintingStyle.stroke + ..strokeWidth = 0.8, + ); + } + @override bool shouldRepaint(covariant LocalVoxelPainter oldDelegate) => oldDelegate.points != points || oldDelegate.odomPose != odomPose; From a257ab490e20230efe934192f0b8dbd7032f1304 Mon Sep 17 00:00:00 2001 From: Xiaole Fang Date: Sat, 16 May 2026 12:49:12 +0800 Subject: [PATCH 05/11] feat(frontend): overlay planning guides on local 3d view --- .../lib/pages/local_voxel_painter.dart | 96 ++++++++++++++++++- app/frontend/lib/pages/operate_tab.dart | 4 + 2 files changed, 98 insertions(+), 2 deletions(-) diff --git a/app/frontend/lib/pages/local_voxel_painter.dart b/app/frontend/lib/pages/local_voxel_painter.dart index 6d86f601..1ff35e8e 100644 --- a/app/frontend/lib/pages/local_voxel_painter.dart +++ b/app/frontend/lib/pages/local_voxel_painter.dart @@ -6,9 +6,20 @@ import '../core/models.dart'; class LocalVoxelPainter extends CustomPainter { final List points; + final List trajectory; + final List globalPath; + final List footprint; + final TrajPoint? navTargetPose; final Pose? odomPose; - const LocalVoxelPainter({required this.points, this.odomPose}); + const LocalVoxelPainter({ + required this.points, + this.trajectory = const [], + this.globalPath = const [], + this.footprint = const [], + this.navTargetPose, + this.odomPose, + }); @override void paint(Canvas canvas, Size size) { @@ -48,6 +59,12 @@ class LocalVoxelPainter extends CustomPainter { canvas.drawCircle(Offset(sx, sy), 2.0, Paint()..color = color.withOpacity(0.82)); } + _drawPath(canvas, center, scaleX, scaleY, globalPath, const Color(0xFF69F0AE), 2.6); + _drawPath(canvas, center, scaleX, scaleY, trajectory, Colors.cyanAccent, 2.6); + _drawFootprint(canvas, center, scaleX, scaleY); + if (navTargetPose != null) { + _drawNavTarget(canvas, center, scaleX, scaleY, navTargetPose!); + } _drawRobotArrow(canvas, center, pose.yaw); } @@ -62,6 +79,76 @@ class LocalVoxelPainter extends CustomPainter { tp.paint(canvas, Offset((size.width - tp.width) / 2, (size.height - tp.height) / 2)); } + Offset _toCanvas(Offset center, double scaleX, double scaleY, TrajPoint p, Pose pose) => + Offset(center.dx + (p.x - pose.x) * scaleX, center.dy - (p.y - pose.y) * scaleY); + + void _drawPath( + Canvas canvas, + Offset center, + double scaleX, + double scaleY, + List pathPoints, + Color color, + double strokeWidth, + ) { + final pose = odomPose; + if (pose == null || pathPoints.length < 2) return; + final path = Path(); + for (var i = 0; i < pathPoints.length; i++) { + final c = _toCanvas(center, scaleX, scaleY, pathPoints[i], pose); + if (i == 0) { + path.moveTo(c.dx, c.dy); + } else { + path.lineTo(c.dx, c.dy); + } + } + canvas.drawPath( + path, + Paint() + ..color = color.withOpacity(0.9) + ..strokeWidth = strokeWidth + ..style = PaintingStyle.stroke + ..strokeCap = StrokeCap.round + ..strokeJoin = StrokeJoin.round, + ); + final end = _toCanvas(center, scaleX, scaleY, pathPoints.last, pose); + canvas.drawCircle(end, 4.5, Paint()..color = color); + } + + void _drawFootprint(Canvas canvas, Offset center, double scaleX, double scaleY) { + final pose = odomPose; + if (pose == null || footprint.length < 3) return; + final pts = footprint.map((p) => _toCanvas(center, scaleX, scaleY, p, pose)).toList(); + final path = Path()..moveTo(pts.first.dx, pts.first.dy); + for (final p in pts.skip(1)) { + path.lineTo(p.dx, p.dy); + } + path.close(); + canvas.drawPath(path, Paint()..color = const Color(0xFF64B5F6).withOpacity(0.22)); + canvas.drawPath( + path, + Paint() + ..color = const Color(0xFF29B6F6) + ..style = PaintingStyle.stroke + ..strokeWidth = 2.6, + ); + } + + void _drawNavTarget(Canvas canvas, Offset center, double scaleX, double scaleY, TrajPoint target) { + final pose = odomPose; + if (pose == null) return; + final c = _toCanvas(center, scaleX, scaleY, target, pose); + canvas.drawCircle( + c, + 8, + Paint() + ..color = const Color(0xFFFF6D00) + ..style = PaintingStyle.stroke + ..strokeWidth = 2.4, + ); + canvas.drawCircle(c, 3, Paint()..color = const Color(0xFFFF6D00)); + } + void _drawRobotArrow(Canvas canvas, Offset center, double yaw) { final cosY = math.cos(yaw); final sinY = math.sin(yaw); @@ -90,5 +177,10 @@ class LocalVoxelPainter extends CustomPainter { @override bool shouldRepaint(covariant LocalVoxelPainter oldDelegate) => - oldDelegate.points != points || oldDelegate.odomPose != odomPose; + oldDelegate.points != points || + oldDelegate.trajectory != trajectory || + oldDelegate.globalPath != globalPath || + oldDelegate.footprint != footprint || + oldDelegate.navTargetPose != navTargetPose || + oldDelegate.odomPose != odomPose; } diff --git a/app/frontend/lib/pages/operate_tab.dart b/app/frontend/lib/pages/operate_tab.dart index 12dd2350..87211ec7 100644 --- a/app/frontend/lib/pages/operate_tab.dart +++ b/app/frontend/lib/pages/operate_tab.dart @@ -370,6 +370,10 @@ class _LocalPlanningView extends StatelessWidget { ? CustomPaint( painter: LocalVoxelPainter( points: p?.voxelPoints ?? const [], + trajectory: p?.trajectory ?? const [], + globalPath: p?.globalPath ?? const [], + footprint: p?.footprint ?? const [], + navTargetPose: p?.navTargetPose, odomPose: p?.odomPose, ), ) From bfb7dd96831a876e1351efdb1ad37bc071cb2a30 Mon Sep 17 00:00:00 2001 From: Xiaole Fang Date: Sat, 16 May 2026 13:11:48 +0800 Subject: [PATCH 06/11] feat(frontend): render local 3d view with angled projection --- .../lib/pages/local_voxel_painter.dart | 146 +++++++++--------- 1 file changed, 69 insertions(+), 77 deletions(-) diff --git a/app/frontend/lib/pages/local_voxel_painter.dart b/app/frontend/lib/pages/local_voxel_painter.dart index 1ff35e8e..9aab3b40 100644 --- a/app/frontend/lib/pages/local_voxel_painter.dart +++ b/app/frontend/lib/pages/local_voxel_painter.dart @@ -23,79 +23,67 @@ class LocalVoxelPainter extends CustomPainter { @override void paint(Canvas canvas, Size size) { - final bg = Paint()..color = const Color(0xFF0F1621); - canvas.drawRect(Offset.zero & size, bg); - - final gridPaint = Paint() - ..color = Colors.white.withOpacity(0.07) - ..strokeWidth = 1; - for (var i = 1; i < 4; i++) { - final x = size.width * i / 4; - final y = size.height * i / 4; - canvas.drawLine(Offset(x, 0), Offset(x, size.height), gridPaint); - canvas.drawLine(Offset(0, y), Offset(size.width, y), gridPaint); - } + canvas.drawRect(Offset.zero & size, Paint()..color = const Color(0xFF0F1621)); final pose = odomPose; - if (points.isEmpty || pose == null) { + if (pose == null) { _drawEmpty(canvas, size); return; } - const worldW = 10.0; - const worldH = 10.0; - final scaleX = size.width / worldW; - final scaleY = size.height / worldH; - final center = Offset(size.width / 2, size.height / 2); + final scale = math.min(size.width, size.height) / 8.0; + final center = Offset(size.width / 2, size.height * 0.62); - final sorted = [...points]..sort((a, b) => a.z.compareTo(b.z)); - for (final p in sorted) { - final sx = center.dx + (p.x - pose.x) * scaleX; - final sy = center.dy - (p.y - pose.y) * scaleY - (p.z * math.min(scaleX, scaleY) * 0.08); - if (sx < -8 || sx > size.width + 8 || sy < -8 || sy > size.height + 8) continue; + _drawGroundGrid(canvas, center, scale); + final sorted = [...points] + ..sort((a, b) => (a.x + a.y + a.z).compareTo(b.x + b.y + b.z)); + for (final p in sorted) { + final c = _project3d(center, scale, p.x - pose.x, p.y - pose.y, p.z); + if (c.dx < -10 || c.dx > size.width + 10 || c.dy < -10 || c.dy > size.height + 10) continue; final zNorm = ((p.z + 0.4) / 1.2).clamp(0.0, 1.0); final color = Color.lerp(const Color(0xFF25D0FF), const Color(0xFFFFB020), zNorm)!; - canvas.drawCircle(Offset(sx, sy), 2.0, Paint()..color = color.withOpacity(0.82)); + canvas.drawCircle(c, 2.1, Paint()..color = color.withOpacity(0.82)); } - _drawPath(canvas, center, scaleX, scaleY, globalPath, const Color(0xFF69F0AE), 2.6); - _drawPath(canvas, center, scaleX, scaleY, trajectory, Colors.cyanAccent, 2.6); - _drawFootprint(canvas, center, scaleX, scaleY); - if (navTargetPose != null) { - _drawNavTarget(canvas, center, scaleX, scaleY, navTargetPose!); - } - _drawRobotArrow(canvas, center, pose.yaw); + _drawPath(canvas, center, scale, globalPath, const Color(0xFF69F0AE), 2.6); + _drawPath(canvas, center, scale, trajectory, Colors.cyanAccent, 2.6); + _drawFootprint(canvas, center, scale); + if (navTargetPose != null) _drawNavTarget(canvas, center, scale, navTargetPose!); + _drawRobotArrow(canvas, center, scale, pose.yaw); } - void _drawEmpty(Canvas canvas, Size size) { - final tp = TextPainter( - text: const TextSpan( - text: 'Waiting for 3D voxel data…', - style: TextStyle(color: Colors.white54, fontSize: 13), - ), - textDirection: TextDirection.ltr, - )..layout(maxWidth: size.width); - tp.paint(canvas, Offset((size.width - tp.width) / 2, (size.height - tp.height) / 2)); + Offset _project3d(Offset center, double scale, double dx, double dy, double z) { + // Isometric-ish projection. World +X points down-right, +Y points up-right, + // +Z lifts upward. This is intentionally not yaw-rotated, matching 2D map axes. + final sx = center.dx + (dx - dy) * scale * 0.72; + final sy = center.dy - (dx + dy) * scale * 0.36 - z * scale * 0.75; + return Offset(sx, sy); } - Offset _toCanvas(Offset center, double scaleX, double scaleY, TrajPoint p, Pose pose) => - Offset(center.dx + (p.x - pose.x) * scaleX, center.dy - (p.y - pose.y) * scaleY); - - void _drawPath( - Canvas canvas, - Offset center, - double scaleX, - double scaleY, - List pathPoints, - Color color, - double strokeWidth, - ) { + TrajPoint _rel(TrajPoint p, Pose pose) => TrajPoint(p.x - pose.x, p.y - pose.y); + + void _drawGroundGrid(Canvas canvas, Offset center, double scale) { + final paint = Paint() + ..color = Colors.white.withOpacity(0.07) + ..strokeWidth = 1; + for (var i = -4; i <= 4; i++) { + final a = _project3d(center, scale, i.toDouble(), -4, 0); + final b = _project3d(center, scale, i.toDouble(), 4, 0); + final c = _project3d(center, scale, -4, i.toDouble(), 0); + final d = _project3d(center, scale, 4, i.toDouble(), 0); + canvas.drawLine(a, b, paint); + canvas.drawLine(c, d, paint); + } + } + + void _drawPath(Canvas canvas, Offset center, double scale, List pts, Color color, double strokeWidth) { final pose = odomPose; - if (pose == null || pathPoints.length < 2) return; + if (pose == null || pts.length < 2) return; final path = Path(); - for (var i = 0; i < pathPoints.length; i++) { - final c = _toCanvas(center, scaleX, scaleY, pathPoints[i], pose); + for (var i = 0; i < pts.length; i++) { + final rp = _rel(pts[i], pose); + final c = _project3d(center, scale, rp.x, rp.y, 0.06); if (i == 0) { path.moveTo(c.dx, c.dy); } else { @@ -105,20 +93,21 @@ class LocalVoxelPainter extends CustomPainter { canvas.drawPath( path, Paint() - ..color = color.withOpacity(0.9) + ..color = color.withOpacity(0.92) ..strokeWidth = strokeWidth ..style = PaintingStyle.stroke ..strokeCap = StrokeCap.round ..strokeJoin = StrokeJoin.round, ); - final end = _toCanvas(center, scaleX, scaleY, pathPoints.last, pose); - canvas.drawCircle(end, 4.5, Paint()..color = color); } - void _drawFootprint(Canvas canvas, Offset center, double scaleX, double scaleY) { + void _drawFootprint(Canvas canvas, Offset center, double scale) { final pose = odomPose; if (pose == null || footprint.length < 3) return; - final pts = footprint.map((p) => _toCanvas(center, scaleX, scaleY, p, pose)).toList(); + final pts = footprint.map((p) { + final rp = _rel(p, pose); + return _project3d(center, scale, rp.x, rp.y, 0.08); + }).toList(); final path = Path()..moveTo(pts.first.dx, pts.first.dy); for (final p in pts.skip(1)) { path.lineTo(p.dx, p.dy); @@ -134,10 +123,11 @@ class LocalVoxelPainter extends CustomPainter { ); } - void _drawNavTarget(Canvas canvas, Offset center, double scaleX, double scaleY, TrajPoint target) { + void _drawNavTarget(Canvas canvas, Offset center, double scale, TrajPoint target) { final pose = odomPose; if (pose == null) return; - final c = _toCanvas(center, scaleX, scaleY, target, pose); + final rp = _rel(target, pose); + final c = _project3d(center, scale, rp.x, rp.y, 0.12); canvas.drawCircle( c, 8, @@ -149,30 +139,32 @@ class LocalVoxelPainter extends CustomPainter { canvas.drawCircle(c, 3, Paint()..color = const Color(0xFFFF6D00)); } - void _drawRobotArrow(Canvas canvas, Offset center, double yaw) { + void _drawRobotArrow(Canvas canvas, Offset center, double scale, double yaw) { final cosY = math.cos(yaw); final sinY = math.sin(yaw); - - 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 tip = _project3d(center, scale, cosY * 0.22, sinY * 0.22, 0.18); + final left = _project3d(center, scale, -sinY * 0.10, cosY * 0.10, 0.18); + final right = _project3d(center, scale, sinY * 0.10, -cosY * 0.10, 0.18); + final base = _project3d(center, scale, -cosY * 0.08, -sinY * 0.08, 0.18); final path = Path() ..moveTo(tip.dx, tip.dy) ..lineTo(left.dx, left.dy) ..lineTo(base.dx, base.dy) ..lineTo(right.dx, right.dy) ..close(); - canvas.drawPath(path, Paint()..color = Colors.white); - canvas.drawPath( - path, - Paint() - ..color = Colors.black45 - ..style = PaintingStyle.stroke - ..strokeWidth = 0.8, - ); + canvas.drawPath(path, Paint()..color = Colors.black45..style = PaintingStyle.stroke..strokeWidth = 0.8); + } + + void _drawEmpty(Canvas canvas, Size size) { + final tp = TextPainter( + text: const TextSpan( + text: 'Waiting for 3D voxel data…', + style: TextStyle(color: Colors.white54, fontSize: 13), + ), + textDirection: TextDirection.ltr, + )..layout(maxWidth: size.width); + tp.paint(canvas, Offset((size.width - tp.width) / 2, (size.height - tp.height) / 2)); } @override From 50cddf42f6f0a16675e42e7fa1d05f03a1f546e7 Mon Sep 17 00:00:00 2001 From: Xiaole Fang Date: Sat, 16 May 2026 13:40:46 +0800 Subject: [PATCH 07/11] style(frontend): tune local 3d voxel colors --- app/frontend/lib/pages/local_voxel_painter.dart | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/app/frontend/lib/pages/local_voxel_painter.dart b/app/frontend/lib/pages/local_voxel_painter.dart index 9aab3b40..6edbd985 100644 --- a/app/frontend/lib/pages/local_voxel_painter.dart +++ b/app/frontend/lib/pages/local_voxel_painter.dart @@ -42,8 +42,10 @@ class LocalVoxelPainter extends CustomPainter { final c = _project3d(center, scale, p.x - pose.x, p.y - pose.y, p.z); if (c.dx < -10 || c.dx > size.width + 10 || c.dy < -10 || c.dy > size.height + 10) continue; final zNorm = ((p.z + 0.4) / 1.2).clamp(0.0, 1.0); - final color = Color.lerp(const Color(0xFF25D0FF), const Color(0xFFFFB020), zNorm)!; - canvas.drawCircle(c, 2.1, Paint()..color = color.withOpacity(0.82)); + final color = zNorm < 0.55 + ? Color.lerp(const Color(0xFF163B2B), const Color(0xFF59C36A), zNorm / 0.55)! + : Color.lerp(const Color(0xFF59C36A), const Color(0xFFFFB020), (zNorm - 0.55) / 0.45)!; + canvas.drawCircle(c, 2.1, Paint()..color = color.withOpacity(0.86)); } _drawPath(canvas, center, scale, globalPath, const Color(0xFF69F0AE), 2.6); From 364b592293ce52f81f486634b555c4d89cdfe41b Mon Sep 17 00:00:00 2001 From: Xiaole Fang Date: Sat, 16 May 2026 14:10:35 +0800 Subject: [PATCH 08/11] feat(frontend): enable zoom for local 3d view --- app/frontend/lib/pages/operate_tab.dart | 21 +++++++++++++-------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/app/frontend/lib/pages/operate_tab.dart b/app/frontend/lib/pages/operate_tab.dart index 87211ec7..d9c7f0ef 100644 --- a/app/frontend/lib/pages/operate_tab.dart +++ b/app/frontend/lib/pages/operate_tab.dart @@ -367,14 +367,19 @@ class _LocalPlanningView extends StatelessWidget { maxScale: 8.0, boundaryMargin: const EdgeInsets.all(double.infinity), child: show3d - ? CustomPaint( - painter: LocalVoxelPainter( - points: p?.voxelPoints ?? const [], - trajectory: p?.trajectory ?? const [], - globalPath: p?.globalPath ?? const [], - footprint: p?.footprint ?? const [], - navTargetPose: p?.navTargetPose, - odomPose: p?.odomPose, + ? InteractiveViewer( + minScale: 0.5, + maxScale: 8.0, + boundaryMargin: const EdgeInsets.all(double.infinity), + child: CustomPaint( + painter: LocalVoxelPainter( + points: p?.voxelPoints ?? const [], + trajectory: p?.trajectory ?? const [], + globalPath: p?.globalPath ?? const [], + footprint: p?.footprint ?? const [], + navTargetPose: p?.navTargetPose, + odomPose: p?.odomPose, + ), ), ) : Stack( From a640a15c9eb957a6bc11e26a13cb9016f638945c Mon Sep 17 00:00:00 2001 From: Xiaole Fang Date: Mon, 18 May 2026 08:03:06 +0800 Subject: [PATCH 09/11] feat(frontend): rotate local 3d viewer --- .../lib/pages/local_voxel_painter.dart | 29 ++- app/frontend/lib/pages/operate_tab.dart | 231 +++++++++++++----- 2 files changed, 194 insertions(+), 66 deletions(-) diff --git a/app/frontend/lib/pages/local_voxel_painter.dart b/app/frontend/lib/pages/local_voxel_painter.dart index 6edbd985..6d10cd8c 100644 --- a/app/frontend/lib/pages/local_voxel_painter.dart +++ b/app/frontend/lib/pages/local_voxel_painter.dart @@ -11,6 +11,7 @@ class LocalVoxelPainter extends CustomPainter { final List footprint; final TrajPoint? navTargetPose; final Pose? odomPose; + final double viewYaw; const LocalVoxelPainter({ required this.points, @@ -19,6 +20,7 @@ class LocalVoxelPainter extends CustomPainter { this.footprint = const [], this.navTargetPose, this.odomPose, + this.viewYaw = 0.0, }); @override @@ -37,7 +39,7 @@ class LocalVoxelPainter extends CustomPainter { _drawGroundGrid(canvas, center, scale); final sorted = [...points] - ..sort((a, b) => (a.x + a.y + a.z).compareTo(b.x + b.y + b.z)); + ..sort((a, b) => _depth(a, pose).compareTo(_depth(b, pose))); for (final p in sorted) { final c = _project3d(center, scale, p.x - pose.x, p.y - pose.y, p.z); if (c.dx < -10 || c.dx > size.width + 10 || c.dy < -10 || c.dy > size.height + 10) continue; @@ -56,13 +58,27 @@ class LocalVoxelPainter extends CustomPainter { } Offset _project3d(Offset center, double scale, double dx, double dy, double z) { - // Isometric-ish projection. World +X points down-right, +Y points up-right, - // +Z lifts upward. This is intentionally not yaw-rotated, matching 2D map axes. - final sx = center.dx + (dx - dy) * scale * 0.72; - final sy = center.dy - (dx + dy) * scale * 0.36 - z * scale * 0.75; + // Isometric-ish projection. viewYaw rotates the world around +Z before + // projection so users can inspect the local voxel map from any side. + final cosYaw = math.cos(viewYaw); + final sinYaw = math.sin(viewYaw); + final rx = dx * cosYaw - dy * sinYaw; + final ry = dx * sinYaw + dy * cosYaw; + final sx = center.dx + (rx - ry) * scale * 0.72; + final sy = center.dy - (rx + ry) * scale * 0.36 - z * scale * 0.75; return Offset(sx, sy); } + double _depth(VoxelPoint p, Pose pose) { + final dx = p.x - pose.x; + final dy = p.y - pose.y; + final cosYaw = math.cos(viewYaw); + final sinYaw = math.sin(viewYaw); + final rx = dx * cosYaw - dy * sinYaw; + final ry = dx * sinYaw + dy * cosYaw; + return rx + ry + p.z; + } + TrajPoint _rel(TrajPoint p, Pose pose) => TrajPoint(p.x - pose.x, p.y - pose.y); void _drawGroundGrid(Canvas canvas, Offset center, double scale) { @@ -176,5 +192,6 @@ class LocalVoxelPainter extends CustomPainter { oldDelegate.globalPath != globalPath || oldDelegate.footprint != footprint || oldDelegate.navTargetPose != navTargetPose || - oldDelegate.odomPose != odomPose; + oldDelegate.odomPose != odomPose || + oldDelegate.viewYaw != viewYaw; } diff --git a/app/frontend/lib/pages/operate_tab.dart b/app/frontend/lib/pages/operate_tab.dart index d9c7f0ef..1a3ff8a4 100644 --- a/app/frontend/lib/pages/operate_tab.dart +++ b/app/frontend/lib/pages/operate_tab.dart @@ -5,6 +5,8 @@ import 'dart:typed_data'; import 'package:dio/dio.dart'; import 'package:flutter/material.dart'; +import 'package:flutter/gestures.dart'; +import 'package:flutter/services.dart'; import 'package:flutter_riverpod/flutter_riverpod.dart'; import 'package:web_socket_channel/web_socket_channel.dart'; @@ -362,68 +364,54 @@ class _LocalPlanningView extends StatelessWidget { return Center( child: SizedBox.square( dimension: side, - child: InteractiveViewer( - minScale: 0.5, - maxScale: 8.0, - boundaryMargin: const EdgeInsets.all(double.infinity), - child: show3d - ? InteractiveViewer( - minScale: 0.5, - maxScale: 8.0, - boundaryMargin: const EdgeInsets.all(double.infinity), - child: CustomPaint( - painter: LocalVoxelPainter( - points: p?.voxelPoints ?? const [], - trajectory: p?.trajectory ?? const [], - globalPath: p?.globalPath ?? const [], - footprint: p?.footprint ?? const [], - navTargetPose: p?.navTargetPose, - odomPose: p?.odomPose, - ), - ), - ) - : Stack( + child: show3d + ? _Local3dPlanningView(planning: p) + : InteractiveViewer( + minScale: 0.5, + maxScale: 8.0, + boundaryMargin: const EdgeInsets.all(double.infinity), + child: Stack( fit: StackFit.expand, children: [ - if (showEsdf && p?.esdfImage != null) - Opacity( - opacity: 0.85, - child: Image.memory(p!.esdfImage!, fit: BoxFit.fill, gaplessPlayback: true), - ), - if (showObstacle && p?.obstacleImage != null) - Opacity( - opacity: 0.45, - child: Image.memory(p!.obstacleImage!, fit: BoxFit.fill, gaplessPlayback: true), - ), - if (p != null) - CustomPaint( - 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, - ), - ), - if (p == null) - const Center( - child: Column( - mainAxisSize: MainAxisSize.min, - children: [ - Icon(Icons.map_outlined, size: 48, color: Colors.white24), - SizedBox(height: 8), - Text('Waiting for planning data…', - style: TextStyle(color: Colors.white38, fontSize: 13)), - ], - ), - ), - ], - ), - ), + if (showEsdf && p?.esdfImage != null) + Opacity( + opacity: 0.85, + child: Image.memory(p!.esdfImage!, fit: BoxFit.fill, gaplessPlayback: true), + ), + if (showObstacle && p?.obstacleImage != null) + Opacity( + opacity: 0.45, + child: Image.memory(p!.obstacleImage!, fit: BoxFit.fill, gaplessPlayback: true), + ), + if (p != null) + CustomPaint( + 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, + ), + ), + if (p == null) + const Center( + child: Column( + mainAxisSize: MainAxisSize.min, + children: [ + Icon(Icons.map_outlined, size: 48, color: Colors.white24), + SizedBox(height: 8), + Text('Waiting for planning data…', + style: TextStyle(color: Colors.white38, fontSize: 13)), + ], + ), + ), + ], + ), + ), ), ); }, @@ -434,6 +422,129 @@ class _LocalPlanningView extends StatelessWidget { } } +class _Local3dPlanningView extends StatefulWidget { + final PlanningState? planning; + + const _Local3dPlanningView({this.planning}); + + @override + State<_Local3dPlanningView> createState() => _Local3dPlanningViewState(); +} + +class _Local3dPlanningViewState extends State<_Local3dPlanningView> { + static const double _minScale = 0.5; + static const double _maxScale = 8.0; + + double _scale = 1.0; + double _viewYaw = 0.0; + Offset _pan = Offset.zero; + + double _startScale = 1.0; + double _startYaw = 0.0; + Offset _startPan = Offset.zero; + Offset _startFocalPoint = Offset.zero; + + void _onScaleStart(ScaleStartDetails details) { + _startScale = _scale; + _startYaw = _viewYaw; + _startPan = _pan; + _startFocalPoint = details.focalPoint; + } + + bool get _isControlPressed { + final keys = HardwareKeyboard.instance.logicalKeysPressed; + return keys.contains(LogicalKeyboardKey.controlLeft) || + keys.contains(LogicalKeyboardKey.controlRight); + } + + void _onScaleUpdate(ScaleUpdateDetails details) { + final ctrlRotate = _isControlPressed && details.pointerCount <= 1; + setState(() { + if (ctrlRotate) { + _viewYaw = _startYaw + (details.focalPoint.dx - _startFocalPoint.dx) * 0.012; + return; + } + + _scale = (_startScale * details.scale).clamp(_minScale, _maxScale).toDouble(); + _pan = _startPan + details.focalPoint - _startFocalPoint; + if (details.pointerCount >= 2) { + _viewYaw = _startYaw + details.rotation; + } + }); + } + + void _onPointerSignal(PointerSignalEvent event) { + if (event is! PointerScrollEvent) return; + final zoom = event.scrollDelta.dy < 0 ? 1.10 : 0.90; + setState(() => _scale = (_scale * zoom).clamp(_minScale, _maxScale).toDouble()); + } + + void _resetView() { + setState(() { + _scale = 1.0; + _viewYaw = 0.0; + _pan = Offset.zero; + }); + } + + @override + Widget build(BuildContext context) { + final p = widget.planning; + return Listener( + onPointerSignal: _onPointerSignal, + child: GestureDetector( + behavior: HitTestBehavior.opaque, + onDoubleTap: _resetView, + onScaleStart: _onScaleStart, + onScaleUpdate: _onScaleUpdate, + child: Stack( + fit: StackFit.expand, + children: [ + Transform.translate( + offset: _pan, + child: Transform.scale( + scale: _scale, + alignment: Alignment.center, + child: CustomPaint( + painter: LocalVoxelPainter( + points: p?.voxelPoints ?? const [], + trajectory: p?.trajectory ?? const [], + globalPath: p?.globalPath ?? const [], + footprint: p?.footprint ?? const [], + navTargetPose: p?.navTargetPose, + odomPose: p?.odomPose, + viewYaw: _viewYaw, + ), + ), + ), + ), + Positioned( + left: 8, + bottom: 8, + child: IgnorePointer( + child: DecoratedBox( + decoration: BoxDecoration( + color: Colors.black45, + borderRadius: BorderRadius.circular(10), + border: Border.all(color: Colors.white12), + ), + child: const Padding( + padding: EdgeInsets.symmetric(horizontal: 8, vertical: 5), + child: Text( + 'Pinch rotate · Ctrl+drag rotate · double tap reset', + style: TextStyle(color: Colors.white54, fontSize: 10), + ), + ), + ), + ), + ), + ], + ), + ), + ); + } +} + class _LocalViewModeButton extends StatelessWidget { final bool show3d; final VoidCallback onTap; From 8e926a3f4e0106899074cc5772399b51dce2efae Mon Sep 17 00:00:00 2001 From: Xiaole Fang Date: Mon, 18 May 2026 08:33:05 +0800 Subject: [PATCH 10/11] feat(frontend): increase 3d voxel height contrast --- .../lib/pages/local_voxel_painter.dart | 37 ++++++++++++++++--- 1 file changed, 32 insertions(+), 5 deletions(-) diff --git a/app/frontend/lib/pages/local_voxel_painter.dart b/app/frontend/lib/pages/local_voxel_painter.dart index 6d10cd8c..3d8e5930 100644 --- a/app/frontend/lib/pages/local_voxel_painter.dart +++ b/app/frontend/lib/pages/local_voxel_painter.dart @@ -38,16 +38,14 @@ class LocalVoxelPainter extends CustomPainter { _drawGroundGrid(canvas, center, scale); + final zRange = _zRange(points); final sorted = [...points] ..sort((a, b) => _depth(a, pose).compareTo(_depth(b, pose))); for (final p in sorted) { final c = _project3d(center, scale, p.x - pose.x, p.y - pose.y, p.z); if (c.dx < -10 || c.dx > size.width + 10 || c.dy < -10 || c.dy > size.height + 10) continue; - final zNorm = ((p.z + 0.4) / 1.2).clamp(0.0, 1.0); - final color = zNorm < 0.55 - ? Color.lerp(const Color(0xFF163B2B), const Color(0xFF59C36A), zNorm / 0.55)! - : Color.lerp(const Color(0xFF59C36A), const Color(0xFFFFB020), (zNorm - 0.55) / 0.45)!; - canvas.drawCircle(c, 2.1, Paint()..color = color.withOpacity(0.86)); + final color = _heightColor(_zNorm(p.z, zRange.$1, zRange.$2)); + canvas.drawCircle(c, 2.25, Paint()..color = color.withOpacity(0.92)); } _drawPath(canvas, center, scale, globalPath, const Color(0xFF69F0AE), 2.6); @@ -79,6 +77,35 @@ class LocalVoxelPainter extends CustomPainter { return rx + ry + p.z; } + (double, double) _zRange(List pts) { + if (pts.isEmpty) return (-0.4, 0.8); + final zs = pts.map((p) => p.z).toList()..sort(); + final loIdx = (zs.length * 0.05).floor().clamp(0, zs.length - 1).toInt(); + final hiIdx = (zs.length * 0.95).floor().clamp(0, zs.length - 1).toInt(); + final lo = zs[loIdx]; + final hi = zs[hiIdx]; + final mid = (lo + hi) * 0.5; + final span = math.max(hi - lo, 0.6); + return (mid - span * 0.5, mid + span * 0.5); + } + + double _zNorm(double z, double zMin, double zMax) => + ((z - zMin) / math.max(zMax - zMin, 0.001)).clamp(0.0, 1.0).toDouble(); + + Color _heightColor(double t) { + const stops = [ + Color(0xFF064E3B), // low: dark green + Color(0xFF22C55E), // lower-mid: vivid green + Color(0xFFFACC15), // mid: yellow + Color(0xFFF97316), // high: orange + Color(0xFFEF4444), // highest: red + ]; + final scaled = (t * (stops.length - 1)).clamp(0.0, stops.length - 1.0); + final i = scaled.floor().clamp(0, stops.length - 2).toInt(); + final localT = scaled - i; + return Color.lerp(stops[i], stops[i + 1], localT)!; + } + TrajPoint _rel(TrajPoint p, Pose pose) => TrajPoint(p.x - pose.x, p.y - pose.y); void _drawGroundGrid(Canvas canvas, Offset center, double scale) { From 309b95949acdadb74d78a23d5e12da2a31cb5ce7 Mon Sep 17 00:00:00 2001 From: Xiaole Fang Date: Mon, 18 May 2026 09:23:58 +0800 Subject: [PATCH 11/11] feat(frontend): add manual local target tool --- app/backend/node_manager.py | 20 ++ app/backend/routers/nav.py | 15 ++ app/frontend/lib/core/models.dart | 4 +- app/frontend/lib/pages/operate_tab.dart | 266 +++++++++++++++++++----- 4 files changed, 253 insertions(+), 52 deletions(-) diff --git a/app/backend/node_manager.py b/app/backend/node_manager.py index e254a789..23974642 100644 --- a/app/backend/node_manager.py +++ b/app/backend/node_manager.py @@ -120,6 +120,9 @@ def __init__(self, tinynav_db_path: str = '/tinynav/tinynav_db'): # Publisher for POI nav target consumed by map_node via /mapping/cmd_pois self._cmd_pois_pub = self.create_publisher(String, '/mapping/cmd_pois', 10) + # Manual local target for planning_node, used by the operate tab long-press tool. + self._target_pose_pub = self.create_publisher(Odometry, '/control/target_pose', 10) + # Latched publisher — new subscribers (cmd_vel_control) get current state immediately on connect _latched_qos = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL) self._pause_pub = self.create_publisher(Bool, '/nav/paused', _latched_qos) @@ -925,6 +928,23 @@ def _publish_cmd_pois(self, poi_id: int | None): payload = {'0': pois[key]} self._cmd_pois_pub.publish(String(data=json.dumps(payload))) + def cmd_manual_target_pose(self, x: float, y: float, z: float): + """Publish a manually selected local-planner target pose. + + planning_node subscribes to /control/target_pose and only reads the + position vector, so Odometry is used here to match that existing API. + """ + msg = Odometry() + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = 'odom' + msg.pose.pose.position.x = float(x) + msg.pose.pose.position.y = float(y) + msg.pose.pose.position.z = float(z) + msg.pose.pose.orientation.w = 1.0 + self._target_pose_pub.publish(msg) + with self._lock: + self._nav_target_pose = {'x': float(x), 'y': float(y)} + def cmd_send_pois(self, poi_ids: list[int]): """Publish selected POIs to map_node and transition to navigation state.""" if not poi_ids: diff --git a/app/backend/routers/nav.py b/app/backend/routers/nav.py index ce1c1188..62aa49c6 100644 --- a/app/backend/routers/nav.py +++ b/app/backend/routers/nav.py @@ -20,6 +20,12 @@ class SendPoisRequest(BaseModel): poi_ids: list[int] +class ManualTargetRequest(BaseModel): + x: float + y: float + z: float + + @router.post('/send-pois') def nav_send_pois(req: SendPoisRequest): node = _require_node() @@ -40,6 +46,15 @@ def nav_go_to_poi(req: GoToPoiRequest): return {'ok': True, 'poi_id': req.poi_id} +@router.post('/manual-target') +def nav_manual_target(req: ManualTargetRequest): + node = _require_node() + if node._odom_pose is None: + raise HTTPException(409, 'Odometry not ready') + node.cmd_manual_target_pose(req.x, req.y, req.z) + return {'ok': True, 'target': {'x': req.x, 'y': req.y, 'z': req.z}} + + @router.post('/cancel') def nav_cancel(): node = _require_node() diff --git a/app/frontend/lib/core/models.dart b/app/frontend/lib/core/models.dart index 93c9c0ea..9bbd7a0e 100644 --- a/app/frontend/lib/core/models.dart +++ b/app/frontend/lib/core/models.dart @@ -68,14 +68,16 @@ class Pose { final double x; final double y; final double yaw; + final double? z; final double? timestamp; - const Pose({required this.x, required this.y, required this.yaw, this.timestamp}); + const Pose({required this.x, required this.y, required this.yaw, this.z, this.timestamp}); factory Pose.fromJson(Map json) => Pose( x: (json['x'] as num).toDouble(), y: (json['y'] as num).toDouble(), yaw: (json['yaw'] as num).toDouble(), + z: (json['z'] as num?)?.toDouble(), timestamp: (json['timestamp'] as num?)?.toDouble(), ); } diff --git a/app/frontend/lib/pages/operate_tab.dart b/app/frontend/lib/pages/operate_tab.dart index 1a3ff8a4..9d939f39 100644 --- a/app/frontend/lib/pages/operate_tab.dart +++ b/app/frontend/lib/pages/operate_tab.dart @@ -327,7 +327,7 @@ class _GlobalMapView extends StatelessWidget { // ── Local planning view ─────────────────────────────────────────────────────── -class _LocalPlanningView extends StatelessWidget { +class _LocalPlanningView extends ConsumerStatefulWidget { final PlanningState? planning; final bool showObstacle; final bool showEsdf; @@ -348,9 +348,157 @@ class _LocalPlanningView extends StatelessWidget { this.show3d = false, }); + @override + ConsumerState<_LocalPlanningView> createState() => _LocalPlanningViewState(); +} + +class _ManualTarget { + final double x; + final double y; + final double z; + final bool usedVoxelZ; + + const _ManualTarget({ + required this.x, + required this.y, + required this.z, + required this.usedVoxelZ, + }); +} + +class _LocalPlanningViewState extends ConsumerState<_LocalPlanningView> { + final TransformationController _txCtrl = TransformationController(); + _ManualTarget? _pendingTarget; + Timer? _manualTargetTimer; + Offset? _manualTargetStart; + + @override + void dispose() { + _manualTargetTimer?.cancel(); + _txCtrl.dispose(); + super.dispose(); + } + + _ManualTarget? _targetFromLocalPosition(Offset viewportPos, Size viewportSize) { + final p = widget.planning; + final pose = p?.odomPose; + if (p == null || pose == null || viewportSize.width <= 0 || viewportSize.height <= 0) { + return null; + } + + final childPos = MatrixUtils.transformPoint( + Matrix4.inverted(_txCtrl.value), + viewportPos, + ); + final gi = p.gridInfo; + final worldW = gi != null ? gi.width * gi.resolution : 10.0; + final worldH = gi != null ? gi.height * gi.resolution : 10.0; + final dx = (childPos.dx - viewportSize.width / 2) * worldW / viewportSize.width; + final dy = (viewportSize.height / 2 - childPos.dy) * worldH / viewportSize.height; + final x = pose.x + dx; + final y = pose.y + dy; + final zHit = _nearbyVoxelMedianZ(p.voxelPoints, x, y); + return _ManualTarget( + x: x, + y: y, + z: zHit ?? pose.z ?? 0.0, + usedVoxelZ: zHit != null, + ); + } + + double? _nearbyVoxelMedianZ(List voxels, double x, double y) { + const radius = 0.35; + final zs = []; + for (final v in voxels) { + final dx = v.x - x; + final dy = v.y - y; + if (dx * dx + dy * dy <= radius * radius) zs.add(v.z); + } + if (zs.isEmpty) return null; + zs.sort(); + return zs[zs.length ~/ 2]; + } + + void _startManualTargetTimer(PointerDownEvent event, Size viewportSize) { + _manualTargetTimer?.cancel(); + _manualTargetStart = event.localPosition; + _manualTargetTimer = Timer(const Duration(seconds: 2), () { + _manualTargetTimer = null; + final start = _manualTargetStart; + if (start != null) _handleLongPress(start, viewportSize); + }); + } + + void _maybeCancelManualTargetTimer(PointerMoveEvent event) { + final start = _manualTargetStart; + if (start == null) return; + if ((event.localPosition - start).distance > 10) { + _cancelManualTargetTimer(); + } + } + + void _cancelManualTargetTimer() { + _manualTargetTimer?.cancel(); + _manualTargetTimer = null; + _manualTargetStart = null; + } + + Future _handleLongPress(Offset localPos, Size viewportSize) async { + final target = _targetFromLocalPosition(localPos, viewportSize); + if (target == null || !mounted) return; + setState(() => _pendingTarget = target); + + final confirmed = await showDialog( + context: context, + builder: (context) => AlertDialog( + title: const Text('Set manual target?'), + content: Text( + 'Publish /control/target_pose to:\n' + 'x=${target.x.toStringAsFixed(2)}, ' + 'y=${target.y.toStringAsFixed(2)}, ' + 'z=${target.z.toStringAsFixed(2)}\n\n' + '${target.usedVoxelZ ? 'z from nearby occupied voxels.' : 'No nearby voxel height; z uses current robot height.'}', + ), + actions: [ + TextButton( + onPressed: () => Navigator.of(context).pop(false), + child: const Text('Cancel'), + ), + FilledButton( + onPressed: () => Navigator.of(context).pop(true), + child: const Text('Publish'), + ), + ], + ), + ); + + if (!mounted) return; + if (confirmed == true) { + try { + await ref.read(dioProvider).post('/nav/manual-target', data: { + 'x': target.x, + 'y': target.y, + 'z': target.z, + }); + if (mounted) { + ScaffoldMessenger.of(context).showSnackBar( + const SnackBar(content: Text('Manual target published')), + ); + } + } catch (e) { + if (mounted) { + ScaffoldMessenger.of(context).showSnackBar( + SnackBar(content: Text('Failed to publish target: $e')), + ); + } + } + } + if (mounted) setState(() => _pendingTarget = null); + } + @override Widget build(BuildContext context) { - final p = planning; + final p = widget.planning; return Stack( fit: StackFit.expand, children: [ @@ -358,60 +506,76 @@ class _LocalPlanningView extends StatelessWidget { ClipRect( child: LayoutBuilder( builder: (context, constraints) { - final side = fillViewport + final side = widget.fillViewport ? max(constraints.maxWidth, constraints.maxHeight) : min(constraints.maxWidth, constraints.maxHeight); + final targetPose = _pendingTarget != null + ? TrajPoint(_pendingTarget!.x, _pendingTarget!.y) + : p?.navTargetPose; return Center( child: SizedBox.square( dimension: side, - child: show3d - ? _Local3dPlanningView(planning: p) - : InteractiveViewer( - minScale: 0.5, - maxScale: 8.0, - boundaryMargin: const EdgeInsets.all(double.infinity), - child: Stack( - fit: StackFit.expand, - children: [ - if (showEsdf && p?.esdfImage != null) - Opacity( - opacity: 0.85, - child: Image.memory(p!.esdfImage!, fit: BoxFit.fill, gaplessPlayback: true), - ), - if (showObstacle && p?.obstacleImage != null) - Opacity( - opacity: 0.45, - child: Image.memory(p!.obstacleImage!, fit: BoxFit.fill, gaplessPlayback: true), - ), - if (p != null) - CustomPaint( - 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, - ), - ), - if (p == null) - const Center( - child: Column( - mainAxisSize: MainAxisSize.min, - children: [ - Icon(Icons.map_outlined, size: 48, color: Colors.white24), - SizedBox(height: 8), - Text('Waiting for planning data…', - style: TextStyle(color: Colors.white38, fontSize: 13)), - ], - ), - ), - ], - ), - ), + child: Builder( + builder: (mapContext) { + final content = widget.show3d + ? _Local3dPlanningView(planning: p) + : InteractiveViewer( + transformationController: _txCtrl, + minScale: 0.5, + maxScale: 8.0, + boundaryMargin: const EdgeInsets.all(double.infinity), + child: Stack( + fit: StackFit.expand, + children: [ + if (widget.showEsdf && p?.esdfImage != null) + Opacity( + opacity: 0.85, + child: Image.memory(p!.esdfImage!, fit: BoxFit.fill, gaplessPlayback: true), + ), + if (widget.showObstacle && p?.obstacleImage != null) + Opacity( + opacity: 0.45, + child: Image.memory(p!.obstacleImage!, fit: BoxFit.fill, gaplessPlayback: true), + ), + if (p != null) + CustomPaint( + painter: LocalPlanningPainter( + trajectory: p.trajectory, + globalPath: p.globalPath, + footprint: p.footprint, + gridInfo: p.gridInfo, + odomPose: p.odomPose, + showTrajectory: widget.showTrajectory, + showGlobalPath: widget.showGlobalPath, + showFootprint: widget.showFootprint, + navTargetPose: targetPose, + ), + ), + if (p == null) + const Center( + child: Column( + mainAxisSize: MainAxisSize.min, + children: [ + Icon(Icons.map_outlined, size: 48, color: Colors.white24), + SizedBox(height: 8), + Text('Waiting for planning data…', + style: TextStyle(color: Colors.white38, fontSize: 13)), + ], + ), + ), + ], + ), + ); + return Listener( + behavior: HitTestBehavior.translucent, + onPointerDown: (event) => _startManualTargetTimer(event, Size(side, side)), + onPointerMove: _maybeCancelManualTargetTimer, + onPointerUp: (_) => _cancelManualTargetTimer(), + onPointerCancel: (_) => _cancelManualTargetTimer(), + child: content, + ); + }, + ), ), ); },