From 9bbeadd2d5c5c1d3f3463ad4366f48bbf5e34bb8 Mon Sep 17 00:00:00 2001 From: dsavvinov Date: Thu, 1 Sep 2016 15:52:41 +0300 Subject: [PATCH] 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) --- server/src/main/java/Logger.kt | 14 +++- .../main/java/algorithm/AbstractAlgorithm.kt | 71 +++++++++++++++++-- .../src/main/java/algorithm/PrettyPrinting.kt | 25 +++++++ .../java/algorithm/RoomBypassingAlgorithm.kt | 67 +++++++++-------- .../src/main/java/algorithm/geometry/Wall.kt | 23 +++--- 5 files changed, 148 insertions(+), 52 deletions(-) create mode 100644 server/src/main/java/algorithm/PrettyPrinting.kt diff --git a/server/src/main/java/Logger.kt b/server/src/main/java/Logger.kt index 1c6baaf5802..eeb474cfed2 100644 --- a/server/src/main/java/Logger.kt +++ b/server/src/main/java/Logger.kt @@ -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() } } \ No newline at end of file diff --git a/server/src/main/java/algorithm/AbstractAlgorithm.kt b/server/src/main/java/algorithm/AbstractAlgorithm.kt index 8f54ea60f4e..4c61b58a75f 100644 --- a/server/src/main/java/algorithm/AbstractAlgorithm.kt +++ b/server/src/main/java/algorithm/AbstractAlgorithm.kt @@ -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() + private var prevState: CarState? = null private var prevSonarDistances = mapOf() 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): IntArray { @@ -95,6 +98,7 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger 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): CarState? protected abstract fun getCommand(anglesDistances: Map, state: CarState): RouteMetricRequest? protected abstract fun afterGetCommand(route: RouteMetricRequest) diff --git a/server/src/main/java/algorithm/PrettyPrinting.kt b/server/src/main/java/algorithm/PrettyPrinting.kt new file mode 100644 index 00000000000..cf8850da177 --- /dev/null +++ b/server/src/main/java/algorithm/PrettyPrinting.kt @@ -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() + + for (i in distances.indices) { + val dist = distances[i].toString() + val dir = fromIntToDirection(directions[i]) + res += "(" + dist + ", " + dir + ")" + } + + return res.joinToString("; ") +} \ No newline at end of file diff --git a/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt b/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt index 07884fdbf85..56874d02cb2 100644 --- a/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt +++ b/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt @@ -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) : 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) : Abs return CarState.WALL } - private fun noParallelWallFound(anglesDistances: Map, 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, 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) : 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) : 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) : 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) : 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) : 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) : 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 } diff --git a/server/src/main/java/algorithm/geometry/Wall.kt b/server/src/main/java/algorithm/geometry/Wall.kt index 543e875be52..efedb37a0b2 100644 --- a/server/src/main/java/algorithm/geometry/Wall.kt +++ b/server/src/main/java/algorithm/geometry/Wall.kt @@ -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 = 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)