Skip to content

Commit 777aadb

Browse files
thias15thias15
authored
Merge pull request #311 from reger24/master
Adjust robot speed by a estimated distancefactor.
2 parents 173b43c + d61b9f2 commit 777aadb

File tree

12 files changed

+144
-47
lines changed

12 files changed

+144
-47
lines changed

‎android/app/README.md‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -168,6 +168,16 @@ Simple UI for tracking objects of 80 different classes. A short description of t
168168
<img src="../../docs/images/screen_object_tracking_2.jpg" alt="Alt text" width="49%" />
169169
</p>
170170

171+
#### Options
172+
- **Dynamic Speed**: reduces the robot speed in "Auto Mode" if it gets closer to the tracked object.
173+
The speed is scaled based on the area of the bouding box (works best in landscape orientation).
174+
- **Model**: choose an object detector based on your phone performance (see below for [benchmarking results](#benchmark)).
175+
- **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/).
176+
- **Confidence**: confidence threshold to determine if detections are accepted. Increase if you get false detections, decrease if the object of interest it not detected.
177+
- **Device**: use CPU, GPU or NNAPI for inference (more details [here](#device)).
178+
- **Threads**: number of threads to use (only makes a difference when CPU is selected as device).
179+
180+
171181
### Point Goal Navigation
172182

173183
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‎

Lines changed: 10 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -26,7 +26,8 @@ public class SharedPreferencesManager {
2626

2727
private static final String OBJECT_TYPE = "OBJECT_TYPE";
2828
private static final String DEFAULT_OBJECT_TYPE = "person";
29-
29+
// object tracker switch for speed adjusted by estimated object distance
30+
private static final String OBJECT_NAV_DYNAMIC_SPEED = "OBJECT_NAV_DYNAMICSPEED";
3031
private static final int DEFAULT_DEVICE = Network.Device.CPU.ordinal();
3132
private static final String DEVICE = "DEVICE";
3233
private static final int DEFAULT_NUM_THREAD = 4;
@@ -137,6 +138,14 @@ public void setDriveMode(int mode) {
137138
preferences.edit().putInt(DRIVE_MODE, mode).apply();
138139
}
139140

141+
public void setDynamicSpeed(boolean isEnabled) {
142+
preferences.edit().putBoolean(OBJECT_NAV_DYNAMIC_SPEED, isEnabled).apply();
143+
}
144+
145+
public boolean getDynamicSpeed() {
146+
return preferences.getBoolean(OBJECT_NAV_DYNAMIC_SPEED, false);
147+
}
148+
140149
public void setLogMode(int mode) {
141150
preferences.edit().putInt(LOG_MODE, mode).apply();
142151
}

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

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -34,7 +34,6 @@ public class SettingsFragment extends PreferenceFragmentCompat {
3434
private SwitchPreferenceCompat storage;
3535
private SwitchPreferenceCompat location;
3636
private SwitchPreferenceCompat mic;
37-
3837
private final ActivityResultLauncher<String[]> requestPermissionLauncher =
3938
registerForActivityResult(
4039
new ActivityResultContracts.RequestMultiplePermissions(),

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

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -206,6 +206,12 @@ public void onNothingSelected(AdapterView<?> parent) {}
206206
Enums.SpeedMode.getByID(preferencesManager.getSpeedMode()))));
207207

208208
binding.autoSwitch.setOnClickListener(v -> setNetworkEnabled(binding.autoSwitch.isChecked()));
209+
binding.dynamicSpeed.setChecked(preferencesManager.getDynamicSpeed());
210+
binding.dynamicSpeed.setOnClickListener(
211+
v -> {
212+
preferencesManager.setDynamicSpeed(binding.dynamicSpeed.isChecked());
213+
tracker.setDynamicSpeed(preferencesManager.getDynamicSpeed());
214+
});
209215
}
210216

211217
private void updateCropImageInfo() {
@@ -223,6 +229,7 @@ private void updateCropImageInfo() {
223229
borderedText.setTypeface(Typeface.MONOSPACE);
224230

225231
tracker = new MultiBoxTracker(requireContext());
232+
tracker.setDynamicSpeed(preferencesManager.getDynamicSpeed());
226233

227234
Timber.i("Camera orientation relative to screen canvas: %d", sensorOrientation);
228235

@@ -378,6 +385,7 @@ private void setNetworkEnabledWithAudio(boolean b) {
378385

379386
private void setNetworkEnabled(boolean b) {
380387
binding.autoSwitch.setChecked(b);
388+
381389
binding.controllerContainer.controlMode.setEnabled(!b);
382390
binding.controllerContainer.driveMode.setEnabled(!b);
383391
binding.controllerContainer.speedInfo.setEnabled(!b);

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

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -181,7 +181,7 @@ public abstract class CameraActivity extends AppCompatActivity
181181

182182
private Intent intentSensorService;
183183
private ServerCommunication serverCommunication;
184-
private SharedPreferencesManager preferencesManager;
184+
protected SharedPreferencesManager preferencesManager;
185185
protected final GameController gameController = new GameController(driveMode);
186186
private PhoneController phoneController;
187187
protected final ControllerHandler controllerHandler = new ControllerHandler();
@@ -452,6 +452,10 @@ public void onReceive(Context context, Intent intent) {
452452
vehicle.requestVehicleConfig();
453453
}
454454

455+
/**
456+
* Initalize bottom sheet views/fields with shared preference values (@see
457+
* SharedPreferenceManager)
458+
*/
455459
@SuppressLint("SetTextI18n")
456460
private void setInitialValues() {
457461
cameraSwitchCompat.setChecked(preferencesManager.getCameraSwitch());

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

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -100,6 +100,7 @@ public void onPreviewSizeChosen(final Size size, final int rotation) {
100100
borderedText.setTypeface(Typeface.MONOSPACE);
101101

102102
tracker = new MultiBoxTracker(this);
103+
tracker.setDynamicSpeed(preferencesManager.getDynamicSpeed());
103104

104105
previewWidth = size.getWidth();
105106
previewHeight = size.getHeight();

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

Lines changed: 38 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -73,6 +73,7 @@ public class MultiBoxTracker {
7373
private int sensorOrientation;
7474
private float leftControl;
7575
private float rightControl;
76+
private boolean useDynamicSpeed = false;
7677

7778
public MultiBoxTracker(final Context context) {
7879
for (final int color : COLORS) {
@@ -143,21 +144,30 @@ private void updateFrameToCanvasMatrix(int canvasHeight, int canvasWidth) {
143144
false);
144145
}
145146

147+
/**
148+
* Determine the robot controls/steering from the position of the tracked object/person on screen.
149+
* The follow speed is adjusted based on the area of the bounding box of the tracked object.
150+
* Assumption: large object box --> close to object --> slow down
151+
*
152+
* @return the adjusted speed control for left and right wheels in the range -1.0 ... 1.0
153+
*/
146154
public synchronized Control updateTarget() {
147155
if (!trackedObjects.isEmpty()) {
148-
// Pick person with highest probability
156+
// Pick detection with highest probability
149157
final RectF trackedPos = new RectF(trackedObjects.get(0).location);
150158
final boolean rotated = sensorOrientation % 180 == 90;
151159
float imgWidth = (float) (rotated ? frameHeight : frameWidth);
160+
// calculate track box area for distance estimate
161+
float boxArea = trackedPos.height() * trackedPos.width();
152162
float centerX = (rotated ? trackedPos.centerY() : trackedPos.centerX());
153163
// Make sure object center is in frame
154164
centerX = Math.max(0.0f, Math.min(centerX, imgWidth));
155165
// Scale relative position along x-axis between -1 and 1
156166
float x_pos_norm = 1.0f - 2.0f * centerX / imgWidth;
157-
// Scale to control signal and account for rotation
167+
// Scale for steering signal and account for rotation,
158168
float x_pos_scaled = rotated ? -x_pos_norm * 1.0f : x_pos_norm * 1.0f;
159169
//// Scale by "exponential" function: y = x / sqrt(1-x^2)
160-
// Math.max (Math.min(x_pos_norm / Math.sqrt(1 - x_pos_norm * x_pos_norm),2),-2) * 255.0f;
170+
// Math.max (Math.min(x_pos_norm / Math.sqrt(1 - x_pos_norm * x_pos_norm),2),-2);
161171

162172
if (x_pos_scaled < 0) {
163173
leftControl = 1.0f;
@@ -166,10 +176,26 @@ public synchronized Control updateTarget() {
166176
leftControl = 1.0f - x_pos_scaled;
167177
rightControl = 1.0f;
168178
}
179+
180+
// adjust speed depending on size of detected object bounding box
181+
if (useDynamicSpeed) {
182+
float scaleFactor = 1.0f - boxArea / (frameWidth * frameHeight);
183+
scaleFactor = scaleFactor > 0.75f ? 1.0f : scaleFactor; // tracked object far, full speed
184+
// apply scale factor if tracked object is not too near, otherwise stop
185+
if (scaleFactor > 0.25f) {
186+
leftControl *= scaleFactor;
187+
rightControl *= scaleFactor;
188+
} else {
189+
leftControl = 0.0f;
190+
rightControl = 0.0f;
191+
}
192+
}
193+
169194
} else {
170195
leftControl = 0.0f;
171196
rightControl = 0.0f;
172197
}
198+
173199
return new Control(
174200
(0 > sensorOrientation) ? rightControl : leftControl,
175201
(0 > sensorOrientation) ? leftControl : rightControl);
@@ -262,6 +288,15 @@ private void processResults(final List<Recognition> results) {
262288
}
263289
}
264290

291+
/**
292+
* Set use of dynamic speed on or off (used in updateTarget())
293+
*
294+
* @param isEnabled
295+
*/
296+
public void setDynamicSpeed(boolean isEnabled) {
297+
useDynamicSpeed = isEnabled;
298+
}
299+
265300
private static class TrackedRecognition {
266301
RectF location;
267302
float detectionConfidence;

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

Lines changed: 46 additions & 32 deletions
Original file line numberDiff line numberDiff line change
@@ -44,17 +44,57 @@
4444
app:layout_constraintStart_toStartOf="parent"
4545
app:layout_constraintTop_toTopOf="parent" />
4646

47-
<androidx.appcompat.widget.SwitchCompat
48-
android:id="@+id/auto_switch"
47+
<CheckBox
48+
android:id="@+id/usbToggle"
49+
android:layout_width="wrap_content"
50+
android:layout_height="wrap_content"
51+
android:layout_marginEnd="16dp"
52+
android:button="@drawable/usb_toggle"
53+
app:layout_constraintBottom_toBottomOf="@+id/camera_toggle"
54+
app:layout_constraintEnd_toStartOf="@+id/camera_toggle"
55+
app:layout_constraintTop_toTopOf="@+id/camera_toggle" />
56+
57+
<ImageView
58+
android:id="@+id/camera_toggle"
59+
android:layout_width="40dp"
60+
android:layout_height="40dp"
61+
android:layout_marginRight="16dp"
62+
android:background="@android:color/transparent"
63+
android:scaleType="center"
64+
android:src="@drawable/ic_cameraswitch"
65+
android:text="@string/camera_facing_back"
66+
app:layout_constraintEnd_toEndOf="parent"
67+
app:layout_constraintTop_toTopOf="@+id/autoLinearLayout"
68+
app:tint="@color/openBotBlue" />
69+
70+
<LinearLayout
71+
android:id="@+id/autoLinearLayout"
4972
android:layout_width="wrap_content"
5073
android:layout_height="wrap_content"
51-
android:layout_marginStart="32dp"
74+
android:layout_marginStart="16dp"
5275
android:layout_marginTop="16dp"
53-
android:layout_marginBottom="16dp"
54-
android:text="Auto Mode"
76+
android:orientation="horizontal"
5577
app:layout_constraintBottom_toTopOf="@+id/linearLayout"
5678
app:layout_constraintStart_toStartOf="parent"
57-
app:layout_constraintTop_toBottomOf="@+id/view" />
79+
app:layout_constraintTop_toBottomOf="@+id/view">
80+
81+
<androidx.appcompat.widget.SwitchCompat
82+
android:id="@+id/auto_switch"
83+
android:layout_width="wrap_content"
84+
android:layout_height="wrap_content"
85+
android:layout_marginStart="16dp"
86+
android:layout_marginTop="4dp"
87+
android:text="@string/auto_mode" />
88+
89+
<androidx.appcompat.widget.AppCompatCheckBox
90+
android:id="@+id/dynamic_speed"
91+
android:layout_width="wrap_content"
92+
android:layout_height="wrap_content"
93+
android:layout_marginStart="4dp"
94+
android:layout_marginTop="4dp"
95+
android:text="@string/dynamic_speed" />
96+
97+
</LinearLayout>
5898

5999

60100
<LinearLayout
@@ -301,32 +341,6 @@
301341

302342
</LinearLayout>
303343

304-
305-
<CheckBox
306-
android:id="@+id/usbToggle"
307-
android:layout_width="wrap_content"
308-
android:layout_height="wrap_content"
309-
android:layout_marginEnd="16dp"
310-
android:button="@drawable/usb_toggle"
311-
app:layout_constraintBottom_toBottomOf="@+id/camera_toggle"
312-
app:layout_constraintEnd_toStartOf="@+id/camera_toggle"
313-
app:layout_constraintTop_toTopOf="@+id/camera_toggle" />
314-
315-
<ImageView
316-
android:id="@+id/camera_toggle"
317-
android:layout_width="40dp"
318-
android:layout_height="40dp"
319-
android:layout_margin="16dp"
320-
android:background="@android:color/transparent"
321-
android:scaleType="center"
322-
android:src="@drawable/ic_cameraswitch"
323-
android:text="@string/camera_facing_back"
324-
app:layout_constraintBottom_toBottomOf="@+id/auto_switch"
325-
app:layout_constraintEnd_toEndOf="parent"
326-
app:layout_constraintTop_toTopOf="@+id/auto_switch"
327-
app:tint="@color/openBotBlue" />
328-
329-
330344
<include
331345
android:id="@+id/controllerContainer"
332346
layout="@layout/control_buttons" />

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

Lines changed: 24 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -32,18 +32,34 @@
3232
app:layout_constraintStart_toStartOf="parent"
3333
app:layout_constraintTop_toTopOf="parent" />
3434

35-
<androidx.appcompat.widget.SwitchCompat
36-
android:id="@+id/auto_switch"
35+
<LinearLayout
36+
android:id="@+id/autoLinearLayout"
3737
android:layout_width="wrap_content"
3838
android:layout_height="wrap_content"
39-
android:layout_marginStart="32dp"
39+
android:layout_marginStart="16dp"
4040
android:layout_marginTop="16dp"
41-
android:layout_marginBottom="16dp"
42-
android:text="Auto Mode"
41+
android:orientation="horizontal"
4342
app:layout_constraintBottom_toTopOf="@+id/linearLayout"
4443
app:layout_constraintStart_toStartOf="parent"
45-
app:layout_constraintTop_toBottomOf="@+id/view" />
44+
app:layout_constraintTop_toBottomOf="@+id/view">
45+
46+
<androidx.appcompat.widget.SwitchCompat
47+
android:id="@+id/auto_switch"
48+
android:layout_width="wrap_content"
49+
android:layout_height="wrap_content"
50+
android:layout_marginStart="16dp"
51+
android:layout_marginTop="4dp"
52+
android:text="@string/auto_mode" />
4653

54+
<androidx.appcompat.widget.AppCompatCheckBox
55+
android:id="@+id/dynamic_speed"
56+
android:layout_width="wrap_content"
57+
android:layout_height="wrap_content"
58+
android:layout_marginStart="4dp"
59+
android:layout_marginTop="4dp"
60+
android:text="@string/dynamic_speed" />
61+
62+
</LinearLayout>
4763

4864
<LinearLayout
4965
android:id="@+id/linearLayout"
@@ -305,14 +321,13 @@
305321
android:id="@+id/camera_toggle"
306322
android:layout_width="40dp"
307323
android:layout_height="40dp"
308-
android:layout_margin="16dp"
324+
android:layout_marginRight="16dp"
309325
android:background="@android:color/transparent"
310326
android:scaleType="center"
311327
android:src="@drawable/ic_cameraswitch"
312328
android:text="@string/camera_facing_back"
313-
app:layout_constraintBottom_toBottomOf="@+id/auto_switch"
329+
app:layout_constraintTop_toTopOf="@+id/autoLinearLayout"
314330
app:layout_constraintEnd_toEndOf="parent"
315-
app:layout_constraintTop_toTopOf="@+id/auto_switch"
316331
app:tint="@color/openBotBlue" />
317332

318333

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

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -92,6 +92,8 @@
9292
<string name="tracking_lost">Tracking lost.</string>
9393
<string name="no_initial_ar_core_pose">No initial AR Core pose.</string>
9494
<string name="ar_core_session_paused">AR Core session paused.</string>
95+
<string name="auto_mode">Auto Mode</string>
96+
<string name="dynamic_speed">Dynamic Speed</string>
9597

9698
<string name="bumpers">Bumpers</string>
9799
<string name="n_a">N/A</string>

0 commit comments

Comments
 (0)