display estimated position (#2)

src/main/java/jtello/core/control/Fliying.java

@@ -1,32 +1,34 @@
 package jtello.core.control;
 
+import jtello.core.movment.NotValidPositionException;
+
 /**
  * Direction for flying
  * @author Ariel
  *
  */
 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;
 
 }

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
+ * <code>SandboxValidator</code>
+ * 
+ * @author Ariel
+ *
+ */
+public class FlightPosition implements Fliying {
+
+	private final Fliying fliying;
+
+	private Position position;
+
+	private List<SandboxValidator> sandboxValidators = new ArrayList<SandboxValidator>();
+
+	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;
+	}
+
+}

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;
+
+}

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 + "]";
+	}
+
+}

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;
+
+}

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<ObjDetector> 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()+")");
+			}
+		});
+	}
+
 }