Merge pull request #311 from reger24/master

Adjust robot speed by a estimated distancefactor.

android/app/README.md

@@ -168,6 +168,16 @@ Simple UI for tracking objects of 80 different classes. A short description of t
 <img src="../../docs/images/screen_object_tracking_2.jpg" alt="Alt text" width="49%" />
 </p>
 
+#### Options
+- **Dynamic Speed**: reduces the robot speed in "Auto Mode" if it gets closer to the tracked object.
+  The speed is scaled based on the area of the bouding box (works best in landscape orientation).
+- **Model**: choose an object detector based on your phone performance (see below for [benchmarking results](#benchmark)).
+- **Object**: pick the object you want to track. The models can detect the 80 COCO [object classes](https://tech.amikelive.com/node-718/what-object-categories-labels-are-in-coco-dataset/).
+- **Confidence**: confidence threshold to determine if detections are accepted. Increase if you get false detections, decrease if the object of interest it not detected.
+- **Device**: use CPU, GPU or NNAPI for inference (more details [here](#device)).
+- **Threads**: number of threads to use (only makes a difference when CPU is selected as device).
+
+
 ### Point Goal Navigation
 
 Note that this fragment requires ARCore and camera permission. If your device does not support ARCore and you continue anyways, the app will crash. In this screen you can specify a goal via a 2D vector with respect to the current position and orientation of the robot. The 2D vector contains the distance to the front and left of the robot in meters. Both values can also be negative and correspond to back and right of the robot in that case. After specifying the goal and pressing `Start` the robot will exectue an AI policy that attempts to reach the goal while avoiding obstacles.

android/app/src/main/java/org/openbot/env/SharedPreferencesManager.java

@@ -26,7 +26,8 @@ public class SharedPreferencesManager {
 
   private static final String OBJECT_TYPE = "OBJECT_TYPE";
   private static final String DEFAULT_OBJECT_TYPE = "person";
-
+  // object tracker switch for speed adjusted by estimated object distance
+  private static final String OBJECT_NAV_DYNAMIC_SPEED = "OBJECT_NAV_DYNAMICSPEED";
   private static final int DEFAULT_DEVICE = Network.Device.CPU.ordinal();
   private static final String DEVICE = "DEVICE";
   private static final int DEFAULT_NUM_THREAD = 4;
@@ -137,6 +138,14 @@ public void setDriveMode(int mode) {
     preferences.edit().putInt(DRIVE_MODE, mode).apply();
   }
 
+  public void setDynamicSpeed(boolean isEnabled) {
+    preferences.edit().putBoolean(OBJECT_NAV_DYNAMIC_SPEED, isEnabled).apply();
+  }
+
+  public boolean getDynamicSpeed() {
+    return preferences.getBoolean(OBJECT_NAV_DYNAMIC_SPEED, false);
+  }
+
   public void setLogMode(int mode) {
     preferences.edit().putInt(LOG_MODE, mode).apply();
   }

android/app/src/main/java/org/openbot/main/SettingsFragment.java

@@ -34,7 +34,6 @@ public class SettingsFragment extends PreferenceFragmentCompat {
   private SwitchPreferenceCompat storage;
   private SwitchPreferenceCompat location;
   private SwitchPreferenceCompat mic;
-
   private final ActivityResultLauncher<String[]> requestPermissionLauncher =
       registerForActivityResult(
           new ActivityResultContracts.RequestMultiplePermissions(),

android/app/src/main/java/org/openbot/objectNav/ObjectNavFragment.java

@@ -206,6 +206,12 @@ public void onNothingSelected(AdapterView<?> parent) {}
                     Enums.SpeedMode.getByID(preferencesManager.getSpeedMode()))));
 
     binding.autoSwitch.setOnClickListener(v -> setNetworkEnabled(binding.autoSwitch.isChecked()));
+    binding.dynamicSpeed.setChecked(preferencesManager.getDynamicSpeed());
+    binding.dynamicSpeed.setOnClickListener(
+        v -> {
+          preferencesManager.setDynamicSpeed(binding.dynamicSpeed.isChecked());
+          tracker.setDynamicSpeed(preferencesManager.getDynamicSpeed());
+        });
   }
 
   private void updateCropImageInfo() {
@@ -223,6 +229,7 @@ private void updateCropImageInfo() {
     borderedText.setTypeface(Typeface.MONOSPACE);
 
     tracker = new MultiBoxTracker(requireContext());
+    tracker.setDynamicSpeed(preferencesManager.getDynamicSpeed());
 
     Timber.i("Camera orientation relative to screen canvas: %d", sensorOrientation);
 
@@ -378,6 +385,7 @@ private void setNetworkEnabledWithAudio(boolean b) {
 
   private void setNetworkEnabled(boolean b) {
     binding.autoSwitch.setChecked(b);
+
     binding.controllerContainer.controlMode.setEnabled(!b);
     binding.controllerContainer.driveMode.setEnabled(!b);
     binding.controllerContainer.speedInfo.setEnabled(!b);

android/app/src/main/java/org/openbot/original/CameraActivity.java

@@ -181,7 +181,7 @@ public abstract class CameraActivity extends AppCompatActivity
 
   private Intent intentSensorService;
   private ServerCommunication serverCommunication;
-  private SharedPreferencesManager preferencesManager;
+  protected SharedPreferencesManager preferencesManager;
   protected final GameController gameController = new GameController(driveMode);
   private PhoneController phoneController;
   protected final ControllerHandler controllerHandler = new ControllerHandler();
@@ -452,6 +452,10 @@ public void onReceive(Context context, Intent intent) {
     vehicle.requestVehicleConfig();
   }
 
+  /**
+   * Initalize bottom sheet views/fields with shared preference values (@see
+   * SharedPreferenceManager)
+   */
   @SuppressLint("SetTextI18n")
   private void setInitialValues() {
     cameraSwitchCompat.setChecked(preferencesManager.getCameraSwitch());

android/app/src/main/java/org/openbot/original/DefaultActivity.java

@@ -100,6 +100,7 @@ public void onPreviewSizeChosen(final Size size, final int rotation) {
     borderedText.setTypeface(Typeface.MONOSPACE);
 
     tracker = new MultiBoxTracker(this);
+    tracker.setDynamicSpeed(preferencesManager.getDynamicSpeed());
 
     previewWidth = size.getWidth();
     previewHeight = size.getHeight();

android/app/src/main/java/org/openbot/tracking/MultiBoxTracker.java

@@ -73,6 +73,7 @@ public class MultiBoxTracker {
   private int sensorOrientation;
   private float leftControl;
   private float rightControl;
+  private boolean useDynamicSpeed = false;
 
   public MultiBoxTracker(final Context context) {
     for (final int color : COLORS) {
@@ -143,21 +144,30 @@ private void updateFrameToCanvasMatrix(int canvasHeight, int canvasWidth) {
             false);
   }
 
+  /**
+   * Determine the robot controls/steering from the position of the tracked object/person on screen.
+   * The follow speed is adjusted based on the area of the bounding box of the tracked object.
+   * Assumption: large object box --> close to object --> slow down
+   *
+   * @return the adjusted speed control for left and right wheels in the range -1.0 ... 1.0
+   */
   public synchronized Control updateTarget() {
     if (!trackedObjects.isEmpty()) {
-      // Pick person with highest probability
+      // Pick detection with highest probability
       final RectF trackedPos = new RectF(trackedObjects.get(0).location);
       final boolean rotated = sensorOrientation % 180 == 90;
       float imgWidth = (float) (rotated ? frameHeight : frameWidth);
+      // calculate track box area for distance estimate
+      float boxArea = trackedPos.height() * trackedPos.width();
       float centerX = (rotated ? trackedPos.centerY() : trackedPos.centerX());
       // Make sure object center is in frame
       centerX = Math.max(0.0f, Math.min(centerX, imgWidth));
       // Scale relative position along x-axis between -1 and 1
       float x_pos_norm = 1.0f - 2.0f * centerX / imgWidth;
-      // Scale to control signal and account for rotation
+      // Scale for steering signal and account for rotation,
       float x_pos_scaled = rotated ? -x_pos_norm * 1.0f : x_pos_norm * 1.0f;
       //// Scale by "exponential" function: y = x / sqrt(1-x^2)
-      // Math.max (Math.min(x_pos_norm / Math.sqrt(1 - x_pos_norm * x_pos_norm),2),-2) * 255.0f;
+      // Math.max (Math.min(x_pos_norm / Math.sqrt(1 - x_pos_norm * x_pos_norm),2),-2);
 
       if (x_pos_scaled < 0) {
         leftControl = 1.0f;
@@ -166,10 +176,26 @@ public synchronized Control updateTarget() {
         leftControl = 1.0f - x_pos_scaled;
         rightControl = 1.0f;
       }
+
+      // adjust speed depending on size of detected object bounding box
+      if (useDynamicSpeed) {
+        float scaleFactor = 1.0f - boxArea / (frameWidth * frameHeight);
+        scaleFactor = scaleFactor > 0.75f ? 1.0f : scaleFactor; // tracked object far, full speed
+        // apply scale factor if tracked object is not too near, otherwise stop
+        if (scaleFactor > 0.25f) {
+          leftControl *= scaleFactor;
+          rightControl *= scaleFactor;
+        } else {
+          leftControl = 0.0f;
+          rightControl = 0.0f;
+        }
+      }
+
     } else {
       leftControl = 0.0f;
       rightControl = 0.0f;
     }
+
     return new Control(
         (0 > sensorOrientation) ? rightControl : leftControl,
         (0 > sensorOrientation) ? leftControl : rightControl);
@@ -262,6 +288,15 @@ private void processResults(final List<Recognition> results) {
     }
   }
 
+  /**
+   * Set use of dynamic speed on or off (used in updateTarget())
+   *
+   * @param isEnabled
+   */
+  public void setDynamicSpeed(boolean isEnabled) {
+    useDynamicSpeed = isEnabled;
+  }
+
   private static class TrackedRecognition {
     RectF location;
     float detectionConfidence;

android/app/src/main/res/layout-land/fragment_object_nav.xml

@@ -44,17 +44,57 @@
             app:layout_constraintStart_toStartOf="parent"
             app:layout_constraintTop_toTopOf="parent" />
 
-        <androidx.appcompat.widget.SwitchCompat
-            android:id="@+id/auto_switch"
+        <CheckBox
+            android:id="@+id/usbToggle"
+            android:layout_width="wrap_content"
+            android:layout_height="wrap_content"
+            android:layout_marginEnd="16dp"
+            android:button="@drawable/usb_toggle"
+            app:layout_constraintBottom_toBottomOf="@+id/camera_toggle"
+            app:layout_constraintEnd_toStartOf="@+id/camera_toggle"
+            app:layout_constraintTop_toTopOf="@+id/camera_toggle" />
+
+        <ImageView
+            android:id="@+id/camera_toggle"
+            android:layout_width="40dp"
+            android:layout_height="40dp"
+            android:layout_marginRight="16dp"
+            android:background="@android:color/transparent"
+            android:scaleType="center"
+            android:src="@drawable/ic_cameraswitch"
+            android:text="@string/camera_facing_back"
+            app:layout_constraintEnd_toEndOf="parent"
+            app:layout_constraintTop_toTopOf="@+id/autoLinearLayout"
+            app:tint="@color/openBotBlue" />
+
+        <LinearLayout
+            android:id="@+id/autoLinearLayout"
             android:layout_width="wrap_content"
             android:layout_height="wrap_content"
-            android:layout_marginStart="32dp"
+            android:layout_marginStart="16dp"
             android:layout_marginTop="16dp"
-            android:layout_marginBottom="16dp"
-            android:text="Auto Mode"
+            android:orientation="horizontal"
             app:layout_constraintBottom_toTopOf="@+id/linearLayout"
             app:layout_constraintStart_toStartOf="parent"
-            app:layout_constraintTop_toBottomOf="@+id/view" />
+            app:layout_constraintTop_toBottomOf="@+id/view">
+
+            <androidx.appcompat.widget.SwitchCompat
+                android:id="@+id/auto_switch"
+                android:layout_width="wrap_content"
+                android:layout_height="wrap_content"
+                android:layout_marginStart="16dp"
+                android:layout_marginTop="4dp"
+                android:text="@string/auto_mode" />
+
+            <androidx.appcompat.widget.AppCompatCheckBox
+                android:id="@+id/dynamic_speed"
+                android:layout_width="wrap_content"
+                android:layout_height="wrap_content"
+                android:layout_marginStart="4dp"
+                android:layout_marginTop="4dp"
+                android:text="@string/dynamic_speed" />
+
+        </LinearLayout>
 
 
         <LinearLayout
@@ -301,32 +341,6 @@
 
         </LinearLayout>
 
-
-        <CheckBox
-            android:id="@+id/usbToggle"
-            android:layout_width="wrap_content"
-            android:layout_height="wrap_content"
-            android:layout_marginEnd="16dp"
-            android:button="@drawable/usb_toggle"
-            app:layout_constraintBottom_toBottomOf="@+id/camera_toggle"
-            app:layout_constraintEnd_toStartOf="@+id/camera_toggle"
-            app:layout_constraintTop_toTopOf="@+id/camera_toggle" />
-
-        <ImageView
-            android:id="@+id/camera_toggle"
-            android:layout_width="40dp"
-            android:layout_height="40dp"
-            android:layout_margin="16dp"
-            android:background="@android:color/transparent"
-            android:scaleType="center"
-            android:src="@drawable/ic_cameraswitch"
-            android:text="@string/camera_facing_back"
-            app:layout_constraintBottom_toBottomOf="@+id/auto_switch"
-            app:layout_constraintEnd_toEndOf="parent"
-            app:layout_constraintTop_toTopOf="@+id/auto_switch"
-            app:tint="@color/openBotBlue" />
-
-
         <include
             android:id="@+id/controllerContainer"
             layout="@layout/control_buttons" />

android/app/src/main/res/layout/fragment_object_nav.xml

@@ -32,18 +32,34 @@
             app:layout_constraintStart_toStartOf="parent"
             app:layout_constraintTop_toTopOf="parent" />
 
-        <androidx.appcompat.widget.SwitchCompat
-            android:id="@+id/auto_switch"
+        <LinearLayout
+            android:id="@+id/autoLinearLayout"
             android:layout_width="wrap_content"
             android:layout_height="wrap_content"
-            android:layout_marginStart="32dp"
+            android:layout_marginStart="16dp"
             android:layout_marginTop="16dp"
-            android:layout_marginBottom="16dp"
-            android:text="Auto Mode"
+            android:orientation="horizontal"
             app:layout_constraintBottom_toTopOf="@+id/linearLayout"
             app:layout_constraintStart_toStartOf="parent"
-            app:layout_constraintTop_toBottomOf="@+id/view" />
+            app:layout_constraintTop_toBottomOf="@+id/view">
+
+            <androidx.appcompat.widget.SwitchCompat
+                android:id="@+id/auto_switch"
+                android:layout_width="wrap_content"
+                android:layout_height="wrap_content"
+                android:layout_marginStart="16dp"
+                android:layout_marginTop="4dp"
+                android:text="@string/auto_mode" />
 
+            <androidx.appcompat.widget.AppCompatCheckBox
+                android:id="@+id/dynamic_speed"
+                android:layout_width="wrap_content"
+                android:layout_height="wrap_content"
+                android:layout_marginStart="4dp"
+                android:layout_marginTop="4dp"
+                android:text="@string/dynamic_speed" />
+
+        </LinearLayout>
 
         <LinearLayout
             android:id="@+id/linearLayout"
@@ -305,14 +321,13 @@
             android:id="@+id/camera_toggle"
             android:layout_width="40dp"
             android:layout_height="40dp"
-            android:layout_margin="16dp"
+            android:layout_marginRight="16dp"
             android:background="@android:color/transparent"
             android:scaleType="center"
             android:src="@drawable/ic_cameraswitch"
             android:text="@string/camera_facing_back"
-            app:layout_constraintBottom_toBottomOf="@+id/auto_switch"
+            app:layout_constraintTop_toTopOf="@+id/autoLinearLayout"
             app:layout_constraintEnd_toEndOf="parent"
-            app:layout_constraintTop_toTopOf="@+id/auto_switch"
             app:tint="@color/openBotBlue" />
 
 

android/app/src/main/res/values/strings.xml

@@ -92,6 +92,8 @@
   <string name="tracking_lost">Tracking lost.</string>
   <string name="no_initial_ar_core_pose">No initial AR Core pose.</string>
   <string name="ar_core_session_paused">AR Core session paused.</string>
+  <string name="auto_mode">Auto Mode</string>
+  <string name="dynamic_speed">Dynamic Speed</string>
 
   <string name="bumpers">Bumpers</string>
   <string name="n_a">N/A</string>