refactoring

Robotcontroller/robotcontroller/NativeNav.java

@@ -144,7 +144,6 @@ public class NativeNav extends RobotController implements MotionSubsystemListene
             throw new IllegalArgumentException("Robot controller "+getClass().getName()+" failed to read configuration: "+split[1]+"' is not number");
         }
 
-
         try {
             targetAngleDeg=Double.parseDouble(split[2]);
             targetAngle=RobotGeomUtil.mssAngle2NavAngle(targetAngleDeg);
@@ -250,7 +249,6 @@ public class NativeNav extends RobotController implements MotionSubsystemListene
         obstaclesLidar.clear();
     }
 
-
     private TrajectorySequence computeRouteAndTrajectories(double pX,double pY,double pA) throws Exception {
 
         double[][] routingObstacles=ObstacleContainer.getObstactles2D(obstaclesLidar);
@@ -288,7 +286,6 @@ public class NativeNav extends RobotController implements MotionSubsystemListene
         return trajSeq;
     }
 
-
     private boolean targetReached() {
         if (Math.hypot(posXMSS-targetX,posYMSS-targetY)>2)
             return false;

Robotcontroller/robotcontroller/NativeNavP2PRouting.java

@@ -41,6 +41,9 @@ public class NativeNavP2PRouting extends Pos2PosRouting {
         Point startPosition = new Point(fromX, fromY);
         Point endPosition = new Point(toX, toY);
 
+        double minTurningRadius = Robot.robotLengthRotation - Robot.headingDistance;
+        double bufferWidth = minTurningRadius + 2.0;
+
         paintStartAndEnd(startPosition, endPosition);
 
         List<Point> obstaclePoints = Arrays.stream(obstacles)
@@ -48,7 +51,7 @@ public class NativeNavP2PRouting extends Pos2PosRouting {
                 .collect(Collectors.toList());
 
         // compute points around obstacles
-        List<Point> bufferPoints = computeBufferPoints(obstaclePoints, 30.0, 64);
+        List<Point> bufferPoints = computeBufferPoints(obstaclePoints, bufferWidth, 64);
         bufferPoints = reducePoints(bufferPoints, 10.0);
         paintBufferPoints(bufferPoints);
 
@@ -57,13 +60,13 @@ public class NativeNavP2PRouting extends Pos2PosRouting {
         bufferPoints.add(endPosition);
 
         PointCloud2D<Point> obstacleCloud = new PointCloudCreator2D<>(PointCloudCreator2D.TYPE_GRID).createFilled(obstacles, Point.class);
-        List<LinkedPoint> vg = generateVisibilityGraph(bufferPoints, obstacleCloud, 28.0);
+        List<LinkedPoint> vg = generateVisibilityGraph(bufferPoints, obstacleCloud, bufferWidth-2.0);
         RoutingGraph routingGraph = new RoutingGraph(vg);
 
         paintGraph(routingGraph);
 
         AStar aStar = new AStar();
-        List<Point> route = null;
+        List<Point> route;
         try {
             route = aStar.computeRoute(routingGraph, startPosition, endPosition);
             paintRoute(route);
@@ -79,8 +82,7 @@ public class NativeNavP2PRouting extends Pos2PosRouting {
     }
 
     @Override
-    // Fake-Routing: es wird einfach nur die Liste oben durchwandert und die Teilliste bis zum Ziel zurückgegeben.
-    // Ein echtes Routingverfahren muss hier eine echt berechnete Liste von Zwischenschritten zurückgeben
+    // Durchgehen der zuvor berechneten Route
     public double[][] getRoute(double fromX,double fromY,double toX,double toY,double[][] obstacles) {
 
         // Suche den Linienabschnitt aus ROUTE, zu dem fromX,fromY am nähesten ist

Robotcontroller/robotcontroller/visualGraph/util.java

 package robotcontroller.visualGraph;
 
-import basics.points.*;
+import basics.points.Point;
+import basics.points.PointCloud2D;
+import basics.points.PointCloudCreator2D;
 import basics.points.util.LinkedPoint;
 
 import java.util.ArrayList;
 import java.util.Collection;
 import java.util.List;
-import java.util.stream.Collectors;
 
 abstract public class util {
 
+    /**
+     * determining if a point is near at least one of a list of other points
+     * @param point point to check
+     * @param points points to check
+     * @param radius distance to check proximity
+     * @return boolean if the point is near at least on of the given points
+     */
     public static boolean isNearPoints(Point point, List<Point> points, double radius) {
         boolean isNear = false;
 
@@ -23,6 +31,12 @@ abstract public class util {
         return isNear;
     }
 
+    /**
+     * Reduce points by removing points within a certain radius
+     * @param points points to reduce
+     * @param radius radius in which other points get removed
+     * @return list of reduces points
+     */
     public static List<Point> reducePoints(List<Point> points, double radius) {
         List<Point> reducedPoints = new ArrayList<>();
         for (Point p : points) {
@@ -34,6 +48,13 @@ abstract public class util {
         return reducedPoints;
     }
 
+    /**
+     * Compute buffer-points with a certain radius around given points.
+     * @param points Points where to compute buffer-points around
+     * @param bufferDist distance of buffer-points to given points
+     * @param numBufferPoints number of points to approximate circle
+     * @return List of buffer-points
+     */
     public static List<Point> computeBufferPoints(List<Point> points, double bufferDist, int numBufferPoints) {
 
         double[][] tmp = points.stream()
@@ -56,7 +77,13 @@ abstract public class util {
         return result;
     }
 
-
+    /**
+     * Compute buffer-points with a certain radius around a given point.
+     * @param point Point where to compute buffer-points around
+     * @param bufferDist distance of buffer-points to given point
+     * @param numBufferPoints number of points to approximate circle
+     * @return List of buffer-points
+     */
     public static List<Point> computeBufferPoints(Point point, double bufferDist, int numBufferPoints) {
         List<Point> bufferPoints = new ArrayList<>();
 
@@ -69,6 +96,13 @@ abstract public class util {
         return bufferPoints;
     }
 
+    /**
+     *
+     * @param driveablePoints List of points on which the robot can travel, i.e. nodes of the visibility graph
+     * @param obsticlePoints List of points which describe the obstacles
+     * @param buffer Distance that must be kept from obstacle points
+     * @return List of points and their direct connections by determining their line of sight
+     */
     public static List<LinkedPoint> generateVisibilityGraph(List<Point> driveablePoints, PointCloud2D<Point> obsticlePoints, double buffer) {
         List<LinkedPoint> visibilityGraph = new ArrayList<>();