From ff5ad083a955800c14c02cd8f3f7303ce7f8aac5 Mon Sep 17 00:00:00 2001 From: e5l Date: Thu, 25 Aug 2016 16:59:26 +0300 Subject: [PATCH] car_fmw: add metric route, improve Engine module --- car_fmw/src/Control.kt | 21 +- car_fmw/src/ProtoRW.kt | 5 + car_fmw/src/Voyager.kt | 2 +- car_fmw/src/include/Engine.kt | 14 +- car_fmw/src/proto/Route.proto | 5 + car_fmw/src/proto/Task.proto | 1 + car_fmw/src/proto_src/Route.kt | 386 ++++++++++++++++++ car_fmw/src/proto_src/Task.kt | 4 +- .../protofiles_sources/server_car/debug.proto | 1 - .../server_car/location.proto | 1 - .../protofiles_sources/server_car/route.proto | 7 +- .../protofiles_sources/server_car/sonar.proto | 5 +- 12 files changed, 435 insertions(+), 17 deletions(-) diff --git a/car_fmw/src/Control.kt b/car_fmw/src/Control.kt index 67fc9296eff..481058737ec 100644 --- a/car_fmw/src/Control.kt +++ b/car_fmw/src/Control.kt @@ -1,4 +1,3 @@ - object Control { val BLINK_DURATION = 1000 @@ -18,6 +17,7 @@ object Control { when (task.type.id) { TaskRequest.Type.DEBUG.id -> debugTask() TaskRequest.Type.ROUTE.id -> routeTask() + TaskRequest.Type.ROUTE_METRIC.id -> routeMetricTask() TaskRequest.Type.SONAR.id -> sonarTask() } @@ -55,6 +55,23 @@ object Control { Writer.writeRoute(response) } + private fun routeMetricTask() { + val route = Reader.readMetricRoute() + val distances = route.distances + var i = 0 + + while (i < distances.size) { + val distance = distances[i] + val direction = route.directions[i] + Engine.drive(direction, distance) + + i++ + } + + val response = RouteResponse.BuilderRouteResponse(0).build() + Writer.writeRoute(response) + } + private fun sonarTask() { val request = Reader.readSonar() val size = request.angles.size @@ -65,7 +82,7 @@ object Control { distances[i] = Sonar.getDistance(request.angles[i]) i++ } - + val response = SonarResponse.BuilderSonarResponse(distances).build() Writer.writeSonar(response) } diff --git a/car_fmw/src/ProtoRW.kt b/car_fmw/src/ProtoRW.kt index 88315ad5a78..c6acbd25261 100644 --- a/car_fmw/src/ProtoRW.kt +++ b/car_fmw/src/ProtoRW.kt @@ -4,6 +4,11 @@ object Reader { return RouteRequest.BuilderRouteRequest(IntArray(0), IntArray(0)).parseFrom(stream).build() } + fun readMetricRoute(): RouteMetricRequest { + val stream = getInputStream() + return RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0)).parseFrom(stream).build() + } + fun readTask(): TaskRequest { val stream = getInputStream() return TaskRequest.BuilderTaskRequest(TaskRequest.Type.DEBUG).parseFrom(stream).build() diff --git a/car_fmw/src/Voyager.kt b/car_fmw/src/Voyager.kt index 1176cbb44d5..5efad5500f5 100644 --- a/car_fmw/src/Voyager.kt +++ b/car_fmw/src/Voyager.kt @@ -12,7 +12,7 @@ object Voyager { distance = Sonar.getSmoothDistance(0) } - Engine.rotate(RouteType.LEFT.id, 45) + Engine.drive(RouteType.LEFT.id, 45) } } } \ No newline at end of file diff --git a/car_fmw/src/include/Engine.kt b/car_fmw/src/include/Engine.kt index ff8b57187da..6d99236cce0 100644 --- a/car_fmw/src/include/Engine.kt +++ b/car_fmw/src/include/Engine.kt @@ -51,14 +51,16 @@ object Engine { } fun drive(direction: Int, distance: Int) { - go(direction) - Time.wait((distance.toDouble() / VELOCITY_DRIVE).toInt()) - stop() - } + val duration = when (direction) { + RouteType.FORWARD.id, + RouteType.BACKWARD.id -> (distance.toDouble() / VELOCITY_DRIVE).toInt() + RouteType.LEFT.id, + RouteType.RIGHT.id -> (distance.toDouble() / VEL0CITY_ROTATE).toInt() + else -> 0 + } - fun rotate(direction: Int, degree: Int) { go(direction) - Time.wait((degree.toDouble() / VEL0CITY_ROTATE).toInt()) + Time.wait(duration) stop() } } \ No newline at end of file diff --git a/car_fmw/src/proto/Route.proto b/car_fmw/src/proto/Route.proto index 249bba0ba20..fdd7eb0cd9a 100644 --- a/car_fmw/src/proto/Route.proto +++ b/car_fmw/src/proto/Route.proto @@ -6,6 +6,11 @@ message RouteRequest { repeated int32 directions = 2; } +message RouteMetricRequest { + repeated int32 distances = 1; + repeated int32 directions = 2; +} + message RouteResponse { int32 code = 1; } \ No newline at end of file diff --git a/car_fmw/src/proto/Task.proto b/car_fmw/src/proto/Task.proto index b61e72a90b0..8e6384bbb73 100644 --- a/car_fmw/src/proto/Task.proto +++ b/car_fmw/src/proto/Task.proto @@ -9,5 +9,6 @@ message TaskRequest { DEBUG = 0; ROUTE = 1; SONAR = 2; + ROUTE_METRIC = 3; } } diff --git a/car_fmw/src/proto_src/Route.kt b/car_fmw/src/proto_src/Route.kt index 91550301708..24a7c6fa650 100644 --- a/car_fmw/src/proto_src/Route.kt +++ b/car_fmw/src/proto_src/Route.kt @@ -384,6 +384,392 @@ class RouteRequest private constructor (var times: IntArray, var directions: Int } +class RouteMetricRequest private constructor (var distances: IntArray, var directions: IntArray) { + //========== Properties =========== + //repeated int32 distances = 1 + + //repeated int32 directions = 2 + + var errorCode: Int = 0 + + //========== Serialization methods =========== + fun writeTo (output: CodedOutputStream) { + //repeated int32 distances = 1 + if (distances.size > 0) { + output.writeTag(1, WireType.LENGTH_DELIMITED) + var arrayByteSize = 0 + + if (distances.size != 0) { + do { + var arraySize = 0 + var i = 0 + while (i < distances.size) { + arraySize += WireFormat.getInt32SizeNoTag(distances[i]) + i += 1 + } + arrayByteSize += arraySize + } while(false) + } + output.writeInt32NoTag(arrayByteSize) + + do { + var i = 0 + while (i < distances.size) { + output.writeInt32NoTag (distances[i]) + i += 1 + } + } while(false) + } + + //repeated int32 directions = 2 + if (directions.size > 0) { + output.writeTag(2, WireType.LENGTH_DELIMITED) + var arrayByteSize = 0 + + if (directions.size != 0) { + do { + var arraySize = 0 + var i = 0 + while (i < directions.size) { + arraySize += WireFormat.getInt32SizeNoTag(directions[i]) + i += 1 + } + arrayByteSize += arraySize + } while(false) + } + output.writeInt32NoTag(arrayByteSize) + + do { + var i = 0 + while (i < directions.size) { + output.writeInt32NoTag (directions[i]) + i += 1 + } + } while(false) + } + + } + + fun mergeWith (other: RouteMetricRequest) { + distances = distances.plus((other.distances)) + directions = directions.plus((other.directions)) + this.errorCode = other.errorCode + } + + fun mergeFromWithSize (input: CodedInputStream, expectedSize: Int) { + val builder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0)) + mergeWith(builder.parseFromWithSize(input, expectedSize).build()) + } + + fun mergeFrom (input: CodedInputStream) { + val builder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0)) + mergeWith(builder.parseFrom(input).build()) + } + + //========== Size-related methods =========== + fun getSize(fieldNumber: Int): Int { + var size = 0 + if (distances.size != 0) { + do { + var arraySize = 0 + size += WireFormat.getTagSize(1, WireType.LENGTH_DELIMITED) + var i = 0 + while (i < distances.size) { + arraySize += WireFormat.getInt32SizeNoTag(distances[i]) + i += 1 + } + size += arraySize + size += WireFormat.getInt32SizeNoTag(arraySize) + } while(false) + } + if (directions.size != 0) { + do { + var arraySize = 0 + size += WireFormat.getTagSize(2, WireType.LENGTH_DELIMITED) + var i = 0 + while (i < directions.size) { + arraySize += WireFormat.getInt32SizeNoTag(directions[i]) + i += 1 + } + size += arraySize + size += WireFormat.getInt32SizeNoTag(arraySize) + } while(false) + } + size += WireFormat.getVarint32Size(size) + WireFormat.getTagSize(fieldNumber, WireType.LENGTH_DELIMITED) + return size + } + + fun getSizeNoTag(): Int { + var size = 0 + if (distances.size != 0) { + do { + var arraySize = 0 + size += WireFormat.getTagSize(1, WireType.LENGTH_DELIMITED) + var i = 0 + while (i < distances.size) { + arraySize += WireFormat.getInt32SizeNoTag(distances[i]) + i += 1 + } + size += arraySize + size += WireFormat.getInt32SizeNoTag(arraySize) + } while(false) + } + if (directions.size != 0) { + do { + var arraySize = 0 + size += WireFormat.getTagSize(2, WireType.LENGTH_DELIMITED) + var i = 0 + while (i < directions.size) { + arraySize += WireFormat.getInt32SizeNoTag(directions[i]) + i += 1 + } + size += arraySize + size += WireFormat.getInt32SizeNoTag(arraySize) + } while(false) + } + return size + } + + //========== Builder =========== + class BuilderRouteMetricRequest constructor (var distances: IntArray, var directions: IntArray) { + //========== Properties =========== + //repeated int32 distances = 1 + fun setDistances(value: IntArray): RouteMetricRequest.BuilderRouteMetricRequest { + distances = value + return this + } + fun setdistancesByIndex(index: Int, value: Int): RouteMetricRequest.BuilderRouteMetricRequest { + distances[index] = value + return this + } + + //repeated int32 directions = 2 + fun setDirections(value: IntArray): RouteMetricRequest.BuilderRouteMetricRequest { + directions = value + return this + } + fun setdirectionsByIndex(index: Int, value: Int): RouteMetricRequest.BuilderRouteMetricRequest { + directions[index] = value + return this + } + + var errorCode: Int = 0 + + //========== Serialization methods =========== + fun writeTo (output: CodedOutputStream) { + //repeated int32 distances = 1 + if (distances.size > 0) { + output.writeTag(1, WireType.LENGTH_DELIMITED) + var arrayByteSize = 0 + + if (distances.size != 0) { + do { + var arraySize = 0 + var i = 0 + while (i < distances.size) { + arraySize += WireFormat.getInt32SizeNoTag(distances[i]) + i += 1 + } + arrayByteSize += arraySize + } while(false) + } + output.writeInt32NoTag(arrayByteSize) + + do { + var i = 0 + while (i < distances.size) { + output.writeInt32NoTag (distances[i]) + i += 1 + } + } while(false) + } + + //repeated int32 directions = 2 + if (directions.size > 0) { + output.writeTag(2, WireType.LENGTH_DELIMITED) + var arrayByteSize = 0 + + if (directions.size != 0) { + do { + var arraySize = 0 + var i = 0 + while (i < directions.size) { + arraySize += WireFormat.getInt32SizeNoTag(directions[i]) + i += 1 + } + arrayByteSize += arraySize + } while(false) + } + output.writeInt32NoTag(arrayByteSize) + + do { + var i = 0 + while (i < directions.size) { + output.writeInt32NoTag (directions[i]) + i += 1 + } + } while(false) + } + + } + + //========== Mutating methods =========== + fun build(): RouteMetricRequest { + val res = RouteMetricRequest(distances, directions) + res.errorCode = errorCode + return res + } + + fun parseFieldFrom(input: CodedInputStream): Boolean { + if (input.isAtEnd()) { return false } + val tag = input.readInt32NoTag() + if (tag == 0) { return false } + val fieldNumber = WireFormat.getTagFieldNumber(tag) + val wireType = WireFormat.getTagWireType(tag) + when(fieldNumber) { + 1 -> { + if (wireType.id != WireType.LENGTH_DELIMITED.id) { + errorCode = 1 + return false + } + val expectedByteSize = input.readInt32NoTag() + var readSize = 0 + var arraySize = 0 + input.mark() + do { + var i = 0 + while(readSize < expectedByteSize) { + var tmp = 0 + tmp = input.readInt32NoTag() + arraySize += 1 + readSize += WireFormat.getInt32SizeNoTag(tmp) + } + } while (false) + var newArray = IntArray(arraySize) + input.reset() + do { + var i = 0 + while(i < arraySize) { + newArray[i] = input.readInt32NoTag() + i += 1} + distances = newArray + } while (false) + } + 2 -> { + if (wireType.id != WireType.LENGTH_DELIMITED.id) { + errorCode = 1 + return false + } + val expectedByteSize = input.readInt32NoTag() + var readSize = 0 + var arraySize = 0 + input.mark() + do { + var i = 0 + while(readSize < expectedByteSize) { + var tmp = 0 + tmp = input.readInt32NoTag() + arraySize += 1 + readSize += WireFormat.getInt32SizeNoTag(tmp) + } + } while (false) + var newArray = IntArray(arraySize) + input.reset() + do { + var i = 0 + while(i < arraySize) { + newArray[i] = input.readInt32NoTag() + i += 1} + directions = newArray + } while (false) + } + else -> errorCode = 4 + } + return true + } + + fun parseFromWithSize(input: CodedInputStream, expectedSize: Int): RouteMetricRequest.BuilderRouteMetricRequest { + while(getSizeNoTag() < expectedSize) { + parseFieldFrom(input) + } + if (getSizeNoTag() > expectedSize) { errorCode = 2 } + return this + } + + fun parseFrom(input: CodedInputStream): RouteMetricRequest.BuilderRouteMetricRequest { + while(parseFieldFrom(input)) {} + return this + } + + //========== Size-related methods =========== + fun getSize(fieldNumber: Int): Int { + var size = 0 + if (distances.size != 0) { + do { + var arraySize = 0 + size += WireFormat.getTagSize(1, WireType.LENGTH_DELIMITED) + var i = 0 + while (i < distances.size) { + arraySize += WireFormat.getInt32SizeNoTag(distances[i]) + i += 1 + } + size += arraySize + size += WireFormat.getInt32SizeNoTag(arraySize) + } while(false) + } + if (directions.size != 0) { + do { + var arraySize = 0 + size += WireFormat.getTagSize(2, WireType.LENGTH_DELIMITED) + var i = 0 + while (i < directions.size) { + arraySize += WireFormat.getInt32SizeNoTag(directions[i]) + i += 1 + } + size += arraySize + size += WireFormat.getInt32SizeNoTag(arraySize) + } while(false) + } + size += WireFormat.getVarint32Size(size) + WireFormat.getTagSize(fieldNumber, WireType.LENGTH_DELIMITED) + return size + } + + fun getSizeNoTag(): Int { + var size = 0 + if (distances.size != 0) { + do { + var arraySize = 0 + size += WireFormat.getTagSize(1, WireType.LENGTH_DELIMITED) + var i = 0 + while (i < distances.size) { + arraySize += WireFormat.getInt32SizeNoTag(distances[i]) + i += 1 + } + size += arraySize + size += WireFormat.getInt32SizeNoTag(arraySize) + } while(false) + } + if (directions.size != 0) { + do { + var arraySize = 0 + size += WireFormat.getTagSize(2, WireType.LENGTH_DELIMITED) + var i = 0 + while (i < directions.size) { + arraySize += WireFormat.getInt32SizeNoTag(directions[i]) + i += 1 + } + size += arraySize + size += WireFormat.getInt32SizeNoTag(arraySize) + } while(false) + } + return size + } + + } + +} + + class RouteResponse private constructor (var code: Int) { //========== Properties =========== //int32 code = 1 diff --git a/car_fmw/src/proto_src/Task.kt b/car_fmw/src/proto_src/Task.kt index d86b6efa0e0..bd2c3a3428c 100644 --- a/car_fmw/src/proto_src/Task.kt +++ b/car_fmw/src/proto_src/Task.kt @@ -9,7 +9,8 @@ class TaskRequest private constructor (var type: TaskRequest.Type) { DEBUG (0), ROUTE (1), SONAR (2), - Unexpected(3); + ROUTE_METRIC (3), + Unexpected(4); companion object { fun fromIntToType (ord: Int): Type { @@ -17,6 +18,7 @@ class TaskRequest private constructor (var type: TaskRequest.Type) { 0 -> Type.DEBUG 1 -> Type.ROUTE 2 -> Type.SONAR + 3 -> Type.ROUTE_METRIC else -> Unexpected } } diff --git a/proto/protofiles_sources/server_car/debug.proto b/proto/protofiles_sources/server_car/debug.proto index 7b0e7908729..0559c8d3cd6 100644 --- a/proto/protofiles_sources/server_car/debug.proto +++ b/proto/protofiles_sources/server_car/debug.proto @@ -2,7 +2,6 @@ syntax = "proto3"; package carkot; message DebugRequest { - Type type = 1; enum Type { diff --git a/proto/protofiles_sources/server_car/location.proto b/proto/protofiles_sources/server_car/location.proto index c6566e71b0e..069d17f4838 100644 --- a/proto/protofiles_sources/server_car/location.proto +++ b/proto/protofiles_sources/server_car/location.proto @@ -2,7 +2,6 @@ syntax = "proto3"; package carkot; message LocationResponse { - LocationData locationResponseData = 1; int32 code = 2; diff --git a/proto/protofiles_sources/server_car/route.proto b/proto/protofiles_sources/server_car/route.proto index 1b9f7ed9291..5f3782f8cc9 100644 --- a/proto/protofiles_sources/server_car/route.proto +++ b/proto/protofiles_sources/server_car/route.proto @@ -2,7 +2,12 @@ syntax = "proto3"; package carkot; message RouteRequest { - repeated int32 times = 1; + repeated int32 times = 1; + repeated int32 directions = 2; +} + +message RouteMetricRequest { + repeated int32 distances = 1; repeated int32 directions = 2; } diff --git a/proto/protofiles_sources/server_car/sonar.proto b/proto/protofiles_sources/server_car/sonar.proto index 6a0c8aab569..dc24ed94bad 100644 --- a/proto/protofiles_sources/server_car/sonar.proto +++ b/proto/protofiles_sources/server_car/sonar.proto @@ -2,13 +2,10 @@ syntax = "proto3"; package carkot; message SonarRequest { - - repeated int32 angles = 1; + repeated int32 angles = 1; } message SonarResponse { - repeated int32 distances = 1; - }