From fbe90231f046a67c7372145590c1f6edf73f06f8 Mon Sep 17 00:00:00 2001 From: MaximZaitsev Date: Wed, 24 Aug 2016 14:00:15 +0300 Subject: [PATCH] make abstract algorithm, fixed bugs --- server/src/main/java/DebugClInterface.kt | 15 +- .../src/main/java/RoomBypassingAlgorithm.kt | 168 ------------------ server/src/main/java/ServerMain.kt | 2 +- .../main/java/algorithm/AbstractAlgorithm.kt | 129 ++++++++++++++ .../java/algorithm/RoomBypassingAlgorithm.kt | 84 +++++++++ .../main/java/{ => algorithm}/RoomModel.kt | 2 + .../src/main/java/car/client/ClientHandler.kt | 27 ++- 7 files changed, 253 insertions(+), 174 deletions(-) delete mode 100644 server/src/main/java/RoomBypassingAlgorithm.kt create mode 100644 server/src/main/java/algorithm/AbstractAlgorithm.kt create mode 100644 server/src/main/java/algorithm/RoomBypassingAlgorithm.kt rename server/src/main/java/{ => algorithm}/RoomModel.kt (84%) diff --git a/server/src/main/java/DebugClInterface.kt b/server/src/main/java/DebugClInterface.kt index 105112deb3a..75880b6e3d9 100644 --- a/server/src/main/java/DebugClInterface.kt +++ b/server/src/main/java/DebugClInterface.kt @@ -1,10 +1,16 @@ import Exceptions.InactiveCarException +import algorithm.AbstractAlgorithm +import algorithm.RoomBypassingAlgorithm import car.client.Client import io.netty.buffer.Unpooled import io.netty.handler.codec.http.* import objects.Car +import java.util.concurrent.Exchanger -class DebugClInterface { +object DebugClInterface { + + val exchanger = Exchanger() + var algorithm: AbstractAlgorithm? = null private val routeRegex = Regex("route [0-9]{1,10}") private val sonarRegex = Regex("sonar [0-9]{1,10}") @@ -50,7 +56,12 @@ class DebugClInterface { "refloc" -> executeRefreshLocationCommand() "sonar" -> executeSonarCommand(readString) "dbinfo" -> executeDebugInfoCommand(readString) - "alg" -> RoomBypassingAlgorithm.iterate() + "alg" -> { + if (algorithm == null) { + algorithm = RoomBypassingAlgorithm(environment.map.values.last(), exchanger) + } + algorithm!!.iterate() + } else -> printNotSupportedCommand(readString) } } diff --git a/server/src/main/java/RoomBypassingAlgorithm.kt b/server/src/main/java/RoomBypassingAlgorithm.kt deleted file mode 100644 index 6b991fdc071..00000000000 --- a/server/src/main/java/RoomBypassingAlgorithm.kt +++ /dev/null @@ -1,168 +0,0 @@ -import Exceptions.InactiveCarException -import car.client.Client -import io.netty.buffer.Unpooled -import io.netty.handler.codec.http.* -import objects.Car -import java.util.concurrent.Exchanger -import java.util.concurrent.TimeUnit - -object RoomBypassingAlgorithm { - - private val FORWARD = 0 - private val BACKWARD = 1 - private val LEFT = 2 - private val RIGHT = 3 - - val exchanger: Exchanger = Exchanger() - var thisCar: Car? = null - - private fun getData(angles: IntArray): DoubleArray { - - val copyElemCount = 5 - val car = thisCar - if (car == null) { - return DoubleArray(0) - } - val anglesCoped = IntArray(angles.size * copyElemCount, { angles[it % copyElemCount] }) - - val message = SonarRequest.BuilderSonarRequest(anglesCoped).build() - val requestBytes = ByteArray(message.getSizeNoTag()) - message.writeTo(CodedOutputStream(requestBytes)) - val request = getDefaultHttpRequest(car.host, sonarUrl, requestBytes) - try { - Client.sendRequest(request, car.host, car.port, mapOf(Pair("angles", anglesCoped))) - } catch (e: InactiveCarException) { - println("connection error!") - } - - try { - val distances = exchanger.exchange(IntArray(0), 20, TimeUnit.SECONDS) - val result = DoubleArray(angles.size) - for (i in 0..result.size - 1) { - result[i] = distances.drop(i * copyElemCount).take(copyElemCount).sum().toDouble() / copyElemCount - } - return result - } catch (e: InterruptedException) { - println("don't have response from car!") - } - return DoubleArray(0) - } - - private fun moveCar(direction: Int, time: Int) { - val car = thisCar - if (car == null) { - return - } - val message = RouteRequest.BuilderRouteRequest(IntArray(1, { time }), IntArray(1, { direction })) - val requestBytes = ByteArray(message.getSizeNoTag()) - message.writeTo(CodedOutputStream(requestBytes)) - val request = getDefaultHttpRequest(car.host, sonarUrl, requestBytes) - try { - Client.sendRequest(request, car.host, car.port, mapOf()) - } catch (e: InactiveCarException) { - println("connection error!") - } - try { - val distances = exchanger.exchange(IntArray(0), 20, TimeUnit.SECONDS) - return - } catch (e: InterruptedException) { - println("don't have response from car!") - } - return - } - - fun iterate(): Boolean { - val angles = listOf(0, 60, 90, 120).toIntArray() - val distances = getData(angles) - if (distances.size != angles.size) { - println("error! angles and distances have various sizes") - return false - } - val anglesDistances = mutableMapOf() - for (i in 0..angles.size - 1) { - anglesDistances.put(angles[i], distances[i]) - } - val command = getCommand(anglesDistances) - - - return true - } - - private fun getIntArray(vararg args: Int): IntArray { - return args - } - - private fun getCommand(anglesDistances: MutableMap): RouteRequest { - val dist0 = anglesDistances[0]!! - val dist60 = anglesDistances[60]!! - val dist90 = anglesDistances[90]!! - val dist120 = anglesDistances[120]!! - val resultBuilder = RouteRequest.BuilderRouteRequest(IntArray(0), IntArray(0)) - if (Math.abs(dist120 - dist60) > 10) { - val rotationDirection = getRotationDirection(dist60, dist120) - resultBuilder.setDirections(getIntArray(rotationDirection)) - resultBuilder.setTimes(getIntArray(1000)) - return resultBuilder.build() - } - - if (dist90 > 40) { - resultBuilder.setDirections(getIntArray(RIGHT)) - resultBuilder.setTimes(getIntArray(1000)) - return resultBuilder.build() - } - if (dist90 < 20) { - resultBuilder.setDirections(getIntArray(LEFT)) - resultBuilder.setTimes(getIntArray(1000)) - return resultBuilder.build() - } - - //TODO учет внешнего угла. В нем может резко (да и не резко) вырасти расстояние на 60 градусов - - - //крутиться не надо, стоим более-менее паралельно стене справа. Смотрим вперед, далеко ли угол (внутренний) - if (dist0 > 100) { - resultBuilder.setDirections(getIntArray(FORWARD)) - resultBuilder.setTimes(getIntArray(1000)) - return resultBuilder.build() - } else { - resultBuilder.setDirections(getIntArray(LEFT, FORWARD)) - resultBuilder.setTimes(getIntArray(500, 1000)) - return resultBuilder.build() - } - } - - private fun getRotationDirection(dist60: Double, dist120: Double): Int { - if (dist120 > dist60) { - return LEFT - } else { - return RIGHT - } - } - - private fun calculateAngleWithWall(anglesDistances: MutableMap): Double { - val dist60 = anglesDistances[60]!! - val dist120 = anglesDistances[120]!! - - //Math.cos(60) = 1/2 - val wallLength = Math.sqrt(Math.pow(dist60, 2.0) + Math.pow(dist120, 2.0) - dist120 * dist60)//in triangle - - val hOnWall = getRangeToWall(wallLength, dist60, dist120) - return 0.0 - } - - //return height in triangle on side a - private fun getRangeToWall(a: Double, b: Double, c: Double): Double { - val halfPerimeter = (a + b + c) / 2 - return Math.sqrt(halfPerimeter * (halfPerimeter - a) * (halfPerimeter - b) * (halfPerimeter - c)) * 2 / a - } - - - private fun getDefaultHttpRequest(host: String, url: String, bytes: ByteArray): DefaultFullHttpRequest { - val request = DefaultFullHttpRequest(HttpVersion.HTTP_1_1, HttpMethod.POST, url, Unpooled.copiedBuffer(bytes)) - request.headers().set(HttpHeaderNames.HOST, host) - request.headers().set(HttpHeaderNames.CONNECTION, HttpHeaderValues.CLOSE) - request.headers().setInt(HttpHeaderNames.CONTENT_LENGTH, request.content().readableBytes()) - return request - } - -} \ No newline at end of file diff --git a/server/src/main/java/ServerMain.kt b/server/src/main/java/ServerMain.kt index 8a6caec7093..2df45c457ae 100644 --- a/server/src/main/java/ServerMain.kt +++ b/server/src/main/java/ServerMain.kt @@ -19,7 +19,7 @@ fun main(args: Array) { webServer.start() //CL user interface - DebugClInterface().run() + DebugClInterface.run() carsDestroy.interrupt() carServer.interrupt() diff --git a/server/src/main/java/algorithm/AbstractAlgorithm.kt b/server/src/main/java/algorithm/AbstractAlgorithm.kt new file mode 100644 index 00000000000..355eac93f05 --- /dev/null +++ b/server/src/main/java/algorithm/AbstractAlgorithm.kt @@ -0,0 +1,129 @@ +package algorithm + +import CodedOutputStream +import Exceptions.InactiveCarException +import RouteRequest +import SonarRequest +import io.netty.buffer.Unpooled +import io.netty.handler.codec.http.* +import objects.Car +import setRouteUrl +import sonarUrl +import java.util.* +import java.util.concurrent.Exchanger +import java.util.concurrent.TimeUnit +import java.util.concurrent.TimeoutException + +abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger) { + + protected val FORWARD = 0 + protected val BACKWARD = 1 + protected val LEFT = 2 + protected val RIGHT = 3 + + private var prevState = CarState.WALL + + private var prevSonarDistances = mapOf() + + protected enum class CarState { + WALL, + INNER, + OUTER + } + + protected fun getData(angles: IntArray): DoubleArray { + + val copyElemCount = 5 + val anglesCoped = IntArray(angles.size * copyElemCount, { angles[it / copyElemCount] }) + + val message = SonarRequest.BuilderSonarRequest(anglesCoped).build() + val requestBytes = ByteArray(message.getSizeNoTag()) + message.writeTo(CodedOutputStream(requestBytes)) + val request = getDefaultHttpRequest(thisCar.host, sonarUrl, requestBytes) + try { + car.client.Client.sendRequest(request, thisCar.host, thisCar.port, mapOf(Pair("angles", anglesCoped))) + } catch (e: InactiveCarException) { + println("connection error!") + } + + try { + val distances = exchanger.exchange(IntArray(0), 20, TimeUnit.SECONDS) + val result = DoubleArray(angles.size) + for (i in 0..result.size - 1) { + //todo sonar can send -1!!! + result[i] = distances.drop(i * copyElemCount).take(copyElemCount).sum().toDouble() / copyElemCount + } + return result + } catch (e: InterruptedException) { + println("don't have response from car!") + } catch (e: TimeoutException) { + println("don't have response from car. Timeout!") + } + return DoubleArray(0) + } + + protected fun moveCar(message: RouteRequest) { + val requestBytes = ByteArray(message.getSizeNoTag()) + message.writeTo(CodedOutputStream(requestBytes)) + val request = getDefaultHttpRequest(thisCar.host, setRouteUrl, requestBytes) + try { + car.client.Client.sendRequest(request, thisCar.host, thisCar.port, mapOf()) + } catch (e: InactiveCarException) { + println("connection error!") + } + try { + exchanger.exchange(IntArray(0), 20, TimeUnit.SECONDS) + return + } catch (e: InterruptedException) { + println("don't have response from car!") + } catch (e: TimeoutException) { + println("don't have response from car. Timeout!") + } + return + } + + fun iterate() { + val angles = getAngles() + val distances = getData(angles) + if (distances.size != angles.size) { + throw RuntimeException("error! angles and distances have various sizes") + } + val anglesDistances = mutableMapOf() + for (i in 0..angles.size - 1) { + anglesDistances.put(angles[i], distances[i]) + } + + val state = getCarState(anglesDistances) + + val command = getCommand(anglesDistances, state) + println(Arrays.toString(command.directions)) + println(Arrays.toString(command.times)) + + this.prevSonarDistances = anglesDistances + this.prevState = state + + moveCar(command) + } + + protected fun getPrevState(): CarState { + return prevState + } + + protected fun getPrevSonarDistances(): Map { + return prevSonarDistances + } + + protected abstract fun getCarState(anglesDistances: Map): CarState + protected abstract fun getCommand(anglesDistances: Map, state: CarState): RouteRequest + protected abstract fun getAngles(): IntArray + + + private fun getDefaultHttpRequest(host: String, url: String, bytes: ByteArray): DefaultFullHttpRequest { + val request = DefaultFullHttpRequest(HttpVersion.HTTP_1_1, HttpMethod.POST, url, Unpooled.copiedBuffer(bytes)) + request.headers().set(HttpHeaderNames.HOST, host) + request.headers().set(HttpHeaderNames.CONNECTION, HttpHeaderValues.CLOSE) + request.headers().setInt(HttpHeaderNames.CONTENT_LENGTH, request.content().readableBytes()) + return request + } + +} \ No newline at end of file diff --git a/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt b/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt new file mode 100644 index 00000000000..2c37973b0e2 --- /dev/null +++ b/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt @@ -0,0 +1,84 @@ +package algorithm + +import RouteRequest +import objects.Car +import java.util.concurrent.Exchanger + +class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger) : AbstractAlgorithm(thisCar, exchanger) { + + private val MOVE_VELOCITY = 30.3//sm/s + private val ROTATION_VELOCITY = 11.0//degrees/s + + + override fun getCarState(anglesDistances: Map): CarState { + val dist0 = anglesDistances[0]!! + val dist90 = anglesDistances[90]!! + if (dist90 < 20) { + return CarState.WALL + } + if (dist0 > 60) { + return CarState.WALL + } + return CarState.INNER + } + + private fun getIntArray(vararg args: Int): IntArray { + return args + } + + override fun getAngles(): IntArray { + return IntArray(19, { it * 10 }) + } + + override fun getCommand(anglesDistances: Map, state: CarState): RouteRequest { + val dist0 = anglesDistances[0]!! + val dist60 = anglesDistances[60]!! + val dist90 = anglesDistances[90]!! + val dist120 = anglesDistances[120]!! + val resultBuilder = RouteRequest.BuilderRouteRequest(IntArray(0), IntArray(0)) + if (state == CarState.WALL) { + if (dist90 > 40 || dist90 < 20) { + val rotationDirection = if (dist90 > 40) RIGHT else LEFT + resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD)) + resultBuilder.setTimes(getIntArray(2000, 1000)) + return resultBuilder.build() + } + + if (Math.abs(dist120 - dist60) > 10) { + val rotationDirection = if (dist120 > dist60) LEFT else RIGHT + resultBuilder.setDirections(getIntArray(rotationDirection)) + resultBuilder.setTimes(getIntArray(1000)) + return resultBuilder.build() + } + + resultBuilder.setDirections(getIntArray(FORWARD)) + resultBuilder.setTimes(getIntArray(1000)) + return resultBuilder.build() + + } else { + + resultBuilder.setDirections(getIntArray(LEFT, FORWARD, LEFT)) + resultBuilder.setTimes(getIntArray((45 * 1000 / ROTATION_VELOCITY).toInt(), 1000, (45 * 1000 / ROTATION_VELOCITY).toInt())) + return resultBuilder.build() + } + } + + private fun calculateAngleWithWall(anglesDistances: MutableMap): Double { + val dist60 = anglesDistances[60]!! + val dist120 = anglesDistances[120]!! + + //Math.cos(60) = 1/2 + val wallLength = Math.sqrt(Math.pow(dist60, 2.0) + Math.pow(dist120, 2.0) - dist120 * dist60)//in triangle + + val hOnWall = getRangeToWall(wallLength, dist60, dist120) + return 0.0 + } + + //return height in triangle on side a + private fun getRangeToWall(a: Double, b: Double, c: Double): Double { + val halfPerimeter = (a + b + c) / 2 + return Math.sqrt(halfPerimeter * (halfPerimeter - a) * (halfPerimeter - b) * (halfPerimeter - c)) * 2 / a + } + + +} \ No newline at end of file diff --git a/server/src/main/java/RoomModel.kt b/server/src/main/java/algorithm/RoomModel.kt similarity index 84% rename from server/src/main/java/RoomModel.kt rename to server/src/main/java/algorithm/RoomModel.kt index a4b52265229..b1af7a177e8 100644 --- a/server/src/main/java/RoomModel.kt +++ b/server/src/main/java/algorithm/RoomModel.kt @@ -1,3 +1,5 @@ +package algorithm + /** * Created by user on 8/23/16. */ diff --git a/server/src/main/java/car/client/ClientHandler.kt b/server/src/main/java/car/client/ClientHandler.kt index d960a84c2dc..c7b0f8ed7db 100644 --- a/server/src/main/java/car/client/ClientHandler.kt +++ b/server/src/main/java/car/client/ClientHandler.kt @@ -1,6 +1,7 @@ package car.client import CodedInputStream +import DebugClInterface import DebugResponseMemoryStats import LocationResponse import SonarResponse @@ -11,8 +12,10 @@ import io.netty.channel.SimpleChannelInboundHandler import io.netty.handler.codec.http.DefaultHttpContent import io.netty.util.AttributeKey import objects.Environment +import setRouteUrl import sonarUrl -import java.util.* +import java.util.concurrent.TimeUnit +import java.util.concurrent.TimeoutException class ClientHandler : SimpleChannelInboundHandler { @@ -41,6 +44,16 @@ class ClientHandler : SimpleChannelInboundHandler { val angles = ctx.channel().attr(AttributeKey.valueOf("angles")).get() handlerSonarResponse(response, angles) } + setRouteUrl -> { + try { + DebugClInterface.exchanger.exchange(IntArray(0), 20, TimeUnit.SECONDS) + } catch (e: InterruptedException) { + println("interrupted before sending datas to algorithm!") + } catch (e: TimeoutException) { + e.printStackTrace() + println("timeout") + } + } else -> { } @@ -71,8 +84,16 @@ class ClientHandler : SimpleChannelInboundHandler { } private fun handlerSonarResponse(message: SonarResponse, angles: IntArray) { - println("request angles: ${Arrays.toString(angles)}") - println("distances from sonar: ${Arrays.toString(message.distances)}") +// println("request angles: ${Arrays.toString(angles)}") +// println("distances from sonar: ${Arrays.toString(message.distances)}") + try { + DebugClInterface.exchanger.exchange(message.distances, 20, TimeUnit.SECONDS) + } catch (e: InterruptedException) { + println("interrupted before sending datas to algorithm!") + } catch (e: TimeoutException) { + e.printStackTrace() + println("timeout") + } } override fun channelRead0(ctx: ChannelHandlerContext?, msg: Any?) {