diff --git a/app/backend/node_manager.py b/app/backend/node_manager.py index 23974642..05e2c5cb 100644 --- a/app/backend/node_manager.py +++ b/app/backend/node_manager.py @@ -89,7 +89,7 @@ def __init__(self, tinynav_db_path: str = '/tinynav/tinynav_db'): self._nav_target_pose: dict | None = None self.create_subscription(Float32, '/mapping/percent', self._on_mapping_percent, 10) - self.create_subscription(Odometry, '/slam/odometry', self._on_slam_odom, 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 ) diff --git a/app/frontend/lib/pages/operate_tab.dart b/app/frontend/lib/pages/operate_tab.dart index 9d939f39..1c77cf2a 100644 --- a/app/frontend/lib/pages/operate_tab.dart +++ b/app/frontend/lib/pages/operate_tab.dart @@ -704,7 +704,7 @@ class _Local3dPlanningViewState extends State<_Local3dPlanningView> { ), ], ), - ), + ) ); } } @@ -734,7 +734,7 @@ class _LocalViewModeButton extends StatelessWidget { fontWeight: FontWeight.w700, ), ), - ), + ) ); } } @@ -1049,6 +1049,7 @@ class _PoiSheet extends ConsumerStatefulWidget { class _PoiSheetState extends ConsumerState<_PoiSheet> { /// POI ids in the exact order they were checked. final List _checkedIds = []; + final ScrollController _poiScrollController = ScrollController(); Future _deletePoi(Poi poi) async { final ok = await showDialog( @@ -1101,6 +1102,12 @@ class _PoiSheetState extends ConsumerState<_PoiSheet> { } } + @override + void dispose() { + _poiScrollController.dispose(); + super.dispose(); + } + @override Widget build(BuildContext context) { final poisAsync = ref.watch(poisProvider); @@ -1108,77 +1115,116 @@ class _PoiSheetState extends ConsumerState<_PoiSheet> { final localized = ref.watch(planningStreamProvider).valueOrNull?.localized ?? false; final canGo = status != null && status.online && localized; - return Padding( - padding: EdgeInsets.fromLTRB( - 16, 12, 16, 24 + MediaQuery.of(context).viewInsets.bottom), - child: Column( - mainAxisSize: MainAxisSize.min, - crossAxisAlignment: CrossAxisAlignment.start, - children: [ - Center( - child: Container( - width: 36, height: 4, - margin: const EdgeInsets.only(bottom: 14), - decoration: BoxDecoration( - color: Colors.grey.shade300, - borderRadius: BorderRadius.circular(2), - ), - ), + return SafeArea( + top: false, + child: FractionallySizedBox( + heightFactor: 0.9, + child: Padding( + padding: EdgeInsets.fromLTRB( + 16, + 12, + 16, + 24 + MediaQuery.of(context).viewInsets.bottom, ), - // ── Header ────────────────────────────────────────────────── - Row(children: [ - const Icon(Icons.place_outlined, size: 20), - const SizedBox(width: 8), - const Text('POIs', style: TextStyle(fontWeight: FontWeight.bold, fontSize: 16)), - const Spacer(), - FilledButton.icon( - onPressed: (canGo && _checkedIds.isNotEmpty) - ? () => _startNav(poisAsync.valueOrNull ?? []) - : null, - icon: const Icon(Icons.navigation_rounded, size: 16), - label: const Text('Go'), - style: FilledButton.styleFrom( - padding: const EdgeInsets.symmetric(horizontal: 12, vertical: 6), - minimumSize: Size.zero, + child: Column( + crossAxisAlignment: CrossAxisAlignment.start, + children: [ + Center( + child: Container( + width: 36, + height: 4, + margin: const EdgeInsets.only(bottom: 14), + decoration: BoxDecoration( + color: Colors.grey.shade300, + borderRadius: BorderRadius.circular(2), + ), + ), ), - ), - ]), - const Divider(height: 20), - // ── POI list ──────────────────────────────────────────────── - poisAsync.when( - data: (pois) => pois.isEmpty - ? const Padding( - padding: EdgeInsets.symmetric(vertical: 20), - child: Center( - child: Text('No POIs yet', style: TextStyle(color: Colors.grey)), - ), - ) - : Column( - children: pois - .map((poi) { - final orderIndex = _checkedIds.indexOf(poi.id); - return _PoiTile( - poi: poi, - checked: orderIndex != -1, - orderNumber: orderIndex == -1 ? null : orderIndex + 1, - onChecked: (v) => setState(() { - if (v) { - if (!_checkedIds.contains(poi.id)) { - _checkedIds.add(poi.id); - } - } else { - _checkedIds.remove(poi.id); - } - }), - onDelete: () => _deletePoi(poi), - ); - }) - .toList(), + Row(children: [ + const Icon(Icons.place_outlined, size: 20), + const SizedBox(width: 8), + const Text('POIs', style: TextStyle(fontWeight: FontWeight.bold, fontSize: 16)), + const Spacer(), + FilledButton.icon( + onPressed: (canGo && _checkedIds.isNotEmpty) + ? () => _startNav(poisAsync.valueOrNull ?? []) + : null, + icon: const Icon(Icons.navigation_rounded, size: 16), + label: const Text('Go'), + style: FilledButton.styleFrom( + padding: const EdgeInsets.symmetric(horizontal: 12, vertical: 6), + minimumSize: Size.zero, ), - loading: () => const Center(child: CircularProgressIndicator()), - error: (e, _) => Text('$e', style: const TextStyle(color: Colors.red)), + ), + ]), + const Divider(height: 20), + Expanded( + child: poisAsync.when( + data: (pois) => pois.isEmpty + ? const Center( + child: Text('No POIs yet', style: TextStyle(color: Colors.grey)), + ) + : ScrollConfiguration( + behavior: ScrollConfiguration.of(context).copyWith( + dragDevices: { + PointerDeviceKind.touch, + PointerDeviceKind.mouse, + PointerDeviceKind.trackpad, + PointerDeviceKind.stylus, + PointerDeviceKind.unknown, + }, + ), + child: Scrollbar( + thumbVisibility: pois.length > 8, + controller: _poiScrollController, + child: ListView.builder( + controller: _poiScrollController, + primary: false, + physics: const AlwaysScrollableScrollPhysics(), + padding: EdgeInsets.zero, + itemCount: pois.length, + itemBuilder: (context, index) { + final poi = pois[index]; + final orderIndex = _checkedIds.indexOf(poi.id); + return _PoiTile( + poi: poi, + checked: orderIndex != -1, + orderNumber: orderIndex == -1 ? null : orderIndex + 1, + onChecked: (v) => setState(() { + if (v) { + if (!_checkedIds.contains(poi.id)) { + _checkedIds.add(poi.id); + } + } else { + _checkedIds.remove(poi.id); + } + }), + onDelete: () => _deletePoi(poi), + ); + }, + ), + ), + ), + loading: () => ListView( + controller: _poiScrollController, + children: const [ + SizedBox( + height: 180, + child: Center(child: CircularProgressIndicator()), + ), + ], + ), + error: (e, _) => ListView( + controller: _poiScrollController, + children: [ + Text('$e', style: const TextStyle(color: Colors.red)), + ], + ), + ), + ), + ], ), - ], + ), ), ); } diff --git a/app/frontend/test/widget_test.dart b/app/frontend/test/widget_test.dart index b96fac57..b7777e74 100644 --- a/app/frontend/test/widget_test.dart +++ b/app/frontend/test/widget_test.dart @@ -1,30 +1,13 @@ -// This is a basic Flutter widget test. -// -// To perform an interaction with a widget in your test, use the WidgetTester -// utility in the flutter_test package. For example, you can send tap and scroll -// gestures. You can also use WidgetTester to find child widgets in the widget -// tree, read text, and verify that the values of widget properties are correct. - -import 'package:flutter/material.dart'; import 'package:flutter_test/flutter_test.dart'; +import 'package:flutter_riverpod/flutter_riverpod.dart'; import 'package:tinynav_app/main.dart'; void main() { - testWidgets('Counter increments smoke test', (WidgetTester tester) async { - // Build our app and trigger a frame. - await tester.pumpWidget(const MyApp()); - - // Verify that our counter starts at 0. - expect(find.text('0'), findsOneWidget); - expect(find.text('1'), findsNothing); - - // Tap the '+' icon and trigger a frame. - await tester.tap(find.byIcon(Icons.add)); + testWidgets('TinyNav app smoke test', (WidgetTester tester) async { + await tester.pumpWidget(const ProviderScope(child: TinyNavApp())); await tester.pump(); - // Verify that our counter has incremented. - expect(find.text('0'), findsNothing); - expect(find.text('1'), findsOneWidget); + expect(find.text('TinyNav'), findsOneWidget); }); } diff --git a/tinynav/core/imu_propagator_node.py b/tinynav/core/imu_propagator_node.py index cfca4bcd..2e643fcf 100644 --- a/tinynav/core/imu_propagator_node.py +++ b/tinynav/core/imu_propagator_node.py @@ -70,9 +70,9 @@ def __init__(self): Imu, "/camera/camera/imu", self.imu_callback, qos_profile ) self.odom_sub = self.create_subscription( - Odometry, "/slam/odometry", self.odom_callback, qos_profile + Odometry, "/slam/odometry_visual", self.odom_callback, qos_profile ) - self.odom_pub = self.create_publisher(Odometry, "/slam/odometry_100hz", 50) + self.odom_pub = self.create_publisher(Odometry, "/slam/odometry", 50) self.imu_buffer = [] self.odom_10hz_buffer = [] diff --git a/tinynav/core/perception_node.py b/tinynav/core/perception_node.py index 76af7e68..8604477b 100644 --- a/tinynav/core/perception_node.py +++ b/tinynav/core/perception_node.py @@ -130,7 +130,7 @@ def __init__(self, verbose_timer: bool = True): self.input_aligner.registerCallback(1, self._aligned_stereo_callback) self.input_aligner_seen_imu = False self.input_aligner_seen_stereo = False - self.odom_pub = self.create_publisher(Odometry, "/slam/odometry", 10) + self.odom_pub = self.create_publisher(Odometry, "/slam/odometry_visual", 10) self.slam_camera_info_pub = self.create_publisher(CameraInfo, "/slam/camera_info", 10) self.depth_pub = self.create_publisher(Image, "/slam/depth", 10) self.disparity_pub_vis = self.create_publisher(Image, '/slam/disparity_vis', 10) diff --git a/tool/poi_editor.py b/tool/poi_editor.py index 3707fff4..2becf8d6 100755 --- a/tool/poi_editor.py +++ b/tool/poi_editor.py @@ -20,6 +20,7 @@ import rclpy import os from math_utils import msg2np, matrix_to_quat +from tool.video_db import VideoDB class SplatFile(TypedDict): centers: npt.NDArray[np.floating] @@ -139,6 +140,33 @@ def load_pointcloud_ply(ply_file_path: Path, center: bool = False) -> dict: "colors": colors, } + +def _open_infra1_video_db(map_dir: Path) -> VideoDB | None: + db_dir = map_dir / "infra1_images_db" + if not db_dir.exists(): + return None + try: + return VideoDB(dir_path=str(db_dir), mode="read") + except Exception as e: + print(f"Warning: failed to open infra1 VideoDB at {db_dir}: {e}") + return None + + +def _load_infra1_camera_image(infra1_db: VideoDB | None, timestamp: str) -> np.ndarray | None: + if infra1_db is None: + return None + try: + # Keep exact-key lookup, but tolerate float-string formatting like "1756222679.0". + ts_key = int(float(timestamp)) + except (TypeError, ValueError): + return None + img = infra1_db.read(ts_key) + if img is None: + return None + if img.ndim == 2: + return cv2.cvtColor(img, cv2.COLOR_GRAY2RGB) + return cv2.cvtColor(img, cv2.COLOR_BGR2RGB) + def create_poi_ui(server,poi_list_container,poi_index:int, poi_points:dict, sphere_handle:viser.SceneHandle): with poi_list_container: @@ -361,7 +389,6 @@ def _(_) -> None: unknown_indices = np.argwhere(x_y_plane == 0) free_indices = np.argwhere(x_y_plane == 1) occupied_indices = np.argwhere(x_y_plane == 2) - esdf_max_dist = 1.0 # Project to one Z plane in world coordinates. z_plane = float(origin[2]) @@ -378,6 +405,7 @@ def _xy_to_world_points(xy_indices: np.ndarray) -> np.ndarray: unknown_points = _xy_to_world_points(unknown_indices) free_points = _xy_to_world_points(free_indices) occupied_points = _xy_to_world_points(occupied_indices) + sdf_search_handle = None def _xyz_to_world_points(xyz_indices: np.ndarray) -> np.ndarray: if len(xyz_indices) == 0: @@ -392,8 +420,6 @@ def _xyz_to_world_points(xyz_indices: np.ndarray) -> np.ndarray: unknown_handle = None free_handle = None occupied_handle = None - sdf_search_handle = None - sdf_3d_handle = None if len(unknown_points) > 0: unknown_colors = np.zeros((len(unknown_points), 3), dtype=np.float32) print(f"Adding {len(unknown_points)} unknown 2D cells (black)") @@ -416,37 +442,39 @@ def _xyz_to_world_points(xyz_indices: np.ndarray) -> np.ndarray: point_shape="rounded", ) - # Load and visualize true 3D SDF voxels (no 2D fallback). + if len(occupied_points) > 0: + occupied_column_height = 0.8 # meters + z_levels = np.arange( + z_plane + float(resolution) * 0.5, + z_plane + occupied_column_height, + float(resolution), + dtype=np.float32, + ) + occupied_column_points = np.repeat(occupied_points, len(z_levels), axis=0) + occupied_column_points[:, 2] = np.tile(z_levels, len(occupied_points)) + # Occupied color = ESDF zero color in the same JET colormap. + wall_zero_bgr = cv2.applyColorMap(np.array([[255]], dtype=np.uint8), cv2.COLORMAP_JET)[0, 0] + wall_zero_rgb = wall_zero_bgr[::-1].astype(np.float32) / 255.0 + wall_light_rgb = np.clip(0.55 * wall_zero_rgb + 0.45 * np.ones(3, dtype=np.float32), 0.0, 1.0) + occupied_colors = np.tile(wall_light_rgb[None, :], (len(occupied_column_points), 1)) + print( + f"Adding {len(occupied_points)} occupied cells as " + f"{len(occupied_column_points)} ESDF-zero-color column points" + ) + occupied_handle = server.scene.add_point_cloud( + "/occupancy_2d/occupied", + points=occupied_column_points, + colors=occupied_colors, + point_size=resolution * 0.8, + point_shape="rounded", + ) + + # Keep only SDF<0.2m visualization from 3D SDF map. if sdf_map_path.exists(): sdf_map = np.load(sdf_map_path).astype(np.float32) if sdf_map.shape == occupancy_grid.shape: traversable_mask = occupancy_grid != 2 sdf_valid_mask = np.logical_and(traversable_mask, np.isfinite(sdf_map)) - sdf_indices_all = np.argwhere(sdf_valid_mask) - max_points = 400_000 - if len(sdf_indices_all) > max_points: - stride = int(np.ceil(len(sdf_indices_all) / max_points)) - sdf_indices = sdf_indices_all[::stride] - else: - sdf_indices = sdf_indices_all - sdf_points = _xyz_to_world_points(sdf_indices) - sdf_values = np.clip( - sdf_map[sdf_indices[:, 0], sdf_indices[:, 1], sdf_indices[:, 2]], - 0.0, - esdf_max_dist, - ) - v = np.uint8((1.0 - sdf_values / esdf_max_dist) * 255.0) - sdf_colors_bgr = cv2.applyColorMap(v.reshape(-1, 1), cv2.COLORMAP_JET).reshape(-1, 3) - sdf_colors = sdf_colors_bgr[:, ::-1].astype(np.float32) / 255.0 - print(f"Adding {len(sdf_points)} sampled 3D SDF voxels") - sdf_3d_handle = server.scene.add_point_cloud( - "/occupancy_3d/sdf", - points=sdf_points, - colors=sdf_colors, - point_size=resolution * 0.7, - point_shape="rounded", - ) - sdf_search_threshold = 0.2 sdf_search_mask = np.logical_and(sdf_valid_mask, sdf_map < sdf_search_threshold) sdf_search_indices_all = np.argwhere(sdf_search_mask) @@ -466,52 +494,17 @@ def _xyz_to_world_points(xyz_indices: np.ndarray) -> np.ndarray: f"(sdf < {sdf_search_threshold:.2f} m, magenta)" ) sdf_search_handle = server.scene.add_point_cloud( - "/occupancy_3d/sdf_search_region", + "/occupancy_2d/sdf_search_region", points=sdf_search_points, colors=sdf_search_colors, point_size=resolution * 0.8, point_shape="rounded", ) - else: - print( - f"Warning: sdf_map shape mismatch, expected {occupancy_grid.shape}, got {sdf_map.shape}. " - "Skip SDF visualization." - ) - else: - print(f"Warning: Missing {sdf_map_path}. Skip SDF visualization.") - - if len(occupied_points) > 0: - occupied_column_height = 0.8 # meters - z_levels = np.arange( - z_plane + float(resolution) * 0.5, - z_plane + occupied_column_height, - float(resolution), - dtype=np.float32, - ) - occupied_column_points = np.repeat(occupied_points, len(z_levels), axis=0) - occupied_column_points[:, 2] = np.tile(z_levels, len(occupied_points)) - # Occupied color = ESDF zero color in the same JET colormap. - wall_zero_bgr = cv2.applyColorMap(np.array([[255]], dtype=np.uint8), cv2.COLORMAP_JET)[0, 0] - wall_zero_rgb = wall_zero_bgr[::-1].astype(np.float32) / 255.0 - wall_light_rgb = np.clip(0.55 * wall_zero_rgb + 0.45 * np.ones(3, dtype=np.float32), 0.0, 1.0) - occupied_colors = np.tile(wall_light_rgb[None, :], (len(occupied_column_points), 1)) - print( - f"Adding {len(occupied_points)} occupied cells as " - f"{len(occupied_column_points)} ESDF-zero-color column points" - ) - occupied_handle = server.scene.add_point_cloud( - "/occupancy_2d/occupied", - points=occupied_column_points, - colors=occupied_colors, - point_size=resolution * 0.8, - point_shape="rounded", - ) if ( unknown_handle is not None or free_handle is not None or occupied_handle is not None - or sdf_3d_handle is not None or sdf_search_handle is not None ): # Default visibility for projected 2D occupancy. @@ -521,27 +514,17 @@ def _xyz_to_world_points(xyz_indices: np.ndarray) -> np.ndarray: free_handle.visible = True if occupied_handle is not None: occupied_handle.visible = True - if sdf_3d_handle is not None: - sdf_3d_handle.visible = False if sdf_search_handle is not None: sdf_search_handle.visible = False - point_size_init = float(resolution * 0.8) point_size_max = max(0.1, point_size_init) with server.gui.add_folder("Occupancy 2D Map") as _: - show_unknown = server.gui.add_checkbox("Show Unknown", initial_value=False) show_free = server.gui.add_checkbox("Show Free", initial_value=True) show_occupied = server.gui.add_checkbox("Show Occupied", initial_value=True) - show_sdf_3d = server.gui.add_checkbox("Show SDF 3D", initial_value=False) show_sdf_search_region = server.gui.add_checkbox("Show SDF<0.2m Region", initial_value=False) point_size_slider = server.gui.add_slider( "Point Size", min=0.001, max=point_size_max, step=0.001, initial_value=point_size_init ) - - @show_unknown.on_update - def _(_) -> None: - if unknown_handle is not None: - unknown_handle.visible = show_unknown.value @show_free.on_update def _(_) -> None: @@ -553,16 +536,11 @@ def _(_) -> None: if occupied_handle is not None: occupied_handle.visible = show_occupied.value - @show_sdf_3d.on_update - def _(_) -> None: - if sdf_3d_handle is not None: - sdf_3d_handle.visible = show_sdf_3d.value - @show_sdf_search_region.on_update def _(_) -> None: if sdf_search_handle is not None: sdf_search_handle.visible = show_sdf_search_region.value - + @point_size_slider.on_update def _(_) -> None: if unknown_handle is not None: @@ -571,8 +549,6 @@ def _(_) -> None: free_handle.point_size = point_size_slider.value if occupied_handle is not None: occupied_handle.point_size = point_size_slider.value - if sdf_3d_handle is not None: - sdf_3d_handle.point_size = point_size_slider.value if sdf_search_handle is not None: sdf_search_handle.point_size = point_size_slider.value else: @@ -591,11 +567,13 @@ def _(_) -> None: raise FileNotFoundError("Neither intrinsics.npy nor rgb_camera_intrinsics.npy exists.") fx, _, cx, cy = camera_K[0, 0], camera_K[1, 1], camera_K[0, 2], camera_K[1, 2] + infra1_db = _open_infra1_video_db(tinynav_map_path) with server.gui.add_folder("cameras") as _: for timestamp, camera_pose in poses.items(): R = vtf.SO3.from_matrix(camera_pose[:3, :3]) t = camera_pose[:3, 3] - _ = server.scene.add_camera_frustum( + timestamp_str = str(timestamp) + frustum = server.scene.add_camera_frustum( name=f"/cameras/camera_{timestamp}", fov=float(2 * np.arctan((cx / fx))), scale=0.01, @@ -607,6 +585,25 @@ def _(_) -> None: jpeg_quality=50 ) + @frustum.on_click + def _(event, cam_pose=camera_pose, ts=timestamp_str, fr=frustum): + q = matrix_to_quat(cam_pose[:3, :3]) # xyzw + target_position = tuple(cam_pose[:3, 3].tolist()) + target_wxyz = (q[3], q[0], q[1], q[2]) + img = _load_infra1_camera_image(infra1_db, ts) + if img is not None: + fr.image = img + else: + print(f"timestamp {ts} don't found image") + client = getattr(event, "client", None) + if client is not None: + client.camera.position = target_position + client.camera.wxyz = target_wxyz + return + for c in server.get_clients().values(): + c.camera.position = target_position + c.camera.wxyz = target_wxyz + # Load splat or point cloud files splat_path = Path(f"{tinynav_map_path}/splat.ply") pointcloud_path = Path(f"{tinynav_map_path}/pointcloud.ply") @@ -673,6 +670,9 @@ def _(_, pc_handle=pc_handle, remove_button=remove_button) -> None: rclpy.shutdown() except KeyboardInterrupt: pass + finally: + if infra1_db is not None: + infra1_db.close() if __name__ == "__main__":