/**
* The constructor.
*
* @param logFileName The experiment log file to analyze.
* @param samplingInterval The interval at which to calculate the heading error in milliseconds.
* @param saveToFile whether to save the error calculations to a file.
*/
public VisualizeHeadingError(String logFileName, long samplingInterval, boolean saveToFile) {
this.logFileName = logFileName;
robotData = new RobotExpData(logFileName);
long startTime = robotData.getExpStartTime();
// Calculate heading divergence every sampling interval
for (long time = startTime; time < robotData.getExpStopTime(); time += samplingInterval) {
// Only do the calculation if the robot has started to move.
if (time >= robotData.getPathEdge(0).getStartTime())
divergenceData.add(getHeadingError(time));
}
if (saveToFile) saveToFile();
showPlot();
}
private void saveToFile() {
String fileName;
if (logFileName.contains("."))
fileName = logFileName.substring(logFileName.lastIndexOf('.')) + "-headingError.txt";
else fileName = logFileName + "-headingError.txt";
FileLogger flogger = new FileLogger(fileName, false);
long startTime = robotData.getExpStartTime();
log("Time (ms)\tDelta Time (ms)\tActual Heading\tIdeal Heading\tHeading Error", flogger);
Enumeration<HeadingDivergenceState> e = divergenceData.elements();
while (e.hasMoreElements()) {
HeadingDivergenceState currState = e.nextElement();
log(
currState.time
+ "\t"
+ (currState.time - startTime)
+ "\t"
+ currState.currHeading
+ "\t"
+ currState.idealHeading
+ "\t"
+ currState.headingError,
flogger);
}
// Print when the robot starts traversing an edge...
log("Start Times of Edge Traversal:", flogger);
log("Time (ms)\tDelta Time (ms)\tDummy", flogger);
Vector<PathEdge> pathEdges = robotData.getPathEdges();
Enumeration<PathEdge> e2 = pathEdges.elements();
while (e2.hasMoreElements()) {
PathEdge currEdge = e2.nextElement();
long relStartTime = (currEdge.getStartTime() - robotData.getExpStartTime()) / 1000;
log(currEdge.getStartTime() + "\t" + relStartTime + "\t" + 0, flogger);
}
}
/** Displays the data in a graph. */
private void showPlot() {
// Create a data series containing the heading error data...
XYSeries headingErrorSeries = new XYSeries("Heading Error");
Enumeration<HeadingDivergenceState> e = divergenceData.elements();
while (e.hasMoreElements()) {
HeadingDivergenceState currState = e.nextElement();
headingErrorSeries.add(
(currState.time - robotData.getExpStartTime()) / 1000, currState.headingError);
}
// Create two data series one containing the times when the robot starts heading
// towards a waypoint, and another containing the times when the robot arrives at
// a waypoint
final XYSeries beginEdgeSeries = new XYSeries("Begin Edge Traveral");
final XYSeries waypointArrivalSeries = new XYSeries("Waypoint Arrival");
Vector<PathEdge> pathEdges = robotData.getPathEdges();
Enumeration<PathEdge> e2 = pathEdges.elements();
while (e2.hasMoreElements()) {
PathEdge currEdge = e2.nextElement();
double beginEdgeTime = (currEdge.getStartTime() - robotData.getExpStartTime()) / 1000.0;
beginEdgeSeries.add(beginEdgeTime, 0);
double wayPointArrivalTime = (currEdge.getEndTime() - robotData.getExpStartTime()) / 1000.0;
waypointArrivalSeries.add(wayPointArrivalTime, 0);
}
// Create a dataset out of the data series
XYSeriesCollection dataset = new XYSeriesCollection();
dataset.addSeries(headingErrorSeries);
dataset.addSeries(beginEdgeSeries);
dataset.addSeries(waypointArrivalSeries);
// Create the chart
JFreeChart chart =
ChartFactory.createXYLineChart(
"Heading Error vs. Time", // chart title
"Time (s)", // x axis label
"Heading Error (radians)", // y axis label
dataset, // the data
PlotOrientation.VERTICAL, // plot orientation (y axis is vertical)
true, // include legend
true, // tooltips
false // urls
);
// Place the legend on top of the chart just below the title.
LegendTitle legend = chart.getLegend();
legend.setPosition(RectangleEdge.TOP);
chart.setBackgroundPaint(Color.white);
XYPlot plot = chart.getXYPlot();
// plot.setBackgroundPaint(Color.lightGray);
// // plot.setAxisOffset(new Spacer(Spacer.ABSOLUTE, 5.0, 5.0, 5.0, 5.0));
// plot.setDomainGridlinePaint(Color.white);
// plot.setRangeGridlinePaint(Color.white);
// Display the points and not the lines connecting the points
XYLineAndShapeRenderer renderer = new XYLineAndShapeRenderer();
renderer.setSeriesLinesVisible(0, true); // display the heading errors as a line
renderer.setSeriesShapesVisible(0, false);
renderer.setSeriesLinesVisible(
1, false); // display the begin edge traversal points as blue dots
renderer.setSeriesShapesVisible(1, true);
renderer.setSeriesPaint(1, Color.BLUE);
renderer.setSeriesShape(1, new java.awt.geom.Ellipse2D.Double(-3, -3, 6, 6));
renderer.setSeriesLinesVisible(
2, false); // display the begin edge traversal points as green dots
renderer.setSeriesShapesVisible(2, true);
renderer.setSeriesPaint(2, Color.GREEN.darker());
renderer.setSeriesShape(2, new java.awt.geom.Ellipse2D.Double(-5, -5, 10, 10));
plot.setRenderer(renderer);
// final NumberAxis domainAxis = (NumberAxis)plot.getDomainAxis();
// domainAxis.setRange(new Range(0,140));
// final NumberAxis rangeAxis = (NumberAxis) plot.getRangeAxis();
// // change the auto tick unit selection to integer units only...
//// rangeAxis.setStandardTickUnits(NumberAxis.createIntegerTickUnits());
// rangeAxis.setRange(new Range(-Math.PI, Math.PI));
ChartPanel chartPanel = new ChartPanel(chart);
chartPanel.setPreferredSize(new java.awt.Dimension(1000, 600));
// Create a frame for the chart, then display it.
ApplicationFrame appFrame = new ApplicationFrame("Heading Error for " + logFileName);
appFrame.setContentPane(chartPanel);
appFrame.pack();
RefineryUtilities.centerFrameOnScreen(appFrame);
appFrame.setVisible(true);
}
/**
* Calculates the heading error of the robot at the specified time.
*
* @param time The time to consider.
* @return The heading error of the robot.
*/
private HeadingDivergenceState getHeadingError(long time) {
double currHeading = robotData.getHeading(time);
double idealHeading = robotData.getIdealHeading(time);
double headingError = pharoslabut.navigate.Navigate.headingError(currHeading, idealHeading);
return new HeadingDivergenceState(time, currHeading, idealHeading, headingError);
}