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

add work with metric route

Browse Source
This commit is contained in:
MaximZaitsev
2016-08-29 10:50:57 +03:00
parent 014523984c
commit d9249c7227
6 changed files with 77 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files
car_srv/kotlinSrv/src/control/emulator/ControllerEmulator.kt
+29 -11
View File
@@ -18,7 +18,7 @@ class ControllerEmulator : Controller {
private val MOVE_VELOCITY = 0.05//sm/ms
private val ROTATION_VELOCITY = 0.05//degrees/ms
private val ADD_RANDOM = true
private val ADD_RANDOM = false
enum class MoveDirection {
LEFT,
@@ -32,7 +32,7 @@ class ControllerEmulator : Controller {
val moveTimes = route.times
val moveDirections = route.directions
//list of move direction and time to this move in ms
val commands: MutableList<Pair<MoveDirection, Int>> = mutableListOf()
val commands: MutableList<Pair<MoveDirection, Double>> = mutableListOf()
moveTimes.forEachIndexed { idx, value ->
val moveDirection =
@@ -44,13 +44,31 @@ class ControllerEmulator : Controller {
else -> MoveDirection.ERROR
}
commands.add(Pair(moveDirection, value))
if (moveDirection == MoveDirection.FORWARD || moveDirection == MoveDirection.BACKWARD) {
commands.add(Pair(moveDirection, value * MOVE_VELOCITY))
} else {
commands.add(Pair(moveDirection, value * ROTATION_VELOCITY))
}
}
executeCommand(commands, 0, callback)
}
override fun executeMetricRoute(request: RouteMetricRequest, callback: (ByteArray) -> Unit) {
throw UnsupportedOperationException()
val commands: MutableList<Pair<MoveDirection, Double>> = mutableListOf()
val moveDistances = request.distances
moveDistances.forEachIndexed { idx, value ->
val moveDirection =
when (request.directions[idx]) {
0 -> MoveDirection.FORWARD
1 -> MoveDirection.BACKWARD
2 -> MoveDirection.LEFT
3 -> MoveDirection.RIGHT
else -> MoveDirection.ERROR
}
commands.add(Pair(moveDirection, value.toDouble()))
}
executeCommand(commands, 0, callback)
}
override fun executeRequestSensorData(sonarRequest: SonarRequest, callback: (ByteArray) -> Unit) {
@@ -151,7 +169,7 @@ class ControllerEmulator : Controller {
}
fun executeCommand(commands: List<Pair<MoveDirection, Int>>, currentCommandIdx: Int, callBack: (ByteArray) -> Unit) {
fun executeCommand(commands: List<Pair<MoveDirection, Double>>, currentCommandIdx: Int, callBack: (ByteArray) -> Unit) {
if (currentCommandIdx == commands.size) {
val responseMessage = RouteResponse.BuilderRouteResponse(0).build()
callBack.invoke(encodeProtoBuf(responseMessage))
@@ -161,14 +179,14 @@ class ControllerEmulator : Controller {
//refresh car state
val carInstance = CarState.instance
val commandTime = currentCommand.second
val commandDistance = currentCommand.second
val delta = if (ADD_RANDOM) Math.random() * 0.2 + 0.9 else 1.0// delta in [0.9, 1.1)
val commandTimeIncludeRandom = (commandTime * delta).toInt()
val commandDistanceIncludeRandom = (commandDistance * delta).toInt()
when (currentCommand.first) {
MoveDirection.FORWARD -> carInstance.moving((commandTimeIncludeRandom * MOVE_VELOCITY).toInt())
MoveDirection.BACKWARD -> carInstance.moving(-(commandTimeIncludeRandom * MOVE_VELOCITY).toInt())
MoveDirection.RIGHT -> carInstance.rotate(-(commandTimeIncludeRandom * ROTATION_VELOCITY).toInt())
MoveDirection.LEFT -> carInstance.rotate((commandTimeIncludeRandom * ROTATION_VELOCITY).toInt())
MoveDirection.FORWARD -> carInstance.moving(commandDistanceIncludeRandom)
MoveDirection.BACKWARD -> carInstance.moving(commandDistanceIncludeRandom)
MoveDirection.RIGHT -> carInstance.rotate(-commandDistanceIncludeRandom)
MoveDirection.LEFT -> carInstance.rotate(commandDistanceIncludeRandom)
else -> {
}
}
server/src/main/java/ServerMain.kt
+1
View File
@@ -5,6 +5,7 @@ val carServerPort: Int = 7925
val webServerPort: Int = 7926
val getLocationUrl = "/getLocation"
val setRouteUrl = "/route"
val setRouteMetricUrl = "/routeMetric"
val connectUrl = "/connect"
val debugMemoryUrl = "/debug/memory"
val sonarUrl = "/sonar"
server/src/main/java/algorithm/AbstractAlgorithm.kt
+12 -7
View File
@@ -2,11 +2,12 @@ package algorithm
import CodedOutputStream
import Exceptions.InactiveCarException
import RouteRequest
import RouteMetricRequest
import SonarRequest
import io.netty.buffer.Unpooled
import io.netty.handler.codec.http.*
import objects.Car
import setRouteMetricUrl
import setRouteUrl
import sonarUrl
import java.util.*
@@ -72,17 +73,21 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
return DoubleArray(0)
}
protected fun moveCar(message: RouteRequest) {
protected fun moveCar(message: RouteMetricRequest) {
val requestBytes = ByteArray(message.getSizeNoTag())
message.writeTo(CodedOutputStream(requestBytes))
val request = getDefaultHttpRequest(thisCar.host, setRouteUrl, requestBytes)
moveCar(requestBytes)
}
private fun moveCar(messageBytes: ByteArray) {
val request = getDefaultHttpRequest(thisCar.host, setRouteMetricUrl, messageBytes)
try {
car.client.Client.sendRequest(request, thisCar.host, thisCar.port, mapOf<String, Int>())
} catch (e: InactiveCarException) {
println("connection error!")
}
try {
exchanger.exchange(IntArray(0), 20, TimeUnit.SECONDS)
exchanger.exchange(IntArray(0), 60, TimeUnit.SECONDS)
return
} catch (e: InterruptedException) {
println("don't have response from car!")
@@ -115,7 +120,7 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
val command = getCommand(anglesDistances, state)
afterGetCommand(command)
println(Arrays.toString(command.directions))
println(Arrays.toString(command.times))
println(Arrays.toString(command.distances))
this.prevSonarDistances = anglesDistances
this.prevState = state
@@ -137,8 +142,8 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
}
protected abstract fun getCarState(anglesDistances: Map<Int, Double>): CarState?
protected abstract fun getCommand(anglesDistances: Map<Int, Double>, state: CarState): RouteRequest
protected abstract fun afterGetCommand(route:RouteRequest)
protected abstract fun getCommand(anglesDistances: Map<Int, Double>, state: CarState): RouteMetricRequest
protected abstract fun afterGetCommand(route: RouteMetricRequest)
private fun getDefaultHttpRequest(host: String, url: String, bytes: ByteArray): DefaultFullHttpRequest {
server/src/main/java/algorithm/RoomBypassingAlgorithm.kt
+17 -23
View File
@@ -1,16 +1,13 @@
package algorithm
import RouteRequest
import algorithm.geometry.Line
import algorithm.geometry.Vector
import objects.Car
import RouteMetricRequest
import java.util.concurrent.Exchanger
class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : AbstractAlgorithm(thisCar, exchanger) {
private val MOVE_VELOCITY = 0.05//sm/ms
private val ROTATION_VELOCITY = 0.05//degrees/ms
public var wallAngleWithOX = 0.0//in radian
public var wallLength = 0.0//sm
var errorCount = 0
@@ -41,40 +38,40 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
return args
}
override fun afterGetCommand(route: RouteRequest) {
override fun afterGetCommand(route: RouteMetricRequest) {
route.directions.forEachIndexed { idx, direction ->
when (direction) {
FORWARD -> {
carX += (MOVE_VELOCITY * route.times[idx] * Math.cos(degreesToRadian(carAngle.toInt()))).toInt()
carY += (MOVE_VELOCITY * route.times[idx] * Math.sin(degreesToRadian(carAngle.toInt()))).toInt()
carX += (Math.cos(degreesToRadian(carAngle.toInt())) * route.distances[idx]).toInt()
carY += (Math.sin(degreesToRadian(carAngle.toInt())) * route.distances[idx]).toInt()
}
BACKWARD -> {
carX -= (MOVE_VELOCITY * route.times[idx] * Math.cos(degreesToRadian(carAngle.toInt()))).toInt()
carY -= (MOVE_VELOCITY * route.times[idx] * Math.sin(degreesToRadian(carAngle.toInt()))).toInt()
carX -= (Math.cos(degreesToRadian(carAngle.toInt())) * route.distances[idx]).toInt()
carY -= (Math.sin(degreesToRadian(carAngle.toInt())) * route.distances[idx]).toInt()
}
LEFT -> {
carAngle += (ROTATION_VELOCITY * route.times[idx]).toInt()
carAngle += route.distances[idx]
}
RIGHT -> {
carAngle -= (ROTATION_VELOCITY * route.times[idx]).toInt()
carAngle -= route.distances[idx]
}
}
}
}
override fun getCommand(anglesDistances: Map<Int, Double>, state: CarState): RouteRequest {
override fun getCommand(anglesDistances: Map<Int, Double>, state: CarState): RouteMetricRequest {
val dist0 = anglesDistances[0]
val dist60 = anglesDistances[60]
val dist90 = anglesDistances[90]
val dist120 = anglesDistances[120]
val dist180 = anglesDistances[180]
val resultBuilder = RouteRequest.BuilderRouteRequest(IntArray(0), IntArray(0))
val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0))
if (dist120 == null || dist90 == null || dist60 == null) {
println("null distance!")
if (errorCount >= 3) {
errorCount = 0
resultBuilder.setDirections(getIntArray(BACKWARD))
resultBuilder.setTimes(getIntArray((15.0 / MOVE_VELOCITY).toInt()))
resultBuilder.setDistances(getIntArray(15))
return resultBuilder.build()
}
errorCount++
@@ -96,20 +93,19 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
if (dist90 > 40 || dist90 < 20) {
val rotationDirection = if (dist90 > 40) RIGHT else LEFT
resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD))
resultBuilder.setTimes(getIntArray((10.0 / ROTATION_VELOCITY).toInt(),
(35.0 / MOVE_VELOCITY).toInt()))
resultBuilder.setDistances(getIntArray(10, 35))
return resultBuilder.build()
}
if (Math.abs(dist120 - dist60) > 10) {
val rotationDirection = if (dist120 > dist60) LEFT else RIGHT
resultBuilder.setDirections(getIntArray(rotationDirection))
resultBuilder.setTimes(getIntArray((15.0 / ROTATION_VELOCITY).toInt()))
resultBuilder.setDistances(getIntArray(15))
return resultBuilder.build()
}
resultBuilder.setDirections(getIntArray(FORWARD))
resultBuilder.setTimes(getIntArray((35.0 / MOVE_VELOCITY).toInt()))
resultBuilder.setDistances(getIntArray(35))
return resultBuilder.build()
}
CarState.INNER -> {
@@ -118,7 +114,7 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
if (Math.abs(dist120 - dist60) > 10) {
val rotationDirection = if (dist120 > dist60) LEFT else RIGHT
resultBuilder.setDirections(getIntArray(rotationDirection))
resultBuilder.setTimes(getIntArray((4 * 3.0 / ROTATION_VELOCITY).toInt()))//todo calibrate
resultBuilder.setDistances(getIntArray(12))
return resultBuilder.build()
}
@@ -162,15 +158,13 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
wallLength = 0.0
RoomModel.lines.add(nextLine)
resultBuilder.setDirections(getIntArray(LEFT, FORWARD, LEFT))
resultBuilder.setTimes(getIntArray((wallsAngleInDegrees / (2 * ROTATION_VELOCITY)).toInt(),
(40 / MOVE_VELOCITY).toInt(), (wallsAngleInDegrees / (2 * ROTATION_VELOCITY)).toInt()))
resultBuilder.setDistances(getIntArray((angle / 2).toInt(), 15, (angle / 2).toInt()))
return resultBuilder.build()
}
CarState.OUTER -> {
//todo calculate target
resultBuilder.setDirections(getIntArray(RIGHT, FORWARD, RIGHT))
resultBuilder.setTimes(getIntArray((45 / ROTATION_VELOCITY).toInt(),
(40 / MOVE_VELOCITY).toInt(), (45 / ROTATION_VELOCITY).toInt()))
resultBuilder.setDistances(getIntArray(45, 40, 45))
return resultBuilder.build()
}
}
server/src/main/java/algorithm/RoomTest.kt
+16 -21
View File
@@ -2,16 +2,12 @@ package algorithm
import objects.Car
import java.util.concurrent.Exchanger
import RouteRequest
import RouteMetricRequest
class RoomTest(thisCar: Car, exchanger: Exchanger<IntArray>) : AbstractAlgorithm(thisCar, exchanger) {
private val MOVE_VELOCITY = 0.05//sm/ms
private val ROTATION_VELOCITY = 0.05//degrees/ms
val points = arrayListOf<Pair<Double, Double>>()
var carX = 0
var carY = 0
var carAngle = 0
@@ -20,47 +16,46 @@ class RoomTest(thisCar: Car, exchanger: Exchanger<IntArray>) : AbstractAlgorithm
return CarState.WALL
}
override fun getCommand(anglesDistances: Map<Int, Double>, state: CarState): RouteRequest {
override fun getCommand(anglesDistances: Map<Int, Double>, state: CarState): RouteMetricRequest {
val dist0 = anglesDistances[0]
val dist60 = anglesDistances[60]
val dist90 = anglesDistances[90]
val dist120 = anglesDistances[120]
val resultBuilder = RouteRequest.BuilderRouteRequest(IntArray(0), IntArray(0))
val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0))
if (dist90 == null) {
resultBuilder.setDirections(getIntArray(RIGHT))
resultBuilder.setTimes(getIntArray((10 / ROTATION_VELOCITY).toInt()))
resultBuilder.setDistances(getIntArray(10))
return resultBuilder.build()
}
val sonarAngle = carAngle - 90
points.add(Pair(carX + dist90 * Math.cos(degreesToRadian(sonarAngle)),
carY + dist90 * Math.sin(degreesToRadian(sonarAngle))))
if (dist120 == null || dist60 == null) {
//hmm
resultBuilder.setDirections(getIntArray(FORWARD))
resultBuilder.setTimes(getIntArray((10 / MOVE_VELOCITY).toInt()))
resultBuilder.setDistances(getIntArray(10))
return resultBuilder.build()
}
if (dist0 != null && dist0 < 60) {
resultBuilder.setDirections(getIntArray(LEFT, FORWARD))
resultBuilder.setTimes(getIntArray((20 / ROTATION_VELOCITY).toInt(), (10 / MOVE_VELOCITY).toInt()))
resultBuilder.setDistances(getIntArray(20, 10))
return resultBuilder.build()
}
val abs = Math.abs(dist120 - dist60)
if (abs > 10) {
val rotationDirection = if (dist120 > dist60) LEFT else RIGHT
resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD))
resultBuilder.setTimes(getIntArray((10.0 / ROTATION_VELOCITY).toInt(), (7 / MOVE_VELOCITY).toInt()))//todo calibrate
resultBuilder.setDistances(getIntArray(10, 7))//todo calibrate
return resultBuilder.build()
}
if (dist90 > 50 || dist90 < 25) {
val rotationDirection = if (dist90 > 50) RIGHT else LEFT
resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD))
resultBuilder.setTimes(getIntArray((10 / ROTATION_VELOCITY).toInt(), (10 / MOVE_VELOCITY).toInt()))
resultBuilder.setDistances(getIntArray(10, 10))
return resultBuilder.build()
}
resultBuilder.setDirections(getIntArray(FORWARD))
resultBuilder.setTimes(getIntArray((20 / ROTATION_VELOCITY).toInt()))
resultBuilder.setDistances(getIntArray(20))
return resultBuilder.build()
}
@@ -68,22 +63,22 @@ class RoomTest(thisCar: Car, exchanger: Exchanger<IntArray>) : AbstractAlgorithm
return args
}
override fun afterGetCommand(route: RouteRequest) {
override fun afterGetCommand(route: RouteMetricRequest) {
route.directions.forEachIndexed { idx, direction ->
when (direction) {
FORWARD -> {
carX += (MOVE_VELOCITY * route.times[idx] * Math.cos(degreesToRadian(carAngle.toInt()))).toInt()
carY += (MOVE_VELOCITY * route.times[idx] * Math.sin(degreesToRadian(carAngle.toInt()))).toInt()
carX += (Math.cos(degreesToRadian(carAngle.toInt())) * route.distances[idx]).toInt()
carY += (Math.sin(degreesToRadian(carAngle.toInt())) * route.distances[idx]).toInt()
}
BACKWARD -> {
carX -= (MOVE_VELOCITY * route.times[idx] * Math.cos(degreesToRadian(carAngle.toInt()))).toInt()
carY -= (MOVE_VELOCITY * route.times[idx] * Math.sin(degreesToRadian(carAngle.toInt()))).toInt()
carX -= (Math.cos(degreesToRadian(carAngle.toInt())) * route.distances[idx]).toInt()
carY -= (Math.sin(degreesToRadian(carAngle.toInt())) * route.distances[idx]).toInt()
}
LEFT -> {
carAngle += (ROTATION_VELOCITY * route.times[idx]).toInt()
carAngle += route.distances[idx]
}
RIGHT -> {
carAngle -= (ROTATION_VELOCITY * route.times[idx]).toInt()
carAngle -= route.distances[idx]
}
}
}
server/src/main/java/car/client/ClientHandler.kt
+2 -1
View File
@@ -12,6 +12,7 @@ import io.netty.channel.SimpleChannelInboundHandler
import io.netty.handler.codec.http.DefaultHttpContent
import io.netty.util.AttributeKey
import objects.Environment
import setRouteMetricUrl
import setRouteUrl
import sonarUrl
import java.util.*
@@ -45,7 +46,7 @@ class ClientHandler : SimpleChannelInboundHandler<Any> {
val angles = ctx.channel().attr(AttributeKey.valueOf<IntArray>("angles")).get()
handlerSonarResponse(response, angles)
}
setRouteUrl -> {
setRouteUrl, setRouteMetricUrl -> {
try {
DebugClInterface.exchanger.exchange(IntArray(0), 20, TimeUnit.SECONDS)
} catch (e: InterruptedException) {