From a589f7da9cf399cb0e22ad13995eaefae7cdf999 Mon Sep 17 00:00:00 2001 From: MaximZaitsev Date: Mon, 5 Sep 2016 13:09:49 +0300 Subject: [PATCH] main server refactoring --- server/src/main/java/DebugClInterface.kt | 285 ------------------ server/src/main/java/Logger.kt | 6 +- server/src/main/java/ServerMain.kt | 8 +- .../main/java/algorithm/AbstractAlgorithm.kt | 82 +---- .../main/java/algorithm/AlgorithmThread.kt | 19 +- .../src/main/java/algorithm/CarController.kt | 60 ++++ .../java/algorithm/RoomBypassingAlgorithm.kt | 87 ++---- .../main/java/algorithm/geometry/AngleData.kt | 1 - .../algorithm/geometry/DoubleComparison.kt | 8 +- .../src/main/java/algorithm/geometry/Line.kt | 29 +- .../src/main/java/algorithm/geometry/Util.kt | 6 - .../main/java/algorithm/geometry/Vector.kt | 2 +- .../src/main/java/algorithm/geometry/Wall.kt | 35 +-- .../main/java/clInterface/DebugClInterface.kt | 62 ++++ .../java/clInterface/executor/Algorithm.kt | 30 ++ .../clInterface/executor/CarsInformation.kt | 12 + .../clInterface/executor/CommandExecutor.kt | 7 + .../clInterface/executor/DebugInformation.kt | 49 +++ .../main/java/clInterface/executor/Explore.kt | 28 ++ .../java/clInterface/executor/RouteMetric.kt | 86 ++++++ .../main/java/clInterface/executor/Sonar.kt | 72 +++++ server/src/main/java/net/ServerHandler.kt | 1 - server/src/main/java/net/web/server/Server.kt | 1 + .../net/web/server/handlers/ChangeMode.kt | 6 +- .../net/web/server/handlers/DirectionOrder.kt | 4 +- server/src/main/java/objects/Car.kt | 28 ++ server/src/main/java/objects/CarConnection.kt | 1 - .../CarController.kt | 2 +- .../RoomScanner.kt | 2 +- .../{RoomScanner => roomScanner}/utils.kt | 2 +- 30 files changed, 524 insertions(+), 497 deletions(-) delete mode 100644 server/src/main/java/DebugClInterface.kt create mode 100644 server/src/main/java/algorithm/CarController.kt delete mode 100644 server/src/main/java/algorithm/geometry/Util.kt create mode 100644 server/src/main/java/clInterface/DebugClInterface.kt create mode 100644 server/src/main/java/clInterface/executor/Algorithm.kt create mode 100644 server/src/main/java/clInterface/executor/CarsInformation.kt create mode 100644 server/src/main/java/clInterface/executor/CommandExecutor.kt create mode 100644 server/src/main/java/clInterface/executor/DebugInformation.kt create mode 100644 server/src/main/java/clInterface/executor/Explore.kt create mode 100644 server/src/main/java/clInterface/executor/RouteMetric.kt create mode 100644 server/src/main/java/clInterface/executor/Sonar.kt rename server/src/main/java/{RoomScanner => roomScanner}/CarController.kt (99%) rename server/src/main/java/{RoomScanner => roomScanner}/RoomScanner.kt (98%) rename server/src/main/java/{RoomScanner => roomScanner}/utils.kt (98%) diff --git a/server/src/main/java/DebugClInterface.kt b/server/src/main/java/DebugClInterface.kt deleted file mode 100644 index e824c4ed612..00000000000 --- a/server/src/main/java/DebugClInterface.kt +++ /dev/null @@ -1,285 +0,0 @@ -import RoomScanner.serialize -import algorithm.AbstractAlgorithm -import algorithm.AlgorithmThread -import algorithm.RoomBypassingAlgorithm -import algorithm.RoomModel -import net.car.client.Client -import objects.Car -import objects.Environment -import java.net.ConnectException -import java.rmi.UnexpectedException - -object DebugClInterface { - - //todo remove - var algorithmImpl: AbstractAlgorithm? = null - var algorithmThread: AlgorithmThread? = null - - private val routeRegex = Regex("route [0-9]{1,10}") - private val sonarRegex = Regex("sonar [0-9]{1,10}") - private val helpString = "available handlers:\n" + - "cars - get list of connected cars\n" + - "route [car_id] - setting a route for net.car with net.car id.\n" + - "sonar [car_id] - get sonar data\n" + - "dbinfo [car_id] [type] - refresh all net.car locations\n" + - "lines - print lines, detected by net.car\n" + - "alg [count] - run algorithm. Make [count] iteration\n" + - "type is string name of value or int value. available values: MEMORYSTATS - 0\n" + - "stop - exit from this interface and stop all threads\n" - - fun run() { - - println(helpString) - while (true) { - val readString = readLine() - if (readString == null || readString.equals("stop")) { - break - } - if (readString.equals("")) { - continue - } - - try { - executeCommand(readString) - } catch (exception: Exception) { - exception.printStackTrace() - println("Fail to execute command[$readString]: $exception") - } - } - } - - private fun printNotSupportedCommand(command: String) { - println("not supported command: $command") - println(helpString) - } - - private fun executeCommand(readString: String) { - val commandType = readString.split(" ")[0].toLowerCase() - when (commandType) { - "cars" -> { - synchronized(Environment, { - println(Environment.map.values) - }) - } - "route" -> executeRouteCommand(readString) - "sonar" -> executeSonarCommand(readString) - "dbinfo" -> executeDebugInfoCommand(readString) - "lines" -> algorithm.RoomModel.walls.forEach { println(it.line) } - "pos" -> { - val tmp = algorithmImpl - if (tmp is RoomBypassingAlgorithm) { - println("walls: ${RoomModel.walls}") - println("x: ${tmp.thisCar.x} y: ${tmp.thisCar.y} angle:${tmp.thisCar.angle}") - } - } - "alg" -> executeAlg(readString) - "explore" -> executeExplore(readString) - else -> printNotSupportedCommand(readString) - } - } - - private fun executeExplore(readString: String) { - val params = readString.split(" ") - val car = Environment.map[params[1].toInt()]!! - val angle = params[2].toInt() - val window = params[3].toInt() - - val request = SonarExploreAngleRequest.BuilderSonarExploreAngleRequest(angle, window).build() - val responseData = car.carConnection.sendRequest( - Client.Request.EXPLORE_ANGLE, - serialize(request.getSizeNoTag(), { request.writeTo(it) }) - ).get().responseBodyAsBytes - - val distances = SonarExploreAngleResponse.BuilderSonarExploreAngleResponse(IntArray(0)).parseFrom(CodedInputStream(responseData)).build().distances - - println("Received distances: [${distances.joinToString()}]") - } - - private fun executeAlg(readString: String) { - val params = readString.split(" ") - val count = - if (params.size == 2) try { - params[1].toInt() - } catch (e: Exception) { - 1 - } else 1 - - var alg = algorithmImpl - if (alg == null) { - alg = RoomBypassingAlgorithm(Environment.map.values.last()) - } - var algThread = algorithmThread - if (algThread == null) { - algThread = AlgorithmThread(alg) - algThread.start() - } - algorithmImpl = alg - algThread.setCount(count) - algorithmThread = algThread - } - - private fun executeDebugInfoCommand(readString: String) { - val params = readString.split(" ") - val car = Environment.map[params[1].toInt()]!! - val type = DebugRequest.Type.fromIntToType(params[2].toInt()) - - val request = DebugRequest.BuilderDebugRequest(type).build() - val requestType = when (type) { - DebugRequest.Type.MEMORY_STATS -> Client.Request.DEBUG_MEMORY - DebugRequest.Type.SONAR_STATS -> Client.Request.DEBUG_SONAR - else -> throw UnexpectedException(type.toString()) - } - - val responseData = car.carConnection.sendRequest( - requestType, - serialize(request.getSizeNoTag(), { request.writeTo(it) }) - ).get().responseBodyAsBytes - - when (type) { - DebugRequest.Type.MEMORY_STATS -> { - val data = DebugResponseMemoryStats.BuilderDebugResponseMemoryStats(0, 0, 0, 0).parseFrom(CodedInputStream(responseData)).build() - println("Heap static tail: ${data.heapStaticTail}") - println("Heap dynamic tail: ${data.heapDynamicTail}") - println("Heap dynamic max size: ${data.heapDynamicMaxBytes}") - println("Heap dynamic total size: ${data.heapDynamicTotalBytes}") - } - DebugRequest.Type.SONAR_STATS -> { - val data = DebugResponseSonarStats.BuilderDebugResponseSonarStats(0, 0, 0, 0).parseFrom(CodedInputStream(responseData)).build() - println("Sonar measurement total: ${data.measurementCount}") - println("Failed checksums: ${data.measurementFailedChecksum}") - println("Failed command: ${data.measurementFailedCommand}") - println("Failed distance: ${data.measurementFailedDistance}") - } - else -> throw UnexpectedException(type.toString()) - } - } - - private fun executeSonarCommand(readString: String) { - if (!sonarRegex.matches(readString)) { - println("incorrect args of command sonar.") - println(helpString) - return - } - val id: Int - try { - id = readString.split(" ")[1].toInt() - } catch (e: NumberFormatException) { - e.printStackTrace() - println("error in converting id to int type") - println(helpString) - return - } - val car: Car? = synchronized(Environment, { - Environment.map[id] - }) - if (car == null) { - println("net.car with id=$id not found") - return - } - val requestMessage = getSonarRequest() ?: return - val requestBytes = ByteArray(requestMessage.getSizeNoTag()) - requestMessage.writeTo(CodedOutputStream(requestBytes)) - try { - car.carConnection.sendRequest(Client.Request.SONAR, requestBytes) - } catch (e: ConnectException) { - synchronized(Environment, { - Environment.map.remove(id) - }) - } - } - - private fun getSonarRequest(): SonarRequest? { - println("print angles, after printing all angles print done") - val angles = arrayListOf() - while (true) { - val command = readLine()!!.toLowerCase() - when (command) { - "reset" -> return null - "done" -> { - val sonarBuilder = SonarRequest.BuilderSonarRequest(angles.toIntArray(), IntArray(angles.size, { 5 }), 3, SonarRequest.Smoothing.MEDIAN) - return sonarBuilder.build() - } - else -> { - try { - val angle = command.toInt() - if (angle < 0 || angle > 180) { - println("incorrect angle $angle. angle must be in [0,180] and div on 4") - } else { - angles.add(angle) - } - } catch (e: NumberFormatException) { - println("error in converting angle to int. try again") - } - } - } - } - } - - private fun executeRouteCommand(readString: String) { - if (!routeRegex.matches(readString)) { - println("incorrect args of command route.") - println(helpString) - return - } - val id: Int - try { - id = readString.split(" ")[1].toInt() - } catch (e: NumberFormatException) { - e.printStackTrace() - println("error in converting id to int type") - println(helpString) - return - } - val car: Car? = - synchronized(Environment, { - Environment.map[id] - }) - if (car == null) { - println("net.car with id=$id not found") - return - } - val routeMessage = getRouteMessage() ?: return - val requestBytes = ByteArray(routeMessage.getSizeNoTag()) - routeMessage.writeTo(CodedOutputStream(requestBytes)) - try { - car.carConnection.sendRequest(Client.Request.ROUTE_METRIC, requestBytes) - } catch (e: ConnectException) { - synchronized(Environment, { - Environment.map.remove(id) - }) - } - } - - private fun getRouteMessage(): RouteRequest? { - println("print way points in polar coordinate als [distance/degrees] [direction] in sm and degrees." + - "after enter all points print \"done\". for reset route print \"reset\". available directions:" + - "0 - FORWARD, 1 - BACKWARD, 2 - LEFT, 3 - RIGHT") - val distances = arrayListOf() - val angles = arrayListOf() - while (true) { - val readLine = readLine()!!.toLowerCase() - when (readLine) { - "reset" -> return null - "done" -> { - val routeBuilder = RouteRequest.BuilderRouteRequest(distances.toIntArray(), angles.toIntArray()) - return routeBuilder.build() - } - else -> { - val wayPointData = readLine.split(" ") - val distance: Int - val angle: Int - try { - distance = wayPointData[0].toInt() - angle = wayPointData[1].toInt() - distances.add(distance) - angles.add(angle) - } catch (e: NumberFormatException) { - println("error in converting target or distance to int. try again") - } catch (e: IndexOutOfBoundsException) { - println("format error, u must print two number separated by spaces. Try again") - } - } - } - } - } -} diff --git a/server/src/main/java/Logger.kt b/server/src/main/java/Logger.kt index eeb474cfed2..e524e2df200 100644 --- a/server/src/main/java/Logger.kt +++ b/server/src/main/java/Logger.kt @@ -1,6 +1,5 @@ import java.io.BufferedWriter import java.io.File -import java.io.FileOutputStream import java.io.FileWriter object Logger { @@ -14,12 +13,13 @@ object Logger { padding += paddingStep } - fun outdent() { + fun outDent() { if (padding.length == 0) { - throw IllegalArgumentException("Called outdent without corresponsing indent") + throw IllegalArgumentException("Called out dent without corresponding indent") } padding = padding.removeSuffix(paddingStep) } + fun log(msg: String) { bufferedWriter.write(padding + msg + "\n") bufferedWriter.flush() diff --git a/server/src/main/java/ServerMain.kt b/server/src/main/java/ServerMain.kt index 9bbe2abcb33..6580a7988fe 100644 --- a/server/src/main/java/ServerMain.kt +++ b/server/src/main/java/ServerMain.kt @@ -1,5 +1,6 @@ -import RoomScanner.CarController -import RoomScanner.RoomScanner +import roomScanner.CarController +import roomScanner.RoomScanner +import clInterface.DebugClInterface import net.car.Dropper import net.car.client.Client import objects.Environment @@ -20,9 +21,6 @@ fun main(args: Array) { } } - //todo запуск потока алгоритма нужно вынести сюда, - //todo а debug интерфейсутребуется исключительно возможность установки кол-ва итераций - //CL user interface DebugClInterface.run() carsDestroy.interrupt() diff --git a/server/src/main/java/algorithm/AbstractAlgorithm.kt b/server/src/main/java/algorithm/AbstractAlgorithm.kt index 1bad3d3ee5a..2389fe798e5 100644 --- a/server/src/main/java/algorithm/AbstractAlgorithm.kt +++ b/server/src/main/java/algorithm/AbstractAlgorithm.kt @@ -1,19 +1,13 @@ package algorithm -import CodedInputStream -import CodedOutputStream import Logger import RouteMetricRequest import SonarRequest -import SonarResponse import algorithm.geometry.Angle import algorithm.geometry.AngleData -import net.car.client.Client import objects.Car -import java.net.ConnectException +import roomScanner.CarController.Direction.* import java.util.* -import java.util.concurrent.TimeUnit -import java.util.concurrent.TimeoutException abstract class AbstractAlgorithm(val thisCar: Car) { @@ -21,11 +15,7 @@ abstract class AbstractAlgorithm(val thisCar: Car) { open val SMOOTHING = SonarRequest.Smoothing.NONE open val WINDOW_SIZE = 0 - protected val FORWARD = 0 - protected val BACKWARD = 1 - protected val LEFT = 2 - protected val RIGHT = 3 - + val carController = CarController(thisCar.carConnection) private val historySize = 10 private val history = Stack() @@ -44,50 +34,6 @@ abstract class AbstractAlgorithm(val thisCar: Car) { private var iterationCounter = 0 - protected fun getData(angles: Array): IntArray { - - val anglesIntArray = (angles.map { it -> it.degs() }).toIntArray() - val message = SonarRequest.BuilderSonarRequest( - angles = anglesIntArray, - attempts = IntArray(angles.size, { ATTEMPTS }), - smoothing = SMOOTHING, - windowSize = WINDOW_SIZE) - .build() - val requestBytes = ByteArray(message.getSizeNoTag()) - message.writeTo(CodedOutputStream(requestBytes)) - try { - val futureListener = thisCar.carConnection.sendRequest(Client.Request.SONAR, requestBytes) - val bytes = futureListener.get(300, TimeUnit.SECONDS).responseBodyAsBytes - val responseMessage = SonarResponse.BuilderSonarResponse(IntArray(0)).build() - responseMessage.mergeFrom(CodedInputStream(bytes)) - return responseMessage.distances - } catch (e: ConnectException) { - println("connection error!") - } catch (e: TimeoutException) { - println("don't have response from net.car. Timeout!") - } - return IntArray(0) - } - - //todo методы управления машинкой должны быть в отдельном классе по аналогии с carController - protected fun moveCar(message: RouteMetricRequest) { - val requestBytes = ByteArray(message.getSizeNoTag()) - message.writeTo(CodedOutputStream(requestBytes)) - moveCar(requestBytes) - } - - - private fun moveCar(messageBytes: ByteArray) { - try { - thisCar.carConnection.sendRequest(Client.Request.ROUTE_METRIC, messageBytes).get(60, TimeUnit.SECONDS) - } catch (e: ConnectException) { - println("connection error!") - } catch (e: TimeoutException) { - println("don't have response from net.car. Timeout!") - } - return - } - fun iterate() { Logger.log("============= STARTING ITERATION $iterationCounter ============") Logger.indent() @@ -96,7 +42,7 @@ abstract class AbstractAlgorithm(val thisCar: Car) { return } val angles = getAngles() - val distances = getData(angles) + val distances = carController.scan(angles.map { it.degs() }.toIntArray(), ATTEMPTS, WINDOW_SIZE, SMOOTHING) if (distances.size != angles.size) { throw RuntimeException("error! angles and distances have various sizes") } @@ -126,22 +72,22 @@ abstract class AbstractAlgorithm(val thisCar: Car) { Logger.indent() Logger.log("Directions = ${Arrays.toString(command.directions)}") Logger.log("Distanced = ${Arrays.toString(command.distances)}") - Logger.outdent() + Logger.outDent() println(Arrays.toString(command.directions)) println(Arrays.toString(command.distances)) this.prevSonarDistances = anglesDistances this.prevState = state - moveCar(command) - Logger.outdent() + carController.moveCar(command) + Logger.outDent() Logger.log("============= FINISHING ITERATION $iterationCounter ============") Logger.log("") } private fun addCancelIterationToLog() { Logger.log("iteration cancelled. need more data from sonar") - Logger.outdent() + Logger.outDent() Logger.log("============= FINISHING ITERATION $iterationCounter ============") Logger.log("") } @@ -169,10 +115,10 @@ abstract class AbstractAlgorithm(val thisCar: Car) { for ((index, dir) in res.directions.withIndex()) { res.directions[index] = when (dir) { - FORWARD -> BACKWARD - BACKWARD -> FORWARD - LEFT -> RIGHT - RIGHT -> LEFT + FORWARD.id -> BACKWARD.id + BACKWARD.id -> FORWARD.id + LEFT.id -> RIGHT.id + RIGHT.id -> LEFT.id else -> throw IllegalArgumentException("Unexpected direction = $dir found during command inversion") } } @@ -186,8 +132,8 @@ abstract class AbstractAlgorithm(val thisCar: Car) { Logger.indent() Logger.log("Last command: ${lastCommand.toString()}") Logger.log("Inverted cmd: ${invertedCommand.toString()}") - Logger.outdent() - moveCar(invertedCommand) + Logger.outDent() + carController.moveCar(invertedCommand) } protected fun rollback(steps: Int) { @@ -199,7 +145,7 @@ abstract class AbstractAlgorithm(val thisCar: Car) { rollback() stepsRemaining-- } - Logger.outdent() + Logger.outDent() Logger.log("=== Finished rollback ===") } diff --git a/server/src/main/java/algorithm/AlgorithmThread.kt b/server/src/main/java/algorithm/AlgorithmThread.kt index 4a0c2542afc..4433a35a268 100644 --- a/server/src/main/java/algorithm/AlgorithmThread.kt +++ b/server/src/main/java/algorithm/AlgorithmThread.kt @@ -1,30 +1,29 @@ package algorithm -class AlgorithmThread(val algorithmImpl: AbstractAlgorithm) : Thread() { +class AlgorithmThread() : Thread() { + var algorithmImpl: AbstractAlgorithm? = null private var count = 0 override fun run() { - while (!algorithmImpl.isCompleted()) { + val currentAlgorithm = algorithmImpl ?: return + while (!currentAlgorithm.isCompleted()) { try { if (count > 0) { - algorithmImpl.iterate() + currentAlgorithm.iterate() count-- } else { Thread.sleep(1000) } } catch (e: InterruptedException) { - + println("algorithm thread is interrupted") + break } } - println("algorithm is finished!") + println("algorithm is completed") } fun setCount(count: Int) { this.count = count } - - fun cancel() { - count = 0 - } -} \ No newline at end of file +} diff --git a/server/src/main/java/algorithm/CarController.kt b/server/src/main/java/algorithm/CarController.kt new file mode 100644 index 00000000000..c944def0a46 --- /dev/null +++ b/server/src/main/java/algorithm/CarController.kt @@ -0,0 +1,60 @@ +package algorithm + +import CodedInputStream +import roomScanner.serialize +import RouteMetricRequest +import SonarRequest +import SonarResponse +import net.car.client.Client +import objects.CarConnection +import java.net.ConnectException +import java.util.concurrent.TimeUnit +import java.util.concurrent.TimeoutException + + +class CarController(val carConnection: CarConnection) { + + fun scan(angles: IntArray, attempts: Int, windowSize: Int, smoothing: SonarRequest.Smoothing): IntArray { + val message = SonarRequest.BuilderSonarRequest( + angles = angles, + attempts = IntArray(angles.size, { attempts }), + smoothing = smoothing, + windowSize = windowSize) + .build() + val data = serialize(message.getSizeNoTag(), { message.writeTo(it) }) + val response: ByteArray + try { + val future = carConnection.sendRequest(Client.Request.SONAR, data) + response = future.get(300, TimeUnit.SECONDS).responseBodyAsBytes + + } catch (e: ConnectException) { + println("connection error!") + return IntArray(0) + } catch (e: TimeoutException) { + println("don't have response from net.car. Timeout!") + return IntArray(0) + } + return SonarResponse.BuilderSonarResponse(IntArray(0)).parseFrom(CodedInputStream(response)).build().distances + } + + fun moveCar(message: RouteMetricRequest) { + val requestBytes = serialize(message.getSizeNoTag(), { message.writeTo(it) }) + moveCar(requestBytes) + } + + fun moveCar(distances: IntArray, directions: IntArray) { + val routeMetric = RouteMetricRequest.BuilderRouteMetricRequest(distances, directions).build() + moveCar(routeMetric) + } + + private fun moveCar(messageBytes: ByteArray) { + try { + carConnection.sendRequest(Client.Request.ROUTE_METRIC, messageBytes).get(60, TimeUnit.SECONDS) + } catch (e: ConnectException) { + println("connection error!") + } catch (e: TimeoutException) { + println("don't have response from net.car. Timeout!") + } + return + } +} \ 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 be1cc86b95f..4cd037ea231 100644 --- a/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt +++ b/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt @@ -6,13 +6,10 @@ import RouteMetricRequest import SonarRequest import algorithm.geometry.* import objects.Car +import roomScanner.CarController.Direction.* class RoomBypassingAlgorithm(thisCar: Car) : AbstractAlgorithm(thisCar) { - - //todo refactoring all algorithm package - - override val ATTEMPTS: Int = 5 override val SMOOTHING = SonarRequest.Smoothing.MEDIAN override val WINDOW_SIZE = 3 @@ -39,27 +36,23 @@ class RoomBypassingAlgorithm(thisCar: Car) : AbstractAlgorithm(thisCar) { } 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)) thisCar.angle = (RoomModel.walls.last().wallAngleOX.degs() - wallAngle.degs()).toDouble() addWall(-wallAngle) - Logger.outdent() + Logger.outDent() calibrateAfterRotate = true - return resultBuilder.build() + return buildRoute( + getIntArray(20, wallAngle.degs(), 75), + getIntArray(FORWARD.id, RIGHT.id, FORWARD.id)) } private fun wallAheadFound(anglesDistances: Map, state: CarState): RouteMetricRequest { - val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0)) - resultBuilder.setDirections(getIntArray(LEFT, FORWARD)) val wallAngle = calculateAngle(anglesDistances, state) - resultBuilder.setDistances(getIntArray(wallAngle.degs(), 15)) thisCar.angle = (RoomModel.walls.last().wallAngleOX.degs() + wallAngle.degs()).toDouble() addWall(wallAngle) calibrateAfterRotate = true - return resultBuilder.build() + return buildRoute(getIntArray(wallAngle.degs(), 15), getIntArray(LEFT.id, FORWARD.id)) } private fun tryAlignParallelToWall(anglesDistances: Map, state: CarState, average: Double): RouteMetricRequest? { @@ -74,50 +67,41 @@ class RoomBypassingAlgorithm(thisCar: Car) : AbstractAlgorithm(thisCar) { distRightBackward.distance < (average / 2) || distRightForward.distance < (average / 2)) { continue } - if (Math.abs(distRightForward.distance - distRightBackward.distance) <= ISOSCALENESS_MIN_DIFF) { return null } - log("Flaw in align found, correcting") - val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0)) val rotationDirection = if (distRightBackward.distance > distRightForward.distance) LEFT else RIGHT - resultBuilder.setDirections(getIntArray(rotationDirection)) - resultBuilder.setDistances(getIntArray(Math.min(Math.abs(distRightBackward.distance - distRightForward.distance), 20))) - return resultBuilder.build() + return buildRoute( + getIntArray(Math.min(Math.abs(distRightBackward.distance - distRightForward.distance), 20)), + getIntArray(rotationDirection.id)) } return null // TODO: everything is broken, what to do? } private fun correctDistanceToParallelWall(anglesDistances: Map, state: CarState): RouteMetricRequest { - val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0)) - val distToWall = anglesDistances[Angle(90)]!!.distance val rotationDirection = if (distToWall > DISTANCE_TO_WALL_UPPER_BOUND) RIGHT else LEFT val backRotationDirection = if (rotationDirection == RIGHT) LEFT else RIGHT - resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD, backRotationDirection)) val rangeToCorridor = if (distToWall > DISTANCE_TO_WALL_UPPER_BOUND) { distToWall - DISTANCE_TO_WALL_UPPER_BOUND } else { DISTANCE_TO_WALL_LOWER_BOUND - distToWall } - resultBuilder.setDistances(getIntArray(2 * rangeToCorridor + 5, 20, (1.5 * rangeToCorridor).toInt())) - return resultBuilder.build() + return buildRoute(getIntArray(2 * rangeToCorridor + 5, 20, (1.5 * rangeToCorridor).toInt()), + getIntArray(rotationDirection.id, FORWARD.id, backRotationDirection.id)) } private fun moveForward(distToWall: Int): RouteMetricRequest { - val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0)) - resultBuilder.setDirections(getIntArray(FORWARD)) - resultBuilder.setDistances(getIntArray(Math.max(distToWall / 4, 20))) - return resultBuilder.build() + return buildRoute(getIntArray(Math.max(distToWall / 4, 20)), getIntArray(FORWARD.id)) } private fun addWall(angleWithPrevWall: Angle) { log("Adding wall") Logger.indent() updateWalls() - Logger.outdent() + Logger.outDent() val firstWall = RoomModel.walls.first() val lastWall = RoomModel.walls.last() if (circleFound) { @@ -134,7 +118,7 @@ class RoomBypassingAlgorithm(thisCar: Car) : AbstractAlgorithm(thisCar) { } } - fun updateWalls() { + private fun updateWalls() { synchronized(RoomModel) { val walls = RoomModel.walls if (walls.size < 2) { @@ -166,7 +150,6 @@ class RoomBypassingAlgorithm(thisCar: Car) : AbstractAlgorithm(thisCar) { val dist80 = anglesDistances[Angle(80)]!! val dist90 = anglesDistances[Angle(90)]!! val dist100 = anglesDistances[Angle(100)]!! - val dist110 = anglesDistances[Angle(110)]!! val sonarAngle = (thisCar.angle - 90).toInt() if (anglesDistances.filter { @@ -191,7 +174,7 @@ class RoomBypassingAlgorithm(thisCar: Car) : AbstractAlgorithm(thisCar) { log("Calibrating after rotate") Logger.indent() val maybeAlignment = tryAlignParallelToWall(anglesDistances, state, average) - Logger.outdent() + Logger.outDent() if (maybeAlignment != null) { log("Realigning") return maybeAlignment @@ -212,8 +195,8 @@ class RoomBypassingAlgorithm(thisCar: Car) : AbstractAlgorithm(thisCar) { // Add point to room map val point = Point( - x = thisCar.x + dist90.distance * Math.cos(degreesToRadian(sonarAngle)), - y = thisCar.y + dist90.distance * Math.sin(degreesToRadian(sonarAngle)) + x = thisCar.x + dist90.distance * Math.cos(Angle(sonarAngle).rads()), + y = thisCar.y + dist90.distance * Math.sin(Angle(sonarAngle).rads()) ) log("Adding middle point ${point.toString()} to wall ${RoomModel.walls.last().id}") RoomModel.walls.last().pushBackPoint(point) @@ -239,8 +222,8 @@ class RoomBypassingAlgorithm(thisCar: Car) : AbstractAlgorithm(thisCar) { continue } val point = Point( - x = thisCar.x + curDist * Math.cos(degreesToRadian((thisCar.angle - i).toInt())), - y = thisCar.y + curDist * Math.sin(degreesToRadian((thisCar.angle - i).toInt())) + x = thisCar.x + curDist * Math.cos(Angle((thisCar.angle - i).toInt()).rads()), + y = thisCar.y + curDist * Math.sin(Angle((thisCar.angle - i).toInt()).rads()) ) log("Adding ${point.toString()} to wall ${RoomModel.walls.last().id}") RoomModel.walls.last().pushBackPoint(point) @@ -251,7 +234,7 @@ class RoomBypassingAlgorithm(thisCar: Car) : AbstractAlgorithm(thisCar) { log("4. Check if we have to align parallel to the wall") Logger.indent() val maybeAlignment = tryAlignParallelToWall(anglesDistances, state, average) - Logger.outdent() + Logger.outDent() if (maybeAlignment != null) { log("Realigning") return maybeAlignment @@ -298,39 +281,21 @@ class RoomBypassingAlgorithm(thisCar: Car) : AbstractAlgorithm(thisCar) { return args } + private fun buildRoute(distances: IntArray, directions: IntArray): RouteMetricRequest { + val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(distances, directions) + return resultBuilder.build() + } + override fun isCompleted(): Boolean { return isCompleted } override fun afterGetCommand(route: RouteMetricRequest) { - route.directions.forEachIndexed { idx, direction -> - when (direction) { - FORWARD -> { - thisCar.x += (Math.cos(degreesToRadian(thisCar.angle.toInt())) * route.distances[idx]).toInt() - thisCar.y += (Math.sin(degreesToRadian(thisCar.angle.toInt())) * route.distances[idx]).toInt() - } - BACKWARD -> { - thisCar.x -= (Math.cos(degreesToRadian(thisCar.angle.toInt())) * route.distances[idx]).toInt() - thisCar.y -= (Math.sin(degreesToRadian(thisCar.angle.toInt())) * route.distances[idx]).toInt() - } - LEFT -> { - route.distances[idx] = (CHARGE_CORRECTION * route.distances[idx]).toInt() - } - RIGHT -> { - route.distances[idx] = (CHARGE_CORRECTION * route.distances[idx]).toInt() - } - } - } + thisCar.refreshPositionAfterRoute(route) if (Math.round(Math.cos(Angle(thisCar.angle.toInt()).rads())).toInt() == 1 && thisCar.angle.toInt() != 0 && Vector(thisCar.x.toDouble(), thisCar.y.toDouble()).length() < RANGE_FROM_ZERO_POINT_TO_FINISH_ALG) { log("Found circle!") circleFound = true } - } - - private fun degreesToRadian(angle: Int): Double { - return Math.PI * angle / 180 - } - } \ 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 2cb0ecf55e8..c640a243877 100644 --- a/server/src/main/java/algorithm/geometry/AngleData.kt +++ b/server/src/main/java/algorithm/geometry/AngleData.kt @@ -3,7 +3,6 @@ package algorithm.geometry class AngleData(val angle: Angle, val distance: Int) { - fun toPoint(carAngleOX: Angle): Point { //convert to global coordinate system diff --git a/server/src/main/java/algorithm/geometry/DoubleComparison.kt b/server/src/main/java/algorithm/geometry/DoubleComparison.kt index 9547fd2dcd0..d0194139ade 100644 --- a/server/src/main/java/algorithm/geometry/DoubleComparison.kt +++ b/server/src/main/java/algorithm/geometry/DoubleComparison.kt @@ -1,13 +1,15 @@ package algorithm.geometry +private val eps = 0.1 + fun Double.lt(other: Double): Boolean { - return this - other < -Util.eps + return this - other < -eps } fun Double.gt(other: Double): Boolean { - return this - other > Util.eps + return this - other > eps } fun Double.eq(other: Double): Boolean { - return Math.abs(this - other) < Util.eps + return Math.abs(this - other) < 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 b17eb6ccfcb..17656e0afc5 100644 --- a/server/src/main/java/algorithm/geometry/Line.kt +++ b/server/src/main/java/algorithm/geometry/Line.kt @@ -1,7 +1,6 @@ package algorithm.geometry class Line(var A: Double, var B: Double, var C: Double) { - val COMPARISON_THRESHOLD = 0 init { normalize() @@ -33,10 +32,8 @@ class Line(var A: Double, var B: Double, var C: Double) { B *= -1 C *= -1 } - } - else - { - if (A.lt(0.0)){ + } else { + if (A.lt(0.0)) { A *= -1 B *= -1 C *= -1 @@ -44,28 +41,8 @@ class Line(var A: Double, var B: Double, var C: Double) { } } - 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 - - Logger.log("Comparing lines: ") - Logger.log(" this = ${this.toString()}") - Logger.log(" other = ${other.toString()}") - - val dist = metricDist(other) - - Logger.log(" Distance = ${dist}") - Logger.log(" COMPARISON_THRESHOLD = ${COMPARISON_THRESHOLD}") - return dist < COMPARISON_THRESHOLD - } - fun getDirectionVector(): Vector { - return Vector (A, -B) + return Vector(A, -B) } } \ 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 deleted file mode 100644 index 8f08eedb065..00000000000 --- a/server/src/main/java/algorithm/geometry/Util.kt +++ /dev/null @@ -1,6 +0,0 @@ -package algorithm.geometry - -object Util { - - val eps = 0.1 -} \ No newline at end of file diff --git a/server/src/main/java/algorithm/geometry/Vector.kt b/server/src/main/java/algorithm/geometry/Vector.kt index b0f2ddead81..62510b9a9c5 100644 --- a/server/src/main/java/algorithm/geometry/Vector.kt +++ b/server/src/main/java/algorithm/geometry/Vector.kt @@ -4,7 +4,7 @@ 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) + 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 diff --git a/server/src/main/java/algorithm/geometry/Wall.kt b/server/src/main/java/algorithm/geometry/Wall.kt index efedb37a0b2..d77b3069cf8 100644 --- a/server/src/main/java/algorithm/geometry/Wall.kt +++ b/server/src/main/java/algorithm/geometry/Wall.kt @@ -1,5 +1,6 @@ package algorithm.geometry +import Logger import java.util.* data class Wall(val wallAngleOX: Angle, @@ -18,7 +19,7 @@ data class Wall(val wallAngleOX: Angle, } var isFinished = false - val MAX_SLOPE = 0.2 + private val MAX_SLOPE = 0.2 fun pushBackPoint(point: Point) { Logger.log("Adding ${point.toString()}") @@ -67,34 +68,34 @@ data class Wall(val wallAngleOX: Angle, private fun approximatePointsByLine(): Line { var n = 0 - var sumx = 0.0 - var sumy = 0.0 - var sumx2 = 0.0 + var sumX = 0.0 + var sumY = 0.0 + var sumX2 = 0.0 for ((x2, y2) in rawPoints) { - sumx += x2 - sumx2 += x2 * x2 - sumy += y2 + sumX += x2 + sumX2 += x2 * x2 + sumY += y2 n++ } - val xbar = sumx / n - val ybar = sumy / n + val xBar = sumX / n + val yBar = sumY / n // second pass: compute summary statistics var xxbar = 0.0 var yybar = 0.0 var xybar = 0.0 for ((x1, y1) in rawPoints) { - xxbar += (x1 - xbar) * (x1 - xbar) - yybar += (y1 - ybar) * (y1 - ybar) - xybar += (x1 - xbar) * (y1 - ybar) + xxbar += (x1 - xBar) * (x1 - xBar) + yybar += (y1 - yBar) * (y1 - yBar) + xybar += (x1 - xBar) * (y1 - yBar) } var beta1 = xybar / xxbar - var beta0 = ybar - beta1 * xbar + var beta0 = yBar - beta1 * xBar if (Math.abs(beta1) > MAX_SLOPE) { beta1 = xybar / yybar - beta0 = xbar - beta1 * ybar + beta0 = xBar - beta1 * yBar return Line(1.0, -beta1, -beta0) } @@ -108,10 +109,4 @@ data class Wall(val wallAngleOX: Angle, fun isHorizontal(): Boolean { return Math.abs(wallAngleOX.degs()) % 180 == 0 } - - 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 diff --git a/server/src/main/java/clInterface/DebugClInterface.kt b/server/src/main/java/clInterface/DebugClInterface.kt new file mode 100644 index 00000000000..972f109195a --- /dev/null +++ b/server/src/main/java/clInterface/DebugClInterface.kt @@ -0,0 +1,62 @@ +package clInterface + +import clInterface.executor.* + +object DebugClInterface { + + private val HELP_STRING = "available handlers:\n" + + "cars - get list of connected cars\n" + + "route [car_id] - setting a route for net.car with net.car id.\n" + + "sonar [car_id] - get sonar data\n" + + "dbinfo [car_id] [type] - refresh all net.car locations\n" + + "type is int value from: MEMORY_STATS - 0, SONAR_STATS - 1\n" + + "alg [count] - run algorithm. Make [count] iteration\n" + + "stop - exit from this interface and stop all threads\n" + + private val algorithmExecutor = Algorithm() + private val executors = mapOf( + Pair("cars", CarsInformation()), + Pair("route", RouteMetric()), + Pair("sonar", Sonar()), + Pair("dbinfo", DebugInformation()), + Pair("alg", algorithmExecutor), + Pair("explore", Explore()) + ) + + fun run() { + println(HELP_STRING) + while (true) { + val readString = readLine() + if (readString == null || readString.equals("stop")) { + algorithmExecutor.interruptAlgorithmThread() + break + } + if (readString.equals("")) { + continue + } + try { + executeCommand(readString) + } catch (exception: Exception) { + exception.printStackTrace() + println("Fail to execute command[$readString]: $exception") + println(HELP_STRING) + } + } + + } + + private fun printNotSupportedCommand(command: String) { + println("Incorrect command: $command") + println(HELP_STRING) + } + + private fun executeCommand(readString: String) { + val command = readString.split(" ")[0].toLowerCase() + val executor = executors[command] + if (executor == null) { + printNotSupportedCommand(command) + return + } + executor.execute(readString) + } +} diff --git a/server/src/main/java/clInterface/executor/Algorithm.kt b/server/src/main/java/clInterface/executor/Algorithm.kt new file mode 100644 index 00000000000..3fa39cf4d09 --- /dev/null +++ b/server/src/main/java/clInterface/executor/Algorithm.kt @@ -0,0 +1,30 @@ +package clInterface.executor + +import algorithm.AlgorithmThread +import algorithm.RoomBypassingAlgorithm +import objects.Environment + +class Algorithm : CommandExecutor { + + private val algorithmThread = AlgorithmThread() + + override fun execute(command: String) { + val params = command.split(" ") + val count = + if (params.size == 2) try { + params[1].toInt() + } catch (e: Exception) { + 1 + } else 1 + if (algorithmThread.algorithmImpl == null) { + algorithmThread.algorithmImpl = RoomBypassingAlgorithm(Environment.map.values.last()) + algorithmThread.start() + } + algorithmThread.setCount(count) + } + + fun interruptAlgorithmThread() { + algorithmThread.setCount(0) + algorithmThread.interrupt() + } +} diff --git a/server/src/main/java/clInterface/executor/CarsInformation.kt b/server/src/main/java/clInterface/executor/CarsInformation.kt new file mode 100644 index 00000000000..a30880b2a0e --- /dev/null +++ b/server/src/main/java/clInterface/executor/CarsInformation.kt @@ -0,0 +1,12 @@ +package clInterface.executor + +import objects.Environment + +class CarsInformation : CommandExecutor { + + override fun execute(command: String) { + synchronized(Environment, { + println(Environment.map.values) + }) + } +} diff --git a/server/src/main/java/clInterface/executor/CommandExecutor.kt b/server/src/main/java/clInterface/executor/CommandExecutor.kt new file mode 100644 index 00000000000..cba954721f6 --- /dev/null +++ b/server/src/main/java/clInterface/executor/CommandExecutor.kt @@ -0,0 +1,7 @@ +package clInterface.executor + +interface CommandExecutor { + + fun execute(command:String) + +} \ No newline at end of file diff --git a/server/src/main/java/clInterface/executor/DebugInformation.kt b/server/src/main/java/clInterface/executor/DebugInformation.kt new file mode 100644 index 00000000000..3e1ab2ff8c9 --- /dev/null +++ b/server/src/main/java/clInterface/executor/DebugInformation.kt @@ -0,0 +1,49 @@ +package clInterface.executor + +import CodedInputStream +import DebugRequest +import DebugResponseMemoryStats +import DebugResponseSonarStats +import roomScanner.serialize +import net.car.client.Client +import objects.Environment +import java.rmi.UnexpectedException + +class DebugInformation : CommandExecutor { + + override fun execute(command: String) { + val params = command.split(" ") + val car = Environment.map[params[1].toInt()]!! + val type = DebugRequest.Type.fromIntToType(params[2].toInt()) + + val request = DebugRequest.BuilderDebugRequest(type).build() + val requestType = when (type) { + DebugRequest.Type.MEMORY_STATS -> Client.Request.DEBUG_MEMORY + DebugRequest.Type.SONAR_STATS -> Client.Request.DEBUG_SONAR + else -> throw UnexpectedException(type.toString()) + } + + val responseData = car.carConnection.sendRequest( + requestType, + serialize(request.getSizeNoTag(), { request.writeTo(it) }) + ).get().responseBodyAsBytes + + when (type) { + DebugRequest.Type.MEMORY_STATS -> { + val data = DebugResponseMemoryStats.BuilderDebugResponseMemoryStats(0, 0, 0, 0).parseFrom(CodedInputStream(responseData)).build() + println("Heap static tail: ${data.heapStaticTail}") + println("Heap dynamic tail: ${data.heapDynamicTail}") + println("Heap dynamic max size: ${data.heapDynamicMaxBytes}") + println("Heap dynamic total size: ${data.heapDynamicTotalBytes}") + } + DebugRequest.Type.SONAR_STATS -> { + val data = DebugResponseSonarStats.BuilderDebugResponseSonarStats(0, 0, 0, 0).parseFrom(CodedInputStream(responseData)).build() + println("Sonar measurement total: ${data.measurementCount}") + println("Failed check sums: ${data.measurementFailedChecksum}") + println("Failed command: ${data.measurementFailedCommand}") + println("Failed distance: ${data.measurementFailedDistance}") + } + else -> throw UnexpectedException(type.toString()) + } + } +} \ No newline at end of file diff --git a/server/src/main/java/clInterface/executor/Explore.kt b/server/src/main/java/clInterface/executor/Explore.kt new file mode 100644 index 00000000000..586d2c985bc --- /dev/null +++ b/server/src/main/java/clInterface/executor/Explore.kt @@ -0,0 +1,28 @@ +package clInterface.executor + +import CodedInputStream +import roomScanner.serialize +import SonarExploreAngleRequest +import SonarExploreAngleResponse +import net.car.client.Client +import objects.Environment + +class Explore : CommandExecutor { + + override fun execute(command: String) { + val params = command.split(" ") + val car = Environment.map[params[1].toInt()]!! + val angle = params[2].toInt() + val window = params[3].toInt() + + val request = SonarExploreAngleRequest.BuilderSonarExploreAngleRequest(angle, window).build() + val responseData = car.carConnection.sendRequest( + Client.Request.EXPLORE_ANGLE, + serialize(request.getSizeNoTag(), { request.writeTo(it) }) + ).get().responseBodyAsBytes + + val distances = SonarExploreAngleResponse.BuilderSonarExploreAngleResponse(IntArray(0)).parseFrom(CodedInputStream(responseData)).build().distances + + println("Received distances: [${distances.joinToString()}]") + } +} \ No newline at end of file diff --git a/server/src/main/java/clInterface/executor/RouteMetric.kt b/server/src/main/java/clInterface/executor/RouteMetric.kt new file mode 100644 index 00000000000..ddc1d904a34 --- /dev/null +++ b/server/src/main/java/clInterface/executor/RouteMetric.kt @@ -0,0 +1,86 @@ +package clInterface.executor + +import CodedOutputStream +import RouteMetricRequest +import net.car.client.Client +import objects.Car +import objects.Environment +import java.net.ConnectException + +class RouteMetric : CommandExecutor { + + private val ROUTE_REGEX = Regex("route [0-9]{1,10}") + private val HELP_STRING = "print way points als [distance/degrees] [direction] in sm and degrees." + + "after enter all points print \"done\". for reset route print \"reset\". available directions:" + + "0 - FORWARD, 1 - BACKWARD, 2 - LEFT, 3 - RIGHT" + + override fun execute(command: String) { + if (!ROUTE_REGEX.matches(command)) { + println("incorrect args of route command") + return + } + val id: Int + try { + id = command.split(" ")[1].toInt() + } catch (e: NumberFormatException) { + e.printStackTrace() + println("error in converting id to int type") + return + } + val car: Car? = + synchronized(Environment, { + Environment.map[id] + }) + if (car == null) { + println("car with id=$id not found") + return + } + val routeMessage = getRouteMessage() ?: return + val requestBytes = ByteArray(routeMessage.getSizeNoTag()) + routeMessage.writeTo(CodedOutputStream(requestBytes)) + try { + car.carConnection.sendRequest(Client.Request.ROUTE_METRIC, requestBytes) + } catch (e: ConnectException) { + synchronized(Environment, { + Environment.map.remove(id) + }) + } + } + + private fun getRouteMessage(): RouteMetricRequest? { + println(HELP_STRING) + val distances = arrayListOf() + val directions = arrayListOf() + while (true) { + val readLine = readLine()!!.toLowerCase() + when (readLine) { + "reset" -> return null + "done" -> { + val routeBuilder = RouteMetricRequest.BuilderRouteMetricRequest(distances.toIntArray(), directions.toIntArray()) + return routeBuilder.build() + } + } + val wayPointData = readLine.split(" ") + val distance: Int + val direction: Int + try { + distance = wayPointData[0].toInt() + direction = wayPointData[1].toInt() + } catch (e: NumberFormatException) { + println("error in converting distance or direction to int. try again") + continue + } catch (e: IndexOutOfBoundsException) { + println("format error, you must print two number separated by spaces. Try again") + continue + } + if (direction != 0 && direction != 1 && direction != 2 && direction != 3) { + println("direction $direction don't supported!") + println(HELP_STRING) + } + distances.add(distance) + directions.add(direction) + } + } + + +} \ No newline at end of file diff --git a/server/src/main/java/clInterface/executor/Sonar.kt b/server/src/main/java/clInterface/executor/Sonar.kt new file mode 100644 index 00000000000..675e3e03ecb --- /dev/null +++ b/server/src/main/java/clInterface/executor/Sonar.kt @@ -0,0 +1,72 @@ +package clInterface.executor + +import CodedOutputStream +import SonarRequest +import net.car.client.Client +import objects.Car +import objects.Environment +import java.net.ConnectException + +class Sonar : CommandExecutor { + + private val SONAR_REGEX = Regex("sonar [0-9]{1,10}") + + override fun execute(command: String) { + if (!SONAR_REGEX.matches(command)) { + println("incorrect args of command sonar.") + return + } + val id: Int + try { + id = command.split(" ")[1].toInt() + } catch (e: NumberFormatException) { + e.printStackTrace() + println("error in converting id to int type") + return + } + val car: Car? = synchronized(Environment, { + Environment.map[id] + }) + if (car == null) { + println("car with id=$id not found") + return + } + val requestMessage = getSonarRequest() ?: return + val requestBytes = ByteArray(requestMessage.getSizeNoTag()) + requestMessage.writeTo(CodedOutputStream(requestBytes)) + try { + car.carConnection.sendRequest(Client.Request.SONAR, requestBytes) + } catch (e: ConnectException) { + synchronized(Environment, { + Environment.map.remove(id) + }) + } + } + + private fun getSonarRequest(): SonarRequest? { + println("print angles, after printing all angles print done") + val angles = arrayListOf() + while (true) { + val command = readLine()!!.toLowerCase() + when (command) { + "reset" -> return null + "done" -> { + val sonarBuilder = SonarRequest.BuilderSonarRequest(angles.toIntArray(), IntArray(angles.size, { 5 }), 3, SonarRequest.Smoothing.MEDIAN) + return sonarBuilder.build() + } + else -> { + try { + val angle = command.toInt() + if (angle < 0 || angle > 180) { + println("incorrect angle $angle. angle must be in [0,180] and div on 4") + } else { + angles.add(angle) + } + } catch (e: NumberFormatException) { + println("error in converting angle to int. try again") + } + } + } + } + } +} \ No newline at end of file diff --git a/server/src/main/java/net/ServerHandler.kt b/server/src/main/java/net/ServerHandler.kt index 47436d827c6..52436b6b253 100644 --- a/server/src/main/java/net/ServerHandler.kt +++ b/server/src/main/java/net/ServerHandler.kt @@ -52,7 +52,6 @@ class ServerHandler(val handlers: Map) : SimpleChannelInboundHa } override fun exceptionCaught(ctx: ChannelHandlerContext?, cause: Throwable?) { - println("exception") cause?.printStackTrace() ctx?.close() } diff --git a/server/src/main/java/net/web/server/Server.kt b/server/src/main/java/net/web/server/Server.kt index 55df06c586e..74331de6aca 100644 --- a/server/src/main/java/net/web/server/Server.kt +++ b/server/src/main/java/net/web/server/Server.kt @@ -1,5 +1,6 @@ package net.web.server +import ModeChange import io.netty.bootstrap.ServerBootstrap import io.netty.channel.nio.NioEventLoopGroup import io.netty.channel.socket.nio.NioServerSocketChannel diff --git a/server/src/main/java/net/web/server/handlers/ChangeMode.kt b/server/src/main/java/net/web/server/handlers/ChangeMode.kt index 0abb1ef8679..968e82a7f78 100644 --- a/server/src/main/java/net/web/server/handlers/ChangeMode.kt +++ b/server/src/main/java/net/web/server/handlers/ChangeMode.kt @@ -1,11 +1,13 @@ package net.web.server.handlers -import net.web.server.Server -import java.util.* import CodedInputStream import CodedOutputStream import GenericResponse +import ModeChange +import Result import net.Handler +import net.web.server.Server +import java.util.* class ChangeMode : Handler { diff --git a/server/src/main/java/net/web/server/handlers/DirectionOrder.kt b/server/src/main/java/net/web/server/handlers/DirectionOrder.kt index b669730ffc9..077eab56391 100644 --- a/server/src/main/java/net/web/server/handlers/DirectionOrder.kt +++ b/server/src/main/java/net/web/server/handlers/DirectionOrder.kt @@ -1,8 +1,10 @@ package net.web.server.handlers -import CodedOutputStream import CodedInputStream +import CodedOutputStream +import DirectionRequest import GenericResponse +import Result import net.Handler import net.web.server.Server diff --git a/server/src/main/java/objects/Car.kt b/server/src/main/java/objects/Car.kt index e79ae8dd999..48fbde6a566 100644 --- a/server/src/main/java/objects/Car.kt +++ b/server/src/main/java/objects/Car.kt @@ -1,12 +1,40 @@ package objects +import roomScanner.CarController.Direction +import RouteMetricRequest +import algorithm.geometry.Angle class Car constructor(val uid: Int, host: String, port: Int) { + + private val CHARGE_CORRECTION = 1.0//on full charge ok is 0.83 - 0.86 + var x = 0.0 var y = 0.0 var angle = 0.0 val carConnection = CarConnection(host, port) + fun refreshPositionAfterRoute(route:RouteMetricRequest) { + val carAngle = angle.toInt() + route.directions.forEachIndexed { idx, direction -> + when (direction) { + Direction.FORWARD.id -> { + x += (Math.cos(Angle(carAngle).rads()) * route.distances[idx]).toInt() + y += (Math.sin(Angle(carAngle).rads()) * route.distances[idx]).toInt() + } + Direction.BACKWARD.id -> { + x -= (Math.cos(Angle(carAngle).rads()) * route.distances[idx]).toInt() + y -= (Math.sin(Angle(carAngle).rads()) * route.distances[idx]).toInt() + } + Direction.LEFT.id -> { + route.distances[idx] = (CHARGE_CORRECTION * route.distances[idx]).toInt() + } + Direction.RIGHT.id -> { + route.distances[idx] = (CHARGE_CORRECTION * route.distances[idx]).toInt() + } + } + } + } + override fun toString(): String { return "$uid ; x:$x; y:$y; target:$angle" } diff --git a/server/src/main/java/objects/CarConnection.kt b/server/src/main/java/objects/CarConnection.kt index d0fa2dd3810..7183b81ab83 100644 --- a/server/src/main/java/objects/CarConnection.kt +++ b/server/src/main/java/objects/CarConnection.kt @@ -4,7 +4,6 @@ import net.car.client.Client import org.asynchttpclient.ListenableFuture import org.asynchttpclient.Response - class CarConnection(val host: String, val port: Int) { fun sendRequest(request: Client.Request, data: ByteArray): ListenableFuture { return Client.makeRequest("http://$host:$port/${request.url}", data) diff --git a/server/src/main/java/RoomScanner/CarController.kt b/server/src/main/java/roomScanner/CarController.kt similarity index 99% rename from server/src/main/java/RoomScanner/CarController.kt rename to server/src/main/java/roomScanner/CarController.kt index 333fe2148fb..db39b840645 100644 --- a/server/src/main/java/RoomScanner/CarController.kt +++ b/server/src/main/java/roomScanner/CarController.kt @@ -1,4 +1,4 @@ -package RoomScanner +package roomScanner import CodedInputStream import RouteMetricRequest diff --git a/server/src/main/java/RoomScanner/RoomScanner.kt b/server/src/main/java/roomScanner/RoomScanner.kt similarity index 98% rename from server/src/main/java/RoomScanner/RoomScanner.kt rename to server/src/main/java/roomScanner/RoomScanner.kt index 22420b76ae9..e4e1fc53200 100644 --- a/server/src/main/java/RoomScanner/RoomScanner.kt +++ b/server/src/main/java/roomScanner/RoomScanner.kt @@ -1,4 +1,4 @@ -package RoomScanner +package roomScanner class RoomScanner(val controller: CarController) : Thread() { private val points = mutableListOf>() diff --git a/server/src/main/java/RoomScanner/utils.kt b/server/src/main/java/roomScanner/utils.kt similarity index 98% rename from server/src/main/java/RoomScanner/utils.kt rename to server/src/main/java/roomScanner/utils.kt index c33ba9abdc6..18b62da44a3 100644 --- a/server/src/main/java/RoomScanner/utils.kt +++ b/server/src/main/java/roomScanner/utils.kt @@ -1,4 +1,4 @@ -package RoomScanner +package roomScanner import CodedOutputStream