public DirectedSparseMultigraph<TrajectoryEnvelope,Integer[]> getCurrentDependencies() {
		DirectedSparseMultigraph<TrajectoryEnvelope,Integer[]> depGraph = new DirectedSparseMultigraph<TrajectoryEnvelope,Integer[]>();
		ConstraintNetwork cn = this.getConstraintSolvers()[0].getConstraintNetwork();
		Constraint[] cons = cn.getConstraints();
		for (Constraint con : cons) {
			if (con instanceof AllenIntervalConstraint) {
				AllenIntervalConstraint aic = (AllenIntervalConstraint)con;
				if (aic.getTypes()[0].equals(AllenIntervalConstraint.Type.BeforeOrMeets)) {

					//The two TEs involved in the constraint
					TrajectoryEnvelope mustWaitToStart = (TrajectoryEnvelope)aic.getTo();
					TrajectoryEnvelope mustFinishBeforeOtherCanStart = (TrajectoryEnvelope)aic.getFrom();
					TrajectoryEnvelope waitingEnvelope = null;

					//Find waitingEnvelope = previous of mustWaitToStart
					TrajectoryEnvelope root = mustWaitToStart;
					while (root.hasSuperEnvelope()) root = root.getSuperEnvelope();
					for (Variable depVar : root.getRecursivelyDependentVariables()) {
						TrajectoryEnvelope depTE = (TrajectoryEnvelope)depVar;
						if (!depTE.hasSubEnvelopes() && depTE.getTrajectory().getSequenceNumberEnd() == mustWaitToStart.getSequenceNumberStart()-1) {
							waitingEnvelope = depTE;
							break;
						}
					}

					//Calculate waiting points
					Integer thresholdPoint = mustFinishBeforeOtherCanStart.getTrajectory().getSequenceNumberEnd();
					Integer waitingPoint = null;
					
					//If there was no previous envelope, then make the robot stay in the start point of this one
					if (waitingEnvelope == null) {
						//System.out.println("IGNORE THIS DEPENDENCY, the following robot is parked anyway " + mustWaitToStart);
						waitingPoint = mustWaitToStart.getTrajectory().getSequenceNumberStart();
						waitingEnvelope = mustWaitToStart;
					}
					else {
						waitingPoint = waitingEnvelope.getTrajectory().getSequenceNumberEnd();
					}
					
					//Add edge in dep graph
					ArrayList<TrajectoryEnvelope> verts = new ArrayList<TrajectoryEnvelope>();
					verts.add(waitingEnvelope);
					verts.add(mustFinishBeforeOtherCanStart);
					depGraph.addVertex(waitingEnvelope);
					depGraph.addVertex(mustFinishBeforeOtherCanStart);
					depGraph.addEdge(new Integer[] {waitingPoint,thresholdPoint}, waitingEnvelope, mustFinishBeforeOtherCanStart);
				}
			}
		}
		return depGraph;
	}
public DirectedSparseMultigraph < TrajectoryEnvelope , Integer [ ] > getCurrentDependencies ( ) { DirectedSparseMultigraph < TrajectoryEnvelope , Integer [ ] > depGraph = new DirectedSparseMultigraph < TrajectoryEnvelope , Integer [ ] > ( ) ; ConstraintNetwork cn = this . getConstraintSolvers ( ) [ 0 ] . getConstraintNetwork ( ) ; Constraint [ ] cons = cn . getConstraints ( ) ; for ( Constraint con : cons ) { if ( con instanceof AllenIntervalConstraint ) { AllenIntervalConstraint aic = ( AllenIntervalConstraint ) con ; if ( aic . getTypes ( ) [ 0 ] . equals ( AllenIntervalConstraint . Type . BeforeOrMeets ) ) { TrajectoryEnvelope mustWaitToStart = ( TrajectoryEnvelope ) aic . getTo ( ) ; TrajectoryEnvelope mustFinishBeforeOtherCanStart = ( TrajectoryEnvelope ) aic . getFrom ( ) ; TrajectoryEnvelope waitingEnvelope = null ; TrajectoryEnvelope root = mustWaitToStart ; while ( root . hasSuperEnvelope ( ) ) root = root . getSuperEnvelope ( ) ; for ( Variable depVar : root . getRecursivelyDependentVariables ( ) ) { TrajectoryEnvelope depTE = ( TrajectoryEnvelope ) depVar ; if ( ! depTE . hasSubEnvelopes ( ) && depTE . getTrajectory ( ) . getSequenceNumberEnd ( ) == mustWaitToStart . getSequenceNumberStart ( ) - 1 ) { waitingEnvelope = depTE ; break ; } } Integer thresholdPoint = mustFinishBeforeOtherCanStart . getTrajectory ( ) . getSequenceNumberEnd ( ) ; Integer waitingPoint = null ; if ( waitingEnvelope == null ) { waitingPoint = mustWaitToStart . getTrajectory ( ) . getSequenceNumberStart ( ) ; waitingEnvelope = mustWaitToStart ; } else { waitingPoint = waitingEnvelope . getTrajectory ( ) . getSequenceNumberEnd ( ) ; } ArrayList < TrajectoryEnvelope > verts = new ArrayList < TrajectoryEnvelope > ( ) ; verts . add ( waitingEnvelope ) ; verts . add ( mustFinishBeforeOtherCanStart ) ; depGraph . addVertex ( waitingEnvelope ) ; depGraph . addVertex ( mustFinishBeforeOtherCanStart ) ; depGraph . addEdge ( new Integer [ ] { waitingPoint , thresholdPoint } , waitingEnvelope , mustFinishBeforeOtherCanStart ) ; } } } return depGraph ; }
Get the dependency graph of trajectory envelopes from the resolving constraints added by this
{@link TrajectoryEnvelopeScheduler}.
@return A directed graph where each edge (x,y) has a label {p1,p2} indicating that
robot x.robotID has to wait at path point p1 for robot y.robotID to reach path point p2.