From f7b164b9fbe7c9a9b1d23f66d9baaf2d296be3e4 Mon Sep 17 00:00:00 2001 From: MaximZaitsev Date: Mon, 29 Aug 2016 15:06:15 +0300 Subject: [PATCH] working algorithm on inner angles --- .../main/java/algorithm/AbstractAlgorithm.kt | 50 ++++-- .../java/algorithm/RoomBypassingAlgorithm.kt | 151 +++++++++++++----- server/src/main/java/algorithm/RoomModel.kt | 41 +++-- .../src/main/java/algorithm/geometry/Angle.kt | 22 +++ .../main/java/algorithm/geometry/AngleData.kt | 6 +- .../algorithm/geometry/DoubleComparison.kt | 13 ++ .../src/main/java/algorithm/geometry/Line.kt | 45 +++++- .../src/main/java/algorithm/geometry/Point.kt | 4 +- .../src/main/java/algorithm/geometry/Util.kt | 10 +- .../src/main/java/algorithm/geometry/Wall.kt | 39 +++++ 10 files changed, 295 insertions(+), 86 deletions(-) create mode 100644 server/src/main/java/algorithm/geometry/Angle.kt create mode 100644 server/src/main/java/algorithm/geometry/DoubleComparison.kt create mode 100644 server/src/main/java/algorithm/geometry/Wall.kt diff --git a/server/src/main/java/algorithm/AbstractAlgorithm.kt b/server/src/main/java/algorithm/AbstractAlgorithm.kt index bdd4ad1aba3..67d1f77c532 100644 --- a/server/src/main/java/algorithm/AbstractAlgorithm.kt +++ b/server/src/main/java/algorithm/AbstractAlgorithm.kt @@ -4,6 +4,7 @@ import CodedOutputStream import Exceptions.InactiveCarException import RouteMetricRequest import SonarRequest +import algorithm.geometry.Angle import algorithm.geometry.AngleData import io.netty.buffer.Unpooled import io.netty.handler.codec.http.* @@ -17,6 +18,10 @@ import java.util.concurrent.TimeoutException abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger) { + open val ATTEMPTS = 1 + open val THRESHOLD = 0 + open val SMOOTHING = SonarRequest.Smoothing.NONE + protected val FORWARD = 0 protected val BACKWARD = 1 protected val LEFT = 2 @@ -24,8 +29,8 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger() - private val defaultAngles = listOf(0, 60, 90, 120, 180).toIntArray() + private var prevSonarDistances = mapOf() + private val defaultAngles = arrayOf(Angle(0), Angle(60), Angle(90), Angle(120), Angle(180)) protected var requiredAngles = defaultAngles protected enum class CarState { @@ -34,23 +39,20 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger): IntArray { + val anglesIntArray = (angles.map { it -> it.degs() }).toIntArray() val message = SonarRequest.BuilderSonarRequest( - angles = angles, - attempts = IntArray(angles.size, { attempts }), - windowSize = 0, - smoothing = smoothing) + angles = anglesIntArray, + attempts = IntArray(angles.size, { ATTEMPTS }), + threshold = THRESHOLD, + smoothing = SMOOTHING) .build() val requestBytes = ByteArray(message.getSizeNoTag()) message.writeTo(CodedOutputStream(requestBytes)) val request = getDefaultHttpRequest(thisCar.host, sonarUrl, requestBytes) try { - car.client.Client.sendRequest(request, thisCar.host, thisCar.port, mapOf(Pair("angles", angles))) + car.client.Client.sendRequest(request, thisCar.host, thisCar.port, mapOf(Pair("angles", anglesIntArray))) } catch (e: InactiveCarException) { println("connection error!") } @@ -91,12 +93,15 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger() + val anglesDistances = mutableMapOf() for (i in 0..angles.size - 1) { if (Math.abs(distances[i]) < 0.01) { continue @@ -106,7 +111,10 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger { + return prevSonarDistances + } + + private fun getAngles(): Array { return requiredAngles } - protected abstract fun getCarState(anglesDistances: Map): CarState? - protected abstract fun getCommand(anglesDistances: Map, state: CarState): RouteMetricRequest + 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/RoomBypassingAlgorithm.kt b/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt index c551c1e4aaa..939fcd20fad 100644 --- a/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt +++ b/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt @@ -1,69 +1,140 @@ package algorithm -import objects.Car -import java.util.concurrent.Exchanger import RouteMetricRequest +import algorithm.geometry.Angle import algorithm.geometry.AngleData import algorithm.geometry.Point +import algorithm.geometry.Wall +import objects.Car +import java.util.concurrent.Exchanger class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger) : AbstractAlgorithm(thisCar, exchanger) { - val points = arrayListOf() - private val DISTANCE_TO_WALL_THRESHOLD = 60 + // TODO: set to appropriate values + override val THRESHOLD = 0 + override val ATTEMPTS: Int = 3 + override val SMOOTHING = SonarRequest.Smoothing.MEDIAN + + private val MAX_DISTANCE_TO_WALL_AHEAD = 60 + private val ISOSCALENESS_THRESHOLD = 10 + private val DISTANCE_TO_WALL_UPPER_BOUND = 50 + private val DISTANCE_TO_WALL_LOWER_BOUND = 25 var carX = 0 var carY = 0 var carAngle = 0 - override fun getCarState(anglesDistances: Map): CarState { + override fun getCarState(anglesDistances: Map): CarState { return CarState.WALL } - override fun getCommand(anglesDistances: Map, state: CarState): RouteMetricRequest { - val dist0 = anglesDistances[0] - val dist60 = anglesDistances[60] - val dist90 = anglesDistances[90] - val dist120 = anglesDistances[120] + private fun noParallelWallFound(anglesDistances: Map, state: CarState): RouteMetricRequest { val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0)) - if (dist90 == null) { - resultBuilder.setDirections(getIntArray(RIGHT)) - resultBuilder.setDistances(getIntArray(10)) - return resultBuilder.build() + 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(RIGHT)) + resultBuilder.setDistances(getIntArray(10)) + return resultBuilder.build() + } + + private fun wallAheadFound(anglesDistances: Map, state: CarState): RouteMetricRequest { + val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0)) + resultBuilder.setDirections(getIntArray(LEFT)) + resultBuilder.setDistances(getIntArray(calculateAngle(anglesDistances, state).degs())) + addWall() + return resultBuilder.build() + } + + private fun alignParallelToWall(anglesDistances: Map, state: CarState): RouteMetricRequest { + val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0)) + + // we're sure that we have 120 and 60 degs measurements as they were checked before + val rotationDirection = if (anglesDistances[Angle(120)]!!.distance > anglesDistances[Angle(60)]!!.distance) LEFT else RIGHT + + resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD)) + resultBuilder.setDistances(getIntArray(10, 7))//todo calibrate + return resultBuilder.build() + } + + private fun correctDistanceToParallelWall(anglesDistances: Map, state: CarState): RouteMetricRequest { + val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0)) + + val rotationDirection = if (anglesDistances[Angle(90)]!!.distance > 50) RIGHT else LEFT + + resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD)) + resultBuilder.setDistances(getIntArray(10, 10)) + return resultBuilder.build() + } + + private fun addWall() { + RoomModel.updateWalls() + + if (RoomModel.walls.last() == RoomModel.walls.first() && RoomModel.walls.size > 1) { + println("YEAH!") + RoomModel.finished = true + RoomModel.walls.removeAt(RoomModel.walls.size - 1) } + RoomModel.walls.add(Wall()) + } + + override fun getCommand(anglesDistances: Map, state: CarState): RouteMetricRequest { + val dist0 = anglesDistances[Angle(0)] + val dist60 = anglesDistances[Angle(60)] + val dist90 = anglesDistances[Angle(90)] + val dist120 = anglesDistances[Angle(120)] val sonarAngle = carAngle - 90 - points.add(Point( - x = carX + dist90.distance * Math.cos(degreesToRadian(sonarAngle)), - y = carY + dist90.distance * Math.sin(degreesToRadian(sonarAngle)) - )) - if (dist120 == null || dist60 == null) { - resultBuilder.setDirections(getIntArray(FORWARD)) - resultBuilder.setDistances(getIntArray(10)) - return resultBuilder.build() - } - if (dist0 != null && dist0.distance < DISTANCE_TO_WALL_THRESHOLD) { - resultBuilder.setDirections(getIntArray(LEFT, FORWARD)) - resultBuilder.setDistances(getIntArray(20, 10)) - return resultBuilder.build() - } - val abs = Math.abs(dist120.distance - dist60.distance) - if (abs > 10) { - val rotationDirection = if (dist120.distance > dist60.distance) LEFT else RIGHT - resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD)) - resultBuilder.setDistances(getIntArray(10, 7))//todo calibrate - return resultBuilder.build() - } - if (dist90.distance > 50 || dist90.distance < 25) { - val rotationDirection = if (dist90.distance > 50) RIGHT else LEFT - resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD)) - resultBuilder.setDistances(getIntArray(10, 10)) - return resultBuilder.build() + + // Check most basic measurements: 60/90/120 + if (dist90 == null) { + return noOrthogonalMeasurementFound(anglesDistances, state) } + if (dist120 == null || dist60 == null) { + return noParallelWallFound(anglesDistances, state) + } + + // Add point to room map + val point = Point( + x = carX + dist90.distance * Math.cos(degreesToRadian(sonarAngle)), + y = carY + dist90.distance * Math.sin(degreesToRadian(sonarAngle)) + ) + RoomModel.walls.last().addPoint(point) + + // Check if corner reached + if (dist0 != null && dist0.distance < MAX_DISTANCE_TO_WALL_AHEAD) { + return wallAheadFound(anglesDistances, state) + } + + // Checks for paralleleness + + // Check if wall is not parallel + val diff = Math.abs(dist120.distance - dist60.distance) + if (diff > ISOSCALENESS_THRESHOLD) { + return alignParallelToWall(anglesDistances, state) + } + + // Check if wall is too close or too far + if (dist90.distance > 50 || dist90.distance < 25) { + return correctDistanceToParallelWall(anglesDistances, state) + } + + // default case: everything is ok, just move forward + val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0)) resultBuilder.setDirections(getIntArray(FORWARD)) resultBuilder.setDistances(getIntArray(20)) return resultBuilder.build() } + private fun calculateAngle(anglesDistances: Map, state: CarState): Angle { + // TODO: stub here, make proper angle calculation + return Angle(90) + } + private fun getIntArray(vararg args: Int): IntArray { return args } diff --git a/server/src/main/java/algorithm/RoomModel.kt b/server/src/main/java/algorithm/RoomModel.kt index 9dc013653a0..693e739856b 100644 --- a/server/src/main/java/algorithm/RoomModel.kt +++ b/server/src/main/java/algorithm/RoomModel.kt @@ -4,11 +4,16 @@ import DebugClInterface import DebugResponse import Waypoints import algorithm.geometry.Line +import algorithm.geometry.Point +import algorithm.geometry.Wall import objects.Car +import java.util.* object RoomModel { val lines = arrayListOf() + val walls = arrayListOf(Wall()) + var finished = false val linesModel = listOf(Line(0.0, 1.0, -300.0), Line(1.0, 0.0, 150.0), @@ -23,21 +28,17 @@ object RoomModel { var currentPosition_y = 0 fun getUpdate(): Waypoints { - val algorithm = DebugClInterface.algorithmImpl - if (algorithm == null || algorithm !is RoomBypassingAlgorithm) { - val emptyArr = IntArray(0) - return Waypoints.BuilderWaypoints(emptyArr, emptyArr, emptyArr, emptyArr, false).build() + val points = getWallsPoints() - } - val begin_x = IntArray(algorithm.points.size) - val begin_y = IntArray(algorithm.points.size) - val end_x = IntArray(algorithm.points.size) - val end_y = IntArray(algorithm.points.size) + val begin_x = IntArray(points.size) + val begin_y = IntArray(points.size) + val end_x = IntArray(points.size) + val end_y = IntArray(points.size) - for (i in 0..algorithm.points.size - 2) { - val curPoint = algorithm.points[i] - val nextPoint = algorithm.points[i + 1] + for (i in 0..points.size - 2) { + val curPoint = points[i] + val nextPoint = points[i + 1] begin_x[i] = (curPoint.x + 0.5).toInt() begin_y[i] = (curPoint.y + 0.5).toInt() @@ -47,6 +48,22 @@ object RoomModel { return Waypoints.BuilderWaypoints(begin_x, begin_y, end_x, end_y, false).build() } + private fun getWallsPoints(): Array { + return walls.flatMap({ it.points }).toTypedArray() + } + + fun updateWalls() { + if (walls.size < 2) { + // no walls to intersect + return + } + val line1 = walls.last().line + val line2 = walls[walls.size - 2].line + + val intersection: Point = line1.intersect(line2) + walls[walls.size - 2].addPoint(intersection) + } + fun getDebugInfo(): DebugResponse { val aRef = IntArray(linesModel.size) diff --git a/server/src/main/java/algorithm/geometry/Angle.kt b/server/src/main/java/algorithm/geometry/Angle.kt new file mode 100644 index 00000000000..e8a2544a479 --- /dev/null +++ b/server/src/main/java/algorithm/geometry/Angle.kt @@ -0,0 +1,22 @@ +package algorithm.geometry + +data class Angle(private val degrees: Int) { + + constructor(radians: Double) : this((180.0 * radians / Math.PI).toInt()) + + fun rads(): Double { + return Math.PI * degrees / 180.0 + } + + fun degs(): Int { + return degrees + } + + operator fun minus(other: Angle): Angle { + return Angle(degrees - other.degrees) + } + + operator fun plus(other: Angle): Angle { + return Angle(degrees + other.degrees) + } +} \ No newline at end of file diff --git a/server/src/main/java/algorithm/geometry/AngleData.kt b/server/src/main/java/algorithm/geometry/AngleData.kt index b2e69aede2a..2cb0ecf55e8 100644 --- a/server/src/main/java/algorithm/geometry/AngleData.kt +++ b/server/src/main/java/algorithm/geometry/AngleData.kt @@ -1,14 +1,14 @@ package algorithm.geometry -class AngleData(val angle: Int, val distance: Int) { +class AngleData(val angle: Angle, val distance: Int) { - fun toPoint(carAngleOX: Int): Point { + fun toPoint(carAngleOX: Angle): Point { //convert to global coordinate system val angle = carAngleOX - angle - val radianAngle = Util.degreesToRadian(angle) + val radianAngle = angle.rads() return Point( x = Math.cos(radianAngle) * distance, diff --git a/server/src/main/java/algorithm/geometry/DoubleComparison.kt b/server/src/main/java/algorithm/geometry/DoubleComparison.kt new file mode 100644 index 00000000000..9dd82c3d636 --- /dev/null +++ b/server/src/main/java/algorithm/geometry/DoubleComparison.kt @@ -0,0 +1,13 @@ +package algorithm.geometry + +fun Double.lt(other: Double): Boolean { + return this - other < -Util.eps +} + +fun Double.gt(other: Double): Boolean { + return this - other > Util.eps +} + +fun Double.eq(other: Double): Boolean { + return Math.abs(this - other) < Util.eps; +} \ No newline at end of file diff --git a/server/src/main/java/algorithm/geometry/Line.kt b/server/src/main/java/algorithm/geometry/Line.kt index 8e835642885..c839861b262 100644 --- a/server/src/main/java/algorithm/geometry/Line.kt +++ b/server/src/main/java/algorithm/geometry/Line.kt @@ -1,21 +1,58 @@ package algorithm.geometry -class Line(val A: Double, val B: Double, var C: Double) { +class Line(var A: Double, var B: Double, var C: Double) { + val COMPARISON_THRESHOLD = 0.5 + init { + normalize() + } - fun getIntersectionPoint(lineTwo: Line): Pair { + fun intersect(lineTwo: Line): Point { val slope = this.A * lineTwo.B - this.B * lineTwo.A if (Math.abs(slope) < 0.001) { throw ArithmeticException("lines is parallel") } val xIntersection = (this.B * lineTwo.C - lineTwo.B * this.C) / slope val yIntersection = (this.C * lineTwo.A - lineTwo.C * this.A) / slope - return Pair(xIntersection, yIntersection) + return Point(xIntersection, yIntersection) } - override fun toString(): String{ + override fun toString(): String { return "Line(A=$A, B=$B, C=$C)" } + fun normalize() { + val div = Math.sqrt(A * A + B * B); + A /= div; + B /= div; + C /= div; + if (A.eq(0.0)) { + if (B.lt(0.0)) { + A *= -1 + B *= -1 + C *= -1 + } + } + else + { + if (A.lt(0.0)){ + A *= -1 + B *= -1 + C *= -1 + } + } + } + + fun metricDist(other: Line): Double { + return Math.sqrt((A - other.A) * (A - other.A) + (B - other.B) * (B - other.B) + (C - other.C) * (C - other.C)) + } + + override fun equals(other: Any?): Boolean { + if (other == null) return false + + if (!(other is Line)) return false + + return metricDist(other) < COMPARISON_THRESHOLD + } } \ No newline at end of file diff --git a/server/src/main/java/algorithm/geometry/Point.kt b/server/src/main/java/algorithm/geometry/Point.kt index 40b317e5bb1..2f3f5708ea0 100644 --- a/server/src/main/java/algorithm/geometry/Point.kt +++ b/server/src/main/java/algorithm/geometry/Point.kt @@ -1,4 +1,6 @@ package algorithm.geometry -class Point(val x: Double, val y: Double) { +data class Point(val x: Double, val y: Double) { + + } \ No newline at end of file diff --git a/server/src/main/java/algorithm/geometry/Util.kt b/server/src/main/java/algorithm/geometry/Util.kt index b6d0aaa0a24..9e13be4b5ea 100644 --- a/server/src/main/java/algorithm/geometry/Util.kt +++ b/server/src/main/java/algorithm/geometry/Util.kt @@ -2,13 +2,5 @@ package algorithm.geometry object Util { - - fun degreesToRadian(angleDegrees: Int): Double { - return Math.PI * angleDegrees / 180.0 - } - - fun radianToDegrees(angleRad: Double): Int { - return (180.0 * angleRad / Math.PI).toInt() - } - + val eps = 1e-2 } \ No newline at end of file diff --git a/server/src/main/java/algorithm/geometry/Wall.kt b/server/src/main/java/algorithm/geometry/Wall.kt new file mode 100644 index 00000000000..6bd1110b3f4 --- /dev/null +++ b/server/src/main/java/algorithm/geometry/Wall.kt @@ -0,0 +1,39 @@ +package algorithm.geometry + +import java.util.* + +data class Wall(val points: ArrayList = arrayListOf(), + var line: Line = Line(0.0, 0.0, 0.0)) { + + + fun addPoint(point: Point) { + points.add(point) + line = approximatePointsByLine() + } + + private fun approximatePointsByLine(): Line { + + val sumX = points.sumByDouble { it.x } + val sumXQuad = points.sumByDouble { it.x * it.x } + val sumY = points.sumByDouble { it.y } + val sumXY = points.sumByDouble { it.y * it.x } + + val pointsCount = points.size + val den = pointsCount * sumXQuad - sumX * sumX + + if (Math.abs(den) < 0.001) { + return Line(1.0, 0.0, -points.first().x) + } + + val k = (pointsCount * sumXY - sumX * sumY) / (pointsCount * sumXQuad - sumX * sumX) + val b = (sumY - sumX * k) / pointsCount + + return Line(-k, 1.0, -b) + } + + override fun equals(other: Any?): Boolean { + if (other == null) return false + if (!(other is Wall)) return false + return line.equals(other.line) + } +} \ No newline at end of file