public static void assertVisibilityGraphStatisticsEqual(VisibilityGraphStatistics expected, VisibilityGraphStatistics actual, double epsilon) { assertEquals(expected.getGoalMapId(), actual.getGoalMapId()); assertEquals(expected.getStartMapId(), actual.getStartMapId()); assertEquals(expected.getInterRegionsMapId(), actual.getInterRegionsMapId()); assertVisibilityMapsEqual(expected.getGoalVisibilityMap(), actual.getGoalVisibilityMap(), epsilon); assertVisibilityMapsEqual(expected.getStartVisibilityMap(), actual.getStartVisibilityMap(), epsilon); assertVisibilityMapsEqual(expected.getInterRegionsVisibilityMap(), actual.getInterRegionsVisibilityMap(), epsilon); assertEquals(expected.getNumberOfNavigableRegions(), actual.getNumberOfNavigableRegions()); for (int i = 0; i < expected.getNumberOfNavigableRegions(); i++) assertNavigableRegionsEqual(expected.getNavigableRegion(i), actual.getNavigableRegion(i), epsilon); }
public void addNavigableRegions(List<NavigableRegion> navigableRegions) { for (int i = 0; i < navigableRegions.size(); i++) addNavigableRegion(navigableRegions.get(i)); }
@Override public void set(VisibilityGraphStatistics other) { setStartVisibilityMapInWorld(other.startMapId, other.startMap); setGoalVisibilityMapInWorld(other.goalMapId, other.goalMap); setInterRegionsVisibilityMapInWorld(other.interRegionsMapId, other.interRegionsMap); setNavigableRegions(other.navigableRegions); }
else statistics = new VisibilityGraphStatistics(); mapToPopulate.put(StatisticsType.VISIBILITY_GRAPH, statistics); statistics.setStartMapId(incomingStatistics.getStartMapId()); if (segmentId == globalStepSequenceIndex.getIntegerValue()) statistics.setGoalMapId(incomingStatistics.getGoalMapId()); statistics.getStartVisibilityMap().addConnections(incomingStatistics.getStartVisibilityMap().getConnections()); statistics.getGoalVisibilityMap().addConnections(incomingStatistics.getGoalVisibilityMap().getConnections()); statistics.getInterRegionsVisibilityMap().addConnections(incomingStatistics.getInterRegionsVisibilityMap().getConnections()); for (int i = 0; i < incomingStatistics.getNumberOfNavigableRegions(); i++) statistics.addNavigableRegion(incomingStatistics.getNavigableRegion(i));
public static VisibilityGraphStatistics convertToVisibilityGraphStatistics(BodyPathPlanStatisticsMessage message) { VisibilityGraphStatistics statistics = new VisibilityGraphStatistics(); VisibilityMapHolder startMap = convertToSingleSourceVisibilityMap(message.getStartVisibilityMap()); VisibilityMapHolder goalMap = convertToSingleSourceVisibilityMap(message.getGoalVisibilityMap()); VisibilityMapHolder interRegionsMap = convertToInterRegionsVisibilityMap(message.getInterRegionsMap()); statistics.setStartVisibilityMapInWorld(startMap.getMapId(), startMap.getVisibilityMapInWorld()); statistics.setGoalVisibilityMapInWorld(goalMap.getMapId(), goalMap.getVisibilityMapInWorld()); statistics.setInterRegionsVisibilityMapInWorld(interRegionsMap.getMapId(), interRegionsMap.getVisibilityMapInWorld()); List<NavigableRegionMessage> navigableRegions = message.getNavigableRegions(); for (int i = 0; i < navigableRegions.size(); i++) statistics.addNavigableRegion(convertToNavigableRegion(navigableRegions.get(i))); return statistics; } }
interRegionVisibilityMap.addConnections(statistics.getInterRegionsVisibilityMap().getConnections()); for (int i = 0; i < statistics.getNumberOfNavigableRegions(); i++) navigableRegionList.add(statistics.getNavigableRegion(i));
private void packVisibilityGraphStatistics(VisibilityGraphStatistics statistics) { VisibilityMapHolder startMap = navigableRegionsManager.getStartMap(); VisibilityMapHolder goalMap = navigableRegionsManager.getGoalMap(); VisibilityMapHolder interRegionsMap = navigableRegionsManager.getInterRegionConnections(); List<NavigableRegion> navigableRegions = navigableRegionsManager.getNavigableRegions(); statistics.setStartVisibilityMapInWorld(startMap.getMapId(), startMap.getVisibilityMapInWorld()); statistics.setGoalVisibilityMapInWorld(goalMap.getMapId(), goalMap.getVisibilityMapInWorld()); statistics.setInterRegionsVisibilityMapInWorld(interRegionsMap.getMapId(), interRegionsMap.getVisibilityMapInWorld()); statistics.addNavigableRegions(navigableRegions); } }
public VisibilityMap getVisibilityMapInWorld() { return statistics.getGoalVisibilityMap(); } };
public VisibilityMap getVisibilityMapInWorld() { return statistics.getStartVisibilityMap(); } };
public int getMapId() { return statistics.getStartMapId(); }
public int getMapId() { return statistics.getGoalMapId(); }
public VisibilityMap getVisibilityMapInLocal() { return statistics.getGoalVisibilityMap(); }
public VisibilityMap getVisibilityMapInLocal() { return statistics.getStartVisibilityMap(); }
public static BodyPathPlanStatisticsMessage convertToBodyPathPlanStatisticsMessage(int planId, VisibilityGraphStatistics statistics) { BodyPathPlanStatisticsMessage message = new BodyPathPlanStatisticsMessage(); message.setPlanId(planId); message.getGoalVisibilityMap().set(convertToVisibilityMapMessage(statistics.getGoalMapId(), statistics.getGoalVisibilityMap())); message.getStartVisibilityMap().set(convertToVisibilityMapMessage(statistics.getStartMapId(), statistics.getStartVisibilityMap())); message.getInterRegionsMap().set(convertToVisibilityMapMessage(statistics.getInterRegionsMapId(), statistics.getInterRegionsVisibilityMap())); for (int i = 0; i < statistics.getNumberOfNavigableRegions(); i++) message.getNavigableRegions().add().set(convertToNavigableRegionMessage(statistics.getNavigableRegion(i))); return message; }
@ContinuousIntegrationAnnotations.ContinuousIntegrationTest(estimatedDuration = 135.9) @Test(timeout = 600000) public void testConvertBodyPathPlanStatistics() { Random random = new Random(1738L); for (int iter = 0; iter < iters; iter++) { VisibilityMapHolder startMap = VisibilityGraphRandomTools.getRandomSingleSourceVisibilityMap(random); VisibilityMapHolder goalMap = VisibilityGraphRandomTools.getRandomSingleSourceVisibilityMap(random); VisibilityMapHolder interRegionsMap = VisibilityGraphRandomTools.getRandomInterRegionVisibilityMap(random); List<NavigableRegion> navigableRegions = new ArrayList<>(); int planId = RandomNumbers.nextInt(random, 0, 1000); int numberOfNavigableRegions = RandomNumbers.nextInt(random, 2, 10); for (int i = 0; i < numberOfNavigableRegions; i++) navigableRegions.add(VisibilityGraphRandomTools.getRandomNavigableRegion(random)); VisibilityGraphStatistics statisticsToConvert = new VisibilityGraphStatistics(); statisticsToConvert.setGoalVisibilityMapInWorld(goalMap.getMapId(), goalMap.getVisibilityMapInWorld()); statisticsToConvert.setStartVisibilityMapInWorld(startMap.getMapId(), startMap.getVisibilityMapInWorld()); statisticsToConvert.setInterRegionsVisibilityMapInWorld(interRegionsMap.getMapId(), interRegionsMap.getVisibilityMapInWorld()); statisticsToConvert.setNavigableRegions(navigableRegions); BodyPathPlanStatisticsMessage message = VisibilityGraphMessagesConverter.convertToBodyPathPlanStatisticsMessage(planId, statisticsToConvert); VisibilityGraphStatistics convertedStatistics = VisibilityGraphMessagesConverter.convertToVisibilityGraphStatistics(message); VisibilityGraphTestTools.assertVisibilityGraphStatisticsEqual(statisticsToConvert, convertedStatistics, epsilon); } } }