Finish STATIC errors for nav, probably a bunch of runtime errors

config/navigation.yaml

@@ -9,6 +9,15 @@ navigation:
   # How many state machine iterations per second
   update_rate: 60.0
 
+  world_frame: "map"
+  rover_frame: "base_link"
+
+  gps_linearization:
+    # Exception, these are in degrees
+    ref_lat: 38.4225202
+    ref_long: -110.7844653
+    ref_alt: 0.0
+
   # How long it takes to forget about a target
   # Useful if the rover turns away from the target for a brief moment
   # Or if there was an ephemeral false positive detection
@@ -20,13 +29,24 @@ navigation:
     min_hits: 3 # TODO(quintin): This is not used, see corresponding comment in context.py
     max_hits: 10
 
-  world_frame: "map"
-  rover_frame: "base_link"
+  drive:
+    max_driving_effort: 0.7
+    min_driving_effort: -0.7
+    max_turning_effort: 1.2
+    min_turning_effort: -1.2
+    turning_p: 3.0
+    driving_p: 20.0
+    lookahead_distance: 1.0
 
-  gps_linearization:
-    reference_point_latitude: 38.4225202
-    reference_point_longitude: -110.7844653
-    reference_point_altitude: 0.0
+  single_tag:
+    stop_threshold: 1.0
+    tag_stop_threshold: 1.75
+    post_avoidance_multiplier: 1.42
+    post_radius: 0.7
+
+  waypoint:
+    stop_threshold: 0.5
+    drive_forward_threshold: 0.34
 
 stuck_detector:
   # How many readings to consider the rover stuck

geometry/__init__.py

@@ -1,8 +1,28 @@
-from manifpy import SE3
+import numpy as np
+from manifpy import SE3, SO2
 
 from .conversions import from_position_orientation, from_tf_tree, to_tf_tree
 
 SE3.from_position_orientation = classmethod(from_position_orientation)
 SE3.from_tf_tree = classmethod(from_tf_tree)
 SE3.to_tf_tree = classmethod(to_tf_tree)
 
+
+def normalized(v: np.ndarray) -> np.ndarray:
+    return v / np.linalg.norm(v)
+
+
+def angle_to_rotate(v1: np.ndarray, v2: np.ndarray) -> float:
+    (v2i, v1j), (v2i, v2j) = normalized(v1), normalized(v2)
+    o1, o2 = SO2(v2i, v1j), SO2(v2i, v2j)
+    (angle,) = (o2 - o1).coeffs()
+    return angle
+
+
+def perpendicular_2d(v: np.ndarray) -> np.ndarray:
+    assert v.shape == (2,) or v.shape == (1, 2) or v.shape == (2, 1)
+
+    # If you are curious, this can be thought of as the 2D cross product
+    # The best theory for this is geometric algebra
+    x, y = v.flatten()
+    return np.array([-y, x]).reshape(v.shape)

msg/GPSPointList.msg

@@ -1 +1 @@
-GPSWaypoint[] point
+GPSWaypoint[] points

navigation/approach_target.py

@@ -1,5 +1,3 @@
-from typing import Optional
-
 import numpy as np
 
 from state_machine.state import State
@@ -14,7 +12,7 @@ def on_enter(self, context: Context) -> None:
     def on_exit(self, context: Context) -> None:
         pass
 
-    def get_target_position(self, context: Context) -> Optional[np.ndarray]:
+    def get_target_position(self, context: Context) -> np.ndarray | None:
         return context.env.current_target_pos()
 
     def determine_next(self, context: Context, is_finished: bool) -> State:
@@ -48,7 +46,7 @@ def on_loop(self, context: Context) -> State:
         rover_in_map = context.rover.get_pose_in_map()
         assert rover_in_map is not None
 
-        cmd_vel, has_arrived = context.rover.driver.get_drive_command(
+        cmd_vel, has_arrived = context.drive.get_drive_command(
             target_position,
             rover_in_map,
             context.node.get_parameter("single_tag/stop_threshold").get_parameter_value().double_value,

navigation/context.py

@@ -4,9 +4,9 @@
 
 import numpy as np
 import pymap3d
-from manifpy import SE3
 
 import tf2_ros
+from geometry import SE3
 from geometry_msgs.msg import Twist
 from mrover.msg import (
     Waypoint,
@@ -31,28 +31,13 @@
 
 NO_TAG: int = -1
 
-# TARGET_EXPIRATION_DURATION = Duration(60)
-
-# LONG_RANGE_EXPIRATION_DURATION = rospy.Duration(rospy.get_param("long_range/time_threshold"))
-# INCREMENT_WEIGHT = rospy.get_param("long_range/increment_weight")
-# DECREMENT_WEIGHT = rospy.get_param("long_range/decrement_weight")
-# MIN_HITS = rospy.get_param("long_range/min_hits")
-# MAX_HITS = rospy.get_param("long_range/max_hits")
-#
-# REF_LAT = rospy.get_param("gps_linearization/reference_point_latitude")
-# REF_LON = rospy.get_param("gps_linearization/reference_point_longitude")
-# REF_ALT = rospy.get_param("gps_linearization/reference_point_altitude")
-#
-# tf_broadcaster: tf2_ros.StaticTransformBroadcaster = tf2_ros.StaticTransformBroadcaster()
-
 
 @dataclass
 class Rover:
     ctx: Context
     stuck: bool
     previous_state: State
     path_history: Path = Path(header=Header(frame_id="map"))
-    driver: DriveController = DriveController()
 
     def get_pose_in_map(self) -> SE3 | None:
         try:
@@ -337,6 +322,7 @@ class Context:
     # Use these as the primary interfaces in states
     course: Course | None
     rover: Rover
+    drive: DriveController
     env: Environment
     disable_requested: bool
 
@@ -348,6 +334,7 @@ def setup(self, node: Node):
         from .state import OffState
 
         self.node = node
+        self.drive = DriveController(node)
 
         self.course = None
         self.rover = Rover(self, False, OffState(), Path())
@@ -370,11 +357,16 @@ def setup(self, node: Node):
         tf2_ros.TransformListener(self.tf_buffer, node)
 
     def recv_enable_auton(self, request: EnableAuton.Request, response: EnableAuton.Response) -> EnableAuton.Response:
-        ref_lat = self.node.get_parameter("gps_linearization/reference_point_latitude").get_parameter_value()
-        ref_long = self.node.get_parameter("gps_linearization/reference_point_longitude").get_parameter_value()
-        ref_alt = self.node.get_parameter("gps_linearization/reference_point_altitude").get_parameter_value()
+
         if request.enable:
-            self.course = convert_and_get_course(self, np.array([ref_lat, ref_long, ref_alt]), request)
+            ref = np.array(
+                [
+                    self.node.get_parameter("gps_linearization/ref_lat").get_parameter_value().double_value,
+                    self.node.get_parameter("gps_linearization/ref_long").get_parameter_value().double_value,
+                    self.node.get_parameter("gps_linearization/ref_alt").get_parameter_value().double_value,
+                ]
+            )
+            self.course = convert_and_get_course(self, ref, request)
         else:
             self.disable_requested = True
         response.success = True

navigation/drive.py

@@ -1,37 +1,22 @@
 from __future__ import annotations
 
 from enum import Enum
-from typing import Tuple, Optional
 
 import numpy as np
 
-from geometry_msgs.msg import Twist
+from geometry import SE3, normalized, angle_to_rotate
+from geometry_msgs.msg import Twist, Vector3
 from rclpy import Node
 
 
-# import rospy
-# from geometry_msgs.msg import Twist
-# from util.SE3 import SE3
-# from util.np_utils import angle_to_rotate, normalized
-#
-# MAX_DRIVING_EFFORT_WAYPOINT = rospy.get_param("drive/max_driving_effort_waypoint")
-# MAX_DRIVING_EFFORT = rospy.get_param("drive/max_driving_effort")
-# MIN_DRIVING_EFFORT = rospy.get_param("drive/min_driving_effort")
-# MAX_TURNING_EFFORT = rospy.get_param("drive/max_turning_effort")
-# MIN_TURNING_EFFORT = rospy.get_param("drive/min_turning_effort")
-# TURNING_P = rospy.get_param("drive/turning_p")
-# DRIVING_P = rospy.get_param("drive/driving_p")
-# LOOKAHEAD_DISTANCE = rospy.get_param("drive/lookahead_distance")
-
-
 class DriveController:
     class DriveMode(Enum):
         TURN_IN_PLACE = 1
         DRIVE_FORWARD = 2
         STOPPED = 3
 
-    _last_angular_error: Optional[float] = None
-    _last_target: Optional[np.ndarray] = None
+    _last_angular_error: float | None = None
+    _last_target: np.ndarray | None = None
     _driver_state: DriveMode = DriveMode.STOPPED
 
     def __init__(self, node: Node):
@@ -47,7 +32,6 @@ def _get_state_machine_output(
         linear_error: float,
         completion_thresh: float,
         turn_in_place_thresh: float,
-        waypoint_state: bool,
     ) -> tuple[Twist, bool]:
         """
         Gets the state machine output for a given angular and linear error.
@@ -58,6 +42,12 @@ def _get_state_machine_output(
         :return: a tuple of the command to send to the rover and a boolean indicating whether we are at the target
         :modifies: self._driver_state
         """
+        turning_p = self.node.get_parameter("drive/turning_p").get_parameter_value().double_value
+        driving_p = self.node.get_parameter("drive/driving_p").get_parameter_value().double_value
+        min_turning_effort = self.node.get_parameter("drive/min_turning_effort").get_parameter_value().double_value
+        max_turning_effort = self.node.get_parameter("drive/max_turning_effort").get_parameter_value().double_value
+        min_driving_effort = self.node.get_parameter("drive/min_driving_effort").get_parameter_value().double_value
+        max_driving_effort = self.node.get_parameter("drive/max_driving_effort").get_parameter_value().double_value
 
         # if we are at the target position, reset the controller and return a zero command
         if abs(linear_error) < completion_thresh:
@@ -86,9 +76,9 @@ def _get_state_machine_output(
 
             # if neither of those things are true, we need to turn in place towards our target heading, so set the z component of the output Twist message
             else:
-                cmd_vel = Twist()
-                turning_p =
-                cmd_vel.angular.z = np.clip(angular_error * TURNING_P, MIN_TURNING_EFFORT, MAX_TURNING_EFFORT)
+                cmd_vel = Twist(
+                    angular=Vector3(z=np.clip(angular_error * turning_p, min_turning_effort, max_turning_effort))
+                )
                 return cmd_vel, False
 
         elif self._driver_state == self.DriveMode.DRIVE_FORWARD:
@@ -107,14 +97,10 @@ def _get_state_machine_output(
                 return Twist(), False
             # otherwise we compute a drive command with both a linear and angular component in the Twist message
             else:
-                cmd_vel = Twist()
-                if waypoint_state:
-                    cmd_vel.linear.x = np.clip(
-                        linear_error * DRIVING_P, MIN_DRIVING_EFFORT, MAX_DRIVING_EFFORT_WAYPOINT
-                    )
-                else:
-                    cmd_vel.linear.x = np.clip(linear_error * DRIVING_P, MIN_DRIVING_EFFORT, MAX_DRIVING_EFFORT)
-                cmd_vel.angular.z = np.clip(angular_error * TURNING_P, MIN_TURNING_EFFORT, MAX_TURNING_EFFORT)
+                cmd_vel = Twist(
+                    linear=Vector3(x=np.clip(linear_error * driving_p, min_driving_effort, max_driving_effort)),
+                    angular=Vector3(z=np.clip(angular_error * turning_p, min_turning_effort, max_turning_effort)),
+                )
                 return cmd_vel, False
         else:
             raise ValueError(f"Invalid drive state {self._driver_state}")
@@ -136,20 +122,19 @@ def compute_lookahead_point(
         :param lookahead_dist: the distance to look ahead on the path
         :return: the lookahead point
         """
-
         # Compute the vector from the previous target position to the current target position
         path_vec = path_end - path_start
         # compute the vector from the previous target position to the rover position
         rover_vec = rover_pos - path_start
         # compute the projection of the rover vector onto the target vector
         proj_vec = np.dot(rover_vec, path_vec) / np.dot(path_vec, path_vec) * path_vec
         lookahead_vec = lookahead_dist * normalized(path_vec)
-        lookahead_pt = proj_vec + lookahead_vec
+        lookahead_point = proj_vec + lookahead_vec
         # if the lookahead point is further away than the target, just return the target
-        if np.linalg.norm(lookahead_pt) > np.linalg.norm(path_vec):
+        if np.linalg.norm(lookahead_point) > np.linalg.norm(path_vec):
             return path_end
         else:
-            return path_start + lookahead_pt
+            return path_start + lookahead_point
 
     def get_drive_command(
         self: DriveController,
@@ -158,8 +143,7 @@ def get_drive_command(
         completion_thresh: float,
         turn_in_place_thresh: float,
         drive_back: bool = False,
-        path_start: Optional[np.ndarray] = None,
-        waypoint_state: bool = False,
+        path_start: np.ndarray | None = None,
     ) -> tuple[Twist, bool]:
         """
         Returns a drive command to get the rover to the target position, calls the state machine to do so and updates the last angular error in the process
@@ -169,13 +153,13 @@ def get_drive_command(
         :param turn_in_place_thresh: The angle threshold to consider the rover facing the target position and ready to drive forward towards it.
         :param drive_back: True if rover should drive backwards, false otherwise.
         :param path_start: If you want the rover to drive on a line segment (and actively try to stay on the line), pass the start of the line segment as this param, otherwise pass None.
-        :param waypoint_state:
         :return: A tuple of the drive command and a boolean indicating whether the rover is at the target position.
         :modifies: self._last_angular_error
         """
 
-        # Get the direction vector of the rover and the target position, zero the Z components of both since our controller only assumes motion and control over the Rover in the XY plane
-        rover_dir = rover_pose.rotation.direction_vector()
+        # Get the direction vector of the rover and the target position,
+        # zero the Z components since our controller only assumes motion and control over the rover in the XY plane
+        rover_dir = rover_pose.rotation()[:, 0]
         rover_dir[2] = 0
 
         if drive_back:
@@ -189,8 +173,9 @@ def get_drive_command(
             self.reset()
             return Twist(), True
 
-        if path_start is not None and np.linalg.norm(path_start - target_pos) > LOOKAHEAD_DISTANCE:
-            target_pos = self.compute_lookahead_point(path_start, target_pos, rover_pos, LOOKAHEAD_DISTANCE)
+        lookahead_distance = self.node.get_parameter("drive/lookahead_distance").get_parameter_value().double_value
+        if path_start is not None and np.linalg.norm(path_start - target_pos) > lookahead_distance:
+            target_pos = self.compute_lookahead_point(path_start, target_pos, rover_pos, lookahead_distance)
 
         target_dir = target_pos - rover_pos
 
@@ -203,7 +188,10 @@ def get_drive_command(
         angular_error = angle_to_rotate(rover_dir, target_dir)
 
         output = self._get_state_machine_output(
-            angular_error, linear_error, completion_thresh, turn_in_place_thresh, waypoint_state
+            angular_error,
+            linear_error,
+            completion_thresh,
+            turn_in_place_thresh,
         )
 
         if drive_back: