Corrected error handling in AbstractMotionPlanner; refactored constru…

…ctors in PathEditor2
FedericoPecora · Jan 16, 2020 · dba6c63 · dba6c63
1 parent 87bcdf3
commit dba6c63
Show file tree

Hide file tree

Showing 2 changed files with 66 additions and 19 deletions.
diff --git a/src/main/java/se/oru/coordination/coordination_oru/motionplanning/AbstractMotionPlanner.java b/src/main/java/se/oru/coordination/coordination_oru/motionplanning/AbstractMotionPlanner.java
@@ -154,7 +154,7 @@ public synchronized boolean plan() {
 		goalFoot = at.transform(goalFoot);
 		for (Geometry obs : this.obstacles) {
 			if (obs.intersects(goalFoot)) {
-				metaCSPLogger.info("Goal intersects with an obstacle - no path can exist!");
+				metaCSPLogger.info("Goal intersects with an obstacle, no path can exist");
 				return false;
 			}
 		}
@@ -163,6 +163,11 @@ public synchronized boolean plan() {
 		if (!verifyPlanning) return ret;
 
 		PoseSteering[] path = getPath();
+		if (path == null) {
+			metaCSPLogger.info("Path planner could not find a plan");
+			return false;
+		}
+
 		for (int i = 0; i < path.length; i++) {
 			Pose p = path[i].getPose();
 			Geometry checkFoot = gf.createPolygon(newFoot);
@@ -172,7 +177,7 @@ public synchronized boolean plan() {
 			checkFoot = at.transform(checkFoot);
 			for (Geometry obs : this.obstacles) {
 				if (obs.intersects(checkFoot)) {
-					metaCSPLogger.info("Path verification failed, returning false!");
+					metaCSPLogger.info("Path verification failed");
 					return false;
 				}
 			}

diff --git a/src/main/java/se/oru/coordination/coordination_oru/util/PathEditor2.java b/src/main/java/se/oru/coordination/coordination_oru/util/PathEditor2.java
@@ -119,33 +119,49 @@ public void setMaxTurningRadius(double val) {
 		MAX_TURNING_RADIUS = val;
 	}
 
+	@Deprecated
+	public PathEditor2(String posesAndPaths) {
+		this(posesAndPaths, null, null, 0.1, 0.1, 0.01, (Coordinate[])null);
+	}
+
+	@Deprecated
 	public PathEditor2(String posesAndPaths, String mapFilename) {
-		this(posesAndPaths, mapFilename, null, 0.1, 0.1, 0.01, null);
+		this(posesAndPaths, mapFilename, null, 0.1, 0.1, 0.01, (Coordinate[])null);
 	}
 
-	public PathEditor2(String posesAndPaths, String mapFilename, Coordinate[] footprint) {
-		this(posesAndPaths, mapFilename, null, 0.1, 0.1, 0.01, footprint);
+	@Deprecated
+	public PathEditor2(String posesAndPaths, String mapFilename, String selectionsFile) {
+		this(posesAndPaths, mapFilename, selectionsFile, 0.1, 0.1, 0.01, (Coordinate[])null);
 	}
 
-	public PathEditor2(String posesAndPaths, String mapFilename, String selectionsFile) {
-		this(posesAndPaths, mapFilename, selectionsFile, 0.1, 0.1, 0.01, null);
+	@Deprecated
+	public PathEditor2(String posesAndPaths, String mapFN, String selectionsF, double deltaX, double deltaY, double deltaTheta) {
+		this(posesAndPaths, mapFN, selectionsF, deltaX, deltaY, deltaTheta, (Coordinate[])null);
 	}
 
-	public PathEditor2(String posesAndPaths, String mapFilename, String selectionsFile, Coordinate[] footprint) {
-		this(posesAndPaths, mapFilename, selectionsFile, 0.1, 0.1, 0.01, footprint);
+	public PathEditor2(Coordinate ... footprint) {
+		this(null, null, null, 0.1, 0.1, 0.01, footprint);
 	}
 
-	public PathEditor2(String posesAndPaths, String mapFN, String selectionsF, double deltaX, double deltaY, double deltaTheta) {
-		this(posesAndPaths, mapFN, selectionsF, deltaX, deltaY, deltaTheta, null);
+	public PathEditor2(String posesAndPaths, Coordinate ... footprint) {
+		this(posesAndPaths, null, null, 0.1, 0.1, 0.01, footprint);
 	}
-
-	public PathEditor2(String posesAndPaths, String mapFN, String selectionsF, double deltaX, double deltaY, double deltaTheta, Coordinate[] footprint) {
+
+	public PathEditor2(String posesAndPaths, String mapFilename, Coordinate ... footprint) {
+		this(posesAndPaths, mapFilename, null, 0.1, 0.1, 0.01, footprint);
+	}
+
+	public PathEditor2(String posesAndPaths, String mapFilename, String selectionsFile, Coordinate ... footprint) {
+		this(posesAndPaths, mapFilename, selectionsFile, 0.1, 0.1, 0.01, footprint);
+	}
+
+	public PathEditor2(String posesAndPaths, String mapFilename, String selectionsFilename, double deltaX, double deltaY, double deltaTheta, Coordinate ... footprint) {
 		this.deltaX = deltaX;
 		this.deltaY = deltaY;
 		this.deltaT = deltaTheta;
-		this.mapFilename = mapFN;
+		this.mapFilename = mapFilename;
 		this.footprint = footprint;
-		if (this.footprint != null) this.footprintGeom = TrajectoryEnvelope.createFootprintPolygon(footprint);
+		this.setPathPlanningFootprint(footprint);
 		if (this.mapFilename != null) {
 			String path = this.mapFilename.substring(0, this.mapFilename.lastIndexOf(File.separator)+1);
 			this.mapImgFilename = path+Missions.getProperty("image", this.mapFilename);
@@ -206,8 +222,8 @@ public PathEditor2(String posesAndPaths, String mapFN, String selectionsF, doubl
 			updatePaths2();
 		}
 
-		if (selectionsF != null) {
-			this.selectionsFile = selectionsF;
+		if (selectionsFilename != null) {
+			this.selectionsFile = selectionsFilename;
 			loadSelectionsFile();
 		}
 
@@ -217,8 +233,19 @@ public PathEditor2(String posesAndPaths, String mapFN, String selectionsF, doubl
 	}
 
 	public void setPathPlanningFootprint(Coordinate ... footprint) {
-		this.footprint = footprint;
-		this.footprintGeom = TrajectoryEnvelope.createFootprintPolygon(footprint);
+		//for (int i = 0; i < footprint.length; i++) System.out.println(i + ": " + footprint[i]);
+		if (!footprint[0].equals(footprint[footprint.length-1])) {
+			Coordinate[] fn = new Coordinate[footprint.length+1];
+			for (int i = 0; i < footprint.length; i++) fn[i] = footprint[i];
+			fn[fn.length-1] = fn[0];
+			//for (int j = 0; j < fn.length; j++) System.out.println(j + "*: " + fn[j]);
+			this.footprint = fn;
+			this.footprintGeom = TrajectoryEnvelope.createFootprintPolygon(fn);
+		}
+		else {
+			this.footprint = footprint;
+			this.footprintGeom = TrajectoryEnvelope.createFootprintPolygon(footprint);
+		}
 	}
 
 	public void setPathPlanningRadius(double rad) {
@@ -2341,5 +2368,20 @@ public void mouseMoved(MouseEvent e) {
 			e1.printStackTrace();
 		}
 	}
+
+	public static void main(String[] args) {
+		//Robot Footprint
+		Coordinate[] footprint = new Coordinate[] {
+			new Coordinate(-1.0,0.5),
+			new Coordinate(1.0,0.5),
+			new Coordinate(1.0,-0.5),
+			new Coordinate(-1.0,-0.5),
+		};
+
+		PathEditor2 pe = new PathEditor2(footprint);
+		pe.setZoomIntensity(0.1);
+		pe.setPanAcceleration(10);
+		pe.setSplineDistance(7);
+	}
 
 }