From 58cdc2fc6cdbbf0640852b158f14dbf25d9a3bf4 Mon Sep 17 00:00:00 2001 From: Krithik Date: Tue, 13 Feb 2024 20:12:36 -0800 Subject: [PATCH 1/3] Made changes to calculate current heading. --- local_pathfinding/node_navigate.py | 21 +++++++++++++++++++-- 1 file changed, 19 insertions(+), 2 deletions(-) diff --git a/local_pathfinding/node_navigate.py b/local_pathfinding/node_navigate.py index 9c00d59..6321d3f 100644 --- a/local_pathfinding/node_navigate.py +++ b/local_pathfinding/node_navigate.py @@ -1,9 +1,11 @@ """The main node of the local_pathfinding package, represented by the `Sailbot` class.""" -import custom_interfaces.msg as ci +import math + import rclpy from rclpy.node import Node +import custom_interfaces.msg as ci from local_pathfinding.local_path import LocalPath @@ -176,7 +178,22 @@ def get_desired_heading(self) -> float: ) # TODO: create function to compute the heading from current position to next local waypoint - return 0.0 + # Find index of self.gps in current waypoints + current_waypoint = self.gps.lat_lon + next_waypoint = None + waypoints = self.local_path.waypoints + for i in range(len(waypoints)): + if waypoints[i] == current_waypoint and i != len(waypoints) - 1: + next_waypoint = waypoints[i+1] + break + if next_waypoint is None: + return 0.0 + heading = math.atan2( + (next_waypoint.longitude-current_waypoint.longitude) * + math.cos(current_waypoint.latitude), + (next_waypoint.latitude-current_waypoint.latitude) + ) + return heading def update_params(self): """Update instance variables that depend on parameters if they have changed.""" From 8788e40e78c4723172a7f5136127dde38abba333 Mon Sep 17 00:00:00 2001 From: Krithik Date: Mon, 19 Feb 2024 23:31:30 -0800 Subject: [PATCH 2/3] Changes according to PR comments --- local_pathfinding/node_navigate.py | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/local_pathfinding/node_navigate.py b/local_pathfinding/node_navigate.py index 6321d3f..2e7426a 100644 --- a/local_pathfinding/node_navigate.py +++ b/local_pathfinding/node_navigate.py @@ -1,13 +1,14 @@ """The main node of the local_pathfinding package, represented by the `Sailbot` class.""" -import math - import rclpy +from pyproj import Geod from rclpy.node import Node import custom_interfaces.msg as ci from local_pathfinding.local_path import LocalPath +GEODESIC = Geod(ellps="WGS84") + def main(args=None): rclpy.init(args=args) @@ -177,22 +178,21 @@ def get_desired_heading(self) -> float: self.gps, self.ais_ships, self.global_path, self.filtered_wind_sensor, self.planner ) - # TODO: create function to compute the heading from current position to next local waypoint - # Find index of self.gps in current waypoints current_waypoint = self.gps.lat_lon next_waypoint = None waypoints = self.local_path.waypoints for i in range(len(waypoints)): - if waypoints[i] == current_waypoint and i != len(waypoints) - 1: - next_waypoint = waypoints[i+1] + if waypoints[i] == current_waypoint and i < len(waypoints) - 1: + next_waypoint = waypoints[i + 1] break if next_waypoint is None: return 0.0 - heading = math.atan2( - (next_waypoint.longitude-current_waypoint.longitude) * - math.cos(current_waypoint.latitude), - (next_waypoint.latitude-current_waypoint.latitude) - ) + heading = GEODESIC.inv( + current_waypoint.longitude, + current_waypoint.latitude, + next_waypoint.longitude, + next_waypoint.latitude, + )[0] return heading def update_params(self): From c7fe3414fb42710d9940d3e4d98d9f219c432708 Mon Sep 17 00:00:00 2001 From: Krithik Date: Mon, 19 Feb 2024 23:43:42 -0800 Subject: [PATCH 3/3] Add named arguments in GEODESIC.inv function --- local_pathfinding/node_navigate.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/local_pathfinding/node_navigate.py b/local_pathfinding/node_navigate.py index df5dfb2..3c19fd2 100644 --- a/local_pathfinding/node_navigate.py +++ b/local_pathfinding/node_navigate.py @@ -188,10 +188,10 @@ def get_desired_heading(self) -> float: if next_waypoint is None: return 0.0 heading = GEODESIC.inv( - current_waypoint.longitude, - current_waypoint.latitude, - next_waypoint.longitude, - next_waypoint.latitude, + lats1=current_waypoint.latitude, + lons1=current_waypoint.longitude, + lats2=next_waypoint.latitude, + lons2=next_waypoint.longitude, )[0] return heading