Raghumanimehta/778 get cost (#798)

* disabled zoom

* resets the state space to the default range of the plot

* reset button location changed

* changed some weights

* get_remaining_cost implemented

* basic test added

* added some more tests, failing still

* the partial tests are failing

* index errors

* still broken, will use mocks now

* no partial tests are passing

* all tests passed

* added a projection based test

* linting

* Update src/local_pathfinding/test/test_ompl_path.py

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>

* Update src/local_pathfinding/local_pathfinding/local_path.py

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>

* added back setCostThreshold in ompl_objectives.py

* local_path.py, update_if_needed docstring change

* variable change

* removed simplifySolution

* linting error fix

* added a factor to force inclusivity

* Update src/local_pathfinding/test/test_ompl_path.py

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>

* updated docus

* LocalPathState constructor now calls update_state method

* Update src/local_pathfinding/local_pathfinding/local_path.py

Co-authored-by: Sean Donaghy <118148642+SPDonaghy@users.noreply.github.com>

* last -> prev

* comment fixes

* comments updated

* Update src/local_pathfinding/local_pathfinding/ompl_path.py

Co-authored-by: Sean Donaghy <118148642+SPDonaghy@users.noreply.github.com>

* Fix formatting issues in docstring of get_remaining_cost

* Clean up comment in OMPL path calculation

Remove unnecessary comment about degenerate segment.

* imporved comment + structure of the funciton

* comment changes

* removed pointer terminology wherever required

---------

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>
Co-authored-by: Sean Donaghy <118148642+SPDonaghy@users.noreply.github.com>

Author: 3 people

src/local_pathfinding/local_pathfinding/local_path.py

@@ -3,11 +3,11 @@
 import math
 from typing import List, Optional
 
+import custom_interfaces.msg as ci
 from rclpy.impl.rcutils_logger import RcutilsLogger
 from shapely.geometry import LineString, MultiPolygon
 
 import local_pathfinding.coord_systems as cs
-import custom_interfaces.msg as ci
 import local_pathfinding.obstacles as ob
 from local_pathfinding.ompl_path import OMPLPath
 
@@ -46,6 +46,34 @@ def __init__(
         filtered_wind_sensor: ci.WindSensor,
         planner: str,
     ):
+        self.update_state(gps, ais_ships, filtered_wind_sensor)
+
+        if not (global_path and global_path.waypoints):
+            raise ValueError("Cannot create a LocalPathState with an empty global_path")
+        self.global_path = global_path
+        self.reference_latlon = target_global_waypoint
+
+        if not planner:
+            raise ValueError("planner must not be None")
+        self.planner = planner
+
+        # obstacles are initialized by OMPLPath right before solving
+        self.obstacles: List[ob.Obstacle] = []
+
+    def update_state(
+        self, gps: ci.GPS, ais_ships: ci.AISShips, filtered_wind_sensor: ci.WindSensor
+    ):
+        """Updates the changeable environment without changing the path or reference_latlon
+
+        This method updates only the dynamic state variables (position, heading, speed,
+        ais_ships, wind) without changing the reference coordinate system or global path.
+        Used when continuing on an existing path to avoid coordinate system mismatches.
+
+        Args:
+            gps (ci.GPS): Current GPS position and heading data
+            ais_ships (ci.AISShips): Updated AIS ship data
+            filtered_wind_sensor (ci.WindSensor): Updated wind sensor data
+        """
         if not gps:
             raise ValueError("gps must not be None")
         self.position = gps.lat_lon
@@ -61,27 +89,15 @@ def __init__(
         self.wind_speed = filtered_wind_sensor.speed.speed
         self.wind_direction = filtered_wind_sensor.direction
 
-        if not (global_path and global_path.waypoints):
-            raise ValueError("Cannot create a LocalPathState with an empty global_path")
-        self.global_path = global_path
-        self.reference_latlon = target_global_waypoint
-
-        if not planner:
-            raise ValueError("planner must not be None")
-        self.planner = planner
-
-        # obstacles are initialized by OMPLPath right before solving
-        self.obstacles: List[ob.Obstacle] = []
-
 
 class LocalPath:
     """Sets and updates the OMPL path and the local waypoints
 
     Attributes:
         _logger (RcutilsLogger): ROS logger.
         _ompl_path (Optional[OMPLPath]): Raw representation of the path from OMPL.
-        _waypoint_index: Local waypoint index (i.e. pointer to the next local waypoint that the
-        boat is following)
+        _prev_lp_wp_index (int): index of the local waypoint that Polaris has already traversed in
+        the path array
         path (Path): Collection of coordinates that form the local path to the next
                           global waypoint.
         state (LocalPathState): the current local path state.
@@ -94,16 +110,17 @@ def __init__(self, parent_logger: RcutilsLogger):
         self.state: Optional[LocalPathState] = None
 
     @staticmethod
-    def calculate_desired_heading_and_waypoint_index(
+    def calculate_desired_heading_and_wp_index(
         path: ci.Path, waypoint_index: int, boat_lat_lon: ci.HelperLatLon
     ):
         """Calculates the desired heading using GEODESIC. Updates the waypoint index (i.e. change
         the waypoint) if the boat is close enough to the current waypoint.
 
         Args:
             path (ci.Path): Array of waypoints
-            waypoint_index (int): Pointer to the current local waypoint index in path array
-            (i.e. the waypoint sailbot is heading towards)
+            prev_lp_wp_index (int): index of the local waypoint that Polaris has already traversed
+            in the path array
+            (i.e. the waypoint sailbot traversed, sailbot is heading towards waypoint_index + 1)
             boat_lat_lon (ci.HelperLatLon): boat coordinates
 
         Returns:
@@ -162,7 +179,7 @@ def update_if_needed(
         gps: ci.GPS,
         ais_ships: ci.AISShips,
         global_path: ci.Path,
-        local_waypoint_index: int,
+        prev_lp_wp_index: int,
         received_new_global_waypoint: bool,
         target_global_waypoint: ci.HelperLatLon,
         filtered_wind_sensor: ci.WindSensor,
@@ -184,7 +201,9 @@ def update_if_needed(
             gps (ci.GPS): Current GPS position and heading data.
             ais_ships (ci.AISShips): AIS data for nearby ships (obstacles).
             global_path (ci.Path): The global path plan to the destination.
-            local_waypoint_index (int): Current index in the local waypoint list.
+            prev_lp_wp_index (int): Current index in the local waypoint list.
+            This is the index that the boat last traversed. The boat is heading towards
+            the index following prev_lp_wp_index
             received_new_global_waypoint (bool): Flag indicating if a new global
                 waypoint was received.
             target_global_waypoint (ci.HelperLatLon): Target waypoint from global path.
@@ -199,53 +218,71 @@ def update_if_needed(
                 - Updated waypoint index
             The method decides whether to return the heading for new path or old path
         """
-        # this raises ValueError if any of the parameters are not properly initialized
-        self._waypoint_index = local_waypoint_index
-        state = LocalPathState(
+        self._prev_lp_wp_index = prev_lp_wp_index
+        old_ompl_path = self._ompl_path
+
+        if (
+            (received_new_global_waypoint or old_ompl_path is None)
+            or (self.path is None)
+            or (self.state is None)
+        ):
+            new_state = LocalPathState(
+                gps, ais_ships, global_path, target_global_waypoint, filtered_wind_sensor, planner
+            )
+            new_ompl_path = OMPLPath(
+                parent_logger=self._logger,
+                local_path_state=new_state,
+                land_multi_polygon=land_multi_polygon,
+            )
+            heading_new_path, wp_index = self.calculate_desired_heading_and_wp_index(
+                new_ompl_path.get_path(), 0, gps.lat_lon
+            )
+            if received_new_global_waypoint:
+                self._logger.debug("Updating local path because we have a new global waypoint")
+            else:
+                self._logger.debug("old path is None")
+            self.state = new_state
+            self._update(new_ompl_path)
+            return heading_new_path, wp_index
+        else:
+            self.state.update_state(gps, ais_ships, filtered_wind_sensor)
+
+        # create a new state with most up-to-date environment information. This is done to compare
+        # a new OMPL path based on this state with the OMPL path that we have been using. Since the
+        # paths are based on environment snapshots of when the path was created, this is important
+        # to ensure that we are not on a bad path.
+        new_state = LocalPathState(
             gps, ais_ships, global_path, target_global_waypoint, filtered_wind_sensor, planner
         )
-        self.state = state
-        ompl_path = OMPLPath(
+        new_ompl_path = OMPLPath(
             parent_logger=self._logger,
-            local_path_state=state,
+            local_path_state=new_state,
             land_multi_polygon=land_multi_polygon,
         )
-        old_ompl_path = self._ompl_path
 
-        heading_new_path, wp_index = self.calculate_desired_heading_and_waypoint_index(
-            ompl_path.get_path(), 0, gps.lat_lon
+        heading_new_path, wp_index = self.calculate_desired_heading_and_wp_index(
+            new_ompl_path.get_path(), 0, gps.lat_lon
         )
-
-        if received_new_global_waypoint:
-            self._logger.debug("Updating local path because we have a new global waypoint")
-            self._update(ompl_path)
-            return heading_new_path, wp_index
-
-        if old_ompl_path is None or self.path is None:
-            # continue on the same path
-            self._logger.debug("old path is none")
-            self._update(ompl_path)
-            return heading_new_path, wp_index
-
-        heading_old_path, updated_wp_index = self.calculate_desired_heading_and_waypoint_index(
-            old_ompl_path.get_path(), local_waypoint_index, gps.lat_lon
+        heading_old_path, updated_wp_index = self.calculate_desired_heading_and_wp_index(
+            old_ompl_path.get_path(), prev_lp_wp_index, gps.lat_lon
         )
-        # check if the current path goes through a collision zone.
-        # No need to check for new path since it's fresh and ompl doesn't generate path that
-        # go through a collision zone
+
         if self.in_collision_zone(
-            local_waypoint_index, self.state.reference_latlon, self.path, self.state.obstacles
+            prev_lp_wp_index, self.state.reference_latlon, self.path, self.state.obstacles
         ):
             self._logger.debug("old path is in collision zone")
-            self._update(ompl_path)
+            self.state = new_state
+            self._update(new_ompl_path)
             return heading_new_path, wp_index
 
         heading_diff_old_path = cs.calculate_heading_diff(self.state.heading, heading_old_path)
         heading_diff_new_path = cs.calculate_heading_diff(self.state.heading, heading_new_path)
 
-        old_cost = old_ompl_path.get_cost(updated_wp_index)
-        new_cost = ompl_path.get_cost(wp_index)
+        old_prev_lp_wp_index = max(updated_wp_index - 1, 0)
+        new_prev_lp_wp_index = max(wp_index - 1, 0)
 
+        old_cost = old_ompl_path.get_remaining_cost(old_prev_lp_wp_index, gps.lat_lon)
+        new_cost = new_ompl_path.get_remaining_cost(new_prev_lp_wp_index, gps.lat_lon)
         max_cost = max(old_cost, new_cost, 1)
         old_cost_normalized = old_cost / max_cost
         new_cost_normalized = new_cost / max_cost
@@ -264,24 +301,21 @@ def update_if_needed(
         metric_new = w_h * heading_diff_new_normalized + w_c * new_cost_normalized
 
         self._logger.debug(
-                f"(old cost: {old_cost:.2f}, "
-                f"new cost: {new_cost:.2f})"
-                f", metric_old: {metric_old:.2f}, "
-                f"metric_new: {metric_new:.2f}, "
-                f"old_cost_normalized: {old_cost_normalized:.2f}, "
-                f"new_cost_normalized: {new_cost_normalized:.2f}"
-            )
+            f"(old cost: {old_cost:.2f}, "
+            f"new cost: {new_cost:.2f})"
+            f", metric_old: {metric_old:.2f}, "
+            f"metric_new: {metric_new:.2f}, "
+            f"old_cost_normalized: {old_cost_normalized:.2f}, "
+            f"new_cost_normalized: {new_cost_normalized:.2f}"
+        )
         if metric_new < metric_old:
-            self._logger.debug(
-                "New path is cheaper, updating local path "
-            )
-            self._update(ompl_path)
+            self._logger.debug("New path is cheaper, updating local path ")
+            self.state = new_state
+            self._update(new_ompl_path)
             return heading_new_path, wp_index
         else:
-            self._logger.debug(
-                "old path is cheaper, continuing on the same path"
-            )
-            return heading_old_path, wp_index
+            self._logger.debug("old path is cheaper, continuing on the same path")
+            return heading_old_path, updated_wp_index
 
     def _update(self, ompl_path: OMPLPath):
 

src/local_pathfinding/local_pathfinding/node_navigate.py

@@ -52,6 +52,8 @@ class Sailbot(Node):
         global_path (ci.Path): Path that we are following.
         filtered_wind_sensor (ci.WindSensor): Filtered data from the wind sensors.
         desired_heading (ci.DesiredHeading): current desired heading.
+        prev_lp_wp_index: local waypoint that Polaris has already traversed/seen. Polaris is moving
+        towards prev_lp_wp_index + 1 waypoint.
 
     Attributes:
         local_path (LocalPath): The path that `Sailbot` is following.
@@ -124,7 +126,7 @@ def __init__(self):
 
         # attributes
         self.local_path = LocalPath(parent_logger=self.get_logger())
-        self.local_waypoint_index = 0
+        self.prev_lp_wp_index = 0
         self.global_waypoint_index = -1
         self.saved_target_global_waypoint = None
         self.use_mock_land = self.get_parameter("use_mock_land").get_parameter_value().bool_value
@@ -292,11 +294,11 @@ def get_desired_heading(self) -> float:
                 self.global_waypoint_index
             ]
 
-        desired_heading, self.local_waypoint_index = self.local_path.update_if_needed(
+        desired_heading, self.prev_lp_wp_index = self.local_path.update_if_needed(
             self.gps,
             self.ais_ships,
             self.global_path,
-            self.local_waypoint_index,
+            self.prev_lp_wp_index,
             received_new_global_waypoint,
             self.saved_target_global_waypoint,
             self.filtered_wind_sensor,

src/local_pathfinding/local_pathfinding/ompl_objectives.py

@@ -12,9 +12,9 @@
 UPWIND_COST_MULTIPLIER = 1.0
 DOWNWIND_COST_MULTIPLIER = 1.0
 ZERO_SPEED_COST = 1.0
-ACCEPTABLE_COST_THRESHOLD = 0.0
-WIND_OBJECTIVE_WEIGHT = 1.0
-TIME_OBJECTIVE_WEIGHT = 1.0
+ACCEPTABLE_COST_THRESHOLD = 0.85
+WIND_OBJECTIVE_WEIGHT = 0.85
+TIME_OBJECTIVE_WEIGHT = 0.15
 
 
 #               Estimated Boat Speeds (kmph) as function of True Wind Speed (kmph)
@@ -109,9 +109,6 @@ def wind_direction_cost(s1: cs.XY, s2: cs.XY, tw_direction_rad: float) -> float:
         tw_angle_rad = abs(wcs.get_true_wind_angle(segment_true_bearing_rad, tw_direction_rad))
         cos_angle = math.cos(tw_angle_rad)
 
-        # The target point of sail (POS) is a beam reach for max speed and stability
-        # A beam reach is when the true wind direction is perpendicular to the heading of the boat
-        # That is why we assign the min cost of 0 to any segment that corresponds to a beam reach
         if cos_angle > 0:
             return UPWIND_COST_MULTIPLIER * cos_angle
         else: