Algorithm: added history of commands and rollback() method to undo last action. Fixed condition for swapping x/y in linear regresison (now only slope of resulting line taken into account)

2016-09-01 15:52:41 +03:00
parent 97a0f343ce
commit 9bbeadd2d5
5 changed files with 148 additions and 52 deletions
@@ -7,9 +7,21 @@ object Logger {
    val file = File("debug.log")
    val fileWriter = FileWriter(file.absoluteFile)
    val bufferedWriter = BufferedWriter(fileWriter)
+    var padding = ""
+    var paddingStep = "  "

+    fun indent() {
+        padding += paddingStep
+    }
+
+    fun outdent() {
+        if (padding.length == 0) {
+            throw IllegalArgumentException("Called outdent without corresponsing indent")
+        }
+        padding = padding.removeSuffix(paddingStep)
+    }
    fun log(msg: String) {
-        bufferedWriter.write(msg + "\n")
+        bufferedWriter.write(padding + msg + "\n")
        bufferedWriter.flush()
    }
 }
@@ -11,6 +11,7 @@ import io.netty.handler.codec.http.*
 import objects.Car
 import setRouteMetricUrl
 import sonarUrl
+import sun.rmi.runtime.Log
 import java.util.*
 import java.util.concurrent.Exchanger
 import java.util.concurrent.TimeUnit
@@ -28,19 +29,21 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
    protected val LEFT = 2
    protected val RIGHT = 3

-    private var prevState: CarState? = null
+    private val historySize = 10
+    private val history = Stack<RouteMetricRequest>()

+    private var prevState: CarState? = null
    private var prevSonarDistances = mapOf<Angle, AngleData>()
    private val defaultAngles = arrayOf(Angle(0), Angle(70), Angle(75), Angle(80), Angle(85), Angle(90), Angle(95), Angle(100), Angle(105), Angle(110), Angle(180))
+
    //    private val defaultAngles = arrayOf(Angle(0), Angle(60), Angle(70), Angle(80), Angle(90), Angle(100), Angle(110), Angle(120), Angle(180))
    protected var requiredAngles = defaultAngles
-
    protected enum class CarState {
        WALL,
        INNER,
        OUTER
-    }

+    }
    private var iterationCounter = 0

    protected fun getData(angles: Array<Angle>): IntArray {
@@ -95,6 +98,7 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA

    fun iterate() {
        Logger.log("============= STARTING ITERATION ${iterationCounter} ============")
+        Logger.indent()
        iterationCounter++
        if (RoomModel.finished) {
            return
@@ -118,10 +122,14 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
            return
        }

+        addToHistory(command)
+
        afterGetCommand(command)
        Logger.log("Sending command:")
-        Logger.log("    Directions = ${Arrays.toString(command.directions)}")
-        Logger.log("    Distanced = ${Arrays.toString(command.distances)}")
+        Logger.indent()
+        Logger.log("Directions = ${Arrays.toString(command.directions)}")
+        Logger.log("Distanced = ${Arrays.toString(command.distances)}")
+        Logger.outdent()
        println(Arrays.toString(command.directions))
        println(Arrays.toString(command.distances))

@@ -129,6 +137,7 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
        this.prevState = state

        moveCar(command)
+        Logger.outdent()
        Logger.log("============= FINISHING ITERATION ${iterationCounter} ============")
        Logger.log("")
    }
@@ -146,6 +155,58 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
        return requiredAngles
    }

+    private fun addToHistory(command: RouteMetricRequest) {
+        history.push(command)
+        while(history.size > historySize) {
+            history.removeAt(0)
+        }
+    }
+
+    private fun popFromHistory(): RouteMetricRequest {
+        return history.pop()
+    }
+
+    private fun inverseCommand(command: RouteMetricRequest): RouteMetricRequest {
+        val res = RouteMetricRequest.BuilderRouteMetricRequest(command.distances, command.directions)
+        res.distances.reverse()
+        res.directions.reverse()
+
+        for ((index, dir) in res.directions.withIndex()) {
+            res.directions[index] = when (dir) {
+                FORWARD -> BACKWARD
+                BACKWARD -> FORWARD
+                LEFT -> RIGHT
+                RIGHT -> LEFT
+                else -> throw IllegalArgumentException("Unexpected direction = ${dir} found during command inversion")
+            }
+        }
+        return res.build()
+    }
+
+    protected fun rollback() {
+        val lastCommand = popFromHistory()
+        val invertedCommand = inverseCommand(lastCommand)
+        Logger.log ("Rollback:")
+        Logger.indent()
+        Logger.log ("Last command: ${lastCommand.toString()}")
+        Logger.log ("Inverted cmd: ${invertedCommand.toString()}")
+        Logger.outdent()
+        moveCar(invertedCommand)
+    }
+
+    protected fun rollback(steps: Int) {
+        Logger.log("=== Starting rollback for ${steps} steps ===")
+        Logger.indent()
+        var stepsRemaining = steps
+        while(stepsRemaining > 0 && history.size > 0) {
+            Logger.log("Step: ${steps - stepsRemaining + 1}")
+            rollback()
+            stepsRemaining--
+        }
+        Logger.outdent()
+        Logger.log("=== Finished rollback ===")
+    }
+
    protected abstract fun getCarState(anglesDistances: Map<Angle, AngleData>): CarState?
    protected abstract fun getCommand(anglesDistances: Map<Angle, AngleData>, state: CarState): RouteMetricRequest?
    protected abstract fun afterGetCommand(route: RouteMetricRequest)
@@ -0,0 +1,25 @@
+package algorithm
+
+import RouteMetricRequest
+
+fun fromIntToDirection(value: Int): String {
+    return when (value) {
+        0 -> "FORWARD"
+        1 -> "BACKWARD"
+        2 -> "LEFT"
+        3 -> "RIGHT"
+        else -> throw IllegalArgumentException("Error parsing Direction from Int: got value = $value")
+    }
+}
+
+fun RouteMetricRequest.toString(): String {
+    var res = emptyArray<String>()
+
+    for (i in distances.indices) {
+        val dist = distances[i].toString()
+        val dir = fromIntToDirection(directions[i])
+        res += "(" + dist + ", " + dir + ")"
+    }
+
+    return res.joinToString("; ")
+}
@@ -4,13 +4,11 @@ import Logger.log
 import RouteMetricRequest
 import SonarRequest
 import algorithm.geometry.*
+import algorithm.geometry.Vector
 import objects.Car
 import java.util.concurrent.Exchanger

 class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : AbstractAlgorithm(thisCar, exchanger) {
-
-
-    // TODO: set to appropriate values
    override val ATTEMPTS: Int = 5
    override val SMOOTHING = SonarRequest.Smoothing.MEDIAN
    override val WINDOW_SIZE = 3
@@ -40,19 +38,14 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
        return CarState.WALL
    }

-    private fun noParallelWallFound(anglesDistances: Map<Angle, AngleData>, state: CarState): RouteMetricRequest {
-        val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0))
-        resultBuilder.setDirections(getIntArray(FORWARD))
-        resultBuilder.setDistances(getIntArray(10))
-        return resultBuilder.build()
-    }
-
    private fun noOrthogonalMeasurementFound(anglesDistances: Map<Angle, AngleData>, state: CarState): RouteMetricRequest {
        val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0))
        resultBuilder.setDirections(getIntArray(FORWARD, RIGHT, FORWARD))
        val wallAngle = calculateAngle(anglesDistances, state)
+        Logger.indent()
        resultBuilder.setDistances(getIntArray(20, wallAngle.degs(), 75))
        addWall(-wallAngle)
+        Logger.outdent()
        carAngle = RoomModel.walls.last().wallAngleOX.degs()
        calibrateAfterRotate = true
        return resultBuilder.build()
@@ -122,11 +115,11 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs

    private fun addWall(angleWithPrevWall: Angle) {
        log("Adding wall")
+        Logger.indent()
        RoomModel.updateWalls()
-
+        Logger.outdent()
        val firstWall = RoomModel.walls.first()
        val lastWall = RoomModel.walls.last()
-//        if (firstWall == lastWall && RoomModel.walls.size > 1) {
        if (circleFound) {
            log("Found circle, finishing algorithm")
            isCompleted = true
@@ -155,7 +148,8 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
                    && it.value.angle.degs() <= 120
                    && it.value.distance == -1
        }.size > anglesDistances.size / 2) {
-            log("Found to many -1 in angle distances, passing")
+            log("Found to many -1 in angle distances, falling back")
+            rollback()
            //todo Теоретически такая ситуация может быть валидной, если сразу после внутреннего угла идёт внешний
            return null
        }
@@ -166,9 +160,12 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
                .sumByDouble { it.distance.toDouble() }) / anglesDistances.values.size
        log("Estimated average = $average")

+        log("1. Checking if we just rotated and should re-calibrate")
        if (calibrateAfterRotate) {
            log("Calibrating after rotate")
+            Logger.indent()
            val maybeAlignment = tryAlignParallelToWall(anglesDistances, state, average)
+            Logger.outdent()
            if (maybeAlignment != null) {
                log("Realigning")
                return maybeAlignment
@@ -176,39 +173,35 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
            log("No need to realign")
            calibrateAfterRotate = false
        }
+        log("")
+

        // Check most basic measurements: 60/90/120
+        log("2. Check if we have measurement on 90")
        if (dist90.distance == -1 || dist90.distance > OUTER_CORNER_DISTANCE_THRESHOLD) {
            log("No orthogonal measurement found")
            return noOrthogonalMeasurementFound(anglesDistances, state)
        }
+        log("")

        // Add point to room map
        val point = Point(
                x = carX + dist90.distance * Math.cos(degreesToRadian(sonarAngle)),
                y = carY + dist90.distance * Math.sin(degreesToRadian(sonarAngle))
        )
-        log("Adding ${point.toString()} to last wall")
+        log("Adding middle point ${point.toString()} to wall ${RoomModel.walls.last().id}")
        RoomModel.walls.last().pushBackPoint(point)
+        log("")

        // Check if corner reached
+        log("3. Check if we reached corner, dist0 = ${dist0.distance}")
        if (dist0.distance != -1 && dist0.distance < MAX_DISTANCE_TO_WALL_AHEAD) {
            log("Wall ahead found")
            return wallAheadFound(anglesDistances, state)
        }
+        log("")

-        // Big fail, try to do something safe
-        if (dist110.distance == -1 && dist70.distance == -1) {
-            log("No parallel wall found")
-            return noParallelWallFound(anglesDistances, state)
-        }
-
-        // Big fail too
-        if (dist110.distance == -1) {
-            log("No back measurement found, wtf")
-            return moveForward(dist0.distance)
-        }
-
+        log("Adding other points")
        for (i in 70..110 step 10) {
            if (i == 90) {
                continue    // point on 90 was already added
@@ -216,44 +209,57 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs

            val curDist = anglesDistances[Angle(i)]!!.distance
            if (curDist > (average * 1.5) || curDist < (average / 2)) {
+                log("For point on ${i} dist = ${curDist}, while window is from ${average * 1.5} till ${average * 2}. Dropping it as outlier")
                continue
            }
            val point = Point(
                    x = carX + curDist * Math.cos(degreesToRadian(carAngle - i)),
                    y = carY + curDist * Math.sin(degreesToRadian(carAngle - i))
            )
+            log("Adding ${point.toString()} to wall ${RoomModel.walls.last().id}")
            RoomModel.walls.last().pushBackPoint(point)
        }
+        log("")

        // Try align parallel to wall
+        log("4. Check if we have to align parallel to the wall")
+        Logger.indent()
        val maybeAlignment = tryAlignParallelToWall(anglesDistances, state, average)
+        Logger.outdent()
        if (maybeAlignment != null) {
            log("Realigning")
            return maybeAlignment
        }
+        log("")

        // Check if wall is too close or too far
+        log ("5. Check if we have to move closer or farther to the wall")
        if (dist90.distance > DISTANCE_TO_WALL_UPPER_BOUND || dist90.distance < DISTANCE_TO_WALL_LOWER_BOUND) {
            log("Flaw in distance to the parallel wall found, correcting")
            return correctDistanceToParallelWall(anglesDistances, state)
        }
+        log("")

        // Approaching inner corner and getting spurious reflection from 2 walls on 60;
        // Just move forward to get closer to the corner;
+        log ("6. Check if we are detecting echo reflections approaching inner corner")
        if (dist70.distance - dist0.distance > ECHO_REFLECTION_DIFF) {
-            log("Spurious reflection detected, moving forward")
+            log("Echo reflection detected, moving forward")
            return moveForward(dist0.distance)
        }
+        log ("")

        // Approaching outer corner (parallel wall is ending soon, but not yet);
        // Just move forward to get to the end of the wall
+        log ("7. Check if we are detecting far wall approaching outer corner")
        if (dist80.distance == -1 || Math.abs(dist100.distance - dist80.distance) > ISOSCALENESS_MAX_DIFF) {
-            log("Aprroaching outer corner, moving forward")
+            log("Approaching outer corner, moving forward")
            return moveForward(dist0.distance)
        }
+        log ("")

        // default case: everything is ok, just move forward
-        log("Default case: moving forward")
+        log("8. Default case: moving forward")
        return moveForward(dist0.distance)
    }

@@ -283,16 +289,15 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
                }
                LEFT -> {
                    route.distances[idx] = (CHARGE_CORRECTION * route.distances[idx]).toInt()
-//                    carAngle += route.distances[idx]
                }
                RIGHT -> {
                    route.distances[idx] = (CHARGE_CORRECTION * route.distances[idx]).toInt()
-//                    carAngle -= route.distances[idx]
                }
            }
        }
        if (Math.round(Math.cos(Angle(carAngle).rads())).toInt() == 1 && carAngle != 0
                && Vector(carX.toDouble(), carY.toDouble()).length() < RANGE_FROM_ZERO_POINT_TO_FINISH_ALG) {
+            log("Found circle!")
            circleFound = true
        }

@@ -18,16 +18,22 @@ data class Wall(val wallAngleOX: Angle,
    }

    var isFinished = false
-    val MAX_REGRESSION_ERROR = 5.0
+    val MAX_SLOPE = 0.2

    fun pushBackPoint(point: Point) {
+        Logger.log("Adding ${point.toString()}")
        rawPoints.add(point)
+        Logger.log("Line before approximation ${line.toString()}")
        line = approximatePointsByLine()
+        Logger.log("Line after approximation ${line.toString()}")
    }

    fun pushFrontPoint(point: Point) {
+        Logger.log("Adding ${point.toString()}")
        rawPoints.add(0, point)
+        Logger.log("Line before approximation ${line.toString()}")
        line = approximatePointsByLine()
+        Logger.log("Line after approximation ${line.toString()}")
    }

    var points: ArrayList<Point> = arrayListOf()
@@ -86,20 +92,7 @@ data class Wall(val wallAngleOX: Angle,
        var beta1 = xybar / xxbar
        var beta0 = ybar - beta1 * xbar

-        // analyze results
-        val df = n - 2
-        var rss = 0.0      // residual sum of squares
-        var ssr = 0.0      // regression sum of squares
-        for ((x, y) in rawPoints) {
-            val fit = beta1 * x + beta0
-            rss += (fit - y) * (fit - y)
-            ssr += (fit - ybar) * (fit - ybar)
-        }
-
-        val svar = rss / df
-        val svar1 = svar / xxbar
-
-        if (svar1.isNaN() || svar1.isInfinite() || Math.sqrt(svar1).gt(MAX_REGRESSION_ERROR) || Math.abs(beta1) > 1) {
+        if (Math.abs(beta1) > MAX_SLOPE) {
            beta1 = xybar / yybar
            beta0 = xbar - beta1 * ybar
            return Line(1.0, -beta1, -beta0)