diff --git a/android/cartfollow-devlog.md b/android/cartfollow-devlog.md new file mode 100644 index 000000000..352e635c0 --- /dev/null +++ b/android/cartfollow-devlog.md @@ -0,0 +1,1084 @@ +# Human Cart Simulator 开发进度记录 + +> 所属项目：自主跟随购物车原型 +> 代码位置：`dev/OpenBot/android/robot/src/main/java/org/openbot/cartfollow/` +> 开发分支：`feature/human-cart-simulator`（Phase 1）/ `feature/distance-control`（Phase 2）/ `feature/person-crop-collector`（Phase 3 起） +> 最后更新：2026-07-09 + +--- + +## 1. 模块总览 + +Human Cart Simulator 是购物车跟随功能的上位机核心模块，在 OpenBot App 中新增一个功能页面（"Cart Simulator"），实现基于手机摄像头人物检测的跟随控制闭环。 + +### 文件清单 + +| 文件 | 作用 | +|------|------| +| `HumanCartSimulatorFragment.java` | 主 UI Fragment：摄像头预览、检测框绘制、目标确认、ReID 调度、行为决策 debug 与人类指令显示 | +| `FollowStateMachine.java` / `FollowState.java` | 管理初始化、确认、重捕获、跟随、谨慎跟随、身份不确定、搜索与停止 | +| `ActionArbitrator.java` / `BehaviorAction.java` / `BehaviorDecisionResult.java` | 阶段 A 行为层：将状态与证据映射为 `FOLLOW_SLOW / MOTION_STOP / LOCAL_SEARCH / BLOCKED_WAIT` 等动作 | +| `IdentityEvidence.java` / `DistanceEvidence.java` / `TraversabilityEvidence.java` / `SystemSafetyEvidence.java` | 统一证据结构，供 ActionArbitrator 和 debug 面板使用 | +| `ControlGenerator.java` | 控制算法：基于 DistanceState 决定 forward，转向由 xError 决定；当前仍只用于模拟提示，不直接控制底盘 | +| `DistanceState.java` / `ImageSetpointDistanceEstimator.java` | 初始化标定 + 图像伺服距离估计，输出 `TOO_FAR / OK / TOO_CLOSE / UNKNOWN` | +| `TargetMemory.java` | 目标记忆：confirmed bbox、颜色特征、距离 setpoint、previous/last bbox 和 ReID gallery | +| `TargetMatcher.java` | legacy 目标匹配：position + size + color + confidence，用于 ReID 不可用时的保守降级 | +| `ReIDFeatureExtractor.java` / `TfliteReIDFeatureExtractor.java` | ReID 抽象接口与 TFLite 推理实现，当前本地测试模型为 `osnet_x0_25_market1501.tflite` | +| `ReIDCoordinator.java` / `ReIDMatchResult.java` / `BboxContinuityEvidence.java` | 管理 ReID gallery、候选人推理、best/second/margin、bbox 连续性与推理耗时 | +| `HumanCommandInterpreter.java` | 将状态、距离和行为动作转换为 Human Cart Simulator 的中文动作提示 | +| `fragment_human_cart_simulator.xml` | 布局文件：OverlayView、指令文本、快照确认面板、倒计时、调试信息和底部面板 | + +### 集成点（在 OpenBot App 中的入口） + +| 文件 | 修改内容 | +|------|----------| +| `FeatureList.java` | 新增 `CART_SIMULATOR` 类别，显示在主菜单 | +| `MainFragment.java` | 添加 Cart Simulator 的导航路由 | +| `nav_graph.xml` | 注册 `cartSimFragment` 导航目标 | +| `strings.xml` | 新增 `cart_simulator` / `cart_sim_start` / `cart_sim_idle` 字符串 | + +### Person Crop Collector（Phase 3 数据闭环入口） + +`Person Crop Collector` 是 ReID 接入前的真实检测框数据采集工具页，代码位置为 `dev/OpenBot/android/robot/src/main/java/org/openbot/cropcollector/`。 + +| 文件 | 作用 | +|------|------| +| `PersonCropCollectorFragment.java` | 复用 OpenBot `Detector`，实时显示 person 检测框，并按 Person ID 启停采集 session | +| `PersonCropSession.java` | 为每次采集创建 `session_id`、`session_info.json`、`metadata.csv` 与 `crops/` 输出目录 | +| `PersonCropSaver.java` | 异步保存带 padding 的 person crop，按 `sensorOrientation` 旋转为正向图，并追加元数据 | +| `PersonCropCaptureConfig.java` | 管理采样间隔、置信度阈值、单人采集、padding、最大 crop 数和 JPEG 质量 | +| `fragment_person_crop_collector.xml` | 采集页 UI：模型选择、置信度、采样间隔、单人模式、Person ID、开始 / 停止按钮 | + +当前输出路径位于 App 外部图片目录下的 `cartfollow_crops/_/`，导出到 PC 后再由 `tools/reid_pc_test/prepare_openbot_crops_dataset.py` 整理为 `images_openbot_clean/`。 + +--- + +## 2. 当前实现状态 + +### 2.1 已完成（commit `dd6aa95` + `409d85f` + `4da208a` + Phase 2） + +| 功能 | 状态 | 说明 | +|------|------|------| +| 人物检测（MobileNet-SSD） | 已完成 | 复用 OpenBot 现有 `Detector`，筛选 `classType="person"` | +| 两阶段目标初始化 | 已完成 | `CAPTURE_TARGET → LOCKED_PENDING_CONFIRM → CONFIRMED_ARMED`，采集时记录 confirmedBbox、面积、上下身颜色直方图、距离 setpoint，截图供用户确认 | +| 用户确认 / 重拍 / 取消 | 已完成 | 确认面板含快照预览与三按钮，状态切换正确 | +| 目标记忆 `TargetMemory` | 已完成 | 保存 confirmedBbox、confirmedArea、上下身 HSV 直方图、动态 lastBbox/lastCenter/lastArea/lastSeenTime、距离 setpoint（desiredHeightRatio/areaRatio/bottomRatio） | +| 目标匹配 `TargetMatcher` | 已完成 | position(0.40) + size(0.20) + color(0.30) + confidence(0.10) 融合评分，阈值 0.5；ReID 接口预留未接入 | +| 确认后重识别启动 | 已完成 | `CONFIRMED_ARMED → REACQUIRE_TARGET`，连续 `REACQUIRE_MATCH_N=8` 帧匹配后进入倒计时 | +| 倒计时启动 | 已完成 | `READY_TO_FOLLOW` 倒计时 3 秒后进入 FOLLOW | +| 完整状态机 `FollowStateMachine` | 已完成 | `IDLE → CAPTURE → CONFIRM → REACQUIRE → COUNTDOWN → FOLLOW → LOST → SEARCH → STOP` 全链路 | +| `LOST → SEARCH → STOP` 执行逻辑 | 已完成 | 连续 `FOLLOW_LOST_M=10` 帧未匹配进入 LOST；LOST 持续 `LOST_TO_SEARCH_MS=800ms` 进入 SEARCH；SEARCH 超时 `SEARCH_TIMEOUT_MS=5000ms` 进入 STOP；期间重新匹配则回 FOLLOW | +| **初始化距离标定 + 图像伺服** | 已完成（Phase 2） | `ImageSetpointDistanceEstimator` 基于采集时记录的 setpoint 输出 `height_scale / area_scale / bottom_shift`，无需恢复真实米制距离 | +| **DistanceState 输出** | 已完成（Phase 2） | 输出 `TOO_FAR / OK / TOO_CLOSE / UNKNOWN`，替代线性 distError；UNKNOWN 时停车 | +| **ControlGenerator 基于 DistanceState** | 已完成（Phase 2） | forward 由 DistanceState 决定：TOO_FAR→MAX_FORWARD，其余→0；移除硬编码 TARGET_H_RATIO/K_DIST/TOO_CLOSE_H_RATIO | +| **距离调试显示** | 已完成（Phase 2） | Simulator 显示 `dist / hScale / aScale / bShift / distConf` | +| **距离感知指令** | 已完成（Phase 2） | `HumanCommandInterpreter` 新增 DistanceState 重载：OK→"保持距离"、UNKNOWN→"距离不明，请停止" | +| 转向方向修正 | 已完成 | `FLIP_TURN=true`，commit `409d85f` 修正 | +| UI 模式切换 | 已完成 | 开关控制检测启停，启动后锁定模型选择 | +| 置信度调节 | 已完成 | +/- 按钮以 5% 步进调节，范围 5%-95% | +| 模型选择 | 已完成 | 支持本地和 URL 模型，修复了 URL 模型的错误提示 | +| 检测框可视化 | 已完成 | 绿色目标框 / 黄色候选框 / 白色普通行人框 / 红色匹配失败框 | +| 快照确认面板 | 已完成 | LOCKED_PENDING_CONFIRM 时显示候选目标截图 + 确认/重拍/取消 | +| 倒计时显示 | 已完成 | READY_TO_FOLLOW 时显示剩余秒数 | +| 调试信息面板 | 已完成 | 显示 state / forward / turn / left / right / persons / fps / dist / hScale / aScale / bShift / distConf | +| 导航集成 | 已完成 | 已注册到主菜单 "Cart Simulator" 入口 | +| **Person Crop Collector** | 已完成（Phase 3 前置） | 已注册到主菜单，可采集真实 OpenBot person bbox crop、`session_info.json` 与 `metadata.csv` | +| **真实 crop 数据 PC 端 ReID 复测** | 已完成首轮 | 基于 `images_openbot_clean`、`osnet_x0_25_market1501.pth`、`diverse gallery` 完成 pairwise / gallery-probe / target-follow 模拟 | +| **Person Sequence Collector** | 已完成 | 可采集无人帧、多人检测、bbox、crop 和人工事件，用于 PC sequence replay | +| **阶段 A 行为层** | 已完成并通过手机体验 | `Evidence -> BehaviorDecisionResult -> BehaviorAction -> HumanCommand` 已接入 Human Cart Simulator | +| **阶段 B Android ReID** | 已完成首版并通过手机运行 | TFLite ReID 可运行，debug 字段正常，实机约 30 FPS；仍需阶段 C 轨迹与身份信念层抑制跟错人 | +| **阶段 C 目标轨迹与身份信念层** | 已完成策略修正版代码接入 | 已包含短时 trackId、lockedTrackId、targetBelief、suspectedTrack、locked ghost、suspected 滞回、loose/default/strict bbox gate、恢复后 relock 与非 locked 空间支持门控，待安装新版 APK 后复测 | +| **诊断日志开关** | 已完成 | Human Cart Simulator 新增“记录日志”开关，默认关闭；关闭时不创建 diagnostics session，不写 CSV/JSON/crop/gallery/event | + +### 2.2 Phase 3 首轮 ReID 实验结果（2026-07-06） + +数据集：`tools/reid_pc_test/images_openbot_clean`，共 3 个身份、209 张真实 OpenBot 检测框 crop。 + +模型：`osnet_x0_25` + `weights/osnet_x0_25_market1501.pth`，CPU 推理，embedding 维度 512。 + +关键结论： + +- Pairwise：同一人均值 `0.709`，不同人均值 `0.620`，均值差距 `0.089`，Top-1 最近邻身份正确率 `0.990`。 +- Gallery-Probe：`gallery-k=8 + diverse` 强制识别准确率 `0.876`，优于 `gallery-k=5` 的 `0.840`。 +- Target-follow 模拟：`gallery-k=8` 时目标存在场景强制选择目标准确率 `0.843`；`margin>=0.05` 时 accepted accuracy `0.957`，`margin>=0.08` 时 `0.986`。 +- 目标缺席场景风险仍高：`gallery-k=8` 下 `margin>=0.05` 的 false accept rate 仍为 `0.457`，`margin>=0.10` 仍为 `0.184`。 + +工程判断： + +```text +当前 ReID 主线暂定为 osnet_x0_25 + diverse confirmedGallery(k=8)。 +ReID margin 可作为身份置信证据，但不能单独恢复 FOLLOW。 +后续必须与位置连续性、bbox 尺寸、运动趋势、连续多帧稳定性和状态机融合。 +``` + +### 2.3 核心控制算法（Phase 2 后） + +``` +输入：匹配目标 bbox + 画面尺寸 + 传感器角度 + TargetMemory(setpoint) +输出：Control(left, right) + DistanceEstimate + +距离估计（ImageSetpointDistanceEstimator）： + 1. 处理 sensorOrientation 旋转 + 2. currentHeightRatio = boxHeight / imgHeight + currentAreaRatio = boxArea / (imgW * imgH) + currentBottomRatio = boxBottom / imgHeight + 3. heightScale = currentHeightRatio / desiredHeightRatio + areaScale = sqrt(currentAreaRatio / desiredAreaRatio) + bottomShift = currentBottomRatio - desiredBottomRatio + 4. 校验：bbox 过小 / height_scale 与 area_scale 对数差异过大 → UNKNOWN + 5. 判态：heightScale < 0.85 → TOO_FAR + heightScale > 1.15 → TOO_CLOSE + 否则 → OK + +控制生成（ControlGenerator）： + xError = target_centerX / imgWidth - 0.5 + turn = K_TURN × xError × (FLIP_TURN ? -1 : 1) + forward = + TOO_FAR → MAX_FORWARD + OK → 0 + TOO_CLOSE→ 0 （首版不主动后退） + UNKNOWN → 0 （不确定就停） + left = forward - turn, right = forward + turn +``` + +当前可调参数（`ControlGenerator`）： + +| 参数 | 默认值 | 含义 | +|------|--------|------| +| `K_TURN` | 1.5 | 转向灵敏度 | +| `MAX_FORWARD` | 0.6 | TOO_FAR 时的固定前进速度 | +| `MIN_CONFIDENCE` | 0.5 | 最小检测置信度 | +| `FLIP_TURN` | true | 转向方向翻转 | + +距离估计参数（`ImageSetpointDistanceEstimator`）： + +| 参数 | 默认值 | 含义 | +|------|--------|------| +| `FAR_THRESHOLD` | 0.85 | heightScale 低于此值判定 TOO_FAR | +| `CLOSE_THRESHOLD` | 1.15 | heightScale 高于此值判定 TOO_CLOSE | +| `UNKNOWN_HEIGHT_DISAGREE` | 0.3 | height/area 对数差异上限 | +| `MIN_BBOX_HEIGHT_RATIO` | 0.1 | bbox 高度占比下限 | + +当前状态机参数（`FollowStateMachine`）： + +| 参数 | 默认值 | 含义 | +|------|--------|------| +| `CAPTURE_FRAMES` | 15 | 采集帧数阈值 | +| `REACQUIRE_MATCH_N` | 8 | 重识别连续匹配帧数 | +| `FOLLOW_LOST_M` | 10 | FOLLOW 连续未匹配进入 LOST 的帧数 | +| `LOST_TO_SEARCH_MS` | 800 | LOST 进入 SEARCH 的延时 | +| `SEARCH_TIMEOUT_MS` | 5000 | SEARCH 超时进入 STOP | +| `COUNTDOWN_MS` | 3000 | 倒计时时长 | + +--- + +## 3. 尚未实现（待开发） + +### 3.1 关键缺失 + +| 功能 | 优先级 | 说明 | +|------|--------|------| +| **`vehicle.setControl()` 集成** | 中（阶段6） | 当前 Control 仅显示在 UI 上，未实际发送给底盘。硬件联调阶段在 `processFrame()` 中调用 `vehicle.setControl()` | +| **目标轨迹与身份信念层** | 高 | ReID 已接入并能运行，但目标离开/返回和干扰者场景仍可能跟错或恢复过慢；下一阶段需要 `TargetTrackManager + IdentityBeliefAccumulator` | +| **参数持久化** | 低 | 当前调参仅内存生效，重启恢复默认 | +| **参数 UI 面板** | 低 | K_TURN / MAX_FORWARD / 阈值等参数需通过代码修改，没有 UI 界面 | +| **bottomShift 参与判态** | 低 | 当前 bottomShift 仅用于显示，未参与距离状态判断。待 90° 旋转下方向实测验证后决定是否纳入 | + +### 3.2 状态机（已完整实现） + +``` +IDLE ──(启动开关)──→ CAPTURE_TARGET ──(采集N帧)──→ LOCKED_PENDING_CONFIRM + │ + 确认 / 重拍 / 取消 + │ +CONFIRMED_ARMED ──(检测到人)──→ REACQUIRE_TARGET ──(连续N帧匹配)──→ READY_TO_FOLLOW + │ + 倒计时3秒 + ↓ + FOLLOW + │ + 连续M帧未匹配 + ↓ + LOST ──(800ms)──→ SEARCH ──(5s超时)──→ STOP + │ │ + └──(重新匹配)──────┴──→ FOLLOW + +STOP ──(用户重新开始)──→ CAPTURE_TARGET +``` + +### 3.3 已知代码问题 + +| 问题 | 说明 | +|------|------| +| 目标选择策略仍依赖位置+颜色 | TargetMatcher 未接入 ReID，多人长时间交叉下可能误锁。阶段3 处理 | +| forward 限幅非对称 | forward 只允许 ≥0，不允许后退。安全优先，合理 | +| bottomShift 旋转方向待验证 | 90° 旋转下 boxBottom 映射方向需实测确认，当前仅显示不参与判态 | + +--- + +## 4. 与下位机的接口 + +| 方向 | 协议 | 说明 | +|------|------|------| +| 上位机→下位机 | `c,` | 由 `Vehicle.sendControl()` 发送，范围 [-255,255] | +| 心跳 | `h` | 由 `Vehicle` 自动管理 | + +**当前状态：Cart Simulator 未调用 `vehicle.setControl()`，因此底盘不会运动。** 首版联调前需要先接通这个链路。 + +--- + +## 5. 后续开发计划 + +> 阶段划分对齐 `design/自主跟随购物车上位机软件开发计划.md` 与 `design/上位机软件开发 Phase 2——修正跟随距离控制计划书.md` + +### Phase 1：状态机与目标初始化闭环（已完成） + +- [x] 完整状态机 `IDLE → CAPTURE → CONFIRM → REACQUIRE → COUNTDOWN → FOLLOW → LOST → SEARCH → STOP` +- [x] 两阶段目标初始化 + 用户确认 + 重识别启动 +- [x] TargetMemory / TargetMatcher / ControlGenerator 接入 +- [x] Human Cart Simulator 实时提示与调试显示 +- [ ] 多人干扰不切换目标（部分保证，待 ReID 增强） + +### Phase 2：修正跟随距离控制（已完成） + +- [x] 新增 `DistanceState` 枚举（`TOO_FAR / OK / TOO_CLOSE / UNKNOWN`） +- [x] 新增 `ImageSetpointDistanceEstimator`，输出 height_scale / area_scale / bottom_shift / state / confidence +- [x] `TargetMemory` 采集时记录 `desired_bbox_height_ratio / area_ratio / bottom_ratio` +- [x] 重构 `ControlGenerator`，forward 由 DistanceState 决定，移除硬编码 setpoint +- [x] `FollowStateMachine.FrameResult` 透传 distanceEstimate +- [x] Human Cart Simulator 显示 dist_state / hScale / aScale / bShift / distConf +- [x] `HumanCommandInterpreter` 纳入 distance state +- [x] 0.8-1.2 m 目标距离标定验证（初步测试基本通过，大多数情况下可以把距离保持为初始化时的距离。bShift在人远离的时候会向更负的方向变化，符合预期。） + +**不在 Phase 2 范围**：`vehicle.setControl()` 接通（阶段6）、ReID 增强（阶段3）、障碍处理（阶段5） + +### Phase 3：真实检测框数据闭环 + ReID + +- [x] 新增 Person Crop Collector，从 OpenBot Android 导出真实 person bbox crop +- [x] PC 端验证 confirmedGallery / reid_score / reid_margin（首轮基于 `osnet_x0_25 + diverse gallery-k=8`） +- [x] Android 端部署 `osnet_x0_25` TFLite 首版，Human Cart Simulator 中 `reidAvailable=true` +- [x] 将 ReID 输出接入 `IdentityEvidence / FollowStateMachine / ActionArbitrator` +- [x] 多人、目标离开、目标返回、遮挡场景完成首轮手机观察 +- [ ] 用阶段 C 的 track/belief 层继续降低跟错人风险，提高目标返回后的恢复速度 + +### Phase 4：目标轨迹与身份信念层（当前下一步） + +- [x] 新增轻量 `TargetTrackManager`，用 bbox IoU / center distance / area ratio 维护短时 trackId +- [x] 新增 `IdentityBeliefAccumulator`，对每个 track 累计 targetBelief +- [x] locked target 不因干扰者单帧 ReID 高分被抢走，状态机恢复改为 belief 优先 +- [x] 目标返回后通过 suspected track + 多帧 belief 稳定恢复到 `REACQUIRE_TARGET / FOLLOW_CAUTION` +- [x] debug 面板显示 `trackId / lockedTrackId / targetBelief / trackAge / missedFrames / beliefReason` +- [ ] 手机实测验收：目标离开、干扰者进入、目标返回、目标在场干扰者穿越、遮挡 + +### Phase 5：距离控制继续收敛 + +- [x] 初始化距离标定 + 图像伺服首版已完成 +- [ ] 结合阶段 C 的稳定目标 track 重新验证 `TOO_FAR / OK / TOO_CLOSE` +- [ ] 评估 bottomShift 是否纳入距离状态判断 + +### Phase 6：局部可通行空间与跟随式避障 + +- [ ] LEFT / CENTER / RIGHT 三方向 free score +- [ ] 候选动作 SLOW_FORWARD / LEFT_ARC / RIGHT_ARC / BLOCKED_WAIT + +### Phase 7：性能评估与增强开关 + +- [ ] 记录 detector、ReID、track/belief、ActionArbitrator 耗时 +- [ ] 根据手机性能决定是否提高 ReID 频率 +- [ ] 评估 MiDaS / Depth Anything Android 部署，只作为距离/障碍风险增强 + +### Phase 8：硬件联调 + +- [ ] 接通 `vehicle.setControl()` 到底盘 +- [ ] 真实车速/转向半径/延迟标定 +- [ ] ToF / 超声波安全冗余 + +--- + +## 6. 提交历史 + +| Commit | 日期 | 说明 | +|--------|------|------| +| `dd6aa95` | 2026-07 | Add Human Cart Simulator for shopping cart follow debugging | +| `409d85f` | 2026-07 | Fix turn direction, confidence button height, and model selection | +| `4da208a` | 2026-07-02 | Add two-stage target init, target memory and full follow state machine | +| `173ef96` | 2026-07-06 | Add DistanceState and ImageSetpointDistanceEstimator for image-based visual servoing | +| `66bbf12` | 2026-07-06 | Calibrate distance setpoint in TargetMemory and refactor ControlGenerator to DistanceState | +| `c880025` | 2026-07-06 | Display distance state and scales in Human Cart Simulator | +| `80fa505` | 2026-07-06 | Add Person Crop Collector entry skeleton | +| `290282c` | 2026-07-06 | Show person detections in crop collector | +| `9e3c7c4` | 2026-07-06 | Save detected person crops with metadata | +| `6d9aa5f` | 2026-07-06 | Add capture controls and status panel | +| `765eb82` | 2026-07-06 | Put Person ID input on its own row for tap accessibility | +| `771345e` | 2026-07-06 | Rotate crop by sensorOrientation before saving to disk | +| recorded | 2026-07-07 | Add PersonSequenceCollector for continuous sequence data collection | +| recorded | 2026-07-08 | Add phase A behavior decision layer and Human Cart Simulator action debug | +| recorded | 2026-07-08 | Add phase B TFLite ReID evidence path for Human Cart Simulator | +| pending | 2026-07-08 | Add phase C TargetTrack and IdentityBelief layer | + +--- + +## 7. Person Sequence Collector（Phase 3 时序数据采集） + +> 更新日期：2026-07-07 +> 代码位置：`dev/OpenBot/android/robot/src/main/java/org/openbot/sequencecollector/` +> 目的：为 PC 端 chronological replay / 状态机回放采集连续时序事实数据。 +> 当前状态：已实现、已构建通过、已安装到手机，并完成两条真实 sequence 采集。 + +### 7.1 模块定位 + +`PersonSequenceCollector` 是独立于 `PersonCropCollector` 的采集工具。它不做 ReID 推理，不控制小车，不写入 `FOLLOW / LOST / REACQUIRE / STOP` 等状态标签，只记录摄像头检测到的事实： + +```text +每个采样帧是否有人； +每个采样帧有几个人； +每个检测框的 bbox / confidence / crop_path； +人工标记的 target_left / target_return / occlusion / distractor 事件。 +``` + +这样 PC 端可以用同一份时序数据复现目标离开、遮挡、返回、干扰者进入等场景，而不是继续依赖随机抽样 rows。 + +### 7.2 新增文件 + +| 文件 | 作用 | +|------|------| +| `sequencecollector/PersonSequenceCollectorFragment.java` | 独立 CameraFragment 页面，复用 OpenBot Detector 检测 person，写入连续时序日志。 | +| `sequencecollector/PersonSequenceCaptureConfig.java` | 管理 frame log / crop / overlay 采样间隔、置信度、是否保存 crop 等配置。 | +| `sequencecollector/PersonSequenceSession.java` | 创建 `cartfollow_sequences//`，初始化 CSV 和 `session_info.json`。 | +| `sequencecollector/PersonSequenceSaver.java` | 单线程异步写 `frame_log.csv`、`detections.csv`、`events.csv` 和可选 crops。 | +| `res/layout/fragment_person_sequence_collector.xml` | Sequence 采集 UI。 | + +入口集成： + +| 文件 | 修改内容 | +|------|----------| +| `FeatureList.java` | 新增 `PERSON_SEQUENCE_COLLECTOR` 主菜单项。 | +| `MainFragment.java` | 新增跳转到 `personSequenceCollectorFragment`。 | +| `nav_graph.xml` | 注册 `personSequenceCollectorFragment`。 | +| `strings.xml` | 新增 Sequence Collector 标题、Start/Stop、idle 文案。 | + +### 7.3 输出目录与文件 + +输出目录位于 App 外部图片目录： + +```text +/sdcard/Android/data/org.openbot/files/Pictures/cartfollow_sequences/ +└── _seq_/ + ├── frame_log.csv + ├── detections.csv + ├── events.csv + ├── session_info.json + ├── crops/ + └── overlays/ +``` + +CSV 字段： + +```text +frame_log.csv: +session_id,frame_id,timestamp_ms,elapsed_ms,image_width,image_height,num_persons,raw_frame_path,overlay_path,event_tag,note + +detections.csv: +session_id,frame_id,det_id,timestamp_ms,confidence,bbox_left,bbox_top,bbox_right,bbox_bottom,bbox_width,bbox_height,bbox_area_ratio,center_x,center_y,edge_touch,crop_path + +events.csv: +session_id,timestamp_ms,frame_id,event_type,note +``` + +当前默认参数： + +| 参数 | 默认值 | +|------|--------| +| `frameLogIntervalMs` | 200 ms | +| `cropIntervalMs` | 500 ms | +| `overlayIntervalMs` | 1000 ms | +| `minConfidence` | 0.5 | +| `saveCrops` | true | +| `saveOverlays` | false | +| `jpegQuality` | 90 | + +说明：`frameLogIntervalMs` 与 `cropIntervalMs` 可在页面中通过 +/- 控件调整。第二条 sequence `yrc2_seq_20260707_152237` 实测使用 `cropIntervalMs=300 ms`，用于提高 PC 端 ReID replay 的帧密度。 + +### 7.4 当前验证状态 + +已完成静态构建验证： + +```powershell +$env:JAVA_HOME='D:\Java\jdk-17' +.\gradlew.bat :robot:assembleDebug +``` + +结果：构建通过。默认 JDK 24 会触发 Android Gradle `jlink` 兼容问题，需使用本机 `D:\Java\jdk-17` 构建。 + +已完成真机验证： + +```text +主菜单能看到 Person Sequence Collector； +进入页面后能显示 person bbox； +Start 后创建 cartfollow_sequences//； +无人帧写入 frame_log.csv，num_persons=0； +多人帧在 detections.csv 写多行； +事件按钮能追加 events.csv； +Stop 后显示 frames / detections / crops / events 和导出路径； +adb pull 后 PC sequence replay 可以读取并扩展使用。 +``` + +已采集数据： + +| sequence | 说明 | PC 侧结论 | +|----------|------|-----------| +| `yrc_seq_20260707_140056` | 首条真实 sequence，包含目标离开、返回、干扰者、遮挡事件。 | 安全性成立，未出现错误恢复 FOLLOW；高 over-stop 主要来自终态 STOP 后尾段。 | +| `yrc2_seq_20260707_152237` | 更结构化 sequence：正常跟随、目标离开、无人帧、返回、干扰者进入/离开、遮挡。 | 暴露“看到了目标但恢复条件太严”的问题；宽松恢复条件可避免 STOP，并保持 `wrong_recovery_count=0`。 | + +### 7.5 对 FollowStateMachine 的最新启发 + +第二条 sequence 表明：目标返回后，系统经常能看到连续稳定 bbox 和中等偏高 ReID 分数，但如果恢复条件只接受很强的 `strong + strict` 连续证据，就会长期卡在 `IDENTITY_UNCERTAIN`，最后超时进入 `STOP`。 + +后续 Android 状态机不应把 `STOP` 当成唯一安全动作，而应区分： + +```text +motion_stop: + 线速度为 0，不继续前进，但仍观察、原地搜索、尝试重捕获。 + +hard STOP: + 搜索失败、风险过高或安全异常后的终态停车，等待人工重新开始。 +``` + +建议新增或细化状态： + +```text +FOLLOW +FOLLOW_CAUTION +IDENTITY_UNCERTAIN +LOCAL_SEARCH +REACQUIRE_TARGET +STOP +``` + +目标丢失时应先进入 `motion_stop + LOCAL_SEARCH`，根据最后 bbox 方向做原地低速搜索；目标返回后若连续多帧满足 `ReID + bbox + prediction` 稳定证据，再进入 `REACQUIRE_TARGET`，最后恢复 `FOLLOW`。只有搜索超时、干扰风险过高、障碍/急停/通信异常时才进入 hard `STOP`。 + +--- + +## 8. 调试提示 + +- 使用 `/dev/OpenBot/android` 在 Android Studio 中打开工程 +- 主菜单 → "Cart Simulator" 进入本模块 +- 打开 Start 开关开始检测，关闭开关回到 IDLE +- 调试信息面板显示实时 state / forward / turn / persons / fps +- 中文指令文本仅供调试参考，实际不会发送给底盘 +- 主菜单 → "Person Crop Collector" 进入 ReID 数据采集页 +- 输入 Person ID，保持 `Single Only` 打开，点击 Start Session 后采集真实 person crop +- 采集目录导出到 PC 后，用 `tools/reid_pc_test/prepare_openbot_crops_dataset.py` 整理为 `images_openbot_clean/` +- 主菜单 → "Person Sequence Collector" 进入连续时序采集页 +- Sequence 采集时事件按钮只需在事件开始/结束时各按一次，PC replay 会用容忍窗口处理人工反应延迟 + +--- + +## 9. Phase B：ReID 身份证据接入与安全重捕获闭环（2026-07-08） + +本阶段根据 Human Cart Simulator 阶段 A 的手机验收反馈推进：阶段 A 的行为层基本可用，但目标重新进入、干扰者进入等场景仍可能因为旧 `TargetMatcher` 单帧匹配而出现“跟错人”。因此 Phase B 的首要目标是切断 `LOST / SEARCH -> FOLLOW` 的单帧恢复路径，并把 ReID 作为身份置信度证据接入状态机。 + +已完成代码改动： + +- 新增 `ReIDMatchResult`、`BboxContinuityEvidence`、`TfliteReIDFeatureExtractor`、`ReIDCoordinator`。 +- `FollowState` 新增 `FOLLOW_CAUTION` 与 `IDENTITY_UNCERTAIN`。 +- `TargetMemory` 增加 previous bbox 记录，用于 bbox 连续性和简单 prediction 计算。 +- `IdentityEvidence` 扩展为同时携带 legacy score、ReID score / margin、bbox gate、稳定帧数和候选切换次数。 +- `FollowStateMachine` 改为支持外部 `IdentityEvidence` 输入；`LOST / SEARCH` 不再允许单帧匹配直接恢复 `FOLLOW`，而是先进入 `REACQUIRE_TARGET`，再经多帧稳定证据恢复。 +- `ActionArbitrator` 增加 `IDENTITY_UNCERTAIN` 和 `FOLLOW_CAUTION` 的动作解释。 +- `HumanCartSimulatorFragment` 接入 `ReIDCoordinator`，在 debug 面板显示 `reidAvailable / gallerySize / bestScore / secondScore / margin / weak-mid-strong / bboxDefault-bboxStrict-prediction / stableMatchCount / candidateSwitchCount / reidLatencyMs / reidReason`。 + +TFLite 路线说明： + +- 首版复用当前工程已有 TensorFlow Lite 2.4，不新增 ONNX Runtime 依赖。 +- 默认模型路径为 `assets/networks/reid/osnet_x0_25_market1501.tflite`。 +- 该模型文件属于本地测试资产，`.gitignore` 已忽略 `*.tflite`，默认不提交。 +- 如果模型不存在或加载失败，App 不崩溃，debug 显示 `reidAvailable=false`，状态机退回更保守的 bbox / color / motion 逻辑。 + +构建验证： + +```powershell +$env:JAVA_HOME='D:\Java\jdk-17' +$env:Path="$env:JAVA_HOME\bin;$env:Path" +.\gradlew.bat :robot:assembleDebug +``` + +结果：构建通过，生成 `robot/build/outputs/apk/debug/robot-debug.apk`。当前本机输出过 Android SDK XML version warning，但未影响构建结果。 + +后续手机验收重点： + +- 无 ReID 模型时：Human Cart Simulator 应正常打开，debug 显示 `reidAvailable=false`，目标丢失后不应单帧恢复 `FOLLOW`。 +- 放入 TFLite 模型后：确认目标后 `gallerySize` 应逐步达到 8 或实际可用数量；多人、目标离开、目标返回、遮挡场景下观察 `bestScore / margin / stableMatchCount` 是否符合预期。 +- 目标返回时允许 `IDENTITY_UNCERTAIN -> REACQUIRE_TARGET -> FOLLOW`，但不允许单帧高分直接恢复 `FOLLOW`。 + +--- + +## 10. Phase B 实机验收更新：ReID 已跑通，但需要 TargetTrack / IdentityBelief（2026-07-08） + +### 10.1 当前实机状态 + +最新 APK 已安装到手机，Human Cart Simulator 中 ReID 首版链路已经成功运行： + +- TFLite 模型路径：`assets/networks/reid/osnet_x0_25_market1501.tflite` +- 模型输入：`[1,3,256,128]` +- 模型输出：`[1,512]` +- debug 面板显示的 ReID 字段基本正常 +- 实机帧率约 30 FPS，首轮性能可接受，后续如有必要仍可继续压榨调度策略 + +当前结论：ReID Android 接入已经从“能否加载/能否推理”进入“如何安全使用推理结果”的阶段。 + +### 10.2 当前体验问题 + +手机实测仍暴露两个关键问题： + +1. 目标离开画面或多人接近时，虽然统计上比纯 bbox / color 匹配更安全，但仍可能识别错目标并跟着别人走。 +2. 目标重新回到画面后，重捕获有时偏慢，甚至无法及时确认。 + +这说明当前 `ReIDMatchResult + bbox gate + stable frame` 仍偏“单帧候选驱动”。ReID 分数和 margin 有帮助，但不能直接等价于目标身份。 + +### 10.3 下一步代码方向 + +下一步建议在 `ReIDCoordinator` 与 `FollowStateMachine` 之间新增轻量轨迹和身份信念层： + +```text +Detector persons + -> TargetTrackManager + -> IdentityBeliefAccumulator + -> FollowStateMachine + -> ActionArbitrator +``` + +建议新增或细化类型： + +- `TargetTrack` + - `trackId` + - `lastBbox` + - `previousBbox` + - `ageFrames` + - `missedFrames` + - `lastSeenTimestampMs` + - `isNearPredictionRegion` + +- `TargetTrackManager` + - 用 bbox IoU、center distance、area ratio 做短时 person bbox 关联。 + - 只维护最近几秒的轻量 track，不做复杂多目标跟踪。 + - 输出当前候选 tracks，供 ReID 和状态机使用。 + +- `IdentityBelief` + - `trackId` + - `targetBelief` + - `reidContribution` + - `bboxContribution` + - `predictionContribution` + - `switchPenalty` + - `stableFrames` + - `beliefReason` + +- `IdentityBeliefAccumulator` + - 对每个 track 累计身份信念。 + - ReID strong/mid、bbox 连续、prediction 命中时加分。 + - 候选切换、bbox 跳变、面积突变、margin 低、多人与目标混淆时扣分。 + +### 10.4 状态机接入原则 + +后续恢复跟随不应再由“单帧候选高分”触发，而应由“稳定 track + 稳定 belief”触发： + +```text +IDENTITY_UNCERTAIN / SEARCH + -> 疑似目标 track 连续稳定 + -> REACQUIRE_TARGET + -> FOLLOW_CAUTION + -> FOLLOW_CONFIDENT +``` + +安全边界保持不变： + +- 身份不确定时线速度为 0。 +- 搜索阶段可以更积极地原地扫描和提高 ReID 频率，但不能向前跟随。 +- 已锁定目标时，干扰者一帧 ReID 高分不能直接抢走目标。 +- hard `STOP` 仍只作为搜索失败、风险过高或安全异常后的兜底终态。 + +### 10.5 下一轮手机验收新增观察项 + +debug 面板建议新增： + +```text +trackId +trackAge +missedFrames +targetBelief +beliefReason +activeTrackCount +lockedTrackId +``` + +验收场景： + +1. 单人正常跟随：trackId 应稳定，targetBelief 应逐步升高。 +2. 目标离开：进入 motion_stop / search，lockedTrack 不应被立即替换。 +3. 干扰者进入：干扰者可形成新 track，但 targetBelief 不应快速超过恢复阈值。 +4. 目标返回：先成为疑似目标，连续稳定后恢复 `REACQUIRE_TARGET -> FOLLOW_CAUTION / FOLLOW_CONFIDENT`。 +5. 目标在场且干扰者穿越：lockedTrack 应尽量保持，必要时进入 `FOLLOW_CAUTION / IDENTITY_UNCERTAIN`，不冒进切换。 + +--- + +## 11. Phase C：目标轨迹与身份信念层首版实现（2026-07-08） + +### 11.1 新增代码 + +| 文件 | 作用 | +|------|------| +| `TargetTrack.java` | 记录短时 track 的 `trackId / lastBbox / previousBbox / ageFrames / missedFrames / stableFrames`。 | +| `TargetTrackManager.java` | 用 bbox IoU、中心距离、面积比例将连续检测框关联为 track，并维护 `lockedTrackId / suspectedTrackId`。 | +| `IdentityBelief.java` | 定义 `BELIEF_CONFIRM=0.75 / BELIEF_CAUTION=0.55 / BELIEF_LOST=0.30` 和 belief debug 字段。 | +| `IdentityBeliefAccumulator.java` | 融合 ReID、bbox continuity、prediction、locked target、track age、candidate switch 和 missed frame，输出带 belief 的 `IdentityEvidence`。 | + +### 11.2 接入点 + +- `HumanCartSimulatorFragment` 每帧检测后先调用 `TargetTrackManager.update()`。 +- ReID 单帧输出不再直接交给状态机，而是先通过 `IdentityBeliefAccumulator.update()` 转换为累计身份信念。 +- 用户点击 Confirm 时调用 `lockClosest(memory.getLastBbox())`，建立 `lockedTrackId`。 +- overlay 现在显示 `T b=`；locked track 绿色，suspected track 黄色。 +- debug 面板新增 `activeTrackCount / trackId / lockedTrackId / suspectedTrackId / trackAge / missedFrames / belief / beliefReason`。 +- `FollowStateMachine` 在 `IdentityEvidence.hasBelief()` 时优先使用 `targetBelief + beliefStableFrames + bbox/prediction/lockedTrack` 判断 `FOLLOW / FOLLOW_CAUTION / REACQUIRE_TARGET / IDENTITY_UNCERTAIN`。 + +### 11.3 构建验证 + +构建命令： + +```powershell +$env:JAVA_HOME='D:\Java\jdk-17' +$env:Path="$env:JAVA_HOME\bin;$env:Path" +.\gradlew.bat :robot:assembleDebug +``` + +结果：构建通过，生成 debug APK。构建日志仍有 TensorFlow Lite manifest namespace warning 和 Kotlin/Javac target warning，均未阻塞构建。 + +### 11.4 手机验收重点 + +1. 单人正常跟随：`trackId` 应稳定，`targetBelief` 应逐步升高并保持。 +2. 目标离开画面：动作应进入 motion stop / local search，不继续前进。 +3. 目标离开后干扰者进入：干扰者可形成新 track，但不应快速获得恢复 `FOLLOW` 的 belief。 +4. 目标返回：应先成为 suspected track，经多帧 belief 稳定后恢复到 `REACQUIRE_TARGET / FOLLOW_CAUTION`。 +5. 目标在场时干扰者穿越：locked track 不应被一帧高 ReID 分数抢走；必要时进入 `FOLLOW_CAUTION / IDENTITY_UNCERTAIN`。 + +--- + +## 12. Phase C 诊断采集与 UI 简化（2026-07-08） + +### 12.1 实现目的 + +阶段 C 首轮实机验收后，仍观察到两类问题： + +- 目标回到画面后长期停留在黄框，迟迟不转为绿框； +- 非目标人物偶发转绿，表现为疑似跟错人。 + +本轮没有继续修改 ReID、belief、relock 或状态机阈值，而是先在 Human Cart Simulator 中加入诊断采集能力，用真实日志解释问题发生在哪个环节。 + +### 12.2 新增代码 + +| 文件 | 作用 | +|------|------| +| `cartfollow/diagnostics/CartFollowDiagnosticConfig.java` | 管理 frame log、crop、overlay 采样间隔和 JPEG 参数。 | +| `cartfollow/diagnostics/CartFollowDiagnosticSession.java` | 创建 `cartfollow_diagnostics//`，初始化 CSV、gallery/crops/overlays 目录和 `session_info.json`。 | +| `cartfollow/diagnostics/CartFollowDiagnosticSaver.java` | 单线程异步写 `frame_log.csv`、`identity_log.csv`、`events.csv`，并低频保存 locked/suspected/best_reid crop 与初始化 gallery snapshot。 | +| `HumanCartSimulatorFragment.java` | 接入诊断 session 生命周期、人工事件按钮、低频日志保存和简洁/完整 debug 切换。 | +| `fragment_human_cart_simulator.xml` | 新增 `调试详情` 按钮和 `目标离开画面 / 目标回到画面` 事件按钮。 | + +### 12.3 输出目录与文件 + +输出位置： + +```text +/sdcard/Android/data/org.openbot/files/Pictures/cartfollow_diagnostics/ +└── cart_diag_/ + ├── frame_log.csv + ├── identity_log.csv + ├── events.csv + ├── session_info.json + ├── crops/ + ├── gallery/ + └── overlays/ +``` + +`frame_log.csv` 记录每 200 ms 左右的状态机、行为动作、人类指令和人数： + +```text +session_id,frame_id,timestamp_ms,elapsed_ms,fps,num_persons, +follow_state,selected_action,action_reason,safety_block_reason,command_text +``` + +`identity_log.csv` 记录每 200 ms 左右的 track、ReID、bbox gate、belief 和 crop 路径： + +```text +session_id,frame_id,timestamp_ms, +track_id,locked_track_id,suspected_track_id,active_track_count, +track_age,missed_frames,best_score,second_score,margin,gallery_size, +weak_ok,mid_ok,strong_ok,bbox_default_ok,bbox_strict_ok,prediction_ok, +target_belief,belief_stable_frames,belief_uncertain_frames, +candidate_switch_count,belief_reason,reid_reason, +locked_crop_path,suspected_crop_path,best_reid_crop_path +``` + +`events.csv` 记录人工事件： + +```text +session_id,timestamp_ms,frame_id,event_type,note +``` + +当前人工事件只包括： + +- `target_left` +- `target_return` +- `session_stop` + +### 12.4 UI 行为 + +- 默认左上角只显示简洁 debug：`fps / state / action / persons / track / locked / suspected / belief / best / margin`。 +- 点击 `调试详情` 后显示原完整 debug，再次点击 `收起详情` 回到简洁显示。 +- `目标离开画面` 按钮在目标确认前禁用。 +- 用户点击 `确认` 后诊断 session 正式启用，事件按钮可用。 +- 第一次点击事件按钮写入 `target_left`，按钮文本切换为 `目标回到画面`。 +- 第二次点击写入 `target_return`，按钮文本切回 `目标离开画面`。 +- 该按钮只写日志，不改变状态机、ReID、track、belief 或 action。 + +### 12.5 构建验证 + +构建命令： + +```powershell +$env:JAVA_HOME='D:\Java\jdk-17' +$env:Path="$env:JAVA_HOME\bin;$env:Path" +.\gradlew.bat :robot:assembleDebug +``` + +结果：构建通过，生成 debug APK。构建日志仍包含 TensorFlow Lite manifest namespace warning、Kotlin/Javac target warning 和若干 deprecated API warning，均未阻塞构建。 + +### 12.6 手机验收重点 + +1. Start 后、确认目标前，事件按钮应禁用。 +2. 确认目标后，事件按钮启用，默认显示 `目标离开画面`。 +3. 目标离开时点击一次，`events.csv` 应出现 `target_left`。 +4. 目标返回时再点击一次，`events.csv` 应出现 `target_return`。 +5. 默认左上角不再显示大块完整 debug；点击 `调试详情` 后可展开完整字段。 +6. Stop / Cancel / Retake / 页面暂停后，诊断 session 应关闭，按钮禁用并复位。 +7. 导出诊断目录后，应能用 `events.csv` 附近的 `frame_log.csv` 和 `identity_log.csv` 判断黄框不转绿或非目标转绿的原因。 + +--- + +## 13. Phase C ReID 输入方向修正（2026-07-08） + +### 13.1 修正原因 + +PC 侧 `cartfollow_diagnostics` 复盘发现，旧版诊断 gallery 全部被标记为 `landscape_or_rotated`。代码检查确认旧版 `ReIDCoordinator` 直接在原始 `workingFrame` 上按 bbox 裁剪，并立即送入 `TfliteReIDFeatureExtractor`，没有按 `sensorOrientation` 将 person crop 转正。 + +这意味着旧版 Android ReID 虽然可以运行并输出分数，但 gallery 与候选 crop 都可能以横向姿态进入模型，影响目标返回和多人干扰场景下的稳定性。 + +### 13.2 本轮代码变化 + +- `ReIDCoordinator.collectInitializationCandidate()` 增加 `sensorOrientation` 参数，gallery candidate crop 裁剪后旋转为 upright 再提取 embedding。 +- `ReIDCoordinator.evaluate()` 和内部 ReID candidate 推理同样使用 upright crop。 +- `HumanCartSimulatorFragment` 调用 ReIDCoordinator 时传入当前 `sensorOrientation`。 +- 诊断 `gallery/` 中保存的初始化候选 crop 改为 upright 版本。 +- `session_info.json` 写入 `reid_crop_upright=true` 和 `sensor_orientation`。 +- debug 简洁面板和完整面板增加 `reidCrop=upright`，用于实机确认新版路径已生效。 + +本轮没有修改 ReID 阈值、belief 阈值、bbox gate、状态机恢复规则或真实底盘控制路径。 + +### 13.3 构建验证 + +构建命令： + +```powershell +$env:JAVA_HOME='D:\Java\jdk-17' +$env:Path="$env:JAVA_HOME\bin;$env:Path" +.\gradlew.bat :robot:assembleDebug +``` + +结果：构建通过。构建日志仍有既有 TensorFlow Lite manifest namespace warning、Kotlin/Javac target warning 和 deprecated Gradle warning，均未阻塞构建。 + +### 13.4 下一轮手机验收重点 + +1. Human Cart Simulator debug 应显示 `reidCrop=upright`。 +2. 新采集的 `session_info.json` 应包含 `reid_crop_upright=true`。 +3. 新采集的 `gallery/` 不应再全部被 PC 分析脚本标记为 `landscape_or_rotated`。 +4. 对比旧数据，观察 `target_return` 后 `frames_to_reacquire / frames_to_follow` 是否缩短。 +5. 若 upright crop 后仍大量出现 `belief_high_bbox_failed`，下一轮再处理分状态 bbox gate 和 recoverable stop。 + +--- + +## 14. Phase C 新旧诊断数据对比结论（2026-07-08） + +### 14.1 分析输入 + +本轮没有继续改 Android 策略，而是先对新旧 `cartfollow_diagnostics` 做 PC 离线对比： + +```text +old: tools/reid_pc_test/images/cartfollow_diagnostics_old/ +new: tools/reid_pc_test/images/cartfollow_diagnostics/ +``` + +新版数据来自 ReID crop upright 修正后的 APK，`session_info.json` 中应出现： + +```text +reid_crop_upright=true +sensor_orientation=90 +``` + +PC 分析命令： + +```powershell +cd tools/reid_pc_test +python analyze_cartfollow_diagnostics_v1.py ^ + --compare-roots old=images/cartfollow_diagnostics_old,new=images/cartfollow_diagnostics ^ + --output outputs/cartfollow_diagnostics_analysis/compare +``` + +### 14.2 对比结果 + +| 数据 | sessions | target_return | recovered_rate | recovered_fast | recovered_slow | not_recovered | hard_stop | best_mean | margin_mean | bbox_default_rate | gallery_candidate_landscape_rate | +|---|---:|---:|---:|---:|---:|---:|---:|---:|---:|---:|---:| +| old | 4 | 11 | 0.5455 | 5 | 1 | 3 | 2 | 0.5017 | 0.3431 | 0.4451 | 1.0000 | +| new | 2 | 16 | 0.8750 | 11 | 3 | 2 | 0 | 0.5992 | 0.4611 | 0.5485 | 0.0000 | + +结论： + +- upright crop 修正确实生效：新版 `gallery_candidate_landscape_rate=0.0000`。 +- ReID 分数整体提高：`best_mean` 和 `margin_mean` 都高于旧版。 +- `target_return` 后恢复率提升，且新版暂未出现 `hard_stop_before_return`。 +- 这轮结果说明 ReID 输入方向已不是主要问题。 + +### 14.3 当前剩余问题 + +新版主要 blocker： + +```text +candidate_switch_penalty: 15 +belief_high_bbox_failed: 10 +``` + +含义： + +- `candidate_switch_penalty` 说明目标返回或多人干扰时，suspected track 仍容易切换，trackId / lockedTrackId 保护还不够稳。 +- `belief_high_bbox_failed` 说明 ReID / belief 已经有较强证据，但 bbox default/strict gate 不通过，导致黄框迟迟不能转绿。 + +因此下一轮 Android 工作不应继续优先调 ReID 模型、TFLite 性能或 `bestScore / margin` 阈值，而应聚焦： + +1. `TargetTrackManager` 的 track association 稳定性。 +2. locked track 的保留与 suspected track 升级规则。 +3. `FOLLOW` 与 `REACQUIRE/IDENTITY_UNCERTAIN/SEARCH` 使用不同 bbox gate。 +4. recoverable stop 与 hard `STOP` 的边界。 + +### 14.4 下一轮验收口径 + +下一轮策略改动后继续采集 `cartfollow_diagnostics`，重点比较： + +```text +recovered_rate +mean_ms_to_follow +not_recovered_in_window +candidate_switch_penalty +belief_high_bbox_failed +非目标转绿是否增加 +gallery_candidate_landscape_rate 是否保持 0 +``` + +如果 `candidate_switch_penalty` 和 `belief_high_bbox_failed` 下降，同时非目标转绿不增加，才说明策略改动真正改善了“目标返回后迟迟不转绿”和“干扰者偶发转绿”两个问题。 + +--- + +## 15. Phase C 下一轮策略版本：track/bbox gate 小步修正（2026-07-08） + +### 15.1 开发定位 + +阶段 C 首版已经接入 `TargetTrackManager + IdentityBeliefAccumulator`，下一轮不再新增大架构，而是优化 track 粘性、locked/suspected 保护和 bbox gate 的状态化使用。 + +状态更新：本节为 2026-07-08 的实施计划，2026-07-09 已完成代码接入；最新实现状态见第 16 节。 + +本轮不做： + +```text +更换 ReID 模型 +启用 dynamic gallery +全局放宽 bbox gate +接通真实底盘前进 +优先做复杂避障 +``` + +### 15.2 计划代码点（已在第 16 节落地） + +| 文件 | 改动 | +|------|------| +| `TargetTrackManager.java` | 增加 locked track ghost memory、suspected track 最小滞回、疑似目标替换门槛。 | +| `BboxContinuityEvidence.java` | 在 default / strict 之外新增 loose admission gate。 | +| `IdentityBeliefAccumulator.java` | 分离 identity belief 与 motion permission；belief 高但 bbox failed 时保留 suspected，不直接清零。 | +| `FollowStateMachine.java` | 在 `IDENTITY_UNCERTAIN / SEARCH / REACQUIRE_TARGET` 使用 admission/confirm/motion 分层恢复。 | +| diagnostics/debug 字段 | 输出 `loose_admission_only / motion_gate_failed / suspected_dwell_hold / locked_ghost_reference` 等 reason。 | + +### 15.3 策略口径 + +恢复链路固定为： + +```text +loose admission + -> suspectedTrack + -> default confirm + -> REACQUIRE_TARGET + -> strict/default motion gate + -> FOLLOW_CAUTION / FOLLOW +``` + +具体原则： + +- loose gate 只允许进入 suspected/reacquire，不允许直接触发前进。 +- `FOLLOW / FOLLOW_CAUTION` 继续使用 default/strict bbox gate、prediction 或 locked track 保护，保持运动门槛保守。 +- `IDENTITY_UNCERTAIN / SEARCH` 可以接纳 loose admission 的疑似目标，但 action 仍为 `MOTION_STOP / LOCAL_SEARCH`。 +- `belief 高 + bbox failed` 表示身份可能正确但运动暂不安全，应保留 belief 或轻微衰减，并保持 `REACQUIRE_HOLD / MOTION_STOP`。 +- hard `STOP` 仍只由搜索超时、急停、通信异常、高风险障碍或人工取消触发。 + +### 15.4 复测场景 + +下一轮 APK 构建后采集 4 段短日志，每段 30-60 秒： + +1. 目标离开后原目标返回。 +2. 目标离开后干扰者进入。 +3. 目标在场时干扰者穿越。 +4. 目标短遮挡、蹲下或局部可见。 + +PC compare 验收指标： + +```text +recovered_rate 不下降； +mean_ms_to_follow 下降或不恶化； +not_recovered_in_window 减少； +candidate_switch_penalty 下降； +belief_high_bbox_failed 下降； +非目标转绿不增加； +hard_stop_count 不增加； +gallery_candidate_landscape_rate 保持 0。 +``` + +--- + +## 16. Phase C relock、空间支持门控与日志开关实现（2026-07-09） + +### 16.1 本轮实现状态 + +本轮 Android 代码已完成接入，目标是修正 2026-07-09 新采集数据暴露的两个问题：目标返回后新 track 没有晋升为 locked track，以及干扰者在没有 bbox / ghost 空间支持时仍可能靠 ReID 抬高 belief。 + +本轮仍保持边界不变： + +```text +不更换 ReID 模型 +不启用 dynamic gallery +不开放真实底盘前进 +不改 ControlGenerator 的真实底盘控制路径 +不提前引入复杂避障 +``` + +### 16.2 恢复后 relock + +- 非 locked track 在 `REACQUIRE_TARGET / READY_TO_FOLLOW / FOLLOW_CAUTION / FOLLOW` 的安全恢复路径中，如果连续通过 motion gate，并在 `FOLLOW_CAUTION / FOLLOW_SLOW` 等保守动作下稳定至少 2 帧，可以晋升为新的 `lockedTrackId`。 +- relock 成功后同步调用 `IdentityBeliefAccumulator.lockTrack(newTrackId)`，清空旧 suspected track。 +- debug / diagnostic reason 输出 `relock_after_recovery`，用于 PC 侧确认目标返回后是否真正完成重新锁定。 +- 普通 `IDENTITY_UNCERTAIN` 中单帧高 ReID 分数不允许直接 relock。 + +### 16.3 非 locked 空间支持门控 + +- 非 locked track 只有满足 `bboxLoose / bboxDefault / prediction / nearLockedGhost` 之一，才允许成为 suspected target。 +- 如果只有 ReID 高分但空间支持全 false，记录 `reid_interest_no_spatial_support` 或 `spatial_support_missing`。 +- 无空间支持时允许保留低强度观察信号，但 belief 不得升到 caution / confirm 阈值，也不能触发 suspected 或 FOLLOW。 +- `candidateSwitchCount` 只在真正选中或切换 suspected / selected track 时增长，减少“观察到高分干扰者”导致的噪声。 + +### 16.4 诊断日志开关 + +- `fragment_human_cart_simulator.xml` 底部控制区新增 `diagnostic_switch`，文案为“记录日志”，默认关闭。 +- `HumanCartSimulatorFragment` 新增 `diagnosticEnabled` 状态。 +- 日志关闭时，`startDiagnosticSession()` 和 `activateDiagnosticSession()` 直接返回，不创建 `cartfollow_diagnostics` session，不写 `session_info.json`，不保存 confirmed snapshot、gallery、crop、CSV、events 或 session_stop。 +- 运行中关闭日志会立即停止 active session，并禁用“目标离开 / 返回”事件按钮。 +- 日志开启并 Confirm 激活 session 后，事件按钮才可用；跟随推理、状态机和 debug 文本不受日志开关影响。 + +### 16.5 构建验证 + +构建命令： + +```powershell +$env:JAVA_HOME='C:\Users\ysyys\.jdks\jbr-17.0.14' +$env:Path="$env:JAVA_HOME\bin;$env:Path" +.\gradlew.bat :robot:assembleDebug +``` + +结果：构建通过，生成 debug APK。构建日志仍包含既有 TensorFlow Lite namespace、Kotlin/Javac target 和 deprecated API warning，未阻塞构建。 + +### 16.6 下一轮手机验收重点 + +1. 日志开关关闭时，Start、Confirm、运行、Retake、Cancel 都不应新增 `cartfollow_diagnostics/cart_diag_*` 目录。 +2. 日志开关开启后，Confirm 才激活 session，并正常生成 `frame_log.csv / identity_log.csv / events.csv / session_info.json / gallery / crops`。 +3. 目标离开后原目标返回：期望恢复到新 track 后自动 relock，后续 `trackId == lockedTrackId`。 +4. 蹲下、弯腰或局部遮挡：期望检测器丢框后可恢复，恢复成功后 relock 到新 track。 +5. 目标离开后干扰者进入：期望干扰者无空间支持时不进入 suspected，不恢复 FOLLOW。 +6. 目标在场时干扰者穿越：期望 `candidateSwitchCount` 明显下降，非 locked FOLLOW 行数减少或归零。 +7. PC compare 继续检查 `candidate_switch_penalty / belief_high_bbox_failed / recovered_rate / hard_stop_count / gallery_candidate_landscape_rate`，合格后再讨论极低速真实底盘联调。 + +--- + +## 17. BLE 真实小车跟随模块首版（2026-07-12） + +### 17.1 实现定位 + +新增主菜单入口 `Real Cart Follow`，将 Human Cart Simulator 的相机、检测、ReID、 +track/belief、状态机、ActionArbitrator 和诊断能力抽取到 +`BaseCartFollowFragment`。Simulator 继续只显示人肉模拟指令；真实模块才允许向 +`Vehicle` 输出控制。 + +### 17.2 BLE 会话与安全边界 + +- 严格匹配 OpenBot BLE Service、RX 和 TX UUID，Notify 订阅完成后才认为串口可用。 +- BLE Ready 后启动幂等 `h750` 心跳并发送 `f`，只有收到 `fCART_AT8236` 和 `r` + 后才允许运动。 +- 页面暂停、模式切换、BLE 断开、推理超过 400 ms 无更新时立即发送零控制。 +- 手机急停发送 `!S,`，ESP32 锁存急停后必须重启才能恢复。 +- 下位机还独立检查 500 ms 非零运动命令保鲜，心跳不能掩盖控制线程卡死。 + +### 17.3 手动与自动模式 + +手动模式为 dead-man 控制，每 100 ms 重发当前命令： + +```text +前进 28 +后退 24 +原地转向 20 +松手 / CANCEL / 退后台 -> 0,0 +``` + +自动模式每次进入页面都需要长按 2 秒解锁，最大协议输出为 32， +`FOLLOW_CAUTION` 进一步降速。`LOCAL_SEARCH` 固定为低速原地旋转，连续运动最多 +2 秒，超时后停车并撤销自动解锁。 + +近场传感器和物理急停尚未接入，因此自动模式只用于空旷场地、有人准备物理断电的 +实验，不视为正式避障能力。 + +### 17.4 自动化验证 + +使用 JDK 17 完成： + +```powershell +.\gradlew.bat :robot:testDebugUnitTest --no-daemon +.\gradlew.bat :robot:assembleDebug --no-daemon +``` + +结果： + +```text +RealCartSafetyControllerTest 6/6 +全部单元测试 11/11 +Debug APK android/robot/build/outputs/apk/debug/robot-debug.apk +``` + +### 17.5 真机验收顺序 + +1. 不接电机验证 BLE 扫描、`f/r/h`、急停和重启解除。 +2. 车轮悬空验证四方向、松手停车、断连停车和双超时停车。 +3. 空载落地只开放手动模式。 +4. 手动全部通过后，才在空旷场地长按解锁自动实验。 +5. 传感器和物理急停到位前，不进入货架、拥挤或无人看护场景。 diff --git a/android/point_goal_navigation_source_analysis.md b/android/point_goal_navigation_source_analysis.md new file mode 100644 index 000000000..eb5b71c42 --- /dev/null +++ b/android/point_goal_navigation_source_analysis.md @@ -0,0 +1,307 @@ +# OpenBot PointGoalNavigation 源码解析 + +本文基于当前 `dev/OpenBot/android` 源码，解释 OpenBot Android 端 `Point Goal Navigation` 功能的实际实现方式。这个功能不能简单理解成传统地图导航或 SLAM 导航；它更像是“ARCore 相对位姿 + 相机图像 + 学习策略网络”的点目标视觉导航 demo。 + +## 一句话结论 + +`PointGoalNavigation` 让用户输入一个相对于机器人当前位姿的二维目标点，例如“前方 1.0 m、左侧 0 m”。启动后，Android 手机用 ARCore 持续估计当前手机/机器人位姿，再把当前相机图像、目标距离、目标相对朝向送入 `navigation.tflite`。模型直接输出左右轮控制量，OpenBot 再通过 `Vehicle.setControl()` 下发给底盘。 + +也就是说，它不是靠地图、路径规划或障碍物几何建模来导航，而是依赖一个训练好的 TFLite 策略网络端到端地产生左右轮速度。 + +## 关键源码位置 + +- 功能入口：`robot/src/main/java/org/openbot/pointGoalNavigation/PointGoalNavigationFragment.java` +- ARCore 封装：`robot/src/main/java/org/openbot/pointGoalNavigation/ArCore.java` +- TFLite 策略封装：`robot/src/main/java/org/openbot/tflite/Navigation.java` +- 目标输入弹窗：`robot/src/main/java/org/openbot/pointGoalNavigation/SetGoalDialogFragment.java` +- UI 布局：`robot/src/main/res/layout/fragment_point_goal_navigation.xml` +- 目标输入布局：`robot/src/main/res/layout/set_goal_dialog_view.xml` +- 功能入口菜单：`robot/src/main/java/org/openbot/common/FeatureList.java` +- Fragment 路由：`robot/src/main/res/navigation/nav_graph.xml` +- 模型配置：`robot/src/main/assets/config.json` +- 内置模型：`robot/src/main/assets/networks/navigation.tflite` + +## 功能入口与运行前提 + +主界面功能列表中有一个 `Point Goal Navigation` 子项。点击后，`MainFragment` 会跳转到 `PointGoalNavigationFragment`。 + +这个 Fragment 的界面本身非常简单，只有一个全屏 `GLSurfaceView`，用于显示 ARCore 相机背景和目标标记。真正的逻辑不在 XML，而在 `PointGoalNavigationFragment` 和 `ArCore` 两个 Java 类里。 + +运行前提有三类： + +1. Android 相机权限。 +2. 设备支持 ARCore，并且系统中可用 Google Play Services for AR。 +3. OpenBot 车辆通信链路可用，因为最终控制会调用 `Vehicle.setControl()`。 + +项目的 `robot/build.gradle` 依赖了 `com.google.ar:core:1.29.0`，`AndroidManifest.xml` 中也声明了 `com.google.ar.core` 为 `optional`。但是代码里创建 ARCore `Session` 时仍然需要运行时 ARCore 服务。如果手机没有可用的 ARCore 运行环境，`setupArCore()` 会捕获 `UnavailableArcoreNotInstalledException`、`UnavailableDeviceNotCompatibleException`、`UnavailableApkTooOldException` 等异常，并弹出 ARCore failure 信息。 + +这解释了为什么在中国大陆没有 Google Play 服务时很难直接实机测试：模型文件本身是内置的，不需要联网下载；真正卡住的是 ARCore 运行服务和设备兼容链路。 + +## 用户输入的目标是什么 + +进入功能后会弹出 `Set Goal` 对话框。用户填写两个数： + +- `Forward [m]`：目标在机器人前方多少米，默认 `1.0`。 +- `Left [m]`：目标在机器人左侧多少米，默认 `0`。 + +这两个值允许为负数。README 中也说明：负的 forward 表示后方，负的 left 表示右侧。 + +源码中点击 `Start` 后会调用： + +```java +startDriving(-left, -forward); +``` + +随后在 `startDriving()` 中有注释： + +```java +// x: right, z: backwards +``` + +也就是说，用户用“前/左”的直觉坐标输入，代码再转换成 ARCore/OpenBot 使用的坐标： + +- ARCore/OpenBot 这里把 `x` 轴正方向当作右侧。 +- 把 `z` 轴正方向当作后方。 +- 因此前方目标要变成负 `z`，左侧目标要变成负 `x`。 + +例如默认输入 `Forward = 1.0, Left = 0`，实际目标平移就是 `(x=0, z=-1.0)`，即当前位姿前方 1 米。 + +## ARCore 在这里做了什么 + +`ArCore` 类负责封装 ARCore `Session`、相机画面、位姿和目标锚点。它不做 SLAM 地图输出，也不做路径规划；它主要提供两件事： + +1. 当前手机/机器人位姿 `currentPose`。 +2. 用户目标点的 ARCore 锚点 `targetAnchor`。 + +启动驾驶时，代码先： + +1. 清除旧锚点。 +2. 把当前 ARCore pose 保存为起点锚点。 +3. 用起点 pose 叠加用户输入的平移，创建目标锚点。 + +核心逻辑是： + +```java +arCore.setStartAnchorAtCurrentPose(); +Pose startPose = arCore.getStartPose(); +arCore.setTargetAnchor(startPose.compose(Pose.makeTranslation(goalX, 0.0f, goalZ))); +``` + +之后每一帧，`ArCore.onDrawFrame()` 会： + +1. 调用 `session.update()` 获取 ARCore 帧。 +2. 检查相机 tracking 状态。 +3. 从 ARCore camera 读取当前 pose。 +4. 读取目标 anchor pose。 +5. 调用 `frame.acquireCameraImage()` 获取 CPU 图像。 +6. 把 `NavigationPoses`、`ImageFrame`、相机内参和时间戳回调给 `PointGoalNavigationFragment.onArCoreUpdate()`。 + +如果 ARCore 不再处于 `TRACKING` 状态，或者相机会话暂停、不可用，Fragment 会停止底盘并弹窗提示。 + +## 相机配置和图像预处理 + +`ArCore.setCameraConfig()` 明确选择： + +- 后置摄像头。 +- 30 FPS。 +- CPU 图像尺寸必须是 `640x480`。 +- 如果有多个可用配置，选择 GPU texture size 更小的配置。 + +每次导航更新时，`PointGoalNavigationFragment` 会把 ARCore 的 `YUV_420_888` 图像转成 RGB bitmap： + +1. 原始 CPU 图像尺寸通常为 `640x480`。 +2. 按 `160 / 480 = 1/3` 缩放，得到约 `213x160`。 +3. 再裁剪 `Bitmap.createBitmap(bitmap, 0, 30, 160, 90)`，得到模型需要的 `160x90` 图像。 + +所以模型输入图像不是完整相机画面，而是缩放后从左上 x=0、y=30 开始截取的 `160x90` 区域。这个裁剪方式是硬编码的，和 `navigation.tflite` 的训练输入尺寸匹配。 + +像素进入 TFLite 前会在 `Navigation.addPixelValue()` 中归一化到 `[0, 1]`： + +```java +channel = channel / 255.0f +``` + +## 模型输入与输出 + +`config.json` 中配置了一个导航模型： + +```json +{ + "class": "NAVIGATION", + "type": "GOALNAV", + "name": "PilotNet-Goal.tflite", + "path": "networks/navigation.tflite", + "inputSize": "160x90" +} +``` + +不过 `PointGoalNavigationFragment.startDriving()` 并没有从模型管理器动态选择模型，而是直接构造了一个固定模型对象： + +```java +new Model( + 0, + CLASS.NAVIGATION, + TYPE.GOALNAV, + "navigation.tflite", + PATH_TYPE.ASSET, + "networks/navigation.tflite", + "160x90"); +``` + +`Navigation.java` 中可以看到模型有两个输入： + +- `serving_default_img_input:0`：形状必须是 `[1, 90, 160, 3]`。 +- `serving_default_goal_input:0`：3 个 float，分别是： + - `goalDistance` + - `sin(deltaYaw)` + - `cos(deltaYaw)` + +其中 `goalDistance` 是当前 pose 到目标 pose 在水平面上的欧氏距离，只使用 `x` 和 `z`： + +```java +sqrt((goal.x - current.x)^2 + (goal.z - current.z)^2) +``` + +`deltaYaw` 是机器人当前朝向和“指向目标方向”之间的偏航角差。代码不是直接输入角度，而是输入 `sin(deltaYaw)` 和 `cos(deltaYaw)`，这样可以避免角度在 `pi` 和 `-pi` 边界附近的不连续问题。 + +模型输出是一个 `float[1][2]`： + +```java +float[][] predicted_ctrl = new float[1][2]; +``` + +这两个值会被包装成： + +```java +new Control(predicted_ctrl[0][0], predicted_ctrl[0][1]) +``` + +也就是左轮和右轮的归一化控制量。`Control` 构造函数会把每个值夹到 `[-1, 1]`。`Vehicle.sendControl()` 再乘以 `speedMultiplier`，默认代码中是 `192`，然后通过串口协议发送： + +```text +c,\n +``` + +因此，PointGoalNavigation 的控制输出不是“线速度 + 角速度”，而是直接输出 OpenBot 差速底盘左右轮命令。 + +## 每帧控制循环 + +完整闭环可以概括为： + +```mermaid +flowchart TD + A["用户输入 Forward / Left"] --> B["转换为 ARCore 相对目标坐标"] + B --> C["创建 startAnchor 和 targetAnchor"] + C --> D["ARCore 每帧估计 currentPose"] + D --> E["获取 640x480 YUV 相机帧"] + E --> F["转 RGB、缩放、裁剪为 160x90"] + D --> G["计算 goalDistance 和 deltaYaw"] + F --> H["navigation.tflite"] + G --> H + H --> I["输出 left / right"] + I --> J["Vehicle.setControl"] + J --> K["串口发送 c,"] +``` + +对应到 `PointGoalNavigationFragment.onArCoreUpdate()`，每帧做的事情是： + +1. 如果 `isRunning == false`，直接忽略。 +2. 计算当前到目标距离。 +3. 如果距离小于 `0.15 m`，认为到达目标，停止底盘。 +4. 否则计算目标相对偏航。 +5. 取当前相机图像并转成 `160x90` bitmap。 +6. 调用 `navigationPolicy.recognizeImage(bitmap, distance, sinYaw, cosYaw)`。 +7. 把模型输出 `Control` 交给 `vehicle.setControl(control)`。 + +## 停车与异常处理 + +这个功能的安全逻辑比较简单，主要有三类停止： + +1. 到达目标：当 `goalDistance < 0.15f` 时，调用 `stop()`，播放 `Goal reached.`，并弹出信息框。 +2. ARCore tracking 丢失：`onArCoreTrackingFailure()` 中调用 `stop()`，播放 `Tracking lost.`，并弹出信息框。 +3. ARCore session 暂停：`onArCoreSessionPaused()` 中调用 `stop()`，播放 `AR Core session paused.`，并弹出信息框。 + +`stop()` 的动作是： + +```java +arCore.detachAnchors(); +vehicle.stopBot(); +isRunning = false; +``` + +这里没有复杂状态机，也没有短时重定位、避障优先级、通信异常仲裁等逻辑。避障能力如果存在，主要隐含在 `navigation.tflite` 策略网络的训练行为里，而不是显式代码规则。 + +## 它没有做什么 + +结合源码可以明确几点： + +- 没有构建全局地图。 +- 没有使用 SLAM 路径规划。 +- 没有 A*、DWA、TEB 等传统局部规划器。 +- 没有显式障碍物检测模块。 +- 没有用深度图参与当前控制。`DepthFrame` 类存在，但当前 `ArCore` 配置没有启用 depth，`PointGoalNavigationFragment` 也没有使用 `DepthFrame`。 +- 没有用相机内参参与当前控制。`CameraIntrinsics` 会被回调传入，但 `onArCoreUpdate()` 中没有实际使用它。 +- 没有用 Google Maps 或网络地图。所谓 point goal 是相对当前位置的局部目标点，不是地图上的经纬度点。 + +## 对本项目的意义 + +对“自主跟随购物车”项目来说，PointGoalNavigation 值得借鉴的地方是： + +1. Android 手机可以承担上位机主脑，直接跑 TFLite 策略并下发左右轮命令。 +2. ARCore 可以提供短距离相对位姿，但它依赖 Google ARCore 运行环境，国内实机可用性不稳定。 +3. 端到端策略网络可以把图像和目标向量直接映射到差速控制，但这要求训练数据和真实场景分布足够接近。 +4. 当前实现安全兜底较薄，不适合作为购物车首版安全策略直接照搬。 + +如果本项目首版继续保持“OpenBot + Android 手机 + MCU + 差速底盘”的主线，更现实的参考方式是： + +- 复用它的“Android 端模型推理 -> `Vehicle.setControl()` -> 下位机串口命令”链路。 +- 不依赖 ARCore 作为核心能力，因为目标手机环境可能无法稳定安装和运行 Google Play Services for AR。 +- 对购物车跟随主线，应继续采用已有的目标检测、目标锁定、ReID 辅助、距离控制和安全状态机，而不是把 PointGoalNavigation 当作可直接替换的导航模块。 + +## 简化伪代码 + +```java +onStartPointGoalNavigation() { + askUserForForwardAndLeftMeters(); + requireCameraPermission(); + startArCoreSession(); +} + +startDriving(forward, left) { + goalX = -left; // ARCore x positive means right + goalZ = -forward; // ARCore z positive means backward + + startAnchor = currentArCorePose; + targetAnchor = startAnchor.compose(translation(goalX, 0, goalZ)); + navigationPolicy = load("networks/navigation.tflite"); + isRunning = true; +} + +onArCoreUpdate(currentPose, targetPose, cameraImage) { + if (!isRunning) return; + + distance = horizontalDistance(currentPose, targetPose); + if (distance < 0.15) { + stop(); + showGoalReached(); + return; + } + + deltaYaw = yawDifference(currentPose.forward, targetPose - currentPose); + image = yuvToRgbResizeAndCrop(cameraImage, 160, 90); + + control = navigationPolicy.run( + image, + distance, + sin(deltaYaw), + cos(deltaYaw)); + + vehicle.setControl(control.left, control.right); +} + +onArCoreTrackingLostOrPaused() { + stop(); + showFailureMessage(); +} +``` + diff --git a/android/robot/src/main/java/org/openbot/OpenBotApplication.java b/android/robot/src/main/java/org/openbot/OpenBotApplication.java index 78b0b07e3..1dd56b8c5 100644 --- a/android/robot/src/main/java/org/openbot/OpenBotApplication.java +++ b/android/robot/src/main/java/org/openbot/OpenBotApplication.java @@ -28,7 +28,6 @@ public void onCreate() { vehicle = new Vehicle(this, baudRate); vehicle.initBle(); vehicle.connectUsb(); - vehicle.initBle(); if (BuildConfig.DEBUG) { Timber.plant( new Timber.DebugTree() { diff --git a/android/robot/src/main/java/org/openbot/cartfollow/ActionArbitrator.java b/android/robot/src/main/java/org/openbot/cartfollow/ActionArbitrator.java new file mode 100644 index 000000000..a95ad7dac --- /dev/null +++ b/android/robot/src/main/java/org/openbot/cartfollow/ActionArbitrator.java @@ -0,0 +1,108 @@ +package org.openbot.cartfollow; + +import android.graphics.RectF; + +public class ActionArbitrator { + + public BehaviorDecisionResult decide( + FollowState state, + IdentityEvidence identity, + DistanceEvidence distance, + TraversabilityEvidence traversability, + SystemSafetyEvidence safety, + TargetMemory memory, + int frameW) { + if (safety != null && safety.emergencyStop) { + return result(state, BehaviorAction.EMERGENCY_STOP, "emergency_stop", safety.reason, 0f); + } + if (safety != null && (!safety.communicationOk || !safety.detectorOk)) { + return result(state, BehaviorAction.EMERGENCY_STOP, "system_not_ready", safety.reason, 0f); + } + if (state == FollowState.STOP) { + return result(state, BehaviorAction.HARD_STOP, "state_stop", "hard_stop_state", 0f); + } + if (state == FollowState.REACQUIRE_TARGET) { + float conf = identity == null ? 0f : identity.confidence; + String reason = + identity != null + && identity.looseAdmissionOk() + && !identity.bboxDefaultOk() + && !identity.predictionOk() + && identity.trackId != identity.lockedTrackId + ? "motion_gate_failed" + : "reacquire_confirming"; + return result(state, BehaviorAction.REACQUIRE_HOLD, reason, null, conf); + } + if (state == FollowState.IDENTITY_UNCERTAIN) { + float conf = identity == null ? 0f : identity.confidence; + return result(state, BehaviorAction.MOTION_STOP, "identity_uncertain", "motion_stop", conf); + } + if (state == FollowState.LOCKED_PENDING_CONFIRM + || state == FollowState.CONFIRMED_ARMED + || state == FollowState.READY_TO_FOLLOW + || state == FollowState.CAPTURE_TARGET + || state == FollowState.IDLE) { + return result(state, BehaviorAction.MOTION_STOP, "not_ready_to_follow", "motion_stop", 0f); + } + if (state == FollowState.LOST || state == FollowState.SEARCH) { + BehaviorAction searchAction = searchAction(memory, frameW); + if (searchAction == BehaviorAction.MOTION_STOP) { + return result(state, searchAction, "target_lost_no_last_bbox", "motion_stop", 0f); + } + return result(state, searchAction, "target_lost_last_side", null, 0.2f); + } + if (identity != null && !identity.matched) { + return result(state, BehaviorAction.MOTION_STOP, "identity_unmatched", identity.reason, 0f); + } + if (traversability != null && traversability.centerBlocked) { + return result(state, BehaviorAction.BLOCKED_WAIT, "center_blocked", traversability.reason, 0f); + } + if (distance != null + && (distance.state == DistanceState.UNKNOWN || distance.state == DistanceState.TOO_CLOSE)) { + return result( + state, + BehaviorAction.MOTION_STOP, + "distance_" + distance.state.name().toLowerCase(), + distance.reason, + distance.confidence); + } + if ((state == FollowState.FOLLOW || state == FollowState.FOLLOW_CAUTION) + && distance != null + && distance.state == DistanceState.OK) { + return result( + state, + BehaviorAction.FOLLOW_CAUTION, + state == FollowState.FOLLOW_CAUTION ? "follow_caution_distance_ok" : "identity_ok_distance_ok", + null, + identityConfidence(identity)); + } + return result( + state, + BehaviorAction.FOLLOW_SLOW, + "identity_ok_follow_allowed", + null, + identityConfidence(identity)); + } + + private static BehaviorDecisionResult result( + FollowState state, + BehaviorAction action, + String actionReason, + String safetyBlockReason, + float confidence) { + return new BehaviorDecisionResult(state, action, actionReason, safetyBlockReason, confidence); + } + + private static float identityConfidence(IdentityEvidence identity) { + return identity == null ? 0f : identity.confidence; + } + + private static BehaviorAction searchAction(TargetMemory memory, int frameW) { + if (memory == null || frameW <= 0) return BehaviorAction.MOTION_STOP; + RectF lastBbox = memory.getLastBbox(); + if (lastBbox == null) return BehaviorAction.MOTION_STOP; + return lastBbox.centerX() < frameW / 2f + ? BehaviorAction.LOCAL_SEARCH_LEFT + : BehaviorAction.LOCAL_SEARCH_RIGHT; + } +} diff --git a/android/robot/src/main/java/org/openbot/cartfollow/BaseCartFollowFragment.java b/android/robot/src/main/java/org/openbot/cartfollow/BaseCartFollowFragment.java new file mode 100644 index 000000000..52a431560 --- /dev/null +++ b/android/robot/src/main/java/org/openbot/cartfollow/BaseCartFollowFragment.java @@ -0,0 +1,1136 @@ +package org.openbot.cartfollow; + +import android.app.Activity; +import android.graphics.Bitmap; +import android.graphics.Canvas; +import android.graphics.Color; +import android.graphics.Matrix; +import android.graphics.Paint; +import android.graphics.RectF; +import android.os.Bundle; +import android.os.Handler; +import android.os.HandlerThread; +import android.os.SystemClock; +import android.view.LayoutInflater; +import android.view.View; +import android.view.ViewGroup; +import android.widget.Toast; +import androidx.annotation.NonNull; +import androidx.annotation.Nullable; +import androidx.camera.core.CameraSelector; +import androidx.camera.core.ImageProxy; +import java.io.IOException; +import java.util.ArrayList; +import java.util.List; +import java.util.Locale; +import org.jetbrains.annotations.NotNull; +import org.openbot.R; +import org.openbot.cartfollow.diagnostics.CartFollowDiagnosticConfig; +import org.openbot.cartfollow.diagnostics.CartFollowDiagnosticSaver; +import org.openbot.cartfollow.diagnostics.CartFollowDiagnosticSession; +import org.openbot.common.CameraFragment; +import org.openbot.databinding.FragmentHumanCartSimulatorBinding; +import org.openbot.env.ImageUtils; +import org.openbot.tflite.Detector; +import org.openbot.tflite.Model; +import org.openbot.tflite.Network; +import org.openbot.utils.CameraUtils; +import org.openbot.utils.Enums; +import org.openbot.vehicle.Control; +import timber.log.Timber; + +public class BaseCartFollowFragment extends CameraFragment { + + private static final int COLOR_TARGET = 0; + private static final int COLOR_CANDIDATE = 1; + private static final int COLOR_NORMAL = 2; + private static final int COLOR_FAIL = 3; + private static final int RECOVERY_RELOCK_MIN_FRAMES = 2; + + protected FragmentHumanCartSimulatorBinding binding; + private Handler handler; + private HandlerThread handlerThread; + + private boolean computingNetwork = false; + private float minConfidence = 0.5f; + + private Detector detector; + private Matrix frameToCropTransform; + private Bitmap croppedBitmap; + private int sensorOrientation; + private Matrix cropToFrameTransform; + + private Model model; + private Network.Device device = Network.Device.CPU; + private int numThreads = -1; + private final String classType = "person"; + + private long lastProcessingTimeMs = -1; + private long frameNum = 0; + + private final ControlGenerator controlGenerator = new ControlGenerator(); + private final HumanCommandInterpreter interpreter = new HumanCommandInterpreter(); + private final TargetMatcher matcher = new TargetMatcher(); + protected final FollowStateMachine stateMachine = + new FollowStateMachine(matcher, controlGenerator); + private final ActionArbitrator actionArbitrator = new ActionArbitrator(); + private final TargetTrackManager targetTrackManager = new TargetTrackManager(); + private final IdentityBeliefAccumulator beliefAccumulator = new IdentityBeliefAccumulator(); + private ReIDCoordinator reidCoordinator; + private final CartFollowDiagnosticConfig diagnosticConfig = new CartFollowDiagnosticConfig(); + private final CartFollowDiagnosticSaver diagnosticSaver = new CartFollowDiagnosticSaver(); + private CartFollowDiagnosticSession diagnosticSession; + private boolean diagnosticEnabled = false; + private boolean diagnosticActive = false; + private boolean targetEventAwaitingReturn = false; + private boolean showFullDebug = false; + private long lastDiagnosticFrameLogMs = 0L; + private long lastDiagnosticCropMs = 0L; + private long lastDiagnosticGalleryMs = 0L; + private int recoveryRelockTrackId = -1; + private int recoveryRelockFrames = 0; + private Bitmap latestConfirmSnapshot; + + private final List drawBoxes = new ArrayList<>(); + private int drawFrameWidth = 0; + private int drawFrameHeight = 0; + private int drawSensorOrientation = 0; + + private final Paint targetBoxPaint = new Paint(); + private final Paint candidateBoxPaint = new Paint(); + private final Paint personBoxPaint = new Paint(); + private final Paint failBoxPaint = new Paint(); + private final Paint boxTextPaint = new Paint(); + + @Override + public void onCreate(@Nullable Bundle savedInstanceState) { + super.onCreate(savedInstanceState); + targetBoxPaint.setColor(Color.GREEN); + targetBoxPaint.setStyle(Paint.Style.STROKE); + targetBoxPaint.setStrokeWidth(8.0f); + candidateBoxPaint.setColor(Color.YELLOW); + candidateBoxPaint.setStyle(Paint.Style.STROKE); + candidateBoxPaint.setStrokeWidth(8.0f); + personBoxPaint.setColor(Color.WHITE); + personBoxPaint.setStyle(Paint.Style.STROKE); + personBoxPaint.setStrokeWidth(6.0f); + failBoxPaint.setColor(Color.RED); + failBoxPaint.setStyle(Paint.Style.STROKE); + failBoxPaint.setStrokeWidth(8.0f); + boxTextPaint.setColor(Color.WHITE); + boxTextPaint.setTextSize(40.0f); + } + + @Override + public View onCreateView( + @NotNull LayoutInflater inflater, ViewGroup container, Bundle savedInstanceState) { + binding = FragmentHumanCartSimulatorBinding.inflate(inflater, container, false); + return inflateFragment(binding, inflater, container); + } + + @Override + public void onViewCreated(@NonNull View view, @Nullable Bundle savedInstanceState) { + super.onViewCreated(view, savedInstanceState); + reidCoordinator = new ReIDCoordinator(requireActivity(), getNumThreads()); + + binding.confidenceValue.setText((int) (minConfidence * 100) + "%"); + binding.plusConfidence.setOnClickListener( + v -> { + int confValue = (int) (minConfidence * 100); + if (confValue >= 95) return; + confValue += 5; + minConfidence = confValue / 100f; + binding.confidenceValue.setText(confValue + "%"); + controlGenerator.MIN_CONFIDENCE = minConfidence; + }); + binding.minusConfidence.setOnClickListener( + v -> { + int confValue = (int) (minConfidence * 100); + if (confValue <= 5) return; + confValue -= 5; + minConfidence = confValue / 100f; + binding.confidenceValue.setText(confValue + "%"); + controlGenerator.MIN_CONFIDENCE = minConfidence; + }); + + List models = getModelNames(f -> f.type.equals(Model.TYPE.DETECTOR)); + initModelSpinner(binding.modelSpinner, models, preferencesManager.getObjectNavModel()); + + setAnalyserResolution(Enums.Preview.HD.getValue()); + + binding.trackingOverlay.addCallback(canvas -> drawOverlay(canvas)); + binding.btnDebugDetails.setOnClickListener( + v -> { + showFullDebug = !showFullDebug; + binding.btnDebugDetails.setText(showFullDebug ? "收起详情" : "调试详情"); + }); + resetTargetEventButton(); + binding.btnTargetEvent.setOnClickListener(v -> recordTargetEvent()); + binding.diagnosticSwitch.setChecked(false); + binding.diagnosticSwitch.setOnClickListener( + v -> { + diagnosticEnabled = binding.diagnosticSwitch.isChecked(); + if (!diagnosticEnabled) { + stopDiagnosticSession(); + } + resetTargetEventButton(); + }); + + binding.btnConfirm.setOnClickListener( + v -> { + if (reidCoordinator != null) reidCoordinator.confirmGallery(); + int lockedTrackId = + targetTrackManager.lockClosest(stateMachine.getMemory().getLastBbox()); + beliefAccumulator.lockTrack(lockedTrackId); + activateDiagnosticSession(); + if (diagnosticActive && diagnosticSession != null && latestConfirmSnapshot != null) { + diagnosticSaver.saveGallerySnapshotAsync( + latestConfirmSnapshot, diagnosticSession, "confirmed_snapshot"); + } + stateMachine.confirm(); + }); + binding.btnRetake.setOnClickListener( + v -> { + if (reidCoordinator != null) reidCoordinator.reset(); + targetTrackManager.reset(); + beliefAccumulator.reset(); + resetRecoveryRelock(); + stopDiagnosticSession(); + startDiagnosticSession(); + stateMachine.retake(); + }); + binding.btnCancel.setOnClickListener( + v -> { + if (reidCoordinator != null) reidCoordinator.reset(); + targetTrackManager.reset(); + beliefAccumulator.reset(); + resetRecoveryRelock(); + stopDiagnosticSession(); + stateMachine.cancel(); + }); + + binding.startSwitch.setChecked(false); + binding.startSwitch.setOnClickListener( + v -> { + if (binding.startSwitch.isChecked()) { + binding.modelSpinner.setEnabled(false); + if (reidCoordinator != null) reidCoordinator.reset(); + targetTrackManager.reset(); + beliefAccumulator.reset(); + resetRecoveryRelock(); + startDiagnosticSession(); + stateMachine.startCapture(); + } else { + binding.modelSpinner.setEnabled(true); + if (reidCoordinator != null) reidCoordinator.reset(); + targetTrackManager.reset(); + beliefAccumulator.reset(); + resetRecoveryRelock(); + stopDiagnosticSession(); + stateMachine.cancel(); + resetUiToIdle(); + } + }); + onCartFollowViewCreated(); + } + + /** Hook for concrete screens to install controls without duplicating the perception pipeline. */ + protected void onCartFollowViewCreated() {} + + private void resetUiToIdle() { + updateCommandText(getString(R.string.cart_sim_idle)); + updateDebugInfo(FollowState.IDLE, new Control(0f, 0f), 0, 0f, null, null, null); + if (binding != null) { + binding.confirmPanel.setVisibility(View.GONE); + binding.countdownText.setVisibility(View.GONE); + binding.trackingOverlay.postInvalidate(); + } + } + + protected void onInferenceConfigurationChanged() { + computingNetwork = false; + if (croppedBitmap == null) return; + final Network.Device device = getDevice(); + final Model model = getModel(); + final int numThreads = getNumThreads(); + runInBackground(() -> recreateNetwork(model, device, numThreads)); + } + + private void recreateNetwork(Model model, Network.Device device, int numThreads) { + if (model == null) return; + Detector newDetector = null; + try { + newDetector = Detector.create(requireActivity(), model, device, numThreads); + } catch (IllegalArgumentException | IOException e) { + Timber.e(e, "Failed to create network."); + String msg = + model.pathType == Model.PATH_TYPE.URL + ? "该模型未下载，请先在主菜单 Model Management 中下载: " + model.name + : "模型加载失败: " + e.getMessage(); + requireActivity() + .runOnUiThread( + () -> + Toast.makeText(requireContext().getApplicationContext(), msg, Toast.LENGTH_LONG) + .show()); + return; + } + + if (detector != null) { + detector.close(); + } + detector = newDetector; + try { + croppedBitmap = + Bitmap.createBitmap( + detector.getImageSizeX(), detector.getImageSizeY(), Bitmap.Config.ARGB_8888); + frameToCropTransform = + ImageUtils.getTransformationMatrix( + getMaxAnalyseImageSize().getWidth(), + getMaxAnalyseImageSize().getHeight(), + croppedBitmap.getWidth(), + croppedBitmap.getHeight(), + sensorOrientation, + detector.getCropRect(), + detector.getMaintainAspect()); + cropToFrameTransform = new Matrix(); + frameToCropTransform.invert(cropToFrameTransform); + } catch (Exception e) { + Timber.e(e, "Failed to configure detector."); + requireActivity() + .runOnUiThread( + () -> + Toast.makeText( + requireContext().getApplicationContext(), + "模型配置失败: " + e.getMessage(), + Toast.LENGTH_LONG) + .show()); + } + } + + @Override + public synchronized void onResume() { + croppedBitmap = null; + handlerThread = new HandlerThread("inference"); + handlerThread.start(); + handler = new Handler(handlerThread.getLooper()); + super.onResume(); + } + + @Override + public synchronized void onPause() { + onCartFollowPause(); + stopDiagnosticSession(); + handlerThread.quitSafely(); + try { + handlerThread.join(); + handlerThread = null; + handler = null; + } catch (final InterruptedException e) { + e.printStackTrace(); + } + super.onPause(); + } + + /** Called before the inference thread and diagnostics are stopped. */ + protected void onCartFollowPause() {} + + @Override + public void onDestroy() { + diagnosticSaver.shutdown(); + super.onDestroy(); + } + + protected synchronized void runInBackground(final Runnable r) { + if (handler != null) handler.post(r); + } + + @Override + protected void processUSBData(String data) {} + + @Override + protected void processControllerKeyData(String commandType) {} + + @Override + protected void processFrame(Bitmap bitmap, ImageProxy image) { + if (detector == null) { + updateCropImageInfo(); + if (detector == null) return; + } + + ++frameNum; + if (binding == null || !isInferenceEnabled()) return; + if (computingNetwork) return; + + computingNetwork = true; + runInBackground( + () -> { + final Canvas canvas = new Canvas(croppedBitmap); + Bitmap workingFrame = bitmap; + if (lensFacing == CameraSelector.LENS_FACING_FRONT) { + Bitmap flipped = CameraUtils.flipBitmapHorizontal(bitmap); + canvas.drawBitmap(flipped, frameToCropTransform, null); + workingFrame = flipped; + } else { + canvas.drawBitmap(bitmap, frameToCropTransform, null); + } + + if (detector != null) { + final long startTime = SystemClock.elapsedRealtime(); + final List results = + detector.recognizeImage(croppedBitmap, classType); + lastProcessingTimeMs = SystemClock.elapsedRealtime() - startTime; + + final List mappedRecognitions = new ArrayList<>(); + for (final Detector.Recognition result : results) { + final RectF location = result.getLocation(); + if (location != null && result.getConfidence() >= minConfidence) { + cropToFrameTransform.mapRect(location); + result.setLocation(location); + mappedRecognitions.add(result); + } + } + + int frameW = getMaxAnalyseImageSize().getWidth(); + int frameH = getMaxAnalyseImageSize().getHeight(); + targetTrackManager.update( + mappedRecognitions, frameW, frameH, SystemClock.elapsedRealtime()); + FollowState currentState = stateMachine.getState(); + Detector.Recognition largestPerson = selectLargest(mappedRecognitions); + if (currentState == FollowState.CAPTURE_TARGET) { + if (reidCoordinator != null) { + reidCoordinator.collectInitializationCandidate( + workingFrame, largestPerson, sensorOrientation); + } + maybeSaveGalleryCandidate(workingFrame, largestPerson, sensorOrientation); + } + TargetMatcher.MatchResult legacyMatch = + matcher.match( + mappedRecognitions, workingFrame, stateMachine.getMemory(), frameW, frameH); + IdentityEvidence identity = + reidCoordinator == null + ? null + : reidCoordinator.evaluate( + mappedRecognitions, + workingFrame, + stateMachine.getMemory(), + currentState, + frameW, + frameH, + sensorOrientation, + legacyMatch.score, + legacyMatch.matched, + legacyMatch.best); + if (identity != null) { + TargetTrack reidCandidateTrack = + targetTrackManager.getTrackForRecognition(identity.bestCandidate); + identity = + beliefAccumulator.update( + identity, + targetTrackManager, + reidCandidateTrack, + stateMachine.getMemory(), + frameW, + frameH); + } + FollowStateMachine.FrameResult fr = + stateMachine.onFrame( + mappedRecognitions, workingFrame, frameW, frameH, sensorOrientation, identity); + fr.behaviorDecision = decideBehavior(fr, frameW, frameH); + maybeRelockAfterRecovery(fr); + + updateDrawState(fr, frameW, frameH, sensorOrientation); + String commandText = commandForState(fr); + updateCommandText(commandText); + float fps = lastProcessingTimeMs > 0 ? 1000f / lastProcessingTimeMs : 0f; + maybeSaveDiagnostics( + workingFrame, fr, fps, commandText, frameW, frameH, sensorOrientation); + updateDebugInfo( + fr.state, + fr.control, + fr.persons.size(), + fps, + fr.distanceEstimate, + fr.behaviorDecision, + fr.identityEvidence); + updateUiForState(fr); + onFollowFrame(fr); + binding.trackingOverlay.postInvalidate(); + } + computingNetwork = false; + }); + } + + private void updateCropImageInfo() { + sensorOrientation = 90 - ImageUtils.getScreenOrientation(requireActivity()); + recreateNetwork(getModel(), getDevice(), getNumThreads()); + } + + protected boolean isInferenceEnabled() { + return binding.startSwitch.isChecked(); + } + + /** Receives the final behavior decision after all identity and safety arbitration. */ + protected void onFollowFrame(FollowStateMachine.FrameResult frameResult) {} + + private void maybeRelockAfterRecovery(FollowStateMachine.FrameResult fr) { + if (fr == null || fr.identityEvidence == null || fr.behaviorDecision == null) { + resetRecoveryRelock(); + return; + } + IdentityEvidence identity = fr.identityEvidence; + if (!isRelockState(fr.state) + || !isRelockAction(fr.behaviorDecision.selectedAction) + || identity.trackId < 0 + || identity.trackId == identity.lockedTrackId + || !passesRelockMotionGate(identity)) { + resetRecoveryRelock(); + return; + } + + if (recoveryRelockTrackId != identity.trackId) { + recoveryRelockTrackId = identity.trackId; + recoveryRelockFrames = 1; + return; + } + recoveryRelockFrames++; + if (recoveryRelockFrames < RECOVERY_RELOCK_MIN_FRAMES) return; + + if (targetTrackManager.lockTrack(identity.trackId, "relock_after_recovery")) { + beliefAccumulator.lockTrack(identity.trackId); + fr.behaviorDecision = + new BehaviorDecisionResult( + fr.behaviorDecision.state, + fr.behaviorDecision.selectedAction, + appendActionReason(fr.behaviorDecision.actionReason, "relock_after_recovery"), + fr.behaviorDecision.safetyBlockReason, + fr.behaviorDecision.confidence, + fr.behaviorDecision.distanceEvidence, + fr.behaviorDecision.traversabilityEvidence); + } + resetRecoveryRelock(); + } + + private static boolean isRelockState(FollowState state) { + return state == FollowState.REACQUIRE_TARGET + || state == FollowState.READY_TO_FOLLOW + || state == FollowState.FOLLOW_CAUTION + || state == FollowState.FOLLOW; + } + + private static boolean isRelockAction(BehaviorAction action) { + return action == BehaviorAction.FOLLOW_CAUTION || action == BehaviorAction.FOLLOW_SLOW; + } + + private static boolean passesRelockMotionGate(IdentityEvidence identity) { + return identity.bboxDefaultOk() || identity.predictionOk(); + } + + private void resetRecoveryRelock() { + recoveryRelockTrackId = -1; + recoveryRelockFrames = 0; + } + + private static String appendActionReason(String reason, String addition) { + if (reason == null || reason.isEmpty()) return addition; + if (reason.contains(addition)) return reason; + return reason + "|" + addition; + } + + private synchronized void updateDrawState( + FollowStateMachine.FrameResult fr, int frameW, int frameH, int sensorOrientation) { + drawBoxes.clear(); + for (Detector.Recognition r : fr.persons) { + if (r == null || r.getLocation() == null) continue; + int colorType = COLOR_NORMAL; + TargetTrack track = targetTrackManager.getTrackForRecognition(r); + if (track != null && targetTrackManager.isLockedTrack(track)) { + colorType = COLOR_TARGET; + } else if (track != null && track.trackId == targetTrackManager.getSuspectedTrackId()) { + colorType = COLOR_CANDIDATE; + } else if (r == fr.target) { + colorType = fr.matched ? COLOR_TARGET : COLOR_FAIL; + } else if (r == fr.candidate) { + colorType = COLOR_CANDIDATE; + } + String label = null; + if (track != null) { + label = + String.format( + Locale.US, "T%d b=%.2f", track.trackId, beliefAccumulator.getBeliefForTrack(track)); + } + drawBoxes.add(new DrawBox(new RectF(r.getLocation()), colorType, label)); + } + drawFrameWidth = frameW; + drawFrameHeight = frameH; + drawSensorOrientation = sensorOrientation; + } + + private void drawOverlay(Canvas canvas) { + if (drawFrameWidth <= 0 || drawFrameHeight <= 0) return; + final boolean rotated = drawSensorOrientation % 180 == 90; + final float multiplier = + Math.min( + canvas.getHeight() / (float) (rotated ? drawFrameWidth : drawFrameHeight), + canvas.getWidth() / (float) (rotated ? drawFrameHeight : drawFrameWidth)); + Matrix matrix = + ImageUtils.getTransformationMatrix( + drawFrameWidth, + drawFrameHeight, + (int) (multiplier * (rotated ? drawFrameHeight : drawFrameWidth)), + (int) (multiplier * (rotated ? drawFrameWidth : drawFrameHeight)), + drawSensorOrientation, + new RectF(0, 0, 0, 0), + false); + + List snapshot; + synchronized (this) { + snapshot = new ArrayList<>(drawBoxes); + } + for (DrawBox box : snapshot) { + RectF rect = new RectF(box.location); + matrix.mapRect(rect); + Paint paint; + String label = box.label; + switch (box.colorType) { + case COLOR_TARGET: + paint = targetBoxPaint; + if (label == null) label = "目标"; + break; + case COLOR_CANDIDATE: + paint = candidateBoxPaint; + if (label == null) label = "候选"; + break; + case COLOR_FAIL: + paint = failBoxPaint; + if (label == null) label = "匹配失败"; + break; + default: + paint = personBoxPaint; + break; + } + float cornerSize = Math.min(rect.width(), rect.height()) / 8.0f; + canvas.drawRoundRect(rect, cornerSize, cornerSize, paint); + if (label != null) { + canvas.drawText(label, rect.left + cornerSize, rect.top, boxTextPaint); + } + } + } + + private String commandForState(FollowStateMachine.FrameResult fr) { + if (fr.behaviorDecision != null) { + switch (fr.behaviorDecision.selectedAction) { + case LOCAL_SEARCH_LEFT: + return HumanCommandInterpreter.CMD_TURN_LEFT; + case LOCAL_SEARCH_RIGHT: + return HumanCommandInterpreter.CMD_TURN_RIGHT; + case BLOCKED_WAIT: + return "前方受阻，请停止等待"; + case MOTION_STOP: + case HARD_STOP: + case EMERGENCY_STOP: + return HumanCommandInterpreter.CMD_STOP; + case REACQUIRE_HOLD: + return "疑似目标，请停止确认"; + case FOLLOW_SLOW: + case FOLLOW_CAUTION: + default: + break; + } + } + switch (fr.state) { + case IDLE: + return "待命，打开 Start 开始采集目标"; + case CAPTURE_TARGET: + return "采集中，请保持站立"; + case LOCKED_PENDING_CONFIRM: + return "请确认是否跟随此人"; + case CONFIRMED_ARMED: + return "已确认，请回到车前"; + case REACQUIRE_TARGET: + return "重识别中…"; + case READY_TO_FOLLOW: + return fr.countdownSec >= 0 ? fr.countdownSec + " 秒后启动" : "准备启动"; + case FOLLOW: + case FOLLOW_CAUTION: + if (fr.distanceEstimate != null) { + return interpreter.interpret(fr.control, fr.state, fr.distanceEstimate.state); + } + return interpreter.interpret(fr.control, fr.state, fr.tooClose); + case IDENTITY_UNCERTAIN: + return "身份不确定，请停止"; + case LOST: + return "目标丢失，请停止"; + case SEARCH: + return "原地搜索中…"; + case STOP: + return "已停止"; + default: + return "请停止"; + } + } + + private void updateCommandText(String text) { + if (binding == null) return; + requireActivity().runOnUiThread(() -> binding.commandText.setText(text)); + } + + private void updateUiForState(FollowStateMachine.FrameResult fr) { + if (binding == null) return; + requireActivity() + .runOnUiThread( + () -> { + if (binding == null) return; + boolean showConfirm = fr.state == FollowState.LOCKED_PENDING_CONFIRM; + binding.confirmPanel.setVisibility(showConfirm ? View.VISIBLE : View.GONE); + if (showConfirm && fr.snapshot != null) { + latestConfirmSnapshot = fr.snapshot; + binding.snapshotView.setImageBitmap(fr.snapshot); + } + boolean showCountdown = fr.state == FollowState.READY_TO_FOLLOW; + binding.countdownText.setVisibility(showCountdown ? View.VISIBLE : View.GONE); + if (showCountdown) { + binding.countdownText.setText( + fr.countdownSec >= 0 ? String.valueOf(fr.countdownSec) : ""); + } + }); + } + + private void startDiagnosticSession() { + stopDiagnosticSession(); + if (!diagnosticEnabled) { + resetDiagnosticState(); + return; + } + diagnosticSession = new CartFollowDiagnosticSession(requireContext().getApplicationContext()); + diagnosticSession.initCsvFiles(); + diagnosticActive = false; + targetEventAwaitingReturn = false; + latestConfirmSnapshot = null; + lastDiagnosticFrameLogMs = 0L; + lastDiagnosticCropMs = 0L; + lastDiagnosticGalleryMs = 0L; + resetTargetEventButton(); + } + + private void activateDiagnosticSession() { + if (!diagnosticEnabled) { + resetDiagnosticState(); + return; + } + if (diagnosticSession == null) { + startDiagnosticSession(); + } + if (diagnosticSession == null) return; + diagnosticActive = true; + targetEventAwaitingReturn = false; + String detectorName = getModel() == null ? "" : getModel().name; + boolean reidAvailable = reidCoordinator != null && reidCoordinator.isAvailable(); + int gallerySize = reidCoordinator == null ? 0 : reidCoordinator.getGallerySize(); + diagnosticSession.writeSessionInfo( + diagnosticConfig, + detectorName, + minConfidence, + reidAvailable, + gallerySize, + true, + sensorOrientation); + resetTargetEventButton(); + Toast.makeText( + requireContext(), + "Diagnostic: " + diagnosticSession.sessionDir.getAbsolutePath(), + Toast.LENGTH_SHORT) + .show(); + } + + private void stopDiagnosticSession() { + if (diagnosticEnabled && diagnosticSession != null && diagnosticActive) { + diagnosticSaver.saveEventAsync(diagnosticSession, frameNum, "session_stop", ""); + } + resetDiagnosticState(); + } + + private void resetDiagnosticState() { + diagnosticActive = false; + diagnosticSession = null; + targetEventAwaitingReturn = false; + lastDiagnosticFrameLogMs = 0L; + lastDiagnosticCropMs = 0L; + lastDiagnosticGalleryMs = 0L; + resetTargetEventButton(); + } + + private void resetTargetEventButton() { + if (binding == null) return; + Activity activity = getActivity(); + if (activity == null) return; + activity.runOnUiThread( + () -> { + if (binding == null) return; + binding.btnTargetEvent.setEnabled(diagnosticEnabled && diagnosticActive); + binding.btnTargetEvent.setText(targetEventAwaitingReturn ? "目标回到画面" : "目标离开画面"); + }); + } + + private void recordTargetEvent() { + if (!diagnosticEnabled || !diagnosticActive || diagnosticSession == null) return; + String eventType = targetEventAwaitingReturn ? "target_return" : "target_left"; + diagnosticSaver.saveEventAsync(diagnosticSession, frameNum, eventType, ""); + targetEventAwaitingReturn = !targetEventAwaitingReturn; + resetTargetEventButton(); + } + + private void maybeSaveDiagnostics( + Bitmap workingFrame, + FollowStateMachine.FrameResult fr, + float fps, + String commandText, + int frameW, + int frameH, + int sensorOrientation) { + if (!diagnosticEnabled || !diagnosticActive || diagnosticSession == null || fr == null) return; + long now = SystemClock.elapsedRealtime(); + boolean shouldLog = + lastDiagnosticFrameLogMs == 0L + || now - lastDiagnosticFrameLogMs >= diagnosticConfig.frameLogIntervalMs; + boolean shouldSaveCrop = + lastDiagnosticCropMs == 0L || now - lastDiagnosticCropMs >= diagnosticConfig.cropIntervalMs; + if (!shouldLog && !shouldSaveCrop) return; + if (shouldLog) lastDiagnosticFrameLogMs = now; + if (shouldSaveCrop) lastDiagnosticCropMs = now; + + Detector.Recognition locked = recognitionForTrack(targetTrackManager.getLockedTrack()); + TargetTrack suspectedTrack = + targetTrackManager.getTrackById(targetTrackManager.getSuspectedTrackId()); + Detector.Recognition suspected = recognitionForTrack(suspectedTrack); + Detector.Recognition bestReid = + reidCoordinator == null ? null : reidCoordinator.getLastBestCandidate(); + diagnosticSaver.saveFrameAsync( + workingFrame, + diagnosticSession, + diagnosticConfig, + frameNum, + frameW, + frameH, + sensorOrientation, + fps, + fr.persons == null ? 0 : fr.persons.size(), + fr.state.name(), + fr.behaviorDecision, + commandText, + fr.identityEvidence, + locked, + suspected, + bestReid, + shouldSaveCrop); + } + + private void maybeSaveGalleryCandidate( + Bitmap frame, Detector.Recognition candidate, int sensorOrientation) { + if (!diagnosticEnabled || diagnosticSession == null || frame == null || candidate == null) + return; + if (candidate.getLocation() == null) return; + long now = SystemClock.elapsedRealtime(); + if (lastDiagnosticGalleryMs != 0L + && now - lastDiagnosticGalleryMs < diagnosticConfig.cropIntervalMs) { + return; + } + Bitmap crop = + cropPerson( + frame, candidate.getLocation(), diagnosticConfig.paddingRatio, sensorOrientation); + if (crop == null) return; + lastDiagnosticGalleryMs = now; + diagnosticSaver.saveGallerySnapshotAsync( + crop, diagnosticSession, "gallery_candidate_" + frameNum); + crop.recycle(); + } + + private static Detector.Recognition recognitionForTrack(TargetTrack track) { + return track == null || !track.isVisible() ? null : track.recognition; + } + + private static Bitmap cropPerson( + Bitmap frame, RectF bbox, float paddingRatio, int sensorOrientation) { + if (frame == null || bbox == null) return null; + float padX = bbox.width() * paddingRatio; + float padY = bbox.height() * paddingRatio; + int left = clamp((int) (bbox.left - padX), 0, frame.getWidth() - 1); + int top = clamp((int) (bbox.top - padY), 0, frame.getHeight() - 1); + int right = clamp((int) (bbox.right + padX), left + 1, frame.getWidth()); + int bottom = clamp((int) (bbox.bottom + padY), top + 1, frame.getHeight()); + int width = right - left; + int height = bottom - top; + if (width <= 0 || height <= 0) return null; + try { + Bitmap rawCrop = Bitmap.createBitmap(frame, left, top, width, height); + int rotation = ((sensorOrientation % 360) + 360) % 360; + if (rotation == 0) return rawCrop; + Matrix matrix = new Matrix(); + matrix.postRotate(rotation); + Bitmap upright = + Bitmap.createBitmap(rawCrop, 0, 0, rawCrop.getWidth(), rawCrop.getHeight(), matrix, true); + rawCrop.recycle(); + return upright; + } catch (Exception e) { + return null; + } + } + + private static int clamp(int value, int min, int max) { + return Math.max(min, Math.min(max, value)); + } + + private void updateDebugInfo( + FollowState state, + Control control, + int persons, + float fps, + ImageSetpointDistanceEstimator.DistanceEstimate dist, + BehaviorDecisionResult behaviorDecision, + IdentityEvidence identityEvidence) { + if (binding == null) return; + float forward = (control.getLeft() + control.getRight()) / 2f; + float turn = (control.getRight() - control.getLeft()) / 2f; + String distLine; + if (dist != null) { + distLine = + String.format( + Locale.US, + "dist=%s\nhScale=%.2f\naScale=%.2f\nbShift=%+.3f\ndistConf=%.2f", + dist.state.name(), + dist.heightScale, + dist.areaScale, + dist.bottomShift, + dist.confidence); + } else { + distLine = "dist=-"; + } + String behaviorLine; + if (behaviorDecision != null) { + behaviorLine = + String.format( + Locale.US, + "action=%s\nactionReason=%s\nsafetyBlock=%s\nactionConf=%.2f", + behaviorDecision.selectedAction.name(), + behaviorDecision.actionReason, + behaviorDecision.safetyBlockReason == null ? "-" : behaviorDecision.safetyBlockReason, + behaviorDecision.confidence); + if (behaviorDecision.traversabilityEvidence != null) { + TraversabilityEvidence trav = behaviorDecision.traversabilityEvidence; + behaviorLine += + String.format( + Locale.US, + "\ncenterBlocked=%s\nfreeLCR=%.2f/%.2f/%.2f\ntravReason=%s", + trav.centerBlocked, + trav.leftFreeScore, + trav.centerFreeScore, + trav.rightFreeScore, + trav.reason); + } + } else { + behaviorLine = "action=-\nactionReason=-\nsafetyBlock=-\nactionConf=0.00"; + } + String identityLine = buildIdentityDebugLine(identityEvidence); + String fullInfo = + String.format( + Locale.US, + "state=%s\nforward=%.2f\nturn=%.2f\nleft=%.2f\nright=%.2f\npersons=%d\nfps=%.1f\n%s\n%s\n%s", + state.name(), + forward, + turn, + control.getLeft(), + control.getRight(), + persons, + fps, + distLine, + behaviorLine, + identityLine); + String compactInfo = + String.format( + Locale.US, + "fps=%.1f\nstate=%s\naction=%s\npersons=%d\ntrack=%d locked=%d suspected=%d\nbelief=%.2f\nbest=%.3f margin=%.3f\nreidCrop=upright", + fps, + state.name(), + behaviorDecision == null ? "-" : behaviorDecision.selectedAction.name(), + persons, + identityEvidence == null ? -1 : identityEvidence.trackId, + identityEvidence == null ? -1 : identityEvidence.lockedTrackId, + identityEvidence == null ? -1 : identityEvidence.suspectedTrackId, + identityEvidence == null ? 0f : identityEvidence.targetBelief, + identityEvidence == null || identityEvidence.reidMatch == null + ? 0f + : identityEvidence.reidMatch.bestScore, + identityEvidence == null || identityEvidence.reidMatch == null + ? 0f + : identityEvidence.reidMatch.margin); + String info = showFullDebug ? fullInfo : compactInfo; + requireActivity().runOnUiThread(() -> binding.debugInfo.setText(info)); + } + + private String buildIdentityDebugLine(IdentityEvidence identity) { + if (identity == null) { + return "reidAvailable=false\nreidCrop=upright\ngallerySize=0\nbestScore=0.000\nsecondScore=0.000\nmargin=0.000\nweak/mid/strong=false/false/false\nbboxLoose=false bboxDefault=false bboxStrict=false prediction=false\nstableMatchCount=0\ncandidateSwitchCount=0\nreidLatencyMs=0\nreidReason=-\nactiveTrackCount=0\ntrackId=-1 lockedTrackId=-1 suspectedTrackId=-1\ntrackAge=0 missedFrames=0\nbelief=0.00 beliefStable=0 beliefUncertain=0\nbeliefReason=-"; + } + ReIDMatchResult reid = identity.reidMatch; + BboxContinuityEvidence bbox = identity.bboxContinuity; + boolean reidAvailable = reid != null && reid.reidAvailable; + int gallerySize = reid == null ? 0 : reid.gallerySize; + float best = reid == null ? 0f : reid.bestScore; + float second = reid == null ? 0f : reid.secondScore; + float margin = reid == null ? 0f : reid.margin; + long latency = reid == null ? 0L : reid.latencyMs; + String reason = reid == null ? identity.reason : reid.reason; + return String.format( + Locale.US, + "reidAvailable=%s\nreidCrop=upright\ngallerySize=%d\nbestScore=%.3f\nsecondScore=%.3f\nmargin=%.3f\nweak/mid/strong=%s/%s/%s\nbboxLoose=%s bboxDefault=%s bboxStrict=%s prediction=%s\nstableMatchCount=%d\ncandidateSwitchCount=%d\nreidLatencyMs=%d\nreidReason=%s\nactiveTrackCount=%d\ntrackId=%d lockedTrackId=%d suspectedTrackId=%d\ntrackAge=%d missedFrames=%d\nbelief=%.2f reidC=%.2f bboxC=%.2f predC=%.2f switchP=%.2f\nbeliefStable=%d beliefUncertain=%d\nbeliefReason=%s", + reidAvailable, + gallerySize, + best, + second, + margin, + identity.weakOk(), + identity.midOk(), + identity.strongOk(), + bbox != null && bbox.looseAdmissionOk, + bbox != null && bbox.bboxDefaultOk, + bbox != null && bbox.bboxStrictOk, + bbox != null && bbox.predictionOk, + identity.stableMatchCount, + identity.candidateSwitchCount, + latency, + reason == null ? "-" : reason, + identity.activeTrackCount, + identity.trackId, + identity.lockedTrackId, + identity.suspectedTrackId, + identity.trackAge, + identity.missedFrames, + identity.targetBelief, + identity.reidContribution, + identity.bboxContribution, + identity.predictionContribution, + identity.switchPenalty, + identity.beliefStableFrames, + identity.beliefUncertainFrames, + identity.beliefReason == null ? "-" : identity.beliefReason); + } + + private BehaviorDecisionResult decideBehavior( + FollowStateMachine.FrameResult fr, int frameW, int frameH) { + IdentityEvidence identity = + fr.identityEvidence != null + ? fr.identityEvidence + : new IdentityEvidence( + fr.matched ? fr.matchScore : 0f, + fr.matchScore, + fr.matched, + fr.matched ? "matched" : "not_matched"); + DistanceEvidence distance; + if (fr.distanceEstimate != null) { + distance = + new DistanceEvidence( + fr.distanceEstimate.state, + fr.distanceEstimate.confidence, + fr.distanceEstimate.failureReason); + } else { + distance = new DistanceEvidence(DistanceState.UNKNOWN, 0f, "distance_not_available"); + } + TraversabilityEvidence traversability = estimateTraversability(fr, frameW, frameH); + SystemSafetyEvidence safety = createSystemSafetyEvidence(); + BehaviorDecisionResult decision = + actionArbitrator.decide( + fr.state, identity, distance, traversability, safety, stateMachine.getMemory(), frameW); + return new BehaviorDecisionResult( + decision.state, + decision.selectedAction, + decision.actionReason, + decision.safetyBlockReason, + decision.confidence, + distance, + traversability); + } + + private TraversabilityEvidence estimateTraversability( + FollowStateMachine.FrameResult fr, int frameW, int frameH) { + if (fr == null || fr.persons == null || frameW <= 0 || frameH <= 0) { + return new TraversabilityEvidence(1f, 1f, 1f, false, "default_clear"); + } + boolean centerBlocked = false; + float centerFreeScore = 1f; + for (Detector.Recognition person : fr.persons) { + if (person == null || person == fr.target || person.getLocation() == null) continue; + RectF b = person.getLocation(); + float cxRatio = b.centerX() / frameW; + boolean inCenter = cxRatio >= 0.33f && cxRatio <= 0.67f; + boolean lowerBodyRisk = b.bottom >= frameH * 0.55f; + boolean largeEnough = b.width() * b.height() >= frameW * frameH * 0.03f; + if (inCenter && lowerBodyRisk && largeEnough) { + centerBlocked = true; + centerFreeScore = Math.min(centerFreeScore, 0.2f); + } + } + return new TraversabilityEvidence( + 1f, + centerFreeScore, + 1f, + centerBlocked, + centerBlocked ? "non_target_in_center_corridor" : "default_clear"); + } + + protected Model getModel() { + return model; + } + + private static Detector.Recognition selectLargest(List persons) { + Detector.Recognition target = null; + float maxArea = -1f; + if (persons == null) return null; + for (Detector.Recognition r : persons) { + if (r == null || r.getLocation() == null) continue; + RectF loc = r.getLocation(); + float area = loc.width() * loc.height(); + if (area > maxArea) { + maxArea = area; + target = r; + } + } + return target; + } + + @Override + protected void setModel(Model model) { + if (this.model != model) { + this.model = model; + preferencesManager.setObjectNavModel(model.name); + onInferenceConfigurationChanged(); + } + } + + protected Network.Device getDevice() { + return device; + } + + protected int getNumThreads() { + return numThreads; + } + + private static class DrawBox { + final RectF location; + final int colorType; + final String label; + + DrawBox(RectF location, int colorType, String label) { + this.location = location; + this.colorType = colorType; + this.label = label; + } + } + + protected SystemSafetyEvidence createSystemSafetyEvidence() { + return new SystemSafetyEvidence( + false, true, detector != null, detector == null ? "detector_not_ready" : "ok"); + } + + protected boolean isDetectorReady() { + return detector != null; + } +} diff --git a/android/robot/src/main/java/org/openbot/cartfollow/BboxContinuityEvidence.java b/android/robot/src/main/java/org/openbot/cartfollow/BboxContinuityEvidence.java new file mode 100644 index 000000000..b8104ac72 --- /dev/null +++ b/android/robot/src/main/java/org/openbot/cartfollow/BboxContinuityEvidence.java @@ -0,0 +1,89 @@ +package org.openbot.cartfollow; + +import android.graphics.RectF; + +public class BboxContinuityEvidence { + public static final float DEFAULT_CENTER_MAX = 0.25f; + public static final float DEFAULT_X_MAX = 0.25f; + public static final float DEFAULT_AREA_MIN = 0.50f; + public static final float DEFAULT_AREA_MAX = 2.00f; + public static final float STRICT_CENTER_MAX = 0.18f; + public static final float STRICT_X_MAX = 0.18f; + public static final float STRICT_AREA_MIN = 0.60f; + public static final float STRICT_AREA_MAX = 1.67f; + public static final float LOOSE_CENTER_MAX = 0.36f; + public static final float LOOSE_X_MAX = 0.36f; + public static final float LOOSE_AREA_MIN = 0.35f; + public static final float LOOSE_AREA_MAX = 2.80f; + + public final float centerJumpRatio; + public final float xJumpRatio; + public final float areaRatio; + public final float predictionError; + public final boolean looseAdmissionOk; + public final boolean bboxDefaultOk; + public final boolean bboxStrictOk; + public final boolean predictionOk; + public final String reason; + + public BboxContinuityEvidence( + float centerJumpRatio, + float xJumpRatio, + float areaRatio, + float predictionError, + String reason) { + this.centerJumpRatio = centerJumpRatio; + this.xJumpRatio = xJumpRatio; + this.areaRatio = areaRatio; + this.predictionError = predictionError; + this.looseAdmissionOk = + centerJumpRatio <= LOOSE_CENTER_MAX + && xJumpRatio <= LOOSE_X_MAX + && areaRatio >= LOOSE_AREA_MIN + && areaRatio <= LOOSE_AREA_MAX; + this.bboxDefaultOk = + centerJumpRatio <= DEFAULT_CENTER_MAX + && xJumpRatio <= DEFAULT_X_MAX + && areaRatio >= DEFAULT_AREA_MIN + && areaRatio <= DEFAULT_AREA_MAX; + this.bboxStrictOk = + centerJumpRatio <= STRICT_CENTER_MAX + && xJumpRatio <= STRICT_X_MAX + && areaRatio >= STRICT_AREA_MIN + && areaRatio <= STRICT_AREA_MAX; + this.predictionOk = predictionError >= 0f && predictionError <= STRICT_CENTER_MAX; + this.reason = reason; + } + + public static BboxContinuityEvidence unavailable(String reason) { + return new BboxContinuityEvidence(1f, 1f, 0f, -1f, reason); + } + + public static BboxContinuityEvidence from( + RectF candidate, RectF last, RectF previous, int frameW, int frameH) { + if (candidate == null || last == null || frameW <= 0 || frameH <= 0) { + return unavailable("bbox_reference_not_available"); + } + float diag = (float) Math.hypot(frameW, frameH); + float centerJump = + (float) + (Math.hypot(candidate.centerX() - last.centerX(), candidate.centerY() - last.centerY()) + / Math.max(1f, diag)); + float xJump = Math.abs(candidate.centerX() - last.centerX()) / Math.max(1f, frameW); + float areaRatio = area(candidate) / Math.max(1f, area(last)); + float predictionError = -1f; + if (previous != null) { + float predX = last.centerX() + (last.centerX() - previous.centerX()); + float predY = last.centerY() + (last.centerY() - previous.centerY()); + predictionError = + (float) + (Math.hypot(candidate.centerX() - predX, candidate.centerY() - predY) + / Math.max(1f, diag)); + } + return new BboxContinuityEvidence(centerJump, xJump, areaRatio, predictionError, "ok"); + } + + private static float area(RectF b) { + return Math.max(0f, b.width()) * Math.max(0f, b.height()); + } +} diff --git a/android/robot/src/main/java/org/openbot/cartfollow/BehaviorAction.java b/android/robot/src/main/java/org/openbot/cartfollow/BehaviorAction.java new file mode 100644 index 000000000..efe1f3416 --- /dev/null +++ b/android/robot/src/main/java/org/openbot/cartfollow/BehaviorAction.java @@ -0,0 +1,13 @@ +package org.openbot.cartfollow; + +public enum BehaviorAction { + FOLLOW_SLOW, + FOLLOW_CAUTION, + MOTION_STOP, + LOCAL_SEARCH_LEFT, + LOCAL_SEARCH_RIGHT, + BLOCKED_WAIT, + REACQUIRE_HOLD, + HARD_STOP, + EMERGENCY_STOP +} diff --git a/android/robot/src/main/java/org/openbot/cartfollow/BehaviorDecisionResult.java b/android/robot/src/main/java/org/openbot/cartfollow/BehaviorDecisionResult.java new file mode 100644 index 000000000..b8c186046 --- /dev/null +++ b/android/robot/src/main/java/org/openbot/cartfollow/BehaviorDecisionResult.java @@ -0,0 +1,37 @@ +package org.openbot.cartfollow; + +public class BehaviorDecisionResult { + public final FollowState state; + public final BehaviorAction selectedAction; + public final String actionReason; + public final String safetyBlockReason; + public final float confidence; + public final DistanceEvidence distanceEvidence; + public final TraversabilityEvidence traversabilityEvidence; + + public BehaviorDecisionResult( + FollowState state, + BehaviorAction selectedAction, + String actionReason, + String safetyBlockReason, + float confidence) { + this(state, selectedAction, actionReason, safetyBlockReason, confidence, null, null); + } + + public BehaviorDecisionResult( + FollowState state, + BehaviorAction selectedAction, + String actionReason, + String safetyBlockReason, + float confidence, + DistanceEvidence distanceEvidence, + TraversabilityEvidence traversabilityEvidence) { + this.state = state; + this.selectedAction = selectedAction; + this.actionReason = actionReason; + this.safetyBlockReason = safetyBlockReason; + this.confidence = confidence; + this.distanceEvidence = distanceEvidence; + this.traversabilityEvidence = traversabilityEvidence; + } +} diff --git a/android/robot/src/main/java/org/openbot/cartfollow/ControlGenerator.java b/android/robot/src/main/java/org/openbot/cartfollow/ControlGenerator.java new file mode 100644 index 000000000..a9b395dde --- /dev/null +++ b/android/robot/src/main/java/org/openbot/cartfollow/ControlGenerator.java @@ -0,0 +1,127 @@ +package org.openbot.cartfollow; + +import android.graphics.RectF; +import java.util.ArrayList; +import java.util.List; +import org.openbot.tflite.Detector.Recognition; +import org.openbot.vehicle.Control; + +public class ControlGenerator { + + public static class Result { + public final Control control; + public final Recognition target; + public final List persons; + public final boolean tooClose; + public final ImageSetpointDistanceEstimator.DistanceEstimate distanceEstimate; + + public Result( + Control control, + Recognition target, + List persons, + boolean tooClose, + ImageSetpointDistanceEstimator.DistanceEstimate distanceEstimate) { + this.control = control; + this.target = target; + this.persons = persons; + this.tooClose = tooClose; + this.distanceEstimate = distanceEstimate; + } + } + + public float K_TURN = 1.5f; + public float MAX_FORWARD = 0.6f; + public float MIN_CONFIDENCE = 0.5f; + public boolean FLIP_TURN = true; + + public final ImageSetpointDistanceEstimator distanceEstimator = new ImageSetpointDistanceEstimator(); + + public Result generate(List results, int frameW, int frameH, int sensorOrientation) { + List persons = new ArrayList<>(); + if (results != null) { + for (Recognition r : results) { + if (r == null || r.getLocation() == null) continue; + if (r.getConfidence() == null || r.getConfidence() < MIN_CONFIDENCE) continue; + if (!"person".equals(r.getTitle())) continue; + persons.add(r); + } + } + + if (persons.isEmpty() || frameW <= 0 || frameH <= 0) { + return new Result(new Control(0f, 0f), null, persons, false, null); + } + + Recognition target = null; + float maxArea = -1f; + for (Recognition r : persons) { + RectF loc = r.getLocation(); + float area = loc.width() * loc.height(); + if (area > maxArea) { + maxArea = area; + target = r; + } + } + + return generateFromTarget(target, persons, frameW, frameH, sensorOrientation, null); + } + + public Result generateFromTarget( + Recognition target, + List persons, + int frameW, + int frameH, + int sensorOrientation, + TargetMemory memory) { + if (target == null || target.getLocation() == null || frameW <= 0 || frameH <= 0) { + return new Result( + new Control(0f, 0f), + null, + persons == null ? new ArrayList<>() : persons, + false, + null); + } + + RectF loc = target.getLocation(); + boolean rotated = sensorOrientation % 180 == 90; + float imgWidth = rotated ? frameH : frameW; + float imgHeight = rotated ? frameW : frameH; + float centerX = rotated ? loc.centerY() : loc.centerX(); + centerX = Math.max(0f, Math.min(centerX, imgWidth)); + + float xError = centerX / imgWidth - 0.5f; + + ImageSetpointDistanceEstimator.Setpoint setpoint = + memory == null ? null : memory.getDistanceSetpoint(); + ImageSetpointDistanceEstimator.DistanceEstimate est = + distanceEstimator.estimate(target, frameW, frameH, sensorOrientation, setpoint); + + float forward; + boolean tooClose; + switch (est.state) { + case TOO_FAR: + forward = MAX_FORWARD; + tooClose = false; + break; + case TOO_CLOSE: + forward = 0f; + tooClose = true; + break; + case OK: + forward = 0f; + tooClose = false; + break; + case UNKNOWN: + default: + forward = 0f; + tooClose = false; + break; + } + + float turn = K_TURN * xError; + if (FLIP_TURN) turn = -turn; + + float left = forward - turn; + float right = forward + turn; + return new Result(new Control(left, right), target, persons, tooClose, est); + } +} diff --git a/android/robot/src/main/java/org/openbot/cartfollow/DistanceEvidence.java b/android/robot/src/main/java/org/openbot/cartfollow/DistanceEvidence.java new file mode 100644 index 000000000..e1e4c3eb5 --- /dev/null +++ b/android/robot/src/main/java/org/openbot/cartfollow/DistanceEvidence.java @@ -0,0 +1,13 @@ +package org.openbot.cartfollow; + +public class DistanceEvidence { + public final DistanceState state; + public final float confidence; + public final String reason; + + public DistanceEvidence(DistanceState state, float confidence, String reason) { + this.state = state == null ? DistanceState.UNKNOWN : state; + this.confidence = confidence; + this.reason = reason; + } +} diff --git a/android/robot/src/main/java/org/openbot/cartfollow/DistanceState.java b/android/robot/src/main/java/org/openbot/cartfollow/DistanceState.java new file mode 100644 index 000000000..41cf52878 --- /dev/null +++ b/android/robot/src/main/java/org/openbot/cartfollow/DistanceState.java @@ -0,0 +1,8 @@ +package org.openbot.cartfollow; + +public enum DistanceState { + TOO_FAR, + OK, + TOO_CLOSE, + UNKNOWN +} diff --git a/android/robot/src/main/java/org/openbot/cartfollow/FollowState.java b/android/robot/src/main/java/org/openbot/cartfollow/FollowState.java new file mode 100644 index 000000000..204f5eaad --- /dev/null +++ b/android/robot/src/main/java/org/openbot/cartfollow/FollowState.java @@ -0,0 +1,16 @@ +package org.openbot.cartfollow; + +public enum FollowState { + IDLE, + CAPTURE_TARGET, + LOCKED_PENDING_CONFIRM, + CONFIRMED_ARMED, + REACQUIRE_TARGET, + READY_TO_FOLLOW, + FOLLOW, + FOLLOW_CAUTION, + IDENTITY_UNCERTAIN, + LOST, + SEARCH, + STOP +} diff --git a/android/robot/src/main/java/org/openbot/cartfollow/FollowStateMachine.java b/android/robot/src/main/java/org/openbot/cartfollow/FollowStateMachine.java new file mode 100644 index 000000000..01f059048 --- /dev/null +++ b/android/robot/src/main/java/org/openbot/cartfollow/FollowStateMachine.java @@ -0,0 +1,579 @@ +package org.openbot.cartfollow; + +import android.graphics.Bitmap; +import android.graphics.RectF; +import java.util.ArrayList; +import java.util.List; +import org.openbot.tflite.Detector.Recognition; +import org.openbot.vehicle.Control; + +public class FollowStateMachine { + + public static class FrameResult { + public final FollowState state; + public final Control control; + public final Recognition target; + public final Recognition candidate; + public final List persons; + public final boolean matched; + public final boolean tooClose; + public final Bitmap snapshot; + public final int countdownSec; + public float matchScore; + public ImageSetpointDistanceEstimator.DistanceEstimate distanceEstimate; + public BehaviorDecisionResult behaviorDecision; + public IdentityEvidence identityEvidence; + + public FrameResult( + FollowState state, + Control control, + Recognition target, + Recognition candidate, + List persons, + boolean matched, + boolean tooClose, + Bitmap snapshot, + int countdownSec) { + this.state = state; + this.control = control; + this.target = target; + this.candidate = candidate; + this.persons = persons; + this.matched = matched; + this.tooClose = tooClose; + this.snapshot = snapshot; + this.countdownSec = countdownSec; + this.matchScore = 0f; + } + } + + public int CAPTURE_FRAMES = 15; + public int REACQUIRE_MATCH_N = 8; + public int FOLLOW_LOST_M = 10; + public long LOST_TO_SEARCH_MS = 800; + public long SEARCH_TIMEOUT_MS = 18000; + public long IDENTITY_UNCERTAIN_TIMEOUT_MS = 18000; + public long COUNTDOWN_MS = 3000; + public int CAUTION_STABLE_FRAMES = 3; + public int UNCERTAIN_FRAMES = 3; + public int UNCERTAIN_RECOVER_FRAMES = 2; + public int REACQUIRE_STRICT_FRAMES = 1; + public int REACQUIRE_DEFAULT_FRAMES = 2; + public int LOST_RECOVER_FRAMES = 3; + + private final TargetMatcher matcher; + private final ControlGenerator controlGenerator; + private final TargetMemory memory = new TargetMemory(); + + private FollowState state = FollowState.IDLE; + private int captureCount = 0; + private int matchCount = 0; + private int lostCount = 0; + private int midDefaultStreak = 0; + private int strongDefaultStreak = 0; + private int strongStrictStreak = 0; + private int unstableStreak = 0; + private long stateEnterTime = 0L; + private Bitmap snapshot = null; + + public FollowStateMachine(TargetMatcher matcher, ControlGenerator controlGenerator) { + this.matcher = matcher; + this.controlGenerator = controlGenerator; + } + + public FollowState getState() { + return state; + } + + public TargetMemory getMemory() { + return memory; + } + + public void startCapture() { + if (state == FollowState.IDLE || state == FollowState.STOP) { + memory.clear(); + snapshot = null; + captureCount = 0; + resetEvidenceCounters(); + state = FollowState.CAPTURE_TARGET; + } + } + + public void confirm() { + if (state == FollowState.LOCKED_PENDING_CONFIRM) { + state = FollowState.CONFIRMED_ARMED; + matchCount = 0; + } + } + + public void retake() { + if (state == FollowState.LOCKED_PENDING_CONFIRM) { + memory.clear(); + snapshot = null; + captureCount = 0; + state = FollowState.CAPTURE_TARGET; + } + } + + public void cancel() { + memory.clear(); + snapshot = null; + captureCount = 0; + matchCount = 0; + lostCount = 0; + resetEvidenceCounters(); + state = FollowState.IDLE; + } + + public FrameResult onFrame( + List persons, Bitmap frame, int frameW, int frameH, int sensorOrientation) { + return onFrame(persons, frame, frameW, frameH, sensorOrientation, null); + } + + public FrameResult onFrame( + List persons, + Bitmap frame, + int frameW, + int frameH, + int sensorOrientation, + IdentityEvidence externalIdentity) { + List safePersons = persons == null ? new ArrayList<>() : persons; + long now = System.currentTimeMillis(); + + switch (state) { + case IDLE: + return new FrameResult( + FollowState.IDLE, new Control(0f, 0f), null, null, safePersons, false, false, null, -1); + + case CAPTURE_TARGET: { + Recognition cand = selectLargest(safePersons); + if (cand == null || cand.getLocation() == null) { + captureCount = 0; + return new FrameResult( + FollowState.CAPTURE_TARGET, new Control(0f, 0f), null, null, safePersons, false, false, null, -1); + } + captureCount++; + if (captureCount >= CAPTURE_FRAMES) { + memory.captureFromBitmap(frame, cand.getLocation(), frameW, frameH, sensorOrientation); + snapshot = cropSnapshot(frame, cand.getLocation()); + captureCount = 0; + state = FollowState.LOCKED_PENDING_CONFIRM; + return new FrameResult( + FollowState.LOCKED_PENDING_CONFIRM, new Control(0f, 0f), null, null, safePersons, false, false, snapshot, -1); + } + return new FrameResult( + FollowState.CAPTURE_TARGET, new Control(0f, 0f), null, cand, safePersons, false, false, null, -1); + } + + case LOCKED_PENDING_CONFIRM: + return new FrameResult( + FollowState.LOCKED_PENDING_CONFIRM, new Control(0f, 0f), null, null, safePersons, false, false, snapshot, -1); + + case CONFIRMED_ARMED: + if (!safePersons.isEmpty()) { + state = FollowState.REACQUIRE_TARGET; + matchCount = 0; + } + return new FrameResult( + state, new Control(0f, 0f), null, null, safePersons, false, false, null, -1); + + case REACQUIRE_TARGET: { + TargetMatcher.MatchResult m = matcher.match(safePersons, frame, memory, frameW, frameH); + IdentityEvidence id = identityFrom(m, externalIdentity); + updateEvidenceCounters(id, m, safePersons); + if (reacquireReady(id, m, safePersons)) matchCount++; + else matchCount = 0; + Recognition selected = selectedCandidate(id, m); + if (selected != null + && (strongStrictStreak >= REACQUIRE_STRICT_FRAMES + || strongDefaultStreak >= REACQUIRE_DEFAULT_FRAMES + || matchCount >= REACQUIRE_MATCH_N)) { + state = FollowState.READY_TO_FOLLOW; + stateEnterTime = now; + memory.updateDynamic(selected); + resetEvidenceCounters(); + } + FrameResult fr = + new FrameResult( + state, new Control(0f, 0f), selected, null, safePersons, id.matched, false, null, -1); + fillIdentity(fr, id); + return fr; + } + + case READY_TO_FOLLOW: { + int cd = (int) Math.ceil((COUNTDOWN_MS - (now - stateEnterTime)) / 1000.0); + if (now - stateEnterTime >= COUNTDOWN_MS) { + state = FollowState.FOLLOW; + lostCount = 0; + return new FrameResult( + FollowState.FOLLOW, new Control(0f, 0f), null, null, safePersons, false, false, null, -1); + } + TargetMatcher.MatchResult m = matcher.match(safePersons, frame, memory, frameW, frameH); + IdentityEvidence id = identityFrom(m, externalIdentity); + Recognition selected = selectedCandidate(id, m); + FrameResult fr = + new FrameResult( + FollowState.READY_TO_FOLLOW, + new Control(0f, 0f), + selected, + null, + safePersons, + id.matched, + false, + null, + Math.max(0, cd)); + fillIdentity(fr, id); + return fr; + } + + case FOLLOW: { + TargetMatcher.MatchResult m = matcher.match(safePersons, frame, memory, frameW, frameH); + IdentityEvidence id = identityFrom(m, externalIdentity); + updateEvidenceCounters(id, m, safePersons); + Recognition selected = selectedCandidate(id, m); + if (selected != null && followConfident(id, m, safePersons)) { + memory.updateDynamic(selected); + lostCount = 0; + ControlGenerator.Result res = + controlGenerator.generateFromTarget( + selected, safePersons, frameW, frameH, sensorOrientation, memory); + FrameResult fr = + new FrameResult( + FollowState.FOLLOW, res.control, selected, null, safePersons, true, res.tooClose, null, -1); + fillIdentity(fr, id); + fr.distanceEstimate = res.distanceEstimate; + return fr; + } + if (selected != null && followCaution(id, m, safePersons)) { + state = FollowState.FOLLOW_CAUTION; + memory.updateDynamic(selected); + ControlGenerator.Result res = + controlGenerator.generateFromTarget( + selected, safePersons, frameW, frameH, sensorOrientation, memory); + FrameResult fr = + new FrameResult( + FollowState.FOLLOW_CAUTION, + res.control, + selected, + null, + safePersons, + true, + res.tooClose, + null, + -1); + fillIdentity(fr, id); + fr.distanceEstimate = res.distanceEstimate; + return fr; + } + lostCount++; + if (lostCount >= FOLLOW_LOST_M) { + state = FollowState.IDENTITY_UNCERTAIN; + stateEnterTime = now; + resetEvidenceCounters(); + } + FrameResult fr = + new FrameResult( + state, new Control(0f, 0f), null, null, safePersons, false, false, null, -1); + fillIdentity(fr, id); + return fr; + } + + case FOLLOW_CAUTION: { + TargetMatcher.MatchResult m = matcher.match(safePersons, frame, memory, frameW, frameH); + IdentityEvidence id = identityFrom(m, externalIdentity); + updateEvidenceCounters(id, m, safePersons); + Recognition selected = selectedCandidate(id, m); + if (selected != null && midDefaultStreak >= CAUTION_STABLE_FRAMES) { + state = FollowState.FOLLOW; + memory.updateDynamic(selected); + ControlGenerator.Result res = + controlGenerator.generateFromTarget( + selected, safePersons, frameW, frameH, sensorOrientation, memory); + FrameResult fr = + new FrameResult( + FollowState.FOLLOW, res.control, selected, null, safePersons, true, res.tooClose, null, -1); + fillIdentity(fr, id); + fr.distanceEstimate = res.distanceEstimate; + return fr; + } + if (selected != null && followCaution(id, m, safePersons)) { + memory.updateDynamic(selected); + ControlGenerator.Result res = + controlGenerator.generateFromTarget( + selected, safePersons, frameW, frameH, sensorOrientation, memory); + FrameResult fr = + new FrameResult( + FollowState.FOLLOW_CAUTION, + res.control, + selected, + null, + safePersons, + true, + res.tooClose, + null, + -1); + fillIdentity(fr, id); + fr.distanceEstimate = res.distanceEstimate; + return fr; + } + unstableStreak++; + if (unstableStreak >= UNCERTAIN_FRAMES) { + state = FollowState.IDENTITY_UNCERTAIN; + stateEnterTime = now; + resetEvidenceCounters(); + } + FrameResult fr = + new FrameResult( + state, new Control(0f, 0f), null, null, safePersons, false, false, null, -1); + fillIdentity(fr, id); + return fr; + } + + case IDENTITY_UNCERTAIN: { + TargetMatcher.MatchResult m = matcher.match(safePersons, frame, memory, frameW, frameH); + IdentityEvidence id = identityFrom(m, externalIdentity); + updateEvidenceCounters(id, m, safePersons); + Recognition selected = selectedCandidate(id, m); + if (selected != null + && (identityAdmissionReady(id) + || strongStrictStreak >= UNCERTAIN_RECOVER_FRAMES + || midDefaultStreak >= UNCERTAIN_RECOVER_FRAMES)) { + state = FollowState.REACQUIRE_TARGET; + matchCount = 0; + stateEnterTime = now; + } else if (now - stateEnterTime >= IDENTITY_UNCERTAIN_TIMEOUT_MS) { + state = FollowState.STOP; + } + FrameResult fr = + new FrameResult( + state, new Control(0f, 0f), selected, null, safePersons, false, false, null, -1); + fillIdentity(fr, id); + return fr; + } + + case LOST: { + TargetMatcher.MatchResult m = matcher.match(safePersons, frame, memory, frameW, frameH); + IdentityEvidence id = identityFrom(m, externalIdentity); + updateEvidenceCounters(id, m, safePersons); + Recognition selected = selectedCandidate(id, m); + if (selected != null && lostRecoverReady(id, m, safePersons)) { + state = FollowState.REACQUIRE_TARGET; + matchCount = 0; + stateEnterTime = now; + } + if (now - stateEnterTime >= LOST_TO_SEARCH_MS) { + state = FollowState.SEARCH; + stateEnterTime = now; + } + FrameResult fr = + new FrameResult( + state, new Control(0f, 0f), selected, null, safePersons, false, false, null, -1); + fillIdentity(fr, id); + return fr; + } + + case SEARCH: { + TargetMatcher.MatchResult m = matcher.match(safePersons, frame, memory, frameW, frameH); + IdentityEvidence id = identityFrom(m, externalIdentity); + updateEvidenceCounters(id, m, safePersons); + Recognition selected = selectedCandidate(id, m); + if (selected != null && lostRecoverReady(id, m, safePersons)) { + state = FollowState.REACQUIRE_TARGET; + matchCount = 0; + stateEnterTime = now; + } + if (now - stateEnterTime >= SEARCH_TIMEOUT_MS) { + state = FollowState.STOP; + } + FrameResult fr = + new FrameResult( + state, new Control(0f, 0f), selected, null, safePersons, false, false, null, -1); + fillIdentity(fr, id); + return fr; + } + + case STOP: + default: + return new FrameResult( + FollowState.STOP, new Control(0f, 0f), null, null, safePersons, false, false, null, -1); + } + } + + private void fillIdentity(FrameResult fr, IdentityEvidence id) { + if (fr == null || id == null) return; + fr.identityEvidence = id; + fr.matchScore = id.confidence; + } + + private IdentityEvidence identityFrom(TargetMatcher.MatchResult m, IdentityEvidence external) { + if (external != null) return external; + return new IdentityEvidence( + m.matched ? m.score : 0f, + m.score, + m.matched, + m.matched ? "legacy_matched" : "legacy_not_matched", + ReIDMatchResult.unavailable("reid_not_connected", 0), + BboxContinuityEvidence.from( + m.best == null ? null : m.best.getLocation(), + memory.getLastBbox(), + memory.getPreviousBbox(), + 1, + 1), + 0, + 0, + m.best); + } + + private Recognition selectedCandidate(IdentityEvidence id, TargetMatcher.MatchResult m) { + if (id != null && id.bestCandidate != null) return id.bestCandidate; + return m == null ? null : m.best; + } + + private boolean followConfident( + IdentityEvidence id, TargetMatcher.MatchResult m, List persons) { + if (id != null && id.hasBelief()) { + return id.beliefConfirmed() + && id.beliefStableFrames >= 2 + && motionGateOk(id); + } + if (id != null && id.reidAvailable()) { + return id.weakOk() && id.bboxDefaultOk(); + } + return m != null && m.matched && (persons == null || persons.size() <= 1 || id.bboxDefaultOk()); + } + + private boolean followCaution( + IdentityEvidence id, TargetMatcher.MatchResult m, List persons) { + if (id != null && id.hasBelief()) { + return id.beliefCaution() && motionGateOk(id); + } + if (id != null && id.reidAvailable()) { + return id.bboxDefaultOk() && (id.weakOk() || id.midOk()); + } + return m != null && m.matched && id != null && id.bboxDefaultOk() && persons != null && persons.size() <= 1; + } + + private boolean reacquireReady( + IdentityEvidence id, TargetMatcher.MatchResult m, List persons) { + if (id != null && id.hasBelief()) { + return id.beliefConfirmed() + && id.beliefStableFrames >= 3 + && motionGateOk(id); + } + if (id != null && id.reidAvailable()) { + return (id.strongOk() && id.bboxDefaultOk()) || (id.midOk() && id.bboxStrictOk()); + } + return m != null && m.matched && id != null && id.bboxStrictOk() && persons != null && persons.size() <= 1; + } + + private boolean lostRecoverReady( + IdentityEvidence id, TargetMatcher.MatchResult m, List persons) { + if (id != null && id.hasBelief()) { + return identityAdmissionReady(id); + } + if (id != null && id.reidAvailable()) { + return strongStrictStreak >= LOST_RECOVER_FRAMES + || strongDefaultStreak >= LOST_RECOVER_FRAMES + || midDefaultStreak >= LOST_RECOVER_FRAMES; + } + return m != null + && m.matched + && id != null + && id.bboxStrictOk() + && persons != null + && persons.size() <= 1 + && strongStrictStreak >= LOST_RECOVER_FRAMES; + } + + private void updateEvidenceCounters( + IdentityEvidence id, TargetMatcher.MatchResult m, List persons) { + if (id != null && id.hasBelief()) { + boolean safeBbox = motionGateOk(id); + boolean midDefault = id.targetBelief >= IdentityBelief.BELIEF_CAUTION && safeBbox; + boolean strongDefault = id.targetBelief >= IdentityBelief.BELIEF_CONFIRM && safeBbox; + boolean strongStrict = + id.targetBelief >= IdentityBelief.BELIEF_CONFIRM + && (id.bboxStrictOk() || id.predictionOk() || id.trackId == id.lockedTrackId); + midDefaultStreak = midDefault ? midDefaultStreak + 1 : 0; + strongDefaultStreak = strongDefault ? strongDefaultStreak + 1 : 0; + strongStrictStreak = strongStrict ? strongStrictStreak + 1 : 0; + if (midDefault || strongDefault || strongStrict) unstableStreak = 0; + return; + } + boolean reidAvailable = id != null && id.reidAvailable(); + boolean midDefault; + boolean strongDefault; + boolean strongStrict; + if (reidAvailable) { + midDefault = id.midOk() && id.bboxDefaultOk(); + strongDefault = id.strongOk() && id.bboxDefaultOk(); + strongStrict = id.strongOk() && id.bboxStrictOk(); + } else { + boolean legacySafe = + m != null && m.matched && id != null && persons != null && persons.size() <= 1; + midDefault = legacySafe && id.bboxDefaultOk(); + strongDefault = legacySafe && id.bboxDefaultOk(); + strongStrict = legacySafe && id.bboxStrictOk(); + } + midDefaultStreak = midDefault ? midDefaultStreak + 1 : 0; + strongDefaultStreak = strongDefault ? strongDefaultStreak + 1 : 0; + strongStrictStreak = strongStrict ? strongStrictStreak + 1 : 0; + if (midDefault || strongDefault || strongStrict) unstableStreak = 0; + } + + private boolean identityAdmissionReady(IdentityEvidence id) { + if (id == null || !id.hasBelief()) return false; + boolean admission = + id.looseAdmissionOk() || id.bboxDefaultOk() || id.predictionOk() || id.trackId == id.lockedTrackId; + return id.beliefCaution() && id.beliefStableFrames >= UNCERTAIN_RECOVER_FRAMES && admission; + } + + private boolean motionGateOk(IdentityEvidence id) { + if (id == null) return false; + return id.bboxDefaultOk() || id.predictionOk() || id.trackId == id.lockedTrackId; + } + + private void resetEvidenceCounters() { + midDefaultStreak = 0; + strongDefaultStreak = 0; + strongStrictStreak = 0; + unstableStreak = 0; + } + + private static Recognition selectLargest(List persons) { + Recognition target = null; + float maxArea = -1f; + for (Recognition r : persons) { + if (r == null || r.getLocation() == null) continue; + RectF loc = r.getLocation(); + float area = loc.width() * loc.height(); + if (area > maxArea) { + maxArea = area; + target = r; + } + } + return target; + } + + private static Bitmap cropSnapshot(Bitmap frame, RectF bbox) { + if (frame == null || bbox == null) return null; + int fw = frame.getWidth(); + int fh = frame.getHeight(); + int l = clamp((int) bbox.left, 0, fw - 1); + int t = clamp((int) bbox.top, 0, fh - 1); + int r = clamp((int) bbox.right, 1, fw); + int b = clamp((int) bbox.bottom, 1, fh); + int w = r - l; + int h = b - t; + if (w <= 0 || h <= 0) return null; + try { + return Bitmap.createBitmap(frame, l, t, w, h); + } catch (Exception e) { + return null; + } + } + + private static int clamp(int v, int lo, int hi) { + return v < lo ? lo : (v > hi ? hi : v); + } +} diff --git a/android/robot/src/main/java/org/openbot/cartfollow/HumanCartSimulatorFragment.java b/android/robot/src/main/java/org/openbot/cartfollow/HumanCartSimulatorFragment.java new file mode 100644 index 000000000..1b7dd16e5 --- /dev/null +++ b/android/robot/src/main/java/org/openbot/cartfollow/HumanCartSimulatorFragment.java @@ -0,0 +1,4 @@ +package org.openbot.cartfollow; + +/** Human-in-the-loop view of the shared cart-follow perception and behavior pipeline. */ +public class HumanCartSimulatorFragment extends BaseCartFollowFragment {} diff --git a/android/robot/src/main/java/org/openbot/cartfollow/HumanCommandInterpreter.java b/android/robot/src/main/java/org/openbot/cartfollow/HumanCommandInterpreter.java new file mode 100644 index 000000000..e00c163b0 --- /dev/null +++ b/android/robot/src/main/java/org/openbot/cartfollow/HumanCommandInterpreter.java @@ -0,0 +1,65 @@ +package org.openbot.cartfollow; + +import org.openbot.vehicle.Control; + +public class HumanCommandInterpreter { + + public static final String CMD_FORWARD = "请向前慢走"; + public static final String CMD_FORWARD_LEFT = "请向前并左转"; + public static final String CMD_FORWARD_RIGHT = "请向前并右转"; + public static final String CMD_TURN_LEFT = "请原地左转"; + public static final String CMD_TURN_RIGHT = "请原地右转"; + public static final String CMD_STOP = "请停止"; + public static final String CMD_KEEP = "保持距离"; + public static final String CMD_LOST = "目标丢失，请停止"; + public static final String CMD_TOO_CLOSE = "目标太近，请停止"; + public static final String CMD_DIST_UNKNOWN = "距离不明，请停止"; + + public float FORWARD_THRESH = 0.2f; + public float TURN_THRESH = 0.2f; + + public String interpret(Control control, FollowState state, boolean tooClose) { + if (state == FollowState.LOST) return CMD_LOST; + if (tooClose) return CMD_TOO_CLOSE; + + float forward = (control.getLeft() + control.getRight()) / 2f; + float turn = (control.getRight() - control.getLeft()) / 2f; + + boolean movingForward = forward > FORWARD_THRESH; + boolean turningLeft = turn < -TURN_THRESH; + boolean turningRight = turn > TURN_THRESH; + + if (movingForward && !turningLeft && !turningRight) return CMD_FORWARD; + if (movingForward && turningLeft) return CMD_FORWARD_LEFT; + if (movingForward && turningRight) return CMD_FORWARD_RIGHT; + if (turningLeft) return CMD_TURN_LEFT; + if (turningRight) return CMD_TURN_RIGHT; + return CMD_STOP; + } + + public String interpret(Control control, FollowState state, DistanceState distState) { + if (state == FollowState.LOST) return CMD_LOST; + if (distState == DistanceState.UNKNOWN) return CMD_DIST_UNKNOWN; + if (distState == DistanceState.TOO_CLOSE) return CMD_TOO_CLOSE; + + float forward = (control.getLeft() + control.getRight()) / 2f; + float turn = (control.getRight() - control.getLeft()) / 2f; + + boolean movingForward = forward > FORWARD_THRESH; + boolean turningLeft = turn < -TURN_THRESH; + boolean turningRight = turn > TURN_THRESH; + + if (distState == DistanceState.OK) { + if (turningLeft) return CMD_TURN_LEFT; + if (turningRight) return CMD_TURN_RIGHT; + return CMD_KEEP; + } + + if (movingForward && !turningLeft && !turningRight) return CMD_FORWARD; + if (movingForward && turningLeft) return CMD_FORWARD_LEFT; + if (movingForward && turningRight) return CMD_FORWARD_RIGHT; + if (turningLeft) return CMD_TURN_LEFT; + if (turningRight) return CMD_TURN_RIGHT; + return CMD_FORWARD; + } +} diff --git a/android/robot/src/main/java/org/openbot/cartfollow/IdentityBelief.java b/android/robot/src/main/java/org/openbot/cartfollow/IdentityBelief.java new file mode 100644 index 000000000..5a26d601a --- /dev/null +++ b/android/robot/src/main/java/org/openbot/cartfollow/IdentityBelief.java @@ -0,0 +1,38 @@ +package org.openbot.cartfollow; + +public class IdentityBelief { + public static final float BELIEF_CONFIRM = 0.75f; + public static final float BELIEF_CAUTION = 0.55f; + public static final float BELIEF_LOST = 0.30f; + + public final int trackId; + public final float targetBelief; + public final float reidContribution; + public final float bboxContribution; + public final float predictionContribution; + public final float switchPenalty; + public final int stableFrames; + public final int uncertainFrames; + public final String beliefReason; + + public IdentityBelief( + int trackId, + float targetBelief, + float reidContribution, + float bboxContribution, + float predictionContribution, + float switchPenalty, + int stableFrames, + int uncertainFrames, + String beliefReason) { + this.trackId = trackId; + this.targetBelief = targetBelief; + this.reidContribution = reidContribution; + this.bboxContribution = bboxContribution; + this.predictionContribution = predictionContribution; + this.switchPenalty = switchPenalty; + this.stableFrames = stableFrames; + this.uncertainFrames = uncertainFrames; + this.beliefReason = beliefReason; + } +} diff --git a/android/robot/src/main/java/org/openbot/cartfollow/IdentityBeliefAccumulator.java b/android/robot/src/main/java/org/openbot/cartfollow/IdentityBeliefAccumulator.java new file mode 100644 index 000000000..301b0ef7f --- /dev/null +++ b/android/robot/src/main/java/org/openbot/cartfollow/IdentityBeliefAccumulator.java @@ -0,0 +1,401 @@ +package org.openbot.cartfollow; + +import android.os.SystemClock; +import java.util.HashMap; +import java.util.List; +import java.util.Locale; +import java.util.Map; +import org.openbot.tflite.Detector.Recognition; + +public class IdentityBeliefAccumulator { + private static final float NO_SPATIAL_SUPPORT_BELIEF_CAP = IdentityBelief.BELIEF_CAUTION - 0.05f; + + private final Map beliefs = new HashMap<>(); + private final Map stableFrames = new HashMap<>(); + private final Map uncertainFrames = new HashMap<>(); + + private int lockedTrackId = -1; + private int lastSelectedTrackId = -1; + private int candidateSwitchCount = 0; + + public void reset() { + beliefs.clear(); + stableFrames.clear(); + uncertainFrames.clear(); + lockedTrackId = -1; + lastSelectedTrackId = -1; + candidateSwitchCount = 0; + } + + public void lockTrack(int trackId) { + lockedTrackId = trackId; + if (trackId >= 0) { + beliefs.put(trackId, IdentityBelief.BELIEF_CONFIRM); + stableFrames.put(trackId, 1); + uncertainFrames.put(trackId, 0); + lastSelectedTrackId = trackId; + } + } + + public float getBeliefForTrack(TargetTrack track) { + if (track == null) return 0f; + Float value = beliefs.get(track.trackId); + return value == null ? 0f : value; + } + + public IdentityEvidence update( + IdentityEvidence base, + TargetTrackManager trackManager, + TargetTrack reidCandidateTrack, + TargetMemory memory, + int frameW, + int frameH) { + if (trackManager == null) return base; + int managerLockedId = trackManager.getLockedTrackId(); + if (managerLockedId != lockedTrackId) { + lockedTrackId = managerLockedId; + if (lockedTrackId >= 0 && !beliefs.containsKey(lockedTrackId)) { + beliefs.put(lockedTrackId, IdentityBelief.BELIEF_CONFIRM); + stableFrames.put(lockedTrackId, 1); + } + } + + List tracks = trackManager.getTracks(); + TargetTrack lockedTrack = trackManager.getLockedTrack(); + TargetTrack selectedTrack = null; + IdentityBelief selectedBelief = null; + BboxContinuityEvidence selectedBbox = null; + + for (TargetTrack track : tracks) { + IdentityBelief belief = + updateTrackBelief(base, track, trackManager, reidCandidateTrack, lockedTrack, memory, frameW, frameH); + BboxContinuityEvidence bbox = bboxEvidence(track, memory, frameW, frameH); + if (!track.isVisible()) continue; + if (selectedTrack == null || shouldSelect(track, belief, selectedTrack, selectedBelief)) { + selectedTrack = track; + selectedBelief = belief; + selectedBbox = bbox; + } + } + + if (lockedTrack != null + && lockedTrack.isVisible() + && getBeliefForTrack(lockedTrack) >= IdentityBelief.BELIEF_LOST) { + selectedTrack = lockedTrack; + selectedBelief = + new IdentityBelief( + lockedTrack.trackId, + getBeliefForTrack(lockedTrack), + 0f, + 0f, + 0f, + 0f, + getStableFrames(lockedTrack.trackId), + getUncertainFrames(lockedTrack.trackId), + "locked_track_priority"); + selectedBbox = bboxEvidence(lockedTrack, memory, frameW, frameH); + } + + if (selectedTrack != null + && selectedBelief != null + && selectedTrack.trackId != managerLockedId + && selectedBelief.targetBelief >= IdentityBelief.BELIEF_CAUTION + && hasSpatialSupport(selectedTrack, selectedBbox, trackManager, frameW, frameH)) { + boolean accepted = + trackManager.updateSuspectedTrack(selectedTrack.trackId, selectedBelief.targetBelief); + if (!accepted) { + TargetTrack held = trackManager.getTrackById(trackManager.getSuspectedTrackId()); + if (held != null && held.isVisible()) { + selectedTrack = held; + selectedBbox = bboxEvidence(held, memory, frameW, frameH); + selectedBelief = + new IdentityBelief( + held.trackId, + getBeliefForTrack(held), + 0f, + 0f, + 0f, + 0f, + getStableFrames(held.trackId), + getUncertainFrames(held.trackId), + "suspected_dwell_hold"); + } + } + } else if (selectedTrack != null + && selectedBelief != null + && selectedTrack.trackId != managerLockedId + && selectedBelief.targetBelief >= IdentityBelief.BELIEF_LOST + && !hasSpatialSupport(selectedTrack, selectedBbox, trackManager, frameW, frameH)) { + selectedBelief = + new IdentityBelief( + selectedBelief.trackId, + Math.min(selectedBelief.targetBelief, NO_SPATIAL_SUPPORT_BELIEF_CAP), + selectedBelief.reidContribution, + selectedBelief.bboxContribution, + selectedBelief.predictionContribution, + selectedBelief.switchPenalty, + selectedBelief.stableFrames, + selectedBelief.uncertainFrames, + appendReason( + selectedBelief.beliefReason, + "reid_interest_no_spatial_support spatial_support_missing")); + } + + if (selectedTrack == null || selectedBelief == null) { + trackManager.updateSuspectedTrack(-1, 0f); + return withBelief( + base, + null, + null, + null, + -1, + managerLockedId, + -1, + trackManager.getActiveTrackCount(), + "no_visible_track"); + } + + boolean countableSelection = + selectedTrack.trackId == managerLockedId + || hasSpatialSupport(selectedTrack, selectedBbox, trackManager, frameW, frameH); + if (countableSelection && selectedTrack.trackId != lastSelectedTrackId) { + if (lastSelectedTrackId >= 0) candidateSwitchCount++; + lastSelectedTrackId = selectedTrack.trackId; + } + + int suspectedTrackId = -1; + if (selectedTrack.trackId != managerLockedId + && selectedBelief.targetBelief >= IdentityBelief.BELIEF_CAUTION) { + suspectedTrackId = trackManager.getSuspectedTrackId(); + } + if (suspectedTrackId < 0) trackManager.updateSuspectedTrack(-1, 0f); + + return withBelief( + base, + selectedTrack, + selectedBbox, + selectedBelief, + selectedTrack.trackId, + managerLockedId, + suspectedTrackId, + trackManager.getActiveTrackCount(), + selectedBelief.beliefReason); + } + + private IdentityBelief updateTrackBelief( + IdentityEvidence base, + TargetTrack track, + TargetTrackManager trackManager, + TargetTrack reidCandidateTrack, + TargetTrack lockedTrack, + TargetMemory memory, + int frameW, + int frameH) { + float old = getBeliefForTrack(track); + if (old <= 0f) old = track.trackId == lockedTrackId ? IdentityBelief.BELIEF_CAUTION : 0.08f; + + boolean isLocked = track.trackId == lockedTrackId; + boolean lockedVisible = lockedTrack != null && lockedTrack.isVisible(); + boolean isReidCandidate = reidCandidateTrack != null && reidCandidateTrack.trackId == track.trackId; + long now = SystemClock.elapsedRealtime(); + boolean nearLockedGhost = + trackManager != null && trackManager.isNearLockedGhost(track, frameW, frameH, now); + + BboxContinuityEvidence bbox = bboxEvidence(track, memory, frameW, frameH); + boolean spatialSupport = hasSpatialSupport(track, bbox, trackManager, frameW, frameH); + boolean missingSpatialSupport = !isLocked && !spatialSupport; + float rawReidContribution = reidContribution(base, isReidCandidate); + float reidContribution = + missingSpatialSupport ? Math.min(rawReidContribution, 0.04f) : rawReidContribution; + float bboxContribution = bbox.bboxStrictOk ? 0.15f : (bbox.bboxDefaultOk ? 0.10f : 0f); + float predictionContribution = bbox.predictionOk ? 0.06f : 0f; + float looseAdmissionContribution = + !missingSpatialSupport && !bbox.bboxDefaultOk && bbox.looseAdmissionOk && isReidCandidate + ? 0.04f + : 0f; + float lockedContribution = isLocked && track.isVisible() ? 0.08f : 0f; + float ghostContribution = !lockedVisible && nearLockedGhost && track.isVisible() ? 0.06f : 0f; + float ageContribution = track.ageFrames >= 3 && track.isVisible() ? 0.04f : 0f; + float switchPenalty = + isReidCandidate && lastSelectedTrackId >= 0 && track.trackId != lastSelectedTrackId ? 0.12f : 0f; + float lockedProtectionPenalty = + !isLocked && lockedVisible && isReidCandidate ? 0.18f : 0f; + float missedPenalty = Math.min(0.30f, track.missedFrames * 0.08f); + float weakMarginPenalty = + isReidCandidate && base != null && base.reidAvailable() && !base.weakOk() ? 0.08f : 0f; + float spatialSupportPenalty = missingSpatialSupport && isReidCandidate ? 0.06f : 0f; + + float decay = track.isVisible() ? 0.80f : 0.70f; + float positive = + reidContribution + + bboxContribution + + predictionContribution + + looseAdmissionContribution + + lockedContribution + + ghostContribution + + ageContribution; + float penalty = + switchPenalty + + lockedProtectionPenalty + + missedPenalty + + weakMarginPenalty + + spatialSupportPenalty; + float belief = clamp(old * decay + positive - penalty, 0f, 1f); + if (missingSpatialSupport && belief >= IdentityBelief.BELIEF_CAUTION) { + belief = Math.min(belief, NO_SPATIAL_SUPPORT_BELIEF_CAP); + } + + int stable = belief >= IdentityBelief.BELIEF_CAUTION ? getStableFrames(track.trackId) + 1 : 0; + int uncertain = + belief < IdentityBelief.BELIEF_CAUTION ? getUncertainFrames(track.trackId) + 1 : 0; + beliefs.put(track.trackId, belief); + stableFrames.put(track.trackId, stable); + uncertainFrames.put(track.trackId, uncertain); + + String reason = + String.format( + Locale.US, + "belief=%.2f old=%.2f reid=%.2f bbox=%.2f pred=%.2f loose=%.2f ghost=%.2f locked=%s switchPenalty=%.2f missed=%d%s%s%s", + belief, + old, + reidContribution, + bboxContribution, + predictionContribution, + looseAdmissionContribution, + ghostContribution, + isLocked, + switchPenalty + lockedProtectionPenalty, + track.missedFrames, + looseAdmissionContribution > 0f ? " loose_admission_only" : "", + ghostContribution > 0f ? " locked_ghost_reference" : "", + missingSpatialSupport && isReidCandidate + ? " reid_interest_no_spatial_support spatial_support_missing" + : ""); + return new IdentityBelief( + track.trackId, + belief, + reidContribution, + bboxContribution, + predictionContribution, + switchPenalty + lockedProtectionPenalty, + stable, + uncertain, + reason); + } + + private IdentityEvidence withBelief( + IdentityEvidence base, + TargetTrack selectedTrack, + BboxContinuityEvidence bbox, + IdentityBelief beliefDetails, + int trackId, + int lockedTrackId, + int suspectedTrackId, + int activeTrackCount, + String reason) { + ReIDMatchResult reid = + base == null ? ReIDMatchResult.unavailable("identity_base_missing", 0) : base.reidMatch; + Recognition candidate = selectedTrack == null ? null : selectedTrack.recognition; + float belief = selectedTrack == null ? 0f : getBeliefForTrack(selectedTrack); + boolean matched = selectedTrack != null && belief >= IdentityBelief.BELIEF_CAUTION; + float confidence = Math.max(base == null ? 0f : base.confidence, belief); + int stable = trackId >= 0 ? getStableFrames(trackId) : 0; + int uncertain = trackId >= 0 ? getUncertainFrames(trackId) : 0; + int rawSwitchCount = base == null ? 0 : base.candidateSwitchCount; + return new IdentityEvidence( + matched ? belief : 0f, + confidence, + matched, + reason, + reid, + bbox, + Math.max(base == null ? 0 : base.stableMatchCount, stable), + rawSwitchCount + candidateSwitchCount, + candidate, + trackId, + lockedTrackId, + suspectedTrackId, + activeTrackCount, + selectedTrack == null ? 0 : selectedTrack.ageFrames, + selectedTrack == null ? 0 : selectedTrack.missedFrames, + belief, + beliefDetails == null ? 0f : beliefDetails.reidContribution, + beliefDetails == null ? 0f : beliefDetails.bboxContribution, + beliefDetails == null ? 0f : beliefDetails.predictionContribution, + beliefDetails == null ? 0f : beliefDetails.switchPenalty, + stable, + uncertain, + reason); + } + + private boolean shouldSelect( + TargetTrack track, + IdentityBelief belief, + TargetTrack selectedTrack, + IdentityBelief selectedBelief) { + if (selectedTrack == null || selectedBelief == null) return true; + if (track.trackId == lockedTrackId && belief.targetBelief >= IdentityBelief.BELIEF_LOST) { + return true; + } + if (selectedTrack.trackId == lockedTrackId + && selectedBelief.targetBelief >= IdentityBelief.BELIEF_LOST) { + return false; + } + if (belief.targetBelief != selectedBelief.targetBelief) { + return belief.targetBelief > selectedBelief.targetBelief; + } + return track.ageFrames > selectedTrack.ageFrames; + } + + private static boolean hasSpatialSupport( + TargetTrack track, + BboxContinuityEvidence bbox, + TargetTrackManager trackManager, + int frameW, + int frameH) { + if (track == null) return false; + if (bbox != null && (bbox.looseAdmissionOk || bbox.bboxDefaultOk || bbox.predictionOk)) { + return true; + } + return trackManager != null + && trackManager.isNearLockedGhost(track, frameW, frameH, SystemClock.elapsedRealtime()); + } + + private static BboxContinuityEvidence bboxEvidence( + TargetTrack track, TargetMemory memory, int frameW, int frameH) { + if (track == null || memory == null || track.lastBbox == null) { + return BboxContinuityEvidence.unavailable("track_bbox_not_available"); + } + return BboxContinuityEvidence.from( + track.lastBbox, memory.getLastBbox(), memory.getPreviousBbox(), frameW, frameH); + } + + private static float reidContribution(IdentityEvidence base, boolean isReidCandidate) { + if (!isReidCandidate || base == null) return 0f; + if (base.strongOk()) return 0.25f; + if (base.midOk()) return 0.18f; + if (base.weakOk()) return 0.10f; + return base.matched ? 0.05f : 0f; + } + + private static String appendReason(String reason, String addition) { + if (addition == null || addition.isEmpty()) return reason; + if (reason == null || reason.isEmpty()) return addition; + if (reason.contains(addition)) return reason; + return reason + " " + addition; + } + + private int getStableFrames(int trackId) { + Integer value = stableFrames.get(trackId); + return value == null ? 0 : value; + } + + private int getUncertainFrames(int trackId) { + Integer value = uncertainFrames.get(trackId); + return value == null ? 0 : value; + } + + private static float clamp(float value, float min, float max) { + return Math.max(min, Math.min(max, value)); + } +} diff --git a/android/robot/src/main/java/org/openbot/cartfollow/IdentityEvidence.java b/android/robot/src/main/java/org/openbot/cartfollow/IdentityEvidence.java new file mode 100644 index 000000000..296f5306a --- /dev/null +++ b/android/robot/src/main/java/org/openbot/cartfollow/IdentityEvidence.java @@ -0,0 +1,162 @@ +package org.openbot.cartfollow; + +import org.openbot.tflite.Detector.Recognition; + +public class IdentityEvidence { + public final float score; + public final float confidence; + public final boolean matched; + public final String reason; + public final ReIDMatchResult reidMatch; + public final BboxContinuityEvidence bboxContinuity; + public final int stableMatchCount; + public final int candidateSwitchCount; + public final Recognition bestCandidate; + public final int trackId; + public final int lockedTrackId; + public final int suspectedTrackId; + public final int activeTrackCount; + public final int trackAge; + public final int missedFrames; + public final float targetBelief; + public final float reidContribution; + public final float bboxContribution; + public final float predictionContribution; + public final float switchPenalty; + public final int beliefStableFrames; + public final int beliefUncertainFrames; + public final String beliefReason; + + public IdentityEvidence(float score, float confidence, boolean matched, String reason) { + this(score, confidence, matched, reason, null, null, 0, 0, null); + } + + public IdentityEvidence( + float score, + float confidence, + boolean matched, + String reason, + ReIDMatchResult reidMatch, + BboxContinuityEvidence bboxContinuity, + int stableMatchCount, + int candidateSwitchCount, + Recognition bestCandidate) { + this( + score, + confidence, + matched, + reason, + reidMatch, + bboxContinuity, + stableMatchCount, + candidateSwitchCount, + bestCandidate, + -1, + -1, + -1, + 0, + 0, + 0, + 0f, + 0f, + 0f, + 0f, + 0f, + 0, + 0, + null); + } + + public IdentityEvidence( + float score, + float confidence, + boolean matched, + String reason, + ReIDMatchResult reidMatch, + BboxContinuityEvidence bboxContinuity, + int stableMatchCount, + int candidateSwitchCount, + Recognition bestCandidate, + int trackId, + int lockedTrackId, + int suspectedTrackId, + int activeTrackCount, + int trackAge, + int missedFrames, + float targetBelief, + float reidContribution, + float bboxContribution, + float predictionContribution, + float switchPenalty, + int beliefStableFrames, + int beliefUncertainFrames, + String beliefReason) { + this.score = score; + this.confidence = confidence; + this.matched = matched; + this.reason = reason; + this.reidMatch = reidMatch; + this.bboxContinuity = bboxContinuity; + this.stableMatchCount = stableMatchCount; + this.candidateSwitchCount = candidateSwitchCount; + this.bestCandidate = bestCandidate; + this.trackId = trackId; + this.lockedTrackId = lockedTrackId; + this.suspectedTrackId = suspectedTrackId; + this.activeTrackCount = activeTrackCount; + this.trackAge = trackAge; + this.missedFrames = missedFrames; + this.targetBelief = targetBelief; + this.reidContribution = reidContribution; + this.bboxContribution = bboxContribution; + this.predictionContribution = predictionContribution; + this.switchPenalty = switchPenalty; + this.beliefStableFrames = beliefStableFrames; + this.beliefUncertainFrames = beliefUncertainFrames; + this.beliefReason = beliefReason; + } + + public boolean hasBelief() { + return trackId >= 0 || activeTrackCount > 0 || lockedTrackId >= 0; + } + + public boolean beliefConfirmed() { + return hasBelief() && targetBelief >= IdentityBelief.BELIEF_CONFIRM; + } + + public boolean beliefCaution() { + return hasBelief() && targetBelief >= IdentityBelief.BELIEF_CAUTION; + } + + public boolean reidAvailable() { + return reidMatch != null && reidMatch.reidAvailable; + } + + public boolean weakOk() { + return reidAvailable() && reidMatch.weakOk; + } + + public boolean midOk() { + return reidAvailable() && reidMatch.midOk; + } + + public boolean strongOk() { + return reidAvailable() && reidMatch.strongOk; + } + + public boolean bboxDefaultOk() { + return bboxContinuity != null && bboxContinuity.bboxDefaultOk; + } + + public boolean bboxStrictOk() { + return bboxContinuity != null && bboxContinuity.bboxStrictOk; + } + + public boolean looseAdmissionOk() { + return bboxContinuity != null && bboxContinuity.looseAdmissionOk; + } + + public boolean predictionOk() { + return bboxContinuity != null && bboxContinuity.predictionOk; + } +} diff --git a/android/robot/src/main/java/org/openbot/cartfollow/ImageSetpointDistanceEstimator.java b/android/robot/src/main/java/org/openbot/cartfollow/ImageSetpointDistanceEstimator.java new file mode 100644 index 000000000..b49b5c57b --- /dev/null +++ b/android/robot/src/main/java/org/openbot/cartfollow/ImageSetpointDistanceEstimator.java @@ -0,0 +1,138 @@ +package org.openbot.cartfollow; + +import android.graphics.RectF; +import org.openbot.tflite.Detector.Recognition; + +/** + * 基于初始化距离标定的图像伺服距离估计器。 + * + *

核心思想：不恢复真实米制距离，而是比较当前目标图像尺度与初始化时记录的期望图像尺度， + * 输出 {@link DistanceState}（TOO_FAR / OK / TOO_CLOSE / UNKNOWN）。 + * + *

主信号：height_scale = current_height_ratio / desired_height_ratio。 + * 辅信号：area_scale = sqrt(current_area_ratio / desired_area_ratio)。 + * 辅信号：bottom_shift = current_bottom_ratio - desired_bottom_ratio（仅用于显示，不参与判态）。 + */ +public class ImageSetpointDistanceEstimator { + + /** heightScale 低于此值判定 TOO_FAR。 */ + public float FAR_THRESHOLD = 0.85f; + /** heightScale 高于此值判定 TOO_CLOSE。 */ + public float CLOSE_THRESHOLD = 1.15f; + /** height_scale 与 area_scale 的对数差异超过此值时判 UNKNOWN。 */ + public float UNKNOWN_HEIGHT_DISAGREE = 0.3f; + /** bbox 高度占比低于此值时判 UNKNOWN（目标过小，不可信）。 */ + public float MIN_BBOX_HEIGHT_RATIO = 0.1f; + + /** 期望图像尺度（初始化标定值）。 */ + public static class Setpoint { + public final float desiredHeightRatio; + public final float desiredAreaRatio; + public final float desiredBottomRatio; + + public Setpoint(float desiredHeightRatio, float desiredAreaRatio, float desiredBottomRatio) { + this.desiredHeightRatio = desiredHeightRatio; + this.desiredAreaRatio = desiredAreaRatio; + this.desiredBottomRatio = desiredBottomRatio; + } + } + + /** 距离估计结果。 */ + public static class DistanceEstimate { + public final float heightScale; + public final float areaScale; + public final float bottomShift; + public final DistanceState state; + public final float confidence; + public final String failureReason; + + public DistanceEstimate( + float heightScale, + float areaScale, + float bottomShift, + DistanceState state, + float confidence, + String failureReason) { + this.heightScale = heightScale; + this.areaScale = areaScale; + this.bottomShift = bottomShift; + this.state = state; + this.confidence = confidence; + this.failureReason = failureReason; + } + + public static DistanceEstimate unknown(String reason) { + return new DistanceEstimate(0f, 0f, 0f, DistanceState.UNKNOWN, 0f, reason); + } + } + + /** + * 估计当前目标距离状态。 + * + * @param target 当前匹配到的目标 Recognition + * @param frameW 分析帧宽（getMaxAnalyseImageSize） + * @param frameH 分析帧高 + * @param sensorOrientation 传感器方向 + * @param setpoint 初始化标定的期望图像尺度，可为 null + */ + public DistanceEstimate estimate( + Recognition target, int frameW, int frameH, int sensorOrientation, Setpoint setpoint) { + if (target == null || target.getLocation() == null || frameW <= 0 || frameH <= 0) { + return DistanceEstimate.unknown("invalid target or frame size"); + } + if (setpoint == null + || setpoint.desiredHeightRatio <= 0f + || setpoint.desiredAreaRatio <= 0f) { + return DistanceEstimate.unknown("setpoint not initialized"); + } + + RectF loc = target.getLocation(); + boolean rotated = sensorOrientation % 180 == 90; + float imgWidth = rotated ? frameH : frameW; + float imgHeight = rotated ? frameW : frameH; + float boxHeight = rotated ? loc.width() : loc.height(); + float boxWidth = rotated ? loc.height() : loc.width(); + float boxBottom = rotated ? loc.right : loc.bottom; + + if (imgHeight <= 0 || imgWidth <= 0 || boxHeight <= 0) { + return DistanceEstimate.unknown("degenerate bbox or image"); + } + + float currentHeightRatio = boxHeight / imgHeight; + float currentAreaRatio = (boxWidth * boxHeight) / (imgWidth * imgHeight); + float currentBottomRatio = boxBottom / imgHeight; + + if (currentHeightRatio < MIN_BBOX_HEIGHT_RATIO) { + return DistanceEstimate.unknown("bbox too small: " + currentHeightRatio); + } + + float heightScale = currentHeightRatio / setpoint.desiredHeightRatio; + float areaScale = + (float) Math.sqrt(currentAreaRatio / setpoint.desiredAreaRatio); + float bottomShift = currentBottomRatio - setpoint.desiredBottomRatio; + + float logDiff = + (float) Math.abs(Math.log(Math.max(1e-6f, heightScale) / Math.max(1e-6f, areaScale))); + if (logDiff > UNKNOWN_HEIGHT_DISAGREE) { + return new DistanceEstimate( + heightScale, + areaScale, + bottomShift, + DistanceState.UNKNOWN, + 0f, + "height/area disagree: " + logDiff); + } + + DistanceState state; + if (heightScale < FAR_THRESHOLD) { + state = DistanceState.TOO_FAR; + } else if (heightScale > CLOSE_THRESHOLD) { + state = DistanceState.TOO_CLOSE; + } else { + state = DistanceState.OK; + } + + float consistency = 1f - Math.min(1f, Math.abs(heightScale - areaScale)); + return new DistanceEstimate(heightScale, areaScale, bottomShift, state, consistency, null); + } +} diff --git a/android/robot/src/main/java/org/openbot/cartfollow/ManualControlArbiter.java b/android/robot/src/main/java/org/openbot/cartfollow/ManualControlArbiter.java new file mode 100644 index 000000000..74fb0a468 --- /dev/null +++ b/android/robot/src/main/java/org/openbot/cartfollow/ManualControlArbiter.java @@ -0,0 +1,61 @@ +package org.openbot.cartfollow; + +/** + * Owns the single active manual direction control for the real-cart screen. + * + *

A newly pressed direction replaces the preceding one. Releases are accepted only from the + * currently active direction, so a delayed release from an old button cannot stop or overwrite a + * newer command. + */ +final class ManualControlArbiter { + enum Control { + FORWARD, + BACKWARD, + LEFT, + RIGHT + } + + static final class PressResult { + final boolean replacedActiveControl; + final long generation; + + private PressResult(boolean replacedActiveControl, long generation) { + this.replacedActiveControl = replacedActiveControl; + this.generation = generation; + } + } + + private Control activeControl; + private long generation; + + synchronized PressResult press(Control control) { + if (control == null) throw new IllegalArgumentException("control must not be null"); + + boolean replaced = activeControl != null && activeControl != control; + if (activeControl != control) generation++; + activeControl = control; + return new PressResult(replaced, generation); + } + + synchronized boolean release(Control control) { + if (control == null || activeControl != control) return false; + activeControl = null; + generation++; + return true; + } + + synchronized boolean clear() { + if (activeControl == null) return false; + activeControl = null; + generation++; + return true; + } + + synchronized Control getActiveControl() { + return activeControl; + } + + synchronized long getGeneration() { + return generation; + } +} diff --git a/android/robot/src/main/java/org/openbot/cartfollow/ReIDCoordinator.java b/android/robot/src/main/java/org/openbot/cartfollow/ReIDCoordinator.java new file mode 100644 index 000000000..75cbd38e3 --- /dev/null +++ b/android/robot/src/main/java/org/openbot/cartfollow/ReIDCoordinator.java @@ -0,0 +1,344 @@ +package org.openbot.cartfollow; + +import android.app.Activity; +import android.graphics.Bitmap; +import android.graphics.Matrix; +import android.graphics.RectF; +import android.os.SystemClock; +import java.io.IOException; +import java.util.ArrayList; +import java.util.Comparator; +import java.util.List; +import org.openbot.tflite.Detector.Recognition; + +public class ReIDCoordinator { + private static final int MAX_PENDING_GALLERY = 12; + private static final int CONFIRMED_GALLERY_K = 8; + private static final int MAX_CANDIDATES = 3; + private static final long FOLLOW_INTERVAL_MS = 1000; + private static final long HIGH_RISK_INTERVAL_MS = 300; + + private final ReIDFeatureExtractor extractor; + private final String disabledReason; + private final List pendingGallery = new ArrayList<>(); + private final List confirmedGallery = new ArrayList<>(); + + private ReIDMatchResult lastResult = ReIDMatchResult.unavailable("not_started", 0); + private Recognition lastBestCandidate = null; + private BboxContinuityEvidence lastBboxEvidence = + BboxContinuityEvidence.unavailable("not_started"); + private long lastRunTimeMs = 0L; + private int stableMatchCount = 0; + private int candidateSwitchCount = 0; + private int lastBestIndex = -1; + + public ReIDCoordinator(Activity activity, int numThreads) { + ReIDFeatureExtractor created = null; + String reason = null; + try { + created = + new TfliteReIDFeatureExtractor( + activity, TfliteReIDFeatureExtractor.DEFAULT_ASSET_PATH, numThreads); + } catch (IOException | IllegalArgumentException e) { + reason = "reid_model_unavailable"; + } + extractor = created; + disabledReason = reason; + } + + public void reset() { + pendingGallery.clear(); + confirmedGallery.clear(); + lastResult = ReIDMatchResult.unavailable(isAvailable() ? "reset" : disabledReason, 0); + lastBestCandidate = null; + lastBboxEvidence = BboxContinuityEvidence.unavailable("reset"); + lastRunTimeMs = 0L; + stableMatchCount = 0; + candidateSwitchCount = 0; + lastBestIndex = -1; + } + + public boolean isAvailable() { + return extractor != null; + } + + public int getGallerySize() { + return confirmedGallery.size(); + } + + public Recognition getLastBestCandidate() { + return lastBestCandidate; + } + + public BboxContinuityEvidence getLastBboxEvidence() { + return lastBboxEvidence; + } + + public ReIDMatchResult getLastResult() { + return lastResult; + } + + public void collectInitializationCandidate( + Bitmap frame, Recognition candidate, int sensorOrientation) { + if (!isAvailable() || frame == null || candidate == null || candidate.getLocation() == null) { + return; + } + if (pendingGallery.size() >= MAX_PENDING_GALLERY) return; + Bitmap crop = cropPerson(frame, candidate.getLocation(), 0.08f, sensorOrientation); + float[] feature = extractor.extract(crop); + crop.recycle(); + if (feature != null) pendingGallery.add(feature); + } + + public void confirmGallery() { + confirmedGallery.clear(); + if (pendingGallery.isEmpty()) return; + for (float[] feature : selectDiverse(pendingGallery, CONFIRMED_GALLERY_K)) { + confirmedGallery.add(feature); + } + lastResult = + ReIDMatchResult.unavailable( + isAvailable() ? "gallery_confirmed" : disabledReason, confirmedGallery.size()); + } + + public IdentityEvidence evaluate( + List persons, + Bitmap frame, + TargetMemory memory, + FollowState state, + int frameW, + int frameH, + int sensorOrientation, + float legacyScore, + boolean legacyMatched, + Recognition legacyBest) { + ReIDMatchResult result = + maybeRunReID(persons, frame, memory, state, frameW, frameH, sensorOrientation, legacyBest); + Recognition best = lastBestCandidate != null ? lastBestCandidate : legacyBest; + boolean matched = legacyMatched; + float confidence = legacyScore; + if (result.reidAvailable && result.weakOk) { + matched = true; + confidence = result.bestScore; + } + return new IdentityEvidence( + matched ? confidence : 0f, + confidence, + matched, + result.reason, + result, + lastBboxEvidence, + stableMatchCount, + candidateSwitchCount, + best); + } + + private ReIDMatchResult maybeRunReID( + List persons, + Bitmap frame, + TargetMemory memory, + FollowState state, + int frameW, + int frameH, + int sensorOrientation, + Recognition legacyBest) { + if (!isAvailable()) { + lastResult = ReIDMatchResult.unavailable(disabledReason, 0); + lastBestCandidate = legacyBest; + lastBboxEvidence = bboxEvidence(legacyBest, memory, frameW, frameH); + return lastResult; + } + if (confirmedGallery.isEmpty()) { + lastResult = ReIDMatchResult.unavailable("gallery_empty", 0); + lastBestCandidate = legacyBest; + lastBboxEvidence = bboxEvidence(legacyBest, memory, frameW, frameH); + return lastResult; + } + if (persons == null || persons.isEmpty() || frame == null) { + lastResult = ReIDMatchResult.unavailable("no_candidates", confirmedGallery.size()); + lastBestCandidate = null; + lastBboxEvidence = BboxContinuityEvidence.unavailable("no_candidates"); + return lastResult; + } + long now = SystemClock.elapsedRealtime(); + boolean highRisk = + state != FollowState.FOLLOW || persons.size() > 1 || legacyBest == null || !same(lastBestCandidate, legacyBest); + long interval = highRisk ? HIGH_RISK_INTERVAL_MS : FOLLOW_INTERVAL_MS; + if (now - lastRunTimeMs < interval && lastResult.reidAvailable) { + return lastResult; + } + + long start = SystemClock.elapsedRealtime(); + List scores = new ArrayList<>(); + for (CandidateRef ref : candidateRefs(persons, legacyBest)) { + Bitmap crop = cropPerson(frame, ref.recognition.getLocation(), 0.08f, sensorOrientation); + float[] feature = extractor.extract(crop); + crop.recycle(); + if (feature == null) continue; + float score = maxSimilarity(feature, confirmedGallery); + scores.add(new CandidateScore(ref.index, ref.recognition, score)); + } + if (scores.isEmpty()) { + lastResult = ReIDMatchResult.unavailable("feature_extract_failed", confirmedGallery.size()); + lastBestCandidate = legacyBest; + lastBboxEvidence = bboxEvidence(legacyBest, memory, frameW, frameH); + return lastResult; + } + scores.sort((a, b) -> Float.compare(b.score, a.score)); + CandidateScore best = scores.get(0); + float second = scores.size() > 1 ? scores.get(1).score : 0f; + if (best.index == lastBestIndex) { + stableMatchCount++; + } else { + if (lastBestIndex >= 0) candidateSwitchCount++; + stableMatchCount = 1; + lastBestIndex = best.index; + } + lastBestCandidate = best.recognition; + lastBboxEvidence = bboxEvidence(best.recognition, memory, frameW, frameH); + lastRunTimeMs = now; + lastResult = + new ReIDMatchResult( + best.score, + second, + best.index, + confirmedGallery.size(), + true, + SystemClock.elapsedRealtime() - start, + "fresh"); + return lastResult; + } + + private static BboxContinuityEvidence bboxEvidence( + Recognition recognition, TargetMemory memory, int frameW, int frameH) { + if (recognition == null || memory == null || recognition.getLocation() == null) { + return BboxContinuityEvidence.unavailable("candidate_not_available"); + } + return BboxContinuityEvidence.from( + recognition.getLocation(), memory.getLastBbox(), memory.getPreviousBbox(), frameW, frameH); + } + + private static List candidateRefs(List persons, Recognition legacyBest) { + List refs = new ArrayList<>(); + if (legacyBest != null) { + int idx = persons.indexOf(legacyBest); + if (idx >= 0) refs.add(new CandidateRef(idx, legacyBest)); + } + List all = new ArrayList<>(); + for (int i = 0; i < persons.size(); i++) { + Recognition r = persons.get(i); + if (r != null && r.getLocation() != null) all.add(new CandidateRef(i, r)); + } + all.sort( + Comparator.comparingDouble( + (CandidateRef ref) -> -area(ref.recognition.getLocation())) + .thenComparingInt(ref -> ref.index)); + for (CandidateRef ref : all) { + boolean exists = false; + for (CandidateRef current : refs) { + if (current.index == ref.index) { + exists = true; + break; + } + } + if (!exists) refs.add(ref); + if (refs.size() >= MAX_CANDIDATES) break; + } + return refs; + } + + private static Bitmap cropPerson( + Bitmap frame, RectF bbox, float paddingRatio, int sensorOrientation) { + int fw = frame.getWidth(); + int fh = frame.getHeight(); + float padX = bbox.width() * paddingRatio; + float padY = bbox.height() * paddingRatio; + int left = clamp((int) (bbox.left - padX), 0, fw - 1); + int top = clamp((int) (bbox.top - padY), 0, fh - 1); + int right = clamp((int) (bbox.right + padX), 1, fw); + int bottom = clamp((int) (bbox.bottom + padY), 1, fh); + int width = Math.max(1, right - left); + int height = Math.max(1, bottom - top); + Bitmap rawCrop = Bitmap.createBitmap(frame, left, top, width, height); + int rotation = ((sensorOrientation % 360) + 360) % 360; + if (rotation == 0) return rawCrop; + Matrix matrix = new Matrix(); + matrix.postRotate(rotation); + Bitmap upright = + Bitmap.createBitmap(rawCrop, 0, 0, rawCrop.getWidth(), rawCrop.getHeight(), matrix, true); + rawCrop.recycle(); + return upright; + } + + private static List selectDiverse(List features, int k) { + List selected = new ArrayList<>(); + if (features.isEmpty()) return selected; + selected.add(features.get(0)); + while (selected.size() < k && selected.size() < features.size()) { + float bestDistance = -1f; + float[] best = null; + for (float[] candidate : features) { + if (selected.contains(candidate)) continue; + float nearestSimilarity = -1f; + for (float[] existing : selected) { + nearestSimilarity = Math.max(nearestSimilarity, dot(candidate, existing)); + } + float distance = 1f - nearestSimilarity; + if (distance > bestDistance) { + bestDistance = distance; + best = candidate; + } + } + if (best == null) break; + selected.add(best); + } + return selected; + } + + private static float maxSimilarity(float[] feature, List gallery) { + float best = -1f; + for (float[] g : gallery) best = Math.max(best, dot(feature, g)); + return Math.max(0f, best); + } + + private static float dot(float[] a, float[] b) { + int n = Math.min(a.length, b.length); + float sum = 0f; + for (int i = 0; i < n; i++) sum += a[i] * b[i]; + return sum; + } + + private static boolean same(Recognition a, Recognition b) { + return a != null && b != null && a == b; + } + + private static float area(RectF b) { + return Math.max(0f, b.width()) * Math.max(0f, b.height()); + } + + private static int clamp(int v, int lo, int hi) { + return v < lo ? lo : (v > hi ? hi : v); + } + + private static class CandidateRef { + final int index; + final Recognition recognition; + + CandidateRef(int index, Recognition recognition) { + this.index = index; + this.recognition = recognition; + } + } + + private static class CandidateScore { + final int index; + final Recognition recognition; + final float score; + + CandidateScore(int index, Recognition recognition, float score) { + this.index = index; + this.recognition = recognition; + this.score = score; + } + } +} diff --git a/android/robot/src/main/java/org/openbot/cartfollow/ReIDFeatureExtractor.java b/android/robot/src/main/java/org/openbot/cartfollow/ReIDFeatureExtractor.java new file mode 100644 index 000000000..9cfdb0a00 --- /dev/null +++ b/android/robot/src/main/java/org/openbot/cartfollow/ReIDFeatureExtractor.java @@ -0,0 +1,5 @@ +package org.openbot.cartfollow; + +public interface ReIDFeatureExtractor { + float[] extract(android.graphics.Bitmap personCrop); +} diff --git a/android/robot/src/main/java/org/openbot/cartfollow/ReIDMatchResult.java b/android/robot/src/main/java/org/openbot/cartfollow/ReIDMatchResult.java new file mode 100644 index 000000000..a1f835331 --- /dev/null +++ b/android/robot/src/main/java/org/openbot/cartfollow/ReIDMatchResult.java @@ -0,0 +1,47 @@ +package org.openbot.cartfollow; + +public class ReIDMatchResult { + public static final float BEST_WEAK = 0.75f; + public static final float MARGIN_WEAK = 0.03f; + public static final float BEST_MID = 0.80f; + public static final float MARGIN_MID = 0.05f; + public static final float BEST_STRONG = 0.85f; + public static final float MARGIN_STRONG = 0.05f; + + public final float bestScore; + public final float secondScore; + public final float margin; + public final int bestCandidateIndex; + public final int gallerySize; + public final boolean reidAvailable; + public final boolean weakOk; + public final boolean midOk; + public final boolean strongOk; + public final long latencyMs; + public final String reason; + + public ReIDMatchResult( + float bestScore, + float secondScore, + int bestCandidateIndex, + int gallerySize, + boolean reidAvailable, + long latencyMs, + String reason) { + this.bestScore = bestScore; + this.secondScore = secondScore; + this.margin = bestScore - secondScore; + this.bestCandidateIndex = bestCandidateIndex; + this.gallerySize = gallerySize; + this.reidAvailable = reidAvailable; + this.weakOk = reidAvailable && bestScore >= BEST_WEAK && margin >= MARGIN_WEAK; + this.midOk = reidAvailable && bestScore >= BEST_MID && margin >= MARGIN_MID; + this.strongOk = reidAvailable && bestScore >= BEST_STRONG && margin >= MARGIN_STRONG; + this.latencyMs = latencyMs; + this.reason = reason; + } + + public static ReIDMatchResult unavailable(String reason, int gallerySize) { + return new ReIDMatchResult(0f, 0f, -1, gallerySize, false, 0L, reason); + } +} diff --git a/android/robot/src/main/java/org/openbot/cartfollow/RealCartFollowFragment.java b/android/robot/src/main/java/org/openbot/cartfollow/RealCartFollowFragment.java new file mode 100644 index 000000000..c94e259bc --- /dev/null +++ b/android/robot/src/main/java/org/openbot/cartfollow/RealCartFollowFragment.java @@ -0,0 +1,284 @@ +package org.openbot.cartfollow; + +import android.os.Handler; +import android.os.Looper; +import android.os.SystemClock; +import android.view.MotionEvent; +import android.view.View; +import androidx.navigation.Navigation; +import org.openbot.R; +import org.openbot.vehicle.Control; + +/** Camera-based cart following with BLE manual control and guarded experimental autonomy. */ +public class RealCartFollowFragment extends BaseCartFollowFragment { + private static final long COMMAND_REPEAT_MS = 100L; + private static final long HANDSHAKE_RETRY_MS = 500L; + private static final long AUTO_UNLOCK_HOLD_MS = 2000L; + + private final RealCartSafetyController safetyController = new RealCartSafetyController(); + private final ManualControlArbiter manualControlArbiter = new ManualControlArbiter(); + private final Handler mainHandler = new Handler(Looper.getMainLooper()); + private volatile RealCartSafetyController.Output latestOutput = + RealCartSafetyController.stop("idle"); + private boolean schedulerRunning; + private long lastHandshakeRequestMs; + private View activeManualButton; + + private final Runnable commandScheduler = + new Runnable() { + @Override + public void run() { + if (!schedulerRunning || binding == null) return; + updateConnectionState(); + long now = SystemClock.elapsedRealtime(); + if (vehicle.isBleSerialReady() + && !vehicle.isCartFirmwareReady() + && now - lastHandshakeRequestMs >= HANDSHAKE_RETRY_MS) { + vehicle.requestVehicleConfig(); + lastHandshakeRequestMs = now; + } + RealCartSafetyController.Output watchdog = safetyController.watchdog(now); + if (watchdog != null) latestOutput = watchdog; + sendOutput(latestOutput); + refreshRealUi(); + mainHandler.postDelayed(this, COMMAND_REPEAT_MS); + } + }; + + @Override + protected void onCartFollowViewCreated() { + vehicle.useBluetoothConnection(); + binding.realControlPanel.setVisibility(View.VISIBLE); + binding.realModeGroup.check(R.id.real_mode_manual); + binding.startSwitch.setChecked(false); + binding.startSwitch.setEnabled(false); + + binding.realModeGroup.addOnButtonCheckedListener( + (group, checkedId, isChecked) -> { + if (!isChecked) return; + setMode( + checkedId == R.id.real_mode_auto + ? RealCartSafetyController.Mode.AUTO + : RealCartSafetyController.Mode.MANUAL); + }); + + installDeadMan( + binding.driveForward, + ManualControlArbiter.Control.FORWARD, + RealCartSafetyController.MANUAL_FORWARD, + RealCartSafetyController.MANUAL_FORWARD); + installDeadMan( + binding.driveBackward, + ManualControlArbiter.Control.BACKWARD, + -RealCartSafetyController.MANUAL_REVERSE, + -RealCartSafetyController.MANUAL_REVERSE); + installDeadMan( + binding.driveLeft, + ManualControlArbiter.Control.LEFT, + -RealCartSafetyController.MANUAL_TURN, + RealCartSafetyController.MANUAL_TURN); + installDeadMan( + binding.driveRight, + ManualControlArbiter.Control.RIGHT, + RealCartSafetyController.MANUAL_TURN, + -RealCartSafetyController.MANUAL_TURN); + + binding.connectBle.setOnClickListener( + v -> Navigation.findNavController(requireView()).navigate(R.id.open_bluetooth_fragment)); + installAutoUnlock(); + binding.emergencyStop.setOnClickListener( + v -> { + safetyController.latchEmergency(); + invalidateManualControl("emergency_stop", true); + vehicle.emergencyStop(); + binding.startSwitch.setChecked(false); + binding.startSwitch.setEnabled(false); + stateMachine.cancel(); + refreshRealUi(); + }); + setMode(RealCartSafetyController.Mode.MANUAL); + } + + @Override + public synchronized void onResume() { + super.onResume(); + safetyController.setForeground(true); + if (vehicle.isBleSerialReady()) vehicle.startHeartbeat(); + startScheduler(); + } + + @Override + protected void onCartFollowPause() { + safetyController.setForeground(false); + invalidateManualControl("paused", true); + vehicle.stopHeartbeat(); + schedulerRunning = false; + mainHandler.removeCallbacks(commandScheduler); + if (binding != null) { + binding.startSwitch.setChecked(false); + binding.startSwitch.setEnabled(false); + } + stateMachine.cancel(); + } + + @Override + protected boolean isInferenceEnabled() { + return safetyController.getMode() == RealCartSafetyController.Mode.AUTO + && safetyController.isAutoUnlocked() + && binding.startSwitch.isChecked(); + } + + @Override + protected void onFollowFrame(FollowStateMachine.FrameResult frameResult) { + latestOutput = safetyController.auto(frameResult, SystemClock.elapsedRealtime()); + } + + @Override + protected SystemSafetyEvidence createSystemSafetyEvidence() { + boolean communicationReady = vehicle != null && vehicle.isCartFirmwareReady(); + return new SystemSafetyEvidence( + safetyController.isEmergencyLatched(), + communicationReady, + isDetectorReady(), + communicationReady ? (isDetectorReady() ? "ok" : "detector_not_ready") : "ble_not_ready"); + } + + @Override + protected void processUSBData(String data) { + updateConnectionState(); + } + + private void setMode(RealCartSafetyController.Mode mode) { + invalidateManualControl("mode_change", true); + safetyController.setMode(mode); + stateMachine.cancel(); + binding.startSwitch.setChecked(false); + boolean auto = mode == RealCartSafetyController.Mode.AUTO; + binding.manualDriveControls.setVisibility(auto ? View.GONE : View.VISIBLE); + binding.unlockAuto.setVisibility(auto ? View.VISIBLE : View.GONE); + binding.realSafetyNotice.setVisibility(auto ? View.VISIBLE : View.GONE); + binding.startSwitch.setEnabled(false); + refreshRealUi(); + } + + private void installDeadMan( + View button, ManualControlArbiter.Control control, int left, int right) { + button.setOnTouchListener( + (view, event) -> { + switch (event.getActionMasked()) { + case MotionEvent.ACTION_DOWN: + RealCartSafetyController.Output nextOutput = safetyController.manual(left, right); + if (nextOutput.isStop()) { + invalidateManualControl("manual_blocked", true); + view.setPressed(false); + return true; + } + + ManualControlArbiter.PressResult pressResult = manualControlArbiter.press(control); + if (pressResult.replacedActiveControl) { + // c0,0 bypasses the firmware ramp. Send it before the new target so the AT8236 + // starts the replacement direction from a known zero output. + latestOutput = RealCartSafetyController.stop("manual_replace"); + sendOutput(latestOutput); + if (activeManualButton != null && activeManualButton != view) { + activeManualButton.setPressed(false); + } + } + activeManualButton = view; + latestOutput = nextOutput; + sendOutput(latestOutput); + view.setPressed(true); + return true; + case MotionEvent.ACTION_UP: + case MotionEvent.ACTION_CANCEL: + case MotionEvent.ACTION_OUTSIDE: + view.setPressed(false); + if (manualControlArbiter.release(control)) { + activeManualButton = null; + latestOutput = RealCartSafetyController.stop("manual_release"); + sendOutput(latestOutput); + } + return true; + default: + return true; + } + }); + } + + private void installAutoUnlock() { + final Runnable unlock = + () -> { + if (binding != null && binding.unlockAuto.isPressed() && safetyController.unlockAuto()) { + binding.startSwitch.setEnabled(true); + refreshRealUi(); + } + }; + binding.unlockAuto.setOnTouchListener( + (view, event) -> { + if (event.getActionMasked() == MotionEvent.ACTION_DOWN) { + view.setPressed(true); + mainHandler.postDelayed(unlock, AUTO_UNLOCK_HOLD_MS); + } else if (event.getActionMasked() == MotionEvent.ACTION_UP + || event.getActionMasked() == MotionEvent.ACTION_CANCEL) { + view.setPressed(false); + mainHandler.removeCallbacks(unlock); + } + return true; + }); + } + + private void updateConnectionState() { + boolean serialReady = vehicle != null && vehicle.isBleSerialReady(); + boolean firmwareReady = vehicle != null && vehicle.isCartFirmwareReady(); + safetyController.setConnection(serialReady, firmwareReady); + if (!firmwareReady) { + invalidateManualControl("ble_not_ready", false); + } + } + + private void invalidateManualControl(String reason, boolean sendStop) { + manualControlArbiter.clear(); + if (activeManualButton != null) activeManualButton.setPressed(false); + activeManualButton = null; + latestOutput = RealCartSafetyController.stop(reason); + if (sendStop) sendOutput(latestOutput); + } + + private void startScheduler() { + if (schedulerRunning) return; + schedulerRunning = true; + mainHandler.post(commandScheduler); + } + + private void sendOutput(RealCartSafetyController.Output output) { + if (vehicle == null || output == null) return; + int multiplier = Math.max(1, vehicle.getSpeedMultiplier()); + vehicle.setControl( + new Control(output.left / (float) multiplier, output.right / (float) multiplier)); + } + + private void refreshRealUi() { + if (binding == null) return; + requireActivity() + .runOnUiThread( + () -> { + if (binding == null) return; + String connection = + vehicle.isCartFirmwareReady() + ? "BLE 已就绪 · CART_AT8236" + : vehicle.isBleSerialReady() ? "BLE 已连接 · 等待固件握手" : "BLE 未连接"; + String output = + latestOutput == null ? "0,0" : latestOutput.left + "," + latestOutput.right; + binding.realConnectionStatus.setText(connection + " · 输出 " + output); + boolean emergency = safetyController.isEmergencyLatched(); + binding.emergencyStop.setEnabled(!emergency); + binding.unlockAuto.setEnabled(!emergency && vehicle.isCartFirmwareReady()); + if (emergency) { + binding.realSafetyNotice.setVisibility(View.VISIBLE); + binding.realSafetyNotice.setText("急停已锁存，请重启 ESP32 后重新连接"); + } else { + binding.realSafetyNotice.setText("近场传感器未接入，仅限空旷实验"); + } + }); + } +} diff --git a/android/robot/src/main/java/org/openbot/cartfollow/RealCartSafetyController.java b/android/robot/src/main/java/org/openbot/cartfollow/RealCartSafetyController.java new file mode 100644 index 000000000..1ff404f60 --- /dev/null +++ b/android/robot/src/main/java/org/openbot/cartfollow/RealCartSafetyController.java @@ -0,0 +1,146 @@ +package org.openbot.cartfollow; + +import org.openbot.vehicle.Control; + +/** Pure safety gate that converts UI or behavior decisions into bounded protocol commands. */ +public final class RealCartSafetyController { + public enum Mode { + MANUAL, + AUTO + } + + public static final int MANUAL_FORWARD = 28; + public static final int MANUAL_REVERSE = 24; + public static final int MANUAL_TURN = 20; + public static final int AUTO_MAX = 32; + public static final int SEARCH_SPEED = 18; + public static final long INFERENCE_TIMEOUT_MS = 400L; + public static final long SEARCH_LIMIT_MS = 2000L; + + public static final class Output { + public final int left; + public final int right; + public final String reason; + + private Output(int left, int right, String reason) { + this.left = left; + this.right = right; + this.reason = reason; + } + + public boolean isStop() { + return left == 0 && right == 0; + } + } + + private Mode mode = Mode.MANUAL; + private boolean foreground; + private boolean connected; + private boolean firmwareReady; + private boolean autoUnlocked; + private boolean emergencyLatched; + private long lastInferenceMs = -1L; + private long searchStartMs = -1L; + + public synchronized void setForeground(boolean foreground) { + this.foreground = foreground; + if (!foreground) autoUnlocked = false; + } + + public synchronized void setConnection(boolean connected, boolean firmwareReady) { + this.connected = connected; + this.firmwareReady = firmwareReady; + if (!connected || !firmwareReady) autoUnlocked = false; + } + + public synchronized void setMode(Mode mode) { + this.mode = mode; + autoUnlocked = false; + searchStartMs = -1L; + lastInferenceMs = -1L; + } + + public synchronized Mode getMode() { + return mode; + } + + public synchronized boolean unlockAuto() { + autoUnlocked = + mode == Mode.AUTO && foreground && connected && firmwareReady && !emergencyLatched; + return autoUnlocked; + } + + public synchronized boolean isAutoUnlocked() { + return autoUnlocked; + } + + public synchronized void latchEmergency() { + emergencyLatched = true; + autoUnlocked = false; + } + + public synchronized boolean isEmergencyLatched() { + return emergencyLatched; + } + + public synchronized Output manual(int left, int right) { + if (!canMove() || mode != Mode.MANUAL) return stop("manual_blocked"); + return new Output(left, right, "manual"); + } + + public synchronized Output auto(FollowStateMachine.FrameResult frame, long nowMs) { + lastInferenceMs = nowMs; + if (!canMove() || mode != Mode.AUTO || !autoUnlocked || frame == null) { + return stop("auto_blocked"); + } + + BehaviorDecisionResult decision = frame.behaviorDecision; + if (decision == null) return stop("decision_missing"); + + switch (decision.selectedAction) { + case FOLLOW_SLOW: + searchStartMs = -1L; + return scale(frame.control, AUTO_MAX, "follow_slow"); + case FOLLOW_CAUTION: + searchStartMs = -1L; + return scale(frame.control, Math.round(AUTO_MAX * 0.65f), "follow_caution"); + case LOCAL_SEARCH_LEFT: + case LOCAL_SEARCH_RIGHT: + if (searchStartMs < 0L) searchStartMs = nowMs; + if (nowMs - searchStartMs > SEARCH_LIMIT_MS) { + autoUnlocked = false; + return stop("search_timeout"); + } + return decision.selectedAction == BehaviorAction.LOCAL_SEARCH_LEFT + ? new Output(-SEARCH_SPEED, SEARCH_SPEED, "search_left") + : new Output(SEARCH_SPEED, -SEARCH_SPEED, "search_right"); + default: + searchStartMs = -1L; + return stop(decision.selectedAction.name().toLowerCase()); + } + } + + public synchronized Output watchdog(long nowMs) { + if (mode == Mode.AUTO + && autoUnlocked + && (lastInferenceMs < 0L || nowMs - lastInferenceMs > INFERENCE_TIMEOUT_MS)) { + autoUnlocked = false; + return stop("inference_timeout"); + } + return null; + } + + public static Output stop(String reason) { + return new Output(0, 0, reason); + } + + private boolean canMove() { + return foreground && connected && firmwareReady && !emergencyLatched; + } + + private static Output scale(Control control, int maxAbs, String reason) { + if (control == null) return stop("control_missing"); + return new Output( + Math.round(control.getLeft() * maxAbs), Math.round(control.getRight() * maxAbs), reason); + } +} diff --git a/android/robot/src/main/java/org/openbot/cartfollow/SystemSafetyEvidence.java b/android/robot/src/main/java/org/openbot/cartfollow/SystemSafetyEvidence.java new file mode 100644 index 000000000..c790fea2e --- /dev/null +++ b/android/robot/src/main/java/org/openbot/cartfollow/SystemSafetyEvidence.java @@ -0,0 +1,16 @@ +package org.openbot.cartfollow; + +public class SystemSafetyEvidence { + public final boolean emergencyStop; + public final boolean communicationOk; + public final boolean detectorOk; + public final String reason; + + public SystemSafetyEvidence( + boolean emergencyStop, boolean communicationOk, boolean detectorOk, String reason) { + this.emergencyStop = emergencyStop; + this.communicationOk = communicationOk; + this.detectorOk = detectorOk; + this.reason = reason; + } +} diff --git a/android/robot/src/main/java/org/openbot/cartfollow/TargetMatcher.java b/android/robot/src/main/java/org/openbot/cartfollow/TargetMatcher.java new file mode 100644 index 000000000..cb445c580 --- /dev/null +++ b/android/robot/src/main/java/org/openbot/cartfollow/TargetMatcher.java @@ -0,0 +1,79 @@ +package org.openbot.cartfollow; + +import android.graphics.Bitmap; +import android.graphics.RectF; +import java.util.List; +import org.openbot.tflite.Detector.Recognition; + +public class TargetMatcher { + + public static class MatchResult { + public final Recognition best; + public final float score; + public final boolean matched; + + public MatchResult(Recognition best, float score, boolean matched) { + this.best = best; + this.score = score; + this.matched = matched; + } + } + + public float W_POSITION = 0.40f; + public float W_SIZE = 0.20f; + public float W_COLOR = 0.30f; + public float W_CONFIDENCE = 0.10f; + public float MATCH_THRESHOLD = 0.5f; + + public MatchResult match( + List persons, Bitmap frame, TargetMemory memory, int frameW, int frameH) { + if (persons == null || persons.isEmpty() || memory == null || memory.isEmpty()) { + return new MatchResult(null, 0f, false); + } + RectF refBbox = memory.getLastBbox(); + float refArea = memory.getLastArea(); + float imgDiag = (float) Math.sqrt((double) frameW * frameW + (double) frameH * frameH); + + Recognition best = null; + float bestScore = -1f; + for (Recognition r : persons) { + if (r == null || r.getLocation() == null) continue; + RectF b = r.getLocation(); + float score = scoreOne(b, r.getConfidence(), frame, memory, refBbox, refArea, imgDiag); + if (score > bestScore) { + bestScore = score; + best = r; + } + } + boolean matched = best != null && bestScore >= MATCH_THRESHOLD; + return new MatchResult(best, Math.max(0f, bestScore), matched); + } + + private float scoreOne( + RectF b, + Float confidence, + Bitmap frame, + TargetMemory memory, + RectF refBbox, + float refArea, + float imgDiag) { + float cx = b.centerX(); + float cy = b.centerY(); + float refCx = refBbox.centerX(); + float refCy = refBbox.centerY(); + float dist = (float) Math.sqrt((cx - refCx) * (cx - refCx) + (cy - refCy) * (cy - refCy)); + float positionScore = 1f - Math.min(1f, dist / Math.max(1f, imgDiag * 0.3f)); + + float area = b.width() * b.height(); + float sizeScore = 1f - Math.min(1f, Math.abs(area - refArea) / Math.max(1f, refArea)); + + float colorScore = memory.colorScore(frame, b); + + float confScore = confidence == null ? 0f : Math.min(1f, confidence); + + return W_POSITION * positionScore + + W_SIZE * sizeScore + + W_COLOR * colorScore + + W_CONFIDENCE * confScore; + } +} diff --git a/android/robot/src/main/java/org/openbot/cartfollow/TargetMemory.java b/android/robot/src/main/java/org/openbot/cartfollow/TargetMemory.java new file mode 100644 index 000000000..0207d6b20 --- /dev/null +++ b/android/robot/src/main/java/org/openbot/cartfollow/TargetMemory.java @@ -0,0 +1,199 @@ +package org.openbot.cartfollow; + +import android.graphics.Bitmap; +import android.graphics.Color; +import android.graphics.RectF; +import org.openbot.tflite.Detector.Recognition; + +public class TargetMemory { + private static final int H_BINS = 8; + private static final int S_BINS = 4; + private static final int HIST_SIZE = H_BINS * S_BINS; + + private RectF confirmedBbox; + private float confirmedArea; + private float confirmedAspectRatio; + private float[] upperColorHist; + private float[] lowerColorHist; + + private float desiredHeightRatio; + private float desiredAreaRatio; + private float desiredBottomRatio; + + private RectF lastBbox; + private RectF previousBbox; + private float lastCenterX; + private float lastCenterY; + private float lastArea; + private long lastSeenTimeMs; + + public void captureFromBitmap(Bitmap bitmap, RectF bbox) { + captureFromBitmap(bitmap, bbox, 0, 0, 0); + } + + public void captureFromBitmap( + Bitmap bitmap, RectF bbox, int frameW, int frameH, int sensorOrientation) { + confirmedBbox = new RectF(bbox); + confirmedArea = bbox.width() * bbox.height(); + confirmedAspectRatio = bbox.width() / Math.max(1f, bbox.height()); + upperColorHist = computeHsvHist(bitmap, upperHalf(bbox)); + lowerColorHist = computeHsvHist(bitmap, lowerHalf(bbox)); + lastBbox = new RectF(bbox); + previousBbox = null; + lastCenterX = bbox.centerX(); + lastCenterY = bbox.centerY(); + lastArea = confirmedArea; + lastSeenTimeMs = System.currentTimeMillis(); + computeDistanceSetpoint(bbox, frameW, frameH, sensorOrientation); + } + + private void computeDistanceSetpoint( + RectF bbox, int frameW, int frameH, int sensorOrientation) { + if (frameW <= 0 || frameH <= 0 || bbox == null) { + desiredHeightRatio = 0f; + desiredAreaRatio = 0f; + desiredBottomRatio = 0f; + return; + } + boolean rotated = sensorOrientation % 180 == 90; + float imgWidth = rotated ? frameH : frameW; + float imgHeight = rotated ? frameW : frameH; + if (imgWidth <= 0 || imgHeight <= 0) { + desiredHeightRatio = 0f; + desiredAreaRatio = 0f; + desiredBottomRatio = 0f; + return; + } + float boxHeight = rotated ? bbox.width() : bbox.height(); + float boxWidth = rotated ? bbox.height() : bbox.width(); + float boxBottom = rotated ? bbox.right : bbox.bottom; + desiredHeightRatio = boxHeight / imgHeight; + desiredAreaRatio = (boxWidth * boxHeight) / (imgWidth * imgHeight); + desiredBottomRatio = boxBottom / imgHeight; + } + + public boolean hasDistanceSetpoint() { + return desiredHeightRatio > 0f && desiredAreaRatio > 0f; + } + + public ImageSetpointDistanceEstimator.Setpoint getDistanceSetpoint() { + if (!hasDistanceSetpoint()) return null; + return new ImageSetpointDistanceEstimator.Setpoint( + desiredHeightRatio, desiredAreaRatio, desiredBottomRatio); + } + + public void updateDynamic(Recognition r) { + if (r == null || r.getLocation() == null) return; + RectF b = r.getLocation(); + if (lastBbox != null) previousBbox = new RectF(lastBbox); + lastBbox = new RectF(b); + lastCenterX = b.centerX(); + lastCenterY = b.centerY(); + lastArea = b.width() * b.height(); + lastSeenTimeMs = System.currentTimeMillis(); + } + + public void clear() { + confirmedBbox = null; + confirmedArea = 0f; + confirmedAspectRatio = 0f; + upperColorHist = null; + lowerColorHist = null; + desiredHeightRatio = 0f; + desiredAreaRatio = 0f; + desiredBottomRatio = 0f; + lastBbox = null; + previousBbox = null; + lastCenterX = 0f; + lastCenterY = 0f; + lastArea = 0f; + lastSeenTimeMs = 0L; + } + + public boolean isEmpty() { + return confirmedBbox == null; + } + + public RectF getLastBbox() { + return lastBbox != null ? new RectF(lastBbox) : (confirmedBbox != null ? new RectF(confirmedBbox) : null); + } + + public RectF getPreviousBbox() { + return previousBbox != null ? new RectF(previousBbox) : null; + } + + public float getLastArea() { + return lastArea > 0 ? lastArea : confirmedArea; + } + + public float[] getUpperColorHist() { + return upperColorHist; + } + + public float[] getLowerColorHist() { + return lowerColorHist; + } + + public float getConfirmedArea() { + return confirmedArea; + } + + private static RectF upperHalf(RectF bbox) { + return new RectF(bbox.left, bbox.top, bbox.right, bbox.top + bbox.height() / 2f); + } + + private static RectF lowerHalf(RectF bbox) { + return new RectF(bbox.left, bbox.top + bbox.height() / 2f, bbox.right, bbox.bottom); + } + + public static float[] computeHsvHist(Bitmap bitmap, RectF rect) { + float[] hist = new float[HIST_SIZE]; + if (bitmap == null || rect == null) return hist; + int bw = bitmap.getWidth(); + int bh = bitmap.getHeight(); + int left = clamp((int) rect.left, 0, bw - 1); + int top = clamp((int) rect.top, 0, bh - 1); + int right = clamp((int) rect.right, 1, bw); + int bottom = clamp((int) rect.bottom, 1, bh); + int w = right - left; + int h = bottom - top; + if (w <= 0 || h <= 0) return hist; + int[] pixels = new int[w * h]; + bitmap.getPixels(pixels, 0, w, left, top, w, h); + float[] hsv = new float[3]; + int count = 0; + for (int px : pixels) { + Color.RGBToHSV(Color.red(px), Color.green(px), Color.blue(px), hsv); + if (hsv[1] < 0.1f) continue; + int hb = (int) (hsv[0] / 360f * H_BINS) % H_BINS; + if (hb < 0) hb += H_BINS; + int sb = Math.min(S_BINS - 1, (int) (hsv[1] * S_BINS)); + hist[hb * S_BINS + sb]++; + count++; + } + if (count > 0) { + for (int i = 0; i < HIST_SIZE; i++) hist[i] /= count; + } + return hist; + } + + public static float histIntersection(float[] a, float[] b) { + if (a == null || b == null || a.length == 0 || b.length == 0) return 0f; + float sum = 0f; + int n = Math.min(a.length, b.length); + for (int i = 0; i < n; i++) sum += Math.min(a[i], b[i]); + return sum; + } + + public float colorScore(Bitmap bitmap, RectF bbox) { + float[] upper = computeHsvHist(bitmap, upperHalf(bbox)); + float[] lower = computeHsvHist(bitmap, lowerHalf(bbox)); + float upperScore = histIntersection(upper, upperColorHist); + float lowerScore = histIntersection(lower, lowerColorHist); + return (upperScore + lowerScore) / 2f; + } + + private static int clamp(int v, int lo, int hi) { + return v < lo ? lo : (v > hi ? hi : v); + } +} diff --git a/android/robot/src/main/java/org/openbot/cartfollow/TargetTrack.java b/android/robot/src/main/java/org/openbot/cartfollow/TargetTrack.java new file mode 100644 index 000000000..04a85e6e8 --- /dev/null +++ b/android/robot/src/main/java/org/openbot/cartfollow/TargetTrack.java @@ -0,0 +1,56 @@ +package org.openbot.cartfollow; + +import android.graphics.RectF; +import org.openbot.tflite.Detector.Recognition; + +public class TargetTrack { + public final int trackId; + public RectF lastBbox; + public RectF previousBbox; + public Recognition recognition; + public int ageFrames; + public int missedFrames; + public int stableFrames; + public long lastSeenTimestampMs; + public String matchReason; + + TargetTrack(int trackId, Recognition recognition, long timestampMs) { + this.trackId = trackId; + this.recognition = recognition; + this.lastBbox = recognition == null ? null : new RectF(recognition.getLocation()); + this.previousBbox = null; + this.ageFrames = 1; + this.missedFrames = 0; + this.stableFrames = 1; + this.lastSeenTimestampMs = timestampMs; + this.matchReason = "new_track"; + } + + void update(Recognition recognition, long timestampMs, String reason) { + if (recognition == null || recognition.getLocation() == null) return; + if (lastBbox != null) previousBbox = new RectF(lastBbox); + lastBbox = new RectF(recognition.getLocation()); + this.recognition = recognition; + ageFrames++; + missedFrames = 0; + stableFrames++; + lastSeenTimestampMs = timestampMs; + matchReason = reason; + } + + void markMissed() { + ageFrames++; + missedFrames++; + stableFrames = 0; + recognition = null; + matchReason = "missed"; + } + + public boolean isVisible() { + return recognition != null && missedFrames == 0 && lastBbox != null; + } + + public float area() { + return lastBbox == null ? 0f : Math.max(0f, lastBbox.width()) * Math.max(0f, lastBbox.height()); + } +} diff --git a/android/robot/src/main/java/org/openbot/cartfollow/TargetTrackManager.java b/android/robot/src/main/java/org/openbot/cartfollow/TargetTrackManager.java new file mode 100644 index 000000000..44cb28a14 --- /dev/null +++ b/android/robot/src/main/java/org/openbot/cartfollow/TargetTrackManager.java @@ -0,0 +1,325 @@ +package org.openbot.cartfollow; + +import android.graphics.RectF; +import java.util.ArrayList; +import java.util.HashSet; +import java.util.List; +import java.util.Set; +import org.openbot.tflite.Detector.Recognition; + +public class TargetTrackManager { + private static final float CENTER_GATE_RATIO = 0.25f; + private static final float AREA_RATIO_MIN = 0.50f; + private static final float AREA_RATIO_MAX = 2.00f; + private static final float IOU_SOFT_GATE = 0.10f; + private static final int MAX_MISSED_FRAMES = 16; + private static final long LOCKED_GHOST_TTL_MS = 3000L; + private static final int SUSPECTED_MIN_DWELL_FRAMES = 3; + private static final int SUSPECTED_REPLACEMENT_STABLE_FRAMES = 2; + private static final float SWITCH_BELIEF_DELTA = 0.15f; + + private final List tracks = new ArrayList<>(); + private int nextTrackId = 1; + private int lockedTrackId = -1; + private int suspectedTrackId = -1; + private int suspectedDwellFrames = 0; + private float suspectedBelief = 0f; + private String suspectedUpdateReason = "none"; + private RectF lockedGhostBbox = null; + private float lockedGhostCenterX = 0f; + private float lockedGhostCenterY = 0f; + private float lockedGhostVelocityX = 0f; + private float lockedGhostVelocityY = 0f; + private long lockedGhostLastSeenMs = 0L; + private String lockedGhostLostSide = "unknown"; + + public void reset() { + tracks.clear(); + nextTrackId = 1; + lockedTrackId = -1; + suspectedTrackId = -1; + suspectedDwellFrames = 0; + suspectedBelief = 0f; + suspectedUpdateReason = "reset"; + lockedGhostBbox = null; + lockedGhostCenterX = 0f; + lockedGhostCenterY = 0f; + lockedGhostVelocityX = 0f; + lockedGhostVelocityY = 0f; + lockedGhostLastSeenMs = 0L; + lockedGhostLostSide = "unknown"; + } + + public void update(List detections, int frameW, int frameH, long timestampMs) { + List safeDetections = detections == null ? new ArrayList<>() : detections; + Set matchedTracks = new HashSet<>(); + Set matchedDetections = new HashSet<>(); + + for (Recognition detection : safeDetections) { + if (detection == null || detection.getLocation() == null) continue; + Match best = null; + for (TargetTrack track : tracks) { + if (track.lastBbox == null || matchedTracks.contains(track)) continue; + Match match = score(track, detection, frameW, frameH); + if (!match.accepted) continue; + if (best == null || match.score > best.score) best = match; + } + if (best != null) { + best.track.update(detection, timestampMs, best.reason); + matchedTracks.add(best.track); + matchedDetections.add(detection); + } + } + + for (TargetTrack track : new ArrayList<>(tracks)) { + if (!matchedTracks.contains(track)) track.markMissed(); + if (track.missedFrames > MAX_MISSED_FRAMES) tracks.remove(track); + } + + for (Recognition detection : safeDetections) { + if (detection == null || detection.getLocation() == null || matchedDetections.contains(detection)) { + continue; + } + tracks.add(new TargetTrack(nextTrackId++, detection, timestampMs)); + } + + TargetTrack lockedTrack = getLockedTrack(); + if (lockedTrack != null && lockedTrack.isVisible()) { + updateLockedGhost(lockedTrack, frameW, timestampMs); + } + } + + public List getTracks() { + return new ArrayList<>(tracks); + } + + public int getActiveTrackCount() { + int count = 0; + for (TargetTrack track : tracks) { + if (track.isVisible()) count++; + } + return count; + } + + public TargetTrack getTrackForRecognition(Recognition recognition) { + if (recognition == null) return null; + for (TargetTrack track : tracks) { + if (track.recognition == recognition) return track; + } + return null; + } + + public TargetTrack getLockedTrack() { + return getTrackById(lockedTrackId); + } + + public TargetTrack getTrackById(int trackId) { + if (trackId < 0) return null; + for (TargetTrack track : tracks) { + if (track.trackId == trackId) return track; + } + return null; + } + + public int getLockedTrackId() { + return lockedTrackId; + } + + public int getSuspectedTrackId() { + return suspectedTrackId; + } + + public void setSuspectedTrackId(int suspectedTrackId) { + this.suspectedTrackId = suspectedTrackId; + this.suspectedDwellFrames = suspectedTrackId < 0 ? 0 : 1; + this.suspectedBelief = 0f; + this.suspectedUpdateReason = "forced"; + } + + public int getSuspectedDwellFrames() { + return suspectedDwellFrames; + } + + public String getSuspectedUpdateReason() { + return suspectedUpdateReason; + } + + public boolean updateSuspectedTrack(int candidateTrackId, float candidateBelief) { + if (candidateTrackId < 0) { + suspectedTrackId = -1; + suspectedDwellFrames = 0; + suspectedBelief = 0f; + suspectedUpdateReason = "clear"; + return true; + } + if (candidateTrackId == suspectedTrackId) { + suspectedDwellFrames++; + suspectedBelief = Math.max(suspectedBelief, candidateBelief); + suspectedUpdateReason = "suspected_dwell_keep"; + return true; + } + TargetTrack candidate = getTrackById(candidateTrackId); + TargetTrack current = getTrackById(suspectedTrackId); + if (suspectedTrackId < 0 || current == null) { + suspectedTrackId = candidateTrackId; + suspectedDwellFrames = 1; + suspectedBelief = candidateBelief; + suspectedUpdateReason = "suspected_init"; + return true; + } + boolean candidateStable = + candidate != null && candidate.isVisible() && candidate.stableFrames >= SUSPECTED_REPLACEMENT_STABLE_FRAMES; + boolean clearlyBetter = candidateBelief >= suspectedBelief + SWITCH_BELIEF_DELTA; + boolean currentWeak = !current.isVisible() || current.missedFrames > 0; + boolean dwellSatisfied = suspectedDwellFrames >= SUSPECTED_MIN_DWELL_FRAMES; + if (candidateStable && clearlyBetter && (currentWeak || dwellSatisfied)) { + suspectedTrackId = candidateTrackId; + suspectedDwellFrames = 1; + suspectedBelief = candidateBelief; + suspectedUpdateReason = "suspected_switch_clear_better"; + return true; + } + suspectedDwellFrames++; + suspectedUpdateReason = "suspected_dwell_hold"; + return false; + } + + public boolean hasLockedGhost(long nowMs) { + return lockedGhostBbox != null && nowMs - lockedGhostLastSeenMs <= LOCKED_GHOST_TTL_MS; + } + + public RectF getLockedGhostBbox(long nowMs) { + return hasLockedGhost(nowMs) ? new RectF(lockedGhostBbox) : null; + } + + public String getLockedGhostLostSide(long nowMs) { + return hasLockedGhost(nowMs) ? lockedGhostLostSide : "unknown"; + } + + public boolean isNearLockedGhost(TargetTrack track, int frameW, int frameH, long nowMs) { + if (track == null || track.lastBbox == null || !hasLockedGhost(nowMs)) return false; + RectF ghost = predictedLockedGhost(nowMs); + float diag = (float) Math.hypot(Math.max(1, frameW), Math.max(1, frameH)); + float centerJump = centerDistance(track.lastBbox, ghost) / Math.max(1f, diag); + float areaRatio = area(track.lastBbox) / Math.max(1f, area(ghost)); + return centerJump <= BboxContinuityEvidence.LOOSE_CENTER_MAX + && areaRatio >= BboxContinuityEvidence.LOOSE_AREA_MIN + && areaRatio <= BboxContinuityEvidence.LOOSE_AREA_MAX; + } + + public int lockClosest(RectF reference) { + TargetTrack best = null; + float bestScore = Float.MAX_VALUE; + for (TargetTrack track : tracks) { + if (!track.isVisible()) continue; + float score = reference == null ? -track.area() : centerDistance(track.lastBbox, reference); + if (best == null || score < bestScore) { + best = track; + bestScore = score; + } + } + if (best == null) { + clearLockedTrack("lock_missing"); + return lockedTrackId; + } + lockTrack(best.trackId, "lock"); + return lockedTrackId; + } + + public boolean lockTrack(int trackId, String reason) { + TargetTrack track = getTrackById(trackId); + if (track == null || !track.isVisible()) return false; + lockedTrackId = trackId; + suspectedTrackId = -1; + suspectedDwellFrames = 0; + suspectedBelief = 0f; + suspectedUpdateReason = reason == null ? "lock_track" : reason; + updateLockedGhost(track, 0, track.lastSeenTimestampMs); + return true; + } + + private void clearLockedTrack(String reason) { + lockedTrackId = -1; + suspectedTrackId = -1; + suspectedDwellFrames = 0; + suspectedBelief = 0f; + suspectedUpdateReason = reason == null ? "clear_lock" : reason; + } + + public boolean isLockedTrack(TargetTrack track) { + return track != null && track.trackId == lockedTrackId; + } + + private static Match score(TargetTrack track, Recognition detection, int frameW, int frameH) { + RectF current = detection.getLocation(); + RectF last = track.lastBbox; + float diag = (float) Math.hypot(Math.max(1, frameW), Math.max(1, frameH)); + float centerJump = centerDistance(current, last) / Math.max(1f, diag); + float areaRatio = area(current) / Math.max(1f, area(last)); + float iou = iou(current, last); + boolean accepted = + (centerJump <= CENTER_GATE_RATIO && areaRatio >= AREA_RATIO_MIN && areaRatio <= AREA_RATIO_MAX) + || iou >= IOU_SOFT_GATE; + float score = iou * 2.0f - centerJump - Math.abs(1f - areaRatio) * 0.25f; + String reason = + String.format( + java.util.Locale.US, "iou=%.2f center=%.3f area=%.2f", iou, centerJump, areaRatio); + return new Match(track, accepted, score, reason); + } + + private static float centerDistance(RectF a, RectF b) { + if (a == null || b == null) return Float.MAX_VALUE; + return (float) Math.hypot(a.centerX() - b.centerX(), a.centerY() - b.centerY()); + } + + private static float area(RectF b) { + return b == null ? 0f : Math.max(0f, b.width()) * Math.max(0f, b.height()); + } + + private void updateLockedGhost(TargetTrack track, int frameW, long timestampMs) { + if (track == null || track.lastBbox == null) return; + if (lockedGhostBbox != null) { + lockedGhostVelocityX = track.lastBbox.centerX() - lockedGhostCenterX; + lockedGhostVelocityY = track.lastBbox.centerY() - lockedGhostCenterY; + } + lockedGhostBbox = new RectF(track.lastBbox); + lockedGhostCenterX = track.lastBbox.centerX(); + lockedGhostCenterY = track.lastBbox.centerY(); + lockedGhostLastSeenMs = timestampMs; + if (frameW > 0) { + lockedGhostLostSide = lockedGhostCenterX < frameW / 2f ? "left" : "right"; + } + } + + private RectF predictedLockedGhost(long nowMs) { + RectF ghost = new RectF(lockedGhostBbox); + float steps = Math.min(3f, Math.max(0f, (nowMs - lockedGhostLastSeenMs) / 250f)); + ghost.offset(lockedGhostVelocityX * steps, lockedGhostVelocityY * steps); + return ghost; + } + + private static float iou(RectF a, RectF b) { + if (a == null || b == null) return 0f; + float left = Math.max(a.left, b.left); + float top = Math.max(a.top, b.top); + float right = Math.min(a.right, b.right); + float bottom = Math.min(a.bottom, b.bottom); + float inter = Math.max(0f, right - left) * Math.max(0f, bottom - top); + float union = area(a) + area(b) - inter; + return union <= 0f ? 0f : inter / union; + } + + private static class Match { + final TargetTrack track; + final boolean accepted; + final float score; + final String reason; + + Match(TargetTrack track, boolean accepted, float score, String reason) { + this.track = track; + this.accepted = accepted; + this.score = score; + this.reason = reason; + } + } +} diff --git a/android/robot/src/main/java/org/openbot/cartfollow/TfliteReIDFeatureExtractor.java b/android/robot/src/main/java/org/openbot/cartfollow/TfliteReIDFeatureExtractor.java new file mode 100644 index 000000000..bb1f060f5 --- /dev/null +++ b/android/robot/src/main/java/org/openbot/cartfollow/TfliteReIDFeatureExtractor.java @@ -0,0 +1,118 @@ +package org.openbot.cartfollow; + +import android.app.Activity; +import android.content.res.AssetFileDescriptor; +import android.graphics.Bitmap; +import java.io.FileInputStream; +import java.io.IOException; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import java.nio.MappedByteBuffer; +import java.nio.channels.FileChannel; +import org.tensorflow.lite.Interpreter; +import org.tensorflow.lite.Tensor; + +public class TfliteReIDFeatureExtractor implements ReIDFeatureExtractor { + public static final String DEFAULT_ASSET_PATH = "networks/reid/osnet_x0_25_market1501.tflite"; + + private static final float[] MEAN = {0.485f, 0.456f, 0.406f}; + private static final float[] STD = {0.229f, 0.224f, 0.225f}; + + private final Interpreter interpreter; + private final int inputWidth; + private final int inputHeight; + private final boolean nchw; + private final int embeddingSize; + private final int[] pixels; + private final ByteBuffer inputBuffer; + + public TfliteReIDFeatureExtractor(Activity activity, String assetPath, int numThreads) + throws IOException { + Interpreter.Options options = new Interpreter.Options(); + options.setNumThreads(numThreads > 0 ? numThreads : 2); + interpreter = new Interpreter(loadModelFile(activity, assetPath), options); + + Tensor inputTensor = interpreter.getInputTensor(0); + int[] inShape = inputTensor.shape(); + if (inShape.length != 4) { + throw new IOException("Unsupported ReID input shape length: " + inShape.length); + } + nchw = inShape[1] == 3; + inputHeight = nchw ? inShape[2] : inShape[1]; + inputWidth = nchw ? inShape[3] : inShape[2]; + if (inputWidth <= 0 || inputHeight <= 0) { + throw new IOException("Invalid ReID input shape."); + } + + int[] outShape = interpreter.getOutputTensor(0).shape(); + int size = 1; + for (int i = 1; i < outShape.length; i++) { + size *= Math.max(1, outShape[i]); + } + embeddingSize = Math.max(1, size); + pixels = new int[inputWidth * inputHeight]; + inputBuffer = ByteBuffer.allocateDirect(4 * inputWidth * inputHeight * 3); + inputBuffer.order(ByteOrder.nativeOrder()); + } + + @Override + public synchronized float[] extract(Bitmap personCrop) { + if (personCrop == null) return null; + Bitmap resized = Bitmap.createScaledBitmap(personCrop, inputWidth, inputHeight, true); + resized.getPixels(pixels, 0, inputWidth, 0, 0, inputWidth, inputHeight); + inputBuffer.rewind(); + if (nchw) { + for (int c = 0; c < 3; c++) { + for (int y = 0; y < inputHeight; y++) { + for (int x = 0; x < inputWidth; x++) { + inputBuffer.putFloat(normalizedChannel(pixels[y * inputWidth + x], c)); + } + } + } + } else { + for (int y = 0; y < inputHeight; y++) { + for (int x = 0; x < inputWidth; x++) { + int px = pixels[y * inputWidth + x]; + inputBuffer.putFloat(normalizedChannel(px, 0)); + inputBuffer.putFloat(normalizedChannel(px, 1)); + inputBuffer.putFloat(normalizedChannel(px, 2)); + } + } + } + float[][] output = new float[1][embeddingSize]; + inputBuffer.rewind(); + interpreter.run(inputBuffer, output); + return l2Normalize(output[0]); + } + + public void close() { + interpreter.close(); + } + + private static float normalizedChannel(int px, int channel) { + int v; + if (channel == 0) v = (px >> 16) & 0xFF; + else if (channel == 1) v = (px >> 8) & 0xFF; + else v = px & 0xFF; + return (v / 255f - MEAN[channel]) / STD[channel]; + } + + private static float[] l2Normalize(float[] values) { + float sum = 0f; + for (float v : values) sum += v * v; + float norm = (float) Math.sqrt(Math.max(sum, 1e-12f)); + for (int i = 0; i < values.length; i++) values[i] /= norm; + return values; + } + + private static MappedByteBuffer loadModelFile(Activity activity, String assetPath) + throws IOException { + AssetFileDescriptor fileDescriptor = activity.getAssets().openFd(assetPath); + FileInputStream inputStream = new FileInputStream(fileDescriptor.getFileDescriptor()); + FileChannel fileChannel = inputStream.getChannel(); + return fileChannel.map( + FileChannel.MapMode.READ_ONLY, + fileDescriptor.getStartOffset(), + fileDescriptor.getDeclaredLength()); + } +} diff --git a/android/robot/src/main/java/org/openbot/cartfollow/TraversabilityEvidence.java b/android/robot/src/main/java/org/openbot/cartfollow/TraversabilityEvidence.java new file mode 100644 index 000000000..7d9eb312e --- /dev/null +++ b/android/robot/src/main/java/org/openbot/cartfollow/TraversabilityEvidence.java @@ -0,0 +1,22 @@ +package org.openbot.cartfollow; + +public class TraversabilityEvidence { + public final float leftFreeScore; + public final float centerFreeScore; + public final float rightFreeScore; + public final boolean centerBlocked; + public final String reason; + + public TraversabilityEvidence( + float leftFreeScore, + float centerFreeScore, + float rightFreeScore, + boolean centerBlocked, + String reason) { + this.leftFreeScore = leftFreeScore; + this.centerFreeScore = centerFreeScore; + this.rightFreeScore = rightFreeScore; + this.centerBlocked = centerBlocked; + this.reason = reason; + } +} diff --git a/android/robot/src/main/java/org/openbot/cartfollow/diagnostics/CartFollowDiagnosticConfig.java b/android/robot/src/main/java/org/openbot/cartfollow/diagnostics/CartFollowDiagnosticConfig.java new file mode 100644 index 000000000..3c0c6a56a --- /dev/null +++ b/android/robot/src/main/java/org/openbot/cartfollow/diagnostics/CartFollowDiagnosticConfig.java @@ -0,0 +1,11 @@ +package org.openbot.cartfollow.diagnostics; + +public class CartFollowDiagnosticConfig { + public long frameLogIntervalMs = 200; + public long cropIntervalMs = 500; + public long overlayIntervalMs = 1000; + public boolean saveCrops = true; + public boolean saveOverlays = false; + public float paddingRatio = 0.08f; + public int jpegQuality = 90; +} diff --git a/android/robot/src/main/java/org/openbot/cartfollow/diagnostics/CartFollowDiagnosticSaver.java b/android/robot/src/main/java/org/openbot/cartfollow/diagnostics/CartFollowDiagnosticSaver.java new file mode 100644 index 000000000..f48a81856 --- /dev/null +++ b/android/robot/src/main/java/org/openbot/cartfollow/diagnostics/CartFollowDiagnosticSaver.java @@ -0,0 +1,357 @@ +package org.openbot.cartfollow.diagnostics; + +import android.graphics.Bitmap; +import android.graphics.Matrix; +import android.graphics.RectF; +import java.io.File; +import java.io.FileOutputStream; +import java.io.FileWriter; +import java.io.IOException; +import java.util.Locale; +import java.util.concurrent.ExecutorService; +import java.util.concurrent.Executors; +import org.openbot.cartfollow.BehaviorDecisionResult; +import org.openbot.cartfollow.BboxContinuityEvidence; +import org.openbot.cartfollow.IdentityEvidence; +import org.openbot.cartfollow.ReIDMatchResult; +import org.openbot.tflite.Detector.Recognition; +import timber.log.Timber; + +public class CartFollowDiagnosticSaver { + private final ExecutorService executor = Executors.newSingleThreadExecutor(); + + public void saveFrameAsync( + Bitmap frame, + CartFollowDiagnosticSession session, + CartFollowDiagnosticConfig config, + long frameNum, + int frameW, + int frameH, + int sensorOrientation, + float fps, + int numPersons, + String followState, + BehaviorDecisionResult decision, + String commandText, + IdentityEvidence identity, + Recognition locked, + Recognition suspected, + Recognition bestReid, + boolean saveCropsForThisFrame) { + if (session == null) return; + final long timestampMs = System.currentTimeMillis(); + final long elapsedMs = timestampMs - session.startedAtMs; + final String action = decision == null ? "" : decision.selectedAction.name(); + final String actionReason = decision == null ? "" : safe(decision.actionReason); + final String safetyBlock = + decision == null || decision.safetyBlockReason == null ? "" : safe(decision.safetyBlockReason); + final String safeCommand = safe(commandText); + final IdentitySnapshot identitySnapshot = new IdentitySnapshot(identity); + final RecognitionSnapshot lockedSnapshot = RecognitionSnapshot.from(locked); + final RecognitionSnapshot suspectedSnapshot = RecognitionSnapshot.from(suspected); + final RecognitionSnapshot bestSnapshot = RecognitionSnapshot.from(bestReid); + Bitmap.Config bitmapConfig = frame == null || frame.getConfig() == null ? Bitmap.Config.ARGB_8888 : frame.getConfig(); + final Bitmap frameCopy = + frame != null && config.saveCrops && saveCropsForThisFrame ? frame.copy(bitmapConfig, false) : null; + + executor.execute( + () -> { + String lockedPath = ""; + String suspectedPath = ""; + String bestPath = ""; + if (frameCopy != null) { + lockedPath = + saveCrop(frameCopy, lockedSnapshot, session, config, frameNum, "locked", sensorOrientation); + suspectedPath = + saveCrop(frameCopy, suspectedSnapshot, session, config, frameNum, "suspected", sensorOrientation); + bestPath = + saveCrop(frameCopy, bestSnapshot, session, config, frameNum, "best_reid", sensorOrientation); + frameCopy.recycle(); + } + appendFrameLog( + session, + frameNum, + timestampMs, + elapsedMs, + fps, + numPersons, + followState, + action, + actionReason, + safetyBlock, + safeCommand); + appendIdentityLog( + session, + frameNum, + timestampMs, + identitySnapshot, + lockedPath, + suspectedPath, + bestPath); + }); + } + + public void saveEventAsync( + CartFollowDiagnosticSession session, long frameNum, String eventType, String note) { + if (session == null) return; + final long timestampMs = System.currentTimeMillis(); + final String safeType = safe(eventType); + final String safeNote = safe(note); + executor.execute( + () -> { + String row = + String.format( + Locale.US, + "%s,%d,%d,%s,%s\n", + csv(session.sessionId), timestampMs, frameNum, csv(safeType), csv(safeNote)); + append(session.eventsCsv, row, "events.csv"); + session.eventRows++; + }); + } + + public void saveGallerySnapshotAsync( + Bitmap bitmap, CartFollowDiagnosticSession session, String label) { + if (bitmap == null || session == null) return; + Bitmap.Config bitmapConfig = bitmap.getConfig() == null ? Bitmap.Config.ARGB_8888 : bitmap.getConfig(); + final Bitmap copy = bitmap.copy(bitmapConfig, false); + final String safeLabel = sanitize(label == null ? "gallery" : label); + executor.execute( + () -> { + String filename = + String.format(Locale.US, "%06d_%s.jpg", session.galleryCount, safeLabel); + File file = new File(session.galleryDir, filename); + try (FileOutputStream fos = new FileOutputStream(file)) { + copy.compress(Bitmap.CompressFormat.JPEG, 90, fos); + session.galleryCount++; + } catch (IOException e) { + Timber.e(e, "Failed to save diagnostic gallery snapshot"); + } finally { + copy.recycle(); + } + }); + } + + public void shutdown() { + executor.shutdown(); + } + + private void appendFrameLog( + CartFollowDiagnosticSession session, + long frameNum, + long timestampMs, + long elapsedMs, + float fps, + int numPersons, + String followState, + String action, + String actionReason, + String safetyBlock, + String commandText) { + String row = + String.format( + Locale.US, + "%s,%d,%d,%d,%.2f,%d,%s,%s,%s,%s,%s\n", + csv(session.sessionId), + frameNum, + timestampMs, + elapsedMs, + fps, + numPersons, + csv(followState), + csv(action), + csv(actionReason), + csv(safetyBlock), + csv(commandText)); + append(session.frameLogCsv, row, "frame_log.csv"); + session.frameRows++; + } + + private void appendIdentityLog( + CartFollowDiagnosticSession session, + long frameNum, + long timestampMs, + IdentitySnapshot id, + String lockedPath, + String suspectedPath, + String bestPath) { + String row = + String.format( + Locale.US, + "%s,%d,%d,%d,%d,%d,%d,%d,%d,%.4f,%.4f,%.4f,%d,%s,%s,%s,%s,%s,%s,%s,%.4f,%d,%d,%d,%s,%s,%s,%s,%s\n", + csv(session.sessionId), + frameNum, + timestampMs, + id.trackId, + id.lockedTrackId, + id.suspectedTrackId, + id.activeTrackCount, + id.trackAge, + id.missedFrames, + id.bestScore, + id.secondScore, + id.margin, + id.gallerySize, + id.weakOk ? "1" : "0", + id.midOk ? "1" : "0", + id.strongOk ? "1" : "0", + id.bboxLooseAdmissionOk ? "1" : "0", + id.bboxDefaultOk ? "1" : "0", + id.bboxStrictOk ? "1" : "0", + id.predictionOk ? "1" : "0", + id.targetBelief, + id.beliefStableFrames, + id.beliefUncertainFrames, + id.candidateSwitchCount, + csv(id.beliefReason), + csv(id.reidReason), + csv(lockedPath), + csv(suspectedPath), + csv(bestPath)); + append(session.identityLogCsv, row, "identity_log.csv"); + session.identityRows++; + } + + private String saveCrop( + Bitmap frame, + RecognitionSnapshot snapshot, + CartFollowDiagnosticSession session, + CartFollowDiagnosticConfig config, + long frameNum, + String role, + int sensorOrientation) { + if (snapshot == null || snapshot.bbox == null) return ""; + RectF bbox = snapshot.bbox; + float padX = bbox.width() * config.paddingRatio; + float padY = bbox.height() * config.paddingRatio; + int left = clamp((int) (bbox.left - padX), 0, frame.getWidth() - 1); + int top = clamp((int) (bbox.top - padY), 0, frame.getHeight() - 1); + int right = clamp((int) (bbox.right + padX), left + 1, frame.getWidth()); + int bottom = clamp((int) (bbox.bottom + padY), top + 1, frame.getHeight()); + int w = right - left; + int h = bottom - top; + if (w <= 0 || h <= 0) return ""; + + Bitmap rawCrop; + try { + rawCrop = Bitmap.createBitmap(frame, left, top, w, h); + } catch (Exception e) { + return ""; + } + + Bitmap uprightCrop; + if (sensorOrientation % 360 != 0) { + Matrix matrix = new Matrix(); + matrix.postRotate(sensorOrientation); + uprightCrop = + Bitmap.createBitmap(rawCrop, 0, 0, rawCrop.getWidth(), rawCrop.getHeight(), matrix, true); + rawCrop.recycle(); + } else { + uprightCrop = rawCrop; + } + + String filename = String.format(Locale.US, "%06d_%s.jpg", frameNum, sanitize(role)); + File cropFile = new File(session.cropsDir, filename); + try (FileOutputStream fos = new FileOutputStream(cropFile)) { + uprightCrop.compress(Bitmap.CompressFormat.JPEG, config.jpegQuality, fos); + session.cropCount++; + return "crops/" + filename; + } catch (IOException e) { + Timber.e(e, "Failed to save diagnostic crop"); + return ""; + } finally { + uprightCrop.recycle(); + } + } + + private void append(File file, String row, String label) { + try (FileWriter writer = new FileWriter(file, true)) { + writer.append(row); + } catch (IOException e) { + Timber.e(e, "Failed to append %s", label); + } + } + + private static int clamp(int value, int min, int max) { + return Math.max(min, Math.min(max, value)); + } + + private static String csv(String value) { + if (value == null) return ""; + return "\"" + value.replace("\"", "\"\"") + "\""; + } + + private static String safe(String value) { + return value == null ? "" : value; + } + + private static String sanitize(String value) { + return safe(value).replaceAll("[^a-zA-Z0-9_\\-]", "_"); + } + + private static class RecognitionSnapshot { + final RectF bbox; + + RecognitionSnapshot(RectF bbox) { + this.bbox = bbox; + } + + static RecognitionSnapshot from(Recognition recognition) { + if (recognition == null || recognition.getLocation() == null) return null; + return new RecognitionSnapshot(new RectF(recognition.getLocation())); + } + } + + private static class IdentitySnapshot { + final int trackId; + final int lockedTrackId; + final int suspectedTrackId; + final int activeTrackCount; + final int trackAge; + final int missedFrames; + final float bestScore; + final float secondScore; + final float margin; + final int gallerySize; + final boolean weakOk; + final boolean midOk; + final boolean strongOk; + final boolean bboxLooseAdmissionOk; + final boolean bboxDefaultOk; + final boolean bboxStrictOk; + final boolean predictionOk; + final float targetBelief; + final int beliefStableFrames; + final int beliefUncertainFrames; + final int candidateSwitchCount; + final String beliefReason; + final String reidReason; + + IdentitySnapshot(IdentityEvidence identity) { + ReIDMatchResult reid = identity == null ? null : identity.reidMatch; + BboxContinuityEvidence bbox = identity == null ? null : identity.bboxContinuity; + trackId = identity == null ? -1 : identity.trackId; + lockedTrackId = identity == null ? -1 : identity.lockedTrackId; + suspectedTrackId = identity == null ? -1 : identity.suspectedTrackId; + activeTrackCount = identity == null ? 0 : identity.activeTrackCount; + trackAge = identity == null ? 0 : identity.trackAge; + missedFrames = identity == null ? 0 : identity.missedFrames; + bestScore = reid == null ? 0f : reid.bestScore; + secondScore = reid == null ? 0f : reid.secondScore; + margin = reid == null ? 0f : reid.margin; + gallerySize = reid == null ? 0 : reid.gallerySize; + weakOk = identity != null && identity.weakOk(); + midOk = identity != null && identity.midOk(); + strongOk = identity != null && identity.strongOk(); + bboxLooseAdmissionOk = bbox != null && bbox.looseAdmissionOk; + bboxDefaultOk = bbox != null && bbox.bboxDefaultOk; + bboxStrictOk = bbox != null && bbox.bboxStrictOk; + predictionOk = bbox != null && bbox.predictionOk; + targetBelief = identity == null ? 0f : identity.targetBelief; + beliefStableFrames = identity == null ? 0 : identity.beliefStableFrames; + beliefUncertainFrames = identity == null ? 0 : identity.beliefUncertainFrames; + candidateSwitchCount = identity == null ? 0 : identity.candidateSwitchCount; + beliefReason = identity == null || identity.beliefReason == null ? "" : identity.beliefReason; + reidReason = reid == null || reid.reason == null ? "" : reid.reason; + } + } +} diff --git a/android/robot/src/main/java/org/openbot/cartfollow/diagnostics/CartFollowDiagnosticSession.java b/android/robot/src/main/java/org/openbot/cartfollow/diagnostics/CartFollowDiagnosticSession.java new file mode 100644 index 000000000..f0eef8dcc --- /dev/null +++ b/android/robot/src/main/java/org/openbot/cartfollow/diagnostics/CartFollowDiagnosticSession.java @@ -0,0 +1,122 @@ +package org.openbot.cartfollow.diagnostics; + +import android.content.Context; +import android.os.Build; +import android.os.Environment; +import android.util.Log; +import java.io.File; +import java.io.FileWriter; +import java.io.IOException; +import java.text.SimpleDateFormat; +import java.util.Date; +import java.util.Locale; +import org.json.JSONException; +import org.json.JSONObject; + +public class CartFollowDiagnosticSession { + private static final String TAG = "CartFollowDiagnostic"; + + private static final String FRAME_LOG_HEADER = + "session_id,frame_id,timestamp_ms,elapsed_ms,fps,num_persons,follow_state," + + "selected_action,action_reason,safety_block_reason,command_text"; + private static final String IDENTITY_LOG_HEADER = + "session_id,frame_id,timestamp_ms,track_id,locked_track_id,suspected_track_id," + + "active_track_count,track_age,missed_frames,best_score,second_score,margin," + + "gallery_size,weak_ok,mid_ok,strong_ok,bbox_loose_admission_ok,bbox_default_ok,bbox_strict_ok," + + "prediction_ok,target_belief,belief_stable_frames,belief_uncertain_frames," + + "candidate_switch_count,belief_reason,reid_reason,locked_crop_path," + + "suspected_crop_path,best_reid_crop_path"; + private static final String EVENTS_HEADER = "session_id,timestamp_ms,frame_id,event_type,note"; + + public final String sessionId; + public final File sessionDir; + public final File cropsDir; + public final File galleryDir; + public final File overlaysDir; + public final File frameLogCsv; + public final File identityLogCsv; + public final File eventsCsv; + public final long startedAtMs; + + public int frameRows = 0; + public int identityRows = 0; + public int eventRows = 0; + public int cropCount = 0; + public int galleryCount = 0; + + public CartFollowDiagnosticSession(Context context) { + this.startedAtMs = System.currentTimeMillis(); + String timestamp = new SimpleDateFormat("yyyyMMdd_HHmmss", Locale.US).format(new Date()); + this.sessionId = "cart_diag_" + timestamp; + File baseDir = + new File( + context.getExternalFilesDir(Environment.DIRECTORY_PICTURES), + "cartfollow_diagnostics"); + this.sessionDir = new File(baseDir, sessionId); + this.cropsDir = new File(sessionDir, "crops"); + this.galleryDir = new File(sessionDir, "gallery"); + this.overlaysDir = new File(sessionDir, "overlays"); + this.cropsDir.mkdirs(); + this.galleryDir.mkdirs(); + this.overlaysDir.mkdirs(); + this.frameLogCsv = new File(sessionDir, "frame_log.csv"); + this.identityLogCsv = new File(sessionDir, "identity_log.csv"); + this.eventsCsv = new File(sessionDir, "events.csv"); + } + + public void initCsvFiles() { + writeHeader(frameLogCsv, FRAME_LOG_HEADER); + writeHeader(identityLogCsv, IDENTITY_LOG_HEADER); + writeHeader(eventsCsv, EVENTS_HEADER); + } + + public void writeSessionInfo( + CartFollowDiagnosticConfig config, + String detectorModelName, + float minConfidence, + boolean reidAvailable, + int gallerySize, + boolean reidCropUpright, + int sensorOrientation) { + JSONObject json = new JSONObject(); + try { + json.put("session_id", sessionId); + json.put("created_at", new SimpleDateFormat("yyyy-MM-dd HH:mm:ss", Locale.US).format(new Date())); + json.put("collector", "CartFollowDiagnostic"); + json.put("app_mode", "HumanCartSimulator"); + json.put("phase", "phase_c_diagnostic"); + json.put("frame_log_interval_ms", config.frameLogIntervalMs); + json.put("crop_interval_ms", config.cropIntervalMs); + json.put("overlay_interval_ms", config.overlayIntervalMs); + json.put("save_crops", config.saveCrops); + json.put("save_overlays", config.saveOverlays); + json.put("bbox_padding_ratio", config.paddingRatio); + json.put("jpeg_quality", config.jpegQuality); + json.put("detector", detectorModelName == null ? "" : detectorModelName); + json.put("min_confidence", minConfidence); + json.put("reid_available", reidAvailable); + json.put("gallery_size", gallerySize); + json.put("reid_crop_upright", reidCropUpright); + json.put("sensor_orientation", sensorOrientation); + json.put("device_model", Build.MODEL == null ? "" : Build.MODEL); + json.put("sdk_int", Build.VERSION.SDK_INT); + } catch (JSONException e) { + Log.e(TAG, "Failed to build session_info.json", e); + return; + } + + try (FileWriter writer = new FileWriter(new File(sessionDir, "session_info.json"), false)) { + writer.write(json.toString(2)); + } catch (IOException | JSONException e) { + Log.e(TAG, "Failed to write session_info.json", e); + } + } + + private void writeHeader(File file, String header) { + try (FileWriter writer = new FileWriter(file, false)) { + writer.append(header).append('\n'); + } catch (IOException e) { + Log.e(TAG, "Failed to init csv: " + file.getName(), e); + } + } +} diff --git a/android/robot/src/main/java/org/openbot/common/FeatureList.java b/android/robot/src/main/java/org/openbot/common/FeatureList.java index 4c5cdbf81..bd7d9d76e 100644 --- a/android/robot/src/main/java/org/openbot/common/FeatureList.java +++ b/android/robot/src/main/java/org/openbot/common/FeatureList.java @@ -36,6 +36,10 @@ public class FeatureList { public static final String AUTOPILOT = "Autopilot"; public static final String PERSON_FOLLOWING = "Person Following"; public static final String OBJECT_NAV = "Object Tracking"; + public static final String CART_SIMULATOR = "Cart Simulator"; + public static final String REAL_CART_FOLLOW = "Real Cart Follow"; + public static final String PERSON_CROP_COLLECTOR = "Person Crop Collector"; + public static final String PERSON_SEQUENCE_COLLECTOR = "Person Sequence Collector"; public static final String MODEL_MANAGEMENT = "Model Management"; public static final String POINT_GOAL_NAVIGATION = "Point Goal Navigation"; public static final String AUTONOMOUS_DRIVING = "Autonomous Driving"; @@ -82,6 +86,12 @@ public static ArrayList getCategories() { subCategories = new ArrayList<>(); subCategories.add(new SubCategory(AUTOPILOT, R.drawable.ic_autopilot, "#44525F")); subCategories.add(new SubCategory(OBJECT_NAV, R.drawable.ic_person_search, "#E7CE88")); + subCategories.add(new SubCategory(CART_SIMULATOR, R.drawable.ic_person_search, "#6BBF8A")); + subCategories.add(new SubCategory(REAL_CART_FOLLOW, R.drawable.ic_electric_car, "#D05A47")); + subCategories.add( + new SubCategory(PERSON_CROP_COLLECTOR, R.drawable.ic_person_search, "#8BBF6B")); + subCategories.add( + new SubCategory(PERSON_SEQUENCE_COLLECTOR, R.drawable.ic_person_search, "#6BA9BF")); subCategories.add( new SubCategory(POINT_GOAL_NAVIGATION, R.drawable.ic_baseline_golf_course, "#1BBFBF")); subCategories.add(new SubCategory(MODEL_MANAGEMENT, R.drawable.ic_list_bulleted_48, "#BC7680")); diff --git a/android/robot/src/main/java/org/openbot/cropcollector/PersonCropCaptureConfig.java b/android/robot/src/main/java/org/openbot/cropcollector/PersonCropCaptureConfig.java new file mode 100644 index 000000000..5cdf4c883 --- /dev/null +++ b/android/robot/src/main/java/org/openbot/cropcollector/PersonCropCaptureConfig.java @@ -0,0 +1,11 @@ +package org.openbot.cropcollector; + +public class PersonCropCaptureConfig { + public String personId = ""; + public long intervalMs = 500; + public float minConfidence = 0.5f; + public boolean singlePersonOnly = true; + public float paddingRatio = 0.08f; + public int maxCrops = 120; + public int jpegQuality = 95; +} diff --git a/android/robot/src/main/java/org/openbot/cropcollector/PersonCropCollectorFragment.java b/android/robot/src/main/java/org/openbot/cropcollector/PersonCropCollectorFragment.java new file mode 100644 index 000000000..8f8769b3c --- /dev/null +++ b/android/robot/src/main/java/org/openbot/cropcollector/PersonCropCollectorFragment.java @@ -0,0 +1,464 @@ +package org.openbot.cropcollector; + +import android.graphics.Bitmap; +import android.graphics.Canvas; +import android.graphics.Color; +import android.graphics.Matrix; +import android.graphics.Paint; +import android.graphics.RectF; +import android.os.Bundle; +import android.os.Handler; +import android.os.HandlerThread; +import android.os.SystemClock; +import android.view.LayoutInflater; +import android.view.View; +import android.view.ViewGroup; +import android.widget.Toast; +import androidx.annotation.NonNull; +import androidx.annotation.Nullable; +import androidx.camera.core.CameraSelector; +import androidx.camera.core.ImageProxy; +import java.io.IOException; +import java.util.ArrayList; +import java.util.List; +import java.util.Locale; +import org.jetbrains.annotations.NotNull; +import org.openbot.R; +import org.openbot.common.CameraFragment; +import org.openbot.databinding.FragmentPersonCropCollectorBinding; +import org.openbot.env.ImageUtils; +import org.openbot.tflite.Detector; +import org.openbot.tflite.Model; +import org.openbot.tflite.Network; +import org.openbot.utils.CameraUtils; +import org.openbot.utils.Enums; +import timber.log.Timber; + +public class PersonCropCollectorFragment extends CameraFragment { + + private FragmentPersonCropCollectorBinding binding; + private Handler handler; + private HandlerThread handlerThread; + + private boolean computingNetwork = false; + private float minConfidence = 0.5f; + + private Detector detector; + private Matrix frameToCropTransform; + private Bitmap croppedBitmap; + private int sensorOrientation; + private Matrix cropToFrameTransform; + + private Model model; + private Network.Device device = Network.Device.CPU; + private int numThreads = -1; + private final String classType = "person"; + + private long lastProcessingTimeMs = -1; + private long frameNum = 0; + + private final PersonCropCaptureConfig config = new PersonCropCaptureConfig(); + private PersonCropSession currentSession = null; + private PersonCropSaver saver = null; + private boolean captureEnabled = false; + private long lastSaveTimeMs = 0; + + private final List drawBoxes = new ArrayList<>(); + private int drawFrameWidth = 0; + private int drawFrameHeight = 0; + private int drawSensorOrientation = 0; + + private final Paint personBoxPaint = new Paint(); + private final Paint boxTextPaint = new Paint(); + + @Override + public void onCreate(@Nullable Bundle savedInstanceState) { + super.onCreate(savedInstanceState); + personBoxPaint.setColor(Color.GREEN); + personBoxPaint.setStyle(Paint.Style.STROKE); + personBoxPaint.setStrokeWidth(6.0f); + boxTextPaint.setColor(Color.WHITE); + boxTextPaint.setTextSize(36.0f); + } + + @Override + public View onCreateView( + @NotNull LayoutInflater inflater, ViewGroup container, Bundle savedInstanceState) { + binding = FragmentPersonCropCollectorBinding.inflate(inflater, container, false); + return inflateFragment(binding, inflater, container); + } + + @Override + public void onViewCreated(@NonNull View view, @Nullable Bundle savedInstanceState) { + super.onViewCreated(view, savedInstanceState); + + binding.confidenceValue.setText((int) (minConfidence * 100) + "%"); + binding.plusConfidence.setOnClickListener( + v -> { + int confValue = (int) (minConfidence * 100); + if (confValue >= 95) return; + confValue += 5; + minConfidence = confValue / 100f; + binding.confidenceValue.setText(confValue + "%"); + }); + binding.minusConfidence.setOnClickListener( + v -> { + int confValue = (int) (minConfidence * 100); + if (confValue <= 5) return; + confValue -= 5; + minConfidence = confValue / 100f; + binding.confidenceValue.setText(confValue + "%"); + }); + + List models = getModelNames(f -> f.type.equals(Model.TYPE.DETECTOR)); + initModelSpinner(binding.modelSpinner, models, preferencesManager.getObjectNavModel()); + + setAnalyserResolution(Enums.Preview.HD.getValue()); + + binding.trackingOverlay.addCallback(canvas -> drawOverlay(canvas)); + + binding.statusText.setText(getString(R.string.person_crop_idle)); + binding.btnStart.setEnabled(true); + binding.btnStop.setEnabled(false); + + binding.intervalValue.setText(config.intervalMs + "ms"); + binding.minusInterval.setOnClickListener( + v -> { + if (config.intervalMs <= 100) return; + config.intervalMs -= 100; + binding.intervalValue.setText(config.intervalMs + "ms"); + }); + binding.plusInterval.setOnClickListener( + v -> { + if (config.intervalMs >= 2000) return; + config.intervalMs += 100; + binding.intervalValue.setText(config.intervalMs + "ms"); + }); + binding.singlePersonSwitch.setOnCheckedChangeListener( + (buttonView, isChecked) -> config.singlePersonOnly = isChecked); + + binding.btnStart.setOnClickListener(v -> startCapture()); + binding.btnStop.setOnClickListener(v -> stopCapture()); + } + + private void startCapture() { + String personId = binding.personIdInput.getText().toString().trim(); + if (personId.isEmpty()) { + Toast.makeText(requireContext(), "请输入 Person ID", Toast.LENGTH_SHORT).show(); + return; + } + + config.personId = personId; + config.minConfidence = minConfidence; + + currentSession = new PersonCropSession(requireContext(), personId); + currentSession.initMetadataCsv(); + currentSession.writeSessionInfo(config); + + if (saver != null) { + saver.shutdown(); + } + saver = new PersonCropSaver(); + captureEnabled = true; + lastSaveTimeMs = 0; + + binding.btnStart.setEnabled(false); + binding.btnStop.setEnabled(true); + binding.personIdInput.setEnabled(false); + binding.modelSpinner.setEnabled(false); + binding.minusInterval.setEnabled(false); + binding.plusInterval.setEnabled(false); + binding.singlePersonSwitch.setEnabled(false); + binding.statusText.setText("采集中: " + currentSession.sessionId); + } + + private void stopCapture() { + captureEnabled = false; + if (saver != null) { + saver.shutdown(); + saver = null; + } + int saved = currentSession != null ? currentSession.savedCount : 0; + int skipped = currentSession != null ? currentSession.skippedCount : 0; + String path = currentSession != null ? currentSession.sessionDir.getAbsolutePath() : ""; + + binding.btnStart.setEnabled(true); + binding.btnStop.setEnabled(false); + binding.personIdInput.setEnabled(true); + binding.modelSpinner.setEnabled(true); + binding.minusInterval.setEnabled(true); + binding.plusInterval.setEnabled(true); + binding.singlePersonSwitch.setEnabled(true); + binding.statusText.setText( + String.format(Locale.US, "已停止: saved=%d skipped=%d\n%s", saved, skipped, path)); + } + + protected void onInferenceConfigurationChanged() { + computingNetwork = false; + if (croppedBitmap == null) return; + final Network.Device device = getDevice(); + final Model model = getModel(); + final int numThreads = getNumThreads(); + runInBackground(() -> recreateNetwork(model, device, numThreads)); + } + + private void recreateNetwork(Model model, Network.Device device, int numThreads) { + if (model == null) return; + Detector newDetector = null; + try { + newDetector = Detector.create(requireActivity(), model, device, numThreads); + } catch (IllegalArgumentException | IOException e) { + Timber.e(e, "Failed to create network."); + String msg = + model.pathType == Model.PATH_TYPE.URL + ? "该模型未下载，请先在主菜单 Model Management 中下载: " + model.name + : "模型加载失败: " + e.getMessage(); + requireActivity() + .runOnUiThread( + () -> + Toast.makeText(requireContext().getApplicationContext(), msg, Toast.LENGTH_LONG) + .show()); + return; + } + + if (detector != null) { + detector.close(); + } + detector = newDetector; + try { + croppedBitmap = + Bitmap.createBitmap( + detector.getImageSizeX(), detector.getImageSizeY(), Bitmap.Config.ARGB_8888); + frameToCropTransform = + ImageUtils.getTransformationMatrix( + getMaxAnalyseImageSize().getWidth(), + getMaxAnalyseImageSize().getHeight(), + croppedBitmap.getWidth(), + croppedBitmap.getHeight(), + sensorOrientation, + detector.getCropRect(), + detector.getMaintainAspect()); + cropToFrameTransform = new Matrix(); + frameToCropTransform.invert(cropToFrameTransform); + } catch (Exception e) { + Timber.e(e, "Failed to configure detector."); + requireActivity() + .runOnUiThread( + () -> + Toast.makeText( + requireContext().getApplicationContext(), + "模型配置失败: " + e.getMessage(), + Toast.LENGTH_LONG) + .show()); + } + } + + @Override + public synchronized void onResume() { + croppedBitmap = null; + handlerThread = new HandlerThread("inference"); + handlerThread.start(); + handler = new Handler(handlerThread.getLooper()); + super.onResume(); + } + + @Override + public synchronized void onPause() { + if (captureEnabled) { + stopCapture(); + } + handlerThread.quitSafely(); + try { + handlerThread.join(); + handlerThread = null; + handler = null; + } catch (final InterruptedException e) { + e.printStackTrace(); + } + super.onPause(); + } + + protected synchronized void runInBackground(final Runnable r) { + if (handler != null) handler.post(r); + } + + @Override + protected void processUSBData(String data) {} + + @Override + protected void processControllerKeyData(String commandType) {} + + @Override + protected void processFrame(Bitmap bitmap, ImageProxy image) { + if (detector == null) { + updateCropImageInfo(); + if (detector == null) return; + } + + ++frameNum; + if (binding == null) return; + if (computingNetwork) return; + + computingNetwork = true; + runInBackground( + () -> { + final Canvas canvas = new Canvas(croppedBitmap); + Bitmap workingFrame = bitmap; + if (lensFacing == CameraSelector.LENS_FACING_FRONT) { + Bitmap flipped = CameraUtils.flipBitmapHorizontal(bitmap); + canvas.drawBitmap(flipped, frameToCropTransform, null); + workingFrame = flipped; + } else { + canvas.drawBitmap(bitmap, frameToCropTransform, null); + } + + if (detector != null) { + final long startTime = SystemClock.elapsedRealtime(); + final List results = + detector.recognizeImage(croppedBitmap, classType); + lastProcessingTimeMs = SystemClock.elapsedRealtime() - startTime; + + final List mappedRecognitions = new ArrayList<>(); + for (final Detector.Recognition result : results) { + final RectF location = result.getLocation(); + if (location != null && result.getConfidence() >= minConfidence) { + cropToFrameTransform.mapRect(location); + result.setLocation(location); + mappedRecognitions.add(result); + } + } + + int frameW = getMaxAnalyseImageSize().getWidth(); + int frameH = getMaxAnalyseImageSize().getHeight(); + updateDrawState(mappedRecognitions, frameW, frameH, sensorOrientation); + float fps = lastProcessingTimeMs > 0 ? 1000f / lastProcessingTimeMs : 0f; + + handleCropCapture(workingFrame, mappedRecognitions); + + updateDebugInfo(mappedRecognitions.size(), fps); + binding.trackingOverlay.postInvalidate(); + } + computingNetwork = false; + }); + } + + private void updateCropImageInfo() { + sensorOrientation = 90 - ImageUtils.getScreenOrientation(requireActivity()); + recreateNetwork(getModel(), getDevice(), getNumThreads()); + } + + private void handleCropCapture(Bitmap workingFrame, List persons) { + if (!captureEnabled || currentSession == null || saver == null) return; + + long now = System.currentTimeMillis(); + if (now - lastSaveTimeMs < config.intervalMs) return; + + if (currentSession.savedCount >= config.maxCrops) { + requireActivity().runOnUiThread(() -> stopCapture()); + return; + } + + if (persons.isEmpty()) { + currentSession.skippedCount++; + return; + } + + if (config.singlePersonOnly && persons.size() != 1) { + currentSession.skippedCount++; + return; + } + + int numPersons = persons.size(); + if (config.singlePersonOnly) { + int cropId = currentSession.savedCount + 1; + currentSession.savedCount = cropId; + saver.saveCropAsync( + workingFrame, persons.get(0), 0, numPersons, currentSession, config, frameNum, cropId, sensorOrientation); + } else { + for (int i = 0; i < numPersons; i++) { + int cropId = currentSession.savedCount + 1; + currentSession.savedCount = cropId; + saver.saveCropAsync( + workingFrame, persons.get(i), i, numPersons, currentSession, config, frameNum, cropId, sensorOrientation); + } + } + lastSaveTimeMs = now; + } + + private synchronized void updateDrawState( + List persons, int frameW, int frameH, int sensorOrientation) { + drawBoxes.clear(); + for (Detector.Recognition r : persons) { + if (r == null || r.getLocation() == null) continue; + drawBoxes.add(new RectF(r.getLocation())); + } + drawFrameWidth = frameW; + drawFrameHeight = frameH; + drawSensorOrientation = sensorOrientation; + } + + private void drawOverlay(Canvas canvas) { + if (drawFrameWidth <= 0 || drawFrameHeight <= 0) return; + final boolean rotated = drawSensorOrientation % 180 == 90; + final float multiplier = + Math.min( + canvas.getHeight() / (float) (rotated ? drawFrameWidth : drawFrameHeight), + canvas.getWidth() / (float) (rotated ? drawFrameHeight : drawFrameWidth)); + Matrix matrix = + ImageUtils.getTransformationMatrix( + drawFrameWidth, + drawFrameHeight, + (int) (multiplier * (rotated ? drawFrameHeight : drawFrameWidth)), + (int) (multiplier * (rotated ? drawFrameWidth : drawFrameHeight)), + drawSensorOrientation, + new RectF(0, 0, 0, 0), + false); + + List snapshot; + synchronized (this) { + snapshot = new ArrayList<>(drawBoxes); + } + for (RectF box : snapshot) { + RectF rect = new RectF(box); + matrix.mapRect(rect); + float cornerSize = Math.min(rect.width(), rect.height()) / 8.0f; + canvas.drawRoundRect(rect, cornerSize, cornerSize, personBoxPaint); + } + } + + private void updateDebugInfo(int persons, float fps) { + if (binding == null) return; + int saved = currentSession != null ? currentSession.savedCount : 0; + int skipped = currentSession != null ? currentSession.skippedCount : 0; + String info = + String.format( + Locale.US, + "persons=%d\nsaved=%d\nskipped=%d\nfps=%.1f", + persons, + saved, + skipped, + fps); + requireActivity().runOnUiThread(() -> binding.debugInfo.setText(info)); + } + + protected Model getModel() { + return model; + } + + @Override + protected void setModel(Model model) { + if (this.model != model) { + this.model = model; + preferencesManager.setObjectNavModel(model.name); + onInferenceConfigurationChanged(); + } + } + + protected Network.Device getDevice() { + return device; + } + + protected int getNumThreads() { + return numThreads; + } +} diff --git a/android/robot/src/main/java/org/openbot/cropcollector/PersonCropSaver.java b/android/robot/src/main/java/org/openbot/cropcollector/PersonCropSaver.java new file mode 100644 index 000000000..081da68e0 --- /dev/null +++ b/android/robot/src/main/java/org/openbot/cropcollector/PersonCropSaver.java @@ -0,0 +1,154 @@ +package org.openbot.cropcollector; + +import android.graphics.Bitmap; +import android.graphics.Matrix; +import android.graphics.RectF; +import java.io.File; +import java.io.FileOutputStream; +import java.io.FileWriter; +import java.io.IOException; +import java.util.Locale; +import java.util.concurrent.ExecutorService; +import java.util.concurrent.Executors; +import org.openbot.tflite.Detector.Recognition; +import timber.log.Timber; + +public class PersonCropSaver { + + private final ExecutorService executor = Executors.newSingleThreadExecutor(); + + public void saveCropAsync( + Bitmap frame, + Recognition person, + int personIndex, + int numPersons, + PersonCropSession session, + PersonCropCaptureConfig config, + long frameNum, + int cropId, + int sensorOrientation) { + RectF bbox = person.getLocation(); + if (bbox == null || frame == null) { + session.skippedCount++; + return; + } + + float padX = bbox.width() * config.paddingRatio; + float padY = bbox.height() * config.paddingRatio; + int left = clamp((int) (bbox.left - padX), 0, frame.getWidth() - 1); + int top = clamp((int) (bbox.top - padY), 0, frame.getHeight() - 1); + int right = clamp((int) (bbox.right + padX), left + 1, frame.getWidth()); + int bottom = clamp((int) (bbox.bottom + padY), top + 1, frame.getHeight()); + int w = right - left; + int h = bottom - top; + if (w <= 0 || h <= 0) { + session.skippedCount++; + return; + } + + final Bitmap rawCrop; + try { + rawCrop = Bitmap.createBitmap(frame, left, top, w, h); + } catch (Exception e) { + session.skippedCount++; + return; + } + + final Bitmap uprightCrop; + if (sensorOrientation % 360 != 0) { + Matrix matrix = new Matrix(); + matrix.postRotate(sensorOrientation); + uprightCrop = Bitmap.createBitmap(rawCrop, 0, 0, rawCrop.getWidth(), rawCrop.getHeight(), matrix, true); + rawCrop.recycle(); + } else { + uprightCrop = rawCrop; + } + + final int imgW = frame.getWidth(); + final int imgH = frame.getHeight(); + final boolean edgeTouch = left == 0 || top == 0 || right == imgW || bottom == imgH; + final RectF bboxCopy = new RectF(bbox); + final float confidence = person.getConfidence() == null ? 0f : person.getConfidence(); + + executor.execute( + () -> { + String filename = + String.format(Locale.US, "%06d_person%d_conf%.2f.jpg", cropId, personIndex, confidence); + File cropFile = new File(session.cropsDir, filename); + + try (FileOutputStream fos = new FileOutputStream(cropFile)) { + uprightCrop.compress(Bitmap.CompressFormat.JPEG, config.jpegQuality, fos); + } catch (IOException e) { + Timber.e(e, "Failed to save crop"); + } finally { + uprightCrop.recycle(); + } + + appendMetadataCsv( + session, + config, + personIndex, + numPersons, + cropId, + frameNum, + filename, + bboxCopy, + imgW, + imgH, + confidence, + edgeTouch); + }); + } + + private void appendMetadataCsv( + PersonCropSession session, + PersonCropCaptureConfig config, + int personIndex, + int numPersons, + int cropId, + long frameNum, + String filename, + RectF bbox, + int imgW, + int imgH, + float confidence, + boolean edgeTouch) { + String cropPath = "crops/" + filename; + String row = + String.format( + Locale.US, + "%s,%s,%d,%d,%d,%s,%d,%d,%.4f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%d,%d,%s,%s\n", + session.sessionId, + config.personId, + frameNum, + cropId, + System.currentTimeMillis(), + cropPath, + numPersons, + personIndex, + confidence, + bbox.left, + bbox.top, + bbox.right, + bbox.bottom, + bbox.width(), + bbox.height(), + imgW, + imgH, + edgeTouch ? "1" : "0", + "single_person_valid"); + try (FileWriter writer = new FileWriter(session.metadataCsv, true)) { + writer.append(row); + } catch (IOException e) { + Timber.e(e, "Failed to append metadata.csv"); + } + } + + public void shutdown() { + executor.shutdown(); + } + + private static int clamp(int v, int lo, int hi) { + return v < lo ? lo : (v > hi ? hi : v); + } +} diff --git a/android/robot/src/main/java/org/openbot/cropcollector/PersonCropSession.java b/android/robot/src/main/java/org/openbot/cropcollector/PersonCropSession.java new file mode 100644 index 000000000..f0c3061be --- /dev/null +++ b/android/robot/src/main/java/org/openbot/cropcollector/PersonCropSession.java @@ -0,0 +1,79 @@ +package org.openbot.cropcollector; + +import android.content.Context; +import android.os.Environment; +import android.util.Log; +import java.io.File; +import java.io.FileWriter; +import java.io.IOException; +import java.text.SimpleDateFormat; +import java.util.Date; +import java.util.Locale; +import org.json.JSONException; +import org.json.JSONObject; + +public class PersonCropSession { + + private static final String TAG = "PersonCropSession"; + + private static final String CSV_HEADER = + "session_id,person_id,frame_id,crop_id,timestamp_ms,crop_path,num_persons,person_index," + + "confidence,bbox_left,bbox_top,bbox_right,bbox_bottom,bbox_width,bbox_height," + + "image_width,image_height,edge_touch,save_reason"; + + public final String sessionId; + public final String personId; + public final File sessionDir; + public final File cropsDir; + public final File metadataCsv; + public int savedCount = 0; + public int skippedCount = 0; + + public PersonCropSession(Context context, String personId) { + this.personId = personId; + String timestamp = new SimpleDateFormat("yyyyMMdd_HHmmss", Locale.US).format(new Date()); + this.sessionId = personId + "_" + timestamp; + + File baseDir = + new File( + context.getExternalFilesDir(Environment.DIRECTORY_PICTURES), "cartfollow_crops"); + this.sessionDir = new File(baseDir, sessionId); + this.cropsDir = new File(sessionDir, "crops"); + this.cropsDir.mkdirs(); + this.metadataCsv = new File(sessionDir, "metadata.csv"); + } + + public void initMetadataCsv() { + try (FileWriter writer = new FileWriter(metadataCsv, false)) { + writer.append(CSV_HEADER).append('\n'); + } catch (IOException e) { + Log.e(TAG, "Failed to init metadata.csv", e); + } + } + + public void writeSessionInfo(PersonCropCaptureConfig config) { + JSONObject json = new JSONObject(); + try { + json.put("session_id", sessionId); + json.put("person_id", personId); + json.put("created_at", new SimpleDateFormat("yyyy-MM-dd HH:mm:ss", Locale.US).format(new Date())); + json.put("capture_interval_ms", config.intervalMs); + json.put("min_confidence", config.minConfidence); + json.put("single_person_only", config.singlePersonOnly); + json.put("bbox_padding_ratio", config.paddingRatio); + json.put("max_crops", config.maxCrops); + json.put("jpeg_quality", config.jpegQuality); + json.put("app_mode", "PersonCropCollector"); + } catch (JSONException e) { + Log.e(TAG, "Failed to build session_info.json", e); + return; + } + + File infoFile = new File(sessionDir, "session_info.json"); + try (FileWriter writer = new FileWriter(infoFile, false)) { + writer.write(json.toString(2)); + } catch (IOException | JSONException e) { + Log.e(TAG, "Failed to write session_info.json", e); + } + } +} diff --git a/android/robot/src/main/java/org/openbot/main/MainFragment.java b/android/robot/src/main/java/org/openbot/main/MainFragment.java index a6c63e40e..e6974b400 100644 --- a/android/robot/src/main/java/org/openbot/main/MainFragment.java +++ b/android/robot/src/main/java/org/openbot/main/MainFragment.java @@ -87,6 +87,26 @@ public void onItemClick(SubCategory subCategory) { .navigate(R.id.action_mainFragment_to_objectNavFragment); break; + case FeatureList.CART_SIMULATOR: + Navigation.findNavController(requireView()) + .navigate(R.id.action_mainFragment_to_cartSimFragment); + break; + + case FeatureList.REAL_CART_FOLLOW: + Navigation.findNavController(requireView()) + .navigate(R.id.action_mainFragment_to_realCartFollowFragment); + break; + + case FeatureList.PERSON_CROP_COLLECTOR: + Navigation.findNavController(requireView()) + .navigate(R.id.action_mainFragment_to_personCropCollectorFragment); + break; + + case FeatureList.PERSON_SEQUENCE_COLLECTOR: + Navigation.findNavController(requireView()) + .navigate(R.id.action_mainFragment_to_personSequenceCollectorFragment); + break; + case FeatureList.POINT_GOAL_NAVIGATION: Navigation.findNavController(requireView()) .navigate(R.id.action_mainFragment_to_pointGoalNavigationFragment); diff --git a/android/robot/src/main/java/org/openbot/sequencecollector/PersonSequenceCaptureConfig.java b/android/robot/src/main/java/org/openbot/sequencecollector/PersonSequenceCaptureConfig.java new file mode 100644 index 000000000..e3e291305 --- /dev/null +++ b/android/robot/src/main/java/org/openbot/sequencecollector/PersonSequenceCaptureConfig.java @@ -0,0 +1,13 @@ +package org.openbot.sequencecollector; + +public class PersonSequenceCaptureConfig { + public String personId = ""; + public long frameLogIntervalMs = 200; + public long cropIntervalMs = 500; + public long overlayIntervalMs = 1000; + public float minConfidence = 0.5f; + public boolean saveCrops = true; + public boolean saveOverlays = false; + public float paddingRatio = 0.08f; + public int jpegQuality = 90; +} diff --git a/android/robot/src/main/java/org/openbot/sequencecollector/PersonSequenceCollectorFragment.java b/android/robot/src/main/java/org/openbot/sequencecollector/PersonSequenceCollectorFragment.java new file mode 100644 index 000000000..96e56b3a2 --- /dev/null +++ b/android/robot/src/main/java/org/openbot/sequencecollector/PersonSequenceCollectorFragment.java @@ -0,0 +1,503 @@ +package org.openbot.sequencecollector; + +import android.graphics.Bitmap; +import android.graphics.Canvas; +import android.graphics.Color; +import android.graphics.Matrix; +import android.graphics.Paint; +import android.graphics.RectF; +import android.os.Bundle; +import android.os.Handler; +import android.os.HandlerThread; +import android.os.SystemClock; +import android.view.LayoutInflater; +import android.view.View; +import android.view.ViewGroup; +import android.widget.Toast; +import androidx.annotation.NonNull; +import androidx.annotation.Nullable; +import androidx.camera.core.CameraSelector; +import androidx.camera.core.ImageProxy; +import java.io.IOException; +import java.util.ArrayList; +import java.util.List; +import java.util.Locale; +import org.jetbrains.annotations.NotNull; +import org.openbot.R; +import org.openbot.common.CameraFragment; +import org.openbot.databinding.FragmentPersonSequenceCollectorBinding; +import org.openbot.env.ImageUtils; +import org.openbot.tflite.Detector; +import org.openbot.tflite.Model; +import org.openbot.tflite.Network; +import org.openbot.utils.CameraUtils; +import org.openbot.utils.Enums; +import timber.log.Timber; + +public class PersonSequenceCollectorFragment extends CameraFragment { + + private FragmentPersonSequenceCollectorBinding binding; + private Handler handler; + private HandlerThread handlerThread; + + private boolean computingNetwork = false; + private float minConfidence = 0.5f; + + private Detector detector; + private Matrix frameToCropTransform; + private Bitmap croppedBitmap; + private int sensorOrientation; + private Matrix cropToFrameTransform; + + private Model model; + private Network.Device device = Network.Device.CPU; + private int numThreads = -1; + private final String classType = "person"; + + private long lastProcessingTimeMs = -1; + private long frameNum = 0; + + private final PersonSequenceCaptureConfig config = new PersonSequenceCaptureConfig(); + private PersonSequenceSession currentSession = null; + private PersonSequenceSaver saver = null; + private boolean captureEnabled = false; + private long lastFrameLogTimeMs = 0; + private long lastCropTimeMs = 0; + private String pendingEventTag = ""; + private String pendingEventNote = ""; + + private final List drawBoxes = new ArrayList<>(); + private int drawFrameWidth = 0; + private int drawFrameHeight = 0; + private int drawSensorOrientation = 0; + + private final Paint personBoxPaint = new Paint(); + private final Paint boxTextPaint = new Paint(); + + @Override + public void onCreate(@Nullable Bundle savedInstanceState) { + super.onCreate(savedInstanceState); + personBoxPaint.setColor(Color.CYAN); + personBoxPaint.setStyle(Paint.Style.STROKE); + personBoxPaint.setStrokeWidth(6.0f); + boxTextPaint.setColor(Color.WHITE); + boxTextPaint.setTextSize(36.0f); + } + + @Override + public View onCreateView( + @NotNull LayoutInflater inflater, ViewGroup container, Bundle savedInstanceState) { + binding = FragmentPersonSequenceCollectorBinding.inflate(inflater, container, false); + return inflateFragment(binding, inflater, container); + } + + @Override + public void onViewCreated(@NonNull View view, @Nullable Bundle savedInstanceState) { + super.onViewCreated(view, savedInstanceState); + + binding.confidenceValue.setText((int) (minConfidence * 100) + "%"); + binding.plusConfidence.setOnClickListener( + v -> { + int confValue = (int) (minConfidence * 100); + if (confValue >= 95) return; + confValue += 5; + minConfidence = confValue / 100f; + binding.confidenceValue.setText(confValue + "%"); + }); + binding.minusConfidence.setOnClickListener( + v -> { + int confValue = (int) (minConfidence * 100); + if (confValue <= 5) return; + confValue -= 5; + minConfidence = confValue / 100f; + binding.confidenceValue.setText(confValue + "%"); + }); + + List models = getModelNames(f -> f.type.equals(Model.TYPE.DETECTOR)); + initModelSpinner(binding.modelSpinner, models, preferencesManager.getObjectNavModel()); + setAnalyserResolution(Enums.Preview.HD.getValue()); + + binding.trackingOverlay.addCallback(canvas -> drawOverlay(canvas)); + binding.statusText.setText(getString(R.string.person_sequence_idle)); + binding.btnStart.setEnabled(true); + binding.btnStop.setEnabled(false); + + binding.frameIntervalValue.setText(config.frameLogIntervalMs + "ms"); + binding.minusFrameInterval.setOnClickListener(v -> adjustFrameInterval(-100)); + binding.plusFrameInterval.setOnClickListener(v -> adjustFrameInterval(100)); + + binding.cropIntervalValue.setText(config.cropIntervalMs + "ms"); + binding.minusCropInterval.setOnClickListener(v -> adjustCropInterval(-100)); + binding.plusCropInterval.setOnClickListener(v -> adjustCropInterval(100)); + + binding.saveCropsSwitch.setChecked(config.saveCrops); + binding.saveCropsSwitch.setOnCheckedChangeListener((buttonView, isChecked) -> config.saveCrops = isChecked); + + binding.btnStart.setOnClickListener(v -> startCapture()); + binding.btnStop.setOnClickListener(v -> stopCapture()); + binding.btnTargetLeft.setOnClickListener(v -> recordManualEvent("target_left")); + binding.btnTargetReturn.setOnClickListener(v -> recordManualEvent("target_return")); + binding.btnOcclusionStart.setOnClickListener(v -> recordManualEvent("occlusion_start")); + binding.btnOcclusionEnd.setOnClickListener(v -> recordManualEvent("occlusion_end")); + binding.btnDistractorEnter.setOnClickListener(v -> recordManualEvent("distractor_enter")); + binding.btnDistractorLeave.setOnClickListener(v -> recordManualEvent("distractor_leave")); + binding.btnManualNote.setOnClickListener(v -> recordManualEvent("manual_note")); + } + + private void adjustFrameInterval(long deltaMs) { + long next = config.frameLogIntervalMs + deltaMs; + if (next < 100 || next > 2000) return; + config.frameLogIntervalMs = next; + binding.frameIntervalValue.setText(config.frameLogIntervalMs + "ms"); + } + + private void adjustCropInterval(long deltaMs) { + long next = config.cropIntervalMs + deltaMs; + if (next < 200 || next > 3000) return; + config.cropIntervalMs = next; + binding.cropIntervalValue.setText(config.cropIntervalMs + "ms"); + } + + private void startCapture() { + String personId = binding.personIdInput.getText().toString().trim(); + if (personId.isEmpty()) { + Toast.makeText(requireContext(), "Please input Person ID", Toast.LENGTH_SHORT).show(); + return; + } + + config.personId = personId; + config.minConfidence = minConfidence; + + currentSession = new PersonSequenceSession(requireContext(), personId); + currentSession.initCsvFiles(); + currentSession.writeSessionInfo(config, model == null ? "" : model.name); + + if (saver != null) { + saver.shutdown(); + } + saver = new PersonSequenceSaver(); + captureEnabled = true; + lastFrameLogTimeMs = 0; + lastCropTimeMs = 0; + pendingEventTag = ""; + pendingEventNote = ""; + + setCaptureControlsEnabled(false); + binding.statusText.setText("Collecting: " + currentSession.sessionId); + } + + private void stopCapture() { + captureEnabled = false; + if (saver != null) { + saver.shutdown(); + saver = null; + } + String path = currentSession != null ? currentSession.sessionDir.getAbsolutePath() : ""; + int frames = currentSession != null ? currentSession.frameRows : 0; + int detections = currentSession != null ? currentSession.detectionRows : 0; + int crops = currentSession != null ? currentSession.cropCount : 0; + int events = currentSession != null ? currentSession.eventRows : 0; + + setCaptureControlsEnabled(true); + binding.statusText.setText( + String.format( + Locale.US, + "Stopped frames=%d det=%d crops=%d events=%d\n%s", + frames, + detections, + crops, + events, + path)); + } + + private void setCaptureControlsEnabled(boolean enabled) { + binding.btnStart.setEnabled(enabled); + binding.btnStop.setEnabled(!enabled); + binding.personIdInput.setEnabled(enabled); + binding.modelSpinner.setEnabled(enabled); + binding.minusFrameInterval.setEnabled(enabled); + binding.plusFrameInterval.setEnabled(enabled); + binding.minusCropInterval.setEnabled(enabled); + binding.plusCropInterval.setEnabled(enabled); + binding.saveCropsSwitch.setEnabled(enabled); + } + + private void recordManualEvent(String eventType) { + String note = binding.noteInput.getText().toString().trim(); + if (captureEnabled && currentSession != null && saver != null) { + pendingEventTag = eventType; + pendingEventNote = note; + saver.saveEventAsync(currentSession, frameNum, eventType, note); + binding.statusText.setText("Event: " + eventType); + } else { + Toast.makeText(requireContext(), "Start a sequence session first", Toast.LENGTH_SHORT).show(); + } + } + + protected void onInferenceConfigurationChanged() { + computingNetwork = false; + if (croppedBitmap == null) return; + final Network.Device device = getDevice(); + final Model model = getModel(); + final int numThreads = getNumThreads(); + runInBackground(() -> recreateNetwork(model, device, numThreads)); + } + + private void recreateNetwork(Model model, Network.Device device, int numThreads) { + if (model == null) return; + Detector newDetector = null; + try { + newDetector = Detector.create(requireActivity(), model, device, numThreads); + } catch (IllegalArgumentException | IOException e) { + Timber.e(e, "Failed to create network."); + String msg = + model.pathType == Model.PATH_TYPE.URL + ? "Model not downloaded. Download it in Model Management first: " + model.name + : "Failed to load model: " + e.getMessage(); + requireActivity() + .runOnUiThread( + () -> Toast.makeText(requireContext().getApplicationContext(), msg, Toast.LENGTH_LONG).show()); + return; + } + + if (detector != null) { + detector.close(); + } + detector = newDetector; + try { + croppedBitmap = + Bitmap.createBitmap( + detector.getImageSizeX(), detector.getImageSizeY(), Bitmap.Config.ARGB_8888); + frameToCropTransform = + ImageUtils.getTransformationMatrix( + getMaxAnalyseImageSize().getWidth(), + getMaxAnalyseImageSize().getHeight(), + croppedBitmap.getWidth(), + croppedBitmap.getHeight(), + sensorOrientation, + detector.getCropRect(), + detector.getMaintainAspect()); + cropToFrameTransform = new Matrix(); + frameToCropTransform.invert(cropToFrameTransform); + } catch (Exception e) { + Timber.e(e, "Failed to configure detector."); + requireActivity() + .runOnUiThread( + () -> + Toast.makeText( + requireContext().getApplicationContext(), + "Failed to configure model: " + e.getMessage(), + Toast.LENGTH_LONG) + .show()); + } + } + + @Override + public synchronized void onResume() { + croppedBitmap = null; + handlerThread = new HandlerThread("sequence-inference"); + handlerThread.start(); + handler = new Handler(handlerThread.getLooper()); + super.onResume(); + } + + @Override + public synchronized void onPause() { + if (captureEnabled) { + stopCapture(); + } + handlerThread.quitSafely(); + try { + handlerThread.join(); + handlerThread = null; + handler = null; + } catch (final InterruptedException e) { + e.printStackTrace(); + } + super.onPause(); + } + + protected synchronized void runInBackground(final Runnable r) { + if (handler != null) handler.post(r); + } + + @Override + protected void processUSBData(String data) {} + + @Override + protected void processControllerKeyData(String commandType) {} + + @Override + protected void processFrame(Bitmap bitmap, ImageProxy image) { + if (detector == null) { + updateCropImageInfo(); + if (detector == null) return; + } + + ++frameNum; + if (binding == null) return; + if (computingNetwork) return; + + computingNetwork = true; + runInBackground( + () -> { + final Canvas canvas = new Canvas(croppedBitmap); + Bitmap workingFrame = bitmap; + if (lensFacing == CameraSelector.LENS_FACING_FRONT) { + Bitmap flipped = CameraUtils.flipBitmapHorizontal(bitmap); + canvas.drawBitmap(flipped, frameToCropTransform, null); + workingFrame = flipped; + } else { + canvas.drawBitmap(bitmap, frameToCropTransform, null); + } + + if (detector != null) { + final long startTime = SystemClock.elapsedRealtime(); + final List results = + detector.recognizeImage(croppedBitmap, classType); + lastProcessingTimeMs = SystemClock.elapsedRealtime() - startTime; + + final List mappedRecognitions = new ArrayList<>(); + for (final Detector.Recognition result : results) { + final RectF location = result.getLocation(); + if (location != null && result.getConfidence() >= minConfidence) { + cropToFrameTransform.mapRect(location); + result.setLocation(location); + mappedRecognitions.add(result); + } + } + + int frameW = getMaxAnalyseImageSize().getWidth(); + int frameH = getMaxAnalyseImageSize().getHeight(); + updateDrawState(mappedRecognitions, frameW, frameH, sensorOrientation); + float fps = lastProcessingTimeMs > 0 ? 1000f / lastProcessingTimeMs : 0f; + + handleSequenceCapture(workingFrame, mappedRecognitions, frameW, frameH); + updateDebugInfo(mappedRecognitions.size(), fps); + binding.trackingOverlay.postInvalidate(); + } + computingNetwork = false; + }); + } + + private void updateCropImageInfo() { + sensorOrientation = 90 - ImageUtils.getScreenOrientation(requireActivity()); + recreateNetwork(getModel(), getDevice(), getNumThreads()); + } + + private void handleSequenceCapture( + Bitmap workingFrame, List persons, int frameW, int frameH) { + if (!captureEnabled || currentSession == null || saver == null) return; + + long now = System.currentTimeMillis(); + if (now - lastFrameLogTimeMs < config.frameLogIntervalMs) return; + + boolean saveCropsForThisFrame = config.saveCrops && now - lastCropTimeMs >= config.cropIntervalMs; + String eventTag = pendingEventTag; + String eventNote = pendingEventNote; + pendingEventTag = ""; + pendingEventNote = ""; + + saver.saveFrameAsync( + workingFrame, + persons, + currentSession, + config, + frameNum, + frameW, + frameH, + sensorOrientation, + saveCropsForThisFrame, + eventTag, + eventNote); + + lastFrameLogTimeMs = now; + if (saveCropsForThisFrame) { + lastCropTimeMs = now; + } + } + + private synchronized void updateDrawState( + List persons, int frameW, int frameH, int sensorOrientation) { + drawBoxes.clear(); + for (Detector.Recognition r : persons) { + if (r == null || r.getLocation() == null) continue; + drawBoxes.add(new RectF(r.getLocation())); + } + drawFrameWidth = frameW; + drawFrameHeight = frameH; + drawSensorOrientation = sensorOrientation; + } + + private void drawOverlay(Canvas canvas) { + if (drawFrameWidth <= 0 || drawFrameHeight <= 0) return; + final boolean rotated = drawSensorOrientation % 180 == 90; + final float multiplier = + Math.min( + canvas.getHeight() / (float) (rotated ? drawFrameWidth : drawFrameHeight), + canvas.getWidth() / (float) (rotated ? drawFrameHeight : drawFrameWidth)); + Matrix matrix = + ImageUtils.getTransformationMatrix( + drawFrameWidth, + drawFrameHeight, + (int) (multiplier * (rotated ? drawFrameHeight : drawFrameWidth)), + (int) (multiplier * (rotated ? drawFrameWidth : drawFrameHeight)), + drawSensorOrientation, + new RectF(0, 0, 0, 0), + false); + + List snapshot; + synchronized (this) { + snapshot = new ArrayList<>(drawBoxes); + } + int idx = 0; + for (RectF box : snapshot) { + RectF rect = new RectF(box); + matrix.mapRect(rect); + float cornerSize = Math.min(rect.width(), rect.height()) / 8.0f; + canvas.drawRoundRect(rect, cornerSize, cornerSize, personBoxPaint); + canvas.drawText("person " + idx, rect.left, Math.max(0, rect.top - 8), boxTextPaint); + idx++; + } + } + + private void updateDebugInfo(int persons, float fps) { + if (binding == null) return; + int frames = currentSession != null ? currentSession.frameRows : 0; + int detections = currentSession != null ? currentSession.detectionRows : 0; + int crops = currentSession != null ? currentSession.cropCount : 0; + int events = currentSession != null ? currentSession.eventRows : 0; + String info = + String.format( + Locale.US, + "persons=%d\nframes=%d\ndet=%d\ncrops=%d\nevents=%d\nfps=%.1f", + persons, + frames, + detections, + crops, + events, + fps); + requireActivity().runOnUiThread(() -> binding.debugInfo.setText(info)); + } + + protected Model getModel() { + return model; + } + + @Override + protected void setModel(Model model) { + if (this.model != model) { + this.model = model; + preferencesManager.setObjectNavModel(model.name); + onInferenceConfigurationChanged(); + } + } + + protected Network.Device getDevice() { + return device; + } + + protected int getNumThreads() { + return numThreads; + } +} diff --git a/android/robot/src/main/java/org/openbot/sequencecollector/PersonSequenceSaver.java b/android/robot/src/main/java/org/openbot/sequencecollector/PersonSequenceSaver.java new file mode 100644 index 000000000..a27d9066d --- /dev/null +++ b/android/robot/src/main/java/org/openbot/sequencecollector/PersonSequenceSaver.java @@ -0,0 +1,260 @@ +package org.openbot.sequencecollector; + +import android.graphics.Bitmap; +import android.graphics.Matrix; +import android.graphics.RectF; +import java.io.File; +import java.io.FileOutputStream; +import java.io.FileWriter; +import java.io.IOException; +import java.util.ArrayList; +import java.util.List; +import java.util.Locale; +import java.util.concurrent.ExecutorService; +import java.util.concurrent.Executors; +import org.openbot.tflite.Detector.Recognition; +import timber.log.Timber; + +public class PersonSequenceSaver { + + private final ExecutorService executor = Executors.newSingleThreadExecutor(); + + public void saveFrameAsync( + Bitmap frame, + List persons, + PersonSequenceSession session, + PersonSequenceCaptureConfig config, + long frameNum, + int imageWidth, + int imageHeight, + int sensorOrientation, + boolean saveCropsForThisFrame, + String eventTag, + String note) { + final long timestampMs = System.currentTimeMillis(); + final long elapsedMs = timestampMs - session.startedAtMs; + final List snapshots = snapshotPersons(persons); + Bitmap.Config bitmapConfig = frame == null || frame.getConfig() == null ? Bitmap.Config.ARGB_8888 : frame.getConfig(); + final Bitmap frameCopy = + (frame != null && saveCropsForThisFrame && config.saveCrops) ? frame.copy(bitmapConfig, false) : null; + final String safeEventTag = eventTag == null ? "" : eventTag; + final String safeNote = note == null ? "" : note; + + executor.execute( + () -> { + appendFrameLog( + session, + frameNum, + timestampMs, + elapsedMs, + imageWidth, + imageHeight, + snapshots.size(), + "", + "", + safeEventTag, + safeNote); + + for (int i = 0; i < snapshots.size(); i++) { + RecognitionSnapshot snapshot = snapshots.get(i); + String cropPath = ""; + if (frameCopy != null) { + cropPath = + saveCrop( + frameCopy, + snapshot, + session, + config, + frameNum, + i, + sensorOrientation); + } + appendDetection(session, frameNum, timestampMs, i, snapshot, imageWidth, imageHeight, cropPath); + } + + if (frameCopy != null) { + frameCopy.recycle(); + } + }); + } + + public void saveEventAsync(PersonSequenceSession session, long frameNum, String eventType, String note) { + final long timestampMs = System.currentTimeMillis(); + final String safeType = eventType == null ? "" : eventType; + final String safeNote = note == null ? "" : note; + executor.execute(() -> appendEvent(session, timestampMs, frameNum, safeType, safeNote)); + } + + private List snapshotPersons(List persons) { + List snapshots = new ArrayList<>(); + if (persons == null) return snapshots; + for (Recognition person : persons) { + if (person == null || person.getLocation() == null) continue; + float confidence = person.getConfidence() == null ? 0f : person.getConfidence(); + snapshots.add(new RecognitionSnapshot(new RectF(person.getLocation()), confidence)); + } + return snapshots; + } + + private void appendFrameLog( + PersonSequenceSession session, + long frameNum, + long timestampMs, + long elapsedMs, + int imageWidth, + int imageHeight, + int numPersons, + String rawFramePath, + String overlayPath, + String eventTag, + String note) { + String row = + String.format( + Locale.US, + "%s,%d,%d,%d,%d,%d,%d,%s,%s,%s,%s\n", + csv(session.sessionId), + frameNum, + timestampMs, + elapsedMs, + imageWidth, + imageHeight, + numPersons, + csv(rawFramePath), + csv(overlayPath), + csv(eventTag), + csv(note)); + append(session.frameLogCsv, row, "frame_log.csv"); + session.frameRows++; + } + + private void appendDetection( + PersonSequenceSession session, + long frameNum, + long timestampMs, + int detId, + RecognitionSnapshot snapshot, + int imageWidth, + int imageHeight, + String cropPath) { + RectF bbox = snapshot.bbox; + float areaRatio = imageWidth > 0 && imageHeight > 0 ? bbox.width() * bbox.height() / (imageWidth * imageHeight) : 0f; + boolean edgeTouch = + bbox.left <= 0 || bbox.top <= 0 || bbox.right >= imageWidth || bbox.bottom >= imageHeight; + String row = + String.format( + Locale.US, + "%s,%d,%d,%d,%.4f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.6f,%.1f,%.1f,%s,%s\n", + csv(session.sessionId), + frameNum, + detId, + timestampMs, + snapshot.confidence, + bbox.left, + bbox.top, + bbox.right, + bbox.bottom, + bbox.width(), + bbox.height(), + areaRatio, + bbox.centerX(), + bbox.centerY(), + edgeTouch ? "1" : "0", + csv(cropPath)); + append(session.detectionsCsv, row, "detections.csv"); + session.detectionRows++; + } + + private void appendEvent( + PersonSequenceSession session, long timestampMs, long frameNum, String eventType, String note) { + String row = + String.format( + Locale.US, + "%s,%d,%d,%s,%s\n", + csv(session.sessionId), timestampMs, frameNum, csv(eventType), csv(note)); + append(session.eventsCsv, row, "events.csv"); + session.eventRows++; + } + + private String saveCrop( + Bitmap frame, + RecognitionSnapshot snapshot, + PersonSequenceSession session, + PersonSequenceCaptureConfig config, + long frameNum, + int detId, + int sensorOrientation) { + RectF bbox = snapshot.bbox; + float padX = bbox.width() * config.paddingRatio; + float padY = bbox.height() * config.paddingRatio; + int left = clamp((int) (bbox.left - padX), 0, frame.getWidth() - 1); + int top = clamp((int) (bbox.top - padY), 0, frame.getHeight() - 1); + int right = clamp((int) (bbox.right + padX), left + 1, frame.getWidth()); + int bottom = clamp((int) (bbox.bottom + padY), top + 1, frame.getHeight()); + int w = right - left; + int h = bottom - top; + if (w <= 0 || h <= 0) return ""; + + Bitmap rawCrop; + try { + rawCrop = Bitmap.createBitmap(frame, left, top, w, h); + } catch (Exception e) { + return ""; + } + + Bitmap uprightCrop; + if (sensorOrientation % 360 != 0) { + Matrix matrix = new Matrix(); + matrix.postRotate(sensorOrientation); + uprightCrop = Bitmap.createBitmap(rawCrop, 0, 0, rawCrop.getWidth(), rawCrop.getHeight(), matrix, true); + rawCrop.recycle(); + } else { + uprightCrop = rawCrop; + } + + String filename = + String.format(Locale.US, "%06d_person%d_conf%.2f.jpg", frameNum, detId, snapshot.confidence); + File cropFile = new File(session.cropsDir, filename); + try (FileOutputStream fos = new FileOutputStream(cropFile)) { + uprightCrop.compress(Bitmap.CompressFormat.JPEG, config.jpegQuality, fos); + session.cropCount++; + return "crops/" + filename; + } catch (IOException e) { + Timber.e(e, "Failed to save sequence crop"); + return ""; + } finally { + uprightCrop.recycle(); + } + } + + private void append(File file, String row, String label) { + try (FileWriter writer = new FileWriter(file, true)) { + writer.append(row); + } catch (IOException e) { + Timber.e(e, "Failed to append %s", label); + } + } + + public void shutdown() { + executor.shutdown(); + } + + private static int clamp(int v, int lo, int hi) { + return v < lo ? lo : (v > hi ? hi : v); + } + + private static String csv(String value) { + if (value == null) return ""; + String escaped = value.replace("\"", "\"\""); + return "\"" + escaped + "\""; + } + + private static class RecognitionSnapshot { + final RectF bbox; + final float confidence; + + RecognitionSnapshot(RectF bbox, float confidence) { + this.bbox = bbox; + this.confidence = confidence; + } + } +} diff --git a/android/robot/src/main/java/org/openbot/sequencecollector/PersonSequenceSession.java b/android/robot/src/main/java/org/openbot/sequencecollector/PersonSequenceSession.java new file mode 100644 index 000000000..3261855db --- /dev/null +++ b/android/robot/src/main/java/org/openbot/sequencecollector/PersonSequenceSession.java @@ -0,0 +1,107 @@ +package org.openbot.sequencecollector; + +import android.content.Context; +import android.os.Environment; +import android.util.Log; +import java.io.File; +import java.io.FileWriter; +import java.io.IOException; +import java.text.SimpleDateFormat; +import java.util.Date; +import java.util.Locale; +import org.json.JSONException; +import org.json.JSONObject; + +public class PersonSequenceSession { + + private static final String TAG = "PersonSequenceSession"; + + private static final String FRAME_LOG_HEADER = + "session_id,frame_id,timestamp_ms,elapsed_ms,image_width,image_height,num_persons," + + "raw_frame_path,overlay_path,event_tag,note"; + private static final String DETECTIONS_HEADER = + "session_id,frame_id,det_id,timestamp_ms,confidence,bbox_left,bbox_top,bbox_right," + + "bbox_bottom,bbox_width,bbox_height,bbox_area_ratio,center_x,center_y,edge_touch," + + "crop_path"; + private static final String EVENTS_HEADER = "session_id,timestamp_ms,frame_id,event_type,note"; + + public final String sessionId; + public final String personId; + public final File sessionDir; + public final File cropsDir; + public final File overlaysDir; + public final File frameLogCsv; + public final File detectionsCsv; + public final File eventsCsv; + public final long startedAtMs; + + public int frameRows = 0; + public int detectionRows = 0; + public int cropCount = 0; + public int eventRows = 0; + + public PersonSequenceSession(Context context, String personId) { + this.personId = personId; + this.startedAtMs = System.currentTimeMillis(); + String timestamp = new SimpleDateFormat("yyyyMMdd_HHmmss", Locale.US).format(new Date()); + this.sessionId = personId + "_seq_" + timestamp; + + File baseDir = + new File( + context.getExternalFilesDir(Environment.DIRECTORY_PICTURES), + "cartfollow_sequences"); + this.sessionDir = new File(baseDir, sessionId); + this.cropsDir = new File(sessionDir, "crops"); + this.overlaysDir = new File(sessionDir, "overlays"); + this.cropsDir.mkdirs(); + this.overlaysDir.mkdirs(); + this.frameLogCsv = new File(sessionDir, "frame_log.csv"); + this.detectionsCsv = new File(sessionDir, "detections.csv"); + this.eventsCsv = new File(sessionDir, "events.csv"); + } + + public void initCsvFiles() { + writeHeader(frameLogCsv, FRAME_LOG_HEADER); + writeHeader(detectionsCsv, DETECTIONS_HEADER); + writeHeader(eventsCsv, EVENTS_HEADER); + } + + private void writeHeader(File file, String header) { + try (FileWriter writer = new FileWriter(file, false)) { + writer.append(header).append('\n'); + } catch (IOException e) { + Log.e(TAG, "Failed to init csv: " + file.getName(), e); + } + } + + public void writeSessionInfo(PersonSequenceCaptureConfig config, String detectorModelName) { + JSONObject json = new JSONObject(); + try { + json.put("session_id", sessionId); + json.put("person_id", personId); + json.put("created_at", new SimpleDateFormat("yyyy-MM-dd HH:mm:ss", Locale.US).format(new Date())); + json.put("collector", "PersonSequenceCollector"); + json.put("mode", "SEQUENCE"); + json.put("frame_log_interval_ms", config.frameLogIntervalMs); + json.put("crop_interval_ms", config.cropIntervalMs); + json.put("overlay_interval_ms", config.overlayIntervalMs); + json.put("min_confidence", config.minConfidence); + json.put("save_crops", config.saveCrops); + json.put("save_overlays", config.saveOverlays); + json.put("bbox_padding_ratio", config.paddingRatio); + json.put("jpeg_quality", config.jpegQuality); + json.put("detector", detectorModelName == null ? "" : detectorModelName); + json.put("app_mode", "PersonSequenceCollector"); + } catch (JSONException e) { + Log.e(TAG, "Failed to build session_info.json", e); + return; + } + + File infoFile = new File(sessionDir, "session_info.json"); + try (FileWriter writer = new FileWriter(infoFile, false)) { + writer.write(json.toString(2)); + } catch (IOException | JSONException e) { + Log.e(TAG, "Failed to write session_info.json", e); + } + } +} diff --git a/android/robot/src/main/java/org/openbot/vehicle/BluetoothManager.java b/android/robot/src/main/java/org/openbot/vehicle/BluetoothManager.java index 7d709178f..71a96468c 100644 --- a/android/robot/src/main/java/org/openbot/vehicle/BluetoothManager.java +++ b/android/robot/src/main/java/org/openbot/vehicle/BluetoothManager.java @@ -25,6 +25,15 @@ import org.openbot.utils.Constants; public class BluetoothManager { + public interface ConnectionListener { + void onBleSerialReady(); + + void onBleDisconnected(); + } + + private static final String SERVICE_UUID = "61653dc3-4021-4d1e-ba83-8b4eec61d613"; + private static final String RX_UUID = "06386c14-86ea-4d71-811c-48f97c58f8c9"; + private static final String TX_UUID = "9bf1103b-834c-47cf-b149-c9e4bcf778a7"; private BleManager manager; private CharacteristicInfo notifyCharacteristic; private CharacteristicInfo writeCharacteristic; @@ -38,11 +47,13 @@ public class BluetoothManager { private int indexValue; public String readValue; private final LocalBroadcastManager localBroadcastManager; - private String serviceUUID = "61653dc3-4021-4d1e-ba83-8b4eec61d613"; - UUID[] uuidArray = new UUID[] {UUID.fromString(serviceUUID)}; + private final ConnectionListener connectionListener; + private boolean notifyEnabled; + UUID[] uuidArray = new UUID[] {UUID.fromString(SERVICE_UUID)}; - public BluetoothManager(Context context) { + public BluetoothManager(Context context, ConnectionListener connectionListener) { this.context = context; + this.connectionListener = connectionListener; initBleManager(); localBroadcastManager = LocalBroadcastManager.getInstance(this.context); } @@ -81,7 +92,7 @@ public void onLeScan(BleDevice device, int rssi, byte[] scanRecord) { } } deviceList.add(device); - adapter.notifyDataSetChanged(); + notifyAdapter(); } @Override @@ -97,7 +108,7 @@ public void onStart(boolean startScanSuccess, String info) { @Override public void onFinish() { - adapter.notifyDataSetChanged(); + notifyAdapter(); } }); } @@ -126,7 +137,7 @@ public void onStart(boolean startConnectSuccess, String info, BleDevice device) bleDevice = device; deviceList.remove(indexValue); deviceList.add(indexValue, device); - adapter.notifyDataSetChanged(); + notifyAdapter(); } @Override @@ -134,7 +145,7 @@ public void onConnected(BleDevice device) { bleDevice = device; deviceList.remove(indexValue); deviceList.add(indexValue, device); - adapter.notifyDataSetChanged(); + notifyAdapter(); addDeviceInfoDataAndUpdate(); Logger.i("Successfully connected: " + " " + device); } @@ -142,7 +153,9 @@ public void onConnected(BleDevice device) { @Override public void onDisconnected(String info, int status, BleDevice device) { bleDevice = null; - adapter.notifyDataSetChanged(); + clearSerialCharacteristics(); + if (connectionListener != null) connectionListener.onBleDisconnected(); + notifyAdapter(); Logger.i("disconnected!"); } @@ -153,7 +166,7 @@ public void onFailure(int failCode, String info, BleDevice device) { deviceList.remove(indexValue); deviceList.add(indexValue, device); Toast.makeText(context, "Connection fail: " + info, Toast.LENGTH_LONG).show(); - adapter.notifyDataSetChanged(); + notifyAdapter(); } }; @@ -165,15 +178,16 @@ public void addDeviceInfoDataAndUpdate() { return; } for (Map.Entry> e : deviceInfo.entrySet()) { + if (!SERVICE_UUID.equalsIgnoreCase(e.getKey().uuid)) continue; for (CharacteristicInfo characteristicInfo : e.getValue()) { - if (characteristicInfo.notify) { + if (TX_UUID.equalsIgnoreCase(characteristicInfo.uuid) && characteristicInfo.notify) { notifyCharacteristic = characteristicInfo; notifyServiceInfo = e.getKey(); if (isBleConnected()) // Set the MTU size to 64 bytes BleManager.getInstance().setMtu(bleDevice, 64, mtuCallback); } - if (characteristicInfo.writable) { + if (RX_UUID.equalsIgnoreCase(characteristicInfo.uuid) && characteristicInfo.writable) { writeServiceInfo = e.getKey(); writeCharacteristic = characteristicInfo; } @@ -182,7 +196,7 @@ public void addDeviceInfoDataAndUpdate() { } public void write(String msg) { - if (isBleConnected()) { + if (isSerialReady()) { BleManager.getInstance() .write( bleDevice, @@ -233,6 +247,10 @@ public void onNotifySuccess(String notifySuccessUuid, BleDevice device) { if (!notifySuccessUuids.contains(notifySuccessUuid)) { notifySuccessUuids.add(notifySuccessUuid); } + if (TX_UUID.equalsIgnoreCase(notifySuccessUuid) && connectionListener != null) { + notifyEnabled = true; + connectionListener.onBleSerialReady(); + } } @Override @@ -245,12 +263,34 @@ public boolean isBleConnected() { return bleDevice != null && bleDevice.connected; } + public boolean isSerialReady() { + return isBleConnected() + && writeServiceInfo != null + && writeCharacteristic != null + && notifyServiceInfo != null + && notifyCharacteristic != null + && notifyEnabled; + } + + private void clearSerialCharacteristics() { + writeServiceInfo = null; + writeCharacteristic = null; + notifyServiceInfo = null; + notifyCharacteristic = null; + notifySuccessUuids.clear(); + notifyEnabled = false; + } + + private void notifyAdapter() { + if (adapter != null) adapter.notifyDataSetChanged(); + } + private void onSerialDataReceived(String data) { // Add whatever you want here Logger.i("Serial data received from BLE: " + data); localBroadcastManager.sendBroadcast( new Intent(Constants.DEVICE_ACTION_DATA_RECEIVED) - .putExtra("from", "usb") + .putExtra("from", "ble") .putExtra("data", data)); } } diff --git a/android/robot/src/main/java/org/openbot/vehicle/Vehicle.java b/android/robot/src/main/java/org/openbot/vehicle/Vehicle.java index 5c33ef2c2..467fb081c 100644 --- a/android/robot/src/main/java/org/openbot/vehicle/Vehicle.java +++ b/android/robot/src/main/java/org/openbot/vehicle/Vehicle.java @@ -49,6 +49,9 @@ public class Vehicle { private boolean hasLedsBack = false; private boolean hasLedsStatus = false; private boolean isReady = false; + private boolean bleSerialReady = false; + private boolean cartFirmwareReady = false; + private long emergencySequence = 0L; private BluetoothManager bluetoothManager; SharedPreferences sharedPreferences; public String connectionType; @@ -171,6 +174,7 @@ public void requestVehicleConfig() { public void processVehicleConfig(String message) { setVehicleType(message.split(":")[0]); + cartFirmwareReady = "CART_AT8236".equals(getVehicleType()); if (message.contains(":v:")) { setHasVoltageDivider(true); @@ -454,6 +458,7 @@ public void run() { } public void startHeartbeat() { + if (heartbeatTimer != null) return; heartbeatTimer = new Timer(); HeartBeatTask heartBeatTask = new HeartBeatTask(); heartbeatTimer.schedule(heartBeatTask, 250, 250); // 250ms delay and 250ms intervals @@ -508,7 +513,29 @@ public void toggleConnection(int position, BleDevice device) { } public void initBle() { - bluetoothManager = new BluetoothManager(context); + if (bluetoothManager != null) return; + bluetoothManager = + new BluetoothManager( + context, + new BluetoothManager.ConnectionListener() { + @Override + public void onBleSerialReady() { + bleSerialReady = true; + setReady(false); + cartFirmwareReady = false; + startHeartbeat(); + requestVehicleConfig(); + } + + @Override + public void onBleDisconnected() { + control = new Control(0, 0); + bleSerialReady = false; + cartFirmwareReady = false; + setReady(false); + stopHeartbeat(); + } + }); } private void sendStringToBle(String message) { @@ -519,6 +546,25 @@ public boolean bleConnected() { return bluetoothManager.isBleConnected(); } + public boolean isBleSerialReady() { + return bleSerialReady && bluetoothManager != null && bluetoothManager.isSerialReady(); + } + + public boolean isCartFirmwareReady() { + return isBleSerialReady() && cartFirmwareReady && isReady(); + } + + public void useBluetoothConnection() { + connectionType = "Bluetooth"; + setConnectionPreferences("connection_type", connectionType); + } + + public synchronized void emergencyStop() { + stopBot(); + emergencySequence++; + sendStringToDevice(String.format(Locale.US, "!S,%d\n", emergencySequence)); + } + private void setConnectionPreferences(String name, String value) { SharedPreferences.Editor editor = sharedPreferences.edit(); editor.putString(name, value); diff --git a/android/robot/src/main/res/layout/fragment_human_cart_simulator.xml b/android/robot/src/main/res/layout/fragment_human_cart_simulator.xml new file mode 100644 index 000000000..d41467e58 --- /dev/null +++ b/android/robot/src/main/res/layout/fragment_human_cart_simulator.xml @@ -0,0 +1,383 @@ + + + + + + + + + + + + + +