car_srv: support sonar explore message

car_srv/kotlinSrv/src/JSImport.kt

@@ -80,6 +80,12 @@ fun <T> encodeProtoBuf(protoMessage: T): ByteArray {
             val codedOutput = CodedOutputStream(routeBytes)
             protoMessage.writeTo(codedOutput)
         }
+        is SonarExploreAngleRequest -> {
+            val protoSize = protoMessage.getSizeNoTag()
+            routeBytes = ByteArray(protoSize)
+            val codedOutput = CodedOutputStream(routeBytes)
+            protoMessage.writeTo(codedOutput)
+        }
 
         else -> {
             println("PROTO MESSAGE DON'T ENCODE!")

car_srv/kotlinSrv/src/MCConnection.kt

@@ -30,6 +30,12 @@ class McTransport() : MCConnectObserver<String> {
         sendBytes(encodeProtoBuf(message))
     }
 
+    fun sendProtoBuf(request: SonarExploreAngleRequest) {
+        val typeMessage = getProtoBufTypeMessage(TaskRequest.Type.SONAR_EXPLORE)
+        sendProtoBufType(typeMessage)
+        sendBytes(encodeProtoBuf(request))
+    }
+
     fun sendProtoBuf(message: RouteRequest) {
         val typeMessage = getProtoBufTypeMessage(TaskRequest.Type.ROUTE)
         sendProtoBufType(typeMessage)

car_srv/kotlinSrv/src/Main.kt

@@ -4,10 +4,7 @@ import net.Client
 import net.server.handlers.AbstractHandler
 import net.server.handlers.debug.Memory
 import net.server.handlers.flash.LoadBin
-import net.server.handlers.main.GetLocation
-import net.server.handlers.main.GetSonarData
-import net.server.handlers.main.SetRoute
-import net.server.handlers.main.SetRouteMetric
+import net.server.handlers.main.*
 import net.server.handlers.rc.Control
 import room.Room
 
@@ -50,6 +47,7 @@ fun main(args: Array<String>) {
     handlers.put("/rc/control", Control())
     handlers.put("/loadBin", LoadBin())
     handlers.put("/sonar", GetSonarData(carController))
+    handlers.put("/sonarExplore", SonarExplore(carController))
     handlers.put("/route", SetRoute(carController))
     handlers.put("/routeMetric", SetRouteMetric(carController))
     handlers.put("/getLocation", GetLocation())

car_srv/kotlinSrv/src/control/Controller.kt

@@ -3,9 +3,11 @@ package control
 import RouteMetricRequest
 import RouteRequest
 import SonarRequest
+import SonarExploreAngleRequest
 
 interface Controller {
     fun executeRoute(route: RouteRequest, callback: (ByteArray) -> Unit)
     fun executeMetricRoute(request: RouteMetricRequest, callback: (ByteArray) -> Unit)
     fun executeRequestSensorData(sonarRequest: SonarRequest, callback: (ByteArray) -> Unit)
+    fun executeRequestSensorExploreData(request: SonarExploreAngleRequest, callback: (ByteArray) -> Unit)
 }

car_srv/kotlinSrv/src/control/car/ControllerToUsb.kt

@@ -3,6 +3,7 @@ package control.car
 import RouteMetricRequest
 import RouteRequest
 import SonarRequest
+import SonarExploreAngleRequest
 import control.Controller
 import mcTransport
 
@@ -34,4 +35,13 @@ class ControllerToUsb : Controller {
         }
         mcTransport.sendProtoBuf(sonarRequest)
     }
+
+    override fun executeRequestSensorExploreData(request: SonarExploreAngleRequest, callback: (ByteArray) -> Unit) {
+        println("SonarExplore data")
+        mcTransport.setCallBack { bytes ->
+            callback.invoke(bytes)
+        }
+
+        mcTransport.sendProtoBuf(request)
+    }
 }

car_srv/kotlinSrv/src/net/server/handlers/main/SonarExplore.kt

@@ -0,0 +1,29 @@
+package net.server.handlers.main
+
+import CodedInputStream
+import McState
+import SonarExploreAngleRequest
+import SonarExploreAngleResponse
+import control.Controller
+import encodeProtoBuf
+import net.server.handlers.AbstractHandler
+
+class SonarExplore(val controller: Controller) : AbstractHandler() {
+    val request = SonarExploreAngleRequest.BuilderSonarExploreAngleRequest(0, 0)
+    val response = SonarExploreAngleResponse.BuilderSonarExploreAngleResponse(IntArray(0))
+
+    override fun getBytesResponse(data: ByteArray, callback: (ByteArray) -> Unit) {
+        val message = request.build()
+        message.mergeFrom(CodedInputStream(data))
+
+        if (McState.instance.isConnected()) {
+            controller.executeRequestSensorExploreData(message, callback)
+            return
+        }
+
+        println("mc is disconnected!")
+        val responseMessage = response.build()
+        callback.invoke(encodeProtoBuf(responseMessage))
+        return
+    }
+}

server/src/main/java/DebugClInterface.kt

@@ -1,6 +1,7 @@
 import Exceptions.InactiveCarException
 import algorithm.AbstractAlgorithm
 import algorithm.RoomBypassingAlgorithm
+import car.client.CarClient
 import car.client.Client
 import io.netty.buffer.Unpooled
 import io.netty.handler.codec.http.*
@@ -37,7 +38,12 @@ object DebugClInterface {
             if (readString.equals("")) {
                 continue
             }
-            executeCommand(readString)
+
+            try {
+                executeCommand(readString)
+            } catch (exception: Exception) {
+                println("Fail to execute command[$readString]: $exception")
+            }
         }
     }
 
@@ -67,10 +73,29 @@ object DebugClInterface {
                 }
             }
             "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 = CarClient.sendRequest(
+                car,
+                CarClient.Request.EXPLORE_ANGLE,
+                CarClient.serialize(request.getSizeNoTag(), { request.writeTo(it) })
+        ).get().responseBodyAsBytes
+
+        val distances = SonarExploreAngleResponse.BuilderSonarExploreAngleResponse(IntArray(0)).parseFrom(CodedInputStream(responseData)).distances
+
+        println("Received distances: [${distances.joinToString()}]")
+    }
+
     private fun executeAlg(readString: String) {
         val params = readString.split(" ")
         var count =

server/src/main/java/RoomScanner/CarController.kt

@@ -1,7 +1,6 @@
 package RoomScanner
 
 import CodedInputStream
-import CodedOutputStream
 import RouteMetricRequest
 import SonarRequest
 import SonarResponse
@@ -40,7 +39,7 @@ class CarController(var car: Car) {
 
     fun scan(angles: IntArray): List<Pair<Double, Double>> {
         val request = SonarRequest.BuilderSonarRequest(angles, IntArray(angles.size, { 1 }), 10, SonarRequest.Smoothing.MEDIAN).build()
-        val data = serialize(request.getSizeNoTag(), { i -> request.writeTo(i) })
+        val data = CarClient.serialize(request.getSizeNoTag(), { i -> request.writeTo(i) })
         val response = CarClient.sendRequest(car, CarClient.Request.SONAR, data).get().responseBodyAsBytes
 
         val distances = SonarResponse.BuilderSonarResponse(IntArray(0)).parseFrom(CodedInputStream(response)).build().distances
@@ -50,7 +49,7 @@ class CarController(var car: Car) {
 
     private fun drive(direction: Direction, distance: Int) {
         val request = RouteMetricRequest.BuilderRouteMetricRequest(intArrayOf(distance), intArrayOf(direction.id)).build()
-        val data = serialize(request.getSizeNoTag(), { i -> request.writeTo(i) })
+        val data = CarClient.serialize(request.getSizeNoTag(), { i -> request.writeTo(i) })
         CarClient.sendRequest(car, CarClient.Request.ROUTE_METRIC, data).get()
     }
 
@@ -63,10 +62,5 @@ class CarController(var car: Car) {
         return Pair(Math.cos(realAngle) * distance + position.first, Math.sin(realAngle) * distance + position.second)
     }
 
-    private inline fun serialize(size: Int, writeTo: (CodedOutputStream) -> Unit): ByteArray {
-        val bytes = ByteArray(size)
-        writeTo(CodedOutputStream(bytes))
 
-        return bytes
-    }
 }

server/src/main/java/car/client/CarClient.kt

@@ -1,5 +1,6 @@
 package car.client
 
+import CodedOutputStream
 import objects.Car
 import org.asynchttpclient.ListenableFuture
 import org.asynchttpclient.Response
@@ -11,9 +12,17 @@ object CarClient {
         ROUTE("route"),
         ROUTE_METRIC("routeMetric"),
         SONAR("sonar"),
-        DEBUG_MEMORY("debug/memory");
+        DEBUG_MEMORY("debug/memory"),
+        EXPLORE_ANGLE("sonarExplore");
     }
 
     fun sendRequest(car: Car, request: Request, data: ByteArray): ListenableFuture<Response>
-        = Client.makeRequest("http://${car.host}:${car.port}/${request.url}", data)
+            = Client.makeRequest("http://${car.host}:${car.port}/${request.url}", data)
+
+    inline fun serialize(size: Int, writeTo: (CodedOutputStream) -> Unit): ByteArray {
+        val bytes = ByteArray(size)
+        writeTo(CodedOutputStream(bytes))
+
+        return bytes
+    }
 }