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 1 commit
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,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


Expand Down Expand Up @@ -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
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:
Krithik1 marked this conversation as resolved.
Show resolved Hide resolved
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)
)
Krithik1 marked this conversation as resolved.
Show resolved Hide resolved
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