azalea/kotlin-fork
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

new algorithm version. Normal work on emulator with large errors in data

Browse Source
This commit is contained in:
MaximZaitsev
2016-08-30 16:05:43 +03:00
parent 3167ec8e86
commit 84ea8f4186
10 changed files with 270 additions and 89 deletions
Download Patch File Download Diff File Expand all files Collapse all files
server/src/main/java/DebugClInterface.kt
+2 -2
View File
@@ -64,7 +64,7 @@ object DebugClInterface {
"refloc" -> executeRefreshLocationCommand()
"sonar" -> executeSonarCommand(readString)
"dbinfo" -> executeDebugInfoCommand(readString)
"lines" -> println(algorithm.RoomModel.lines)
"lines" -> algorithm.RoomModel.walls.forEach { println(it.line) }
"pos" -> {
val tmp = algorithmImpl
if (tmp is RoomBypassingAlgorithm) {
@@ -207,7 +207,7 @@ object DebugClInterface {
when (command) {
"reset" -> return null
"done" -> {
val sonarBuilder = SonarRequest.BuilderSonarRequest(angles.toIntArray(), IntArray(angles.size, { 1 }), 0, SonarRequest.Smoothing.NONE)
val sonarBuilder = SonarRequest.BuilderSonarRequest(angles.toIntArray(), IntArray(angles.size, { 5 }), 3, SonarRequest.Smoothing.MEDIAN)
return sonarBuilder.build()
}
else -> {
server/src/main/java/algorithm/AbstractAlgorithm.kt
+4 -7
View File
@@ -19,8 +19,8 @@ import java.util.concurrent.TimeoutException
abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntArray>) {
open val ATTEMPTS = 1
open val THRESHOLD = 0
open val SMOOTHING = SonarRequest.Smoothing.NONE
open val WINDOW_SIZE = 0
protected val FORWARD = 0
protected val BACKWARD = 1
@@ -30,7 +30,7 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
private var prevState: CarState? = null
private var prevSonarDistances = mapOf<Angle, AngleData>()
private val defaultAngles = arrayOf(Angle(0), Angle(60), Angle(90), Angle(120), 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 {
@@ -45,8 +45,8 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
val message = SonarRequest.BuilderSonarRequest(
angles = anglesIntArray,
attempts = IntArray(angles.size, { ATTEMPTS }),
threshold = THRESHOLD,
smoothing = SMOOTHING)
smoothing = SMOOTHING,
windowSize = WINDOW_SIZE)
.build()
val requestBytes = ByteArray(message.getSizeNoTag())
message.writeTo(CodedOutputStream(requestBytes))
@@ -103,9 +103,6 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
}
val anglesDistances = mutableMapOf<Angle, AngleData>()
for (i in 0..angles.size - 1) {
if (Math.abs(distances[i]) < 0.01) {
continue
}
anglesDistances.put(angles[i], AngleData(angles[i], distances[i]))
}
server/src/main/java/algorithm/RoomBypassingAlgorithm.kt
+104 -37
View File
@@ -1,6 +1,7 @@
package algorithm
import RouteMetricRequest
import SonarRequest
import algorithm.geometry.Angle
import algorithm.geometry.AngleData
import algorithm.geometry.Point
@@ -11,14 +12,19 @@ import java.util.concurrent.Exchanger
class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : AbstractAlgorithm(thisCar, exchanger) {
// TODO: set to appropriate values
override val THRESHOLD = 0
override val ATTEMPTS: Int = 3
override val ATTEMPTS: Int = 5
override val SMOOTHING = SonarRequest.Smoothing.MEDIAN
override val WINDOW_SIZE = 3
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
private val MAX_DISTANCE_TO_WALL_AHEAD = 30 // reached the corner and should turn left
private val OUTER_CORNER_DISTANCE_THRESHOLD = 60 // reached outer corner
private val ISOSCALENESS_MIN_DIFF = 10 // have to correct alignment to the wall
private val ISOSCALENESS_MAX_DIFF = 50 // almost reached outer corner but not yet, and 120 meas. founds far wall
private val DISTANCE_TO_WALL_UPPER_BOUND = 50 // have to move closer to the parallel wall
private val DISTANCE_TO_WALL_LOWER_BOUND = 25 // have to move farther to the parallel wall
private val SPURIOUS_REFLECTION_DIFF = 70 // we're approaching corner and get spurious reflection from two walls on 60 meas. ! Should be before outer corner case !
private var calibrateAfterRotate = false
var carX = 0
var carY = 0
@@ -37,16 +43,21 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
private fun noOrthogonalMeasurementFound(anglesDistances: Map<Angle, AngleData>, state: CarState): RouteMetricRequest {
val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0))
resultBuilder.setDirections(getIntArray(RIGHT))
resultBuilder.setDistances(getIntArray(10))
resultBuilder.setDirections(getIntArray(FORWARD, RIGHT, FORWARD))
val wallAngle = calculateAngle(anglesDistances, state)
resultBuilder.setDistances(getIntArray(15, wallAngle.degs(), 60))
addWall(-wallAngle)
calibrateAfterRotate = true
return resultBuilder.build()
}
private fun wallAheadFound(anglesDistances: Map<Angle, AngleData>, state: CarState): RouteMetricRequest {
val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0))
resultBuilder.setDirections(getIntArray(LEFT))
resultBuilder.setDistances(getIntArray(calculateAngle(anglesDistances, state).degs()))
addWall()
resultBuilder.setDirections(getIntArray(LEFT, FORWARD))
val wallAngle = calculateAngle(anglesDistances, state)
resultBuilder.setDistances(getIntArray(wallAngle.degs(), 15))
addWall(wallAngle)
calibrateAfterRotate = true
return resultBuilder.build()
}
@@ -71,31 +82,53 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
return resultBuilder.build()
}
private fun addWall() {
private fun moveForward(): RouteMetricRequest {
val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0))
resultBuilder.setDirections(getIntArray(FORWARD))
resultBuilder.setDistances(getIntArray(20))
return resultBuilder.build()
}
private fun addWall(angleWithPrevWall: Angle) {
RoomModel.updateWalls()
if (RoomModel.walls.last() == RoomModel.walls.first() && RoomModel.walls.size > 1) {
println("YEAH!")
val firstWall = RoomModel.walls.first()
val lastWall = RoomModel.walls.last()
if (firstWall == lastWall && RoomModel.walls.size > 1) {
RoomModel.finished = true
RoomModel.walls.removeAt(RoomModel.walls.size - 1)
lastWall.pushBackPoint(firstWall.rawPoints.last())
lastWall.markAsFinished()
RoomModel.walls.removeAt(0)
} else {
RoomModel.walls.add(Wall(lastWall.wallAngleOX + angleWithPrevWall))
}
RoomModel.walls.add(Wall())
}
override fun getCommand(anglesDistances: Map<Angle, AngleData>, state: CarState): RouteMetricRequest {
val dist0 = anglesDistances[Angle(0)]
val dist60 = anglesDistances[Angle(60)]
val dist90 = anglesDistances[Angle(90)]
val dist120 = anglesDistances[Angle(120)]
val dist0 = anglesDistances[Angle(0)]!!
val dist60 = anglesDistances[Angle(60)]!!
val dist90 = anglesDistances[Angle(90)]!!
val dist120 = anglesDistances[Angle(120)]!!
val sonarAngle = carAngle - 90
// Check most basic measurements: 60/90/120
if (dist90 == null) {
return noOrthogonalMeasurementFound(anglesDistances, state)
if (calibrateAfterRotate) {
if (Math.abs(dist60.distance - dist120.distance) > 4) {
println("Flaw in align found, correcting")
val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0))
val rotationDirection = if (anglesDistances[Angle(120)]!!.distance > anglesDistances[Angle(60)]!!.distance) LEFT else RIGHT
resultBuilder.setDirections(getIntArray(rotationDirection))
resultBuilder.setDistances(getIntArray(5))
return resultBuilder.build()
} else {
calibrateAfterRotate = false
carAngle = RoomModel.walls.last().wallAngleOX.degs()
}
}
if (dist120 == null || dist60 == null) {
return noParallelWallFound(anglesDistances, state)
// Check most basic measurements: 60/90/120
if (dist90.distance == -1 || dist90.distance > OUTER_CORNER_DISTANCE_THRESHOLD) {
println("No orthogonal measurement found")
return noOrthogonalMeasurementFound(anglesDistances, state)
}
// Add point to room map
@@ -103,31 +136,65 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
x = carX + dist90.distance * Math.cos(degreesToRadian(sonarAngle)),
y = carY + dist90.distance * Math.sin(degreesToRadian(sonarAngle))
)
RoomModel.walls.last().addPoint(point)
RoomModel.walls.last().pushBackPoint(point)
// Check if corner reached
if (dist0 != null && dist0.distance < MAX_DISTANCE_TO_WALL_AHEAD) {
if (dist0.distance != -1 && dist0.distance < MAX_DISTANCE_TO_WALL_AHEAD) {
println("Wall ahead found")
return wallAheadFound(anglesDistances, state)
}
// Checks for paralleleness
// Big fail, try to do something safe
if (dist120.distance == -1 && dist60.distance == -1) {
println("No parallel wall found")
return noParallelWallFound(anglesDistances, state)
}
// Check if wall is not parallel
val diff = Math.abs(dist120.distance - dist60.distance)
if (diff > ISOSCALENESS_THRESHOLD) {
// Big fail too
if (dist120.distance == -1) {
println("No back measurement found, wtf")
return moveForward()
}
// Approaching inner corner and getting spurious reflection from 2 walls on 60 - just move forward to get closer to corner
if (dist60.distance - dist0.distance > SPURIOUS_REFLECTION_DIFF) {
println("Spurious reflection detected, moving forward")
return moveForward()
}
// Approaching outer corner (parallel wall is ending soon, but not yet) - just move forward to get to the end of the wall
if (dist60.distance == -1 || Math.abs(dist120.distance - dist60.distance) > ISOSCALENESS_MAX_DIFF) {
println("Aprroaching outer corner, moving forward")
return moveForward()
}
RoomModel.walls.last().rawPoints.removeAt(RoomModel.walls.last().rawPoints.size - 1)
for (i in 60..120 step 10) {
val point = Point(
x = carX + anglesDistances[Angle(i)]!!.distance * Math.cos(degreesToRadian(carAngle - i)),
y = carY + anglesDistances[Angle(i)]!!.distance * Math.sin(degreesToRadian(carAngle - i))
)
RoomModel.walls.last().pushBackPoint(point)
}
// Check if realignment need
if (Math.abs(dist60.distance - dist120.distance) > ISOSCALENESS_MIN_DIFF) {
println("Flaw in align found, correcting")
return alignParallelToWall(anglesDistances, state)
}
// car stay parallel with wall
// carAngle = RoomModel.walls.last().wallAngleOX.degs()
// Check if wall is too close or too far
if (dist90.distance > 50 || dist90.distance < 25) {
if (dist90.distance > DISTANCE_TO_WALL_UPPER_BOUND || dist90.distance < DISTANCE_TO_WALL_LOWER_BOUND) {
println("Flaw in distance to the parallel wall found, correcting")
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()
println("Default case: moving forward")
return moveForward()
}
private fun calculateAngle(anglesDistances: Map<Angle, AngleData>, state: CarState): Angle {
server/src/main/java/algorithm/RoomModel.kt
+38 -23
View File
@@ -3,6 +3,7 @@ package algorithm
import DebugClInterface
import DebugResponse
import Waypoints
import algorithm.geometry.Angle
import algorithm.geometry.Line
import algorithm.geometry.Point
import algorithm.geometry.Wall
@@ -10,9 +11,7 @@ import objects.Car
import java.util.*
object RoomModel {
val lines = arrayListOf<Line>()
val walls = arrayListOf<Wall>(Wall())
val walls = arrayListOf<Wall>(Wall(Angle(0)))
var finished = false
val linesModel = listOf(Line(0.0, 1.0, -300.0),
@@ -61,33 +60,49 @@ object RoomModel {
val line2 = walls[walls.size - 2].line
val intersection: Point = line1.intersect(line2)
walls[walls.size - 2].addPoint(intersection)
val lastWall = walls[walls.size - 1]
lastWall.pushFrontPoint(intersection)
if (lastWall.isHorizontal()) {
lastWall.line = Line(0.0, 1.0, -intersection.y)
} else if (lastWall.isVertical()) {
lastWall.line = Line(1.0, 0.0, -intersection.x)
}
walls[walls.size - 2].pushBackPoint(intersection)
walls[walls.size - 2].markAsFinished()
}
fun getDebugInfo(): DebugResponse {
val aRef = IntArray(linesModel.size)
val bRef = IntArray(linesModel.size)
val cRef = IntArray(linesModel.size)
val a = IntArray(lines.size)
val b = IntArray(lines.size)
val c = IntArray(lines.size)
val points = getWallsPoints()
val begin_x = IntArray(points.size)
val begin_y = IntArray(points.size)
val end_x = IntArray(points.size)
val end_y = IntArray(points.size)
val rawPoints = walls.flatMap({ it.rawPoints }).toTypedArray()
val pointsX = rawPoints.map { it.x.toInt() }.toIntArray()
val pointsY = rawPoints.map { it.y.toInt() }.toIntArray()
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()
end_x[i] = (nextPoint.x + 0.5).toInt()
end_y[i] = (nextPoint.y + 0.5).toInt()
}
val alg = DebugClInterface.algorithmImpl
val car = if (alg == null) Car(0, "", 0) else alg.thisCar
val mult = 100
linesModel.forEachIndexed { idx, line ->
aRef[idx] = (line.A * mult).toInt()
bRef[idx] = (line.B * mult).toInt()
cRef[idx] = (line.C * mult).toInt()
}
lines.forEachIndexed { idx, line ->
a[idx] = (line.A * mult).toInt()
b[idx] = (line.B * mult).toInt()
c[idx] = (line.C * mult).toInt()
if (alg is RoomBypassingAlgorithm) {
return DebugResponse.BuilderDebugResponse(begin_x, begin_y, end_x, end_y, alg.carX,
alg.carY, alg.carAngle, pointsX, pointsY).build()
}
return DebugResponse.BuilderDebugResponse(begin_x, begin_y, end_x, end_y, 0,
0, 0, pointsX, pointsY).build()
val resultBuilder = DebugResponse.BuilderDebugResponse(a, b, c, car.x, car.y, car.angle, aRef, bRef, cRef)
return resultBuilder.build()
}
}
server/src/main/java/algorithm/geometry/Angle.kt
+4
View File
@@ -16,6 +16,10 @@ data class Angle(private val degrees: Int) {
return Angle(degrees - other.degrees)
}
operator fun unaryMinus(): Angle {
return Angle(-degrees)
}
operator fun plus(other: Angle): Angle {
return Angle(degrees + other.degrees)
}
server/src/main/java/algorithm/geometry/Line.kt
+6 -1
View File
@@ -1,7 +1,7 @@
package algorithm.geometry
class Line(var A: Double, var B: Double, var C: Double) {
val COMPARISON_THRESHOLD = 0.5
val COMPARISON_THRESHOLD = 20
init {
normalize()
@@ -55,4 +55,9 @@ class Line(var A: Double, var B: Double, var C: Double) {
return metricDist(other) < COMPARISON_THRESHOLD
}
fun getDirectionVector(): Vector {
return Vector (A, -B)
}
}
server/src/main/java/algorithm/geometry/Point.kt
+3 -2
View File
@@ -1,6 +1,7 @@
package algorithm.geometry
data class Point(val x: Double, val y: Double) {
operator fun plus(other: Vector): Point {
return Point(x + other.x, y + other.y)
}
}
server/src/main/java/algorithm/geometry/Util.kt
+1 -1
View File
@@ -2,5 +2,5 @@ package algorithm.geometry
object Util {
val eps = 1e-2
val eps = 0.1
}
server/src/main/java/algorithm/geometry/Vector.kt
+19 -1
View File
@@ -1,9 +1,10 @@
package algorithm.geometry
class Vector constructor(val x: Double, val y: Double) {
class Vector constructor(var x: Double, var y: Double) {
constructor(x1: Double, y1: Double, x2: Double, y2: Double) : this(x2 - x1, y2 - y1)
constructor(begin: Point, end: Point) : this (begin.x, begin.y, end.x, end.y)
fun scalarProduct(vector: Vector): Double {
return this.x * vector.x + this.y * vector.y
@@ -13,4 +14,21 @@ class Vector constructor(val x: Double, val y: Double) {
return Math.sqrt(x * x + y * y)
}
fun angleTo(other: Vector): Angle {
var sp = scalarProduct(other)
var cosinus = sp / length() / other.length()
return Angle(Math.acos(cosinus))
}
fun inverse() {
x *= -1
y *= -1
}
fun normalize() {
val div = Math.sqrt(x * x + y * y)
x /= div
y /= div
}
}
server/src/main/java/algorithm/geometry/Wall.kt
+89 -15
View File
@@ -1,34 +1,108 @@
package algorithm.geometry
import java.util.*
import algorithm.geometry.Vector
data class Wall(val points: ArrayList<Point> = arrayListOf<Point>(),
data class Wall(val wallAngleOX: Angle,
val rawPoints: ArrayList<Point> = arrayListOf<Point>(),
var line: Line = Line(0.0, 0.0, 0.0)) {
var isFinished = false
val MAX_REGRESSION_ERROR = 10.0
fun addPoint(point: Point) {
points.add(point)
fun pushBackPoint(point: Point) {
rawPoints.add(point)
line = approximatePointsByLine()
}
fun pushFrontPoint(point: Point) {
rawPoints.add(0, point)
line = approximatePointsByLine()
}
var points: ArrayList<Point> = arrayListOf()
fun markAsFinished() {
isFinished = true
points = generatePoints()
}
private fun generatePoints(): ArrayList<Point> {
val direction = Vector(rawPoints.first(), rawPoints.last())
val res: ArrayList<Point> = arrayListOf()
val pointsCount = direction.length().toInt()
direction.normalize()
var curPoint = rawPoints.first()
res.add(curPoint)
val endPoint = rawPoints.last()
for (i in 0..pointsCount - 1) {
curPoint += direction
res.add(curPoint)
}
res.add(endPoint)
return res
}
private fun approximatePointsByLine(): Line {
var n = 0
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 }
var sumx = 0.0
var sumy = 0.0
var sumx2 = 0.0
for (pt in rawPoints) {
sumx += pt.x
sumx2 += pt.x * pt.x
sumy += pt.y
n++
}
val xbar = sumx / n
val ybar = sumy / n
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)
// second pass: compute summary statistics
var xxbar = 0.0
var yybar = 0.0
var xybar = 0.0
for (pt in rawPoints) {
xxbar += (pt.x - xbar) * (pt.x - xbar)
yybar += (pt.y - ybar) * (pt.y - ybar)
xybar += (pt.x - xbar) * (pt.y - ybar)
}
val k = (pointsCount * sumXY - sumX * sumY) / (pointsCount * sumXQuad - sumX * sumX)
val b = (sumY - sumX * k) / pointsCount
var beta1 = xybar / xxbar
var beta0 = ybar - beta1 * xbar
return Line(-k, 1.0, -b)
// analyze results
val df = n - 2
var rss = 0.0 // residual sum of squares
var ssr = 0.0 // regression sum of squares
for (pt in rawPoints) {
val fit = beta1 * pt.x + beta0
rss += (fit - pt.y) * (fit - pt.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) {
beta1 = xybar / yybar
beta0 = xbar - beta1 * ybar
return Line(1.0, -beta1, -beta0)
}
return Line(-beta1, 1.0, -beta0)
}
fun isVertical(): Boolean {
return Math.abs(wallAngleOX.degs()) % 180 == 90
}
fun isHorizontal(): Boolean {
return Math.abs(wallAngleOX.degs()) % 180 == 0
}
override fun equals(other: Any?): Boolean {