car_fmw: sonar explore angle feature

car_fmw/src/Control.kt

@@ -16,6 +16,7 @@ object Control {
             TaskRequest.Type.ROUTE.id -> routeTask()
             TaskRequest.Type.ROUTE_METRIC.id -> routeMetricTask()
             TaskRequest.Type.SONAR.id -> sonarTask()
+            TaskRequest.Type.SONAR_EXPLORE.id -> sonarExploreTask()
         }
     }
 
@@ -83,6 +84,17 @@ object Control {
         Writer.writeSonar(response)
     }
 
+    private fun sonarExploreTask() {
+        val request = Reader.readSonarExplore()
+
+        val angle = request.angle
+        val window = request.windowSize
+        val result = Sonar.getRange(max(angle - window, 0), min(angle + window, 360))
+
+        val response = SonarExploreAngleResponse.BuilderSonarExploreAngleResponse(result).build()
+        Writer.writeSonarExplore(response)
+    }
+
     private fun sonarMeasure(smoothing: SonarRequest.Smoothing, attempts: Int, angle: Int, windowSize: Int): Int {
         val data = IntArray(attempts)
         var i = 0

car_fmw/src/ProtoRW.kt

@@ -24,6 +24,11 @@ object Reader {
         return SonarRequest.BuilderSonarRequest(IntArray(0), IntArray(0), 0, SonarRequest.Smoothing.NONE).parseFrom(stream).build()
     }
 
+    fun readSonarExplore(): SonarExploreAngleRequest {
+        val stream = getInputStream()
+        return SonarExploreAngleRequest.BuilderSonarExploreAngleRequest(0, 0, 0).parseFrom(stream).build()
+    }
+
     private fun getInputStream(): CodedInputStream {
         val buffer = Connection.receiveByteArray()
         return CodedInputStream(buffer)
@@ -49,6 +54,12 @@ object Writer {
         Connection.sendByteArray(stream.buffer)
     }
 
+    fun writeSonarExplore(response: SonarExploreAngleResponse) {
+        val stream = getOutputStream(response.getSizeNoTag())
+        response.writeTo(stream)
+        Connection.sendByteArray(stream.buffer)
+    }
+
     private fun getOutputStream(size: Int): CodedOutputStream {
         val buffer = ByteArray(size)
         return CodedOutputStream(buffer)

car_fmw/src/include/Sonar.kt

@@ -42,6 +42,17 @@ object Sonar {
         return -1
     }
 
+    fun getRange(start: Int, stop: Int): IntArray {
+        val result = IntArray(stop - start + 1)
+        var i = 0
+        while (i < stop - start + 1) {
+            result[i] = getDistance(start + i)
+            i++
+        }
+
+        return result
+    }
+
     private fun getOneOfRange(start: Int, stop: Int): Int {
         var i = start
         var distance = getDistance(start)
@@ -58,18 +69,6 @@ object Sonar {
         return distance
     }
 
-    private fun getRange(start: Int, stop: Int): IntArray {
-        val result = IntArray(stop - start + 1)
-        var i = 0
-        while (i < stop - start + 1) {
-            result[i] = getDistance(start + i)
-            i++
-        }
-
-        return result
-    }
-
-
     fun calibrate() {
         getDistance(180)
     }

car_fmw/src/include/utils.kt

@@ -2,3 +2,11 @@
 fun positive(i: Int): Boolean {
     return i > 0
 }
+
+fun min(first: Int, second: Int): Int {
+    if (first < second) return first else return second
+}
+
+fun max(first: Int, second: Int): Int {
+    if (first > second) return first else return second
+}

proto/protofiles_sources/server_car/sonar.proto

@@ -16,3 +16,12 @@ message SonarRequest {
 message SonarResponse {
     repeated int32 distances = 1;
 }
+
+message SonarExploreAngleRequest {
+    int32 angle = 1;
+    int32 windowSize = 2;
+}
+
+message SonarExploreAngleResponse {
+    repeated int32 distances = 1;
+}

proto/protofiles_sources/server_car/task.proto

@@ -10,5 +10,6 @@ message TaskRequest {
         ROUTE = 1;
         SONAR = 2;
         ROUTE_METRIC = 3;
+        SONAR_EXPLORE = 4;
     }
 }