diff --git a/car_srv/kotlinSrv/src/control/emulator/ControllerEmulator.kt b/car_srv/kotlinSrv/src/control/emulator/ControllerEmulator.kt index afa146dd657..e16a5cba2bf 100644 --- a/car_srv/kotlinSrv/src/control/emulator/ControllerEmulator.kt +++ b/car_srv/kotlinSrv/src/control/emulator/ControllerEmulator.kt @@ -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> = mutableListOf() + val commands: MutableList> = 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> = 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>, currentCommandIdx: Int, callBack: (ByteArray) -> Unit) { + fun executeCommand(commands: List>, 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 -> { } } diff --git a/server/src/main/java/ServerMain.kt b/server/src/main/java/ServerMain.kt index 2df45c457ae..f978aeb3a87 100644 --- a/server/src/main/java/ServerMain.kt +++ b/server/src/main/java/ServerMain.kt @@ -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" diff --git a/server/src/main/java/algorithm/AbstractAlgorithm.kt b/server/src/main/java/algorithm/AbstractAlgorithm.kt index e1f49e07504..b8f2de5dfcd 100644 --- a/server/src/main/java/algorithm/AbstractAlgorithm.kt +++ b/server/src/main/java/algorithm/AbstractAlgorithm.kt @@ -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()) } 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): CarState? - protected abstract fun getCommand(anglesDistances: Map, state: CarState): RouteRequest - protected abstract fun afterGetCommand(route:RouteRequest) + protected abstract fun getCommand(anglesDistances: Map, state: CarState): RouteMetricRequest + protected abstract fun afterGetCommand(route: RouteMetricRequest) private fun getDefaultHttpRequest(host: String, url: String, bytes: ByteArray): DefaultFullHttpRequest { diff --git a/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt b/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt index f564c664ea5..32d21b63eff 100644 --- a/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt +++ b/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt @@ -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) : 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) : 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, state: CarState): RouteRequest { + override fun getCommand(anglesDistances: Map, 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) : 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) : 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) : 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() } } diff --git a/server/src/main/java/algorithm/RoomTest.kt b/server/src/main/java/algorithm/RoomTest.kt index 8438f1cf536..88970b85ea6 100644 --- a/server/src/main/java/algorithm/RoomTest.kt +++ b/server/src/main/java/algorithm/RoomTest.kt @@ -2,16 +2,12 @@ package algorithm import objects.Car import java.util.concurrent.Exchanger -import RouteRequest +import RouteMetricRequest class RoomTest(thisCar: Car, exchanger: Exchanger) : AbstractAlgorithm(thisCar, exchanger) { - private val MOVE_VELOCITY = 0.05//sm/ms - private val ROTATION_VELOCITY = 0.05//degrees/ms - val points = arrayListOf>() - var carX = 0 var carY = 0 var carAngle = 0 @@ -20,47 +16,46 @@ class RoomTest(thisCar: Car, exchanger: Exchanger) : AbstractAlgorithm return CarState.WALL } - override fun getCommand(anglesDistances: Map, state: CarState): RouteRequest { + override fun getCommand(anglesDistances: Map, 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) : 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] } } } diff --git a/server/src/main/java/car/client/ClientHandler.kt b/server/src/main/java/car/client/ClientHandler.kt index 05d21ff6fca..083c45c7922 100644 --- a/server/src/main/java/car/client/ClientHandler.kt +++ b/server/src/main/java/car/client/ClientHandler.kt @@ -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 { val angles = ctx.channel().attr(AttributeKey.valueOf("angles")).get() handlerSonarResponse(response, angles) } - setRouteUrl -> { + setRouteUrl, setRouteMetricUrl -> { try { DebugClInterface.exchanger.exchange(IntArray(0), 20, TimeUnit.SECONDS) } catch (e: InterruptedException) {