From 71ee1fcc29d297feea4cfe7628df03ed0e49da12 Mon Sep 17 00:00:00 2001 From: Yuan SY Date: Sun, 12 Jul 2026 17:03:05 +0800 Subject: [PATCH] Add BLE real cart follow module --- android/cartfollow-devlog.md | 63 + .../java/org/openbot/OpenBotApplication.java | 1 - .../cartfollow/BaseCartFollowFragment.java | 1136 +++++++++++++++++ .../HumanCartSimulatorFragment.java | 1109 +--------------- .../cartfollow/RealCartFollowFragment.java | 250 ++++ .../cartfollow/RealCartSafetyController.java | 146 +++ .../java/org/openbot/common/FeatureList.java | 2 + .../java/org/openbot/main/MainFragment.java | 5 + .../org/openbot/vehicle/BluetoothManager.java | 66 +- .../java/org/openbot/vehicle/Vehicle.java | 48 +- .../layout/fragment_human_cart_simulator.xml | 122 +- .../src/main/res/navigation/nav_graph.xml | 9 + .../RealCartSafetyControllerTest.java | 105 ++ 13 files changed, 1939 insertions(+), 1123 deletions(-) create mode 100644 android/robot/src/main/java/org/openbot/cartfollow/BaseCartFollowFragment.java create mode 100644 android/robot/src/main/java/org/openbot/cartfollow/RealCartFollowFragment.java create mode 100644 android/robot/src/main/java/org/openbot/cartfollow/RealCartSafetyController.java create mode 100644 android/robot/src/test/java/org/openbot/cartfollow/RealCartSafetyControllerTest.java diff --git a/android/cartfollow-devlog.md b/android/cartfollow-devlog.md index 03578a026..352e635c0 100644 --- a/android/cartfollow-devlog.md +++ b/android/cartfollow-devlog.md @@ -1019,3 +1019,66 @@ $env:Path="$env:JAVA_HOME\bin;$env:Path" 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/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/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/HumanCartSimulatorFragment.java b/android/robot/src/main/java/org/openbot/cartfollow/HumanCartSimulatorFragment.java index 58c509a4b..1b7dd16e5 100644 --- a/android/robot/src/main/java/org/openbot/cartfollow/HumanCartSimulatorFragment.java +++ b/android/robot/src/main/java/org/openbot/cartfollow/HumanCartSimulatorFragment.java @@ -1,1109 +1,4 @@ package org.openbot.cartfollow; -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.app.Activity; -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 HumanCartSimulatorFragment 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; - - private 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(); - private 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(); - } - }); - } - - 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() { - stopDiagnosticSession(); - handlerThread.quitSafely(); - try { - handlerThread.join(); - handlerThread = null; - handler = null; - } catch (final InterruptedException e) { - e.printStackTrace(); - } - super.onPause(); - } - - @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 || !binding.startSwitch.isChecked()) 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); - binding.trackingOverlay.postInvalidate(); - } - computingNetwork = false; - }); - } - - private void updateCropImageInfo() { - sensorOrientation = 90 - ImageUtils.getScreenOrientation(requireActivity()); - recreateNetwork(getModel(), getDevice(), getNumThreads()); - } - - 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 = - new SystemSafetyEvidence( - false, true, detector != null, detector == null ? "detector_not_ready" : "ok"); - 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; - } - } -} +/** 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/RealCartFollowFragment.java b/android/robot/src/main/java/org/openbot/cartfollow/RealCartFollowFragment.java new file mode 100644 index 000000000..539c64481 --- /dev/null +++ b/android/robot/src/main/java/org/openbot/cartfollow/RealCartFollowFragment.java @@ -0,0 +1,250 @@ +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 Handler mainHandler = new Handler(Looper.getMainLooper()); + private volatile RealCartSafetyController.Output latestOutput = + RealCartSafetyController.stop("idle"); + private boolean schedulerRunning; + private long lastHandshakeRequestMs; + + 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, + RealCartSafetyController.MANUAL_FORWARD, + RealCartSafetyController.MANUAL_FORWARD); + installDeadMan( + binding.driveBackward, + -RealCartSafetyController.MANUAL_REVERSE, + -RealCartSafetyController.MANUAL_REVERSE); + installDeadMan( + binding.driveLeft, + -RealCartSafetyController.MANUAL_TURN, + RealCartSafetyController.MANUAL_TURN); + installDeadMan( + binding.driveRight, + 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(); + latestOutput = RealCartSafetyController.stop("emergency_stop"); + 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); + latestOutput = RealCartSafetyController.stop("paused"); + sendOutput(latestOutput); + vehicle.stopBot(); + 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) { + latestOutput = RealCartSafetyController.stop("mode_change"); + sendOutput(latestOutput); + 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, int left, int right) { + button.setOnTouchListener( + (view, event) -> { + switch (event.getActionMasked()) { + case MotionEvent.ACTION_DOWN: + latestOutput = safetyController.manual(left, right); + view.setPressed(true); + return true; + case MotionEvent.ACTION_UP: + case MotionEvent.ACTION_CANCEL: + case MotionEvent.ACTION_OUTSIDE: + latestOutput = RealCartSafetyController.stop("manual_release"); + sendOutput(latestOutput); + view.setPressed(false); + 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 && latestOutput != null && !latestOutput.isStop()) { + latestOutput = RealCartSafetyController.stop("ble_not_ready"); + } + } + + 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/common/FeatureList.java b/android/robot/src/main/java/org/openbot/common/FeatureList.java index 39639f852..bd7d9d76e 100644 --- a/android/robot/src/main/java/org/openbot/common/FeatureList.java +++ b/android/robot/src/main/java/org/openbot/common/FeatureList.java @@ -37,6 +37,7 @@ public class FeatureList { 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"; @@ -86,6 +87,7 @@ public static ArrayList getCategories() { 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( 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 628e9a1f7..e6974b400 100644 --- a/android/robot/src/main/java/org/openbot/main/MainFragment.java +++ b/android/robot/src/main/java/org/openbot/main/MainFragment.java @@ -92,6 +92,11 @@ public void onItemClick(SubCategory subCategory) { .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); 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 index ed3d244a5..d41467e58 100644 --- a/android/robot/src/main/res/layout/fragment_human_cart_simulator.xml +++ b/android/robot/src/main/res/layout/fragment_human_cart_simulator.xml @@ -108,7 +108,7 @@ android:orientation="horizontal" android:padding="12dp" android:visibility="gone" - app:layout_constraintBottom_toTopOf="@+id/bottom_panel" + app:layout_constraintBottom_toTopOf="@+id/real_control_panel" app:layout_constraintEnd_toEndOf="parent" app:layout_constraintStart_toStartOf="parent"> @@ -163,6 +163,126 @@ + + + + + + + + + + + + + +