azalea/kotlin-fork
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

remove using protobuf from algorithm module

Browse Source
This commit is contained in:
MaximZaitsev
2016-09-05 16:38:41 +03:00
parent 86aa39ddac
commit f27e52200a
5 changed files with 99 additions and 123 deletions
Download Patch File Download Diff File Expand all files Collapse all files
server/src/main/java/algorithm/AbstractAlgorithm.kt
+35 -17
View File
@@ -1,13 +1,16 @@
package algorithm
import CodedInputStream
import Logger
import RouteMetricRequest
import SonarRequest
import SonarResponse
import algorithm.geometry.Angle
import algorithm.geometry.AngleData
import objects.Car
import roomScanner.CarController.Direction.*
import java.util.*
import java.util.concurrent.TimeUnit
import java.util.concurrent.TimeoutException
abstract class AbstractAlgorithm(val thisCar: Car) {
@@ -15,9 +18,8 @@ abstract class AbstractAlgorithm(val thisCar: Car) {
open val SMOOTHING = SonarRequest.Smoothing.NONE
open val WINDOW_SIZE = 0
val carController = CarController(thisCar.carConnection)
private val historySize = 10
private val history = Stack<RouteMetricRequest>()
private val history = Stack<RouteData>()
private var prevState: CarState? = null
private var prevSonarDistances = mapOf<Angle, AngleData>()
@@ -42,7 +44,18 @@ abstract class AbstractAlgorithm(val thisCar: Car) {
return
}
val angles = getAngles()
val distances = carController.scan(angles.map { it.degs() }.toIntArray(), ATTEMPTS, WINDOW_SIZE, SMOOTHING)
val future = thisCar.scan(angles.map { it.degs() }.toIntArray(), ATTEMPTS, WINDOW_SIZE, SMOOTHING)
val bytes: ByteArray
try {
bytes = future.get(300, TimeUnit.SECONDS).responseBodyAsBytes
} catch (e: TimeoutException) {
println("time out")
return
}
val sonarResponse = SonarResponse.BuilderSonarResponse(IntArray(0)).build()
sonarResponse.mergeFrom(CodedInputStream(bytes))
val distances = sonarResponse.distances
if (distances.size != angles.size) {
throw RuntimeException("error! angles and distances have various sizes")
}
@@ -79,7 +92,10 @@ abstract class AbstractAlgorithm(val thisCar: Car) {
this.prevSonarDistances = anglesDistances
this.prevState = state
carController.moveCar(command)
command.distances.forEachIndexed { idx, distance ->
thisCar.moveCar(distance, command.directions[idx])
}
Logger.outDent()
Logger.log("============= FINISHING ITERATION $iterationCounter ============")
Logger.log("")
@@ -97,32 +113,32 @@ abstract class AbstractAlgorithm(val thisCar: Car) {
return requiredAngles
}
private fun addToHistory(command: RouteMetricRequest) {
private fun addToHistory(command: RouteData) {
history.push(command)
while (history.size > historySize) {
history.removeAt(0)
}
}
private fun popFromHistory(): RouteMetricRequest {
private fun popFromHistory(): RouteData {
return history.pop()
}
private fun inverseCommand(command: RouteMetricRequest): RouteMetricRequest {
val res = RouteMetricRequest.BuilderRouteMetricRequest(command.distances, command.directions)
private fun inverseCommand(command: RouteData): RouteData {
val res = RouteData(command.distances, command.directions)
res.distances.reverse()
res.directions.reverse()
for ((index, dir) in res.directions.withIndex()) {
res.directions[index] = when (dir) {
FORWARD.id -> BACKWARD.id
BACKWARD.id -> FORWARD.id
LEFT.id -> RIGHT.id
RIGHT.id -> LEFT.id
FORWARD -> BACKWARD
BACKWARD -> FORWARD
LEFT -> RIGHT
RIGHT -> LEFT
else -> throw IllegalArgumentException("Unexpected direction = $dir found during command inversion")
}
}
return res.build()
return res
}
protected fun rollback() {
@@ -133,7 +149,9 @@ abstract class AbstractAlgorithm(val thisCar: Car) {
Logger.log("Last command: ${lastCommand.toString()}")
Logger.log("Inverted cmd: ${invertedCommand.toString()}")
Logger.outDent()
carController.moveCar(invertedCommand)
invertedCommand.distances.forEachIndexed { idx, distance ->
thisCar.moveCar(distance, invertedCommand.directions[idx])
}
}
protected fun rollback(steps: Int) {
@@ -150,8 +168,8 @@ abstract class AbstractAlgorithm(val thisCar: Car) {
}
protected abstract fun getCarState(anglesDistances: Map<Angle, AngleData>): CarState?
protected abstract fun getCommand(anglesDistances: Map<Angle, AngleData>, state: CarState): RouteMetricRequest?
protected abstract fun afterGetCommand(route: RouteMetricRequest)
protected abstract fun getCommand(anglesDistances: Map<Angle, AngleData>, state: CarState): RouteData?
protected abstract fun afterGetCommand(route: RouteData)
abstract fun isCompleted(): Boolean
}
server/src/main/java/algorithm/CarController.kt
-60
View File
@@ -1,60 +0,0 @@
package algorithm
import CodedInputStream
import roomScanner.serialize
import RouteMetricRequest
import SonarRequest
import SonarResponse
import net.car.client.Client
import objects.CarConnection
import java.net.ConnectException
import java.util.concurrent.TimeUnit
import java.util.concurrent.TimeoutException
class CarController(val carConnection: CarConnection) {
fun scan(angles: IntArray, attempts: Int, windowSize: Int, smoothing: SonarRequest.Smoothing): IntArray {
val message = SonarRequest.BuilderSonarRequest(
angles = angles,
attempts = IntArray(angles.size, { attempts }),
smoothing = smoothing,
windowSize = windowSize)
.build()
val data = serialize(message.getSizeNoTag(), { message.writeTo(it) })
val response: ByteArray
try {
val future = carConnection.sendRequest(Client.Request.SONAR, data)
response = future.get(300, TimeUnit.SECONDS).responseBodyAsBytes
} catch (e: ConnectException) {
println("connection error!")
return IntArray(0)
} catch (e: TimeoutException) {
println("don't have response from net.car. Timeout!")
return IntArray(0)
}
return SonarResponse.BuilderSonarResponse(IntArray(0)).parseFrom(CodedInputStream(response)).build().distances
}
fun moveCar(message: RouteMetricRequest) {
val requestBytes = serialize(message.getSizeNoTag(), { message.writeTo(it) })
moveCar(requestBytes)
}
fun moveCar(distances: IntArray, directions: IntArray) {
val routeMetric = RouteMetricRequest.BuilderRouteMetricRequest(distances, directions).build()
moveCar(routeMetric)
}
private fun moveCar(messageBytes: ByteArray) {
try {
carConnection.sendRequest(Client.Request.ROUTE_METRIC, messageBytes).get(60, TimeUnit.SECONDS)
} catch (e: ConnectException) {
println("connection error!")
} catch (e: TimeoutException) {
println("don't have response from net.car. Timeout!")
}
return
}
}
server/src/main/java/algorithm/RoomBypassingAlgorithm.kt
+32 -24
View File
@@ -2,7 +2,6 @@ package algorithm
import Logger
import Logger.log
import RouteMetricRequest
import SonarRequest
import algorithm.geometry.*
import objects.Car
@@ -32,27 +31,27 @@ class RoomBypassingAlgorithm(thisCar: Car) : AbstractAlgorithm(thisCar) {
return CarState.WALL
}
private fun noOrthogonalMeasurementFound(anglesDistances: Map<Angle, AngleData>, state: CarState): RouteMetricRequest {
private fun noOrthogonalMeasurementFound(anglesDistances: Map<Angle, AngleData>, state: CarState): RouteData {
val wallAngle = calculateAngle(anglesDistances, state)
Logger.indent()
thisCar.angle = (RoomModel.walls.last().wallAngleOX.degs() - wallAngle.degs()).toDouble()
addWall(-wallAngle)
Logger.outDent()
calibrateAfterRotate = true
return buildRoute(
return RouteData(
getIntArray(20, wallAngle.degs(), 75),
getIntArray(FORWARD.id, RIGHT.id, FORWARD.id))
arrayOf(FORWARD, RIGHT, FORWARD))
}
private fun wallAheadFound(anglesDistances: Map<Angle, AngleData>, state: CarState): RouteMetricRequest {
private fun wallAheadFound(anglesDistances: Map<Angle, AngleData>, state: CarState): RouteData {
val wallAngle = calculateAngle(anglesDistances, state)
thisCar.angle = (RoomModel.walls.last().wallAngleOX.degs() + wallAngle.degs()).toDouble()
addWall(wallAngle)
calibrateAfterRotate = true
return buildRoute(getIntArray(wallAngle.degs(), 15), getIntArray(LEFT.id, FORWARD.id))
return RouteData(getIntArray(wallAngle.degs(), 15), arrayOf(LEFT, FORWARD))
}
private fun tryAlignParallelToWall(anglesDistances: Map<Angle, AngleData>, state: CarState, average: Double): RouteMetricRequest? {
private fun tryAlignParallelToWall(anglesDistances: Map<Angle, AngleData>, state: CarState, average: Double): RouteData? {
for (i in 0..15 step 5) {
val distRightForward = anglesDistances[Angle(70 + i)]!!
val distRightBackward = anglesDistances[Angle(110 - i)]!!
@@ -69,15 +68,15 @@ class RoomBypassingAlgorithm(thisCar: Car) : AbstractAlgorithm(thisCar) {
}
log("Flaw in align found, correcting")
val rotationDirection = if (distRightBackward.distance > distRightForward.distance) LEFT else RIGHT
return buildRoute(
return RouteData(
getIntArray(Math.min(Math.abs(distRightBackward.distance - distRightForward.distance), 20)),
getIntArray(rotationDirection.id))
arrayOf(rotationDirection))
}
return null // TODO: everything is broken, what to do?
}
private fun correctDistanceToParallelWall(anglesDistances: Map<Angle, AngleData>, state: CarState): RouteMetricRequest {
private fun correctDistanceToParallelWall(anglesDistances: Map<Angle, AngleData>, state: CarState): RouteData {
val distToWall = anglesDistances[Angle(90)]!!.distance
val rotationDirection = if (distToWall > DISTANCE_TO_WALL_UPPER_BOUND) RIGHT else LEFT
val backRotationDirection = if (rotationDirection == RIGHT) LEFT else RIGHT
@@ -86,12 +85,12 @@ class RoomBypassingAlgorithm(thisCar: Car) : AbstractAlgorithm(thisCar) {
} else {
DISTANCE_TO_WALL_LOWER_BOUND - distToWall
}
return buildRoute(getIntArray(2 * rangeToCorridor + 5, 20, (1.5 * rangeToCorridor).toInt()),
getIntArray(rotationDirection.id, FORWARD.id, backRotationDirection.id))
return RouteData(getIntArray(2 * rangeToCorridor + 5, 20, (1.5 * rangeToCorridor).toInt()),
arrayOf(rotationDirection, FORWARD, backRotationDirection))
}
private fun moveForward(distToWall: Int): RouteMetricRequest {
return buildRoute(getIntArray(Math.max(distToWall / 4, 20)), getIntArray(FORWARD.id))
private fun moveForward(distToWall: Int): RouteData {
return RouteData(getIntArray(Math.max(distToWall / 4, 20)), arrayOf(FORWARD))
}
private fun addWall(angleWithPrevWall: Angle) {
@@ -141,7 +140,7 @@ class RoomBypassingAlgorithm(thisCar: Car) : AbstractAlgorithm(thisCar) {
}
}
override fun getCommand(anglesDistances: Map<Angle, AngleData>, state: CarState): RouteMetricRequest? {
override fun getCommand(anglesDistances: Map<Angle, AngleData>, state: CarState): RouteData? {
val dist0 = anglesDistances[Angle(0)]!!
val dist70 = anglesDistances[Angle(70)]!!
val dist80 = anglesDistances[Angle(80)]!!
@@ -194,7 +193,7 @@ class RoomBypassingAlgorithm(thisCar: Car) : AbstractAlgorithm(thisCar) {
x = thisCar.x + dist90.distance * Math.cos(Angle(sonarAngle).rads()),
y = thisCar.y + dist90.distance * Math.sin(Angle(sonarAngle).rads())
)
log("Adding middle point ${point.toString()} to wall ${RoomModel.walls.last().id}")
log("Adding middle point ${point.toString()} to wall ${RoomModel.walls.last()}")
RoomModel.walls.last().pushBackPoint(point)
log("")
@@ -221,7 +220,7 @@ class RoomBypassingAlgorithm(thisCar: Car) : AbstractAlgorithm(thisCar) {
x = thisCar.x + curDist * Math.cos(Angle((thisCar.angle - i).toInt()).rads()),
y = thisCar.y + curDist * Math.sin(Angle((thisCar.angle - i).toInt()).rads())
)
log("Adding ${point.toString()} to wall ${RoomModel.walls.last().id}")
log("Adding ${point.toString()} to wall ${RoomModel.walls.last()}")
RoomModel.walls.last().pushBackPoint(point)
}
log("")
@@ -277,17 +276,26 @@ class RoomBypassingAlgorithm(thisCar: Car) : AbstractAlgorithm(thisCar) {
return args
}
private fun buildRoute(distances: IntArray, directions: IntArray): RouteMetricRequest {
val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(distances, directions)
return resultBuilder.build()
}
override fun isCompleted(): Boolean {
return isCompleted
}
override fun afterGetCommand(route: RouteMetricRequest) {
thisCar.refreshPositionAfterRoute(route)
override fun afterGetCommand(route: RouteData) {
val carAngle = thisCar.angle.toInt()
route.distances.forEachIndexed { idx, value ->
when (route.directions[idx]) {
FORWARD -> {
thisCar.x += (Math.cos(Angle(carAngle).rads()) * route.distances[idx]).toInt()
thisCar.y += (Math.sin(Angle(carAngle).rads()) * route.distances[idx]).toInt()
}
BACKWARD -> {
thisCar.x -= (Math.cos(Angle(carAngle).rads()) * route.distances[idx]).toInt()
thisCar.y -= (Math.sin(Angle(carAngle).rads()) * route.distances[idx]).toInt()
}
else -> {
}
}
}
if (Math.round(Math.cos(Angle(thisCar.angle.toInt()).rads())).toInt() == 1 && thisCar.angle.toInt() != 0
&& Vector(thisCar.x.toDouble(), thisCar.y.toDouble()).length() < RANGE_FROM_ZERO_POINT_TO_FINISH_ALG) {
log("Found circle!")
server/src/main/java/algorithm/RouteData.kt
+7
View File
@@ -0,0 +1,7 @@
package algorithm
import roomScanner.CarController
data class RouteData (val distances:IntArray, val directions:Array<CarController.Direction>){
}
server/src/main/java/objects/Car.kt
+25 -22
View File
@@ -1,8 +1,13 @@
package objects
import roomScanner.CarController.Direction
import CodedOutputStream
import RouteMetricRequest
import algorithm.geometry.Angle
import SonarRequest
import net.car.client.Client
import org.asynchttpclient.ListenableFuture
import org.asynchttpclient.Response
import roomScanner.CarController.Direction
import roomScanner.serialize
class Car constructor(val uid: Int, host: String, port: Int) {
@@ -13,26 +18,24 @@ class Car constructor(val uid: Int, host: String, port: Int) {
var angle = 0.0
val carConnection = CarConnection(host, port)
fun refreshPositionAfterRoute(route:RouteMetricRequest) {
val carAngle = angle.toInt()
route.directions.forEachIndexed { idx, direction ->
when (direction) {
Direction.FORWARD.id -> {
x += (Math.cos(Angle(carAngle).rads()) * route.distances[idx]).toInt()
y += (Math.sin(Angle(carAngle).rads()) * route.distances[idx]).toInt()
}
Direction.BACKWARD.id -> {
x -= (Math.cos(Angle(carAngle).rads()) * route.distances[idx]).toInt()
y -= (Math.sin(Angle(carAngle).rads()) * route.distances[idx]).toInt()
}
Direction.LEFT.id -> {
route.distances[idx] = (CHARGE_CORRECTION * route.distances[idx]).toInt()
}
Direction.RIGHT.id -> {
route.distances[idx] = (CHARGE_CORRECTION * route.distances[idx]).toInt()
}
}
}
fun moveCar(distance: Int, direction: Direction): ListenableFuture<Response> {
val route = RouteMetricRequest.BuilderRouteMetricRequest(
IntArray(1, { (distance * CHARGE_CORRECTION).toInt() }), IntArray(1, { direction.id }))
val bytesRoute = ByteArray(route.getSizeNoTag())
route.writeTo(CodedOutputStream(bytesRoute))
return carConnection.sendRequest(Client.Request.ROUTE_METRIC, bytesRoute)
}
fun scan(angles: IntArray, attempts: Int, windowSize: Int, smoothing: SonarRequest.Smoothing): ListenableFuture<Response> {
val message = SonarRequest.BuilderSonarRequest(
angles = angles,
attempts = IntArray(angles.size, { attempts }),
smoothing = smoothing,
windowSize = windowSize)
.build()
val data = serialize(message.getSizeNoTag(), { message.writeTo(it) })
return carConnection.sendRequest(Client.Request.SONAR, data)
}
override fun toString(): String {