
	 * between two path indices. The estimate is based on this {@link Trajectory}'s
	 * temporal profile.
	 * @param sequenceNumNow The path index from which the estimate should be computed.
	 * @param sequenceNumTo The path index to which the estimate should be computed.
	 * @return An estimate of the time left to move on this {@link Trajectory}
	 * given the current path index.
	 */
	public double getTimeToEstimate(int sequenceNumNow, int sequenceNumTo) {
		double timeCounter = 0.0;
		for (int i = sequenceNumNow-this.sequenceNumberStart; i <= sequenceNumTo; i++) {
			timeCounter += this.getDTs()[i];
		}
		return timeCounter;
	}
	
	/**
	 * Get an estimate of the time left to move on this {@link Trajectory}
	 * given the current path index. The estimate is based on this {@link Trajectory}'s
	 * temporal profile.
	 * @param sequenceNum The path index from which the estimate should be computed.
	 * @return An estimate of the time left to move on this {@link Trajectory}
	 * given the current path index.
	 */
	public double getTimeLeftEstimate(int sequenceNum) {
		double timeCounter = 0.0;
		for (int i = sequenceNum-this.sequenceNumberStart; i < this.getDTs().length; i++) {
			timeCounter += this.getDTs()[i];
		}
		return timeCounter;
	}
	
//	public double getTimeLeftEstimate(int sequenceNum, int endIndex) {
//		double timeCounter = 0.0;
//		for (int i = sequenceNum; i < endIndex; i++) {
//			timeCounter += this.getDTs()[i];
//		}
//		return timeCounter;
//	}

	private void updateDts() {
		double dt = 0.1;
		ArrayList<Double> u = new ArrayList<Double>();
		ArrayList<Double> s = new ArrayList<Double>();
		//u = u + a * dt;
		//s = s + u * dt;
		s.add(0.0);
		u.add(0.0);
		double totDistance = 0.0;
		for (int i = 1; i < dts.length; i++) {
			Coordinate prev = getPositions()[i-1];
			Coordinate current = getPositions()[i];
			totDistance += prev.distance(current);
		}
		while (s.get(s.size()-1) < totDistance) {
			double percentComplete = 0.01;
			double newPercent = s.get(s.size()-1)/totDistance;
			if (newPercent > percentComplete) percentComplete = newPercent;
//			System.out.println(percentComplete*100 + "% --> " + getAcceleration(percentComplete));
			u.add(u.get(u.size()-1)+getAcceleration(percentComplete)*dt);
			s.add(s.get(s.size()-1)+u.get(u.size()-1)*dt);
		}
		totDistance = 0.0;
		dts[0] = 0.0;
		double prevSum = 0.0;
		for (int i = 1; i < dts.length; i++) {
			Coordinate prev = getPositions()[i-1];
			Coordinate current = getPositions()[i];
			totDistance += prev.distance(current);
			int countDts = 0;
			for (int j = 0; j < s.size(); j++) {
				if (s.get(j) > totDistance) {
					countDts = j-1;
					break;
				}
			}
			prevSum += dts[i-1];
			dts[i] = Math.max(countDts*dt-prevSum,0.001);
		}
	}
	
	/**
	 * Set the minimum transition times between path poses
	 * of this {@link Trajectory}. This is essentially the fastest
	 * time profile of this {@link Trajectory}.
	 * @param dts A vector containing the minimum transition times between path poses
	 * of this {@link Trajectory}. Values are assumed to be in seconds. 
	 */
	public void setDTs(double[] dts) {
		this.dts = dts;
	}
	
//	public double getMinTraversalTime() {
//		return dts[dts.length-1];
//	}

	/**
	 * Get the path (list of {@link PoseSteering}s) of this {@link Trajectory}.
	 * @return The path (list of {@link PoseSteering}s) of this {@link Trajectory}.
	 */
	public PoseSteering[] getPoseSteering() {
		return psa;
	}
	
	/**
	 * Get the path (list of {@link Pose}s) of this {@link Trajectory}.
	 * @return The path (list of {@link Pose}s) of this {@link Trajectory}.
	 */
	public Pose[] getPose() {
		Pose[] ret = new Pose[psa.length];
		for (int i = 0; i < ret.length; i++) {
			ret[i] = psa[i].getPose();
		}
		return ret;
	}
	
	/**
	 * Get the list of positions in this {@link Trajectory}'s path.
	 * @return The list of positions in this {@link Trajectory}'s path.
	 */
	public Coordinate[] getPositions() {
		ArrayList<Coordinate> ret = new ArrayList<Coordinate>();
		for (PoseSteering ps : psa) {
			ret.add(ps.getPose().getPosition());
		}
		return ret.toArray(new Coordinate[ret.size()]);
	}
	
	private PoseSteering[] readPath(String fileName) {
		ArrayList<PoseSteering> ret = new ArrayList<PoseSteering>();
		try {
			Scanner in = new Scanner(new FileReader(fileName));
			while (in.hasNextLine()) {
				String line = in.nextLine().trim();
				if (line.length()!= 0) {
					String[] oneline = line.split(" ");
					PoseSteering ps = null;
					//x,y,z,r,p,y,s
					if (oneline.length == 7) {
					ps = new PoseSteering(
							new Double(oneline[0]).doubleValue(),
							new Double(oneline[1]).doubleValue(),
							new Double(oneline[2]).doubleValue(),
							new Double(oneline[3]).doubleValue(),
							new Double(oneline[4]).doubleValue(),
							new Double(oneline[5]).doubleValue(),
							new Double(oneline[6]).doubleValue());
					}
					//x,y,z,r,p,y
					else if (oneline.length == 6) {
					ps = new PoseSteering(
							new Double(oneline[0]).doubleValue(),
							new Double(oneline[1]).doubleValue(),
							new Double(oneline[2]).doubleValue(),
							new Double(oneline[3]).doubleValue(),
							new Double(oneline[4]).doubleValue(),
							new Double(oneline[5]).doubleValue(),
							0.0);
					}
					//x,y,z,th,s
					else if (oneline.length == 4) {
						ps = new PoseSteering(
								new Double(oneline[0]).doubleValue(),
								new Double(oneline[1]).doubleValue(),
								new Double(oneline[2]).doubleValue(),
								new Double(oneline[3]).doubleValue());
					}
					//x,y,z,th
					else {
						ps = new PoseSteering(
								new Double(oneline[0]).doubleValue(),
								new Double(oneline[1]).doubleValue(),
								new Double(oneline[2]).doubleValue(),
								0.0);					
					}
					ret.add(ps);
				}
			}
			in.close();
		}
		catch (FileNotFoundException e) { e.printStackTrace(); }
		return ret.toArray(new PoseSteering[ret.size()]);
	}
		
}
<commit_msg>Added path length getter in Trajecotry<commit_after>package org.metacsp.multi.spatioTemporal.paths;

import java.io.FileNotFoundException;
import java.io.FileReader;
import java.io.Serializable