Fixed CN serialization, added loading and saving; added addTrajectory…

…Envelopes(ConstraintNetwork con) to TrajectoryEnvelopeAnimator; added polygon fixing in PolygonalDomain.

src/main/java/org/metacsp/examples/meta/TestTrajectoryEnvelopeLoadingFromFile.java

@@ -0,0 +1,13 @@
+package org.metacsp.examples.meta;
+import org.metacsp.framework.ConstraintNetwork;
+import org.metacsp.utility.UI.TrajectoryEnvelopeAnimator;
+
+public class TestTrajectoryEnvelopeLoadingFromFile {
+
+	public static void main(String[] args) {		
+		ConstraintNetwork con = ConstraintNetwork.loadConstraintNetwork("savedConstraintNetworks/example.cn");
+		TrajectoryEnvelopeAnimator tea = new TrajectoryEnvelopeAnimator("Trajectory Envelopes loaded from file");
+		tea.addTrajectoryEnvelopes(con);
+	}
+
+}

src/main/java/org/metacsp/examples/meta/TestTrajectoryEnvelopeSavingToFile.java

@@ -0,0 +1,60 @@
+package org.metacsp.examples.meta;
+import java.util.Arrays;
+
+import org.metacsp.framework.ConstraintNetwork;
+import org.metacsp.framework.Variable;
+import org.metacsp.meta.spatioTemporal.paths.Map;
+import org.metacsp.meta.spatioTemporal.paths.TrajectoryEnvelopeScheduler;
+import org.metacsp.multi.spatioTemporal.paths.Trajectory;
+import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelope;
+import org.metacsp.multi.spatioTemporal.paths.TrajectoryEnvelopeSolver;
+import org.metacsp.utility.UI.TrajectoryEnvelopeAnimator;
+
+import com.vividsolutions.jts.geom.Coordinate;
+
+public class TestTrajectoryEnvelopeSavingToFile {
+
+	public static void main(String[] args) {
+
+		TrajectoryEnvelopeScheduler metaSolver = new TrajectoryEnvelopeScheduler(0, 100000);
+		TrajectoryEnvelopeSolver solver = (TrajectoryEnvelopeSolver)metaSolver.getConstraintSolvers()[0];
+		Variable[] vars = solver.createVariables(2);
+		TrajectoryEnvelope var0 = (TrajectoryEnvelope)vars[0];
+		TrajectoryEnvelope var1 = (TrajectoryEnvelope)vars[1];
+
+		Coordinate frontLeft = new Coordinate(1.8, 0.7);
+		Coordinate frontRight = new Coordinate(1.8, -0.7);
+		Coordinate backRight = new Coordinate(-1.8, -0.7);
+		Coordinate backLeft = new Coordinate(-1.8, 0.7);
+
+		Trajectory traj0 = new Trajectory("paths/path1.path");
+		var0.setFootprint(backLeft,backRight,frontLeft,frontRight);
+		var0.setTrajectory(traj0);
+
+		Trajectory traj1 = new Trajectory("paths/path3.path");
+		var1.setFootprint(backLeft,backRight,frontLeft,frontRight);
+		var1.setTrajectory(traj1);
+
+		var0.setRobotID(1);
+		var1.setRobotID(2);
+
+		System.out.println(var0 + " has domain " + var0.getDomain());
+		System.out.println(var1 + " has domain " + var1.getDomain());
+
+		Map map = new Map(null, null);		
+		metaSolver.addMetaConstraint(map);
+
+		ConstraintNetwork refined1 = metaSolver.refineTrajectoryEnvelopes();
+		System.out.println("REFINED: "+  refined1);
+
+		boolean solved = metaSolver.backtrack();
+		System.out.println("Solved? " + solved);
+		if (solved) System.out.println("Added resolvers:\n" + Arrays.toString(metaSolver.getAddedResolvers()));
+
+		ConstraintNetwork.saveConstraintNetwork(solver.getConstraintNetwork(),"savedConstraintNetworks/example.cn");
+		TrajectoryEnvelopeAnimator tea = new TrajectoryEnvelopeAnimator("Trajectory Envelopes - after solving");
+		tea.addTrajectoryEnvelopes(solver.getConstraintNetwork());
+
+	}
+
+}

src/main/java/org/metacsp/framework/ConstraintNetwork.java

@@ -1,6 +1,9 @@
 package org.metacsp.framework;
 
 import java.awt.EventQueue;
+import java.io.FileInputStream;
+import java.io.FileNotFoundException;
+import java.io.FileOutputStream;
 import java.io.IOException;
 import java.io.ObjectInputStream;
 import java.io.ObjectOutputStream;
@@ -68,6 +71,8 @@ public void run() {
 	//end for changelistener
 
 	public static HashMap<FieldOfObject,Object> backupForSerialization = new HashMap<FieldOfObject,Object>();
+	//This will back up the domain valuechoice functions for serialization
+	private HashMap<Class<?>,HashMap<String,ValueChoiceFunction>> domainValueChoiceFunctions = new HashMap<Class<?>, HashMap<String,ValueChoiceFunction>>();
 
 	private class FieldOfObject {
 		private Field field;
@@ -729,6 +734,7 @@ Variable[] getNativeVariables(){
 	}
 
 	private void writeObject(ObjectOutputStream out) throws IOException {
+		this.domainValueChoiceFunctions = Domain.valueChoiceFunctions;
 		out.defaultWriteObject();
 		for (Field f : ConstraintNetwork.class.getDeclaredFields()) {
 			if (Modifier.isTransient(f.getModifiers())) {
@@ -749,6 +755,7 @@ private void readObject(ObjectInputStream in) throws IOException, ClassNotFoundE
 				catch (IllegalAccessException e) { e.printStackTrace(); }
 			}
 		}
+		Domain.valueChoiceFunctions = this.domainValueChoiceFunctions;
 	}
 
 	/**
@@ -827,5 +834,28 @@ public Constraint[] getMaskedConstraints() {
 		}
 		return ret.toArray(new Constraint[ret.size()]);
 	}
+
+	public static void saveConstraintNetwork(ConstraintNetwork cn, String filename) {
+		try {
+			FileOutputStream out = new FileOutputStream(filename);
+			ObjectOutputStream oos = new ObjectOutputStream(out);
+			oos.writeObject(cn);
+		}
+		catch (FileNotFoundException e) { e.printStackTrace(); }
+		catch (IOException e) { e.printStackTrace(); }		
+	}
+
+	public static ConstraintNetwork loadConstraintNetwork(String filename) {
+		try {
+			FileInputStream in = new FileInputStream(filename);
+			ObjectInputStream ois = new ObjectInputStream(in);
+			ConstraintNetwork con = (ConstraintNetwork)ois.readObject();
+			return con;
+		}
+		catch (FileNotFoundException e) { e.printStackTrace(); }
+		catch (IOException e) { e.printStackTrace(); }
+		catch (ClassNotFoundException e) { e.printStackTrace(); }
+		return null;
+	}
 
 }

src/main/java/org/metacsp/framework/Domain.java

@@ -38,7 +38,7 @@ public abstract class Domain implements Comparable<Object>, Serializable {
 
 	private static final long serialVersionUID = -1526153338750435200L;
 
-	private static HashMap<Class<?>,HashMap<String,ValueChoiceFunction>> valueChoiceFunctions = new HashMap<Class<?>, HashMap<String,ValueChoiceFunction>>();
+	public static HashMap<Class<?>,HashMap<String,ValueChoiceFunction>> valueChoiceFunctions = new HashMap<Class<?>, HashMap<String,ValueChoiceFunction>>();
 
 	protected Variable myVariable;
 

src/main/java/org/metacsp/framework/ValueChoiceFunction.java

@@ -22,18 +22,22 @@
  ******************************************************************************/
 package org.metacsp.framework;
 
+import java.io.Serializable;
+
 /**
  * Basic abstract class for defining {@link ValueChoiceFunction}s of {@link Variable} {@link Domain}s.
  * @author Federico Pecora
  *
  */
-public interface ValueChoiceFunction {
+public abstract class ValueChoiceFunction implements Serializable {
 
+	private static final long serialVersionUID = 658591850210595281L;
+
 	/**
 	 * Method to be implemented by the {@link Domain} developer to choose a value. 
 	 * @param dom The domain from which to choose a value.
 	 * @return A value chosen from the given {@link Domain} according to this {@link ValueChoiceFunction}.
 	 */
-	abstract Object getValue(Domain dom);
+	public abstract Object getValue(Domain dom);
 
 }

src/main/java/org/metacsp/framework/meta/MetaConstraint.java

@@ -59,7 +59,7 @@ public abstract class MetaConstraint extends Constraint {
 	protected ValueOrderingH valOH;
 	protected MetaConstraintSolver metaCS = null;
 	protected boolean independentMC = false;
-	protected Logger logger = MetaCSPLogging.getLogger(this.getClass());
+	protected transient Logger logger = MetaCSPLogging.getLogger(this.getClass());
 
 	/**
 	 * Creates a {@link MetaConstraint} with given variable and value ordering heuristics (one or both of these can be <code>null</code>).

src/main/java/org/metacsp/multi/spatial/DE9IM/PolygonalDomain.java

@@ -1,13 +1,17 @@
 package org.metacsp.multi.spatial.DE9IM;
 
+import java.util.logging.Logger;
+
 import org.metacsp.framework.Variable;
+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.LinearRing;
 import com.vividsolutions.jts.geom.MultiPolygon;
 import com.vividsolutions.jts.geom.Point;
+import com.vividsolutions.jts.geom.TopologyException;
 
 /**
  * Represents polygonal domains for {@link GeometricShapeVariable}s.
@@ -18,6 +22,7 @@
 public class PolygonalDomain extends GeometricShapeDomain {
 
 	private static final long serialVersionUID = 1543675650668270396L;
+	private transient Logger metaCSPLogger = MetaCSPLogging.getLogger(this.getClass());
 
 	protected PolygonalDomain(Variable v) {
 		super(v);
@@ -56,7 +61,17 @@ protected void updateGeometry() {
 			newCoords[coordinates.length] = this.coordinates[0];
 			this.geom = new GeometryFactory().createPolygon(newCoords);						
 			if (!this.geom.isValid()) {
-				this.geom = this.geom.symDifference(this.geom.getBoundary());
+				try { 
+					this.geom = this.geom.symDifference(this.geom.getBoundary());
+				}
+				catch(TopologyException e) {
+					metaCSPLogger.info("Trying to fix GeometricShapeVariable " + this.getVariable().getID());
+					this.geom = this.geom.buffer(0.1);
+					if (!this.geom.isValid()) {
+						metaCSPLogger.severe("... giving up!");
+						throw e;
+					}
+				}
 			}
 		}
 	}

src/main/java/org/metacsp/multi/spatioTemporal/paths/TrajectoryEnvelope.java

@@ -1,5 +1,7 @@
 package org.metacsp.multi.spatioTemporal.paths;
 
+import java.math.RoundingMode;
+import java.text.DecimalFormat;
 import java.util.ArrayList;
 import java.util.Collections;
 import java.util.Comparator;
@@ -74,6 +76,20 @@ public Polygon getFootprint() {
 		return footprint;
 	}
 
+	public void printInfo() {
+
+		double[] teDTs = this.getTrajectory().getDTs();
+		double[] teCTs = this.getCTs();
+
+		DecimalFormat df = new DecimalFormat("#0.00");
+		df.setRoundingMode(RoundingMode.HALF_DOWN);
+		System.out.println("------------------------------------------\n" + this + "\nGround env: " + this.getGroundEnvelopes()  + "\nDTs and CTs\n------------------------------------------");
+		for (int i = 0; i < teDTs.length; i++) {
+			System.out.println(i + ": " + df.format(teDTs[i]) + " \t " + df.format(teCTs[i]));
+		}
+
+	}
+
 	public void setFootprint(Polygon footprint) {
 		this.footprint = footprint;
 	}

src/main/java/org/metacsp/time/APSPSolver.java

@@ -61,7 +61,7 @@ public class APSPSolver extends ConstraintSolver {
 	 */
 	private static final long serialVersionUID = -5029122662268797937L;
 
-	transient private Logger logger = MetaCSPLogging.getLogger(this.getClass());
+	private transient Logger logger = MetaCSPLogging.getLogger(this.getClass());
 
 	private boolean doFromScratchInsteadOfIncremental = false;
 	private boolean addingIndependentConstraints = false;

src/main/java/org/metacsp/utility/UI/TrajectoryEnvelopeAnimator.java

@@ -34,6 +34,8 @@
 import javax.swing.event.ChangeEvent;
 import javax.swing.event.ChangeListener;
 
+import org.metacsp.framework.ConstraintNetwork;
+import org.metacsp.framework.Variable;
 import org.metacsp.multi.spatial.DE9IM.GeometricShapeDomain;
 import org.metacsp.multi.spatial.DE9IM.PointDomain;
 import org.metacsp.multi.spatioTemporal.paths.Pose;
@@ -214,6 +216,19 @@ public void addTrajectoryEnvelopes(TrajectoryEnvelope ... te) {
 		updateTime();
 	}
 
+	public void addTrajectoryEnvelopes(ConstraintNetwork con) {
+		for (Variable v : con.getVariables()) {
+			if (v instanceof TrajectoryEnvelope) {
+				TrajectoryEnvelope te = (TrajectoryEnvelope)v;
+				if (!te.hasSubEnvelopes()) {
+					this.tes.add(te);
+				}
+			}
+		}
+		updateBounds();
+		updateTime();
+	}
+
 	public void addMarkers(String[] ids, Pose[] poses) {
 		for (int i = 0; i < poses.length; i++) {
 			this.markers.put(ids[i], poses[i]);