Skip to content
Closed
Show file tree
Hide file tree
Changes from all commits
Commits
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
8 changes: 8 additions & 0 deletions app/backend/node_manager.py
Original file line number Diff line number Diff line change
Expand Up @@ -146,6 +146,7 @@ def __init__(self, tinynav_db_path: str = '/tinynav/tinynav_db'):
self._cmd_vel_proc: subprocess.Popen | None = None

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

Expand All @@ -157,6 +158,11 @@ 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_mapping_percent(self, msg: Float32):
with self._lock:
self.mapping_percent = float(msg.data)
Expand Down Expand Up @@ -186,7 +192,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
3 changes: 3 additions & 0 deletions app/frontend/lib/core/providers.dart
Original file line number Diff line number Diff line change
Expand Up @@ -98,6 +98,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
155 changes: 116 additions & 39 deletions app/frontend/lib/pages/operate_tab.dart
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,7 @@ class _OperateTabState extends ConsumerState<OperateTab> {
bool _showEsdf = true;
bool _showTrajectory = true;
bool _showGlobalPath = true;
bool _showGlobalMap = false;

@override
void initState() {
Expand Down Expand Up @@ -85,9 +86,20 @@ class _OperateTabState extends ConsumerState<OperateTab> {
@override
Widget build(BuildContext context) {
final poisAsync = ref.watch(poisProvider);
final poseAsync = ref.watch(poseStreamProvider);
final planningAsync = ref.watch(planningStreamProvider);
final planning = planningAsync.valueOrNull;
final localized = planning?.localized ?? false;
final activeNavPois = ref.watch(activeNavPoisProvider);
final mapInfo = ref.watch(mapInfoProvider).valueOrNull;
final baseUrl = ref.watch(baseUrlProvider);

ref.listen<AsyncValue<DeviceStatus>>(deviceStatusProvider, (prev, next) {
final prevState = prev?.valueOrNull?.rawState;
final nextState = next.valueOrNull?.rawState;
if (prevState == 'navigation' && nextState != 'navigation') {
ref.read(activeNavPoisProvider.notifier).state = const [];
}
});

return Column(
children: [
Expand All @@ -100,43 +112,62 @@ class _OperateTabState extends ConsumerState<OperateTab> {
child: Stack(
children: [
Positioned.fill(
child: _LocalPlanningView(
planning: planning,
showObstacle: _showObstacle,
showEsdf: _showEsdf,
showTrajectory: _showTrajectory,
showGlobalPath: _showGlobalPath,
),
child: (_showGlobalMap && localized && mapInfo != null && baseUrl != null)
? _GlobalMapView(
mapInfo: mapInfo,
baseUrl: baseUrl,
planning: planning,
pois: activeNavPois,
)
: _LocalPlanningView(
planning: planning,
showObstacle: _showObstacle,
showEsdf: _showEsdf,
showTrajectory: _showTrajectory,
showGlobalPath: _showGlobalPath,
),
),
if (planning != null)
Positioned(
top: 8,
left: 8,
child: _LocalizationChip(localized: planning.localized),
child: Row(
mainAxisSize: MainAxisSize.min,
children: [
_LocalizationChip(localized: localized),
if (localized) ...[
const SizedBox(width: 6),
_MapToggleButton(
showGlobalMap: _showGlobalMap,
onTap: () => setState(() => _showGlobalMap = !_showGlobalMap),
),
],
],
),
),
Positioned(
top: 8,
right: 8,
child: _LayerTogglePanel(
showObstacle: _showObstacle,
showEsdf: _showEsdf,
showTrajectory: _showTrajectory,
showGlobalPath: _showGlobalPath,
onChanged: (obs, esdf, traj, gp) => setState(() {
_showObstacle = obs;
_showEsdf = esdf;
_showTrajectory = traj;
_showGlobalPath = gp;
}),
if (!_showGlobalMap)
Positioned(
top: 8,
right: 8,
child: _LayerTogglePanel(
showObstacle: _showObstacle,
showEsdf: _showEsdf,
showTrajectory: _showTrajectory,
showGlobalPath: _showGlobalPath,
onChanged: (obs, esdf, traj, gp) => setState(() {
_showObstacle = obs;
_showEsdf = esdf;
_showTrajectory = traj;
_showGlobalPath = gp;
}),
),
),
),
Positioned(
bottom: 10,
left: 10,
child: _PoiButton(
poisAsync: poisAsync,
statusAsync: ref.watch(deviceStatusProvider),
pose: poseAsync.valueOrNull,
),
),
Positioned(
Expand Down Expand Up @@ -414,6 +445,46 @@ class _LayerRow extends StatelessWidget {
}
}

// ── Map toggle button ─────────────────────────────────────────────────────────

class _MapToggleButton extends StatelessWidget {
final bool showGlobalMap;
final VoidCallback onTap;

const _MapToggleButton({required this.showGlobalMap, required this.onTap});

@override
Widget build(BuildContext context) {
return GestureDetector(
onTap: onTap,
child: Container(
padding: const EdgeInsets.symmetric(horizontal: 10, vertical: 6),
decoration: BoxDecoration(
color: showGlobalMap
? Colors.blueAccent.withOpacity(0.85)
: Colors.black54,
borderRadius: BorderRadius.circular(20),
),
child: Row(
mainAxisSize: MainAxisSize.min,
children: [
Icon(
showGlobalMap ? Icons.map_rounded : Icons.grid_view_rounded,
color: Colors.white,
size: 14,
),
const SizedBox(width: 4),
Text(
showGlobalMap ? 'Global' : 'Local',
style: const TextStyle(color: Colors.white, fontSize: 12),
),
],
),
),
);
}
}

// ── Localization chip ─────────────────────────────────────────────────────────

class _LocalizationChip extends StatelessWidget {
Expand Down Expand Up @@ -452,12 +523,10 @@ class _LocalizationChip extends StatelessWidget {
class _PoiButton extends ConsumerStatefulWidget {
final AsyncValue<List<Poi>> poisAsync;
final AsyncValue<DeviceStatus> statusAsync;
final Pose? pose;

const _PoiButton({
required this.poisAsync,
required this.statusAsync,
this.pose,
});

@override
Expand Down Expand Up @@ -510,7 +579,7 @@ class _PoiButtonState extends ConsumerState<_PoiButton> {
shape: const RoundedRectangleBorder(
borderRadius: BorderRadius.vertical(top: Radius.circular(20)),
),
builder: (_) => _PoiSheet(pose: widget.pose),
builder: (_) => const _PoiSheet(),
),
style: FilledButton.styleFrom(
backgroundColor: Colors.black87,
Expand All @@ -524,8 +593,7 @@ class _PoiButtonState extends ConsumerState<_PoiButton> {
}

class _PoiSheet extends ConsumerStatefulWidget {
final Pose? pose;
const _PoiSheet({this.pose});
const _PoiSheet();

@override
ConsumerState<_PoiSheet> createState() => _PoiSheetState();
Expand Down Expand Up @@ -566,13 +634,14 @@ class _PoiSheetState extends ConsumerState<_PoiSheet> {
}

Future<void> _startNav(List<Poi> pois) async {
final ids = pois
.where((p) => _checkedIds.contains(p.id))
.map((p) => p.id)
.toList();
if (ids.isEmpty) return;
final selectedPois = pois.where((p) => _checkedIds.contains(p.id)).toList();
if (selectedPois.isEmpty) return;
try {
await ref.read(dioProvider).post('/nav/send-pois', data: {'poi_ids': ids});
await ref.read(dioProvider).post(
'/nav/send-pois',
data: {'poi_ids': selectedPois.map((p) => p.id).toList()},
);
ref.read(activeNavPoisProvider.notifier).state = selectedPois;
} on DioException catch (e) {
if (mounted) {
ScaffoldMessenger.of(context).showSnackBar(SnackBar(
Expand Down Expand Up @@ -615,7 +684,7 @@ class _PoiSheetState extends ConsumerState<_PoiSheet> {
const Spacer(),
FilledButton.icon(
onPressed: (canGo && _checkedIds.isNotEmpty)
? () => poisAsync.whenData((pois) => _startNav(pois))
? () => _startNav(poisAsync.valueOrNull ?? [])
: null,
icon: const Icon(Icons.navigation_rounded, size: 16),
label: const Text('Go'),
Expand Down Expand Up @@ -891,7 +960,11 @@ class _CameraPanelState extends ConsumerState<_CameraPanel> {
planning.localized && baseUrl != null)
Positioned(
top: 8, left: 8,
child: _MapPip(mapInfo: mapInfo, planning: planning, baseUrl: baseUrl),
child: _MapPip(
mapInfo: mapInfo,
planning: planning,
baseUrl: baseUrl,
),
),
// ── Topic selector ───────────────────────────────────────────
Positioned(
Expand Down Expand Up @@ -1013,7 +1086,11 @@ class _MapPip extends StatelessWidget {
final PlanningState planning;
final String baseUrl;

const _MapPip({required this.mapInfo, required this.planning, required this.baseUrl});
const _MapPip({
required this.mapInfo,
required this.planning,
required this.baseUrl,
});

@override
Widget build(BuildContext context) {
Expand Down
8 changes: 7 additions & 1 deletion tinynav/core/map_node.py
Original file line number Diff line number Diff line change
Expand Up @@ -249,9 +249,11 @@ def __init__(self, tinynav_db_path: str, tinynav_map_path: str, verbose_timer: b

self.pois = {}
self.poi_index = -1
self._nav_completed = False

self.poi_pub = self.create_publisher(Odometry, "/mapping/poi", 10)
self.poi_change_pub = self.create_publisher(Odometry, "/mapping/poi_change", 10)
self.nav_done_pub = self.create_publisher(Bool, '/mapping/nav_done', 10)

self.current_pose_pub = self.create_publisher(Odometry, "/mapping/current_pose", 10)
self.global_plan_pub = self.create_publisher(Path, '/mapping/global_plan', 10)
Expand Down Expand Up @@ -281,6 +283,7 @@ def pois_callback(self, msg: String):
return

self.poi_index = min(0, len(self.pois) - 1)
self._nav_completed = False
self.get_logger().info(f"Parsed POIs: {self.pois}")
except json.JSONDecodeError as e:
self.get_logger().error(f"Failed to parse POIs JSON: {e}")
Expand Down Expand Up @@ -613,7 +616,10 @@ def try_publish_nav_path(self, timestamp: int):
break

if self.poi_index >= len(self.pois):
self.get_logger().info("All POIs have been visited, skip publishing nav path")
if not self._nav_completed:
self._nav_completed = True
self.get_logger().info("All POIs have been visited, nav done")
self.nav_done_pub.publish(Bool(data=True))
return

target_poi = self.pois[self.poi_index]
Expand Down
Loading