package se.oru.coordination.coordination_oru.motionplanning; import java.io.File; import java.util.ArrayList; import java.util.Collections; import java.util.logging.Logger; import org.metacsp.multi.spatioTemporal.paths.Pose; import org.metacsp.multi.spatioTemporal.paths.PoseSteering; import org.metacsp.utility.logging.MetaCSPLogging; import com.vividsolutions.jts.geom.Coordinate; import com.vividsolutions.jts.geom.Geometry; import com.vividsolutions.jts.geom.GeometryFactory; import com.vividsolutions.jts.geom.util.AffineTransformation; import se.oru.coordination.coordination_oru.util.Missions; public abstract class AbstractMotionPlanner { protected Logger metaCSPLogger = MetaCSPLogging.getLogger(this.getClass()); protected Pose start = null; protected Pose[] goal = null; protected Coordinate[] footprintCoords = null; protected boolean verifyPlanning = true; protected OccupancyMap om = null; protected boolean noMap = true; protected PoseSteering[] pathPS = null; public abstract AbstractMotionPlanner getCopy(); public void setFootprint(Coordinate ... coords) { this.footprintCoords = coords; } public void setStart(Pose p) { Pose newStart = new Pose(p.getX(),p.getY(),Missions.wrapAngle180b(p.getTheta())); //Pose newStart = new Pose(p.getX(),p.getY(),Missions.wrapAngle180a(p.getTheta())); //Pose newStart = new Pose(p.getX(),p.getY(),Missions.wrapAngle180(p.getTheta())); //Pose newStart = new Pose(p.getX(),p.getY(),Missions.wrapAngle360(p.getTheta())); this.start = newStart; } @Deprecated public void setGoal(Pose p) { this.setGoals(p); } public void addGoals(Pose ... p) { ArrayList<Pose> newGoals = new ArrayList<Pose>(); if (this.goal != null && this.goal.length > 0) { for (Pose g : this.goal) newGoals.add(g); } for (Pose ng : p) newGoals.add(ng); this.setGoals(newGoals.toArray(new Pose[newGoals.size()])); } public void setGoals(Pose ... p) { ArrayList<Pose> newGoals = new ArrayList<Pose>(); if (p != null) { Pose prev = null; for (Pose pose : p) { if (prev == null || prev != null && !prev.equals(pose)) newGoals.add(new Pose(pose.getX(),pose.getY(),Missions.wrapAngle180b(pose.getTheta()))); else metaCSPLogger.warning("Removing duplicated useless goal " + pose.toString() + "."); prev = pose; } } //for (Pose pose : p) newGoals.add(new Pose(pose.getX(),pose.getY(),Missions.wrapAngle180a(pose.getTheta()))); //for (Pose pose : p) newGoals.add(new Pose(pose.getX(),pose.getY(),Missions.wrapAngle180(pose.getTheta()))); //for (Pose pose : p) newGoals.add(new Pose(pose.getX(),pose.getY(),Missions.wrapAngle360(pose.getTheta()))); this.goal = newGoals.toArray(new Pose[newGoals.size()]); } public void setMap(String mapYAMLFile) { this.noMap = false; this.om = new OccupancyMap(mapYAMLFile); } public PoseSteering[] getPath() { return this.pathPS; } public PoseSteering[] getPathInv() { if (this.pathPS == null) return this.pathPS; ArrayList<PoseSteering> inv = new ArrayList<PoseSteering>(); for (PoseSteering ps : this.pathPS) inv.add(ps); Collections.reverse(inv); return inv.toArray(new PoseSteering[inv.size()]); } public synchronized void addObstacles(Geometry geom, Pose ... poses) { if (this.om == null) this.om = new OccupancyMap(1000, 1000, 1); this.om.addObstacles(geom, poses); } public synchronized void addObstacles(Geometry ... geom) { if (this.om == null) this.om = new OccupancyMap(1000, 1000, 1); this.om.addObstacles(geom); } public synchronized void writeDebugImage() { om.saveDebugObstacleImage(this.start, this.goal[this.goal.length-1], getFootprintAsGeometry()); } public synchronized void clearObstacles() { if (this.noMap) this.om = null; else this.om.clearObstacles(); } public OccupancyMap getOccupancyMap() { return this.om; } /** * This method needs to write the protected field PoseSteering[] pathPS. * @return <code>true</code> iff path planning was successful. */ public abstract boolean doPlanning(); private Geometry getFootprintAsGeometry() { GeometryFactory gf = new GeometryFactory(); Coordinate[] newFoot = new Coordinate[footprintCoords.length+1]; for (int j = 0; j < footprintCoords.length; j++) { newFoot[j] = footprintCoords[j]; } newFoot[footprintCoords.length] = footprintCoords[0]; Geometry foot = gf.createPolygon(newFoot); return foot; } public synchronized boolean plan() { Geometry goalFoot = getFootprintAsGeometry(); AffineTransformation at = new AffineTransformation(); at.rotate(this.goal[this.goal.length-1].getTheta()); at.translate(this.goal[this.goal.length-1].getX(), this.goal[this.goal.length-1].getY()); goalFoot = at.transform(goalFoot); if (this.om != null) { for (Geometry obs : this.om.getObstacles()) { if (obs.intersects(goalFoot)) { metaCSPLogger.info("Goal intersects with an obstacle, no path can exist"); return false; } } } boolean ret = doPlanning(); 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 = getFootprintAsGeometry(); at = new AffineTransformation(); at.rotate(p.getTheta()); at.translate(p.getX(), p.getY()); checkFoot = at.transform(checkFoot); if (this.om != null) { for (Geometry obs : this.om.getObstacles()) { if (obs.intersects(checkFoot)) { metaCSPLogger.info("Path verification failed"); return false; } } } } return ret; } public static boolean deleteDir(File dir) { if (dir.isDirectory()) { String[] children = dir.list(); for (int i=0; i<children.length; i++) { boolean success = deleteDir(new File(dir, children[i])); if (!success) { return false; } } } return dir.delete(); } public boolean isFree(Pose p) { AbstractMotionPlanner planner = this.getCopy(); planner.setStart(p); planner.setGoals(p); return planner.plan(); } }