From a3dadd38626be07abc6866e754dcb3648116a8e4 Mon Sep 17 00:00:00 2001 From: e5l Date: Tue, 30 Aug 2016 13:46:01 +0300 Subject: [PATCH] car_srv: support sonar explore message --- car_srv/kotlinSrv/src/JSImport.kt | 6 ++++ car_srv/kotlinSrv/src/MCConnection.kt | 6 ++++ car_srv/kotlinSrv/src/Main.kt | 6 ++-- car_srv/kotlinSrv/src/control/Controller.kt | 2 ++ .../src/control/car/ControllerToUsb.kt | 10 +++++++ .../net/server/handlers/main/SonarExplore.kt | 29 +++++++++++++++++++ server/src/main/java/DebugClInterface.kt | 27 ++++++++++++++++- .../main/java/RoomScanner/CarController.kt | 10 ++----- server/src/main/java/car/client/CarClient.kt | 13 +++++++-- 9 files changed, 94 insertions(+), 15 deletions(-) create mode 100644 car_srv/kotlinSrv/src/net/server/handlers/main/SonarExplore.kt diff --git a/car_srv/kotlinSrv/src/JSImport.kt b/car_srv/kotlinSrv/src/JSImport.kt index 0fcb4df056c..6adf303ee41 100644 --- a/car_srv/kotlinSrv/src/JSImport.kt +++ b/car_srv/kotlinSrv/src/JSImport.kt @@ -80,6 +80,12 @@ fun 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!") diff --git a/car_srv/kotlinSrv/src/MCConnection.kt b/car_srv/kotlinSrv/src/MCConnection.kt index b769584b44b..86cc561a7a7 100644 --- a/car_srv/kotlinSrv/src/MCConnection.kt +++ b/car_srv/kotlinSrv/src/MCConnection.kt @@ -30,6 +30,12 @@ class McTransport() : MCConnectObserver { 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) diff --git a/car_srv/kotlinSrv/src/Main.kt b/car_srv/kotlinSrv/src/Main.kt index 21e436a94cf..9c0644d4a77 100644 --- a/car_srv/kotlinSrv/src/Main.kt +++ b/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) { 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()) diff --git a/car_srv/kotlinSrv/src/control/Controller.kt b/car_srv/kotlinSrv/src/control/Controller.kt index 6721b5b17a3..a2574b829a0 100644 --- a/car_srv/kotlinSrv/src/control/Controller.kt +++ b/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) } \ No newline at end of file diff --git a/car_srv/kotlinSrv/src/control/car/ControllerToUsb.kt b/car_srv/kotlinSrv/src/control/car/ControllerToUsb.kt index 25dad70f2db..bfe31e48893 100644 --- a/car_srv/kotlinSrv/src/control/car/ControllerToUsb.kt +++ b/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) + } } \ No newline at end of file diff --git a/car_srv/kotlinSrv/src/net/server/handlers/main/SonarExplore.kt b/car_srv/kotlinSrv/src/net/server/handlers/main/SonarExplore.kt new file mode 100644 index 00000000000..15bdf279c38 --- /dev/null +++ b/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 + } +} diff --git a/server/src/main/java/DebugClInterface.kt b/server/src/main/java/DebugClInterface.kt index 333a7581eb6..c045f61e87d 100644 --- a/server/src/main/java/DebugClInterface.kt +++ b/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 = diff --git a/server/src/main/java/RoomScanner/CarController.kt b/server/src/main/java/RoomScanner/CarController.kt index f4b97da6a47..5832fefd0e8 100644 --- a/server/src/main/java/RoomScanner/CarController.kt +++ b/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> { 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 - } } diff --git a/server/src/main/java/car/client/CarClient.kt b/server/src/main/java/car/client/CarClient.kt index 150219e1118..624e54ee5e9 100644 --- a/server/src/main/java/car/client/CarClient.kt +++ b/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 - = 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 + } }