diff --git a/server/src/main/java/algorithm/AbstractAlgorithm.kt b/server/src/main/java/algorithm/AbstractAlgorithm.kt index 2389fe798e5..b23181bcacf 100644 --- a/server/src/main/java/algorithm/AbstractAlgorithm.kt +++ b/server/src/main/java/algorithm/AbstractAlgorithm.kt @@ -1,13 +1,16 @@ package algorithm +import CodedInputStream import Logger -import RouteMetricRequest import SonarRequest +import SonarResponse import algorithm.geometry.Angle import algorithm.geometry.AngleData import objects.Car import roomScanner.CarController.Direction.* import java.util.* +import java.util.concurrent.TimeUnit +import java.util.concurrent.TimeoutException abstract class AbstractAlgorithm(val thisCar: Car) { @@ -15,9 +18,8 @@ abstract class AbstractAlgorithm(val thisCar: Car) { open val SMOOTHING = SonarRequest.Smoothing.NONE open val WINDOW_SIZE = 0 - val carController = CarController(thisCar.carConnection) private val historySize = 10 - private val history = Stack() + private val history = Stack() private var prevState: CarState? = null private var prevSonarDistances = mapOf() @@ -42,7 +44,18 @@ abstract class AbstractAlgorithm(val thisCar: Car) { return } val angles = getAngles() - val distances = carController.scan(angles.map { it.degs() }.toIntArray(), ATTEMPTS, WINDOW_SIZE, SMOOTHING) + val future = thisCar.scan(angles.map { it.degs() }.toIntArray(), ATTEMPTS, WINDOW_SIZE, SMOOTHING) + val bytes: ByteArray + try { + bytes = future.get(300, TimeUnit.SECONDS).responseBodyAsBytes + } catch (e: TimeoutException) { + println("time out") + return + } + val sonarResponse = SonarResponse.BuilderSonarResponse(IntArray(0)).build() + sonarResponse.mergeFrom(CodedInputStream(bytes)) + val distances = sonarResponse.distances + if (distances.size != angles.size) { throw RuntimeException("error! angles and distances have various sizes") } @@ -79,7 +92,10 @@ abstract class AbstractAlgorithm(val thisCar: Car) { this.prevSonarDistances = anglesDistances this.prevState = state - carController.moveCar(command) + command.distances.forEachIndexed { idx, distance -> + thisCar.moveCar(distance, command.directions[idx]) + } + Logger.outDent() Logger.log("============= FINISHING ITERATION $iterationCounter ============") Logger.log("") @@ -97,32 +113,32 @@ abstract class AbstractAlgorithm(val thisCar: Car) { return requiredAngles } - private fun addToHistory(command: RouteMetricRequest) { + private fun addToHistory(command: RouteData) { history.push(command) while (history.size > historySize) { history.removeAt(0) } } - private fun popFromHistory(): RouteMetricRequest { + private fun popFromHistory(): RouteData { return history.pop() } - private fun inverseCommand(command: RouteMetricRequest): RouteMetricRequest { - val res = RouteMetricRequest.BuilderRouteMetricRequest(command.distances, command.directions) + private fun inverseCommand(command: RouteData): RouteData { + val res = RouteData(command.distances, command.directions) res.distances.reverse() res.directions.reverse() for ((index, dir) in res.directions.withIndex()) { res.directions[index] = when (dir) { - FORWARD.id -> BACKWARD.id - BACKWARD.id -> FORWARD.id - LEFT.id -> RIGHT.id - RIGHT.id -> LEFT.id + FORWARD -> BACKWARD + BACKWARD -> FORWARD + LEFT -> RIGHT + RIGHT -> LEFT else -> throw IllegalArgumentException("Unexpected direction = $dir found during command inversion") } } - return res.build() + return res } protected fun rollback() { @@ -133,7 +149,9 @@ abstract class AbstractAlgorithm(val thisCar: Car) { Logger.log("Last command: ${lastCommand.toString()}") Logger.log("Inverted cmd: ${invertedCommand.toString()}") Logger.outDent() - carController.moveCar(invertedCommand) + invertedCommand.distances.forEachIndexed { idx, distance -> + thisCar.moveCar(distance, invertedCommand.directions[idx]) + } } protected fun rollback(steps: Int) { @@ -150,8 +168,8 @@ abstract class AbstractAlgorithm(val thisCar: Car) { } protected abstract fun getCarState(anglesDistances: Map): CarState? - protected abstract fun getCommand(anglesDistances: Map, state: CarState): RouteMetricRequest? - protected abstract fun afterGetCommand(route: RouteMetricRequest) + protected abstract fun getCommand(anglesDistances: Map, state: CarState): RouteData? + protected abstract fun afterGetCommand(route: RouteData) abstract fun isCompleted(): Boolean } \ No newline at end of file diff --git a/server/src/main/java/algorithm/CarController.kt b/server/src/main/java/algorithm/CarController.kt deleted file mode 100644 index c944def0a46..00000000000 --- a/server/src/main/java/algorithm/CarController.kt +++ /dev/null @@ -1,60 +0,0 @@ -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 301a01fef8b..aea042f5793 100644 --- a/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt +++ b/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt @@ -2,7 +2,6 @@ package algorithm import Logger import Logger.log -import RouteMetricRequest import SonarRequest import algorithm.geometry.* import objects.Car @@ -32,27 +31,27 @@ class RoomBypassingAlgorithm(thisCar: Car) : AbstractAlgorithm(thisCar) { return CarState.WALL } - private fun noOrthogonalMeasurementFound(anglesDistances: Map, state: CarState): RouteMetricRequest { + private fun noOrthogonalMeasurementFound(anglesDistances: Map, state: CarState): RouteData { val wallAngle = calculateAngle(anglesDistances, state) Logger.indent() thisCar.angle = (RoomModel.walls.last().wallAngleOX.degs() - wallAngle.degs()).toDouble() addWall(-wallAngle) Logger.outDent() calibrateAfterRotate = true - return buildRoute( + return RouteData( getIntArray(20, wallAngle.degs(), 75), - getIntArray(FORWARD.id, RIGHT.id, FORWARD.id)) + arrayOf(FORWARD, RIGHT, FORWARD)) } - private fun wallAheadFound(anglesDistances: Map, state: CarState): RouteMetricRequest { + private fun wallAheadFound(anglesDistances: Map, state: CarState): RouteData { val wallAngle = calculateAngle(anglesDistances, state) thisCar.angle = (RoomModel.walls.last().wallAngleOX.degs() + wallAngle.degs()).toDouble() addWall(wallAngle) calibrateAfterRotate = true - return buildRoute(getIntArray(wallAngle.degs(), 15), getIntArray(LEFT.id, FORWARD.id)) + return RouteData(getIntArray(wallAngle.degs(), 15), arrayOf(LEFT, FORWARD)) } - private fun tryAlignParallelToWall(anglesDistances: Map, state: CarState, average: Double): RouteMetricRequest? { + private fun tryAlignParallelToWall(anglesDistances: Map, state: CarState, average: Double): RouteData? { for (i in 0..15 step 5) { val distRightForward = anglesDistances[Angle(70 + i)]!! val distRightBackward = anglesDistances[Angle(110 - i)]!! @@ -69,15 +68,15 @@ class RoomBypassingAlgorithm(thisCar: Car) : AbstractAlgorithm(thisCar) { } log("Flaw in align found, correcting") val rotationDirection = if (distRightBackward.distance > distRightForward.distance) LEFT else RIGHT - return buildRoute( + return RouteData( getIntArray(Math.min(Math.abs(distRightBackward.distance - distRightForward.distance), 20)), - getIntArray(rotationDirection.id)) + arrayOf(rotationDirection)) } return null // TODO: everything is broken, what to do? } - private fun correctDistanceToParallelWall(anglesDistances: Map, state: CarState): RouteMetricRequest { + private fun correctDistanceToParallelWall(anglesDistances: Map, state: CarState): RouteData { 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 @@ -86,12 +85,12 @@ class RoomBypassingAlgorithm(thisCar: Car) : AbstractAlgorithm(thisCar) { } else { DISTANCE_TO_WALL_LOWER_BOUND - distToWall } - return buildRoute(getIntArray(2 * rangeToCorridor + 5, 20, (1.5 * rangeToCorridor).toInt()), - getIntArray(rotationDirection.id, FORWARD.id, backRotationDirection.id)) + return RouteData(getIntArray(2 * rangeToCorridor + 5, 20, (1.5 * rangeToCorridor).toInt()), + arrayOf(rotationDirection, FORWARD, backRotationDirection)) } - private fun moveForward(distToWall: Int): RouteMetricRequest { - return buildRoute(getIntArray(Math.max(distToWall / 4, 20)), getIntArray(FORWARD.id)) + private fun moveForward(distToWall: Int): RouteData { + return RouteData(getIntArray(Math.max(distToWall / 4, 20)), arrayOf(FORWARD)) } private fun addWall(angleWithPrevWall: Angle) { @@ -141,7 +140,7 @@ class RoomBypassingAlgorithm(thisCar: Car) : AbstractAlgorithm(thisCar) { } } - override fun getCommand(anglesDistances: Map, state: CarState): RouteMetricRequest? { + override fun getCommand(anglesDistances: Map, state: CarState): RouteData? { val dist0 = anglesDistances[Angle(0)]!! val dist70 = anglesDistances[Angle(70)]!! val dist80 = anglesDistances[Angle(80)]!! @@ -194,7 +193,7 @@ class RoomBypassingAlgorithm(thisCar: Car) : AbstractAlgorithm(thisCar) { 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}") + log("Adding middle point ${point.toString()} to wall ${RoomModel.walls.last()}") RoomModel.walls.last().pushBackPoint(point) log("") @@ -221,7 +220,7 @@ class RoomBypassingAlgorithm(thisCar: Car) : AbstractAlgorithm(thisCar) { 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}") + log("Adding ${point.toString()} to wall ${RoomModel.walls.last()}") RoomModel.walls.last().pushBackPoint(point) } log("") @@ -277,17 +276,26 @@ 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) { - thisCar.refreshPositionAfterRoute(route) + override fun afterGetCommand(route: RouteData) { + val carAngle = thisCar.angle.toInt() + route.distances.forEachIndexed { idx, value -> + when (route.directions[idx]) { + FORWARD -> { + thisCar.x += (Math.cos(Angle(carAngle).rads()) * route.distances[idx]).toInt() + thisCar.y += (Math.sin(Angle(carAngle).rads()) * route.distances[idx]).toInt() + } + BACKWARD -> { + thisCar.x -= (Math.cos(Angle(carAngle).rads()) * route.distances[idx]).toInt() + thisCar.y -= (Math.sin(Angle(carAngle).rads()) * route.distances[idx]).toInt() + } + else -> { + } + } + } 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!") diff --git a/server/src/main/java/algorithm/RouteData.kt b/server/src/main/java/algorithm/RouteData.kt new file mode 100644 index 00000000000..8f3a058ca6c --- /dev/null +++ b/server/src/main/java/algorithm/RouteData.kt @@ -0,0 +1,7 @@ +package algorithm + +import roomScanner.CarController + +data class RouteData (val distances:IntArray, val directions:Array){ + +} \ No newline at end of file diff --git a/server/src/main/java/objects/Car.kt b/server/src/main/java/objects/Car.kt index 48fbde6a566..76d679d3605 100644 --- a/server/src/main/java/objects/Car.kt +++ b/server/src/main/java/objects/Car.kt @@ -1,8 +1,13 @@ package objects -import roomScanner.CarController.Direction +import CodedOutputStream import RouteMetricRequest -import algorithm.geometry.Angle +import SonarRequest +import net.car.client.Client +import org.asynchttpclient.ListenableFuture +import org.asynchttpclient.Response +import roomScanner.CarController.Direction +import roomScanner.serialize class Car constructor(val uid: Int, host: String, port: Int) { @@ -13,26 +18,24 @@ class Car constructor(val uid: Int, host: String, port: Int) { 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() - } - } - } + fun moveCar(distance: Int, direction: Direction): ListenableFuture { + + val route = RouteMetricRequest.BuilderRouteMetricRequest( + IntArray(1, { (distance * CHARGE_CORRECTION).toInt() }), IntArray(1, { direction.id })) + val bytesRoute = ByteArray(route.getSizeNoTag()) + route.writeTo(CodedOutputStream(bytesRoute)) + return carConnection.sendRequest(Client.Request.ROUTE_METRIC, bytesRoute) + } + + fun scan(angles: IntArray, attempts: Int, windowSize: Int, smoothing: SonarRequest.Smoothing): ListenableFuture { + val message = SonarRequest.BuilderSonarRequest( + angles = angles, + attempts = IntArray(angles.size, { attempts }), + smoothing = smoothing, + windowSize = windowSize) + .build() + val data = serialize(message.getSizeNoTag(), { message.writeTo(it) }) + return carConnection.sendRequest(Client.Request.SONAR, data) } override fun toString(): String {