Feature/person crop collector

android/robot/src/main/java/org/openbot/cartfollow/ActionArbitrator.java

@@ -0,0 +1,100 @@
+package org.openbot.cartfollow;
+
+import android.graphics.RectF;
+
+public class ActionArbitrator {
+
+  public BehaviorDecisionResult decide(
+      FollowState state,
+      IdentityEvidence identity,
+      DistanceEvidence distance,
+      TraversabilityEvidence traversability,
+      SystemSafetyEvidence safety,
+      TargetMemory memory,
+      int frameW) {
+    if (safety != null && safety.emergencyStop) {
+      return result(state, BehaviorAction.EMERGENCY_STOP, "emergency_stop", safety.reason, 0f);
+    }
+    if (safety != null && (!safety.communicationOk || !safety.detectorOk)) {
+      return result(state, BehaviorAction.EMERGENCY_STOP, "system_not_ready", safety.reason, 0f);
+    }
+    if (state == FollowState.STOP) {
+      return result(state, BehaviorAction.HARD_STOP, "state_stop", "hard_stop_state", 0f);
+    }
+    if (state == FollowState.REACQUIRE_TARGET) {
+      float conf = identity == null ? 0f : identity.confidence;
+      return result(state, BehaviorAction.REACQUIRE_HOLD, "reacquire_confirming", null, conf);
+    }
+    if (state == FollowState.IDENTITY_UNCERTAIN) {
+      float conf = identity == null ? 0f : identity.confidence;
+      return result(state, BehaviorAction.MOTION_STOP, "identity_uncertain", "motion_stop", conf);
+    }
+    if (state == FollowState.LOCKED_PENDING_CONFIRM
+        || state == FollowState.CONFIRMED_ARMED
+        || state == FollowState.READY_TO_FOLLOW
+        || state == FollowState.CAPTURE_TARGET
+        || state == FollowState.IDLE) {
+      return result(state, BehaviorAction.MOTION_STOP, "not_ready_to_follow", "motion_stop", 0f);
+    }
+    if (state == FollowState.LOST || state == FollowState.SEARCH) {
+      BehaviorAction searchAction = searchAction(memory, frameW);
+      if (searchAction == BehaviorAction.MOTION_STOP) {
+        return result(state, searchAction, "target_lost_no_last_bbox", "motion_stop", 0f);
+      }
+      return result(state, searchAction, "target_lost_last_side", null, 0.2f);
+    }
+    if (identity != null && !identity.matched) {
+      return result(state, BehaviorAction.MOTION_STOP, "identity_unmatched", identity.reason, 0f);
+    }
+    if (traversability != null && traversability.centerBlocked) {
+      return result(state, BehaviorAction.BLOCKED_WAIT, "center_blocked", traversability.reason, 0f);
+    }
+    if (distance != null
+        && (distance.state == DistanceState.UNKNOWN || distance.state == DistanceState.TOO_CLOSE)) {
+      return result(
+          state,
+          BehaviorAction.MOTION_STOP,
+          "distance_" + distance.state.name().toLowerCase(),
+          distance.reason,
+          distance.confidence);
+    }
+    if ((state == FollowState.FOLLOW || state == FollowState.FOLLOW_CAUTION)
+        && distance != null
+        && distance.state == DistanceState.OK) {
+      return result(
+          state,
+          BehaviorAction.FOLLOW_CAUTION,
+          state == FollowState.FOLLOW_CAUTION ? "follow_caution_distance_ok" : "identity_ok_distance_ok",
+          null,
+          identityConfidence(identity));
+    }
+    return result(
+        state,
+        BehaviorAction.FOLLOW_SLOW,
+        "identity_ok_follow_allowed",
+        null,
+        identityConfidence(identity));
+  }
+
+  private static BehaviorDecisionResult result(
+      FollowState state,
+      BehaviorAction action,
+      String actionReason,
+      String safetyBlockReason,
+      float confidence) {
+    return new BehaviorDecisionResult(state, action, actionReason, safetyBlockReason, confidence);
+  }
+
+  private static float identityConfidence(IdentityEvidence identity) {
+    return identity == null ? 0f : identity.confidence;
+  }
+
+  private static BehaviorAction searchAction(TargetMemory memory, int frameW) {
+    if (memory == null || frameW <= 0) return BehaviorAction.MOTION_STOP;
+    RectF lastBbox = memory.getLastBbox();
+    if (lastBbox == null) return BehaviorAction.MOTION_STOP;
+    return lastBbox.centerX() < frameW / 2f
+        ? BehaviorAction.LOCAL_SEARCH_LEFT
+        : BehaviorAction.LOCAL_SEARCH_RIGHT;
+  }
+}

android/robot/src/main/java/org/openbot/cartfollow/BboxContinuityEvidence.java

@@ -0,0 +1,79 @@
+package org.openbot.cartfollow;
+
+import android.graphics.RectF;
+
+public class BboxContinuityEvidence {
+  public static final float DEFAULT_CENTER_MAX = 0.25f;
+  public static final float DEFAULT_X_MAX = 0.25f;
+  public static final float DEFAULT_AREA_MIN = 0.50f;
+  public static final float DEFAULT_AREA_MAX = 2.00f;
+  public static final float STRICT_CENTER_MAX = 0.18f;
+  public static final float STRICT_X_MAX = 0.18f;
+  public static final float STRICT_AREA_MIN = 0.60f;
+  public static final float STRICT_AREA_MAX = 1.67f;
+
+  public final float centerJumpRatio;
+  public final float xJumpRatio;
+  public final float areaRatio;
+  public final float predictionError;
+  public final boolean bboxDefaultOk;
+  public final boolean bboxStrictOk;
+  public final boolean predictionOk;
+  public final String reason;
+
+  public BboxContinuityEvidence(
+      float centerJumpRatio,
+      float xJumpRatio,
+      float areaRatio,
+      float predictionError,
+      String reason) {
+    this.centerJumpRatio = centerJumpRatio;
+    this.xJumpRatio = xJumpRatio;
+    this.areaRatio = areaRatio;
+    this.predictionError = predictionError;
+    this.bboxDefaultOk =
+        centerJumpRatio <= DEFAULT_CENTER_MAX
+            && xJumpRatio <= DEFAULT_X_MAX
+            && areaRatio >= DEFAULT_AREA_MIN
+            && areaRatio <= DEFAULT_AREA_MAX;
+    this.bboxStrictOk =
+        centerJumpRatio <= STRICT_CENTER_MAX
+            && xJumpRatio <= STRICT_X_MAX
+            && areaRatio >= STRICT_AREA_MIN
+            && areaRatio <= STRICT_AREA_MAX;
+    this.predictionOk = predictionError >= 0f && predictionError <= STRICT_CENTER_MAX;
+    this.reason = reason;
+  }
+
+  public static BboxContinuityEvidence unavailable(String reason) {
+    return new BboxContinuityEvidence(1f, 1f, 0f, -1f, reason);
+  }
+
+  public static BboxContinuityEvidence from(
+      RectF candidate, RectF last, RectF previous, int frameW, int frameH) {
+    if (candidate == null || last == null || frameW <= 0 || frameH <= 0) {
+      return unavailable("bbox_reference_not_available");
+    }
+    float diag = (float) Math.hypot(frameW, frameH);
+    float centerJump =
+        (float)
+            (Math.hypot(candidate.centerX() - last.centerX(), candidate.centerY() - last.centerY())
+                / Math.max(1f, diag));
+    float xJump = Math.abs(candidate.centerX() - last.centerX()) / Math.max(1f, frameW);
+    float areaRatio = area(candidate) / Math.max(1f, area(last));
+    float predictionError = -1f;
+    if (previous != null) {
+      float predX = last.centerX() + (last.centerX() - previous.centerX());
+      float predY = last.centerY() + (last.centerY() - previous.centerY());
+      predictionError =
+          (float)
+              (Math.hypot(candidate.centerX() - predX, candidate.centerY() - predY)
+                  / Math.max(1f, diag));
+    }
+    return new BboxContinuityEvidence(centerJump, xJump, areaRatio, predictionError, "ok");
+  }
+
+  private static float area(RectF b) {
+    return Math.max(0f, b.width()) * Math.max(0f, b.height());
+  }
+}

android/robot/src/main/java/org/openbot/cartfollow/BehaviorAction.java

@@ -0,0 +1,13 @@
+package org.openbot.cartfollow;
+
+public enum BehaviorAction {
+  FOLLOW_SLOW,
+  FOLLOW_CAUTION,
+  MOTION_STOP,
+  LOCAL_SEARCH_LEFT,
+  LOCAL_SEARCH_RIGHT,
+  BLOCKED_WAIT,
+  REACQUIRE_HOLD,
+  HARD_STOP,
+  EMERGENCY_STOP
+}

android/robot/src/main/java/org/openbot/cartfollow/BehaviorDecisionResult.java

@@ -0,0 +1,37 @@
+package org.openbot.cartfollow;
+
+public class BehaviorDecisionResult {
+  public final FollowState state;
+  public final BehaviorAction selectedAction;
+  public final String actionReason;
+  public final String safetyBlockReason;
+  public final float confidence;
+  public final DistanceEvidence distanceEvidence;
+  public final TraversabilityEvidence traversabilityEvidence;
+
+  public BehaviorDecisionResult(
+      FollowState state,
+      BehaviorAction selectedAction,
+      String actionReason,
+      String safetyBlockReason,
+      float confidence) {
+    this(state, selectedAction, actionReason, safetyBlockReason, confidence, null, null);
+  }
+
+  public BehaviorDecisionResult(
+      FollowState state,
+      BehaviorAction selectedAction,
+      String actionReason,
+      String safetyBlockReason,
+      float confidence,
+      DistanceEvidence distanceEvidence,
+      TraversabilityEvidence traversabilityEvidence) {
+    this.state = state;
+    this.selectedAction = selectedAction;
+    this.actionReason = actionReason;
+    this.safetyBlockReason = safetyBlockReason;
+    this.confidence = confidence;
+    this.distanceEvidence = distanceEvidence;
+    this.traversabilityEvidence = traversabilityEvidence;
+  }
+}

android/robot/src/main/java/org/openbot/cartfollow/DistanceEvidence.java

@@ -0,0 +1,13 @@
+package org.openbot.cartfollow;
+
+public class DistanceEvidence {
+  public final DistanceState state;
+  public final float confidence;
+  public final String reason;
+
+  public DistanceEvidence(DistanceState state, float confidence, String reason) {
+    this.state = state == null ? DistanceState.UNKNOWN : state;
+    this.confidence = confidence;
+    this.reason = reason;
+  }
+}

android/robot/src/main/java/org/openbot/cartfollow/FollowState.java

@@ -8,6 +8,8 @@ public enum FollowState {
   REACQUIRE_TARGET,
   READY_TO_FOLLOW,
   FOLLOW,
+  FOLLOW_CAUTION,
+  IDENTITY_UNCERTAIN,
   LOST,
   SEARCH,
   STOP