From 1a9a9134124fb3e0303bec79a9685a50cd39230c Mon Sep 17 00:00:00 2001 From: Harish Thakur Date: Wed, 21 Oct 2026 17:47:35 +0530 Subject: [PATCH 1/3] Replace the Set Goal dialog with tapping the goal on the floor --- .../app/robot/pointGoalNavigation/ArCore.java | 253 +++++++++++++++--- .../pointGoalNavigation/ArCoreListener.java | 6 + .../pointGoalNavigation/ArrivalCheck.java | 38 +++ .../pointGoalNavigation/GoalValidator.java | 94 +++++++ .../PointGoalNavigationFragment.java | 215 +++++++++++---- .../SetGoalDialogFragment.java | 50 ---- .../rendering/DisplayRotationHelper.java | 5 + .../rendering/TwoDRenderer.java | 17 +- .../openbot/app/robot/vehicle/Vehicle.java | 1 + .../layout/fragment_point_goal_navigation.xml | 23 ++ .../main/res/layout/set_goal_dialog_view.xml | 38 --- .../app/src/main/res/values-de/strings.xml | 9 +- .../app/src/main/res/values-es/strings.xml | 9 +- .../app/src/main/res/values-fr/strings.xml | 9 +- .../app/src/main/res/values-hi/strings.xml | 9 +- .../app/src/main/res/values-ko/strings.xml | 9 +- .../app/src/main/res/values-zh/strings.xml | 9 +- .../app/src/main/res/values/strings.xml | 11 +- .../pointGoalNavigation/ArrivalCheckTest.java | 65 +++++ .../GoalValidatorTest.java | 117 ++++++++ 20 files changed, 786 insertions(+), 201 deletions(-) create mode 100644 robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/ArrivalCheck.java create mode 100644 robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/GoalValidator.java delete mode 100644 robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/SetGoalDialogFragment.java delete mode 100644 robot/android/app/src/main/res/layout/set_goal_dialog_view.xml create mode 100644 robot/android/app/src/test/java/org/openbot/app/robot/pointGoalNavigation/ArrivalCheckTest.java create mode 100644 robot/android/app/src/test/java/org/openbot/app/robot/pointGoalNavigation/GoalValidatorTest.java diff --git a/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/ArCore.java b/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/ArCore.java index 5d22c23cea..01b0c3e546 100644 --- a/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/ArCore.java +++ b/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/ArCore.java @@ -4,11 +4,11 @@ import android.media.Image; import android.opengl.GLES20; import android.opengl.GLSurfaceView; +import android.opengl.Matrix; import android.os.Handler; import android.os.HandlerThread; import android.os.SystemClock; import android.util.Size; -import android.view.ViewGroup; import com.google.ar.core.Anchor; import com.google.ar.core.Camera; import com.google.ar.core.CameraConfig; @@ -18,11 +18,13 @@ import com.google.ar.core.Config.LightEstimationMode; import com.google.ar.core.Config.PlaneFindingMode; import com.google.ar.core.Frame; +import com.google.ar.core.Plane; import com.google.ar.core.Pose; import com.google.ar.core.Session; import com.google.ar.core.TrackingFailureReason; import com.google.ar.core.TrackingState; import com.google.ar.core.exceptions.CameraNotAvailableException; +import com.google.ar.core.exceptions.NotTrackingException; import com.google.ar.core.exceptions.NotYetAvailableException; import com.google.ar.core.exceptions.SessionPausedException; import com.google.ar.core.exceptions.UnavailableApkTooOldException; @@ -30,6 +32,8 @@ import com.google.ar.core.exceptions.UnavailableDeviceNotCompatibleException; import com.google.ar.core.exceptions.UnavailableSdkTooOldException; import java.io.IOException; +import java.util.ArrayList; +import java.util.Arrays; import java.util.EnumSet; import java.util.List; import javax.microedition.khronos.egl.EGLConfig; @@ -52,8 +56,7 @@ public class ArCore implements GLSurfaceView.Renderer { private Session session; private Camera camera; private final TwoDRenderer twoDRenderer = new TwoDRenderer(); - private Pose currentPose; - private float gpuTextureAspectRatio = 16.0f / 9.0f; + private volatile Pose currentPose; private ArCoreListener arCoreListener = null; private final float[] anchorMatrix = new float[16]; private Anchor startAnchor, targetAnchor; @@ -61,6 +64,21 @@ public class ArCore implements GLSurfaceView.Renderer { private GLSurfaceView surfaceView; private boolean renderFrame = true; private Pose startPose = null, targetPose = null; + // Written on the UI thread, consumed on the GL thread. + private volatile float[] pendingTap = null; + // Floor and wall detection cost CPU, so only the tap-to-goal screen turns them on. + private boolean goalPlacementEnabled = false; + // The marker quad reaches this far below its center; lift it so the pin tip touches the floor. + private static final float MARKER_TIP_OFFSET_M = 0.1f; + + private static final String PREFS = "point_goal_navigation"; + private static final String PREF_CAMERA_HEIGHT = "camera_height_m"; + private static final int CALIBRATE_EVERY_FRAMES = 15; + // Height of the phone camera above the floor. Distances come from it, so it is re-measured + // from ARCore's floor plane whenever one is found and remembered for next time. + private float cameraHeightM; + private boolean cameraHeightMeasured = false; + private int frameCount = 0; public ArCore(Context context, GLSurfaceView surfaceView, Handler handlerMain) { this.appContext = context; @@ -94,13 +112,16 @@ public void onSurfaceCreated(GL10 gl, EGLConfig config) { @Override public void onSurfaceChanged(GL10 gl, int width, int height) { + // The surfaceView is left at its XML match_parent size (full screen) rather than resized to + // the camera's native capture aspect ratio - BackgroundRenderer.draw() already renders a + // full-viewport quad and relies on Frame#transformCoordinates2d to crop/rotate the camera + // texture to fill whatever the real viewport is, which is the standard ARCore pattern. + // Shrinking the surface to match the camera's aspect ratio here fought that: it produced + // letterboxing and, combined with DisplayRotationHelper/Session#setDisplayGeometry being fed + // a viewport that didn't match the true full-screen size, incorrect marker projection too. + Timber.d("onSurfaceChanged: gl surface (%d, %d)", width, height); displayRotationHelper.onSurfaceChanged(width, height); GLES20.glViewport(0, 0, width, height); - - ViewGroup.LayoutParams lp = surfaceView.getLayoutParams(); - lp.height = height; - lp.width = (int) (lp.height * gpuTextureAspectRatio); - runOnMainThread(() -> surfaceView.setLayoutParams(lp)); } public Pose getStartPose() { @@ -114,7 +135,7 @@ public void setStartAnchorAtCurrentPose() { } public Pose getTargetPose() { - return targetAnchor.getPose(); + return targetAnchor == null ? null : targetAnchor.getPose(); } public void setTargetAnchor(Pose pose) { @@ -125,6 +146,42 @@ public void setTargetAnchorAtCurrentPose() { targetAnchor = session.createAnchor(currentPose); } + /** + * Replaces the current target anchor with a plain anchor at its current pose. Anchors created + * from a hit result stay attached to their plane and move as ARCore refines it, which would make + * the goal drift while driving. A plain {@code session.createAnchor(pose)} anchor stays fixed. + */ + public void freezeTargetAnchor() { + if (targetAnchor == null) { + Timber.d("freezeTargetAnchor: no target anchor to freeze"); + return; + } + Pose frozenPose = targetAnchor.getPose(); + targetAnchor.detach(); + targetAnchor = session.createAnchor(frozenPose); + Timber.d( + "freezeTargetAnchor: frozen at (%.3f, %.3f, %.3f)", + frozenPose.tx(), frozenPose.ty(), frozenPose.tz()); + } + + /** + * Requests that the given screen-space tap (in {@link #surfaceView} pixel coordinates) be turned + * into a goal pose on the next rendered frame. The actual hit-test runs on the GL thread inside + * {@link #onDrawFrame}, since it needs the current frame's {@link Frame} object. + */ + public void requestTapAt(float screenX, float screenY) { + pendingTap = new float[] {screenX, screenY}; + } + + /** Turns on floor/wall detection and depth for tap-to-goal. Call before {@link #resume()}. */ + public void setGoalPlacementEnabled(boolean enabled) { + goalPlacementEnabled = enabled; + cameraHeightM = + appContext + .getSharedPreferences(PREFS, Context.MODE_PRIVATE) + .getFloat(PREF_CAMERA_HEIGHT, GoalValidator.DEFAULT_CAMERA_HEIGHT_M); + } + @Override public void onDrawFrame(GL10 gl) { // Clear screen to notify driver it should not load any pixels from previous frame. @@ -201,6 +258,16 @@ public void onDrawFrame(GL10 gl) { targetPose = targetAnchor.getPose(); } + if (goalPlacementEnabled && frameCount++ % CALIBRATE_EVERY_FRAMES == 0) { + calibrateCameraHeight(); + } + + float[] tap = pendingTap; + if (tap != null) { + pendingTap = null; + placeGoalFromTap(tap[0], tap[1]); + } + // Get image. // TODO: If needed, we could possibly implement some performance optimization here: // Do we really need to copy here? We could use several cycling textures instead of only @@ -257,20 +324,151 @@ public void onDrawFrame(GL10 gl) { final float[] colorCorrectionRgba = new float[4]; frame.getLightEstimate().getColorCorrection(colorCorrectionRgba, 0); - float translation[] = new float[3]; - float rotation[] = new float[4]; - targetAnchor.getPose().getTranslation(translation, 0); - currentPose.getRotationQuaternion(rotation, 0); + // Stand the pin upright in the world (local +y = world up) and turn it about the + // vertical axis to face the camera, so it looks right however the phone is mounted: + // portrait, landscape or upside down. Its tip sits on the goal spot on the floor. + float[] goal = targetAnchor.getPose().getTranslation(); + float yaw = (float) Math.atan2(currentPose.tx() - goal[0], currentPose.tz() - goal[2]); + Pose markerPose = + new Pose( + new float[] {goal[0], goal[1] + MARKER_TIP_OFFSET_M, goal[2]}, + new float[] {0f, (float) Math.sin(yaw / 2), 0f, (float) Math.cos(yaw / 2)}); + markerPose.toMatrix(anchorMatrix, 0); + twoDRenderer.updateModelMatrix(anchorMatrix, 1.0f); + twoDRenderer.draw(viewmtx, projmtx); + } + } + } - Pose rotatedPose = new Pose(translation, rotation); - rotatedPose.toMatrix(anchorMatrix, 0); + /** + * The goal is where the tap ray meets the floor, with the floor at the known camera height below + * the phone. This only needs the camera's orientation (gravity is exact from the IMU), so it's + * correct from the first frame. Image depth is not used: a phone standing still on the robot has + * no parallax yet, so early depth guesses are off and get corrected as it drives, which made the + * goal jump. + */ + private void placeGoalFromTap(float tapX, float tapY) { + float[] cameraPos = currentPose.getTranslation(); + float[][] ray = tapRay(tapX, tapY); + float[] goal = + ray == null + ? null + : GoalValidator.intersectFloor(ray[0], ray[1], cameraPos[1] - cameraHeightM); + + GoalValidator.Result result = GoalValidator.validate(cameraPos, goal); + Timber.d( + "goal tap (%.0f, %.0f): camera=%s height=%.3f (%s) goal=%s -> %s", + tapX, + tapY, + Arrays.toString(cameraPos), + cameraHeightM, + cameraHeightMeasured ? "measured" : "saved", + Arrays.toString(goal), + result); + if (result != GoalValidator.Result.OK) { + notifyRejected(result); + return; + } - float scaleFactor = 1.0f; - // Update and draw the model and its shadow. - twoDRenderer.updateModelMatrix(anchorMatrix, scaleFactor); - twoDRenderer.draw(viewmtx, projmtx); + try { + // A plain anchor at a world position stays fixed; it doesn't follow the screen. + Anchor newAnchor = session.createAnchor(Pose.makeTranslation(goal[0], goal[1], goal[2])); + if (targetAnchor != null) targetAnchor.detach(); + targetAnchor = newAnchor; + targetPose = newAnchor.getPose(); + float distance = GoalValidator.horizontalDistance(cameraPos, goal); + runOnMainThread( + () -> { + if (arCoreListener != null) arCoreListener.onGoalPlaced(distance); + }); + } catch (NotTrackingException e) { + Timber.d(e, "ARCore lost tracking while placing goal anchor."); + } + } + + /** Measures the camera height from ARCore's floor plane, if it sees one. */ + private void calibrateCameraHeight() { + float cameraY = currentPose.ty(); + Float floorY = GoalValidator.pickFloorY(collectUpwardPlanes(), cameraY); + if (floorY == null) return; + float height = cameraY - floorY; + // Only heights a phone on the robot can have; not while someone holds it up by hand. + if (height < GoalValidator.MIN_MOUNT_HEIGHT_M || height > GoalValidator.MAX_MOUNT_HEIGHT_M) { + return; + } + if (!cameraHeightMeasured || Math.abs(height - cameraHeightM) > 0.005f) { + Timber.d("camera height measured: %.3f m", height); + appContext + .getSharedPreferences(PREFS, Context.MODE_PRIVATE) + .edit() + .putFloat(PREF_CAMERA_HEIGHT, height) + .apply(); + } + cameraHeightM = height; + cameraHeightMeasured = true; + } + + public float getCameraHeight() { + return cameraHeightM; + } + + /** World-space ray {origin, direction} through a screen pixel. */ + private float[][] tapRay(float tapX, float tapY) { + int width = surfaceView.getWidth(); + int height = surfaceView.getHeight(); + if (width == 0 || height == 0) return null; + + float[] projection = new float[16]; + camera.getProjectionMatrix(projection, 0, 0.1f, 100.0f); + float[] view = new float[16]; + camera.getViewMatrix(view, 0); + float[] viewProjection = new float[16]; + Matrix.multiplyMM(viewProjection, 0, projection, 0, view, 0); + float[] inverse = new float[16]; + if (!Matrix.invertM(inverse, 0, viewProjection, 0)) return null; + + float ndcX = 2f * tapX / width - 1f; + float ndcY = 1f - 2f * tapY / height; + float[] near = unproject(inverse, ndcX, ndcY, -1f); + float[] far = unproject(inverse, ndcX, ndcY, 1f); + float[] direction = {far[0] - near[0], far[1] - near[1], far[2] - near[2]}; + float length = + (float) + Math.sqrt( + direction[0] * direction[0] + + direction[1] * direction[1] + + direction[2] * direction[2]); + if (length < 1e-6f) return null; + for (int i = 0; i < 3; i++) direction[i] /= length; + return new float[][] {near, direction}; + } + + private static float[] unproject(float[] inverse, float x, float y, float z) { + float[] in = {x, y, z, 1f}; + float[] out = new float[4]; + Matrix.multiplyMV(out, 0, inverse, 0, in, 0); + return new float[] {out[0] / out[3], out[1] / out[3], out[2] / out[3]}; + } + + private void notifyRejected(GoalValidator.Result result) { + runOnMainThread( + () -> { + if (arCoreListener != null) arCoreListener.onGoalRejected(result); + }); + } + + /** Returns {y, area} for every tracked upward-facing plane that hasn't been merged away. */ + private List collectUpwardPlanes() { + List planes = new ArrayList<>(); + for (Plane plane : session.getAllTrackables(Plane.class)) { + if (plane.getTrackingState() != TrackingState.TRACKING + || plane.getSubsumedBy() != null + || plane.getType() != Plane.Type.HORIZONTAL_UPWARD_FACING) { + continue; } + planes.add(new float[] {plane.getCenterPose().ty(), plane.getExtentX() * plane.getExtentZ()}); } + return planes; } protected synchronized void runOnMainThread(final Runnable r) { @@ -319,14 +517,6 @@ private boolean is640x480(Size size) { } } - private void setSurfaceViewAspectRatio(float aspectRatio) { - gpuTextureAspectRatio = aspectRatio; - - ViewGroup.LayoutParams lp = surfaceView.getLayoutParams(); - lp.width = (int) (lp.height * gpuTextureAspectRatio); - runOnMainThread(() -> surfaceView.setLayoutParams(lp)); - } - private void setCameraConfig() throws CameraNotAvailableException { // INFO: To require depth sensor add // `.setDepthSensorUsage(EnumSet.of(CameraConfig.DepthSensorUsage.REQUIRE_AND_USE))` @@ -388,10 +578,6 @@ private void setCameraConfig() throws CameraNotAvailableException { + ")"); session.setCameraConfig(minCameraConfig); - setSurfaceViewAspectRatio( - 1.0f - * minCameraConfig.getTextureSize().getWidth() - / minCameraConfig.getTextureSize().getHeight()); } private void setConfig() { @@ -405,6 +591,11 @@ private void setConfig() { .setPlaneFindingMode(PlaneFindingMode.DISABLED) .setUpdateMode(Config.UpdateMode.BLOCKING); + if (goalPlacementEnabled) { + // Only used to measure the camera height above the floor. + config.setPlaneFindingMode(PlaneFindingMode.HORIZONTAL); + } + session.configure(config); } diff --git a/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/ArCoreListener.java b/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/ArCoreListener.java index d382e76e57..63586f7a67 100644 --- a/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/ArCoreListener.java +++ b/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/ArCoreListener.java @@ -13,4 +13,10 @@ void onArCoreUpdate( void onArCoreTrackingFailure(long timestamp, TrackingFailureReason trackingFailureReason); void onArCoreSessionPaused(long timestamp); + + /** A tapped goal passed all checks; distance is from the robot, in meters. */ + default void onGoalPlaced(float distanceM) {} + + /** A tapped point is not a goal the robot can drive to. */ + default void onGoalRejected(GoalValidator.Result reason) {} } diff --git a/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/ArrivalCheck.java b/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/ArrivalCheck.java new file mode 100644 index 0000000000..7111aa2572 --- /dev/null +++ b/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/ArrivalCheck.java @@ -0,0 +1,38 @@ +package org.openbot.app.robot.pointGoalNavigation; + +/** + * Decides when the robot has arrived. Besides being within the goal radius, a robot that is close + * and has just passed the goal (it's now beside or behind it, or getting further away again) has + * arrived too: turning back to hit the exact spot only makes it circle around it. + */ +public final class ArrivalCheck { + + /** Within this distance of the goal the robot has arrived. */ + public static final float GOAL_RADIUS_M = 0.10f; + + /** Inside this distance, passing the goal also counts as arriving. */ + public static final float ARRIVAL_ZONE_M = 0.30f; + + /** Moving away by more than this from the closest point means the goal was passed. */ + public static final float MOVING_AWAY_M = 0.03f; + + private float closest = Float.MAX_VALUE; + + public void reset() { + closest = Float.MAX_VALUE; + } + + /** + * @param distance distance to the goal in meters + * @param deltaYaw angle between heading and goal direction, as from computeDeltaYaw + */ + public boolean hasArrived(float distance, float deltaYaw) { + closest = Math.min(closest, distance); + if (distance < GOAL_RADIUS_M) return true; + if (distance >= ARRIVAL_ZONE_M) return false; + + boolean goalBesideOrBehind = Math.abs(deltaYaw) > Math.PI / 2; + boolean movingAway = distance > closest + MOVING_AWAY_M; + return goalBesideOrBehind || movingAway; + } +} diff --git a/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/GoalValidator.java b/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/GoalValidator.java new file mode 100644 index 0000000000..3099c55a85 --- /dev/null +++ b/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/GoalValidator.java @@ -0,0 +1,94 @@ +package org.openbot.app.robot.pointGoalNavigation; + +import androidx.annotation.Nullable; +import java.util.List; + +/** + * Math for turning a tap into a goal on the floor, in ARCore world coordinates (y is up, meters). + * Kept lenient on purpose: it only rejects taps a floor robot can never drive to. + */ +public final class GoalValidator { + + /** The floor is always at least this far below the phone on the robot. */ + public static final float MIN_CAMERA_HEIGHT_M = 0.03f; + + /** Phone camera height on an OpenBot DIY body, until it has been measured. */ + public static final float DEFAULT_CAMERA_HEIGHT_M = 0.12f; + + /** Measured heights outside this range mean the phone isn't on the robot right now. */ + public static final float MIN_MOUNT_HEIGHT_M = 0.04f; + + public static final float MAX_MOUNT_HEIGHT_M = 0.40f; + + /** Closer goals would count as reached right away. */ + public static final float MIN_GOAL_DISTANCE_M = 0.30f; + + /** + * Further than this the tap ray is so flat that a tiny angle error is a big distance error (at 12 + * cm height, 0.1 degree is already ~25 cm at 4 m). + */ + public static final float MAX_RAY_DISTANCE_M = 6f; + + public enum Result { + OK, + NOT_FLOOR, + TOO_CLOSE + } + + private GoalValidator() {} + + /** + * @param camera camera position {x, y, z} + * @param hit tapped point on the floor {x, y, z}, or null if the tap doesn't reach the floor + */ + public static Result validate(float[] camera, @Nullable float[] hit) { + if (hit == null) return Result.NOT_FLOOR; + // Windows, walls, sky: anything at or above the camera can't be on the floor. + if (hit[1] > camera[1] - MIN_CAMERA_HEIGHT_M) return Result.NOT_FLOOR; + if (horizontalDistance(camera, hit) < MIN_GOAL_DISTANCE_M) return Result.TOO_CLOSE; + return Result.OK; + } + + /** + * Where the tap ray meets the floor. Taps at or above the horizon (window, wall, sky) never do. + * + * @return {x, y, z}, or null if the ray doesn't point down at the floor + */ + @Nullable + public static float[] intersectFloor(float[] origin, float[] direction, float floorY) { + if (direction[1] >= -1e-4f) return null; + float t = (floorY - origin[1]) / direction[1]; + if (t <= 0) return null; + float[] hit = { + origin[0] + t * direction[0], floorY, origin[2] + t * direction[2], + }; + if (horizontalDistance(origin, hit) > MAX_RAY_DISTANCE_M) return null; + return hit; + } + + /** + * Floor height from the tracked upward-facing planes: the largest one below the camera. The + * lowest one would pick up false planes reflected in shiny floors. + * + * @param planes each entry is {y, area} + */ + @Nullable + public static Float pickFloorY(List planes, float cameraY) { + Float floorY = null; + float bestArea = 0f; + for (float[] plane : planes) { + if (plane[0] > cameraY - MIN_CAMERA_HEIGHT_M) continue; + if (plane[1] > bestArea) { + bestArea = plane[1]; + floorY = plane[0]; + } + } + return floorY; + } + + public static float horizontalDistance(float[] a, float[] b) { + float dx = a[0] - b[0]; + float dz = a[2] - b[2]; + return (float) Math.sqrt(dx * dx + dz * dz); + } +} diff --git a/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/PointGoalNavigationFragment.java b/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/PointGoalNavigationFragment.java index 17b9ea22ad..5becd01782 100644 --- a/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/PointGoalNavigationFragment.java +++ b/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/PointGoalNavigationFragment.java @@ -2,11 +2,15 @@ import static java.lang.Math.abs; +import android.annotation.SuppressLint; import android.graphics.Bitmap; import android.os.Bundle; import android.os.Handler; +import android.os.HandlerThread; import android.os.Looper; +import android.os.SystemClock; import android.view.LayoutInflater; +import android.view.MotionEvent; import android.view.View; import android.view.ViewGroup; import androidx.activity.result.ActivityResultLauncher; @@ -14,10 +18,10 @@ import androidx.annotation.NonNull; import androidx.annotation.Nullable; import androidx.lifecycle.ViewModelProvider; +import com.google.android.material.snackbar.Snackbar; import com.google.ar.core.Pose; import com.google.ar.core.TrackingFailureReason; import com.google.ar.core.exceptions.CameraNotAvailableException; -import com.google.ar.core.exceptions.NotTrackingException; import com.google.ar.core.exceptions.UnavailableApkTooOldException; import com.google.ar.core.exceptions.UnavailableArcoreNotInstalledException; import com.google.ar.core.exceptions.UnavailableDeviceNotCompatibleException; @@ -36,6 +40,7 @@ import org.openbot.app.robot.utils.Constants; import org.openbot.app.robot.utils.PermissionUtils; import org.openbot.app.robot.vehicle.Control; +import org.openbot.app.robot.vehicle.Vehicle; import timber.log.Timber; public class PointGoalNavigationFragment extends ControlsFragment implements ArCoreListener { @@ -44,7 +49,21 @@ public class PointGoalNavigationFragment extends ControlsFragment implements ArC private Handler handlerMain; private ArCore arCore; private FragmentPointGoalNavigationBinding binding; - private boolean isRunning = false; + // Read by the watchdog thread. + private volatile boolean isRunning = false; + private volatile long lastNavUpdateMs = 0; + private final ArrivalCheck arrivalCheck = new ArrivalCheck(); + private HandlerThread watchdogThread; + private Handler watchdogHandler; + + // ARCore calls back every frame (~33 ms); this long without one means the app is stuck. + private static final long WATCHDOG_TIMEOUT_MS = 700; + private static final long WATCHDOG_CHECK_MS = 200; + // Frames that waited this long in the main-thread queue are dropped; driving on them is late + // anyway, and a backlog would delay the Stop button. + private static final long MAX_FRAME_AGE_MS = 100; + // Resend the stop once in case the first command got lost over Bluetooth. + private static final long STOP_RESEND_MS = 100; private boolean isPermissionRequested = false; private Navigation navigationPolicy; static final int kMaxChannelValue = 262143; @@ -83,6 +102,7 @@ public void onViewCreated(@NonNull View view, @Nullable Bundle savedInstanceStat handlerMain = new Handler(Looper.getMainLooper()); arCore = new ArCore(requireContext(), binding.surfaceView, handlerMain); + arCore.setGoalPlacementEnabled(true); if (vehicle == null) { mainViewModel @@ -93,13 +113,69 @@ public void onViewCreated(@NonNull View view, @Nullable Bundle savedInstanceStat if (v != null && vehicle == null) { vehicle = v; if (isAdded()) { - showStartDialog(); + setupTapToSetGoal(); } } }); return; } - showStartDialog(); + setupTapToSetGoal(); + } + + // performClick() is called on ACTION_UP; lint can't see that through a stock GLSurfaceView. + @SuppressLint("ClickableViewAccessibility") + private void setupTapToSetGoal() { + binding.surfaceView.setOnTouchListener( + (v, event) -> { + if (event.getAction() == MotionEvent.ACTION_UP) { + v.performClick(); + if (!isRunning) arCore.requestTapAt(event.getX(), event.getY()); + } + return true; + }); + binding.start.setOnClickListener(v -> startDriving()); + // Stop on finger down, not on release, so the robot stops the moment the button is touched. + binding.stop.setOnTouchListener( + (v, event) -> { + if (event.getAction() == MotionEvent.ACTION_DOWN && isRunning) { + Timber.d("nav: stopped by user"); + stop(); + showHint(getString(R.string.point_goal_tap_hint)); + } + return false; + }); + + watchdogThread = new HandlerThread("PointGoalWatchdog"); + watchdogThread.start(); + watchdogHandler = new Handler(watchdogThread.getLooper()); + + Snackbar.make(binding.getRoot(), R.string.point_goal_tap_hint, Snackbar.LENGTH_LONG).show(); + } + + @Override + public void onGoalPlaced(float distanceM) { + if (binding == null || isRunning) return; + binding.start.setVisibility(View.VISIBLE); + showHint(getString(R.string.point_goal_set, distanceM)); + } + + @Override + public void onGoalRejected(GoalValidator.Result reason) { + if (binding == null || isRunning) return; + switch (reason) { + case NOT_FLOOR: + showHint(getString(R.string.point_goal_not_floor)); + break; + case TOO_CLOSE: + showHint(getString(R.string.point_goal_too_close, GoalValidator.MIN_GOAL_DISTANCE_M)); + break; + default: + break; + } + } + + private void showHint(String message) { + Snackbar.make(binding.getRoot(), message, Snackbar.LENGTH_LONG).show(); } @Override @@ -110,15 +186,32 @@ public void onArCoreUpdate( long timestamp) { Pose targetPose = navigationPoses.getTargetPose(); Pose currentPose = navigationPoses.getCurrentPose(); + lastNavUpdateMs = SystemClock.elapsedRealtime(); + long frameAgeMs = (SystemClock.elapsedRealtimeNanos() - timestamp) / 1_000_000; + if (isRunning && frameAgeMs > MAX_FRAME_AGE_MS) { + Timber.d("nav: dropping frame that is %d ms old", frameAgeMs); + return; + } if (isRunning && vehicle != null && targetPose != null && currentPose != null) { float goalDistance = computeDistance(targetPose, currentPose); - if (goalDistance < 0.15f) { + Timber.d( + "nav: current=(%.3f, %.3f, %.3f) target=(%.3f, %.3f, %.3f) distance=%.3f", + currentPose.tx(), + currentPose.ty(), + currentPose.tz(), + targetPose.tx(), + targetPose.ty(), + targetPose.tz(), + goalDistance); + + float deltaYaw = computeDeltaYaw(currentPose, targetPose); + if (arrivalCheck.hasArrived(goalDistance, deltaYaw)) { + Timber.d("nav: goal reached at distance=%.3f deltaYaw=%.3f", goalDistance, deltaYaw); stop(); audioPlayer.playFromStringID(R.string.goal_reached); showInfoDialog(getString(R.string.goal_reached)); } else { - float deltaYaw = computeDeltaYaw(currentPose, targetPose); Bitmap bitmap = convertRGBFrameToScaledBitmap(rgb, 160.f / 480.f); bitmap = Bitmap.createBitmap(bitmap, 0, 30, 160, 90); @@ -127,7 +220,9 @@ public void onArCoreUpdate( navigationPolicy.recognizeImage( bitmap, goalDistance, (float) Math.sin(deltaYaw), (float) Math.cos(deltaYaw)); - Timber.d("control: (" + control.getLeft() + ", " + control.getRight() + ")"); + Timber.d( + "nav: deltaYaw=%.3f control=(%.3f, %.3f)", + deltaYaw, control.getLeft(), control.getRight()); vehicle.setControl(control); } } @@ -232,6 +327,7 @@ private static int YUV2RGB(int y, int u, int v) { @Override public void onArCoreTrackingFailure(long timestamp, TrackingFailureReason trackingFailureReason) { + lastNavUpdateMs = SystemClock.elapsedRealtime(); if (isRunning) { stop(); audioPlayer.playFromStringID(R.string.tracking_lost); @@ -246,12 +342,49 @@ public void onArCoreSessionPaused(long timestamp) { showInfoDialog(getString(R.string.ar_core_session_paused)); } + /** + * Runs on its own thread, so it still works when the main thread or ARCore hangs: if the + * navigation loop stops calling back while driving, the robot is stopped right away instead of + * driving on with its last command. + */ + private final Runnable watchdog = + new Runnable() { + @Override + public void run() { + if (!isRunning) return; + long silentMs = SystemClock.elapsedRealtime() - lastNavUpdateMs; + if (silentMs > WATCHDOG_TIMEOUT_MS) { + Timber.w("nav: no update for %d ms, stopping robot", silentMs); + if (vehicle != null) vehicle.stopBot(); + isRunning = false; + handlerMain.post( + () -> { + if (binding == null) return; + stop(); + showInfoDialog(getString(R.string.point_goal_watchdog)); + }); + return; + } + watchdogHandler.postDelayed(this, WATCHDOG_CHECK_MS); + } + }; + private void stop() { - arCore.detachAnchors(); + // Motors first, everything else after. + isRunning = false; if (vehicle != null) { vehicle.stopBot(); + Vehicle stoppedVehicle = vehicle; + handlerMain.postDelayed( + () -> { + if (!isRunning) stoppedVehicle.stopBot(); + }, + STOP_RESEND_MS); } - isRunning = false; + if (watchdogHandler != null) watchdogHandler.removeCallbacks(watchdog); + binding.stop.setVisibility(View.GONE); + arCore.detachAnchors(); + binding.start.setVisibility(View.GONE); } private static float computeDistance(Pose goalPose, Pose robotPose) { @@ -279,6 +412,8 @@ public void onResume() { public void onPause() { super.onPause(); + // Leaving the screen (home button, another app, closing) must never leave the robot driving. + if (isRunning) stop(); arCore.pause(); } @@ -299,6 +434,9 @@ protected void processUSBData(String data) {} public void onDestroy() { super.onDestroy(); + if (vehicle != null && isRunning) vehicle.stopBot(); + isRunning = false; + if (watchdogThread != null) watchdogThread.quitSafely(); arCore.closeSession(); } @@ -356,32 +494,6 @@ private void setupArCore() { "ARCore failure. Make sure that your device is compatible and the ARCore SDK is installed."); } - private void showStartDialog() { - if (getChildFragmentManager().findFragmentByTag(SetGoalDialogFragment.TAG) == null) { - SetGoalDialogFragment dialog = SetGoalDialogFragment.newInstance(); - dialog.setCancelable(false); - dialog.show(getChildFragmentManager(), SetGoalDialogFragment.TAG); - } - - getChildFragmentManager() - .setFragmentResultListener( - SetGoalDialogFragment.TAG, - getViewLifecycleOwner(), - (requestKey, result) -> { - Boolean start = result.getBoolean("start"); - - if (start) { - Float forward = result.getFloat("forward"); - Float left = result.getFloat("left"); - - // x: right, z: backwards - startDriving(-left, -forward); - } else { - requireActivity().onBackPressed(); - } - }); - } - private void showInfoDialog(String message) { if (getChildFragmentManager().findFragmentByTag(InfoDialogFragment.TAG) == null) { InfoDialogFragment dialog = InfoDialogFragment.newInstance(message); @@ -397,7 +509,7 @@ private void showInfoDialog(String message) { Boolean restart = result.getBoolean("restart"); if (restart) { - showStartDialog(); + showHint(getString(R.string.point_goal_tap_hint)); } else { requireActivity().onBackPressed(); } @@ -429,24 +541,15 @@ private void showPermissionDialog() { }); } - private void startDriving(float goalX, float goalZ) { - Timber.i("setting goal at (" + goalX + ", " + goalZ + ")"); - - try { - arCore.detachAnchors(); - arCore.setStartAnchorAtCurrentPose(); - - Pose startPose = arCore.getStartPose(); - if (startPose == null) { - showInfoDialog(getString(R.string.no_initial_ar_core_pose)); - return; - } - arCore.setTargetAnchor(startPose.compose(Pose.makeTranslation(goalX, 0.0f, goalZ))); - } catch (NotTrackingException e) { - e.printStackTrace(); - showInfoDialog(getString(R.string.tracking_lost)); + private void startDriving() { + if (arCore.getTargetPose() == null) { + // No goal has been tapped yet; the Start button should be hidden in this state, but + // guard against it anyway (e.g. a stale click event). return; } + arCore.freezeTargetAnchor(); + binding.start.setVisibility(View.GONE); + Model model = new Model( 0, @@ -465,6 +568,14 @@ private void startDriving(float goalX, float goalZ) { return; } + // The firmware stops the motors by itself if these stop arriving (app killed or crashed). + // USB connections already send them; Bluetooth ones don't, so start them here. + if (vehicle != null) vehicle.startHeartbeat(); + + lastNavUpdateMs = SystemClock.elapsedRealtime(); + arrivalCheck.reset(); isRunning = true; + binding.stop.setVisibility(View.VISIBLE); + watchdogHandler.postDelayed(watchdog, WATCHDOG_CHECK_MS); } } diff --git a/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/SetGoalDialogFragment.java b/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/SetGoalDialogFragment.java deleted file mode 100644 index fff28d1975..0000000000 --- a/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/SetGoalDialogFragment.java +++ /dev/null @@ -1,50 +0,0 @@ -package org.openbot.app.robot.pointGoalNavigation; - -import android.app.Dialog; -import android.os.Bundle; -import android.view.LayoutInflater; -import androidx.annotation.Nullable; -import androidx.appcompat.app.AlertDialog; -import androidx.fragment.app.DialogFragment; -import com.google.android.material.dialog.MaterialAlertDialogBuilder; -import org.openbot.app.robot.R; -import org.openbot.app.robot.databinding.SetGoalDialogViewBinding; - -public class SetGoalDialogFragment extends DialogFragment { - - public static final String TAG = SetGoalDialogFragment.class.getName(); - private AlertDialog dialog; - private SetGoalDialogViewBinding binding; - - public static SetGoalDialogFragment newInstance() { - SetGoalDialogFragment f = new SetGoalDialogFragment(); - return f; - } - - @Override - public Dialog onCreateDialog(@Nullable Bundle savedInstanceState) { - LayoutInflater inflater = requireActivity().getLayoutInflater(); - - binding = SetGoalDialogViewBinding.inflate(inflater, null, false); - - MaterialAlertDialogBuilder builder = - new MaterialAlertDialogBuilder(getActivity()) - .setTitle(R.string.set_goal_title) - .setMessage(R.string.set_goal_message) - .setView(binding.getRoot()) - .setNeutralButton(R.string.cancel, (dialogInterface, i) -> setFragmentResult(false)) - .setPositiveButton(R.string.start, (dialogInterface, i) -> setFragmentResult(true)); - - dialog = builder.create(); - - return dialog; - } - - private void setFragmentResult(boolean start) { - Bundle result = new Bundle(); - result.putBoolean("start", start); - result.putFloat("forward", Float.parseFloat(binding.forward.getText().toString())); - result.putFloat("left", Float.parseFloat(binding.left.getText().toString())); - getParentFragmentManager().setFragmentResult(TAG, result); - } -} diff --git a/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/rendering/DisplayRotationHelper.java b/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/rendering/DisplayRotationHelper.java index 4c8e1de697..f1860f923b 100644 --- a/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/rendering/DisplayRotationHelper.java +++ b/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/rendering/DisplayRotationHelper.java @@ -77,6 +77,11 @@ public void updateSessionIfNeeded(Session session) { } } + /** Returns the current display rotation as one of {@code Surface.ROTATION_0/90/180/270}. */ + public int getRotation() { + return display.getRotation(); + } + @Override public void onDisplayAdded(int displayId) {} diff --git a/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/rendering/TwoDRenderer.java b/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/rendering/TwoDRenderer.java index 4c69960768..b68f8e0adf 100644 --- a/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/rendering/TwoDRenderer.java +++ b/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/rendering/TwoDRenderer.java @@ -186,12 +186,27 @@ private int loadGLShader(String tag, int type, String source) { } public void updateModelMatrix(float[] modelMatrix, float scaleFactor) { + updateModelMatrix(modelMatrix, scaleFactor, 0f); + } + + /** + * @param rotationDegrees additional in-plane rotation around the quad's own facing axis (Z, + * local to the quad), applied before placing it via {@code modelMatrix}. Used to compensate + * for the marker rendering rotated relative to the visible camera feed in portrait. + */ + public void updateModelMatrix(float[] modelMatrix, float scaleFactor, float rotationDegrees) { float[] scaleMatrix = new float[16]; Matrix.setIdentityM(scaleMatrix, 0); scaleMatrix[0] = scaleFactor; scaleMatrix[5] = scaleFactor; scaleMatrix[10] = scaleFactor; - Matrix.multiplyMM(mModelMatrix, 0, modelMatrix, 0, scaleMatrix, 0); + + float[] rotationMatrix = new float[16]; + Matrix.setRotateM(rotationMatrix, 0, rotationDegrees, 0f, 0f, 1f); + + float[] scaledRotation = new float[16]; + Matrix.multiplyMM(scaledRotation, 0, scaleMatrix, 0, rotationMatrix, 0); + Matrix.multiplyMM(mModelMatrix, 0, modelMatrix, 0, scaledRotation, 0); } public void draw(float[] cameraView, float[] cameraPerspective) { diff --git a/robot/android/app/src/main/java/org/openbot/app/robot/vehicle/Vehicle.java b/robot/android/app/src/main/java/org/openbot/app/robot/vehicle/Vehicle.java index f5f91c3358..202a7ba7e5 100644 --- a/robot/android/app/src/main/java/org/openbot/app/robot/vehicle/Vehicle.java +++ b/robot/android/app/src/main/java/org/openbot/app/robot/vehicle/Vehicle.java @@ -454,6 +454,7 @@ public void run() { } public void startHeartbeat() { + if (heartbeatTimer != null) return; // already running, don't start a second timer heartbeatTimer = new Timer(); HeartBeatTask heartBeatTask = new HeartBeatTask(); heartbeatTimer.schedule(heartBeatTask, 250, 250); // 250ms delay and 250ms intervals diff --git a/robot/android/app/src/main/res/layout/fragment_point_goal_navigation.xml b/robot/android/app/src/main/res/layout/fragment_point_goal_navigation.xml index 565ee0401d..456b3e6262 100644 --- a/robot/android/app/src/main/res/layout/fragment_point_goal_navigation.xml +++ b/robot/android/app/src/main/res/layout/fragment_point_goal_navigation.xml @@ -11,4 +11,27 @@ android:layout_height="match_parent" android:layout_gravity="center" /> + + + + diff --git a/robot/android/app/src/main/res/layout/set_goal_dialog_view.xml b/robot/android/app/src/main/res/layout/set_goal_dialog_view.xml deleted file mode 100644 index 8874afa672..0000000000 --- a/robot/android/app/src/main/res/layout/set_goal_dialog_view.xml +++ /dev/null @@ -1,38 +0,0 @@ - - - - - - - - - - - - - - - - diff --git a/robot/android/app/src/main/res/values-de/strings.xml b/robot/android/app/src/main/res/values-de/strings.xml index 00c7486848..e3eea93417 100644 --- a/robot/android/app/src/main/res/values-de/strings.xml +++ b/robot/android/app/src/main/res/values-de/strings.xml @@ -74,6 +74,11 @@ Alle auswählen Alle löschen Tracking verloren. + Tippen Sie auf den Boden, wohin der Roboter fahren soll, und drücken Sie dann Start. + Ziel in %1$.1f m Entfernung gesetzt. Drücken Sie Start. + Das ist nicht der Boden. Tippen Sie auf den Boden, auf dem der Roboter fahren kann. + Zu nah. Tippen Sie auf eine Stelle, die mindestens %1$.1f m entfernt ist. + Der Roboter wurde angehalten, weil die App nicht mehr reagiert hat. Keine anfängliche AR-Core-Pose. AR-Core-Sitzung pausiert. Verbindungs-Timeout @@ -157,11 +162,7 @@ Lokaler Server - Ziel festlegen - Befestigen Sie das Telefon am Roboter und legen Sie ein Ziel fest. Der Roboter versucht, das Ziel nach dem Drücken von Start zu erreichen. Start - Vorwärts [m] - Links [m] Daten protokollieren Modell-​Auflösung Sensordaten diff --git a/robot/android/app/src/main/res/values-es/strings.xml b/robot/android/app/src/main/res/values-es/strings.xml index 2a06667f6e..bec397cc3e 100644 --- a/robot/android/app/src/main/res/values-es/strings.xml +++ b/robot/android/app/src/main/res/values-es/strings.xml @@ -74,6 +74,11 @@ Seleccionar todo Borrar todo Seguimiento perdido. + Toca el suelo donde quieres que vaya el robot y luego pulsa Iniciar. + Objetivo fijado a %1$.1f m. Pulsa Iniciar. + Eso no es el suelo. Toca el suelo por donde el robot pueda moverse. + Demasiado cerca. Toca un punto a al menos %1$.1f m. + El robot se detuvo porque la aplicación dejó de responder. No hay pose inicial de AR Core. Sesión de AR Core en pausa. tiempo de conexión agotado @@ -157,11 +162,7 @@ Servidor local - Establecer objetivo - Monta el teléfono en el robot y especifica un objetivo. El robot intentará alcanzar el objetivo después de pulsar Iniciar. Iniciar - Adelante [m] - Izquierda [m] Registrar datos Resolución del modelo Datos del sensor diff --git a/robot/android/app/src/main/res/values-fr/strings.xml b/robot/android/app/src/main/res/values-fr/strings.xml index 46e23baeb5..0c27d4c0ee 100644 --- a/robot/android/app/src/main/res/values-fr/strings.xml +++ b/robot/android/app/src/main/res/values-fr/strings.xml @@ -74,6 +74,11 @@ Tout sélectionner Tout effacer Suivi perdu. + Touchez le sol à l\'endroit où le robot doit aller, puis appuyez sur Démarrer. + Objectif placé à %1$.1f m. Appuyez sur Démarrer. + Ce n\'est pas le sol. Touchez le sol où le robot peut rouler. + Trop proche. Touchez un point à au moins %1$.1f m. + Le robot a été arrêté car l\'application ne répondait plus. Aucune pose initiale AR Core. Session AR Core en pause. délai de connexion dépassé @@ -157,11 +162,7 @@ Serveur local - Définir un objectif - Montez le téléphone sur le robot et définissez un objectif. Le robot essaiera d\'atteindre l\'objectif après avoir appuyé sur Démarrer. Démarrer - Avant [m] - Gauche [m] Enregistrer les données Résolution du modèle Données des capteurs diff --git a/robot/android/app/src/main/res/values-hi/strings.xml b/robot/android/app/src/main/res/values-hi/strings.xml index 776e5a516d..4c6baf754a 100644 --- a/robot/android/app/src/main/res/values-hi/strings.xml +++ b/robot/android/app/src/main/res/values-hi/strings.xml @@ -74,6 +74,11 @@ सभी चुनें सभी साफ़ करें ट्रैकिंग खो गई। + फ़र्श पर वहाँ टैप करें जहाँ रोबोट को जाना है, फिर स्टार्ट दबाएँ। + लक्ष्य %1$.1f मी दूर सेट हुआ। चलाने के लिए स्टार्ट दबाएँ। + यह फ़र्श नहीं है। फ़र्श पर वहाँ टैप करें जहाँ रोबोट चल सकता है। + बहुत पास। कम से कम %1$.1f मी दूर की जगह पर टैप करें। + ऐप ने जवाब देना बंद कर दिया, इसलिए रोबोट रोक दिया गया। कोई प्रारंभिक AR Core पोज़ नहीं। AR Core सत्र रोका गया। कनेक्ट टाइमआउट @@ -157,11 +162,7 @@ लोकल सर्वर - लक्ष्य सेट करें - फ़ोन को रोबोट पर लगाएं और एक लक्ष्य निर्दिष्ट करें। स्टार्ट दबाने के बाद रोबोट लक्ष्य तक पहुंचने की कोशिश करेगा। शुरू करें - आगे [मी] - बाएं [मी] डेटा लॉग करें मॉडल रिज़ॉल्यूशन सेंसर डेटा diff --git a/robot/android/app/src/main/res/values-ko/strings.xml b/robot/android/app/src/main/res/values-ko/strings.xml index 0d2578dd87..d3c4c08bf9 100644 --- a/robot/android/app/src/main/res/values-ko/strings.xml +++ b/robot/android/app/src/main/res/values-ko/strings.xml @@ -74,6 +74,11 @@ 전체 선택 전체 지우기 추적이 끊겼습니다. + 로봇이 갈 바닥 위치를 탭한 다음 시작을 누르세요. + %1$.1f m 거리에 목표가 설정되었습니다. 시작을 누르세요. + 바닥이 아닙니다. 로봇이 주행할 수 있는 바닥을 탭하세요. + 너무 가깝습니다. 최소 %1$.1f m 떨어진 곳을 탭하세요. + 앱이 응답하지 않아 로봇을 정지했습니다. 초기 AR Core 포즈가 없습니다. AR Core 세션이 일시 중지되었습니다. 연결 시간 초과 @@ -157,11 +162,7 @@ 로컬 서버 - 목표 설정 - 로봇에 휴대폰을 장착하고 목표를 지정하세요. 시작을 누르면 로봇이 목표에 도달하려고 시도합니다. 시작 - 전진 [m] - 왼쪽 [m] 데이터 기록 모델 해상도 센서 데이터 diff --git a/robot/android/app/src/main/res/values-zh/strings.xml b/robot/android/app/src/main/res/values-zh/strings.xml index 87bb0c5947..87d312d72d 100644 --- a/robot/android/app/src/main/res/values-zh/strings.xml +++ b/robot/android/app/src/main/res/values-zh/strings.xml @@ -74,6 +74,11 @@ 全选 全部清除 跟踪丢失。 + 点击地面上希望机器人前往的位置，然后按开始。 + 目标已设置在 %1$.1f 米外。按开始出发。 + 这不是地面。请点击机器人可以行驶的地面。 + 太近了。请点击至少 %1$.1f 米外的位置。 + 应用无响应，机器人已停止。 没有初始 AR Core 姿态。 AR Core 会话已暂停。 连接超时 @@ -157,11 +162,7 @@ 本地服务器 - 设置目标 - 将手机安装在机器人上并指定一个目标。按下开始后，机器人将尝试到达该目标。 开始 - 前进 [米] - 左侧 [米] 记录数据 模型分辨率 传感器数据 diff --git a/robot/android/app/src/main/res/values/strings.xml b/robot/android/app/src/main/res/values/strings.xml index 96bf27b216..dd71b75f66 100644 --- a/robot/android/app/src/main/res/values/strings.xml +++ b/robot/android/app/src/main/res/values/strings.xml @@ -94,8 +94,14 @@ Select All Clear All Tracking lost. + Tap on the floor where you want the robot to go, then press Start. + Goal set %1$.1f m away. Press Start to drive. + That\'s not the floor. Tap on the floor where the robot can drive. + Too close. Tap a spot at least %1$.1f m away. + The robot was stopped because the app stopped responding. No initial AR Core pose. AR Core session paused. + Start connect timeout connect fail connection has disconnected @@ -236,11 +242,6 @@ Model Management Default - Set Goal - Mount the phone on the robot and specify a goal. The robot will try to reach the goal after pressing start. - Start - Forward [m] - Left [m] Log Data Model Resolution Sensor Data diff --git a/robot/android/app/src/test/java/org/openbot/app/robot/pointGoalNavigation/ArrivalCheckTest.java b/robot/android/app/src/test/java/org/openbot/app/robot/pointGoalNavigation/ArrivalCheckTest.java new file mode 100644 index 0000000000..b0648faa92 --- /dev/null +++ b/robot/android/app/src/test/java/org/openbot/app/robot/pointGoalNavigation/ArrivalCheckTest.java @@ -0,0 +1,65 @@ +package org.openbot.app.robot.pointGoalNavigation; + +import static org.junit.Assert.assertFalse; +import static org.junit.Assert.assertTrue; + +import org.junit.Before; +import org.junit.Test; + +public class ArrivalCheckTest { + + private ArrivalCheck check; + + @Before + public void setUp() { + check = new ArrivalCheck(); + } + + @Test + public void farAway_notArrived() { + assertFalse(check.hasArrived(2f, 0f)); + } + + @Test + public void withinTenCentimeters_arrived() { + assertTrue(check.hasArrived(0.09f, 0f)); + } + + @Test + public void closeAndStillAhead_keepsDriving() { + assertFalse(check.hasArrived(0.25f, 0.2f)); + assertFalse(check.hasArrived(0.18f, 0.3f)); + } + + @Test + public void passedTheGoalSideways_arrived() { + // 20 cm away, goal now beside the robot: it went past, don't turn back. + assertTrue(check.hasArrived(0.20f, (float) Math.toRadians(100))); + } + + @Test + public void startingWithGoalBehindFarAway_keepsDriving() { + // A goal behind the robot at the start is a turn, not an arrival. + assertFalse(check.hasArrived(1.5f, (float) Math.PI)); + } + + @Test + public void gettingFurtherAwayAfterClosestPoint_arrived() { + assertFalse(check.hasArrived(0.22f, 0.4f)); + assertFalse(check.hasArrived(0.15f, 0.8f)); + assertTrue(check.hasArrived(0.19f, 0.9f)); + } + + @Test + public void smallNoiseNearClosestPoint_isNotMovingAway() { + assertFalse(check.hasArrived(0.20f, 0.2f)); + assertFalse(check.hasArrived(0.21f, 0.2f)); + } + + @Test + public void reset_forgetsPreviousRun() { + check.hasArrived(0.12f, 0f); + check.reset(); + assertFalse(check.hasArrived(0.2f, 0f)); + } +} diff --git a/robot/android/app/src/test/java/org/openbot/app/robot/pointGoalNavigation/GoalValidatorTest.java b/robot/android/app/src/test/java/org/openbot/app/robot/pointGoalNavigation/GoalValidatorTest.java new file mode 100644 index 0000000000..2404158c54 --- /dev/null +++ b/robot/android/app/src/test/java/org/openbot/app/robot/pointGoalNavigation/GoalValidatorTest.java @@ -0,0 +1,117 @@ +package org.openbot.app.robot.pointGoalNavigation; + +import static org.junit.Assert.assertEquals; +import static org.junit.Assert.assertNull; + +import java.util.Arrays; +import java.util.Collections; +import org.junit.Test; +import org.openbot.app.robot.pointGoalNavigation.GoalValidator.Result; + +public class GoalValidatorTest { + + // Phone mounted on the robot ~15 cm above the floor, world y is up. + private static final float[] CAMERA = {0f, 0f, 0f}; + + @Test + public void floorTwoMetersAhead_isAccepted() { + assertEquals(Result.OK, GoalValidator.validate(CAMERA, new float[] {0f, -0.15f, -2f})); + } + + @Test + public void farFloorAcrossTheRoom_isAccepted() { + assertEquals(Result.OK, GoalValidator.validate(CAMERA, new float[] {3f, -0.15f, -4f})); + } + + @Test + public void tapNotReachingTheFloor_isNotFloor() { + assertEquals(Result.NOT_FLOOR, GoalValidator.validate(CAMERA, null)); + } + + @Test + public void spotThirtyCentimetersAway_measuresThirtyCentimeters() { + // Camera 12 cm up; a floor spot 0.3 m ahead is atan(0.12 / 0.3) = 21.8 deg below horizon. + double angle = Math.atan2(0.12, 0.3); + float[] direction = {0f, (float) -Math.sin(angle), (float) -Math.cos(angle)}; + float[] hit = GoalValidator.intersectFloor(new float[] {0f, 0.12f, 0f}, direction, 0f); + assertEquals(0.3f, GoalValidator.horizontalDistance(new float[] {0, 0.12f, 0}, hit), 1e-4f); + assertEquals(Result.OK, GoalValidator.validate(new float[] {0, 0.12f, 0}, hit)); + } + + @Test + public void spotTwoMetersToTheSide_measuresTwoMeters() { + double angle = Math.atan2(0.12, 2.0); + float[] direction = {(float) Math.cos(angle), (float) -Math.sin(angle), 0f}; + float[] hit = GoalValidator.intersectFloor(new float[] {0f, 0.12f, 0f}, direction, 0f); + assertEquals(2f, hit[0], 1e-3f); + assertEquals(0f, hit[2], 1e-5f); + } + + @Test + public void farSpotBeyondTrustedRange_isRejected() { + double angle = Math.atan2(0.12, 8.0); + float[] direction = {0f, (float) -Math.sin(angle), (float) -Math.cos(angle)}; + assertNull(GoalValidator.intersectFloor(new float[] {0f, 0.12f, 0f}, direction, 0f)); + } + + @Test + public void windowOrBuildingAboveCamera_isNotFloor() { + assertEquals(Result.NOT_FLOOR, GoalValidator.validate(CAMERA, new float[] {0f, 3f, -40f})); + } + + @Test + public void pointAtCameraHeight_isNotFloor() { + assertEquals(Result.NOT_FLOOR, GoalValidator.validate(CAMERA, new float[] {0f, 0f, -2f})); + } + + @Test + public void goalUnderTheRobot_isTooClose() { + assertEquals(Result.TOO_CLOSE, GoalValidator.validate(CAMERA, new float[] {0f, -0.15f, -0.2f})); + } + + @Test + public void rayDownToTheFloor_hitsAtTheRightDistance() { + // Camera 0.15 m up, looking 45 degrees down: hits the floor 0.15 m ahead. + float[] hit = + GoalValidator.intersectFloor( + new float[] {0f, 0.15f, 0f}, new float[] {0f, -0.7071f, -0.7071f}, 0f); + assertEquals(0f, hit[1], 1e-5f); + assertEquals(-0.15f, hit[2], 1e-4f); + } + + @Test + public void rayLevelOrUp_neverHitsTheFloor() { + assertNull(GoalValidator.intersectFloor(new float[] {0, 0.15f, 0}, new float[] {0, 0, -1}, 0f)); + assertNull( + GoalValidator.intersectFloor(new float[] {0, 0.15f, 0}, new float[] {0, 0.5f, -1}, 0f)); + } + + @Test + public void rayAlmostLevel_isTooFarToTrust() { + assertNull( + GoalValidator.intersectFloor(new float[] {0, 0.15f, 0}, new float[] {0, -0.001f, -1}, 0f)); + } + + @Test + public void distanceIgnoresHeight() { + float d = GoalValidator.horizontalDistance(new float[] {0, 0, 0}, new float[] {3, -5, 4}); + assertEquals(5f, d, 1e-5f); + } + + @Test + public void floorIsLargestPlaneBelowCamera() { + Float floorY = + GoalValidator.pickFloorY( + Arrays.asList( + new float[] {-0.15f, 6f}, // floor + new float[] {0.45f, 1f}, // table above the camera + new float[] {-0.5f, 0.5f}), // small reflection under a shiny floor + 0f); + assertEquals(-0.15f, floorY, 1e-5f); + } + + @Test + public void noPlanes_noFloor() { + assertNull(GoalValidator.pickFloorY(Collections.emptyList(), 0f)); + } +} From 8508b1e923027d4bd238308e7a842f16950ef4a8 Mon Sep 17 00:00:00 2001 From: Harish Thakur Date: Fri, 23 Oct 2026 11:45:14 +0530 Subject: [PATCH 2/3] Stabilize point-goal and translate error messages --- .../app/robot/pointGoalNavigation/ArCore.java | 46 ++++++----------- .../PointGoalNavigationFragment.java | 50 +++++++++---------- .../rendering/DisplayRotationHelper.java | 5 -- .../rendering/TwoDRenderer.java | 17 +------ .../openbot/app/robot/vehicle/Vehicle.java | 1 - .../app/src/main/res/values-de/strings.xml | 2 + .../app/src/main/res/values-es/strings.xml | 2 + .../app/src/main/res/values-fr/strings.xml | 2 + .../app/src/main/res/values-hi/strings.xml | 2 + .../app/src/main/res/values-ko/strings.xml | 2 + .../app/src/main/res/values-zh/strings.xml | 2 + .../app/src/main/res/values/strings.xml | 2 + 12 files changed, 55 insertions(+), 78 deletions(-) diff --git a/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/ArCore.java b/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/ArCore.java index 01b0c3e546..26552c7064 100644 --- a/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/ArCore.java +++ b/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/ArCore.java @@ -59,14 +59,16 @@ public class ArCore implements GLSurfaceView.Renderer { private volatile Pose currentPose; private ArCoreListener arCoreListener = null; private final float[] anchorMatrix = new float[16]; - private Anchor startAnchor, targetAnchor; + private Anchor startAnchor; + // Set on the GL thread (tap) and cleared on the UI thread (Stop), so read it once into a local. + private volatile Anchor targetAnchor; private Handler handlerMain; private GLSurfaceView surfaceView; private boolean renderFrame = true; private Pose startPose = null, targetPose = null; // Written on the UI thread, consumed on the GL thread. private volatile float[] pendingTap = null; - // Floor and wall detection cost CPU, so only the tap-to-goal screen turns them on. + // Floor detection costs CPU, so only the tap-to-goal screen turns it on. private boolean goalPlacementEnabled = false; // The marker quad reaches this far below its center; lift it so the pin tip touches the floor. private static final float MARKER_TIP_OFFSET_M = 0.1f; @@ -135,7 +137,8 @@ public void setStartAnchorAtCurrentPose() { } public Pose getTargetPose() { - return targetAnchor == null ? null : targetAnchor.getPose(); + Anchor target = targetAnchor; + return target == null ? null : target.getPose(); } public void setTargetAnchor(Pose pose) { @@ -146,24 +149,6 @@ public void setTargetAnchorAtCurrentPose() { targetAnchor = session.createAnchor(currentPose); } - /** - * Replaces the current target anchor with a plain anchor at its current pose. Anchors created - * from a hit result stay attached to their plane and move as ARCore refines it, which would make - * the goal drift while driving. A plain {@code session.createAnchor(pose)} anchor stays fixed. - */ - public void freezeTargetAnchor() { - if (targetAnchor == null) { - Timber.d("freezeTargetAnchor: no target anchor to freeze"); - return; - } - Pose frozenPose = targetAnchor.getPose(); - targetAnchor.detach(); - targetAnchor = session.createAnchor(frozenPose); - Timber.d( - "freezeTargetAnchor: frozen at (%.3f, %.3f, %.3f)", - frozenPose.tx(), frozenPose.ty(), frozenPose.tz()); - } - /** * Requests that the given screen-space tap (in {@link #surfaceView} pixel coordinates) be turned * into a goal pose on the next rendered frame. The actual hit-test runs on the GL thread inside @@ -173,7 +158,7 @@ public void requestTapAt(float screenX, float screenY) { pendingTap = new float[] {screenX, screenY}; } - /** Turns on floor/wall detection and depth for tap-to-goal. Call before {@link #resume()}. */ + /** Turns on floor detection for tap-to-goal (camera height). Call before {@link #resume()}. */ public void setGoalPlacementEnabled(boolean enabled) { goalPlacementEnabled = enabled; cameraHeightM = @@ -254,8 +239,9 @@ public void onDrawFrame(GL10 gl) { if (startAnchor != null) { startPose = startAnchor.getPose(); } - if (targetAnchor != null) { - targetPose = targetAnchor.getPose(); + Anchor target = targetAnchor; + if (target != null) { + targetPose = target.getPose(); } if (goalPlacementEnabled && frameCount++ % CALIBRATE_EVERY_FRAMES == 0) { @@ -309,7 +295,8 @@ public void onDrawFrame(GL10 gl) { backgroundRenderer.draw(frame); // visualize goal - if (targetAnchor != null && trackingState == TrackingState.TRACKING) { + Anchor marker = targetAnchor; + if (marker != null && trackingState == TrackingState.TRACKING) { // Get projection matrix. float[] projmtx = new float[16]; camera.getProjectionMatrix(projmtx, 0, 0.1f, 100.0f); @@ -327,7 +314,7 @@ public void onDrawFrame(GL10 gl) { // Stand the pin upright in the world (local +y = world up) and turn it about the // vertical axis to face the camera, so it looks right however the phone is mounted: // portrait, landscape or upside down. Its tip sits on the goal spot on the floor. - float[] goal = targetAnchor.getPose().getTranslation(); + float[] goal = marker.getPose().getTranslation(); float yaw = (float) Math.atan2(currentPose.tx() - goal[0], currentPose.tz() - goal[2]); Pose markerPose = new Pose( @@ -373,8 +360,9 @@ private void placeGoalFromTap(float tapX, float tapY) { try { // A plain anchor at a world position stays fixed; it doesn't follow the screen. Anchor newAnchor = session.createAnchor(Pose.makeTranslation(goal[0], goal[1], goal[2])); - if (targetAnchor != null) targetAnchor.detach(); + Anchor previous = targetAnchor; targetAnchor = newAnchor; + if (previous != null) previous.detach(); targetPose = newAnchor.getPose(); float distance = GoalValidator.horizontalDistance(cameraPos, goal); runOnMainThread( @@ -408,10 +396,6 @@ private void calibrateCameraHeight() { cameraHeightMeasured = true; } - public float getCameraHeight() { - return cameraHeightM; - } - /** World-space ray {origin, direction} through a screen pixel. */ private float[][] tapRay(float tapX, float tapY) { int width = surfaceView.getWidth(); diff --git a/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/PointGoalNavigationFragment.java b/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/PointGoalNavigationFragment.java index 5becd01782..ad69965f71 100644 --- a/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/PointGoalNavigationFragment.java +++ b/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/PointGoalNavigationFragment.java @@ -82,7 +82,6 @@ public static PointGoalNavigationFragment newInstance() { @Override public void onCreate(Bundle savedInstanceState) { super.onCreate(savedInstanceState); - if (getArguments() != null) {} } @Override @@ -359,7 +358,8 @@ public void run() { isRunning = false; handlerMain.post( () -> { - if (binding == null) return; + // The motors are already stopped above; skip the UI if the screen is going away. + if (binding == null || !isAdded() || getView() == null) return; stop(); showInfoDialog(getString(R.string.point_goal_watchdog)); }); @@ -437,6 +437,10 @@ public void onDestroy() { if (vehicle != null && isRunning) vehicle.stopBot(); isRunning = false; if (watchdogThread != null) watchdogThread.quitSafely(); + if (navigationPolicy != null) { + navigationPolicy.close(); + navigationPolicy = null; + } arCore.closeSession(); } @@ -490,8 +494,7 @@ private void setupArCore() { e.printStackTrace(); } - showInfoDialog( - "ARCore failure. Make sure that your device is compatible and the ARCore SDK is installed."); + showInfoDialog(getString(R.string.point_goal_arcore_failure)); } private void showInfoDialog(String message) { @@ -547,31 +550,28 @@ private void startDriving() { // guard against it anyway (e.g. a stale click event). return; } - arCore.freezeTargetAnchor(); binding.start.setVisibility(View.GONE); - Model model = - new Model( - 0, - CLASS.NAVIGATION, - TYPE.GOALNAV, - "navigation.tflite", - PATH_TYPE.ASSET, - "networks/navigation.tflite", - "160x90"); - - try { - navigationPolicy = new Navigation(requireActivity(), model, Device.CPU, 1); - } catch (IOException e) { - e.printStackTrace(); - showInfoDialog("Navigation policy could not be initialized."); - return; + // Load the model once and reuse it; loading it on every Start leaked a copy each time. + if (navigationPolicy == null) { + Model model = + new Model( + 0, + CLASS.NAVIGATION, + TYPE.GOALNAV, + "navigation.tflite", + PATH_TYPE.ASSET, + "networks/navigation.tflite", + "160x90"); + try { + navigationPolicy = new Navigation(requireActivity(), model, Device.CPU, 1); + } catch (IOException e) { + e.printStackTrace(); + showInfoDialog(getString(R.string.point_goal_model_failure)); + return; + } } - // The firmware stops the motors by itself if these stop arriving (app killed or crashed). - // USB connections already send them; Bluetooth ones don't, so start them here. - if (vehicle != null) vehicle.startHeartbeat(); - lastNavUpdateMs = SystemClock.elapsedRealtime(); arrivalCheck.reset(); isRunning = true; diff --git a/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/rendering/DisplayRotationHelper.java b/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/rendering/DisplayRotationHelper.java index f1860f923b..4c8e1de697 100644 --- a/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/rendering/DisplayRotationHelper.java +++ b/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/rendering/DisplayRotationHelper.java @@ -77,11 +77,6 @@ public void updateSessionIfNeeded(Session session) { } } - /** Returns the current display rotation as one of {@code Surface.ROTATION_0/90/180/270}. */ - public int getRotation() { - return display.getRotation(); - } - @Override public void onDisplayAdded(int displayId) {} diff --git a/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/rendering/TwoDRenderer.java b/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/rendering/TwoDRenderer.java index b68f8e0adf..4c69960768 100644 --- a/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/rendering/TwoDRenderer.java +++ b/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/rendering/TwoDRenderer.java @@ -186,27 +186,12 @@ private int loadGLShader(String tag, int type, String source) { } public void updateModelMatrix(float[] modelMatrix, float scaleFactor) { - updateModelMatrix(modelMatrix, scaleFactor, 0f); - } - - /** - * @param rotationDegrees additional in-plane rotation around the quad's own facing axis (Z, - * local to the quad), applied before placing it via {@code modelMatrix}. Used to compensate - * for the marker rendering rotated relative to the visible camera feed in portrait. - */ - public void updateModelMatrix(float[] modelMatrix, float scaleFactor, float rotationDegrees) { float[] scaleMatrix = new float[16]; Matrix.setIdentityM(scaleMatrix, 0); scaleMatrix[0] = scaleFactor; scaleMatrix[5] = scaleFactor; scaleMatrix[10] = scaleFactor; - - float[] rotationMatrix = new float[16]; - Matrix.setRotateM(rotationMatrix, 0, rotationDegrees, 0f, 0f, 1f); - - float[] scaledRotation = new float[16]; - Matrix.multiplyMM(scaledRotation, 0, scaleMatrix, 0, rotationMatrix, 0); - Matrix.multiplyMM(mModelMatrix, 0, modelMatrix, 0, scaledRotation, 0); + Matrix.multiplyMM(mModelMatrix, 0, modelMatrix, 0, scaleMatrix, 0); } public void draw(float[] cameraView, float[] cameraPerspective) { diff --git a/robot/android/app/src/main/java/org/openbot/app/robot/vehicle/Vehicle.java b/robot/android/app/src/main/java/org/openbot/app/robot/vehicle/Vehicle.java index 202a7ba7e5..f5f91c3358 100644 --- a/robot/android/app/src/main/java/org/openbot/app/robot/vehicle/Vehicle.java +++ b/robot/android/app/src/main/java/org/openbot/app/robot/vehicle/Vehicle.java @@ -454,7 +454,6 @@ public void run() { } public void startHeartbeat() { - if (heartbeatTimer != null) return; // already running, don't start a second timer heartbeatTimer = new Timer(); HeartBeatTask heartBeatTask = new HeartBeatTask(); heartbeatTimer.schedule(heartBeatTask, 250, 250); // 250ms delay and 250ms intervals diff --git a/robot/android/app/src/main/res/values-de/strings.xml b/robot/android/app/src/main/res/values-de/strings.xml index e3eea93417..8739f30de3 100644 --- a/robot/android/app/src/main/res/values-de/strings.xml +++ b/robot/android/app/src/main/res/values-de/strings.xml @@ -79,6 +79,8 @@ Das ist nicht der Boden. Tippen Sie auf den Boden, auf dem der Roboter fahren kann. Zu nah. Tippen Sie auf eine Stelle, die mindestens %1$.1f m entfernt ist. Der Roboter wurde angehalten, weil die App nicht mehr reagiert hat. + ARCore-Fehler. Stellen Sie sicher, dass Ihr Gerät kompatibel ist und ARCore installiert ist. + Die Navigationssteuerung konnte nicht initialisiert werden. Keine anfängliche AR-Core-Pose. AR-Core-Sitzung pausiert. Verbindungs-Timeout diff --git a/robot/android/app/src/main/res/values-es/strings.xml b/robot/android/app/src/main/res/values-es/strings.xml index bec397cc3e..7c3c90c779 100644 --- a/robot/android/app/src/main/res/values-es/strings.xml +++ b/robot/android/app/src/main/res/values-es/strings.xml @@ -79,6 +79,8 @@ Eso no es el suelo. Toca el suelo por donde el robot pueda moverse. Demasiado cerca. Toca un punto a al menos %1$.1f m. El robot se detuvo porque la aplicación dejó de responder. + Error de ARCore. Asegúrate de que tu dispositivo sea compatible y de que ARCore esté instalado. + No se pudo inicializar la política de navegación. No hay pose inicial de AR Core. Sesión de AR Core en pausa. tiempo de conexión agotado diff --git a/robot/android/app/src/main/res/values-fr/strings.xml b/robot/android/app/src/main/res/values-fr/strings.xml index 0c27d4c0ee..95c9535ca6 100644 --- a/robot/android/app/src/main/res/values-fr/strings.xml +++ b/robot/android/app/src/main/res/values-fr/strings.xml @@ -79,6 +79,8 @@ Ce n\'est pas le sol. Touchez le sol où le robot peut rouler. Trop proche. Touchez un point à au moins %1$.1f m. Le robot a été arrêté car l\'application ne répondait plus. + Erreur ARCore. Vérifiez que votre appareil est compatible et qu\'ARCore est installé. + Impossible d\'initialiser la politique de navigation. Aucune pose initiale AR Core. Session AR Core en pause. délai de connexion dépassé diff --git a/robot/android/app/src/main/res/values-hi/strings.xml b/robot/android/app/src/main/res/values-hi/strings.xml index 4c6baf754a..5318ece33e 100644 --- a/robot/android/app/src/main/res/values-hi/strings.xml +++ b/robot/android/app/src/main/res/values-hi/strings.xml @@ -79,6 +79,8 @@ यह फ़र्श नहीं है। फ़र्श पर वहाँ टैप करें जहाँ रोबोट चल सकता है। बहुत पास। कम से कम %1$.1f मी दूर की जगह पर टैप करें। ऐप ने जवाब देना बंद कर दिया, इसलिए रोबोट रोक दिया गया। + ARCore त्रुटि। सुनिश्चित करें कि आपका डिवाइस संगत है और ARCore इंस्टॉल है। + नेविगेशन पॉलिसी शुरू नहीं हो सकी। कोई प्रारंभिक AR Core पोज़ नहीं। AR Core सत्र रोका गया। कनेक्ट टाइमआउट diff --git a/robot/android/app/src/main/res/values-ko/strings.xml b/robot/android/app/src/main/res/values-ko/strings.xml index d3c4c08bf9..4218b22ed4 100644 --- a/robot/android/app/src/main/res/values-ko/strings.xml +++ b/robot/android/app/src/main/res/values-ko/strings.xml @@ -79,6 +79,8 @@ 바닥이 아닙니다. 로봇이 주행할 수 있는 바닥을 탭하세요. 너무 가깝습니다. 최소 %1$.1f m 떨어진 곳을 탭하세요. 앱이 응답하지 않아 로봇을 정지했습니다. + ARCore 오류입니다. 기기가 호환되고 ARCore가 설치되어 있는지 확인하세요. + 내비게이션 정책을 초기화할 수 없습니다. 초기 AR Core 포즈가 없습니다. AR Core 세션이 일시 중지되었습니다. 연결 시간 초과 diff --git a/robot/android/app/src/main/res/values-zh/strings.xml b/robot/android/app/src/main/res/values-zh/strings.xml index 87d312d72d..9170ebff95 100644 --- a/robot/android/app/src/main/res/values-zh/strings.xml +++ b/robot/android/app/src/main/res/values-zh/strings.xml @@ -79,6 +79,8 @@ 这不是地面。请点击机器人可以行驶的地面。 太近了。请点击至少 %1$.1f 米外的位置。 应用无响应，机器人已停止。 + ARCore 出错。请确认您的设备兼容并已安装 ARCore。 + 无法初始化导航策略。 没有初始 AR Core 姿态。 AR Core 会话已暂停。 连接超时 diff --git a/robot/android/app/src/main/res/values/strings.xml b/robot/android/app/src/main/res/values/strings.xml index dd71b75f66..6a5a3c240d 100644 --- a/robot/android/app/src/main/res/values/strings.xml +++ b/robot/android/app/src/main/res/values/strings.xml @@ -99,6 +99,8 @@ That\'s not the floor. Tap on the floor where the robot can drive. Too close. Tap a spot at least %1$.1f m away. The robot was stopped because the app stopped responding. + ARCore failure. Make sure that your device is compatible and the ARCore SDK is installed. + Navigation policy could not be initialized. No initial AR Core pose. AR Core session paused. Start From f287068f8da7e22f1976c5a4e99355dc4bf610cf Mon Sep 17 00:00:00 2001 From: Harish Thakur Date: Mon, 26 Oct 2026 13:59:17 +0530 Subject: [PATCH 3/3] Address review: safety timer, camera height, untracked taps --- .../app/robot/pointGoalNavigation/ArCore.java | 64 ++++++++++++------- .../pointGoalNavigation/GoalValidator.java | 18 +++++- .../PointGoalNavigationFragment.java | 51 ++++++++++++--- .../app/src/main/res/values-de/strings.xml | 1 + .../app/src/main/res/values-es/strings.xml | 1 + .../app/src/main/res/values-fr/strings.xml | 1 + .../app/src/main/res/values-hi/strings.xml | 1 + .../app/src/main/res/values-ko/strings.xml | 1 + .../app/src/main/res/values-zh/strings.xml | 1 + .../app/src/main/res/values/strings.xml | 1 + .../GoalValidatorTest.java | 10 +++ 11 files changed, 116 insertions(+), 34 deletions(-) diff --git a/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/ArCore.java b/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/ArCore.java index 26552c7064..20cd0ae450 100644 --- a/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/ArCore.java +++ b/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/ArCore.java @@ -73,13 +73,11 @@ public class ArCore implements GLSurfaceView.Renderer { // The marker quad reaches this far below its center; lift it so the pin tip touches the floor. private static final float MARKER_TIP_OFFSET_M = 0.1f; - private static final String PREFS = "point_goal_navigation"; - private static final String PREF_CAMERA_HEIGHT = "camera_height_m"; private static final int CALIBRATE_EVERY_FRAMES = 15; - // Height of the phone camera above the floor. Distances come from it, so it is re-measured - // from ARCore's floor plane whenever one is found and remembered for next time. - private float cameraHeightM; - private boolean cameraHeightMeasured = false; + // Re-measured from ARCore's floor plane; never saved. + // Written by the UI thread and the GL thread. + private volatile float cameraHeightM = GoalValidator.DEFAULT_CAMERA_HEIGHT_M; + private volatile boolean cameraHeightMeasured = false; private int frameCount = 0; public ArCore(Context context, GLSurfaceView surfaceView, Handler handlerMain) { @@ -131,7 +129,7 @@ public Pose getStartPose() { } public void setStartAnchorAtCurrentPose() { - if (currentPose != null) { + if (session != null && currentPose != null) { startAnchor = session.createAnchor(currentPose); } } @@ -142,10 +140,12 @@ public Pose getTargetPose() { } public void setTargetAnchor(Pose pose) { + if (session == null) return; targetAnchor = session.createAnchor(pose); } public void setTargetAnchorAtCurrentPose() { + if (session == null) return; targetAnchor = session.createAnchor(currentPose); } @@ -161,10 +161,11 @@ public void requestTapAt(float screenX, float screenY) { /** Turns on floor detection for tap-to-goal (camera height). Call before {@link #resume()}. */ public void setGoalPlacementEnabled(boolean enabled) { goalPlacementEnabled = enabled; - cameraHeightM = - appContext - .getSharedPreferences(PREFS, Context.MODE_PRIVATE) - .getFloat(PREF_CAMERA_HEIGHT, GoalValidator.DEFAULT_CAMERA_HEIGHT_M); + } + + /** Used until the floor is measured. */ + public void setStartingCameraHeight(float heightM) { + if (!cameraHeightMeasured) cameraHeightM = heightM; } @Override @@ -224,6 +225,12 @@ public void onDrawFrame(GL10 gl) { if (trackingState != TrackingState.TRACKING) { Timber.d("ARCore is not tracking."); + // No pose yet: reject the tap. + if (pendingTap != null) { + pendingTap = null; + notifyRejected(GoalValidator.Result.NOT_TRACKING); + } + TrackingFailureReason trackingFailureReason = camera.getTrackingFailureReason(); runOnMainThread( () -> { @@ -349,7 +356,7 @@ private void placeGoalFromTap(float tapX, float tapY) { tapY, Arrays.toString(cameraPos), cameraHeightM, - cameraHeightMeasured ? "measured" : "saved", + cameraHeightMeasured ? "measured" : "starting", Arrays.toString(goal), result); if (result != GoalValidator.Result.OK) { @@ -386,11 +393,6 @@ private void calibrateCameraHeight() { } if (!cameraHeightMeasured || Math.abs(height - cameraHeightM) > 0.005f) { Timber.d("camera height measured: %.3f m", height); - appContext - .getSharedPreferences(PREFS, Context.MODE_PRIVATE) - .edit() - .putFloat(PREF_CAMERA_HEIGHT, height) - .apply(); } cameraHeightM = height; cameraHeightMeasured = true; @@ -469,10 +471,18 @@ public void resume() throws UnavailableSdkTooOldException, UnavailableDeviceNotCompatibleException, UnavailableArcoreNotInstalledException, UnavailableApkTooOldException, CameraNotAvailableException { - session = new Session(appContext); - - setConfig(); - setCameraConfig(); + // Reuse the session; two would fight over the camera. + if (session == null) { + session = new Session(appContext); + try { + setConfig(); + setCameraConfig(); + } catch (RuntimeException | CameraNotAvailableException e) { + session.close(); + session = null; + throw e; + } + } // Note that order matters - see the note in onPause(), the reverse applies here. session.resume(); @@ -599,20 +609,26 @@ public void closeSession() { // thread. // Reference: https://developers.google.com/ar/reference/java/com/google/ar/core/Session#close() if (session != null) { + Session closing = session; runOnMainThread( () -> { + // Already closed by an earlier call. + if (session != closing) return; pause(); + session = null; HandlerThread handlerThread = new HandlerThread("ArCore Session Close Handler"); handlerThread.start(); Handler sessionCloseHandler = new Handler(handlerThread.getLooper()); - sessionCloseHandler.post(() -> session.close()); + sessionCloseHandler.post(closing::close); }); } } public void detachAnchors() { - for (Anchor anchor : session.getAllAnchors()) { - anchor.detach(); + if (session != null) { + for (Anchor anchor : session.getAllAnchors()) { + anchor.detach(); + } } startAnchor = null; diff --git a/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/GoalValidator.java b/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/GoalValidator.java index 3099c55a85..db4af23270 100644 --- a/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/GoalValidator.java +++ b/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/GoalValidator.java @@ -15,6 +15,21 @@ public final class GoalValidator { /** Phone camera height on an OpenBot DIY body, until it has been measured. */ public static final float DEFAULT_CAMERA_HEIGHT_M = 0.12f; + /** Starting camera height per robot body, until the floor is measured (about +-3 cm). */ + public static float startingCameraHeight(@Nullable String vehicleType) { + if (vehicleType == null) return DEFAULT_CAMERA_HEIGHT_M; + switch (vehicleType) { + case "RTR_520": + return 0.16f; + case "RC_CAR": + return 0.22f; + case "MTV": + return 0.23f; + default: + return DEFAULT_CAMERA_HEIGHT_M; + } + } + /** Measured heights outside this range mean the phone isn't on the robot right now. */ public static final float MIN_MOUNT_HEIGHT_M = 0.04f; @@ -32,7 +47,8 @@ public final class GoalValidator { public enum Result { OK, NOT_FLOOR, - TOO_CLOSE + TOO_CLOSE, + NOT_TRACKING } private GoalValidator() {} diff --git a/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/PointGoalNavigationFragment.java b/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/PointGoalNavigationFragment.java index ad69965f71..ff922689c0 100644 --- a/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/PointGoalNavigationFragment.java +++ b/robot/android/app/src/main/java/org/openbot/app/robot/pointGoalNavigation/PointGoalNavigationFragment.java @@ -9,10 +9,12 @@ import android.os.HandlerThread; import android.os.Looper; import android.os.SystemClock; +import android.view.Gravity; import android.view.LayoutInflater; import android.view.MotionEvent; import android.view.View; import android.view.ViewGroup; +import android.widget.FrameLayout; import androidx.activity.result.ActivityResultLauncher; import androidx.activity.result.contract.ActivityResultContracts; import androidx.annotation.NonNull; @@ -49,6 +51,7 @@ public class PointGoalNavigationFragment extends ControlsFragment implements ArC private Handler handlerMain; private ArCore arCore; private FragmentPointGoalNavigationBinding binding; + private Snackbar hintSnackbar; // Read by the watchdog thread. private volatile boolean isRunning = false; private volatile long lastNavUpdateMs = 0; @@ -82,6 +85,11 @@ public static PointGoalNavigationFragment newInstance() { @Override public void onCreate(Bundle savedInstanceState) { super.onCreate(savedInstanceState); + + // Fragment-scoped: started and quit once. + watchdogThread = new HandlerThread("PointGoalWatchdog"); + watchdogThread.start(); + watchdogHandler = new Handler(watchdogThread.getLooper()); } @Override @@ -128,7 +136,12 @@ private void setupTapToSetGoal() { (v, event) -> { if (event.getAction() == MotionEvent.ACTION_UP) { v.performClick(); - if (!isRunning) arCore.requestTapAt(event.getX(), event.getY()); + if (!isRunning) { + arCore.setStartingCameraHeight( + GoalValidator.startingCameraHeight( + vehicle == null ? null : vehicle.getVehicleType())); + arCore.requestTapAt(event.getX(), event.getY()); + } } return true; }); @@ -144,11 +157,7 @@ private void setupTapToSetGoal() { return false; }); - watchdogThread = new HandlerThread("PointGoalWatchdog"); - watchdogThread.start(); - watchdogHandler = new Handler(watchdogThread.getLooper()); - - Snackbar.make(binding.getRoot(), R.string.point_goal_tap_hint, Snackbar.LENGTH_LONG).show(); + showHint(getString(R.string.point_goal_tap_hint)); } @Override @@ -168,13 +177,29 @@ public void onGoalRejected(GoalValidator.Result reason) { case TOO_CLOSE: showHint(getString(R.string.point_goal_too_close, GoalValidator.MIN_GOAL_DISTANCE_M)); break; + case NOT_TRACKING: + showHint(getString(R.string.point_goal_not_tracking)); + break; default: break; } } private void showHint(String message) { - Snackbar.make(binding.getRoot(), message, Snackbar.LENGTH_LONG).show(); + if (binding == null) return; + hintSnackbar = Snackbar.make(binding.getRoot(), message, Snackbar.LENGTH_LONG); + hintSnackbar.setAnimationMode(Snackbar.ANIMATION_MODE_FADE); + View view = hintSnackbar.getView(); + if (view.getLayoutParams() instanceof FrameLayout.LayoutParams) { + // Top of this screen, which is already below the status bar. + int[] rootLocation = new int[2]; + binding.getRoot().getLocationInWindow(rootLocation); + FrameLayout.LayoutParams params = (FrameLayout.LayoutParams) view.getLayoutParams(); + params.gravity = Gravity.TOP | Gravity.CENTER_HORIZONTAL; + params.topMargin += rootLocation[1]; + view.setLayoutParams(params); + } + hintSnackbar.show(); } @Override @@ -185,9 +210,9 @@ public void onArCoreUpdate( long timestamp) { Pose targetPose = navigationPoses.getTargetPose(); Pose currentPose = navigationPoses.getCurrentPose(); - lastNavUpdateMs = SystemClock.elapsedRealtime(); long frameAgeMs = (SystemClock.elapsedRealtimeNanos() - timestamp) / 1_000_000; if (isRunning && frameAgeMs > MAX_FRAME_AGE_MS) { + // Late frames don't feed the watchdog. Timber.d("nav: dropping frame that is %d ms old", frameAgeMs); return; } @@ -223,6 +248,7 @@ public void onArCoreUpdate( "nav: deltaYaw=%.3f control=(%.3f, %.3f)", deltaYaw, control.getLeft(), control.getRight()); vehicle.setControl(control); + lastNavUpdateMs = SystemClock.elapsedRealtime(); } } } @@ -412,6 +438,12 @@ public void onResume() { public void onPause() { super.onPause(); + // Hide the hint when leaving. + if (hintSnackbar != null) { + hintSnackbar.dismiss(); + hintSnackbar = null; + } + // Leaving the screen (home button, another app, closing) must never leave the robot driving. if (isRunning) stop(); arCore.pause(); @@ -467,8 +499,9 @@ private void getCameraPermission() { registerForActivityResult( new ActivityResultContracts.RequestPermission(), isGranted -> { + // If not resumed yet, onResume starts ARCore. if (isGranted) { - setupArCore(); + if (isResumed()) setupArCore(); } else { showPermissionDialog(); } diff --git a/robot/android/app/src/main/res/values-de/strings.xml b/robot/android/app/src/main/res/values-de/strings.xml index 8739f30de3..34a80fe8bb 100644 --- a/robot/android/app/src/main/res/values-de/strings.xml +++ b/robot/android/app/src/main/res/values-de/strings.xml @@ -77,6 +77,7 @@ Tippen Sie auf den Boden, wohin der Roboter fahren soll, und drücken Sie dann Start. Ziel in %1$.1f m Entfernung gesetzt. Drücken Sie Start. Das ist nicht der Boden. Tippen Sie auf den Boden, auf dem der Roboter fahren kann. + Noch kein Tracking. Bewegen Sie das Telefon langsam, bis der Boden erkannt wird, und tippen Sie erneut. Zu nah. Tippen Sie auf eine Stelle, die mindestens %1$.1f m entfernt ist. Der Roboter wurde angehalten, weil die App nicht mehr reagiert hat. ARCore-Fehler. Stellen Sie sicher, dass Ihr Gerät kompatibel ist und ARCore installiert ist. diff --git a/robot/android/app/src/main/res/values-es/strings.xml b/robot/android/app/src/main/res/values-es/strings.xml index 7c3c90c779..73765a06f1 100644 --- a/robot/android/app/src/main/res/values-es/strings.xml +++ b/robot/android/app/src/main/res/values-es/strings.xml @@ -77,6 +77,7 @@ Toca el suelo donde quieres que vaya el robot y luego pulsa Iniciar. Objetivo fijado a %1$.1f m. Pulsa Iniciar. Eso no es el suelo. Toca el suelo por donde el robot pueda moverse. + Aún sin seguimiento. Mueve el teléfono despacio hasta encontrar el suelo y vuelve a tocar. Demasiado cerca. Toca un punto a al menos %1$.1f m. El robot se detuvo porque la aplicación dejó de responder. Error de ARCore. Asegúrate de que tu dispositivo sea compatible y de que ARCore esté instalado. diff --git a/robot/android/app/src/main/res/values-fr/strings.xml b/robot/android/app/src/main/res/values-fr/strings.xml index 95c9535ca6..496161262a 100644 --- a/robot/android/app/src/main/res/values-fr/strings.xml +++ b/robot/android/app/src/main/res/values-fr/strings.xml @@ -77,6 +77,7 @@ Touchez le sol à l\'endroit où le robot doit aller, puis appuyez sur Démarrer. Objectif placé à %1$.1f m. Appuyez sur Démarrer. Ce n\'est pas le sol. Touchez le sol où le robot peut rouler. + Pas encore de suivi. Déplacez lentement le téléphone jusqu\'à ce que le sol soit détecté, puis touchez à nouveau. Trop proche. Touchez un point à au moins %1$.1f m. Le robot a été arrêté car l\'application ne répondait plus. Erreur ARCore. Vérifiez que votre appareil est compatible et qu\'ARCore est installé. diff --git a/robot/android/app/src/main/res/values-hi/strings.xml b/robot/android/app/src/main/res/values-hi/strings.xml index 5318ece33e..fb85272263 100644 --- a/robot/android/app/src/main/res/values-hi/strings.xml +++ b/robot/android/app/src/main/res/values-hi/strings.xml @@ -77,6 +77,7 @@ फ़र्श पर वहाँ टैप करें जहाँ रोबोट को जाना है, फिर स्टार्ट दबाएँ। लक्ष्य %1$.1f मी दूर सेट हुआ। चलाने के लिए स्टार्ट दबाएँ। यह फ़र्श नहीं है। फ़र्श पर वहाँ टैप करें जहाँ रोबोट चल सकता है। + ट्रैकिंग अभी शुरू नहीं हुई। फ़र्श मिलने तक फ़ोन को धीरे-धीरे हिलाएँ, फिर दोबारा टैप करें। बहुत पास। कम से कम %1$.1f मी दूर की जगह पर टैप करें। ऐप ने जवाब देना बंद कर दिया, इसलिए रोबोट रोक दिया गया। ARCore त्रुटि। सुनिश्चित करें कि आपका डिवाइस संगत है और ARCore इंस्टॉल है। diff --git a/robot/android/app/src/main/res/values-ko/strings.xml b/robot/android/app/src/main/res/values-ko/strings.xml index 4218b22ed4..d20f0f6a58 100644 --- a/robot/android/app/src/main/res/values-ko/strings.xml +++ b/robot/android/app/src/main/res/values-ko/strings.xml @@ -77,6 +77,7 @@ 로봇이 갈 바닥 위치를 탭한 다음 시작을 누르세요. %1$.1f m 거리에 목표가 설정되었습니다. 시작을 누르세요. 바닥이 아닙니다. 로봇이 주행할 수 있는 바닥을 탭하세요. + 아직 추적 중이 아닙니다. 바닥이 인식될 때까지 휴대폰을 천천히 움직인 후 다시 탭하세요. 너무 가깝습니다. 최소 %1$.1f m 떨어진 곳을 탭하세요. 앱이 응답하지 않아 로봇을 정지했습니다. ARCore 오류입니다. 기기가 호환되고 ARCore가 설치되어 있는지 확인하세요. diff --git a/robot/android/app/src/main/res/values-zh/strings.xml b/robot/android/app/src/main/res/values-zh/strings.xml index 9170ebff95..b17a28406b 100644 --- a/robot/android/app/src/main/res/values-zh/strings.xml +++ b/robot/android/app/src/main/res/values-zh/strings.xml @@ -77,6 +77,7 @@ 点击地面上希望机器人前往的位置，然后按开始。 目标已设置在 %1$.1f 米外。按开始出发。 这不是地面。请点击机器人可以行驶的地面。 + 尚未开始追踪。请缓慢移动手机直到识别出地面，然后再次点击。 太近了。请点击至少 %1$.1f 米外的位置。 应用无响应，机器人已停止。 ARCore 出错。请确认您的设备兼容并已安装 ARCore。 diff --git a/robot/android/app/src/main/res/values/strings.xml b/robot/android/app/src/main/res/values/strings.xml index 6a5a3c240d..80dfbd1774 100644 --- a/robot/android/app/src/main/res/values/strings.xml +++ b/robot/android/app/src/main/res/values/strings.xml @@ -97,6 +97,7 @@ Tap on the floor where you want the robot to go, then press Start. Goal set %1$.1f m away. Press Start to drive. That\'s not the floor. Tap on the floor where the robot can drive. + Not tracking yet. Move the phone slowly until the floor is found, then tap again. Too close. Tap a spot at least %1$.1f m away. The robot was stopped because the app stopped responding. ARCore failure. Make sure that your device is compatible and the ARCore SDK is installed. diff --git a/robot/android/app/src/test/java/org/openbot/app/robot/pointGoalNavigation/GoalValidatorTest.java b/robot/android/app/src/test/java/org/openbot/app/robot/pointGoalNavigation/GoalValidatorTest.java index 2404158c54..a7b447fba8 100644 --- a/robot/android/app/src/test/java/org/openbot/app/robot/pointGoalNavigation/GoalValidatorTest.java +++ b/robot/android/app/src/test/java/org/openbot/app/robot/pointGoalNavigation/GoalValidatorTest.java @@ -13,6 +13,16 @@ public class GoalValidatorTest { // Phone mounted on the robot ~15 cm above the floor, world y is up. private static final float[] CAMERA = {0f, 0f, 0f}; + @Test + public void startingCameraHeight_followsTheRobotBody() { + assertEquals(0.16f, GoalValidator.startingCameraHeight("RTR_520"), 1e-6f); + assertEquals(0.22f, GoalValidator.startingCameraHeight("RC_CAR"), 1e-6f); + assertEquals(0.23f, GoalValidator.startingCameraHeight("MTV"), 1e-6f); + assertEquals(0.12f, GoalValidator.startingCameraHeight("DIY"), 1e-6f); + assertEquals(0.12f, GoalValidator.startingCameraHeight(""), 1e-6f); + assertEquals(0.12f, GoalValidator.startingCameraHeight(null), 1e-6f); + } + @Test public void floorTwoMetersAhead_isAccepted() { assertEquals(Result.OK, GoalValidator.validate(CAMERA, new float[] {0f, -0.15f, -2f}));