NavigableRegionsManager navigableRegionsManager = new NavigableRegionsManager(parameters.get()); navigableRegionsManager.setPlanarRegions(planarRegions); bodyPath = navigableRegionsManager.calculateBodyPathWithOcclusions(start, goal); bodyPath = navigableRegionsManager.calculateBodyPath(start, goal); messager.submitMessage(StartVisibilityMap, navigableRegionsManager.getStartMap()); messager.submitMessage(GoalVisibilityMap, navigableRegionsManager.getGoalMap()); messager.submitMessage(NavigableRegionVisibilityMap, navigableRegionsManager.getNavigableRegions()); messager.submitMessage(InterRegionVisibilityMap, navigableRegionsManager.getInterRegionConnections()); messager.submitMessage(NavigableRegionData, navigableRegionsManager.getNavigableRegions());
private void runTest(PlanarRegionsList planarRegionsList) { Point3D start = new Point3D(); Point3D goal = new Point3D(2.0, -1.0, 0.0); NavigableRegionsManager navigableRegionsManager = new NavigableRegionsManager(planarRegionsList.getPlanarRegionsAsList()); List<Point3DReadOnly> path = navigableRegionsManager.calculateBodyPathWithOcclusions(start, goal); if(visualize) { visualize(path, planarRegionsList, start, goal); } }
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); } }
YoGraphicsListRegistry graphicsListRegistry = new YoGraphicsListRegistry(); NavigableRegionsManager vizGraphs = new NavigableRegionsManager(VISIBILITY_GRAPH_PARAMETERS); vizGraphs.setPlanarRegions(visiblePlanarRegions.getPlanarRegionsAsList()); bodyPath = vizGraphs.calculateBodyPathWithOcclusions(currentPosition, goal);
navigableRegionsManager.setPlanarRegions(planarRegionsList.getPlanarRegionsAsList()); List<Point3DReadOnly> path = new ArrayList<>(navigableRegionsManager.calculateBodyPath(startPos, goalPos));
goalPos = PlanarRegionTools.projectPointToPlanes(goalPos, new PlanarRegionsList(regions)); NavigableRegionsManager navigableRegionsManager = new NavigableRegionsManager(new DefaultVisibilityGraphParameters()); List<Point3DReadOnly> path = new ArrayList<>(navigableRegionsManager.calculateBodyPath(startPos, goalPos)); for (Point3DReadOnly waypoint3d : path)
public VisibilityGraphPathPlanner(String prefix, FootstepPlannerParameters footstepPlannerParameters, VisibilityGraphsParameters visibilityGraphsParameters, YoVariableRegistry parentRegistry) { super(prefix, footstepPlannerParameters, parentRegistry); this.navigableRegionsManager = new NavigableRegionsManager(visibilityGraphsParameters); }
public List<Point3DReadOnly> calculateBodyPathWithOcclusions(Point3DReadOnly start, Point3DReadOnly goal) { List<Point3DReadOnly> path = calculateBodyPath(start, goal); if (path == null) { if (!OcclusionTools.isTheGoalIntersectingAnyObstacles(navigableRegions.get(0), start, goal)) { if(debug) { PrintTools.info("StraightLine available"); } path = new ArrayList<>(); path.add(start); path.add(goal); return path; } NavigableRegion regionContainingPoint = PlanarRegionTools.getNavigableRegionContainingThisPoint(start, navigableRegions); List<Cluster> intersectingClusters = OcclusionTools.getListOfIntersectingObstacles(regionContainingPoint.getObstacleClusters(), start, goal); Cluster closestCluster = ClusterTools.getTheClosestCluster(start, intersectingClusters); Point3D closestExtrusion = ClusterTools.getTheClosestVisibleExtrusionPoint(1.0, start, goal, closestCluster.getNavigableExtrusionsInWorld(), regionContainingPoint.getHomeRegion()); path = calculateBodyPath(start, closestExtrusion); path.add(goal); return path; } else { return path; } }
public NavigableRegionsManager(VisibilityGraphsParameters parameters, List<PlanarRegion> regions) { this.parameters = parameters == null ? new DefaultVisibilityGraphParameters() : parameters; setPlanarRegions(regions); }
NavigableRegionsManager manager = new NavigableRegionsManager(parameters); manager.setPlanarRegions(planarRegionsList.getPlanarRegionsAsList()); path = manager.calculateBodyPath(start, goal); messager.submitMessage(UIVisibilityGraphsTopics.NavigableRegionData, manager.getNavigableRegions()); messager.submitMessage(UIVisibilityGraphsTopics.StartVisibilityMap, manager.getStartMap()); messager.submitMessage(UIVisibilityGraphsTopics.GoalVisibilityMap, manager.getGoalMap()); messager.submitMessage(UIVisibilityGraphsTopics.NavigableRegionVisibilityMap, manager.getNavigableRegions()); messager.submitMessage(UIVisibilityGraphsTopics.InterRegionVisibilityMap, manager.getInterRegionConnections());