feat(frontend): add local 3D map view

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,13 +110,19 @@ 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)
 
         # 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)
@@ -298,6 +305,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 +514,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
@@ -903,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:

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()

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<String, dynamic> 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(),
       );
 }
@@ -145,6 +147,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 +191,7 @@ class PlanningState {
   final GridInfo? gridInfo;
   final TrajPoint? navTargetPose;
   final List<TrajPoint> footprint;
+  final List<VoxelPoint> voxelPoints;
 
   const PlanningState({
     required this.localized,
@@ -196,6 +206,7 @@ class PlanningState {
     this.gridInfo,
     this.navTargetPose,
     this.footprint = const [],
+    this.voxelPoints = const [],
   });
 
   factory PlanningState.fromJson(Map<String, dynamic> j) {
@@ -238,6 +249,14 @@ class PlanningState {
         final m = p as Map<String, dynamic>;
         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<String, dynamic>;
+        return VoxelPoint(
+          (m['x'] as num).toDouble(),
+          (m['y'] as num).toDouble(),
+          (m['z'] as num).toDouble(),
+        );
+      }).toList(),
     );
   }
 }