diff --git a/src/local_pathfinding/local_pathfinding/local_path.py b/src/local_pathfinding/local_pathfinding/local_path.py index 75551d21c..cc7541555 100644 --- a/src/local_pathfinding/local_pathfinding/local_path.py +++ b/src/local_pathfinding/local_pathfinding/local_path.py @@ -47,6 +47,8 @@ def __init__( HelperLatLon(latitude=waypoint.latitude, longitude=waypoint.longitude) for waypoint in global_path.waypoints ] + else: + self.global_path = global_path if filtered_wind_sensor: # TODO: remove when mock can be run self.wind_speed = filtered_wind_sensor.speed.speed diff --git a/src/local_pathfinding/local_pathfinding/ompl_path.py b/src/local_pathfinding/local_pathfinding/ompl_path.py index 37d52a62b..559965bfe 100644 --- a/src/local_pathfinding/local_pathfinding/ompl_path.py +++ b/src/local_pathfinding/local_pathfinding/ompl_path.py @@ -5,17 +5,18 @@ won't work). The C++ API is documented on the OMPL website: https://ompl.kavrakilab.org/api_overview.html. """ + from __future__ import annotations from typing import TYPE_CHECKING, List, Tuple, Type -from custom_interfaces.msg import HelperLatLon from ompl import base as ob from ompl import geometric as og from ompl import util as ou from rclpy.impl.rcutils_logger import RcutilsLogger import local_pathfinding.coord_systems as cs +from custom_interfaces.msg import HelperLatLon from local_pathfinding.objectives import get_sailing_objective if TYPE_CHECKING: @@ -39,7 +40,7 @@ def __init__(self, local_path_state: LocalPathState, logger: RcutilsLogger): self.reference_latlon = ( local_path_state.global_path[-1] - if local_path_state and len(local_path_state.global_path) > 0 + if local_path_state.global_path and len(local_path_state.global_path) > 0 else HelperLatLon(latitude=0.0, longitude=0.0) ) diff --git a/src/local_pathfinding/test/test_ompl_path.py b/src/local_pathfinding/test/test_ompl_path.py index 73013c310..434037344 100644 --- a/src/local_pathfinding/test/test_ompl_path.py +++ b/src/local_pathfinding/test/test_ompl_path.py @@ -21,7 +21,14 @@ def test_OMPLPathState(): - state = ompl_path.OMPLPathState(local_path_state=None, logger=RcutilsLogger()) + local_path_state = LocalPathState( + gps=GPS(), + ais_ships=AISShips(), + global_path=Path(), + filtered_wind_sensor=WindSensor(), + planner="rrtstar", + ) + state = ompl_path.OMPLPathState(local_path_state, logger=RcutilsLogger()) assert state.state_domain == (-1, 1), "incorrect value for attribute state_domain" assert state.state_range == (-1, 1), "incorrect value for attribute start_state" assert state.start_state == pytest.approx(