From 72aa4087b5fda21ac9acaee2d6216bec81b7e469 Mon Sep 17 00:00:00 2001 From: Xiaole Fang Date: Sat, 4 Jul 2026 07:20:07 +0800 Subject: [PATCH] feat(map): create POIs from bag marks --- app/backend/node_manager.py | 133 +++++++++++++++++++++++++++- app/backend/routers/bag.py | 22 +++++ app/frontend/lib/core/models.dart | 3 + app/frontend/lib/pages/map_tab.dart | 69 +++++++++++++++ tests/test_backend.py | 3 + 5 files changed, 226 insertions(+), 4 deletions(-) diff --git a/app/backend/node_manager.py b/app/backend/node_manager.py index 777d3468..869a9e12 100644 --- a/app/backend/node_manager.py +++ b/app/backend/node_manager.py @@ -64,6 +64,7 @@ 'default': (_PREVIEW_MAX_EDGE_PX, _PREVIEW_JPEG_QUALITY), 'high': (_PREVIEW_HIGH_MAX_EDGE_PX, _PREVIEW_HIGH_JPEG_QUALITY), } +_POI_MARKS_FILE = 'poi_marks.json' def _resize_preview_frame(arr: np.ndarray, max_edge_px: int = _PREVIEW_MAX_EDGE_PX) -> np.ndarray: @@ -120,8 +121,15 @@ def __init__(self, tinynav_db_path: str = '/tinynav/tinynav_db'): self._voxel_points: list = [] self._grid_info: dict | None = None self._nav_target_pose: dict | None = None + self._last_mapping_image_stamp_ns: int | None = None self.create_subscription(Float32, '/mapping/percent', self._on_mapping_percent, 10) + self.create_subscription( + Image, + '/camera/camera/infra1/image_rect_raw', + self._on_mapping_image_stamp, + 10, + ) self.create_subscription(Odometry, '/slam/odometry_visual', self._on_slam_odom, 10) self.create_subscription( Odometry, '/mapping/current_pose_in_map', self._on_pose_in_map, 10 @@ -235,6 +243,11 @@ def _on_mapping_percent(self, msg: Float32): with self._lock: self.mapping_percent = float(msg.data) + def _on_mapping_image_stamp(self, msg: Image): + timestamp_ns = int(msg.header.stamp.sec * 1_000_000_000) + int(msg.header.stamp.nanosec) + with self._lock: + self._last_mapping_image_stamp_ns = timestamp_ns + def _on_slam_odom(self, msg: Odometry): pose = self._odom_to_dict(msg, source='slam') with self._lock: @@ -608,6 +621,50 @@ def get_preview_profile(self, quality: str) -> tuple[int, int] | None: # Command API (called from FastAPI handlers — thread-safe enough) # # ------------------------------------------------------------------ # + def _poi_marks_path(self, bag_path: str | None = None) -> str: + return os.path.join(bag_path or self.bag_path, _POI_MARKS_FILE) + + def _load_poi_marks(self, bag_path: str | None = None) -> list[dict]: + path = self._poi_marks_path(bag_path) + if not os.path.exists(path): + return [] + with open(path) as f: + data = json.load(f) + return data if isinstance(data, list) else [] + + def _save_poi_marks(self, marks: list[dict], bag_path: str | None = None): + path = self._poi_marks_path(bag_path) + os.makedirs(os.path.dirname(path), exist_ok=True) + with open(path, 'w') as f: + json.dump(marks, f, indent=2) + + def record_poi_mark(self, name: str, timestamp_ns: int | None = None) -> dict: + clean_name = name.strip() + if not clean_name: + raise ValueError('POI name is required') + if timestamp_ns is None: + with self._lock: + timestamp_ns = self._last_mapping_image_stamp_ns + if timestamp_ns is None: + raise ValueError('No camera timestamp available') + if not os.path.isdir(self.bag_path): + raise ValueError('Bag directory is not ready yet') + + marks = self._load_poi_marks() + mark = { + 'id': len(marks), + 'name': clean_name, + 'timestamp_ns': int(timestamp_ns), + 'created_at': time.time(), + } + marks.append(mark) + self._save_poi_marks(marks) + self.get_logger().info(f'Recorded POI mark {clean_name} at {timestamp_ns}') + return mark + + def get_poi_mark_count(self, bag_path: str | None = None) -> int: + return len(self._load_poi_marks(bag_path)) + def set_active_bag(self, bag_name: str): """Select a bag from rosbags/ by name for map building.""" path = os.path.join(self.tinynav_db_path, 'rosbags', bag_name) @@ -637,6 +694,7 @@ def get_status(self) -> dict: 'battery': battery, 'bagStatus': 'recording' if raw == 'realsense_bag_record' else 'idle', 'bagFileReady': bag_files_exist, + 'poiMarkCount': self.get_poi_mark_count(), 'mapStatus': self._derive_map_status(raw, pct, map_files_exist), 'mappingPercent': pct, 'navStatus': 'navigating' if raw == 'navigation' else 'idle', @@ -945,6 +1003,7 @@ def _on_build_map_done(self): """Wait for build_map to finish, then convert, archive, and restart.""" import shutil from datetime import datetime + active_bag = self.active_bag_path proc_build = self.processes.get('build_map') if proc_build: proc_build.wait() @@ -961,10 +1020,10 @@ def _on_build_map_done(self): shutil.move(self.map_path, dest) os.symlink(dest, self.map_path) - # Auto-create a home POI at the SLAM origin (0,0,0) if none exist. - # map_node requires at least one POI as a global localization anchor. - pois_path = os.path.join(dest, 'pois.json') - if not os.path.exists(pois_path): + if not self._generate_pois_from_marks(active_bag, dest): + # Auto-create a home POI at the SLAM origin (0,0,0) if none exist. + # map_node requires at least one POI as a global localization anchor. + pois_path = os.path.join(dest, 'pois.json') with open(pois_path, 'w') as _f: json.dump( {'0': {'id': 0, 'name': 'home', 'position': [0.0, 0.0, 0.0]}}, @@ -977,6 +1036,72 @@ def _on_build_map_done(self): self._pub_state() self._restart_sensor_procs() + @staticmethod + def _nearest_pose(poses: dict, timestamp_ns: int): + if not poses: + return None, None + items = [(int(key), pose) for key, pose in poses.items()] + nearest_key, nearest_pose = min(items, key=lambda item: abs(item[0] - timestamp_ns)) + return nearest_key, nearest_pose + + def _generate_pois_from_marks(self, bag_path: str | None, map_path: str) -> bool: + if bag_path is None: + return False + marks = self._load_poi_marks(bag_path) + if not marks: + return False + + poses_path = os.path.join(map_path, 'poses.npy') + if not os.path.exists(poses_path): + self.get_logger().warn('Cannot generate POIs: poses.npy not found') + return False + + try: + optimized_poses = np.load(poses_path, allow_pickle=True).item() + continuous_path = os.path.join(map_path, 'mapping_continuous_odom.npy') + continuous_poses = ( + np.load(continuous_path, allow_pickle=True).item() + if os.path.exists(continuous_path) + else {} + ) + except Exception as e: + self.get_logger().warn(f'Cannot load map poses for POI marks: {e}') + return False + + pois: dict[str, dict] = {} + for mark in marks: + timestamp_ns = int(mark.get('timestamp_ns', 0)) + keyframe_ts, optimized_keyframe_pose = self._nearest_pose(optimized_poses, timestamp_ns) + if keyframe_ts is None: + continue + + pose = optimized_keyframe_pose + mark_ts, raw_mark_pose = self._nearest_pose(continuous_poses, timestamp_ns) + raw_key_ts, raw_keyframe_pose = self._nearest_pose(continuous_poses, keyframe_ts) + if mark_ts is not None and raw_key_ts is not None: + try: + pose = optimized_keyframe_pose @ np.linalg.inv(raw_keyframe_pose) @ raw_mark_pose + except Exception as e: + self.get_logger().warn(f'Failed to correct POI mark pose: {e}') + pose = optimized_keyframe_pose + + position = [float(v) for v in pose[:3, 3]] + poi_id = len(pois) + pois[str(poi_id)] = { + 'id': poi_id, + 'name': str(mark.get('name') or f'poi_{poi_id + 1}'), + 'position': position, + } + + if not pois: + return False + + pois_path = os.path.join(map_path, 'pois.json') + with open(pois_path, 'w') as f: + json.dump(pois, f, indent=2) + self.get_logger().info(f'Generated {len(pois)} POIs from bag marks') + return True + def cmd_map_build(self): self._stop_sensor_procs() diff --git a/app/backend/routers/bag.py b/app/backend/routers/bag.py index c3daee52..464ae97d 100644 --- a/app/backend/routers/bag.py +++ b/app/backend/routers/bag.py @@ -1,4 +1,8 @@ +from typing import Optional + from fastapi import APIRouter, HTTPException +from pydantic import BaseModel + from ..state import runner router = APIRouter(tags=['bag']) @@ -10,6 +14,11 @@ def _require_node(): return runner.node +class PoiMarkRequest(BaseModel): + name: str + timestamp_ns: Optional[int] = None + + @router.post('/start') def bag_start(): node = _require_node() @@ -39,4 +48,17 @@ def bag_status(): 'status': 'recording' if node.state == 'realsense_bag_record' else 'idle', 'bagFileReady': os.path.exists(bag_file), 'bagPath': node.bag_path, + 'poiMarkCount': node.get_poi_mark_count(), } + + +@router.post('/poi-marks') +def bag_poi_mark(req: PoiMarkRequest): + node = _require_node() + if node.state != 'realsense_bag_record': + raise HTTPException(409, 'POI marks can only be recorded while bag recording') + try: + mark = node.record_poi_mark(req.name, req.timestamp_ns) + except ValueError as e: + raise HTTPException(400, str(e)) + return {'ok': True, 'mark': mark, 'count': node.get_poi_mark_count()} diff --git a/app/frontend/lib/core/models.dart b/app/frontend/lib/core/models.dart index 9bbd7a0e..d1a8e74e 100644 --- a/app/frontend/lib/core/models.dart +++ b/app/frontend/lib/core/models.dart @@ -36,6 +36,7 @@ class DeviceStatus { final String rawState; final bool navNodesRunning; final bool navPaused; + final int poiMarkCount; const DeviceStatus({ required this.online, @@ -48,6 +49,7 @@ class DeviceStatus { required this.rawState, required this.navNodesRunning, required this.navPaused, + required this.poiMarkCount, }); factory DeviceStatus.fromJson(Map json) => DeviceStatus( @@ -61,6 +63,7 @@ class DeviceStatus { rawState: json['rawState'] as String? ?? 'unknown', navNodesRunning: json['navNodesRunning'] as bool? ?? false, navPaused: json['navPaused'] as bool? ?? false, + poiMarkCount: (json['poiMarkCount'] as num?)?.toInt() ?? 0, ); } diff --git a/app/frontend/lib/pages/map_tab.dart b/app/frontend/lib/pages/map_tab.dart index 6632d634..9858b9fd 100644 --- a/app/frontend/lib/pages/map_tab.dart +++ b/app/frontend/lib/pages/map_tab.dart @@ -74,11 +74,69 @@ class _BagRecordCard extends ConsumerStatefulWidget { class _BagRecordCardState extends ConsumerState<_BagRecordCard> { bool _busy = false; + int? _localPoiMarkCount; Future _call(String path) async { setState(() => _busy = true); try { await ref.read(dioProvider).post(path); + if (path == '/bag/start' && mounted) { + setState(() => _localPoiMarkCount = 0); + } + } on DioException catch (e) { + if (mounted) _snack(context, e.response?.data?['detail'] ?? e.message ?? 'Error'); + } finally { + if (mounted) setState(() => _busy = false); + } + } + + Future _setPoiMark() async { + final defaultName = + 'poi_${(_localPoiMarkCount ?? widget.status.poiMarkCount) + 1}'; + final controller = TextEditingController(text: defaultName); + final name = await showDialog( + context: context, + builder: (context) => AlertDialog( + title: const Text('Set POI'), + content: TextField( + controller: controller, + autofocus: true, + decoration: const InputDecoration( + labelText: 'POI name', + hintText: 'printer', + ), + textInputAction: TextInputAction.done, + onSubmitted: (value) => Navigator.of(context).pop(value.trim()), + ), + actions: [ + TextButton( + onPressed: () => Navigator.of(context).pop(), + child: const Text('Cancel'), + ), + FilledButton( + onPressed: () => Navigator.of(context).pop(controller.text.trim()), + child: const Text('Save'), + ), + ], + ), + ); + controller.dispose(); + if (name == null || name.isEmpty) return; + + setState(() => _busy = true); + try { + final resp = await ref + .read(dioProvider) + .post('/bag/poi-marks', data: {'name': name}); + final count = + resp.data is Map ? resp.data['count'] as int? : null; + if (mounted) { + setState(() { + _localPoiMarkCount = + count ?? ((_localPoiMarkCount ?? widget.status.poiMarkCount) + 1); + }); + _snack(context, 'POI "$name" marked'); + } } on DioException catch (e) { if (mounted) _snack(context, e.response?.data?['detail'] ?? e.message ?? 'Error'); } finally { @@ -92,6 +150,7 @@ class _BagRecordCardState extends ConsumerState<_BagRecordCard> { final isRecording = s.rawState == 'realsense_bag_record'; final canStart = s.online && s.rawState == 'idle'; final canStop = s.online && isRecording; + final poiMarkCount = _localPoiMarkCount ?? s.poiMarkCount; return _SectionCard( icon: Icons.videocam_outlined, @@ -103,6 +162,7 @@ class _BagRecordCardState extends ConsumerState<_BagRecordCard> { crossAxisAlignment: CrossAxisAlignment.start, children: [ _InfoRow('Status', s.bagStatus), + if (poiMarkCount > 0) _InfoRow('POI marks', '$poiMarkCount'), const SizedBox(height: 12), Row(children: [ Expanded( @@ -122,6 +182,15 @@ class _BagRecordCardState extends ConsumerState<_BagRecordCard> { ), ), ]), + const SizedBox(height: 10), + SizedBox( + width: double.infinity, + child: OutlinedButton.icon( + onPressed: isRecording && !_busy ? _setPoiMark : null, + icon: const Icon(Icons.add_location_alt_outlined), + label: const Text('Set POI'), + ), + ), ], ), ); diff --git a/tests/test_backend.py b/tests/test_backend.py index 033f2aeb..cd026dcf 100644 --- a/tests/test_backend.py +++ b/tests/test_backend.py @@ -234,6 +234,9 @@ def get_status(self): 'rawState': self.state, } + def get_poi_mark_count(self, bag_path=None): + return 0 + def cmd_bag_start(self): self.state = 'realsense_bag_record' self._started.append('bag')