public static void main(String[] args) throws Exception { VisibilityGraphsFrameworkTest test = new VisibilityGraphsFrameworkTest(); String prefix = "unitTestData/testable/"; test.setup(); test.runAssertionsOnDataset(dataset -> test.runAssertionsWithoutOcclusion(dataset), prefix + "20171218_205120_BodyPathPlannerEnvironment"); test.tearDown(); } }
private String fail(String datasetName, String message) { return assertTrue(datasetName, message, false); }
private String calculateAndTestVizGraphsBodyPath(String datasetName, Point3D start, Point3D goal, PlanarRegionsList planarRegionsList) { return calculateAndTestVizGraphsBodyPath(datasetName, start, goal, planarRegionsList, null); }
observer.set(walkerPosition); observer.addZ(0.8); Pair<PlanarRegionsList, List<Point3D>> result = createVisibleRegions(planarRegionsList, observer, knownRegions); errorMessages += calculateAndTestVizGraphsBodyPath(datasetName, walkerPosition, goal, visibleRegions, latestBodyPath); long endTime = System.currentTimeMillis(); if (endTime - startTime > maxSolveTimeInMilliseconds) errorMessages += fail(datasetName, "Took too long to compute a new body path."); return addPrefixToErrorMessages(datasetName, errorMessages); walkerPosition.set(travelAlongBodyPath(walkerSpeed, walkerPosition, latestBodyPath)); return addPrefixToErrorMessages(datasetName, errorMessages); // The ui has been closed return addPrefixToErrorMessages(datasetName, errorMessages);
private String calculateAndTestVizGraphsBodyPath(String datasetName, Point3D start, Point3D goal, PlanarRegionsList planarRegionsList, List<Point3DReadOnly> bodyPathToPack) VisibilityGraphsParameters parameters = createTestParameters(); NavigableRegionsManager manager = new NavigableRegionsManager(parameters); manager.setPlanarRegions(planarRegionsList.getPlanarRegionsAsList()); String errorMessages = basicBodyPathSanityChecks(datasetName, start, goal, path); walkerShape.setPosition(walkerBody3D); errorMessages += walkerCollisionChecks(datasetName, walkerShape, planarRegionsList, collisions);
private String runAssertionsWithoutOcclusion(VisibilityGraphsUnitTestDataset dataset) { String datasetName = dataset.getDatasetName(); PlanarRegionsList planarRegionsList = dataset.getPlanarRegionsList(); Point3D start = dataset.getStart(); Point3D goal = dataset.getGoal(); if (VISUALIZE) { messager.submitMessage(UIVisibilityGraphsTopics.PlanarRegionData, planarRegionsList); messager.submitMessage(UIVisibilityGraphsTopics.StartPosition, start); messager.submitMessage(UIVisibilityGraphsTopics.GoalPosition, goal); } String errorMessages = calculateAndTestVizGraphsBodyPath(datasetName, start, goal, planarRegionsList); return addPrefixToErrorMessages(datasetName, errorMessages); }
@Before public void setup() throws InterruptedException, Exception { VISUALIZE = VISUALIZE && !ContinuousIntegrationTools.isRunningOnContinuousIntegrationServer(); DEBUG = (VISUALIZE || (DEBUG && !ContinuousIntegrationTools.isRunningOnContinuousIntegrationServer())); if (VISUALIZE) { messager = new SharedMemoryJavaFXMessager(UIVisibilityGraphsTopics.API); messager.startMessager(); // Did not find a better solution for starting JavaFX and still be able to move on. new Thread(() -> launch()).start(); while (ui == null) ThreadTools.sleep(200); messager.submitMessage(UIVisibilityGraphsTopics.ShowBodyPath, showBodyPath); messager.submitMessage(UIVisibilityGraphsTopics.ShowClusterRawPoints, showClusterRawPoints); messager.submitMessage(UIVisibilityGraphsTopics.ShowClusterNavigableExtrusions, showClusterNavigableExtrusions); messager.submitMessage(UIVisibilityGraphsTopics.ShowClusterNonNavigableExtrusions, showClusterNonNavigableExtrusions); messager.submitMessage(UIVisibilityGraphsTopics.ShowNavigableRegionVisibilityMaps, showRegionInnerConnections); messager.submitMessage(UIVisibilityGraphsTopics.ShowInterRegionVisibilityMap, showRegionInterConnections); } }
errorMessages += fail(datasetName, "Body path is going through a region."); // TODO figure out the proper intersection break; Point3D intersectionWorld = new Point3D(intersectionLocal); planarRegion.transformFromLocalToWorld(intersectionWorld); errorMessages += fail(datasetName, "Body path is going through a region at: " + intersectionWorld); collisionsToPack.add(intersectionWorld);
return new Pair<>(combine(knownRegions, visible), rayImpactLocations);
private String basicBodyPathSanityChecks(String datasetName, Point3DReadOnly start, Point3DReadOnly goal, List<? extends Point3DReadOnly> path) { String errorMessages = ""; errorMessages += assertTrue(datasetName, "Path is null!", path != null); if (!errorMessages.isEmpty()) return errorMessages; // Cannot test anything else when no path is returned. errorMessages += assertTrue(datasetName, "Path does not contain any waypoints", path.size() > 0); if (!errorMessages.isEmpty()) return errorMessages; // Cannot test anything else when no path is returned. Point3DReadOnly pathEnd = path.get(path.size() - 1); Point3DReadOnly pathStart = path.get(0); errorMessages += assertTrue(datasetName, "Body path does not end at desired goal position: desired = " + goal + ", actual = " + pathEnd, pathEnd.geometricallyEquals(goal, START_GOAL_EPSILON)); errorMessages += assertTrue(datasetName, "Body path does not start from desired start position: desired = " + start + ", actual = " + pathStart, pathStart.geometricallyEquals(start, START_GOAL_EPSILON)); return errorMessages; }