optimise fastpathplanner

Author: energy-in-joles

conftest.py

@@ -59,14 +59,14 @@ def pytest_generate_tests(metafunc):
 
 
 # temporarily excludes the motion planning tests until the motion planning algorithms are working
-def pytest_collection_modifyitems(config, items):
-    if config.getoption("--include-motion-planning"):
-        return
+# def pytest_collection_modifyitems(config, items):
+#     if config.getoption("--include-motion-planning"):
+#         return
 
-    skip_mp = pytest.mark.skip(reason="motion planning not ready")
-    for item in items:
-        if "motion_planning" in item.keywords:
-            item.add_marker(skip_mp)
+#     skip_mp = pytest.mark.skip(reason="motion planning not ready")
+#     for item in items:
+#         if "motion_planning" in item.keywords:
+#             item.add_marker(skip_mp)
 
 
 @pytest.fixture

utama_core/global_utils/math_utils.py

@@ -288,10 +288,10 @@ def segments_intersect(
     p2 = np.asarray(seg2_start)
     q2 = np.asarray(seg2_end)
 
-    o1 = orientation(p1, q1, p2)
-    o2 = orientation(p1, q1, q2)
-    o3 = orientation(p2, q2, p1)
-    o4 = orientation(p2, q2, q1)
+    o1 = point_orientation(p1, q1, p2)
+    o2 = point_orientation(p1, q1, q2)
+    o3 = point_orientation(p2, q2, p1)
+    o4 = point_orientation(p2, q2, q1)
 
     if o1 != o2 and o3 != o4:
         return True
@@ -308,7 +308,7 @@ def segments_intersect(
     return False
 
 
-def orientation(p_1: np.ndarray, p_2: np.ndarray, p_3: np.ndarray) -> int:
+def point_orientation(p_1: np.ndarray, p_2: np.ndarray, p_3: np.ndarray) -> int:
     """Calculate the orientation of 3 points (e.g. on a line or in a triangle).
 
     Args:

utama_core/motion_planning/src/fastpathplanning/config.py

@@ -3,9 +3,16 @@
 
 class fastpathplanningconfig:
     ROBOT_DIAMETER = 2 * ROBOT_RADIUS
-    OBSTACLE_CLEARANCE = ROBOT_DIAMETER * 1.1
-    SUBGOAL_DISTANCE = OBSTACLE_CLEARANCE * 1.1
+
+    # how fat is the danger zone around obstacles, in multiples of robot diameter
+    CLEARANCE_MULTIPLIER = 1.5
+    OBSTACLE_CLEARANCE = ROBOT_DIAMETER * CLEARANCE_MULTIPLIER
+
+    # How far outside the danger zone shold the waypoint be
+    SUBGOAL_MULTIPLIER = 1.2
+    SUBGOAL_DISTANCE = OBSTACLE_CLEARANCE * SUBGOAL_MULTIPLIER
+
     LOOK_AHEAD_RANGE = 3
     MAXRECURSION_LENGTH = 3
-    PROJECTEDFRAMES = 15
-    PROJECTION_DISTANCE = OBSTACLE_CLEARANCE * 4
+    PROJECTEDFRAMES = 20
+    PROJECTION_DISTANCE = 1