make abstract algorithm, fixed bugs

2016-08-24 14:00:15 +03:00
parent 10acb69eb3
commit fbe90231f0
7 changed files with 253 additions and 174 deletions
@@ -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<IntArray>()
+    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)
        }
    }
@@ -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<IntArray> = 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<String, Int>())
-        } 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<Int, Double>()
-        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<Int, Double>): 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<Int, Double>): 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
-    }
-
-}
@@ -19,7 +19,7 @@ fun main(args: Array<String>) {
    webServer.start()

    //CL user interface
-    DebugClInterface().run()
+    DebugClInterface.run()

    carsDestroy.interrupt()
    carServer.interrupt()
@@ -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<IntArray>) {
+
+    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<Int, Double>()
+
+    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<String, Int>())
+        } 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<Int, Double>()
+        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<Int, Double> {
+        return prevSonarDistances
+    }
+
+    protected abstract fun getCarState(anglesDistances: Map<Int, Double>): CarState
+    protected abstract fun getCommand(anglesDistances: Map<Int, Double>, 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
+    }
+
+}
@@ -0,0 +1,84 @@
+package algorithm
+
+import RouteRequest
+import objects.Car
+import java.util.concurrent.Exchanger
+
+class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : AbstractAlgorithm(thisCar, exchanger) {
+
+    private val MOVE_VELOCITY = 30.3//sm/s
+    private val ROTATION_VELOCITY = 11.0//degrees/s
+
+
+    override fun getCarState(anglesDistances: Map<Int, Double>): 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<Int, Double>, 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<Int, Double>): 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
+    }
+
+
+}
@@ -1,3 +1,5 @@
+package algorithm
+
 /**
 * Created by user on 8/23/16.
 */
@@ -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<Any> {

@@ -41,6 +44,16 @@ class ClientHandler : SimpleChannelInboundHandler<Any> {
                val angles = ctx.channel().attr(AttributeKey.valueOf<IntArray>("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<Any> {
    }

    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?) {