diff --git a/src/global_launch/config/globals.yaml b/src/global_launch/config/globals.yaml index adfff3a5c..5fe3d0e1e 100644 --- a/src/global_launch/config/globals.yaml +++ b/src/global_launch/config/globals.yaml @@ -3,13 +3,13 @@ ros__parameters: # Publishers' period (seconds): how often the publishers publish pub_period_sec: 1.0 + global_path_interval_spacing_km: 3.0 test_plan: "basic.yaml" # local_pathfinding parameters mock_global_path: ros__parameters: global_path_filepath: "/workspaces/sailbot_workspace/src/local_pathfinding/global_paths/mock_global_path.csv" - interval_spacing: 30.0 write: false gps_threshold: 1.5 force: false diff --git a/src/local_pathfinding/local_pathfinding/mock_nodes/node_mock_global_path.py b/src/local_pathfinding/local_pathfinding/mock_nodes/node_mock_global_path.py index 792da3731..3c69aaf47 100644 --- a/src/local_pathfinding/local_pathfinding/mock_nodes/node_mock_global_path.py +++ b/src/local_pathfinding/local_pathfinding/mock_nodes/node_mock_global_path.py @@ -4,10 +4,10 @@ import os import time -import custom_interfaces.msg as ci import rclpy from rclpy.node import Node +import custom_interfaces.msg as ci import local_pathfinding.coord_systems as cs import local_pathfinding.global_path as gp @@ -49,7 +49,7 @@ def __init__(self): namespace="", parameters=[ ("global_path_filepath", rclpy.Parameter.Type.STRING), - ("interval_spacing", rclpy.Parameter.Type.DOUBLE), + ("global_path_interval_spacing_km", rclpy.Parameter.Type.DOUBLE), ("write", rclpy.Parameter.Type.BOOL), ("gps_threshold", rclpy.Parameter.Type.DOUBLE), ("force", rclpy.Parameter.Type.BOOL), @@ -115,7 +115,7 @@ def global_path_callback(self, gps: ci.GPS): # we need to obtain the actual distances between every waypoint in the global path path_spacing = gp.calculate_interval_spacing(pos, global_path.waypoints) - interval_spacing = self.get_parameter("interval_spacing")._value + interval_spacing = self.get_parameter("global_path_interval_spacing_km")._value # this checks if global path is just a destination point if len(global_path.waypoints) < 2: @@ -187,7 +187,7 @@ def check_pos(self, gps: ci.GPS): )[2] ) gps_threshold = self.get_parameter("gps_threshold")._value - interval_spacing = self.get_parameter("interval_spacing")._value + interval_spacing = self.get_parameter("global_path_interval_spacing_km")._value if position_delta > gps_threshold * interval_spacing: self.get_logger().debug( diff --git a/src/local_pathfinding/local_pathfinding/mock_nodes/wind_params.yaml b/src/local_pathfinding/local_pathfinding/mock_nodes/wind_params.yaml index 3378a2fdf..4c773b8d5 100644 --- a/src/local_pathfinding/local_pathfinding/mock_nodes/wind_params.yaml +++ b/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 diff --git a/src/local_pathfinding/local_pathfinding/node_navigate.py b/src/local_pathfinding/local_pathfinding/node_navigate.py index 741069cfe..fbdaa6fc3 100644 --- a/src/local_pathfinding/local_pathfinding/node_navigate.py +++ b/src/local_pathfinding/local_pathfinding/node_navigate.py @@ -1,18 +1,17 @@ """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 REALLY_FAR_M = 100000000 @@ -68,6 +67,7 @@ def __init__(self): ("pub_period_sec", rclpy.Parameter.Type.DOUBLE), ("path_planner", rclpy.Parameter.Type.STRING), ("test_plan", rclpy.Parameter.Type.STRING), + ("global_path_interval_spacing_km", rclpy.Parameter.Type.DOUBLE), ], ) @@ -106,6 +106,11 @@ def __init__(self): self.pub_period_sec = ( self.get_parameter("pub_period_sec").get_parameter_value().double_value ) + self.global_path_interval_spacing_km = ( + self.get_parameter("global_path_interval_spacing_km") + .get_parameter_value() + .double_value + ) self.get_logger().debug(f"Got parameter: {self.pub_period_sec=}") # we need to give the solver time to run and the callback to return before calling again @@ -260,7 +265,7 @@ def get_desired_heading(self) -> float: ): received_new_global_waypoint = True self.global_waypoint_index = self.determine_start_point_in_new_global_path( - self.global_path, self.gps.lat_lon + self.global_path, self.gps.lat_lon, self.global_path_interval_spacing_km ) self.saved_target_global_waypoint = self.global_path.waypoints[ self.global_waypoint_index @@ -300,7 +305,7 @@ def get_desired_heading(self) -> float: @staticmethod def determine_start_point_in_new_global_path( - global_path: ci.Path, boat_lat_lon: ci.HelperLatLon + global_path: ci.Path, boat_lat_lon: ci.HelperLatLon, global_path_spacing_km: float ): """Used when we receive a new global path. Finds the index of the first waypoint within 'pathfinding range' of gps location. @@ -321,7 +326,7 @@ def determine_start_point_in_new_global_path( if distance_to_waypoint_m < closest_m: closest_m, index_of_closest = distance_to_waypoint_m, waypoint_index - if distance_to_waypoint_m < cs.km_to_meters(PATHFINDING_RANGE_KM): + if distance_to_waypoint_m < cs.km_to_meters(global_path_spacing_km): return waypoint_index return index_of_closest diff --git a/src/local_pathfinding/local_pathfinding/ompl_path.py b/src/local_pathfinding/local_pathfinding/ompl_path.py index 4744e27ef..bb61cf4ce 100644 --- a/src/local_pathfinding/local_pathfinding/ompl_path.py +++ b/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,7 +29,7 @@ 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 @@ -37,7 +37,7 @@ # 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, target_lp_wp_index: int, boat_lat_lon: ci.HelperLat # 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: diff --git a/src/local_pathfinding/test/test_node_navigate.py b/src/local_pathfinding/test/test_node_navigate.py index 13d60b78a..51ad2d766 100644 --- a/src/local_pathfinding/test/test_node_navigate.py +++ b/src/local_pathfinding/test/test_node_navigate.py @@ -1,14 +1,19 @@ -import custom_interfaces.msg as ci +import os + import pytest +import yaml +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 +with open(os.getcwd() + "/../global_launch/config/globals.yaml", "r") as f: + config = yaml.safe_load(f) +GLOBAL_PATH_SPACING_KM = config["/**"]["ros__parameters"]["global_path_interval_spacing_km"] +PATH_RANGE_DEG = GLOBAL_PATH_SPACING_KM / ONE_DEGREE_KM @pytest.mark.parametrize( @@ -133,6 +138,6 @@ def test_find_next_global_waypoint_index( global_path: ci.Path, boat_lat_lon: ci.HelperLatLon, correct_index: int ): calculated_answer = nn.Sailbot.determine_start_point_in_new_global_path( - global_path, boat_lat_lon + global_path, boat_lat_lon, GLOBAL_PATH_SPACING_KM ) assert calculated_answer == correct_index diff --git a/src/local_pathfinding/test/test_ompl_path.py b/src/local_pathfinding/test/test_ompl_path.py index de39c4ac7..53a01a5fa 100644 --- a/src/local_pathfinding/test/test_ompl_path.py +++ b/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(1, 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( @@ -308,10 +306,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, + ) + ), + longitude=( + random.uniform( + start_latlon.longitude + end_points_inclusive_factor, + end_latlon.longitude - end_points_inclusive_factor, + ) + ), # noqa ) boat_latlon = mid_point(current_wp_latlon, next_wp_latlon) @@ -326,9 +332,9 @@ def mid_point(start_latlon: HelperLatLon, end_latlon: HelperLatLon): cost_from_next_wp = fresh_ompl_path.get_remaining_cost(target_wp_index + 1, next_wp_latlon) full_cost = fresh_ompl_path.get_cost() - assert cost <= full_cost, ( - f"Remaining cost {cost} should be less than or equal to full cost {full_cost}" - ) + assert ( + cost <= full_cost + ), f"Remaining cost {cost} should be less than or equal to full cost {full_cost}" assert cost > cost_from_next_wp, ( f"Cost from waypoint {target_wp_index} ({cost}) should be greater than " f"cost from waypoint {target_wp_index + 1} ({cost_from_next_wp})" @@ -358,9 +364,9 @@ def test_get_remaining_cost_no_partial(fresh_ompl_path, target_wp_index): cost_from_next_wp = fresh_ompl_path.get_remaining_cost(target_wp_index + 1, next_wp_latlon) full_cost = fresh_ompl_path.get_cost() - assert cost <= full_cost, ( - f"Remaining cost {cost} should be less than or equal to full cost {full_cost}" - ) + assert ( + cost <= full_cost + ), f"Remaining cost {cost} should be less than or equal to full cost {full_cost}" assert cost > cost_from_next_wp, ( f"Cost from waypoint {target_wp_index} ({cost}) should be greater than " f"cost from waypoint {target_wp_index + 1} ({cost_from_next_wp})"