Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
20 commits
Select commit Hold shift + click to select a range
0a9f5ff
feat(nav): auto-restore POI button when all POIs are visited
Apr 30, 2026
91dea0d
feat(ui): auto-close POI sheet after Go succeeds
Apr 30, 2026
e99e5ac
fix(ui): properly close POI sheet after Go
Apr 30, 2026
66b07d8
feat(ui): show robot localization + active POIs + footprint on map
Apr 30, 2026
3ec41df
fix(backend): set map_pose from relocalization topic
Apr 30, 2026
c346114
fix(ui): make footprint rectangle more visible
Apr 30, 2026
d6d82d7
fix(ui): draw footprint outline on top of robot arrow
Apr 30, 2026
20ae3d7
fix(ui): fix footprint invisible due to same size as arrow
Apr 30, 2026
12289e0
fix(footprint): use actual /planning/footprint PointCloud data
Apr 30, 2026
b26f6b0
refactor: remove dead code from multi-iteration cleanup
Apr 30, 2026
9d29aaf
revert: remove footprint feature and auto-close POI sheet
Apr 30, 2026
8a0f1e7
fix(ui): show active nav POIs on PiP map after Go
Apr 30, 2026
744fd47
refactor(ui): remove POI markers from PiP map
Apr 30, 2026
ca5daa0
feat(nav): publish /mapping/nav_progress with dynamic speed estimate
Apr 30, 2026
94f7891
feat(nav): show per-POI progress bar, distance and ETA in nav status …
Apr 30, 2026
c0753db
fix(ui): use mapGlobalPath for global map overlay to match map coordi…
Apr 30, 2026
71a00aa
fix(ui): replace LinearProgressIndicator borderRadius with ClipRRect …
Apr 30, 2026
1b1cf5d
refactor(nav): stream nav_progress via dedicated /ws/nav-progress Web…
Apr 30, 2026
710513c
fix(ui): move nav progress bar to operate tab map overlay
Apr 30, 2026
e5d85ee
feat(ui): show POI name and concise progress in nav overlay
Apr 30, 2026
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
22 changes: 22 additions & 0 deletions app/backend/node_manager.py
Original file line number Diff line number Diff line change
Expand Up @@ -145,7 +145,12 @@ 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

self._nav_progress: dict | None = None
self.nav_progress_callbacks: list = []

self.create_subscription(Float32, '/battery', self._on_battery, 10)
self.create_subscription(Bool, '/mapping/nav_done', self._on_nav_done, 10)
self.create_subscription(String, '/mapping/nav_progress', self._on_nav_progress, 10)
self._detect_and_init_sensor()
self._start_unitree_if_configured()

Expand All @@ -157,6 +162,21 @@ def _on_battery(self, msg: Float32):
with self._lock:
self._battery = float(msg.data)

def _on_nav_done(self, msg: Bool):
if msg.data and self.state == 'navigation':
self.state = 'idle'
self._pub_state()

def _on_nav_progress(self, msg: String):
try:
data = json.loads(msg.data)
with self._lock:
self._nav_progress = data
for cb in self.nav_progress_callbacks:
cb(data)
except json.JSONDecodeError:
pass

def _on_mapping_percent(self, msg: Float32):
with self._lock:
self.mapping_percent = float(msg.data)
Expand Down Expand Up @@ -186,7 +206,9 @@ def _on_pose_in_map(self, msg: Odometry):
pass

def _on_relocalization(self, msg: Odometry):
pose = self._odom_to_dict(msg, source='map')
with self._lock:
self._map_pose = pose
self._localized = True

def _on_nav_target_pose(self, msg: Odometry):
Expand Down
32 changes: 32 additions & 0 deletions app/backend/ws.py
Original file line number Diff line number Diff line change
Expand Up @@ -85,6 +85,38 @@ def _on_pose(pose: dict):
pass


# --------------------------------------------------------------------------- #
# /ws/nav-progress — pushed on every /mapping/nav_progress message #
# --------------------------------------------------------------------------- #

@router.websocket('/ws/nav-progress')
async def ws_nav_progress(ws: WebSocket):
await ws.accept()
queue: asyncio.Queue = asyncio.Queue(maxsize=10)
loop = asyncio.get_event_loop()

def _on_progress(data: dict):
loop.call_soon_threadsafe(lambda: _safe_put(queue, data))

node = runner.node
if node is None:
await ws.close(code=1013)
return

node.nav_progress_callbacks.append(_on_progress)
try:
while True:
data = await queue.get()
await ws.send_text(json.dumps(data))
except WebSocketDisconnect:
pass
finally:
try:
node.nav_progress_callbacks.remove(_on_progress)
except ValueError:
pass


# --------------------------------------------------------------------------- #
# /ws/map-update — polls for occupancy_grid.npy mtime changes #
# --------------------------------------------------------------------------- #
Expand Down
24 changes: 24 additions & 0 deletions app/frontend/lib/core/models.dart
Original file line number Diff line number Diff line change
@@ -1,6 +1,30 @@
import 'dart:convert';
import 'dart:typed_data';

class NavProgress {
final int poiIndex;
final double percent;
final double pathRemainingM;
final double pathTotalM;
final double estimatedRemainingS;

const NavProgress({
required this.poiIndex,
required this.percent,
required this.pathRemainingM,
required this.pathTotalM,
required this.estimatedRemainingS,
});

factory NavProgress.fromJson(Map<String, dynamic> json) => NavProgress(
poiIndex: json['poi_index'] as int? ?? 0,
percent: (json['percent'] as num?)?.toDouble() ?? 0.0,
pathRemainingM: (json['path_remaining_m'] as num?)?.toDouble() ?? 0.0,
pathTotalM: (json['path_total_m'] as num?)?.toDouble() ?? 0.0,
estimatedRemainingS: (json['estimated_remaining_s'] as num?)?.toDouble() ?? -1.0,
);
}

class DeviceStatus {
final bool online;
final double? battery;
Expand Down
16 changes: 16 additions & 0 deletions app/frontend/lib/core/providers.dart
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,19 @@ final deviceStatusProvider = StreamProvider<DeviceStatus>((ref) {
);
});

/// Streams NavProgress from WS /ws/nav-progress (pushed on every ROS message).
final navProgressStreamProvider = StreamProvider<NavProgress>((ref) {
final ip = ref.watch(deviceIpProvider);
if (ip == null) return const Stream.empty();

final channel = WebSocketChannel.connect(Uri.parse('ws://$ip:8000/ws/nav-progress'));
ref.onDispose(() => channel.sink.close());

return channel.stream.map(
(data) => NavProgress.fromJson(jsonDecode(data as String) as Map<String, dynamic>),
);
});

/// Streams robot Pose from WS /ws/pose (pushed on every odometry message).
final poseStreamProvider = StreamProvider<Pose>((ref) {
final ip = ref.watch(deviceIpProvider);
Expand Down Expand Up @@ -98,6 +111,9 @@ final imageTopicsProvider = FutureProvider.autoDispose<List<String>>((ref) async
/// Currently selected bag name for map building (null = use last verified).
final selectedBagProvider = StateProvider<String?>((ref) => null);

/// POIs currently being navigated (set when Go is pressed, cleared when nav done).
final activeNavPoisProvider = StateProvider<List<Poi>>((ref) => const []);

/// Currently selected preview topic (null = preview closed).
final selectedPreviewTopicProvider = StateProvider<String?>((ref) => null);

Expand Down
115 changes: 88 additions & 27 deletions app/frontend/lib/pages/nav_tab.dart
Original file line number Diff line number Diff line change
Expand Up @@ -83,6 +83,20 @@ class _NavStatusCard extends ConsumerStatefulWidget {

class _NavStatusCardState extends ConsumerState<_NavStatusCard> {
bool _canceling = false;
bool _showCompletion = false;

@override
void didUpdateWidget(_NavStatusCard oldWidget) {
super.didUpdateWidget(oldWidget);
final wasNavigating = oldWidget.status.rawState == 'navigation';
final isNavigating = widget.status.rawState == 'navigation';
if (wasNavigating && !isNavigating) {
setState(() => _showCompletion = true);
Future.delayed(const Duration(milliseconds: 1200), () {
if (mounted) setState(() => _showCompletion = false);
});
}
}

Future<void> _cancel() async {
setState(() => _canceling = true);
Expand All @@ -106,39 +120,86 @@ class _NavStatusCardState extends ConsumerState<_NavStatusCard> {
Widget build(BuildContext context) {
final s = widget.status;
final isNavigating = s.rawState == 'navigation';
final np = isNavigating ? ref.watch(navProgressStreamProvider).valueOrNull : null;
final showCompletion = _showCompletion;

String subtitle;
double progressValue;
if (showCompletion) {
subtitle = 'Arrived!';
progressValue = 1.0;
} else if (isNavigating && np != null) {
progressValue = (np.percent / 100.0).clamp(0.0, 1.0);
final remaining = np.pathRemainingM < 1000 ? '${np.pathRemainingM.toStringAsFixed(1)}m' : '--';
final eta = np.estimatedRemainingS >= 0
? '~${np.estimatedRemainingS.toStringAsFixed(0)}s'
: '--';
subtitle = 'POI #${np.poiIndex + 1} · $remaining · $eta';
} else if (isNavigating) {
subtitle = 'Navigating...';
progressValue = 0.0;
} else {
subtitle = s.navStatus;
progressValue = 0.0;
}

final active = isNavigating || showCompletion;
final progressColor = showCompletion ? Colors.green : Colors.blue;

return Card(
child: Padding(
padding: const EdgeInsets.all(16),
child: Row(children: [
Icon(
isNavigating ? Icons.navigation : Icons.navigation_outlined,
color: isNavigating ? Colors.blue : Colors.grey,
size: 28,
),
const SizedBox(width: 12),
Expanded(
child: Column(crossAxisAlignment: CrossAxisAlignment.start, children: [
const Text('Navigation',
style: TextStyle(fontWeight: FontWeight.bold, fontSize: 16)),
Text(
isNavigating ? 'Navigating...' : s.navStatus,
style: TextStyle(color: isNavigating ? Colors.blue : Colors.grey),
child: Column(crossAxisAlignment: CrossAxisAlignment.start, children: [
Row(children: [
Icon(
active ? Icons.navigation : Icons.navigation_outlined,
color: active ? progressColor : Colors.grey,
size: 28,
),
const SizedBox(width: 12),
Expanded(
child: Column(crossAxisAlignment: CrossAxisAlignment.start, children: [
const Text('Navigation',
style: TextStyle(fontWeight: FontWeight.bold, fontSize: 16)),
Text(
subtitle,
style: TextStyle(color: active ? progressColor : Colors.grey),
),
]),
),
if (isNavigating)
OutlinedButton(
onPressed: _canceling ? null : _cancel,
style: OutlinedButton.styleFrom(foregroundColor: Colors.red),
child: _canceling
? const SizedBox(
width: 16,
height: 16,
child: CircularProgressIndicator(strokeWidth: 2),
)
: const Text('Cancel'),
),
]),
if (active) ...[
const SizedBox(height: 10),
ClipRRect(
borderRadius: BorderRadius.circular(3),
child: LinearProgressIndicator(
value: progressValue,
color: progressColor,
backgroundColor: progressColor.withOpacity(0.15),
minHeight: 6,
),
),
const SizedBox(height: 4),
Align(
alignment: Alignment.centerRight,
child: Text(
'${(progressValue * 100).toStringAsFixed(1)}%',
style: TextStyle(fontSize: 12, color: progressColor),
),
]),
),
if (isNavigating)
OutlinedButton(
onPressed: _canceling ? null : _cancel,
style: OutlinedButton.styleFrom(foregroundColor: Colors.red),
child: _canceling
? const SizedBox(
width: 16,
height: 16,
child: CircularProgressIndicator(strokeWidth: 2),
)
: const Text('Cancel'),
),
],
]),
),
);
Expand Down
Loading
Loading