From 032a9845d3fdc1c562069c5d09e4b4fc46614656 Mon Sep 17 00:00:00 2001 From: Xiaole Fang Date: Sun, 28 Jun 2026 12:14:47 +0800 Subject: [PATCH 1/7] feat(nav): add manual target stop control --- app/backend/node_manager.py | 47 ++++++++++++++++++-- app/backend/routers/nav.py | 20 ++++++++- app/frontend/lib/core/models.dart | 3 ++ app/frontend/lib/pages/operate_tab.dart | 59 ++++++++++++++++++++++++- app/frontend/web/flutter_bootstrap.js | 4 +- tinynav/core/planning_node.py | 11 ++++- 6 files changed, 134 insertions(+), 10 deletions(-) diff --git a/app/backend/node_manager.py b/app/backend/node_manager.py index 777d3468..5dfecfd9 100644 --- a/app/backend/node_manager.py +++ b/app/backend/node_manager.py @@ -162,6 +162,7 @@ def __init__(self, tinynav_db_path: str = '/tinynav/tinynav_db'): self._nav_active_pub = self.create_publisher(Bool, '/nav/active', _latched_qos) self._nav_paused = False self._nav_active = False + self._manual_target_active = False # Publisher for robot action commands (sit / stand) self._action_pub = self.create_publisher(String, '/service/command', 10) @@ -266,10 +267,16 @@ def _on_relocalization(self, msg: Odometry): self._localized = True def _on_nav_target_pose(self, msg: Odometry): + x = float(msg.pose.pose.position.x) + y = float(msg.pose.pose.position.y) + if not (math.isfinite(x) and math.isfinite(y)): + with self._lock: + self._nav_target_pose = None + return with self._lock: self._nav_target_pose = { - 'x': msg.pose.pose.position.x, - 'y': msg.pose.pose.position.y, + 'x': x, + 'y': y, } def _on_height_map(self, msg: Image): @@ -631,6 +638,7 @@ def get_status(self) -> dict: nav_nodes = self._nav_nodes_running nav_paused = self._nav_paused nav_active = self._nav_active + manual_target_active = self._manual_target_active 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 +652,7 @@ def get_status(self) -> dict: 'navNodesRunning': nav_nodes, 'navPaused': nav_paused, 'navActive': nav_active, + 'manualTargetActive': manual_target_active, } @staticmethod @@ -771,6 +780,7 @@ def cmd_stop_nav_nodes(self): self._global_path = [] self._nav_target_pose = None self._nav_paused = False + self._manual_target_active = False self.get_logger().info('Nav nodes stopped') def cmd_restart_nav_nodes(self): @@ -1005,11 +1015,13 @@ def _publish_cmd_pois(self, poi_id: int | None) -> bool: self._cmd_pois_pub.publish(String(data=json.dumps(payload))) return True - def cmd_manual_target_pose(self, x: float, y: float, z: float): + def cmd_manual_target_pose(self, x: float, y: float, z: float, activate: bool = True): """Publish a manually selected local-planner target pose. planning_node subscribes to /control/target_pose and only reads the position vector, so Odometry is used here to match that existing API. + When requested, mark navigation active so cmd_vel_control consumes the + resulting /planning/trajectory_path and publishes /cmd_vel. """ msg = Odometry() msg.header.stamp = self.get_clock().now().to_msg() @@ -1021,6 +1033,32 @@ def cmd_manual_target_pose(self, x: float, y: float, z: float): self._target_pose_pub.publish(msg) with self._lock: self._nav_target_pose = {'x': float(x), 'y': float(y)} + nav_running = self._nav_nodes_running + if activate and nav_running: + with self._lock: + self._manual_target_active = True + self._set_nav_active(True) + self.state = 'navigation' + self._pub_state() + + def cmd_clear_manual_target_pose(self): + """Clear the manually selected local-planner target pose.""" + msg = Odometry() + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = 'odom' + msg.pose.pose.position.x = math.nan + msg.pose.pose.position.y = math.nan + msg.pose.pose.position.z = math.nan + msg.pose.pose.orientation.w = 1.0 + self._target_pose_pub.publish(msg) + self._set_nav_active(False) + with self._lock: + self._nav_target_pose = None + self._manual_target_active = False + nav_running = self._nav_nodes_running + if nav_running and self.state == 'navigation': + self.state = 'idle' + self._pub_state() def cmd_send_pois(self, poi_ids: list[int]): """Publish selected POIs to map_node and transition to navigation state.""" @@ -1045,6 +1083,7 @@ def cmd_send_pois(self, poi_ids: list[int]): self._cmd_pois_pub.publish(String(data=json.dumps(payload))) self._set_nav_active(bool(payload)) with self._lock: + self._manual_target_active = False nav_running = self._nav_nodes_running if nav_running: self.state = 'navigation' @@ -1054,6 +1093,8 @@ def cmd_send_pois(self, poi_ids: list[int]): self._start('navigation') def cmd_nav_start(self, poi_id: str | None = None): + with self._lock: + self._manual_target_active = False if poi_id is not None: self._set_nav_active(self._publish_cmd_pois(int(poi_id))) else: diff --git a/app/backend/routers/nav.py b/app/backend/routers/nav.py index 62aa49c6..e872acab 100644 --- a/app/backend/routers/nav.py +++ b/app/backend/routers/nav.py @@ -24,6 +24,7 @@ class ManualTargetRequest(BaseModel): x: float y: float z: float + activate: bool = True @router.post('/send-pois') @@ -51,8 +52,23 @@ def nav_manual_target(req: ManualTargetRequest): node = _require_node() if node._odom_pose is None: raise HTTPException(409, 'Odometry not ready') - node.cmd_manual_target_pose(req.x, req.y, req.z) - return {'ok': True, 'target': {'x': req.x, 'y': req.y, 'z': req.z}} + if node.state == 'navigation' and not node._manual_target_active: + raise HTTPException(409, 'Manual target is disabled while POI navigation is running') + if req.activate and not node._nav_nodes_running: + raise HTTPException(409, 'Nav nodes not running') + node.cmd_manual_target_pose(req.x, req.y, req.z, activate=req.activate) + return { + 'ok': True, + 'target': {'x': req.x, 'y': req.y, 'z': req.z}, + 'active': req.activate, + } + + +@router.post('/manual-target/clear') +def nav_manual_target_clear(): + node = _require_node() + node.cmd_clear_manual_target_pose() + return {'ok': True} @router.post('/cancel') diff --git a/app/frontend/lib/core/models.dart b/app/frontend/lib/core/models.dart index 9bbd7a0e..2ca8c262 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 manualTargetActive; const DeviceStatus({ required this.online, @@ -48,6 +49,7 @@ class DeviceStatus { required this.rawState, required this.navNodesRunning, required this.navPaused, + required this.manualTargetActive, }); 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, + manualTargetActive: json['manualTargetActive'] as bool? ?? false, ); } diff --git a/app/frontend/lib/pages/operate_tab.dart b/app/frontend/lib/pages/operate_tab.dart index 8b7e3487..e39ee9b1 100644 --- a/app/frontend/lib/pages/operate_tab.dart +++ b/app/frontend/lib/pages/operate_tab.dart @@ -132,6 +132,8 @@ class _OperateTabState extends ConsumerState { final status = ref.watch(deviceStatusProvider).valueOrNull; final isNavigating = status?.rawState == 'navigation'; + final manualTargetDisabled = + status?.rawState == 'navigation' && status?.manualTargetActive != true; final np = isNavigating ? ref.watch(navProgressStreamProvider).valueOrNull : null; return Column( @@ -161,6 +163,8 @@ class _OperateTabState extends ConsumerState { showFootprint: _showFootprint, fillViewport: _localMapFill, show3d: _showLocal3d, + activateManualTarget: status?.navNodesRunning == true, + manualTargetDisabled: manualTargetDisabled, ), ), if (planning != null) @@ -336,6 +340,8 @@ class _LocalPlanningView extends ConsumerStatefulWidget { final bool showFootprint; final bool fillViewport; final bool show3d; + final bool activateManualTarget; + final bool manualTargetDisabled; const _LocalPlanningView({ this.planning, @@ -346,6 +352,8 @@ class _LocalPlanningView extends ConsumerStatefulWidget { this.showFootprint = true, this.fillViewport = false, this.show3d = false, + this.activateManualTarget = false, + this.manualTargetDisabled = false, }); @override @@ -444,6 +452,14 @@ class _LocalPlanningViewState extends ConsumerState<_LocalPlanningView> { } Future _handleLongPress(Offset localPos, Size viewportSize) async { + if (widget.manualTargetDisabled) { + ScaffoldMessenger.of(context).showSnackBar( + const SnackBar( + content: Text('Manual target is disabled during POI navigation'), + ), + ); + return; + } final target = _targetFromLocalPosition(localPos, viewportSize); if (target == null || !mounted) return; setState(() => _pendingTarget = target); @@ -479,10 +495,15 @@ class _LocalPlanningViewState extends ConsumerState<_LocalPlanningView> { 'x': target.x, 'y': target.y, 'z': target.z, + 'activate': widget.activateManualTarget, }); if (mounted) { ScaffoldMessenger.of(context).showSnackBar( - const SnackBar(content: Text('Manual target published')), + SnackBar( + content: Text(widget.activateManualTarget + ? 'Manual target published and navigation activated' + : 'Manual target published to planning only'), + ), ); } } catch (e) { @@ -496,10 +517,27 @@ class _LocalPlanningViewState extends ConsumerState<_LocalPlanningView> { if (mounted) setState(() => _pendingTarget = null); } + Future _clearManualTarget() async { + try { + await ref.read(dioProvider).post('/nav/manual-target/clear'); + if (!mounted) return; + setState(() => _pendingTarget = null); + ScaffoldMessenger.of(context).showSnackBar( + const SnackBar(content: Text('Manual target stopped')), + ); + } catch (e) { + if (!mounted) return; + ScaffoldMessenger.of(context).showSnackBar( + SnackBar(content: Text('Failed to stop target: $e')), + ); + } + } + @override Widget build(BuildContext context) { final p = widget.planning; final gi = p?.gridInfo; + final hasManualTarget = _pendingTarget != null || p?.navTargetPose != null; final localAspectRatio = (gi != null && gi.height > 0) ? gi.width / gi.height : 1.0; @@ -668,6 +706,25 @@ class _LocalPlanningViewState extends ConsumerState<_LocalPlanningView> { ), ), ), + if (hasManualTarget) + Positioned( + top: 14, + left: 0, + right: 0, + child: Center( + child: FilledButton.icon( + style: FilledButton.styleFrom( + backgroundColor: const Color(0xFFD32F2F), + foregroundColor: Colors.white, + padding: + const EdgeInsets.symmetric(horizontal: 12, vertical: 8), + ), + onPressed: _clearManualTarget, + icon: const Icon(Icons.stop_circle_outlined, size: 18), + label: const Text('Stop Target'), + ), + ), + ), ], ); } diff --git a/app/frontend/web/flutter_bootstrap.js b/app/frontend/web/flutter_bootstrap.js index 2714bd2b..74d931ac 100644 --- a/app/frontend/web/flutter_bootstrap.js +++ b/app/frontend/web/flutter_bootstrap.js @@ -5,9 +5,7 @@ _flutter.loader.load({ config: { // Keep Flutter CanvasKit/skwasm resources on the robot web server. // Without this, Flutter may fetch CanvasKit from www.gstatic.com at runtime. - canvasKitBaseUrl: "canvaskit/", - // Keep Flutter fallback font fetches local instead of fonts.gstatic.com. - fontFallbackBaseUrl: "fonts/" + canvasKitBaseUrl: "canvaskit/" }, serviceWorkerSettings: { serviceWorkerVersion: {{flutter_service_worker_version}} diff --git a/tinynav/core/planning_node.py b/tinynav/core/planning_node.py index 0ed60068..937f2489 100644 --- a/tinynav/core/planning_node.py +++ b/tinynav/core/planning_node.py @@ -396,7 +396,16 @@ def poi_change_callback(self, msg): self.target_pose = None def target_pose_callback(self, msg): - self.target_pose = np.array([msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z]) + target_pose = np.array([ + msg.pose.pose.position.x, + msg.pose.pose.position.y, + msg.pose.pose.position.z, + ]) + if not np.all(np.isfinite(target_pose)): + self.target_pose = None + self.get_logger().info("Manual target pose cleared.") + return + self.target_pose = target_pose def info_callback(self, msg): if self.K is None: From 7abfb609532c089aa0d743c77b75001bc9e81454 Mon Sep 17 00:00:00 2001 From: xiaole-dm Date: Mon, 29 Jun 2026 09:09:48 +0800 Subject: [PATCH 2/7] fix(nav): activate manual target control path --- app/backend/node_manager.py | 32 +++++++++++++++++++++++++ app/backend/routers/nav.py | 2 -- app/frontend/lib/pages/operate_tab.dart | 16 ++++++------- 3 files changed, 39 insertions(+), 11 deletions(-) diff --git a/app/backend/node_manager.py b/app/backend/node_manager.py index 5dfecfd9..7adcd307 100644 --- a/app/backend/node_manager.py +++ b/app/backend/node_manager.py @@ -702,6 +702,34 @@ def _launch_proc(self, name: str, cmd: list[str], env: dict | None = None, lf.close() return proc + @staticmethod + def _proc_running(proc: subprocess.Popen | None) -> bool: + return proc is not None and proc.poll() is None + + def _ensure_unitree_running(self): + if self._proc_running(self._unitree_proc): + return + self._start_unitree_if_configured() + + def _ensure_planning_running(self): + if self._proc_running(self._planning_proc) or self._proc_running(self.processes.get('planning')): + return + _env = os.environ.copy() + _env['PYTHONPATH'] = _VENV_SITE + ':' + _env.get('PYTHONPATH', '') + self._planning_proc = self._launch_proc( + 'planning', + ['uv', 'run', 'python', '/tinynav/tinynav/core/planning_node.py'], + env=_env, + ) + + def _ensure_nav_nodes_running(self): + if self._proc_running(self._map_node_proc) and self._proc_running(self._cmd_vel_proc): + with self._lock: + self._nav_nodes_running = True + return + self.cmd_stop_nav_nodes() + self.cmd_start_nav_nodes() + def _stop_sensor_procs(self): for attr in ('_looper_bridge_proc', '_realsense_proc', '_perception_proc', '_planning_proc'): self._kill_proc(getattr(self, attr)) @@ -1023,6 +1051,10 @@ def cmd_manual_target_pose(self, x: float, y: float, z: float, activate: bool = When requested, mark navigation active so cmd_vel_control consumes the resulting /planning/trajectory_path and publishes /cmd_vel. """ + if activate: + self._ensure_unitree_running() + self._ensure_planning_running() + self._ensure_nav_nodes_running() msg = Odometry() msg.header.stamp = self.get_clock().now().to_msg() msg.header.frame_id = 'odom' diff --git a/app/backend/routers/nav.py b/app/backend/routers/nav.py index e872acab..e4b9509c 100644 --- a/app/backend/routers/nav.py +++ b/app/backend/routers/nav.py @@ -54,8 +54,6 @@ def nav_manual_target(req: ManualTargetRequest): raise HTTPException(409, 'Odometry not ready') if node.state == 'navigation' and not node._manual_target_active: raise HTTPException(409, 'Manual target is disabled while POI navigation is running') - if req.activate and not node._nav_nodes_running: - raise HTTPException(409, 'Nav nodes not running') node.cmd_manual_target_pose(req.x, req.y, req.z, activate=req.activate) return { 'ok': True, diff --git a/app/frontend/lib/pages/operate_tab.dart b/app/frontend/lib/pages/operate_tab.dart index e39ee9b1..151f2c29 100644 --- a/app/frontend/lib/pages/operate_tab.dart +++ b/app/frontend/lib/pages/operate_tab.dart @@ -163,7 +163,7 @@ class _OperateTabState extends ConsumerState { showFootprint: _showFootprint, fillViewport: _localMapFill, show3d: _showLocal3d, - activateManualTarget: status?.navNodesRunning == true, + manualTargetActive: status?.manualTargetActive == true, manualTargetDisabled: manualTargetDisabled, ), ), @@ -340,7 +340,7 @@ class _LocalPlanningView extends ConsumerStatefulWidget { final bool showFootprint; final bool fillViewport; final bool show3d; - final bool activateManualTarget; + final bool manualTargetActive; final bool manualTargetDisabled; const _LocalPlanningView({ @@ -352,7 +352,7 @@ class _LocalPlanningView extends ConsumerStatefulWidget { this.showFootprint = true, this.fillViewport = false, this.show3d = false, - this.activateManualTarget = false, + this.manualTargetActive = false, this.manualTargetDisabled = false, }); @@ -495,14 +495,12 @@ class _LocalPlanningViewState extends ConsumerState<_LocalPlanningView> { 'x': target.x, 'y': target.y, 'z': target.z, - 'activate': widget.activateManualTarget, + 'activate': true, }); if (mounted) { ScaffoldMessenger.of(context).showSnackBar( - SnackBar( - content: Text(widget.activateManualTarget - ? 'Manual target published and navigation activated' - : 'Manual target published to planning only'), + const SnackBar( + content: Text('Manual target published and navigation activated'), ), ); } @@ -537,7 +535,7 @@ class _LocalPlanningViewState extends ConsumerState<_LocalPlanningView> { Widget build(BuildContext context) { final p = widget.planning; final gi = p?.gridInfo; - final hasManualTarget = _pendingTarget != null || p?.navTargetPose != null; + final hasManualTarget = _pendingTarget != null || widget.manualTargetActive; final localAspectRatio = (gi != null && gi.height > 0) ? gi.width / gi.height : 1.0; From c793098ddf2257b4dc5fa3e4032eab3f95ba6555 Mon Sep 17 00:00:00 2001 From: xiaole-dm Date: Mon, 29 Jun 2026 09:54:38 +0800 Subject: [PATCH 3/7] fix(nav): feed visual odom to cmd vel control --- app/backend/node_manager.py | 21 +++++++++++++++++++++ app/backend/routers/nav.py | 5 ++++- tinynav/platforms/cmd_vel_control.py | 3 +++ 3 files changed, 28 insertions(+), 1 deletion(-) diff --git a/app/backend/node_manager.py b/app/backend/node_manager.py index 7adcd307..35ab39ed 100644 --- a/app/backend/node_manager.py +++ b/app/backend/node_manager.py @@ -191,6 +191,7 @@ def __init__(self, tinynav_db_path: str = '/tinynav/tinynav_db'): self._nav_nodes_running: bool = False self._map_node_proc: subprocess.Popen | None = None self._cmd_vel_proc: subprocess.Popen | None = None + self._proc_log_paths: dict[str, str] = {} self._nav_progress: dict | None = None self.nav_progress_callbacks: list = [] @@ -639,6 +640,12 @@ def get_status(self) -> dict: nav_paused = self._nav_paused nav_active = self._nav_active manual_target_active = self._manual_target_active + planning_running = ( + self._proc_running(self._planning_proc) + or self._proc_running(self.processes.get('planning')) + ) + cmd_vel_running = self._proc_running(self._cmd_vel_proc) + unitree_running = self._proc_running(self._unitree_proc) 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 { @@ -653,6 +660,9 @@ def get_status(self) -> dict: 'navPaused': nav_paused, 'navActive': nav_active, 'manualTargetActive': manual_target_active, + 'planningRunning': planning_running, + 'cmdVelControlRunning': cmd_vel_running, + 'unitreeControlRunning': unitree_running, } @staticmethod @@ -688,6 +698,7 @@ def _make_log(self, name: str): os.makedirs(logs_dir, exist_ok=True) ts = datetime.now().strftime('%Y_%m_%d_%H_%M_%S') path = os.path.join(logs_dir, f'{ts}_{name}.txt') + self._proc_log_paths[name] = path return open(path, 'w') def _launch_proc(self, name: str, cmd: list[str], env: dict | None = None, @@ -706,6 +717,14 @@ def _launch_proc(self, name: str, cmd: list[str], env: dict | None = None, def _proc_running(proc: subprocess.Popen | None) -> bool: return proc is not None and proc.poll() is None + def _require_proc_running(self, name: str, proc: subprocess.Popen | None): + time.sleep(0.2) + if self._proc_running(proc): + return + log_path = self._proc_log_paths.get(name) + suffix = f'; see {log_path}' if log_path else '' + raise RuntimeError(f'{name} failed to start{suffix}') + def _ensure_unitree_running(self): if self._proc_running(self._unitree_proc): return @@ -721,6 +740,7 @@ def _ensure_planning_running(self): ['uv', 'run', 'python', '/tinynav/tinynav/core/planning_node.py'], env=_env, ) + self._require_proc_running('planning', self._planning_proc) def _ensure_nav_nodes_running(self): if self._proc_running(self._map_node_proc) and self._proc_running(self._cmd_vel_proc): @@ -729,6 +749,7 @@ def _ensure_nav_nodes_running(self): return self.cmd_stop_nav_nodes() self.cmd_start_nav_nodes() + self._require_proc_running('cmd_vel_control', self._cmd_vel_proc) def _stop_sensor_procs(self): for attr in ('_looper_bridge_proc', '_realsense_proc', '_perception_proc', '_planning_proc'): diff --git a/app/backend/routers/nav.py b/app/backend/routers/nav.py index e4b9509c..b7608e53 100644 --- a/app/backend/routers/nav.py +++ b/app/backend/routers/nav.py @@ -54,7 +54,10 @@ def nav_manual_target(req: ManualTargetRequest): raise HTTPException(409, 'Odometry not ready') if node.state == 'navigation' and not node._manual_target_active: raise HTTPException(409, 'Manual target is disabled while POI navigation is running') - node.cmd_manual_target_pose(req.x, req.y, req.z, activate=req.activate) + try: + node.cmd_manual_target_pose(req.x, req.y, req.z, activate=req.activate) + except RuntimeError as exc: + raise HTTPException(409, str(exc)) from exc return { 'ok': True, 'target': {'x': req.x, 'y': req.y, 'z': req.z}, diff --git a/tinynav/platforms/cmd_vel_control.py b/tinynav/platforms/cmd_vel_control.py index 9fde958c..4e36e0fb 100644 --- a/tinynav/platforms/cmd_vel_control.py +++ b/tinynav/platforms/cmd_vel_control.py @@ -19,6 +19,9 @@ def __init__(self): self.logger = self.get_logger() # Use ROS2 logger self.cmd_pub = self.create_publisher(Twist, '/cmd_vel', 10) self.pose_sub = self.create_subscription(Odometry, '/slam/odometry', self.pose_callback, 10) + self.pose_visual_sub = self.create_subscription( + Odometry, '/slam/odometry_visual', self.pose_callback, 10 + ) self.create_subscription(Path, '/planning/trajectory_path', self.path_callback, 10) self.T_robot_to_camera = np.array([ [0, -1, 0, 0], From 8c224f1a5fcb8f18d5501bfd119e35fc55097a7c Mon Sep 17 00:00:00 2001 From: xiaole-dm Date: Mon, 29 Jun 2026 13:22:43 +0800 Subject: [PATCH 4/7] fix(nav): clear paused state before target control --- app/backend/node_manager.py | 22 +++++++++++++++------- 1 file changed, 15 insertions(+), 7 deletions(-) diff --git a/app/backend/node_manager.py b/app/backend/node_manager.py index 35ab39ed..13a139c4 100644 --- a/app/backend/node_manager.py +++ b/app/backend/node_manager.py @@ -196,6 +196,7 @@ def __init__(self, tinynav_db_path: str = '/tinynav/tinynav_db'): self._nav_progress: dict | None = None self.nav_progress_callbacks: list = [] + self._pause_pub.publish(Bool(data=False)) self._nav_active_pub.publish(Bool(data=False)) self.create_subscription(Float32, '/battery', self._on_battery, 10) @@ -217,6 +218,11 @@ def _set_nav_active(self, active: bool): self._nav_active = bool(active) self._nav_active_pub.publish(Bool(data=bool(active))) + def _set_nav_paused(self, paused: bool): + with self._lock: + self._nav_paused = bool(paused) + self._pause_pub.publish(Bool(data=bool(paused))) + def _on_nav_done(self, msg: Bool): if msg.data and self.state == 'navigation': self._set_nav_active(False) @@ -796,6 +802,7 @@ def _restart_sensor_procs(self): # ------------------------------------------------------------------ # def cmd_start_nav_nodes(self): + self._set_nav_paused(False) self._set_nav_active(False) _env = os.environ.copy() _env['PYTHONPATH'] = _VENV_SITE + ':' + _env.get('PYTHONPATH', '') @@ -818,6 +825,7 @@ def cmd_start_nav_nodes(self): def cmd_stop_nav_nodes(self): self._set_nav_active(False) + self._set_nav_paused(False) self._kill_proc(self._map_node_proc) self._kill_proc(self._cmd_vel_proc) self._map_node_proc = None @@ -828,11 +836,11 @@ def cmd_stop_nav_nodes(self): self._map_pose = None self._global_path = [] self._nav_target_pose = None - self._nav_paused = False self._manual_target_active = False self.get_logger().info('Nav nodes stopped') def cmd_restart_nav_nodes(self): + self._set_nav_paused(False) self._set_nav_active(False) self._kill_proc(self._map_node_proc) self._kill_proc(self._planning_proc) @@ -1076,6 +1084,7 @@ def cmd_manual_target_pose(self, x: float, y: float, z: float, activate: bool = self._ensure_unitree_running() self._ensure_planning_running() self._ensure_nav_nodes_running() + self._set_nav_paused(False) msg = Odometry() msg.header.stamp = self.get_clock().now().to_msg() msg.header.frame_id = 'odom' @@ -1105,6 +1114,7 @@ def cmd_clear_manual_target_pose(self): msg.pose.pose.orientation.w = 1.0 self._target_pose_pub.publish(msg) self._set_nav_active(False) + self._set_nav_paused(False) with self._lock: self._nav_target_pose = None self._manual_target_active = False @@ -1115,6 +1125,7 @@ def cmd_clear_manual_target_pose(self): def cmd_send_pois(self, poi_ids: list[int]): """Publish selected POIs to map_node and transition to navigation state.""" + self._set_nav_paused(False) if not poi_ids: self._cmd_pois_pub.publish(String(data='{}')) self._set_nav_active(False) @@ -1146,6 +1157,7 @@ def cmd_send_pois(self, poi_ids: list[int]): self._start('navigation') def cmd_nav_start(self, poi_id: str | None = None): + self._set_nav_paused(False) with self._lock: self._manual_target_active = False if poi_id is not None: @@ -1177,14 +1189,10 @@ def cmd_nav_cancel(self): self._stop_all() def cmd_nav_pause(self): - with self._lock: - self._nav_paused = True - self._pause_pub.publish(Bool(data=True)) + self._set_nav_paused(True) def cmd_nav_resume(self): - with self._lock: - self._nav_paused = False - self._pause_pub.publish(Bool(data=False)) + self._set_nav_paused(False) def cmd_action(self, action: str): self._action_pub.publish(String(data=f'play {action}')) From e58d06caf41ad55a4ce88e64598f71d9657d625f Mon Sep 17 00:00:00 2001 From: xiaole-dm Date: Tue, 30 Jun 2026 08:54:40 +0800 Subject: [PATCH 5/7] feat(nav): add manual target latency tracing --- app/backend/node_manager.py | 28 ++++ tinynav/core/latency_trace.py | 78 +++++++++++ tinynav/core/planning_node.py | 50 +++++++ tinynav/platforms/cmd_vel_control.py | 48 ++++++- tinynav/platforms/unitree_control.py | 39 ++++++ tool/latency_trace_recorder.py | 199 +++++++++++++++++++++++++++ 6 files changed, 441 insertions(+), 1 deletion(-) create mode 100644 tinynav/core/latency_trace.py create mode 100644 tool/latency_trace_recorder.py diff --git a/app/backend/node_manager.py b/app/backend/node_manager.py index 13a139c4..79db90bd 100644 --- a/app/backend/node_manager.py +++ b/app/backend/node_manager.py @@ -29,6 +29,12 @@ from sensor_msgs.msg import CompressedImage, Image, PointCloud, PointCloud2 from std_msgs.msg import Bool, Float32, String +from tinynav.core.latency_trace import ( + encode_trace_frame, + make_trace_id, + make_trace_publisher, + publish_trace, +) from tool.ros2_node_manager import Ros2NodeManager _REALSENSE_SCRIPT = '/tinynav/scripts/run_realsense_sensor.sh' @@ -155,6 +161,7 @@ def __init__(self, tinynav_db_path: str = '/tinynav/tinynav_db'): # Manual local target for planning_node, used by the operate tab long-press tool. self._target_pose_pub = self.create_publisher(Odometry, '/control/target_pose', 10) + self._latency_trace_pub = make_trace_publisher(self) # Latched publisher — new subscribers (cmd_vel_control) get current state immediately on connect _latched_qos = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL) @@ -1085,14 +1092,35 @@ def cmd_manual_target_pose(self, x: float, y: float, z: float, activate: bool = self._ensure_planning_running() self._ensure_nav_nodes_running() self._set_nav_paused(False) + trace_id = make_trace_id() + publish_trace( + self, + self._latency_trace_pub, + trace_id, + "backend", + "manual_target_requested", + x=float(x), + y=float(y), + z=float(z), + activate=bool(activate), + ) msg = Odometry() msg.header.stamp = self.get_clock().now().to_msg() msg.header.frame_id = 'odom' + msg.child_frame_id = encode_trace_frame(trace_id) msg.pose.pose.position.x = float(x) msg.pose.pose.position.y = float(y) msg.pose.pose.position.z = float(z) msg.pose.pose.orientation.w = 1.0 self._target_pose_pub.publish(msg) + publish_trace( + self, + self._latency_trace_pub, + trace_id, + "backend", + "target_pose_published", + source_stamp=msg.header.stamp, + ) with self._lock: self._nav_target_pose = {'x': float(x), 'y': float(y)} nav_running = self._nav_nodes_running diff --git a/tinynav/core/latency_trace.py b/tinynav/core/latency_trace.py new file mode 100644 index 00000000..65008e8a --- /dev/null +++ b/tinynav/core/latency_trace.py @@ -0,0 +1,78 @@ +import json +import time +from typing import Any + +from builtin_interfaces.msg import Time as RosTime +from rclpy.node import Node +from std_msgs.msg import String + + +TRACE_TOPIC = "/debug/latency_trace" +TRACE_PREFIX = "manual_target:" + + +def make_trace_id() -> str: + return f"manual-{time.time_ns()}" + + +def make_trace_publisher(node: Node): + return node.create_publisher(String, TRACE_TOPIC, 10) + + +def ros_time_ns(stamp: RosTime | None) -> int | None: + if stamp is None: + return None + return int(stamp.sec) * 1_000_000_000 + int(stamp.nanosec) + + +def node_time_ns(node: Node) -> int: + return ros_time_ns(node.get_clock().now().to_msg()) or time.time_ns() + + +def encode_trace_frame(trace_id: str) -> str: + return f"{TRACE_PREFIX}{trace_id}" + + +def decode_trace_frame(value: str | None) -> str | None: + if not value or not value.startswith(TRACE_PREFIX): + return None + trace_id = value[len(TRACE_PREFIX):].strip() + return trace_id or None + + +def parse_trace_event(data: str) -> dict[str, Any] | None: + try: + event = json.loads(data) + except json.JSONDecodeError: + return None + if not isinstance(event, dict): + return None + return event + + +def publish_trace( + node: Node, + publisher, + trace_id: str | None, + stage: str, + event: str, + *, + source_stamp: RosTime | None = None, + **extra: Any, +) -> None: + if not trace_id: + return + payload = { + "trace_id": trace_id, + "stage": stage, + "event": event, + "t_ros_ns": node_time_ns(node), + "t_wall_ns": time.time_ns(), + } + source_stamp_ns = ros_time_ns(source_stamp) + if source_stamp_ns is not None: + payload["source_stamp_ns"] = source_stamp_ns + payload.update({k: v for k, v in extra.items() if v is not None}) + msg = String() + msg.data = json.dumps(payload, separators=(",", ":"), sort_keys=True) + publisher.publish(msg) diff --git a/tinynav/core/planning_node.py b/tinynav/core/planning_node.py index 937f2489..3af0676e 100644 --- a/tinynav/core/planning_node.py +++ b/tinynav/core/planning_node.py @@ -15,6 +15,11 @@ from std_msgs.msg import Header from codetiming import Timer import cv2 +from tinynav.core.latency_trace import ( + decode_trace_frame, + make_trace_publisher, + publish_trace, +) from tinynav.core.math_utils import rotvec_to_matrix, quat_to_matrix, matrix_to_quat, msg2np @@ -389,11 +394,16 @@ def __init__(self): self.create_subscription(Odometry, '/control/target_pose', self.target_pose_callback, 10) self.target_pose = None + self.target_trace_id = None + self._trace_planning_input_reported = None + self._latency_trace_pub = make_trace_publisher(self) self.poi_change_sub = self.create_subscription(Odometry, "/mapping/poi_change", self.poi_change_callback, 10) def poi_change_callback(self, msg): self.target_pose = None + self.target_trace_id = None + self._trace_planning_input_reported = None def target_pose_callback(self, msg): target_pose = np.array([ @@ -403,9 +413,24 @@ def target_pose_callback(self, msg): ]) if not np.all(np.isfinite(target_pose)): self.target_pose = None + self.target_trace_id = None + self._trace_planning_input_reported = None self.get_logger().info("Manual target pose cleared.") return self.target_pose = target_pose + self.target_trace_id = decode_trace_frame(msg.child_frame_id) + self._trace_planning_input_reported = None + publish_trace( + self, + self._latency_trace_pub, + self.target_trace_id, + "planning", + "target_pose_received", + source_stamp=msg.header.stamp, + x=float(target_pose[0]), + y=float(target_pose[1]), + z=float(target_pose[2]), + ) def info_callback(self, msg): if self.K is None: @@ -555,6 +580,22 @@ def publish_3d_occupancy_cloud_with_esdf(self, grid3d, ESDF_map, resolution=0.1, def sync_callback(self, depth_msg, odom_msg): if self.K is None: return + if ( + self.target_pose is not None + and self.target_trace_id is not None + and self._trace_planning_input_reported != self.target_trace_id + ): + self._trace_planning_input_reported = self.target_trace_id + publish_trace( + self, + self._latency_trace_pub, + self.target_trace_id, + "planning", + "planning_input_received", + source_stamp=odom_msg.header.stamp, + depth_stamp_ns=int(depth_msg.header.stamp.sec) * 1_000_000_000 + + int(depth_msg.header.stamp.nanosec), + ) with Timer(name='preprocess', text="[{name}] Elapsed time: {milliseconds:.0f} ms"): depth = self.bridge.imgmsg_to_cv2(depth_msg, desired_encoding='32FC1') stamp = Time.from_msg(odom_msg.header.stamp).nanoseconds / 1e9 @@ -664,6 +705,15 @@ def cost_function(traj, param, score, target_pose): pose.pose.orientation.z = qz pose.pose.orientation.w = qw path.poses.append(pose) + publish_trace( + self, + self._latency_trace_pub, + self.target_trace_id, + "planning", + "trajectory_published", + source_stamp=path.header.stamp, + path_len=len(path.poses), + ) self.path_pub.publish(path) def main(args=None): diff --git a/tinynav/platforms/cmd_vel_control.py b/tinynav/platforms/cmd_vel_control.py index 4e36e0fb..b1ef7f0d 100644 --- a/tinynav/platforms/cmd_vel_control.py +++ b/tinynav/platforms/cmd_vel_control.py @@ -3,12 +3,18 @@ from geometry_msgs.msg import Twist from nav_msgs.msg import Path from nav_msgs.msg import Odometry -from std_msgs.msg import Bool +from std_msgs.msg import Bool, String from rclpy.qos import DurabilityPolicy, QoSProfile from scipy.spatial.transform import Rotation as R import numpy as np import logging import time +from tinynav.core.latency_trace import ( + TRACE_TOPIC, + make_trace_publisher, + parse_trace_event, + publish_trace, +) # Module-level logger for cases where self.get_logger() is not available logger = logging.getLogger(__name__) @@ -23,6 +29,10 @@ def __init__(self): Odometry, '/slam/odometry_visual', self.pose_callback, 10 ) self.create_subscription(Path, '/planning/trajectory_path', self.path_callback, 10) + self.create_subscription(String, TRACE_TOPIC, self.trace_callback, 10) + self.latency_trace_pub = make_trace_publisher(self) + self.active_trace_id = None + self._trace_cmd_published_for_path = False self.T_robot_to_camera = np.array([ [0, -1, 0, 0], [0, 0, -1, 0], @@ -90,6 +100,30 @@ def _on_nav_active(self, msg: Bool): def pose_callback(self, msg): self.pose = msg + def trace_callback(self, msg: String): + event = parse_trace_event(msg.data) + if not event: + return + if event.get("stage") in ("backend", "planning") and event.get("trace_id"): + self.active_trace_id = event.get("trace_id") + if event.get("stage") == "planning" and event.get("event") == "trajectory_published": + self._trace_cmd_published_for_path = False + + def _publish_cmd_trace(self, out: Twist): + if self._trace_cmd_published_for_path: + return + publish_trace( + self, + self.latency_trace_pub, + self.active_trace_id, + "cmd_vel_control", + "cmd_vel_published", + vx=float(out.linear.x), + vy=float(out.linear.y), + wz=float(out.angular.z), + ) + self._trace_cmd_published_for_path = True + def _clamp_step(self, target: float, current: float, max_delta: float) -> float: return float(np.clip(target - current, -max_delta, max_delta) + current) @@ -129,6 +163,7 @@ def cmd_timer_callback(self): out.linear.x = target_cmd.linear.x out.angular.z = 0.0 self.cmd_pub.publish(out) + self._publish_cmd_trace(out) self.prev_cmd = out return @@ -156,6 +191,7 @@ def cmd_timer_callback(self): out.angular.z = 0.0 self.cmd_pub.publish(out) + self._publish_cmd_trace(out) self.prev_cmd = out def path_callback(self, msg): @@ -166,6 +202,16 @@ def path_callback(self, msg): if len(msg.poses) < 2: return self.path = msg + self._trace_cmd_published_for_path = False + publish_trace( + self, + self.latency_trace_pub, + self.active_trace_id, + "cmd_vel_control", + "path_received", + source_stamp=msg.header.stamp, + path_len=len(msg.poses), + ) ros_now = self.get_clock().now().to_msg() self.last_path_time = ros_now.sec + ros_now.nanosec * 1e-9 diff --git a/tinynav/platforms/unitree_control.py b/tinynav/platforms/unitree_control.py index 7cfd8125..569a2eb2 100644 --- a/tinynav/platforms/unitree_control.py +++ b/tinynav/platforms/unitree_control.py @@ -9,6 +9,12 @@ from enum import Enum import logging import time +from tinynav.core.latency_trace import ( + TRACE_TOPIC, + make_trace_publisher, + parse_trace_event, + publish_trace, +) logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) @@ -31,6 +37,7 @@ def __init__(self, networkInterface: str = "enP8p1s0"): self.battery = 0.0 self.last_twist_time = None self.logger = logging.getLogger(__name__) + self.active_trace_id = None self.twist_subscriber = ChannelSubscriber("rt/cmd_vel", Twist_) self.twist_subscriber.Init(self.TwistMessageHandler, 10) @@ -43,9 +50,21 @@ def __init__(self, networkInterface: str = "enP8p1s0"): self.publisher_battery = self.create_publisher(Float32, '/battery', 10) self.publisher_robot_status = self.create_publisher(String, '/robot_status', 10) + self.latency_trace_pub = make_trace_publisher(self) + self.create_subscription(String, TRACE_TOPIC, self.TraceMessageHandler, 10) self._status_timer = self.create_timer(1.0, self._publish_robot_status) + def TraceMessageHandler(self, msg: String): + event = parse_trace_event(msg.data) + if not event: + return + if ( + event.get("stage") in ("backend", "planning", "cmd_vel_control") + and event.get("trace_id") + ): + self.active_trace_id = event.get("trace_id") + # twist message handler def TwistMessageHandler(self, msg: Twist_): current_time = time.time() @@ -53,12 +72,32 @@ def TwistMessageHandler(self, msg: Twist_): time_interval = current_time - self.last_twist_time self.logger.debug(f"cmd_vel callback time interval: {time_interval*1000:.2f} ms") self.last_twist_time = current_time + publish_trace( + self, + self.latency_trace_pub, + self.active_trace_id, + "unitree_control", + "cmd_vel_received", + vx=float(msg.linear.x), + vy=float(msg.linear.y), + wz=float(msg.angular.z), + ) if (msg.linear.x != 0 or msg.linear.y != 0 or msg.angular.z != 0): self.logger.debug(f"Moving with velocity: {msg.linear.x}, {msg.linear.y}, {msg.angular.z}") self.sport_client.Move(msg.linear.x, msg.linear.y, msg.angular.z) + command = "Move" else: self.sport_client.StopMove() + command = "StopMove" + publish_trace( + self, + self.latency_trace_pub, + self.active_trace_id, + "unitree_control", + "robot_command_sent", + command=command, + ) time.sleep(0.02) def ActionMessageHandler(self, msg: String_): diff --git a/tool/latency_trace_recorder.py b/tool/latency_trace_recorder.py new file mode 100644 index 00000000..6fc8d66a --- /dev/null +++ b/tool/latency_trace_recorder.py @@ -0,0 +1,199 @@ +import argparse +import csv +import json +import os +from collections import defaultdict + +import rclpy +from rclpy.node import Node +from std_msgs.msg import String + +from tinynav.core.latency_trace import TRACE_TOPIC, parse_trace_event + + +STAGE_EVENTS = [ + ("backend", "manual_target_requested"), + ("backend", "target_pose_published"), + ("planning", "target_pose_received"), + ("planning", "planning_input_received"), + ("planning", "trajectory_published"), + ("cmd_vel_control", "path_received"), + ("cmd_vel_control", "cmd_vel_published"), + ("unitree_control", "cmd_vel_received"), + ("unitree_control", "robot_command_sent"), +] + +SUMMARY_SEGMENTS = [ + ( + "backend_publish_ms", + ("backend", "manual_target_requested"), + ("backend", "target_pose_published"), + ), + ( + "backend_to_planning_ms", + ("backend", "target_pose_published"), + ("planning", "target_pose_received"), + ), + ( + "planning_wait_input_ms", + ("planning", "target_pose_received"), + ("planning", "planning_input_received"), + ), + ( + "planning_compute_ms", + ("planning", "planning_input_received"), + ("planning", "trajectory_published"), + ), + ( + "path_delivery_ms", + ("planning", "trajectory_published"), + ("cmd_vel_control", "path_received"), + ), + ( + "cmd_generation_ms", + ("cmd_vel_control", "path_received"), + ("cmd_vel_control", "cmd_vel_published"), + ), + ( + "cmd_to_unitree_ms", + ("cmd_vel_control", "cmd_vel_published"), + ("unitree_control", "cmd_vel_received"), + ), + ( + "unitree_dispatch_ms", + ("unitree_control", "cmd_vel_received"), + ("unitree_control", "robot_command_sent"), + ), + ( + "total_ms", + ("backend", "manual_target_requested"), + ("unitree_control", "robot_command_sent"), + ), +] + + +class LatencyTraceRecorder(Node): + def __init__(self, csv_path: str | None, summary_csv_path: str | None): + super().__init__("latency_trace_recorder") + self.events_by_trace = defaultdict(dict) + self.summary_written = set() + self.event_writer = None + self.summary_writer = None + self.event_file = None + self.summary_file = None + + if csv_path: + os.makedirs(os.path.dirname(os.path.abspath(csv_path)), exist_ok=True) + self.event_file = open(csv_path, "w", newline="") + self.event_writer = csv.DictWriter( + self.event_file, + fieldnames=[ + "trace_id", + "stage", + "event", + "t_ros_ns", + "t_wall_ns", + "source_stamp_ns", + "payload_json", + ], + ) + self.event_writer.writeheader() + + if summary_csv_path: + os.makedirs(os.path.dirname(os.path.abspath(summary_csv_path)), exist_ok=True) + self.summary_file = open(summary_csv_path, "w", newline="") + self.summary_writer = csv.DictWriter( + self.summary_file, + fieldnames=["trace_id"] + [name for name, _, _ in SUMMARY_SEGMENTS], + ) + self.summary_writer.writeheader() + + self.create_subscription(String, TRACE_TOPIC, self.trace_callback, 100) + self.get_logger().info(f"Recording latency traces from {TRACE_TOPIC}") + + def destroy_node(self): + if self.event_file: + self.event_file.close() + if self.summary_file: + self.summary_file.close() + super().destroy_node() + + def trace_callback(self, msg: String): + event = parse_trace_event(msg.data) + if not event: + return + trace_id = event.get("trace_id") + stage = event.get("stage") + event_name = event.get("event") + if not trace_id or not stage or not event_name: + return + + key = (stage, event_name) + self.events_by_trace[trace_id][key] = event + if self.event_writer: + self.event_writer.writerow({ + "trace_id": trace_id, + "stage": stage, + "event": event_name, + "t_ros_ns": event.get("t_ros_ns"), + "t_wall_ns": event.get("t_wall_ns"), + "source_stamp_ns": event.get("source_stamp_ns"), + "payload_json": json.dumps(event, sort_keys=True), + }) + self.event_file.flush() + + summary = self._build_summary(trace_id) + if summary: + line = " ".join( + f"{name}={value:.1f}" + for name, value in summary.items() + if name != "trace_id" and value is not None + ) + self.get_logger().info(f"{trace_id} {line}") + if ( + self.summary_writer + and trace_id not in self.summary_written + and self._has_full_summary(trace_id) + ): + self.summary_writer.writerow(summary) + self.summary_file.flush() + self.summary_written.add(trace_id) + + def _build_summary(self, trace_id: str) -> dict | None: + events = self.events_by_trace[trace_id] + if not all(key in events for key in STAGE_EVENTS[:7]): + return None + summary = {"trace_id": trace_id} + for name, start_key, end_key in SUMMARY_SEGMENTS: + start = events.get(start_key) + end = events.get(end_key) + if start is None or end is None: + summary[name] = None + continue + summary[name] = (int(end["t_ros_ns"]) - int(start["t_ros_ns"])) / 1e6 + return summary + + def _has_full_summary(self, trace_id: str) -> bool: + events = self.events_by_trace[trace_id] + return all(key in events for key in STAGE_EVENTS) + + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument("--csv", default="/tmp/tinynav_latency_trace_events.csv") + parser.add_argument("--summary-csv", default="/tmp/tinynav_latency_trace_summary.csv") + args = parser.parse_args() + + rclpy.init() + node = LatencyTraceRecorder(args.csv, args.summary_csv) + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() From 755d9a69fcc80113ed766f2d5419d5f25ad2c027 Mon Sep 17 00:00:00 2001 From: xiaole-dm Date: Tue, 30 Jun 2026 09:04:07 +0800 Subject: [PATCH 6/7] fix(trace): keep first latency events for summary --- tinynav/platforms/cmd_vel_control.py | 5 +++++ tool/latency_trace_recorder.py | 2 +- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/tinynav/platforms/cmd_vel_control.py b/tinynav/platforms/cmd_vel_control.py index b1ef7f0d..90ce561c 100644 --- a/tinynav/platforms/cmd_vel_control.py +++ b/tinynav/platforms/cmd_vel_control.py @@ -33,6 +33,7 @@ def __init__(self): self.latency_trace_pub = make_trace_publisher(self) self.active_trace_id = None self._trace_cmd_published_for_path = False + self._trace_path_received_for_current_path = False self.T_robot_to_camera = np.array([ [0, -1, 0, 0], [0, 0, -1, 0], @@ -108,8 +109,11 @@ def trace_callback(self, msg: String): self.active_trace_id = event.get("trace_id") if event.get("stage") == "planning" and event.get("event") == "trajectory_published": self._trace_cmd_published_for_path = False + self._trace_path_received_for_current_path = False def _publish_cmd_trace(self, out: Twist): + if not self._trace_path_received_for_current_path: + return if self._trace_cmd_published_for_path: return publish_trace( @@ -203,6 +207,7 @@ def path_callback(self, msg): return self.path = msg self._trace_cmd_published_for_path = False + self._trace_path_received_for_current_path = True publish_trace( self, self.latency_trace_pub, diff --git a/tool/latency_trace_recorder.py b/tool/latency_trace_recorder.py index 6fc8d66a..5f1255a1 100644 --- a/tool/latency_trace_recorder.py +++ b/tool/latency_trace_recorder.py @@ -129,7 +129,7 @@ def trace_callback(self, msg: String): return key = (stage, event_name) - self.events_by_trace[trace_id][key] = event + self.events_by_trace[trace_id].setdefault(key, event) if self.event_writer: self.event_writer.writerow({ "trace_id": trace_id, From 6c31c92fa69e16ac3b803d094da803ee43b62f11 Mon Sep 17 00:00:00 2001 From: xiaole-dm Date: Tue, 30 Jun 2026 09:40:28 +0800 Subject: [PATCH 7/7] fix(trace): order latency summary events --- tinynav/platforms/cmd_vel_control.py | 7 ++++-- tinynav/platforms/unitree_control.py | 3 ++- tool/latency_trace_recorder.py | 36 ++++++++++++++++++++++------ 3 files changed, 36 insertions(+), 10 deletions(-) diff --git a/tinynav/platforms/cmd_vel_control.py b/tinynav/platforms/cmd_vel_control.py index 90ce561c..7cd510b6 100644 --- a/tinynav/platforms/cmd_vel_control.py +++ b/tinynav/platforms/cmd_vel_control.py @@ -105,9 +105,12 @@ def trace_callback(self, msg: String): event = parse_trace_event(msg.data) if not event: return - if event.get("stage") in ("backend", "planning") and event.get("trace_id"): + if ( + event.get("stage") == "planning" + and event.get("event") == "trajectory_published" + and event.get("trace_id") + ): self.active_trace_id = event.get("trace_id") - if event.get("stage") == "planning" and event.get("event") == "trajectory_published": self._trace_cmd_published_for_path = False self._trace_path_received_for_current_path = False diff --git a/tinynav/platforms/unitree_control.py b/tinynav/platforms/unitree_control.py index 569a2eb2..e51d5660 100644 --- a/tinynav/platforms/unitree_control.py +++ b/tinynav/platforms/unitree_control.py @@ -60,7 +60,8 @@ def TraceMessageHandler(self, msg: String): if not event: return if ( - event.get("stage") in ("backend", "planning", "cmd_vel_control") + event.get("stage") == "cmd_vel_control" + and event.get("event") == "cmd_vel_published" and event.get("trace_id") ): self.active_trace_id = event.get("trace_id") diff --git a/tool/latency_trace_recorder.py b/tool/latency_trace_recorder.py index 5f1255a1..9bc36228 100644 --- a/tool/latency_trace_recorder.py +++ b/tool/latency_trace_recorder.py @@ -75,7 +75,7 @@ class LatencyTraceRecorder(Node): def __init__(self, csv_path: str | None, summary_csv_path: str | None): super().__init__("latency_trace_recorder") - self.events_by_trace = defaultdict(dict) + self.events_by_trace = defaultdict(lambda: defaultdict(list)) self.summary_written = set() self.event_writer = None self.summary_writer = None @@ -129,7 +129,7 @@ def trace_callback(self, msg: String): return key = (stage, event_name) - self.events_by_trace[trace_id].setdefault(key, event) + self.events_by_trace[trace_id][key].append(event) if self.event_writer: self.event_writer.writerow({ "trace_id": trace_id, @@ -160,13 +160,15 @@ def trace_callback(self, msg: String): self.summary_written.add(trace_id) def _build_summary(self, trace_id: str) -> dict | None: - events = self.events_by_trace[trace_id] - if not all(key in events for key in STAGE_EVENTS[:7]): + selected = self._select_ordered_events(trace_id, STAGE_EVENTS) + if selected is None: + selected = self._select_ordered_events(trace_id, STAGE_EVENTS[:7]) + if selected is None: return None summary = {"trace_id": trace_id} for name, start_key, end_key in SUMMARY_SEGMENTS: - start = events.get(start_key) - end = events.get(end_key) + start = selected.get(start_key) + end = selected.get(end_key) if start is None or end is None: summary[name] = None continue @@ -174,8 +176,28 @@ def _build_summary(self, trace_id: str) -> dict | None: return summary def _has_full_summary(self, trace_id: str) -> bool: + return self._select_ordered_events(trace_id, STAGE_EVENTS) is not None + + def _select_ordered_events(self, trace_id: str, keys: list[tuple[str, str]]) -> dict | None: events = self.events_by_trace[trace_id] - return all(key in events for key in STAGE_EVENTS) + selected = {} + last_t = -1 + for key in keys: + candidates = sorted( + events.get(key, []), + key=lambda event: int(event.get("t_ros_ns", 0)), + ) + match = None + for event in candidates: + event_t = int(event.get("t_ros_ns", 0)) + if event_t >= last_t: + match = event + last_t = event_t + break + if match is None: + return None + selected[key] = match + return selected def main():