From c2ce5630a460cbab43b17ebff5482e4d0c51547e Mon Sep 17 00:00:00 2001 From: xiaolefang-dm Date: Sat, 13 Jun 2026 23:10:02 +0800 Subject: [PATCH 1/5] =?UTF-8?q?feat:=20auto-localization=20assist=20?= =?UTF-8?q?=E2=80=94=20yaw=20sweep=20until=20localized?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit When the 'Assist' toggle is enabled on the Operate page: - Nav ON starts map_node but NOT cmd_vel_control - Backend runs a yaw sweep (±expanding angles, 5s dwell each) - Once localized, sweep stops and cmd_vel_control starts normally Backend changes (node_manager.py): - _loc_assist_enabled / _loc_assist_thread / _loc_assist_stop_event state - _loc_assist_loop(): expanding yaw sweep pattern via /cmd_vel - _on_localization_achieved(): stops sweep, launches cmd_vel_control - cmd_start_nav_nodes skips cmd_vel_control when assist is on - cmd_stop_nav_nodes stops assist thread - get_status() includes locAssistEnabled API (routers/nav.py): - POST /nav/loc-assist {enabled: bool} — toggle - GET /nav/loc-assist — read state Frontend: - DeviceStatus.locAssistEnabled field - _LocAssistToggle button on Operate page (bottom-right, above Nav) --- app/backend/node_manager.py | 150 +++++++++++++++++++++++- app/backend/routers/nav.py | 16 +++ app/frontend/lib/core/models.dart | 3 + app/frontend/lib/pages/operate_tab.dart | 66 +++++++++++ 4 files changed, 230 insertions(+), 5 deletions(-) diff --git a/app/backend/node_manager.py b/app/backend/node_manager.py index 777d3468..3f0e04e7 100644 --- a/app/backend/node_manager.py +++ b/app/backend/node_manager.py @@ -191,6 +191,11 @@ def __init__(self, tinynav_db_path: str = '/tinynav/tinynav_db'): self._map_node_proc: subprocess.Popen | None = None self._cmd_vel_proc: subprocess.Popen | None = None + # Auto-localization assist: sweep yaw while waiting for localization + self._loc_assist_enabled: bool = False + self._loc_assist_thread: threading.Thread | None = None + self._loc_assist_stop_event = threading.Event() + self._nav_progress: dict | None = None self.nav_progress_callbacks: list = [] @@ -249,10 +254,13 @@ def _on_slam_odom(self, msg: Odometry): def _on_pose_in_map(self, msg: Odometry): pose = self._odom_to_dict(msg, source='map') with self._lock: + was_localized = self._localized self.current_pose = pose self._map_pose = pose self._odom_pose_at_kf = self._odom_pose # freeze odom at this keyframe self._localized = True + if not was_localized: + self._on_localization_achieved() for cb in self.pose_callbacks: try: cb(pose) @@ -262,8 +270,11 @@ def _on_pose_in_map(self, msg: Odometry): def _on_relocalization(self, msg: Odometry): pose = self._odom_to_dict(msg, source='map') with self._lock: + was_localized = self._localized self._map_pose = pose self._localized = True + if not was_localized: + self._on_localization_achieved() def _on_nav_target_pose(self, msg: Odometry): with self._lock: @@ -631,6 +642,7 @@ def get_status(self) -> dict: nav_nodes = self._nav_nodes_running nav_paused = self._nav_paused nav_active = self._nav_active + loc_assist = self._loc_assist_enabled bag_files_exist = self.active_bag_path is not None map_files_exist = os.path.exists(os.path.join(self.map_path, 'occupancy_grid.npy')) return { @@ -644,6 +656,7 @@ def get_status(self) -> dict: 'navNodesRunning': nav_nodes, 'navPaused': nav_paused, 'navActive': nav_active, + 'locAssistEnabled': loc_assist, } @staticmethod @@ -749,17 +762,24 @@ def cmd_start_nav_nodes(self): ], env=_env, ) - self._cmd_vel_proc = self._launch_proc( - 'cmd_vel_control', - ['uv', 'run', 'python', '/tinynav/tinynav/platforms/cmd_vel_control.py'], - env=_env, - ) + with self._lock: + loc_assist = self._loc_assist_enabled + if loc_assist: + # Don't start cmd_vel_control yet; start localization assist sweep + self._start_loc_assist(_env) + else: + self._cmd_vel_proc = self._launch_proc( + 'cmd_vel_control', + ['uv', 'run', 'python', '/tinynav/tinynav/platforms/cmd_vel_control.py'], + env=_env, + ) with self._lock: self._nav_nodes_running = True self.get_logger().info('Nav nodes started') def cmd_stop_nav_nodes(self): self._set_nav_active(False) + self._stop_loc_assist() self._kill_proc(self._map_node_proc) self._kill_proc(self._cmd_vel_proc) self._map_node_proc = None @@ -775,6 +795,7 @@ def cmd_stop_nav_nodes(self): def cmd_restart_nav_nodes(self): self._set_nav_active(False) + self._stop_loc_assist() self._kill_proc(self._map_node_proc) self._kill_proc(self._planning_proc) self._kill_proc(self._cmd_vel_proc) @@ -811,6 +832,125 @@ def cmd_restart_nav_nodes(self): self._pub_state() self.get_logger().info('Nav nodes restarted (emergency stop)') + # ------------------------------------------------------------------ # + # Localization assist: yaw sweep until localized # + # ------------------------------------------------------------------ # + + def cmd_set_loc_assist(self, enabled: bool): + """Enable or disable the auto-localization assist toggle.""" + with self._lock: + self._loc_assist_enabled = enabled + self.get_logger().info(f'Localization assist {"enabled" if enabled else "disabled"}') + + def _start_loc_assist(self, env: dict): + """Start the yaw sweep thread (no cmd_vel_control process).""" + self._loc_assist_stop_event.clear() + self._loc_assist_thread = threading.Thread( + target=self._loc_assist_loop, daemon=True + ) + self._loc_assist_thread.start() + self.get_logger().info('Localization assist sweep started') + + def _stop_loc_assist(self): + """Stop the yaw sweep thread if running, publish zero cmd_vel.""" + self._loc_assist_stop_event.set() + if self._loc_assist_thread is not None: + self._loc_assist_thread.join(timeout=6.0) + self._loc_assist_thread = None + # Ensure robot stops + self._publish_cmd_vel(0.0, 0.0) + + def _publish_cmd_vel(self, linear_x: float, angular_z: float): + msg = Twist() + msg.linear.x = float(linear_x) + msg.angular.z = float(angular_z) + self._cmd_vel_pub.publish(msg) + + def _loc_assist_loop(self): + """ + Yaw sweep pattern: + - Start facing current direction, wait dwell_s + - Turn CW 20°, wait dwell_s + - Turn CCW 40° (net -20° from start), wait dwell_s + - Turn CW 60° (net +40° from start), wait dwell_s + - Turn CCW 80° (net -40° from start), wait dwell_s + - ... expanding sweep until localized + + Uses angular velocity to turn for a computed duration, then dwells. + Stops immediately when localized or stop event is set. + """ + dwell_s = 5.0 + angular_speed = 0.4 # rad/s + step_deg = 20.0 + step_rad = math.radians(step_deg) + stop = self._loc_assist_stop_event + + # Dwell at initial position + if self._wait_or_localized(dwell_s, stop): + return + + turn_index = 1 # 1, 2, 3, 4, ... + direction = 1 # +1 = CW, -1 = CCW + + while not stop.is_set(): + # Turn: angle = turn_index * step_rad + angle = turn_index * step_rad + duration = angle / angular_speed + # Publish angular velocity + self._publish_cmd_vel(0.0, -direction * angular_speed) + if self._wait_or_localized(duration, stop): + return + # Stop turning + self._publish_cmd_vel(0.0, 0.0) + # Dwell + if self._wait_or_localized(dwell_s, stop): + return + # Next sweep: increase index, flip direction + turn_index += 1 + direction *= -1 + + def _wait_or_localized(self, duration: float, stop: threading.Event) -> bool: + """ + Wait for `duration` seconds, checking localization and stop event + every 0.1s. Returns True if should stop (localized or event set). + """ + elapsed = 0.0 + interval = 0.1 + while elapsed < duration: + if stop.is_set(): + self._publish_cmd_vel(0.0, 0.0) + return True + with self._lock: + if self._localized: + self._publish_cmd_vel(0.0, 0.0) + return True + time.sleep(interval) + elapsed += interval + return False + + def _on_localization_achieved(self): + """ + Called when localization succeeds for the first time. + Stops the assist sweep and launches cmd_vel_control. + """ + with self._lock: + loc_assist = self._loc_assist_enabled + nav_running = self._nav_nodes_running + if not loc_assist or not nav_running: + return + # Stop the sweep + self._stop_loc_assist() + # Now start cmd_vel_control + if self._cmd_vel_proc is None: + _env = os.environ.copy() + _env['PYTHONPATH'] = _VENV_SITE + ':' + _env.get('PYTHONPATH', '') + self._cmd_vel_proc = self._launch_proc( + 'cmd_vel_control', + ['uv', 'run', 'python', '/tinynav/tinynav/platforms/cmd_vel_control.py'], + env=_env, + ) + self.get_logger().info('Localization achieved — cmd_vel_control started') + def cmd_bag_start(self): if self._sensor_mode == 'looper': self._stop_sensor_procs() diff --git a/app/backend/routers/nav.py b/app/backend/routers/nav.py index 62aa49c6..d71e8bde 100644 --- a/app/backend/routers/nav.py +++ b/app/backend/routers/nav.py @@ -112,3 +112,19 @@ def nav_nodes_disable(): raise HTTPException(409, 'Nav nodes not running') node.cmd_stop_nav_nodes() return {'ok': True} + + +@router.post('/loc-assist') +def nav_loc_assist(req: dict): + node = _require_node() + enabled = bool(req.get('enabled', False)) + node.cmd_set_loc_assist(enabled) + return {'ok': True, 'enabled': enabled} + + +@router.get('/loc-assist') +def nav_loc_assist_status(): + node = _require_node() + with node._lock: + enabled = node._loc_assist_enabled + return {'enabled': enabled} diff --git a/app/frontend/lib/core/models.dart b/app/frontend/lib/core/models.dart index 9bbd7a0e..11a1e858 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 bool locAssistEnabled; const DeviceStatus({ required this.online, @@ -48,6 +49,7 @@ class DeviceStatus { required this.rawState, required this.navNodesRunning, required this.navPaused, + required this.locAssistEnabled, }); 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, + locAssistEnabled: json['locAssistEnabled'] as bool? ?? false, ); } diff --git a/app/frontend/lib/pages/operate_tab.dart b/app/frontend/lib/pages/operate_tab.dart index 8b7e3487..ae1c7c48 100644 --- a/app/frontend/lib/pages/operate_tab.dart +++ b/app/frontend/lib/pages/operate_tab.dart @@ -249,6 +249,11 @@ class _OperateTabState extends ConsumerState { right: 10, child: _NavNodesButton(statusAsync: ref.watch(deviceStatusProvider)), ), + Positioned( + bottom: 50, + right: 10, + child: _LocAssistToggle(statusAsync: ref.watch(deviceStatusProvider)), + ), ], ), ), @@ -1415,6 +1420,67 @@ class _NavNodesButtonState extends ConsumerState<_NavNodesButton> { } } +// ── Localization assist toggle ──────────────────────────────────────────────── + +class _LocAssistToggle extends ConsumerStatefulWidget { + final AsyncValue statusAsync; + const _LocAssistToggle({required this.statusAsync}); + + @override + ConsumerState<_LocAssistToggle> createState() => _LocAssistToggleState(); +} + +class _LocAssistToggleState extends ConsumerState<_LocAssistToggle> { + bool _loading = false; + + Future _toggle(bool currentlyEnabled) async { + setState(() => _loading = true); + try { + await ref.read(dioProvider).post( + '/nav/loc-assist', + data: {'enabled': !currentlyEnabled}, + ); + } on DioException catch (e) { + if (mounted) { + ScaffoldMessenger.of(context).showSnackBar(SnackBar( + content: Text(e.response?.data?['detail'] ?? e.message ?? 'Error'), + backgroundColor: Colors.red, + )); + } + } finally { + if (mounted) setState(() => _loading = false); + } + } + + @override + Widget build(BuildContext context) { + final status = widget.statusAsync.valueOrNull; + final enabled = status?.locAssistEnabled ?? false; + + return FilledButton.icon( + onPressed: _loading ? null : () => _toggle(enabled), + style: FilledButton.styleFrom( + backgroundColor: enabled + ? const Color(0xFFFFB74D).withOpacity(0.9) + : Colors.black87, + foregroundColor: Colors.white, + padding: const EdgeInsets.symmetric(horizontal: 14, vertical: 8), + ), + icon: _loading + ? const SizedBox( + width: 14, + height: 14, + child: CircularProgressIndicator(strokeWidth: 2, color: Colors.white), + ) + : Icon( + enabled ? Icons.explore : Icons.explore_off_outlined, + size: 16, + ), + label: Text(enabled ? 'Assist ON' : 'Assist'), + ); + } +} + // ── Pause / Continue button ─────────────────────────────────────────────────── class _PauseButton extends ConsumerStatefulWidget { From 172b4855c005d0959ed11c1ced7184675eb72646 Mon Sep 17 00:00:00 2001 From: xiaolefang-dm Date: Sun, 14 Jun 2026 16:21:05 +0800 Subject: [PATCH 2/5] fix(nav): close loop localization assist yaw sweep --- app/backend/node_manager.py | 130 +++++++++++++++++++++++++++++++----- 1 file changed, 114 insertions(+), 16 deletions(-) diff --git a/app/backend/node_manager.py b/app/backend/node_manager.py index 3f0e04e7..d1d9927e 100644 --- a/app/backend/node_manager.py +++ b/app/backend/node_manager.py @@ -109,6 +109,7 @@ def __init__(self, tinynav_db_path: str = '/tinynav/tinynav_db'): # Planning / localization state (read via get_planning_snapshot) self._odom_pose: dict | None = None + self._odom_pose_received_at: float | None = None self._odom_pose_at_kf: dict | None = None # odom pose snapshotted at last mapPose update self._map_pose: dict | None = None self._localized: bool = False @@ -245,6 +246,7 @@ def _on_slam_odom(self, msg: Odometry): with self._lock: self.current_pose = pose self._odom_pose = pose + self._odom_pose_received_at = time.monotonic() for cb in self.pose_callbacks: try: cb(pose) @@ -854,7 +856,7 @@ def _start_loc_assist(self, env: dict): def _stop_loc_assist(self): """Stop the yaw sweep thread if running, publish zero cmd_vel.""" self._loc_assist_stop_event.set() - if self._loc_assist_thread is not None: + if self._loc_assist_thread is not None and self._loc_assist_thread is not threading.current_thread(): self._loc_assist_thread.join(timeout=6.0) self._loc_assist_thread = None # Ensure robot stops @@ -876,11 +878,14 @@ def _loc_assist_loop(self): - Turn CCW 80° (net -40° from start), wait dwell_s - ... expanding sweep until localized - Uses angular velocity to turn for a computed duration, then dwells. - Stops immediately when localized or stop event is set. + The turn amount is closed-loop against SLAM odometry yaw. While turning, + publish cmd_vel continuously so downstream controllers do not need to + latch a single Twist command. """ dwell_s = 5.0 angular_speed = 0.4 # rad/s + cmd_rate_hz = 10.0 + yaw_tolerance = math.radians(2.0) step_deg = 20.0 step_rad = math.radians(step_deg) stop = self._loc_assist_stop_event @@ -893,15 +898,18 @@ def _loc_assist_loop(self): direction = 1 # +1 = CW, -1 = CCW while not stop.is_set(): - # Turn: angle = turn_index * step_rad + # Turn relative to the current odom yaw. Positive angular.z is CCW, + # so the previous CW command maps to a negative target delta. angle = turn_index * step_rad - duration = angle / angular_speed - # Publish angular velocity - self._publish_cmd_vel(0.0, -direction * angular_speed) - if self._wait_or_localized(duration, stop): + target_delta = -direction * angle + if self._turn_relative_by_odom( + target_delta=target_delta, + angular_speed=angular_speed, + cmd_rate_hz=cmd_rate_hz, + yaw_tolerance=yaw_tolerance, + stop=stop, + ): return - # Stop turning - self._publish_cmd_vel(0.0, 0.0) # Dwell if self._wait_or_localized(dwell_s, stop): return @@ -909,6 +917,101 @@ def _loc_assist_loop(self): turn_index += 1 direction *= -1 + @staticmethod + def _wrap_angle(angle: float) -> float: + """Wrap an angle to [-pi, pi].""" + return math.atan2(math.sin(angle), math.cos(angle)) + + def _latest_odom_yaw(self, max_age_s: float = 1.0) -> float | None: + with self._lock: + pose = self._odom_pose + received_at = self._odom_pose_received_at + if pose is None or received_at is None: + return None + if time.monotonic() - received_at > max_age_s: + return None + yaw = pose.get('yaw') + return float(yaw) if yaw is not None else None + + def _turn_relative_by_odom( + self, + target_delta: float, + angular_speed: float, + cmd_rate_hz: float, + yaw_tolerance: float, + stop: threading.Event, + ) -> bool: + """ + Turn until odometry yaw reaches target_delta relative to the turn start. + Returns True if the assist loop should stop (localized or stop event set). + """ + interval = 1.0 / max(cmd_rate_hz, 1.0) + max_duration = abs(target_delta) / max(angular_speed, 1e-3) + 3.0 + + start_wait = time.monotonic() + start_yaw = self._latest_odom_yaw() + while start_yaw is None: + if self._should_stop_loc_assist(stop): + return True + # Do not blind-turn without fresh odometry. + self._publish_cmd_vel(0.0, 0.0) + if time.monotonic() - start_wait > 5.0: + self.get_logger().warn('Localization assist waiting for fresh odometry yaw') + start_wait = time.monotonic() + time.sleep(interval) + start_yaw = self._latest_odom_yaw() + + angular_z = math.copysign(abs(angular_speed), target_delta) + start_time = time.monotonic() + previous_yaw = start_yaw + accumulated_delta = 0.0 + + while True: + if self._should_stop_loc_assist(stop): + return True + + current_yaw = self._latest_odom_yaw() + if current_yaw is None: + # Odometry disappeared; stop rather than continuing open-loop. + self._publish_cmd_vel(0.0, 0.0) + time.sleep(interval) + continue + + accumulated_delta += self._wrap_angle(current_yaw - previous_yaw) + previous_yaw = current_yaw + remaining = target_delta - accumulated_delta + if abs(remaining) <= yaw_tolerance: + self._publish_cmd_vel(0.0, 0.0) + return False + + # If we overshot, stop this segment instead of commanding a reverse + # correction sweep. The next sweep segment will continue the pattern. + if math.copysign(1.0, remaining) != math.copysign(1.0, target_delta): + self._publish_cmd_vel(0.0, 0.0) + return False + + if time.monotonic() - start_time > max_duration: + self.get_logger().warn( + f'Localization assist turn timeout: target_delta={target_delta:.3f} ' + f'accumulated_delta={accumulated_delta:.3f} remaining={remaining:.3f}' + ) + self._publish_cmd_vel(0.0, 0.0) + return False + + self._publish_cmd_vel(0.0, angular_z) + time.sleep(interval) + + def _should_stop_loc_assist(self, stop: threading.Event) -> bool: + if stop.is_set(): + self._publish_cmd_vel(0.0, 0.0) + return True + with self._lock: + localized = self._localized + if localized: + self._publish_cmd_vel(0.0, 0.0) + return True + return False + def _wait_or_localized(self, duration: float, stop: threading.Event) -> bool: """ Wait for `duration` seconds, checking localization and stop event @@ -917,13 +1020,8 @@ def _wait_or_localized(self, duration: float, stop: threading.Event) -> bool: elapsed = 0.0 interval = 0.1 while elapsed < duration: - if stop.is_set(): - self._publish_cmd_vel(0.0, 0.0) + if self._should_stop_loc_assist(stop): return True - with self._lock: - if self._localized: - self._publish_cmd_vel(0.0, 0.0) - return True time.sleep(interval) elapsed += interval return False From df844a7236961a30e9567ebe36d6f2c21bc82b94 Mon Sep 17 00:00:00 2001 From: xiaolefang-dm Date: Mon, 15 Jun 2026 12:03:16 +0800 Subject: [PATCH 3/5] fix(nav): avoid duplicate cmd_vel controller start --- app/backend/node_manager.py | 23 ++++++++++++++++++----- 1 file changed, 18 insertions(+), 5 deletions(-) diff --git a/app/backend/node_manager.py b/app/backend/node_manager.py index d1d9927e..0d8b44cb 100644 --- a/app/backend/node_manager.py +++ b/app/backend/node_manager.py @@ -846,6 +846,9 @@ def cmd_set_loc_assist(self, enabled: bool): def _start_loc_assist(self, env: dict): """Start the yaw sweep thread (no cmd_vel_control process).""" + if self._loc_assist_thread is not None and self._loc_assist_thread.is_alive(): + self.get_logger().info('Localization assist sweep already running') + return self._loc_assist_stop_event.clear() self._loc_assist_thread = threading.Thread( target=self._loc_assist_loop, daemon=True @@ -1034,20 +1037,30 @@ def _on_localization_achieved(self): with self._lock: loc_assist = self._loc_assist_enabled nav_running = self._nav_nodes_running + cmd_vel_proc = self._cmd_vel_proc + if cmd_vel_proc is not None and cmd_vel_proc.poll() is None: + return + if cmd_vel_proc is not None: + self._cmd_vel_proc = None if not loc_assist or not nav_running: return # Stop the sweep self._stop_loc_assist() - # Now start cmd_vel_control - if self._cmd_vel_proc is None: - _env = os.environ.copy() - _env['PYTHONPATH'] = _VENV_SITE + ':' + _env.get('PYTHONPATH', '') + # Now start cmd_vel_control. Re-check under the lock because both + # /mapping/current_pose_in_map and /map/relocalization can report the + # first successful localization close together. + _env = os.environ.copy() + _env['PYTHONPATH'] = _VENV_SITE + ':' + _env.get('PYTHONPATH', '') + with self._lock: + cmd_vel_proc = self._cmd_vel_proc + if cmd_vel_proc is not None and cmd_vel_proc.poll() is None: + return self._cmd_vel_proc = self._launch_proc( 'cmd_vel_control', ['uv', 'run', 'python', '/tinynav/tinynav/platforms/cmd_vel_control.py'], env=_env, ) - self.get_logger().info('Localization achieved — cmd_vel_control started') + self.get_logger().info('Localization achieved — cmd_vel_control started') def cmd_bag_start(self): if self._sensor_mode == 'looper': From 06acfdbdaa35506f82cd8a91e33afa28f9128eaf Mon Sep 17 00:00:00 2001 From: Xiaole Fang Date: Fri, 19 Jun 2026 20:13:20 +0800 Subject: [PATCH 4/5] fix(nav): require assist before nav starts --- app/backend/node_manager.py | 7 +++++-- app/backend/routers/nav.py | 6 +++++- app/frontend/lib/pages/operate_tab.dart | 17 +++++++++++++++-- 3 files changed, 25 insertions(+), 5 deletions(-) diff --git a/app/backend/node_manager.py b/app/backend/node_manager.py index 0d8b44cb..1e99c7a3 100644 --- a/app/backend/node_manager.py +++ b/app/backend/node_manager.py @@ -838,11 +838,14 @@ def cmd_restart_nav_nodes(self): # Localization assist: yaw sweep until localized # # ------------------------------------------------------------------ # - def cmd_set_loc_assist(self, enabled: bool): - """Enable or disable the auto-localization assist toggle.""" + def cmd_set_loc_assist(self, enabled: bool) -> bool: + """Enable or disable localization assist before nav nodes start.""" with self._lock: + if self._nav_nodes_running: + return False self._loc_assist_enabled = enabled self.get_logger().info(f'Localization assist {"enabled" if enabled else "disabled"}') + return True def _start_loc_assist(self, env: dict): """Start the yaw sweep thread (no cmd_vel_control process).""" diff --git a/app/backend/routers/nav.py b/app/backend/routers/nav.py index d71e8bde..d84cb6d8 100644 --- a/app/backend/routers/nav.py +++ b/app/backend/routers/nav.py @@ -118,7 +118,11 @@ def nav_nodes_disable(): def nav_loc_assist(req: dict): node = _require_node() enabled = bool(req.get('enabled', False)) - node.cmd_set_loc_assist(enabled) + if not node.cmd_set_loc_assist(enabled): + raise HTTPException( + 409, + 'Turn Nav off before changing Assist; enable Assist before starting Nav', + ) return {'ok': True, 'enabled': enabled} diff --git a/app/frontend/lib/pages/operate_tab.dart b/app/frontend/lib/pages/operate_tab.dart index ae1c7c48..4a14497b 100644 --- a/app/frontend/lib/pages/operate_tab.dart +++ b/app/frontend/lib/pages/operate_tab.dart @@ -1433,7 +1433,19 @@ class _LocAssistToggle extends ConsumerStatefulWidget { class _LocAssistToggleState extends ConsumerState<_LocAssistToggle> { bool _loading = false; - Future _toggle(bool currentlyEnabled) async { + Future _toggle(bool currentlyEnabled, bool navNodesRunning) async { + if (navNodesRunning) { + ScaffoldMessenger.of(context).showSnackBar( + const SnackBar( + content: Text( + 'Turn Nav off before changing Assist; enable Assist before starting Nav', + ), + backgroundColor: Colors.orange, + ), + ); + return; + } + setState(() => _loading = true); try { await ref.read(dioProvider).post( @@ -1456,9 +1468,10 @@ class _LocAssistToggleState extends ConsumerState<_LocAssistToggle> { Widget build(BuildContext context) { final status = widget.statusAsync.valueOrNull; final enabled = status?.locAssistEnabled ?? false; + final navNodesRunning = status?.navNodesRunning ?? false; return FilledButton.icon( - onPressed: _loading ? null : () => _toggle(enabled), + onPressed: _loading ? null : () => _toggle(enabled, navNodesRunning), style: FilledButton.styleFrom( backgroundColor: enabled ? const Color(0xFFFFB74D).withOpacity(0.9) From dfc0a5838261f179f3b0d2c14cf3a23696d82618 Mon Sep 17 00:00:00 2001 From: xiaolefang-dm Date: Mon, 22 Jun 2026 15:47:59 +0800 Subject: [PATCH 5/5] feat(backend): guard looper navigation on VIO status --- app/backend/node_manager.py | 140 ++++++++++++++++++++++++++++++++++-- 1 file changed, 135 insertions(+), 5 deletions(-) diff --git a/app/backend/node_manager.py b/app/backend/node_manager.py index 1e99c7a3..711617a5 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), } +_VIO_STATUS_NORMAL = {'TRACKING', 'TRACKING_STATIC'} def _resize_preview_frame(arr: np.ndarray, max_edge_px: int = _PREVIEW_MAX_EDGE_PX) -> np.ndarray: @@ -197,6 +198,16 @@ def __init__(self, tinynav_db_path: str = '/tinynav/tinynav_db'): self._loc_assist_thread: threading.Thread | None = None self._loc_assist_stop_event = threading.Event() + # Insight VIO guard is only enabled for looper mode. When VIO loses + # tracking, stop nav nodes and later resume the remaining POI ids after + # VIO recovers and relocalization succeeds. + self._vio_status: str | None = None + self._vio_status_sub = None + self._vio_guard_stopped: bool = False + self._vio_guard_recovering: bool = False + self._vio_resume_poi_ids: list[int] = [] + self._active_nav_poi_ids: list[int] = [] + self._nav_progress: dict | None = None self.nav_progress_callbacks: list = [] @@ -237,6 +248,92 @@ def _on_nav_progress(self, msg: String): except json.JSONDecodeError: pass + def _on_vio_status(self, msg: String): + status = msg.data.strip().upper() + normal = status in _VIO_STATUS_NORMAL + + with self._lock: + previous_status = self._vio_status + self._vio_status = status + already_stopped = self._vio_guard_stopped + recovering = self._vio_guard_recovering + nav_running = self._nav_nodes_running + + if normal: + if already_stopped and not recovering: + self._recover_from_vio_guard_stop(status) + return + + if already_stopped or not nav_running: + return + + self._stop_for_vio_guard(status, previous_status) + + def _stop_for_vio_guard(self, status: str, previous_status: str | None): + resume_ids = self._remaining_nav_poi_ids_for_resume() + with self._lock: + self._vio_guard_stopped = True + self._vio_guard_recovering = False + self._vio_resume_poi_ids = resume_ids + self._localized = False + + self.get_logger().warn( + f'Insight VIO abnormal ({previous_status!r} -> {status!r}); ' + f'stopping nav nodes, remaining_pois={resume_ids!r}' + ) + self.cmd_stop_nav_nodes() + with self._lock: + if self.state == 'navigation': + self.state = 'idle' + self._pub_state() + + def _recover_from_vio_guard_stop(self, status: str): + with self._lock: + resume_ids = list(self._vio_resume_poi_ids) + if not self._vio_guard_stopped or self._vio_guard_recovering: + return + self._vio_guard_recovering = True + + self.get_logger().info( + f'Insight VIO recovered ({status!r}); starting nav nodes before resuming POIs={resume_ids!r}' + ) + self.cmd_start_nav_nodes() + + def _remaining_nav_poi_ids_for_resume(self) -> list[int]: + with self._lock: + poi_ids = list(self._active_nav_poi_ids) + progress = dict(self._nav_progress) if self._nav_progress else None + + if not poi_ids: + return [] + + index = 0 + if progress: + try: + index = int(progress.get('poi_index', 0)) + percent = float(progress.get('percent', 0.0)) + if percent >= 100.0: + index += 1 + except (TypeError, ValueError): + index = 0 + index = max(0, min(index, len(poi_ids))) + return poi_ids[index:] + + def _resume_vio_pois_after_localized(self): + with self._lock: + if not self._vio_guard_stopped or not self._vio_guard_recovering: + return + resume_ids = list(self._vio_resume_poi_ids) + self._vio_guard_stopped = False + self._vio_guard_recovering = False + self._vio_resume_poi_ids = [] + + if resume_ids: + self.get_logger().info(f'Resuming POIs after VIO recovery localization: {resume_ids!r}') + self.cmd_send_pois(resume_ids) + else: + self.get_logger().info('VIO recovered and localized; no remaining POIs to resume') + def _on_mapping_percent(self, msg: Float32): with self._lock: self.mapping_percent = float(msg.data) @@ -445,6 +542,12 @@ def _detect_and_init_sensor(self): self._sensor_mode = 'realsense' self.get_logger().info('Sensor mode: realsense — launching driver + perception + planning') + if self._sensor_mode == 'looper' and self._vio_status_sub is None: + self._vio_status_sub = self.create_subscription( + String, '/insight/vio_status', self._on_vio_status, 10 + ) + self.get_logger().info('Insight VIO guard enabled for looper sensor mode') + if self._sensor_mode in ('looper', 'realsense'): _env = os.environ.copy() _env['PYTHONPATH'] = _VENV_SITE + ':' + _env.get('PYTHONPATH', '') @@ -645,6 +748,9 @@ def get_status(self) -> dict: nav_paused = self._nav_paused nav_active = self._nav_active loc_assist = self._loc_assist_enabled + vio_guard_enabled = self._sensor_mode == 'looper' + vio_status = self._vio_status if vio_guard_enabled else None + vio_guard_stopped = self._vio_guard_stopped if vio_guard_enabled else False bag_files_exist = self.active_bag_path is not None map_files_exist = os.path.exists(os.path.join(self.map_path, 'occupancy_grid.npy')) return { @@ -659,6 +765,9 @@ def get_status(self) -> dict: 'navPaused': nav_paused, 'navActive': nav_active, 'locAssistEnabled': loc_assist, + 'vioGuardEnabled': vio_guard_enabled, + 'vioStatus': vio_status, + 'vioGuardStopped': vio_guard_stopped, } @staticmethod @@ -1042,10 +1151,19 @@ def _on_localization_achieved(self): nav_running = self._nav_nodes_running cmd_vel_proc = self._cmd_vel_proc if cmd_vel_proc is not None and cmd_vel_proc.poll() is None: - return - if cmd_vel_proc is not None: - self._cmd_vel_proc = None - if not loc_assist or not nav_running: + already_running = True + else: + already_running = False + if cmd_vel_proc is not None: + self._cmd_vel_proc = None + if not nav_running: + self._resume_vio_pois_after_localized() + return + if already_running: + self._resume_vio_pois_after_localized() + return + if not loc_assist: + self._resume_vio_pois_after_localized() return # Stop the sweep self._stop_loc_assist() @@ -1064,6 +1182,7 @@ def _on_localization_achieved(self): env=_env, ) self.get_logger().info('Localization achieved — cmd_vel_control started') + self._resume_vio_pois_after_localized() def cmd_bag_start(self): if self._sensor_mode == 'looper': @@ -1278,6 +1397,9 @@ def cmd_manual_target_pose(self, x: float, y: float, z: float): def cmd_send_pois(self, poi_ids: list[int]): """Publish selected POIs to map_node and transition to navigation state.""" + with self._lock: + self._active_nav_poi_ids = list(poi_ids) + self._nav_progress = None if not poi_ids: self._cmd_pois_pub.publish(String(data='{}')) self._set_nav_active(False) @@ -1309,7 +1431,11 @@ def cmd_send_pois(self, poi_ids: list[int]): def cmd_nav_start(self, poi_id: str | None = None): if poi_id is not None: - self._set_nav_active(self._publish_cmd_pois(int(poi_id))) + poi_int = int(poi_id) + with self._lock: + self._active_nav_poi_ids = [poi_int] + self._nav_progress = None + self._set_nav_active(self._publish_cmd_pois(poi_int)) else: self._set_nav_active(False) with self._lock: @@ -1326,6 +1452,10 @@ def cmd_nav_cancel(self): if self.state != 'navigation': return with self._lock: + self._active_nav_poi_ids = [] + self._vio_resume_poi_ids = [] + self._vio_guard_stopped = False + self._vio_guard_recovering = False nav_running = self._nav_nodes_running if nav_running: # Clear the active nav target so map_node stops pathing.