diff --git a/rc-app/src/main/java/nl/vaneijndhoven/dukes/app/LaneDetectionApp.java b/rc-app/src/main/java/nl/vaneijndhoven/dukes/app/LaneDetectionApp.java index b5a98aa9..45fa874f 100644 --- a/rc-app/src/main/java/nl/vaneijndhoven/dukes/app/LaneDetectionApp.java +++ b/rc-app/src/main/java/nl/vaneijndhoven/dukes/app/LaneDetectionApp.java @@ -7,6 +7,8 @@ import javafx.scene.Scene; import javafx.scene.layout.BorderPane; import javafx.stage.Stage; +import nl.vaneijndhoven.dukes.common.Config; +import nl.vaneijndhoven.dukes.common.Environment; /** * JavaFx Lane Detection App diff --git a/rc-car/src/main/java/nl/vaneijndhoven/dukes/car/SteeringHandler.java b/rc-car/src/main/java/nl/vaneijndhoven/dukes/car/SteeringHandler.java index 4dc6c86b..4f9ed93b 100644 --- a/rc-car/src/main/java/nl/vaneijndhoven/dukes/car/SteeringHandler.java +++ b/rc-car/src/main/java/nl/vaneijndhoven/dukes/car/SteeringHandler.java @@ -4,6 +4,7 @@ import io.vertx.core.logging.Logger; import io.vertx.core.logging.LoggerFactory; import nl.vaneijndhoven.dukes.car.SteeringMap; +import nl.vaneijndhoven.dukes.common.Config; import nl.vaneijndhoven.dukes.drivecontrol.Car; /** @@ -83,17 +84,17 @@ void handleServo(JsonObject messageBody) { SteeringMap steeringMap = car.getSteering().getSteeringMap(); - if ("left".equals(position)) { + if (Config.POSITION_LEFT.equals(position)) { currentWheelPosition = currentWheelPosition - steeringMap.stepSize(); if (currentWheelPosition < steeringMap.maxLeft()) { currentWheelPosition = steeringMap.maxLeft(); } - } else if ("right".equals(position)) { + } else if (Config.POSITION_RIGHT.equals(position)) { currentWheelPosition = currentWheelPosition + steeringMap.stepSize(); if (currentWheelPosition > steeringMap.maxRight()) { currentWheelPosition = steeringMap.maxRight(); } - } else if ("center".equals(position)) { + } else if (Config.POSITION_CENTER.equals(position)) { currentWheelPosition = steeringMap.center(); } turnWrapper(currentWheelPosition); diff --git a/rc-car/src/test/java/nl/vaneijndhoven/dukes/car/TestHandlers.java b/rc-car/src/test/java/nl/vaneijndhoven/dukes/car/TestHandlers.java index 34347469..f9d346ec 100644 --- a/rc-car/src/test/java/nl/vaneijndhoven/dukes/car/TestHandlers.java +++ b/rc-car/src/test/java/nl/vaneijndhoven/dukes/car/TestHandlers.java @@ -159,13 +159,13 @@ public void testSteeringHandler() throws Exception { SteeringHandler sh=new SteeringHandler(car); Steering steering = car.getSteering(); JsonObject msg=new JsonObject(); - msg.put("position","center"); + msg.put("position",Config.POSITION_CENTER); sh.handleServo(msg); assertEquals(130,steering.getServo()); - msg.put("position","left"); + msg.put("position",Config.POSITION_RIGHT); sh.handleServo(msg); assertEquals(135,steering.getServo()); - msg.put("position","left"); + msg.put("position",Config.POSITION_LEFT); sh.handleServo(msg); assertEquals(140,steering.getServo()); if (debug) diff --git a/rc-common/src/main/java/nl/vaneijndhoven/dukes/common/Config.java b/rc-common/src/main/java/nl/vaneijndhoven/dukes/common/Config.java index 54b90c97..4510abfc 100644 --- a/rc-common/src/main/java/nl/vaneijndhoven/dukes/common/Config.java +++ b/rc-common/src/main/java/nl/vaneijndhoven/dukes/common/Config.java @@ -48,6 +48,10 @@ public class Config { public static final String ENGINE_MAX_SPEED_REVERSE = "engine.max.speed.reverse"; public static final String ENGINE_MIN_SPEED_FORWARD = "engine.min.speed.forward"; public static final String ENGINE_MAX_SPEED_FORWARD = "engine.max.speed.forward"; + // Commands + public static final String POSITION_LEFT = "left"; + public static final String POSITION_RIGHT = "right"; + public static final String POSITION_CENTER = "center"; /** * configure the logging diff --git a/rc-detect/src/main/java/nl/vaneijndhoven/opencv/video/LaneDetectionController.java b/rc-detect/src/main/java/nl/vaneijndhoven/opencv/video/LaneDetectionController.java index 87e07a4b..e384cdcc 100644 --- a/rc-detect/src/main/java/nl/vaneijndhoven/opencv/video/LaneDetectionController.java +++ b/rc-detect/src/main/java/nl/vaneijndhoven/opencv/video/LaneDetectionController.java @@ -1,15 +1,14 @@ package nl.vaneijndhoven.opencv.video; +import org.opencv.core.Mat; + import io.vertx.core.Vertx; import io.vertx.core.logging.Logger; import io.vertx.core.logging.LoggerFactory; +import nl.vaneijndhoven.dukes.common.Config; import nl.vaneijndhoven.opencv.edgedectection.CannyEdgeDetector; -import nl.vaneijndhoven.opencv.lanedetection.ImageLaneDetection; import nl.vaneijndhoven.opencv.linedetection.ProbabilisticHoughLinesLineDetector; import nl.vaneijndhoven.opencv.tools.ImageCollector; -import org.opencv.core.Mat; - -import java.util.Map; /** * lane detection controller @@ -17,213 +16,214 @@ */ public class LaneDetectionController { - protected static final Logger LOG = LoggerFactory + protected static final Logger LOG = LoggerFactory .getLogger(LaneDetectionController.class); - public static double DEFAULT_LANE_BOUNDARY_ANGLE = 45; - public static double DEFAULT_LANE_BOUNDARY_TOLERANCE = 30; - - public static final double DEFAULT_CANNY_THRESHOLD_1 = 131d; - public static final double DEFAULT_CANNY_THRESHOLD_2 = 397d; - public static final int DEFAULT_LINE_DETECT_RHO = 1; - public static final double DEFAULT_LINE_DETECT_THETA = Math.PI / 180; - public static final double DEFAULT_LINE_DETECT_THRESHOLD = 42d; - public static final double DEFAULT_LINE_DETECT_MIN_LINE_LENGTH = 0d; - public static final double DEFAULT_LINE_DETECT_MAX_LINE_GAP = 98d; - - - - private static final long MAX_DURATION_NO_LINES_DETECTED = 1000; - - private long COMMAND_LOOP_INTERVAL = 100L; - private boolean stoppingZoneDetected = false; - private Vertx vertx; - private long tsLastLinesDetected = System.currentTimeMillis(); - - private double previousAngle = 0; - - private double minDistanceToStoppingZone = Double.MAX_VALUE; - private long tsLastCommand = System.currentTimeMillis(); - private long tsLastConnectionOKmessageSent = System.currentTimeMillis(); - private double lastRudderPercentageSent = 0d; - - private boolean emergencyStopActivated = false; - - public LaneDetectionController(Vertx vertx) { - this.vertx = vertx; - initDefaults(); - } - - private void initDefaults() { - minDistanceToStoppingZone = Double.MAX_VALUE; - tsLastCommand = System.currentTimeMillis(); - lastRudderPercentageSent = 0d; - } + public static double DEFAULT_LANE_BOUNDARY_ANGLE = 45; + public static double DEFAULT_LANE_BOUNDARY_TOLERANCE = 30; + + public static final double DEFAULT_CANNY_THRESHOLD_1 = 131d; + public static final double DEFAULT_CANNY_THRESHOLD_2 = 397d; + public static final int DEFAULT_LINE_DETECT_RHO = 1; + public static final double DEFAULT_LINE_DETECT_THETA = Math.PI / 180; + public static final double DEFAULT_LINE_DETECT_THRESHOLD = 42d; + public static final double DEFAULT_LINE_DETECT_MIN_LINE_LENGTH = 0d; + public static final double DEFAULT_LINE_DETECT_MAX_LINE_GAP = 98d; + + private static final long MAX_DURATION_NO_LINES_DETECTED = 1000; + + private long COMMAND_LOOP_INTERVAL = 100L; + private boolean stoppingZoneDetected = false; + private Vertx vertx; + private long tsLastLinesDetected = System.currentTimeMillis(); + + private double previousAngle = 0; + + private double minDistanceToStoppingZone = Double.MAX_VALUE; + private long tsLastCommand = System.currentTimeMillis(); + private long tsLastConnectionOKmessageSent = System.currentTimeMillis(); + private double lastRudderPercentageSent = 0d; + + private boolean emergencyStopActivated = false; + + public LaneDetectionController(Vertx vertx) { + this.vertx = vertx; + initDefaults(); + } + + private void initDefaults() { + minDistanceToStoppingZone = Double.MAX_VALUE; + tsLastCommand = System.currentTimeMillis(); + lastRudderPercentageSent = 0d; + } + + public void performLaneDetection(Mat originalImage, + CannyEdgeDetector.Config cannyConfig, + ProbabilisticHoughLinesLineDetector.Config lineDetectorConfig, + ImageCollector collector) { + // ImageLaneDetection laneDetect = new ImageLaneDetection(cannyConfig, + // lineDetectorConfig, matrix); + // Map laneDetectResult = + // laneDetect.detectLane(originalImage, collector); + // + // double distanceToStoppingZoneStart = (double) + // laneDetectResult.get("distanceToStoppingZone"); + // double distanceToStoppingZoneEnd = (double) + // laneDetectResult.get("distanceToStoppingZoneEnd"); + // double angle = (double) laneDetectResult.get("angle"); + // + // processLane(angle, distanceToStoppingZoneStart, + // distanceToStoppingZoneEnd); + } + + private void processLane(double angle, double distanceToStoppingZoneStart, + double distanceToStoppingZoneEnd) { + long currentTime = System.currentTimeMillis(); + + logConnectionOK(); + detectStoppingZone(distanceToStoppingZoneStart); + verifyAngleFound(angle, currentTime); + + double rudderPercentage = 100 * (Math.abs(angle) / 60) * 0.45; // 0.5 voor + // rechtdoor + // rijden + + // double damp = Math.abs(previousAngle) - angle); + + // System.out.println("prev: " + previousAngle + ", angle: " + angle + ", + // rudderPercentage: " + rudderPercentage + ", damp: " + damp); + String direction="?"; + if (currentTime - tsLastCommand > COMMAND_LOOP_INTERVAL) { + + if ((previousAngle != 0) + && (!((previousAngle < 0 && angle > 0) + || (previousAngle > 0 && angle < 0))) + && (Math.abs(angle) < Math.abs(previousAngle))) { + direction=Config.POSITION_CENTER; + rudderPercentage = 0; + } else { + if (rudderPercentage > 0) { + + if (angle > 0) { + direction = Config.POSITION_LEFT; + rudderPercentage = -rudderPercentage * 1; + } else { + direction = Config.POSITION_RIGHT; + rudderPercentage = rudderPercentage * 1.3; + } + + } + } + String msg = String.format("steering %s, percentage: %3.1f %% ", + direction, rudderPercentage); + LOG.debug(msg); + + eventBusSendAfterMS(0, "setwheel:" + rudderPercentage); + tsLastCommand = currentTime; + lastRudderPercentageSent = rudderPercentage; + previousAngle = angle; - public void performLaneDetection(Mat originalImage, CannyEdgeDetector.Config cannyConfig, ProbabilisticHoughLinesLineDetector.Config lineDetectorConfig, ImageCollector collector) { -// ImageLaneDetection laneDetect = new ImageLaneDetection(cannyConfig, lineDetectorConfig, matrix); -// Map laneDetectResult = laneDetect.detectLane(originalImage, collector); -// -// double distanceToStoppingZoneStart = (double) laneDetectResult.get("distanceToStoppingZone"); -// double distanceToStoppingZoneEnd = (double) laneDetectResult.get("distanceToStoppingZoneEnd"); -// double angle = (double) laneDetectResult.get("angle"); -// -// processLane(angle, distanceToStoppingZoneStart, distanceToStoppingZoneEnd); } - - private void processLane(double angle, double distanceToStoppingZoneStart, double distanceToStoppingZoneEnd) { - long currentTime = System.currentTimeMillis(); - - logConnectionOK(); - detectStoppingZone(distanceToStoppingZoneStart); - verifyAngleFound(angle, currentTime); - - - - double rudderPercentage = 100 * (Math.abs(angle) / 60) * 0.45; // 0.5 voor rechtdoor rijden - - - - -// double damp = Math.abs(previousAngle) - angle); - - - // System.out.println("prev: " + previousAngle + ", angle: " + angle + ", rudderPercentage: " + rudderPercentage + ", damp: " + damp); - - - - - if (currentTime - tsLastCommand > COMMAND_LOOP_INTERVAL) { - - if ( - (previousAngle != 0) && - (!((previousAngle < 0 && angle > 0) || (previousAngle > 0 && angle < 0))) - && (Math.abs(angle) < Math.abs(previousAngle))) { - System.out.println("center"); - rudderPercentage = 0; - } else { - - - if (rudderPercentage > 0) { - String direction; - if (angle > 0) { - direction = "left"; - rudderPercentage = -rudderPercentage * 1; - } else { - direction = "right"; - rudderPercentage = rudderPercentage * 1.3; - } - String msg=String.format("steering %s, percentage: %3.1f %% ",direction,rudderPercentage); - LOG.debug(msg); - - - } - - - } - - eventBusSendAfterMS(0, "setwheel:" + rudderPercentage); - tsLastCommand = currentTime; - lastRudderPercentageSent = rudderPercentage; - previousAngle = angle; - + } + + private void verifyAngleFound(double angle, long currentTime) { + if (!(angle > 0) && !(angle < 0)) { + // no angle detected + if ((currentTime - tsLastLinesDetected > MAX_DURATION_NO_LINES_DETECTED) + && !emergencyStopActivated) { + System.out.println("no angle found for " + + MAX_DURATION_NO_LINES_DETECTED + "ms, emergency stop"); + if (vertx != null) { + emergencyStopActivated = true; + vertx.eventBus().send("control", "speed:stop"); } + } + } else { + // all good + tsLastLinesDetected = currentTime; } - - - private void verifyAngleFound(double angle, long currentTime) { - if (!(angle > 0) && !(angle < 0)) { - // no angle detected - if ((currentTime - tsLastLinesDetected > MAX_DURATION_NO_LINES_DETECTED) && !emergencyStopActivated) { - System.out.println("no angle found for " + MAX_DURATION_NO_LINES_DETECTED + "ms, emergency stop"); - if (vertx != null) { - emergencyStopActivated = true; - vertx.eventBus().send("control", "speed:stop"); - } - } - } else { - // all good - tsLastLinesDetected = currentTime; + } + + private void detectStoppingZone(double distanceToStoppingZoneStart) { + // System.out.println("distanceToStoppingZoneStart: " + + // distanceToStoppingZoneStart); + + if (distanceToStoppingZoneStart > 0) { + + if (distanceToStoppingZoneStart < minDistanceToStoppingZone) { + minDistanceToStoppingZone = distanceToStoppingZoneStart; + System.out.println("new minimal distance to stopping zone: " + + minDistanceToStoppingZone); + } + + if (minDistanceToStoppingZone < 100) { + if (distanceToStoppingZoneStart - minDistanceToStoppingZone > 30) { + if (!stoppingZoneDetected) { + System.out.println("--- stop ---"); + eventBusSendAfterMS(1000, "speed:brake"); + stoppingZoneDetected = true; + } } + } + + // if (distanceToStoppingZoneStart - minDistanceToStoppingZone > 20) { + // System.out.println("--- stop ---"); + // eventBusSendAfterMS(10,"speed:brake"); + // } + + // System.out.println("distanceToStoppingZoneStart: " + + // distanceToStoppingZoneStart); + // + // if (distanceToStoppingZoneStart > 5 && distanceToStoppingZoneStart < + // 15) { + // System.out.println("--- stop ---"); + // eventBusSendAfterMS(10,"speed:brake"); + // } + + // if (distanceToStoppingZoneStart < minDistanceToStoppingZone) { + // minDistanceToStoppingZone = distanceToStoppingZoneStart; + // System.out.println("new minimal distance to stopping zone: " + + // minDistanceToStoppingZone); + // } + // + // if (Math.abs(distanceToStoppingZoneStart - minDistanceToStoppingZone) > + // 10) { + // System.out.println("distance increasing: min=" + + // minDistanceToStoppingZone + ", cur = " + distanceToStoppingZoneStart); + // + // int wait = 500; + // stoppingZoneDetected = true; + // System.out.println("stopping zone detected, stopping in " + wait + " + // ms"); + // // eventBusSendAfterMS(wait,"speed:brake"); + // } } - - private void detectStoppingZone(double distanceToStoppingZoneStart) { - // System.out.println("distanceToStoppingZoneStart: " + distanceToStoppingZoneStart); - - if (distanceToStoppingZoneStart > 0) { - - if (distanceToStoppingZoneStart < minDistanceToStoppingZone) { - minDistanceToStoppingZone = distanceToStoppingZoneStart; - System.out.println("new minimal distance to stopping zone: " + minDistanceToStoppingZone); - } - - if (minDistanceToStoppingZone < 100) { - if (distanceToStoppingZoneStart - minDistanceToStoppingZone > 30) { - if (!stoppingZoneDetected) { - System.out.println("--- stop ---"); - eventBusSendAfterMS(1000,"speed:brake"); - stoppingZoneDetected = true; - } - } - } - -// if (distanceToStoppingZoneStart - minDistanceToStoppingZone > 20) { -// System.out.println("--- stop ---"); -// eventBusSendAfterMS(10,"speed:brake"); -// } - - -// System.out.println("distanceToStoppingZoneStart: " + distanceToStoppingZoneStart); -// -// if (distanceToStoppingZoneStart > 5 && distanceToStoppingZoneStart < 15) { -// System.out.println("--- stop ---"); -// eventBusSendAfterMS(10,"speed:brake"); -// } - -// if (distanceToStoppingZoneStart < minDistanceToStoppingZone) { -// minDistanceToStoppingZone = distanceToStoppingZoneStart; -// System.out.println("new minimal distance to stopping zone: " + minDistanceToStoppingZone); -// } -// -// if (Math.abs(distanceToStoppingZoneStart - minDistanceToStoppingZone) > 10) { -// System.out.println("distance increasing: min=" + minDistanceToStoppingZone + ", cur = " + distanceToStoppingZoneStart); -// -// int wait = 500; -// stoppingZoneDetected = true; -// System.out.println("stopping zone detected, stopping in " + wait + " ms"); -// // eventBusSendAfterMS(wait,"speed:brake"); -// } + } + + private void eventBusSendAfterMS(long ms, String command) { + new java.util.Timer().schedule(new java.util.TimerTask() { + @Override + public void run() { + // System.out.println("Sending command '" + command + "'."); + if (vertx != null) { + vertx.eventBus().send("control", command); + } else { + System.out.println( + "Couldn't send command '" + command + "', Vert.x not inited"); } - } - - private void eventBusSendAfterMS(long ms, String command) { - new java.util.Timer().schedule( - new java.util.TimerTask() { - @Override - public void run() { -// System.out.println("Sending command '" + command + "'."); - if (vertx != null) { - vertx.eventBus().send("control", command); - } else { - System.out.println("Couldn't send command '" + command + "', Vert.x not inited"); - } - } - }, - ms - ); - } + } + }, ms); + } + private void logConnectionOK() { + int logConnectionOKinterval = 1000; - private void logConnectionOK() { - int logConnectionOKinterval = 1000; - - long currentTime = System.currentTimeMillis(); - if (currentTime - tsLastConnectionOKmessageSent > logConnectionOKinterval) { - eventBusSendAfterMS(0, "log:LaneDetectionController connected"); - tsLastConnectionOKmessageSent = currentTime; - } - + long currentTime = System.currentTimeMillis(); + if (currentTime - tsLastConnectionOKmessageSent > logConnectionOKinterval) { + eventBusSendAfterMS(0, "log:LaneDetectionController connected"); + tsLastConnectionOKmessageSent = currentTime; } + } }