Skip to content
This repository has been archived by the owner on Mar 23, 2024. It is now read-only.

Calculates current heading #88

Open
wants to merge 5 commits into
base: main
Choose a base branch
from
Open

Calculates current heading #88

Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
58cdc2f
Made changes to calculate current heading.
Krithik1 Feb 14, 2024
8788e40
Changes according to PR comments
Krithik1 Feb 20, 2024
2944157
Merge remote-tracking branch 'origin' into user/Krithik1/Publish_Desi…
Krithik1 Feb 20, 2024
c7fe341
Add named arguments in GEODESIC.inv function
Krithik1 Feb 20, 2024
052962f
Merge branch 'main' into user/Krithik1/Publish_Desired_Heading
patrick-5546 Mar 9, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
21 changes: 19 additions & 2 deletions local_pathfinding/node_navigate.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,11 +1,14 @@
"""The main node of the local_pathfinding package, represented by the `Sailbot` class."""

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)
Expand Down Expand Up @@ -175,8 +178,22 @@ def get_desired_heading(self) -> float:
self.gps, self.ais_ships, self.global_path, self.filtered_wind_sensor, self.planner
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is about line 175:

The function description says we should return something that violates the heading convention in the case of an error, but the heading convention allows $-180 \degree < \text{heading} \leq 180 \degree$.

Can you please change the return value on line 175 to something outside those bounds? I'd suggest 404 since that's a common error number for websites haha

)

# TODO: create function to compute the heading from current position to next local waypoint
return 0.0
current_waypoint = self.gps.lat_lon
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I believe self.gps.lat_lon is the current GPS position of the boat, which is generally not one of the waypoints. I think Jay might be working on code to determine the next local waypoint, but we can sync up with him tomorrow

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 = GEODESIC.inv(
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think GEODESIC.inv() has multiple returns (documentation), and I think you only want the forward azimuth (image at bottom)

I believe Patrick is a fan of this kind of syntax for this situation: _, value, _ = myfunc(), where myfunc() returns a touple:

def myfunc():
	return (1, 2, 3)

In this example, print(value) outputs 2.

image

lats1=current_waypoint.latitude,
lons1=current_waypoint.longitude,
lats2=next_waypoint.latitude,
lons2=next_waypoint.longitude,
)[0]
return heading
Krithik1 marked this conversation as resolved.
Show resolved Hide resolved
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Would it be possible to add some tests for this function?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

For the tests, should I create a new test_node_navigate.py file and manually set the value of self.gps to calculate the desired heading?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think this would be the best way to test this:

  1. Create a helper function to get_desired_heading() that accepts "simple" arguments like waypoints or lists of waypoints, rather than "complicated" arguments like self (which is a whole sailbot object). get_desired_heading() will basically just call the helper function.
  2. Create test_node_navigate.py like you suggested, and just test the helper function. I'd suggest using simple tests like when the desired heading is $0 \degree$ or $90 \degree$, and maybe some more complicated tests where the "actual" value is computed with geodesic.inv() as well.


def update_params(self):
"""Update instance variables that depend on parameters if they have changed."""
Expand Down