From 85737e2f1e4e0f8b4dea761925c764e0b3748a0a Mon Sep 17 00:00:00 2001 From: arielr52 Date: Mon, 14 Oct 2019 17:10:18 -0400 Subject: [PATCH] display estimated position (#2) --- .../java/jtello/core/control/Fliying.java | 18 ++- .../jtello/core/movment/FlightPosition.java | 139 ++++++++++++++++++ .../movment/NotValidPositionException.java | 12 ++ .../java/jtello/core/movment/Position.java | 61 ++++++++ .../jtello/core/movment/SandboxValidator.java | 16 ++ .../java/jtello/core/ui/DroneController.java | 35 +++++ src/main/java/jtello/core/ui/DroneModel.java | 94 ++++++------ src/main/java/jtello/core/ui/DroneView.fxml | 25 +++- .../jtello/core/movment/PositionTest.java | 37 +++++ 9 files changed, 382 insertions(+), 55 deletions(-) create mode 100644 src/main/java/jtello/core/movment/FlightPosition.java create mode 100644 src/main/java/jtello/core/movment/NotValidPositionException.java create mode 100644 src/main/java/jtello/core/movment/Position.java create mode 100644 src/main/java/jtello/core/movment/SandboxValidator.java create mode 100644 src/test/java/jtello/core/movment/PositionTest.java diff --git a/src/main/java/jtello/core/control/Fliying.java b/src/main/java/jtello/core/control/Fliying.java index 7d1982e..8ce2216 100644 --- a/src/main/java/jtello/core/control/Fliying.java +++ b/src/main/java/jtello/core/control/Fliying.java @@ -1,5 +1,7 @@ package jtello.core.control; +import jtello.core.movment.NotValidPositionException; + /** * Direction for flying * @author Ariel @@ -7,26 +9,26 @@ */ public interface Fliying { - boolean takeOff(); + boolean takeOff() ; boolean land(); - boolean left(int cm); + boolean left(int cm)throws NotValidPositionException; - boolean right(int cm); + boolean right(int cm)throws NotValidPositionException; - boolean forward(int cm); + boolean forward(int cm)throws NotValidPositionException; - boolean back(int cm); + boolean back(int cm)throws NotValidPositionException; - boolean up(int cm); + boolean up(int cm)throws NotValidPositionException; - boolean down(int cm); + boolean down(int cm)throws NotValidPositionException; boolean rotateRight(int deg); boolean rotateLeft(int deg); - boolean flipForward(); + boolean flipForward() throws NotValidPositionException; } \ No newline at end of file diff --git a/src/main/java/jtello/core/movment/FlightPosition.java b/src/main/java/jtello/core/movment/FlightPosition.java new file mode 100644 index 0000000..a4206ed --- /dev/null +++ b/src/main/java/jtello/core/movment/FlightPosition.java @@ -0,0 +1,139 @@ +package jtello.core.movment; + +import java.util.ArrayList; +import java.util.List; + +import jtello.core.control.Fliying; + +/** + * Track the drone position, allow validating the next position with + * SandboxValidator + * + * @author Ariel + * + */ +public class FlightPosition implements Fliying { + + private final Fliying fliying; + + private Position position; + + private List sandboxValidators = new ArrayList(); + + public FlightPosition(Fliying fliying) { + this.position = new Position(); + this.fliying = fliying; + } + + public Position getPosition() { + return position; + } + + public void addSandboxValidator(SandboxValidator sandboxValidator) { + sandboxValidators.add(sandboxValidator); + } + + void validatetMove(Position nextPosition) throws NotValidPositionException { + for (SandboxValidator validator : sandboxValidators) { + validator.newPosition(nextPosition); + } + } + + @Override + public boolean takeOff() { + boolean result = fliying.takeOff(); + if (result) { + position.moveZ(50); + } + return result; + } + + @Override + public boolean land() { + boolean result = fliying.land(); + if (result) { + this.position = new Position(); + } + return result; + } + + @Override + public boolean left(int cm) throws NotValidPositionException { + validatetMove(position.clone().moveX(-cm)); + boolean result = fliying.left(cm); + if (result) + position.moveX(-cm); + return result; + } + + @Override + public boolean right(int cm) throws NotValidPositionException { + validatetMove(position.clone().moveX(cm)); + boolean result = fliying.right(cm); + if (result) + position.moveX(cm); + return result; + } + + @Override + public boolean forward(int cm) throws NotValidPositionException { + validatetMove(position.clone().moveY(cm)); + boolean result = fliying.forward(cm); + if (result) + position.moveY(cm); + return result; + } + + @Override + public boolean back(int cm) throws NotValidPositionException { + validatetMove(position.clone().moveY(-cm)); + boolean result = fliying.back(cm); + if (result) + position.moveY(-cm); + return result; + } + + @Override + public boolean up(int cm) throws NotValidPositionException { + validatetMove(position.clone().moveZ(cm)); + boolean result = fliying.up(cm); + if (result) + position.moveZ(cm); + return result; + } + + @Override + public boolean down(int cm) throws NotValidPositionException { + validatetMove(position.clone().moveZ(-cm)); + boolean result = fliying.down(cm); + if (result) + position.moveZ(-cm); + return result; + } + + @Override + public boolean rotateRight(int deg) { + boolean result = fliying.rotateRight(deg); + if (result) + position.rotate(deg); + return result; + } + + @Override + public boolean rotateLeft(int deg) { + boolean result = fliying.rotateLeft(deg); + if (result) + position.rotate(-deg); + return result; + } + + @Override + public boolean flipForward() throws NotValidPositionException { + validatetMove(position.clone().moveY(30)); + boolean result = fliying.flipForward(); + if (result) + position.moveY(30); + return result; + } + +} diff --git a/src/main/java/jtello/core/movment/NotValidPositionException.java b/src/main/java/jtello/core/movment/NotValidPositionException.java new file mode 100644 index 0000000..9c84c12 --- /dev/null +++ b/src/main/java/jtello/core/movment/NotValidPositionException.java @@ -0,0 +1,12 @@ +package jtello.core.movment; + +/** + * The position is not valid + * @author Ariel + * + */ +public class NotValidPositionException extends Exception { + + private static final long serialVersionUID = -7126774839714087672L; + +} diff --git a/src/main/java/jtello/core/movment/Position.java b/src/main/java/jtello/core/movment/Position.java new file mode 100644 index 0000000..ba5ad9c --- /dev/null +++ b/src/main/java/jtello/core/movment/Position.java @@ -0,0 +1,61 @@ +package jtello.core.movment; + +import javafx.geometry.Point3D; + +/** + * An estimation of the drone location + * @author Ariel + * + */ +public class Position { + + // in cm + private Point3D location = new Point3D(0, 0, 0); + + // 0-359 + private double direction = 0; + + public Position moveX(double value) { + double x = Math.cos(Math.toRadians(direction))*value; + location = location.add(x, 0, 0); + double y = Math.sin(Math.toRadians(direction))*value; + location = location.add(0, y, 0); + return this; + } + + public Position moveY(double value) { + double x = Math.sin(Math.toRadians(direction))*value; + location = location.add(x, 0, 0); + double y = Math.cos(Math.toRadians(direction))*value; + location = location.add(0, y, 0); + return this; + } + + public Position moveZ(double value) { + location = location.add(0, 0, value); + return this; + } + + public Position rotate(double value) { + direction += value; + direction = direction % 360; + return this; + } + + public Point3D getLocation() { + return location; + } + + public Position clone() { + Position clone = new Position(); + clone.direction=direction; + clone.location=new Point3D(location.getX(), location.getY(), location.getZ()); + return clone; + } + + @Override + public String toString() { + return "Position [location=" + location + ", direction=" + direction + "]"; + } + +} diff --git a/src/main/java/jtello/core/movment/SandboxValidator.java b/src/main/java/jtello/core/movment/SandboxValidator.java new file mode 100644 index 0000000..7c04a97 --- /dev/null +++ b/src/main/java/jtello/core/movment/SandboxValidator.java @@ -0,0 +1,16 @@ +package jtello.core.movment; + +/** + * Callback interface to validate that drone location + * @author Ariel + * + */ +public interface SandboxValidator { + + /** + * @param position the next position the drone will fly to. + * @return true if valid and the drone can move to the new position; + */ + public boolean newPosition(Position position) throws NotValidPositionException; + +} diff --git a/src/main/java/jtello/core/ui/DroneController.java b/src/main/java/jtello/core/ui/DroneController.java index 679f5d2..ddbc0be 100644 --- a/src/main/java/jtello/core/ui/DroneController.java +++ b/src/main/java/jtello/core/ui/DroneController.java @@ -6,10 +6,15 @@ import org.opencv.core.Mat; import javafx.application.Platform; +import javafx.event.Event; import javafx.fxml.FXML; +import javafx.geometry.Point3D; +import javafx.scene.control.CheckBox; import javafx.scene.control.Label; +import javafx.scene.control.Slider; import javafx.scene.image.Image; import javafx.scene.image.ImageView; +import jtello.core.movment.Position; import jtello.core.ui.utils.Utils; /** @@ -31,11 +36,31 @@ public class DroneController { @FXML private ImageView currentFrame; + @FXML + private Label positionLabel; + + @FXML + private Slider sandboxX; + + @FXML + private Slider sandboxY; + + @FXML + private Slider sandboxZ; + + @FXML + private CheckBox detectObj; + Optional objDetectorOptional = Optional.empty(); public DroneController() { } + @FXML + protected void detectObjSelected(Event event) { + log.info("detectObjSelected event=" + event); + } + public void setObjDetector(ObjDetector objDetector) { objDetectorOptional = Optional.ofNullable(objDetector); } @@ -67,4 +92,14 @@ public void run() { }); } + public void updatePosition(Position position) { + Platform.runLater(new Runnable() { + @Override + public void run() { + Point3D location = position.getLocation(); + positionLabel.setText("Estimated location: ("+(int)location.getX()+","+(int)location.getY()+","+(int)location.getZ()+")"); + } + }); + } + } diff --git a/src/main/java/jtello/core/ui/DroneModel.java b/src/main/java/jtello/core/ui/DroneModel.java index 7c1e45d..5cabfdf 100644 --- a/src/main/java/jtello/core/ui/DroneModel.java +++ b/src/main/java/jtello/core/ui/DroneModel.java @@ -20,6 +20,8 @@ import jtello.core.communication.Communication; import jtello.core.communication.Video; import jtello.core.control.Control; +import jtello.core.movment.FlightPosition; +import jtello.core.movment.NotValidPositionException; /** * The UI Model @@ -35,12 +37,14 @@ public class DroneModel extends Application { DroneController controller; + FlightPosition flightPosition; + @Override public void start(Stage primaryStage) { try { FXMLLoader loader = new FXMLLoader(getClass().getResource("DroneView.fxml")); BorderPane rootElement = (BorderPane) loader.load(); - Scene scene = new Scene(rootElement, 1100, 800); + Scene scene = new Scene(rootElement, 1200, 800); scene.getStylesheets().add(getClass().getResource("application.css").toExternalForm()); primaryStage.setTitle("JTello"); primaryStage.setScene(scene); @@ -53,7 +57,6 @@ public void handle(WindowEvent we) { })); controller = loader.getController(); - startVideo(); new Thread(() -> controlDrone(primaryStage)).start(); @@ -88,10 +91,11 @@ void controlDrone(Stage primaryStage) { }).start(); Control control = new Control(communication); + flightPosition = new FlightPosition(control); boolean connected = control.commandAndVideoStream(); if (!connected) { log.fatal("Fail to connect to the Drone! exiting"); - System.exit(-1); + // System.exit(-1); } primaryStage.addEventHandler(KeyEvent.KEY_PRESSED, new EventHandler() { @@ -99,21 +103,22 @@ public void handle(KeyEvent key) { executor.submit(() -> executeKeyEvent(control, key)); } }); + ObjDetector objDetector = new ObjDetector(); controller.setObjDetector(objDetector); - + new Thread(() -> { while (true) { objDetector.get().ifPresent(objCenter -> { - Point2D frameCenter = objDetector.getFrameCenter(); - //20% of screen - int offset = (int) (frameCenter.x/10) ; - log.debug("objCenter= "+objCenter+", offset="+offset); - if(objCenter.x+offset + +
- + - + + + diff --git a/src/test/java/jtello/core/movment/PositionTest.java b/src/test/java/jtello/core/movment/PositionTest.java new file mode 100644 index 0000000..a3a92c8 --- /dev/null +++ b/src/test/java/jtello/core/movment/PositionTest.java @@ -0,0 +1,37 @@ +package jtello.core.movment; + +import static org.junit.Assert.assertEquals; + +import org.junit.Test; + +public class PositionTest { + + @Test + public void test_moveX() { + Position testObj = new Position(); + Position newPosition = testObj.moveX(10); + assertEquals(10, newPosition.getLocation().getX(), 0.1); + + testObj = new Position(); + + testObj.rotate(45); + newPosition = testObj.moveX(10); + assertEquals(7.07, newPosition.getLocation().getX(), 0.1); + assertEquals(7.07, newPosition.getLocation().getY(), 0.1); + } + + + @Test + public void test_moveY() { + Position testObj = new Position(); + Position newPosition = testObj.moveY(10); + assertEquals(10, newPosition.getLocation().getY(), 0.1); + + testObj = new Position(); + + testObj.rotate(30); + newPosition = testObj.moveY(10); + assertEquals(5, newPosition.getLocation().getX(), 0.1); + assertEquals(8.7, newPosition.getLocation().getY(), 0.1); + } +}