Reduce global path spacing/state space size from 30km to 3km

src/global_launch/config/globals.yaml

@@ -9,7 +9,7 @@
 mock_global_path:
    ros__parameters:
       global_path_filepath: "/workspaces/sailbot_workspace/src/local_pathfinding/global_paths/mock_global_path.csv"
-      interval_spacing: 30.0
+      interval_spacing: 3.0
       write: false
       gps_threshold: 1.5
       force: false

src/local_pathfinding/local_pathfinding/mock_nodes/wind_params.yaml

@@ -1,9 +1,8 @@
 /mock_wind_sensor:
   ros__parameters:
-    tw_dir_deg: 0
-    tw_speed_kmph: 0.0
-
+    tw_dir_deg: 80
+    tw_speed_kmph: 10.0
 /mock_gps:
   ros__parameters:
-    tw_dir_deg: 0
-    tw_speed_kmph: 0.0
+    tw_dir_deg: 80
+    tw_speed_kmph: 10.0

src/local_pathfinding/local_pathfinding/node_navigate.py

@@ -1,18 +1,18 @@
 """The main node of the local_pathfinding package, represented by the `Sailbot` class."""
 
-import custom_interfaces.msg as ci
 import rclpy
 from rclpy.node import Node
+from test_plans.test_plan import TestPlan
 
+import custom_interfaces.msg as ci
 import local_pathfinding.coord_systems as cs
 import local_pathfinding.global_path as gp
 import local_pathfinding.obstacles as ob
 from local_pathfinding.local_path import LocalPath
 from local_pathfinding.ompl_path import MAX_SOLVER_RUN_TIME_SEC
-from test_plans.test_plan import TestPlan
 
 GLOBAL_WAYPOINT_REACHED_THRESH_KM = 3
-PATHFINDING_RANGE_KM = 30
+PATHFINDING_RANGE_KM = 3.0
 REALLY_FAR_M = 100000000
 
 

src/local_pathfinding/local_pathfinding/ompl_path.py

@@ -13,13 +13,13 @@
 import pickle
 from typing import TYPE_CHECKING, Any, Union
 
-import custom_interfaces.msg as ci
 from ompl import base
 from ompl import geometric as og
 from ompl import util as ou
 from rclpy.impl.rcutils_logger import RcutilsLogger
 from shapely.geometry import MultiPolygon, Point, Polygon, box
 
+import custom_interfaces.msg as ci
 import local_pathfinding.coord_systems as cs
 import local_pathfinding.obstacles as ob
 from local_pathfinding.ompl_objectives import get_sailing_objective
@@ -29,15 +29,15 @@
 
 ou.setLogLevel(ou.LOG_WARN)
 
-BOX_BUFFER_SIZE_KM = 1.0
+BOX_BUFFER_SIZE_KM = 2.0
 # for now this is statically defined and subject to change or may be made dynamic
 MIN_TURNING_RADIUS_KM = 0.05  # 50 m
 # sets an upper limit on the allowable edge length in the graph formed by the RRT* algorithm
 # We set this as small as possible to reduce instances where both endpoints of a path segment are
 # valid but the segment straddles an obstacle collision zone
 # setting this any smaller can lead to OMPL not being able to construct a tree that reaches
 # the goal state
-MAX_EDGE_LEN_KM = 5.0
+MAX_EDGE_LEN_KM = 0.5
 MAX_SOLVER_RUN_TIME_SEC = 1.0
 
 LAND_KEY = -1
@@ -276,9 +276,11 @@ def get_remaining_cost(self, last_lp_wp_index: int, boat_lat_lon: ci.HelperLatLo
 
             # clamp to [0, total_seg_dist]
             if projected_dist > total_seg_dist:
-                self._logger.warning("Projection of the boat position wrt" +
-                                     "the segment is longer than the segment itself," +
-                                     "the boat should've switched local waypoint by this point")
+                self._logger.warning(
+                    "Projection of the boat position wrt"
+                    + "the segment is longer than the segment itself,"
+                    + "the boat should've switched local waypoint by this point"
+                )
                 # assuming pessimistic case
                 projected_dist = total_seg_dist
             elif projected_dist < 0.0:

src/local_pathfinding/test/test_node_navigate.py

@@ -1,14 +1,13 @@
-import custom_interfaces.msg as ci
 import pytest
 
+import custom_interfaces.msg as ci
 import local_pathfinding.node_navigate as nn
 from local_pathfinding.local_path import LocalPath
 
 LOCAL_WAYPOINT_REACHED_THRESH_KM = 0.5
 GLOBAL_WAYPOINT_REACHED_THRESH_KM = 3
-PATHFINDING_RANGE_KM = 30
 ONE_DEGREE_KM = 111  # One degree longitude at equator = 111km
-PATH_RANGE_DEG = PATHFINDING_RANGE_KM / ONE_DEGREE_KM
+PATH_RANGE_DEG = nn.PATHFINDING_RANGE_KM / ONE_DEGREE_KM
 
 
 @pytest.mark.parametrize(

src/local_pathfinding/test/test_ompl_path.py

@@ -2,6 +2,13 @@
 import random
 
 import pytest
+from ompl import base
+from rclpy.impl.rcutils_logger import RcutilsLogger
+from shapely.geometry import MultiPolygon, Point, box
+
+import local_pathfinding.coord_systems as cs
+import local_pathfinding.obstacles as ob
+import local_pathfinding.ompl_path as ompl_path
 from custom_interfaces.msg import (
     GPS,
     AISShips,
@@ -14,13 +21,6 @@
     Path,
     WindSensor,
 )
-from ompl import base
-from rclpy.impl.rcutils_logger import RcutilsLogger
-from shapely.geometry import MultiPolygon, Point, box
-
-import local_pathfinding.coord_systems as cs
-import local_pathfinding.obstacles as ob
-import local_pathfinding.ompl_path as ompl_path
 from local_pathfinding.local_path import LocalPathState
 
 LAND_KEY = -1
@@ -35,17 +35,17 @@ def fresh_ompl_path():
             ais_ships=AISShips(),
             global_path=Path(
                 waypoints=[
-                    HelperLatLon(latitude=0.02, longitude=0.02),
-                    HelperLatLon(latitude=0.04, longitude=0.03),
-                    HelperLatLon(latitude=0.06, longitude=0.05),
-                    HelperLatLon(latitude=0.08, longitude=0.07),
-                    HelperLatLon(latitude=0.10, longitude=0.09),
-                    HelperLatLon(latitude=0.12, longitude=0.11),
-                    HelperLatLon(latitude=0.15, longitude=0.15),
                     HelperLatLon(latitude=0.1, longitude=0.1),
+                    HelperLatLon(latitude=0.15, longitude=0.15),
+                    HelperLatLon(latitude=0.12, longitude=0.11),
+                    HelperLatLon(latitude=0.10, longitude=0.09),
+                    HelperLatLon(latitude=0.08, longitude=0.07),
+                    HelperLatLon(latitude=0.06, longitude=0.05),
+                    HelperLatLon(latitude=0.04, longitude=0.03),
+                    HelperLatLon(latitude=0.02, longitude=0.02),
                 ]
             ),
-            target_global_waypoint=HelperLatLon(latitude=0.1, longitude=0.1),
+            target_global_waypoint=HelperLatLon(latitude=0.02, longitude=0.02),
             filtered_wind_sensor=WindSensor(),
             planner="rrtstar",
         ),
@@ -283,9 +283,7 @@ def test_create_space(
 @pytest.mark.parametrize("boat_latlon", [HelperLatLon(latitude=0.0, longitude=0.0)])
 def test_get_remaining_cost_full_path(fresh_ompl_path, boat_latlon):
     remaining_cost = fresh_ompl_path.get_remaining_cost(0, boat_latlon)
-    assert remaining_cost == pytest.approx(
-        fresh_ompl_path.get_cost(), abs=0.01
-    )
+    assert remaining_cost == pytest.approx(fresh_ompl_path.get_cost(), abs=0.01)
 
 
 @pytest.mark.parametrize(
@@ -307,10 +305,18 @@ def mid_point(start_latlon: HelperLatLon, end_latlon: HelperLatLon):
             return start_latlon
         end_points_inclusive_factor = 1e-4
         return HelperLatLon(
-            latitude=(random.uniform(start_latlon.latitude + end_points_inclusive_factor,
-                                     end_latlon.latitude - end_points_inclusive_factor)),  # noqa
-            longitude=(random.uniform(start_latlon.longitude + end_points_inclusive_factor,
-                                      end_latlon.longitude - end_points_inclusive_factor)),  # noqa
+            latitude=(
+                random.uniform(
+                    start_latlon.latitude + end_points_inclusive_factor,
+                    end_latlon.latitude - end_points_inclusive_factor,
+                )
+            ),  # noqa
+            longitude=(
+                random.uniform(
+                    start_latlon.longitude + end_points_inclusive_factor,
+                    end_latlon.longitude - end_points_inclusive_factor,
+                )
+            ),  # noqa
         )
 
     boat_latlon = mid_point(boat_latlon, next_wp_latlon)