From 92836b838f0770c6a078c388fadd56e971ea042b Mon Sep 17 00:00:00 2001 From: MaximZaitsev Date: Mon, 5 Sep 2016 15:12:46 +0300 Subject: [PATCH] now room configurations can read from files --- car_srv/kotlinSrv/src/Main.kt | 22 ++- car_srv/kotlinSrv/src/Test.kt | 2 +- .../control/emulator/ControllerEmulator.kt | 21 +-- car_srv/kotlinSrv/src/control/emulator/Rng.kt | 2 + car_srv/kotlinSrv/src/room/Room.kt | 147 +++++++++--------- car_srv/kotlinSrv/src/room/Wall.kt | 33 +++- 6 files changed, 128 insertions(+), 99 deletions(-) diff --git a/car_srv/kotlinSrv/src/Main.kt b/car_srv/kotlinSrv/src/Main.kt index e43e2279989..aabb105d316 100644 --- a/car_srv/kotlinSrv/src/Main.kt +++ b/car_srv/kotlinSrv/src/Main.kt @@ -20,11 +20,11 @@ fun main(args: Array) { println("incorrect config format!") js("process.exit(1)") } - val clArgs = js("process.argv") var runAsEmulator = false var runTests = false var nextSeed = false + var room: Room = Room.testRoom1() for (arg in clArgs) { if (nextSeed) { Rng.SEED = parseInt(arg, 10).toLong() @@ -34,17 +34,27 @@ fun main(args: Array) { when (arg) { "-t" -> runTests = true "-e" -> runAsEmulator = true - "-tr1" -> Room.walls = Room.testRoom1() - "-tr2" -> Room.walls = Room.testRoom2() - "-tr3" -> Room.walls = Room.testRoom3() - "-r" -> Room.randomOn() + "-tr1" -> room = Room.testRoom1() + "-tr2" -> room = Room.testRoom2() + "-tr3" -> room = Room.testRoom3() + "-r" -> Rng.ADD_RANDOM = true "-s" -> nextSeed = true } + if (arg.toString().contains("--room=")) { + val pathToFile = arg.toString().split("=")[1].replace("\"", "") + val data: String = fs.readFileSync(pathToFile, "utf8") + val roomOrNull = Room.roomFromString(data) + if (roomOrNull == null) { + println("error parsing string $data \nto room") + return + } + room = roomOrNull + } } val carController = if (runAsEmulator) { McState.instance.connect("") - ControllerEmulator() + ControllerEmulator(room) } else { ControllerToUsb() } diff --git a/car_srv/kotlinSrv/src/Test.kt b/car_srv/kotlinSrv/src/Test.kt index 5518c944892..62b5bc60b76 100644 --- a/car_srv/kotlinSrv/src/Test.kt +++ b/car_srv/kotlinSrv/src/Test.kt @@ -6,7 +6,7 @@ fun runTests() { } private fun testCarEmulator() { - val controller = ControllerEmulator() + val controller = ControllerEmulator(Room.testRoom1()) var sonarRequest = SonarRequest.BuilderSonarRequest(IntArray(2, { x -> x * 90 }), IntArray(0), 0, SonarRequest.Smoothing.NONE).build() var sonarResponse = SonarResponse.BuilderSonarResponse(IntArray(0)).build() controller.executeRequestSensorData(sonarRequest, { bytes -> diff --git a/car_srv/kotlinSrv/src/control/emulator/ControllerEmulator.kt b/car_srv/kotlinSrv/src/control/emulator/ControllerEmulator.kt index fc447cc5499..abd20fcc4c0 100644 --- a/car_srv/kotlinSrv/src/control/emulator/ControllerEmulator.kt +++ b/car_srv/kotlinSrv/src/control/emulator/ControllerEmulator.kt @@ -4,8 +4,8 @@ import CarState import RouteMetricRequest import RouteRequest import RouteResponse -import SonarRequest import SonarExploreAngleRequest +import SonarRequest import SonarResponse import control.Controller import encodeProtoBuf @@ -14,7 +14,7 @@ import geometry.Vector import room.Room import kotlin.Pair -class ControllerEmulator : Controller { +class ControllerEmulator(val room: Room) : Controller { private val MOVE_VELOCITY = 0.05//sm/ms private val ROTATION_VELOCITY = 0.05//degrees/ms @@ -111,7 +111,7 @@ class ControllerEmulator : Controller { if (distance == -1) { distances.add(distance) } else { - val delta = if (Room.ADD_RANDOM) Rng.nextInt(-2, 2) else 0//return one of -2 -1 0 1 or 2 + val delta = if (Rng.ADD_RANDOM) Rng.nextInt(-2, 2) else 0//return one of -2 -1 0 1 or 2 distances.add(distance + delta) } } @@ -122,7 +122,7 @@ class ControllerEmulator : Controller { private fun getDistance(xSensor0: Int, ySensor0: Int, sensorLine: Line, sensorVector: Vector): Int { var result = Int.MAX_VALUE - for (wall in Room.walls) { + for (wall in room.walls) { val wallLine = wall.line val slope = sensorLine.A * wallLine.B - sensorLine.B * wallLine.A if (Math.abs(slope) < 0.01) { @@ -148,9 +148,6 @@ class ControllerEmulator : Controller { result = currentDistance } } -// if (result < 5 || result > 500) { -// return -1 -// } return result } @@ -163,14 +160,6 @@ class ControllerEmulator : Controller { return angleTmp % 360 } - //return random int from array - private fun getRandomIntFrom(values: IntArray): Int { - val randomValue = (Math.random() * 1000).toInt()//value in [0,999] - val randomArrayIdx = randomValue * values.size / 1000//index in [0, values.size-1] - return values[randomArrayIdx] - } - - fun executeCommand(commands: List>, currentCommandIdx: Int, callBack: (ByteArray) -> Unit) { if (currentCommandIdx == commands.size) { val responseMessage = RouteResponse.BuilderRouteResponse(0).build() @@ -182,7 +171,7 @@ class ControllerEmulator : Controller { //refresh car state val carInstance = CarState.instance val commandDistance = currentCommand.second - val coef = if (Room.ADD_RANDOM) Rng.nextInt(900, 1100).toDouble() / 1000.0 else 1.0 + val coef = if (Rng.ADD_RANDOM) Rng.nextInt(900, 1100).toDouble() / 1000.0 else 1.0 val commandDistanceIncludeRandom = (commandDistance * coef).toInt() when (currentCommand.first) { MoveDirection.FORWARD -> carInstance.moving(commandDistanceIncludeRandom) diff --git a/car_srv/kotlinSrv/src/control/emulator/Rng.kt b/car_srv/kotlinSrv/src/control/emulator/Rng.kt index eebb6980f5f..0d88af4f150 100644 --- a/car_srv/kotlinSrv/src/control/emulator/Rng.kt +++ b/car_srv/kotlinSrv/src/control/emulator/Rng.kt @@ -7,6 +7,8 @@ object Rng { val c = 1013904223L val mod = 2147483648L + var ADD_RANDOM = false + fun abs(value: Long): Long { if (value < 0) return -value diff --git a/car_srv/kotlinSrv/src/room/Room.kt b/car_srv/kotlinSrv/src/room/Room.kt index 885faa89bac..71610b09298 100644 --- a/car_srv/kotlinSrv/src/room/Room.kt +++ b/car_srv/kotlinSrv/src/room/Room.kt @@ -2,86 +2,83 @@ package room import geometry.Line -object Room { +class Room() { + val walls = arrayListOf() - var walls = testRoom1() - var ADD_RANDOM = false - - /* - _________ - | | - | | - |________| - - */ - - fun randomOn() { - ADD_RANDOM = true + constructor(walls: Collection) : this() { + this.walls.addAll(walls) } - fun testRoom1(): List { - val upLine = Line(0.0, 1.0, -300.0) - val leftLine = Line(1.0, 0.0, 150.0) - val bottomLine = Line(0.0, 1.0, 45.0) - val rightLine = Line(1.0, 0.0, -200.0) -// - val walls = listOf(Wall(upLine, 200, -150, 300, 300), - Wall(leftLine, -150, -150, 300, -45), - Wall(bottomLine, -150, 200, -45, -45), - Wall(rightLine, 200, 200, -45, 300) - ) - return walls + companion object { + fun roomFromString(roomString: String): Room? { + val result = Room() + roomString.split("\n").forEach { wallString -> + if (!wallString.equals("")) { + val wall = Wall.wallFromString(wallString) ?: return null + result.walls.add(wall) + } + } + return result + } + + fun testRoom1(): Room { + val upLine = Line(0.0, 1.0, -300.0) + val leftLine = Line(1.0, 0.0, 150.0) + val bottomLine = Line(0.0, 1.0, 45.0) + val rightLine = Line(1.0, 0.0, -200.0) + val walls = listOf(Wall(upLine, 200, -150, 300, 300), + Wall(leftLine, -150, -150, 300, -45), + Wall(bottomLine, -150, 200, -45, -45), + Wall(rightLine, 200, 200, -45, 300) + ) + return Room(walls) + } + + fun testRoom2(): Room { + + val upLine = Line(0.0, 1.0, -300.0) + val leftLine = Line(1.0, 0.0, 150.0) + val bottomLine = Line(0.0, 1.0, 45.0) + val rightLine = Line(1.0, 0.0, -200.0) + + val bottomLine2 = Line(0.0, 1.0, -100.0) + val rightLine2 = Line(1.0, 0.0, -300.0) + + val walls = listOf(Wall(upLine, 300, -150, 300, 300), + Wall(leftLine, -150, -150, 300, -45), + Wall(bottomLine, -150, 200, -45, -45), + Wall(rightLine, 200, 200, -45, 100), + Wall(bottomLine2, 200, 300, 100, 100), + Wall(rightLine2, 300, 300, 100, 300) + ) + return Room(walls) + } + + fun testRoom3(): Room { + + val upLine = Line(0.0, 1.0, -300.0) + val leftLine = Line(1.0, 0.0, 150.0) + val bottomLine = Line(0.0, 1.0, 20.0) + val rightLine = Line(-1.2, 1.0, 140.0) + val rightLine2 = Line(1.0, 0.0, -200.0) + + val walls = listOf(Wall(upLine, 200, -150, 300, 300), + Wall(leftLine, -150, -150, 300, -20), + Wall(bottomLine, -150, 100, -20, -20), + Wall(rightLine, 100, 200, -20, 100), + Wall(rightLine2, 200, 200, 100, 300) + ) + + return Room(walls) + } } - fun testRoom2(): List { - /* - ________ - | | - | ___| - |___| - - */ - val upLine = Line(0.0, 1.0, -300.0) - val leftLine = Line(1.0, 0.0, 150.0) - val bottomLine = Line(0.0, 1.0, 45.0) - val rightLine = Line(1.0, 0.0, -200.0) - - val bottomLine2 = Line(0.0, 1.0, -100.0) - val rightLine2 = Line(1.0, 0.0, -300.0) - - val walls = listOf(Wall(upLine, 300, -150, 300, 300), - Wall(leftLine, -150, -150, 300, -45), - Wall(bottomLine, -150, 200, -45, -45), - Wall(rightLine, 200, 200, -45, 100), - Wall(bottomLine2, 200, 300, 100, 100), - Wall(rightLine2, 300, 300, 100, 300) - ) - return walls + fun roomToString(): String { + var result = "" + walls.forEach { + result += it.wallToString() + "\n" + } + return result } - - - fun testRoom3(): List { - /* - _______ - | | - |____/ - - - */ - val upLine = Line(0.0, 1.0, -300.0) - val leftLine = Line(1.0, 0.0, 150.0) - val bottomLine = Line(0.0, 1.0, 20.0) - val rightLine = Line(-1.2, 1.0, 140.0) - val rightLine2 = Line(1.0, 0.0, -200.0) - - val walls = listOf(Wall(upLine, 200, -150, 300, 300), - Wall(leftLine, -150, -150, 300, -20), - Wall(bottomLine, -150, 100, -20, -20), - Wall(rightLine, 100, 200, -20, 100), - Wall(rightLine2, 200, 200, 100, 300) - ) - return walls - } - } diff --git a/car_srv/kotlinSrv/src/room/Wall.kt b/car_srv/kotlinSrv/src/room/Wall.kt index babd1ccbde4..4b1e5043282 100644 --- a/car_srv/kotlinSrv/src/room/Wall.kt +++ b/car_srv/kotlinSrv/src/room/Wall.kt @@ -4,4 +4,35 @@ import geometry.Line class Wall constructor(val line: Line, val xFrom: Int, val xTo: Int, val yFrom: Int, val yTo: Int) { -} \ No newline at end of file + companion object { + fun wallFromString(wall: String): Wall? { + val points = wall.replace(" ", "").split(",") + var xFrom = 0 + var xTo = 0 + var yTo = 0 + var yFrom = 0 + points.forEach { + val keyValue = it.split(":") + if (keyValue.size != 2) { + return null + } + try { + when (keyValue[0]) { + "xFrom" -> xFrom = parseInt(keyValue[1]) + "yFrom" -> yFrom = parseInt(keyValue[1]) + "xTo" -> xTo = parseInt(keyValue[1]) + "yTo" -> yTo = parseInt(keyValue[1]) + } + } catch (e: Exception) { + return null + } + } + val line = Line((yFrom - yTo).toDouble(), (xTo - xFrom).toDouble(), (xFrom * yTo - yFrom * xTo).toDouble()) + return Wall(line, xFrom, xTo, yFrom, yTo) + } + } + + fun wallToString(): String { + return "xFrom: $xFrom, yFrom: $yFrom, xTo: $xTo, yTo: $yTo" + } +}