From 03d81c85a6cbd0b31e0627ad57c6c82ef6ab8a76 Mon Sep 17 00:00:00 2001
From: Vaibhav Hariani <vaibhav.hariani@gmail.com>
Date: Sun, 2 Jun 2024 10:54:36 -0400
Subject: [PATCH] Fixed a bug with slope in lane_follower, prevented individual
 follower from not returning a slope, and added a constant for the minimum
 safe braking distance.

---
 ros2/cmd/src/modules/State_Machine_Interface.py |  3 +++
 .../lane_behaviors/individual_follower.py       |  2 +-
 .../src/modules/lane_behaviors/lane_follower.py | 17 ++++-------------
 3 files changed, 8 insertions(+), 14 deletions(-)

diff --git a/ros2/cmd/src/modules/State_Machine_Interface.py b/ros2/cmd/src/modules/State_Machine_Interface.py
index b28dace92..e3a8c393d 100644
--- a/ros2/cmd/src/modules/State_Machine_Interface.py
+++ b/ros2/cmd/src/modules/State_Machine_Interface.py
@@ -11,6 +11,9 @@
 
 class Interface(Node):
     LANE_WIDTH = 3.048  # METERS
+    MIN_SAFE_BRAKING_DIST = 2.2 
+    # Given that we are moving at 5 mph (2.35 m/s)
+    # We should start braking at least x meters before our end goal, given the safe braking decel Jeanette has specified.
 
     def __init__(
         self,
diff --git a/ros2/cmd/src/modules/lane_behaviors/individual_follower.py b/ros2/cmd/src/modules/lane_behaviors/individual_follower.py
index ba12e56b5..b2fa9a00c 100644
--- a/ros2/cmd/src/modules/lane_behaviors/individual_follower.py
+++ b/ros2/cmd/src/modules/lane_behaviors/individual_follower.py
@@ -89,7 +89,7 @@ def Plot_Line(self, smoothen=False, prevFrameCount=6):
         if x_pos.any() and y_pos.any():
             self._fit = np.polyfit(y_pos, x_pos, 2)
         else:
-            return None, empty_windows, 0
+            return None, empty_windows, 0, 0
 
         ploty = np.linspace(
             0, self._binary_warped.shape[0] - 1, self._binary_warped.shape[0]
diff --git a/ros2/cmd/src/modules/lane_behaviors/lane_follower.py b/ros2/cmd/src/modules/lane_behaviors/lane_follower.py
index e01274a2b..04e34a8f3 100644
--- a/ros2/cmd/src/modules/lane_behaviors/lane_follower.py
+++ b/ros2/cmd/src/modules/lane_behaviors/lane_follower.py
@@ -230,12 +230,12 @@ def timer_callback(self):
 
         # Setting the slopes if there is enough relevant data: If not, we discard and assume what we're seeing is noise
         self.left_slope = (
-            left_slope * LaneFollower.PIXELS_TO_METERS
+            left_slope / LaneFollower.PIXELS_TO_METERS
             if empty_left > LaneFollower.EMPTY_WINDOWS_THRESHOLD
             else self.left_slope
         )
         self.right_slope = (
-            right_slope * LaneFollower.PIXELS_TO_METERS
+            right_slope / LaneFollower.PIXELS_TO_METERS
             if empty_right > LaneFollower.EMPTY_WINDOWS_THRESHOLD
             else self.right_slope
         )
@@ -264,17 +264,8 @@ def timer_callback(self):
             self._Left_Lane = (
                 False if empty_left - 2 > empty_right else self._Left_Lane
             )
-            # The slope calculations are returned in meters, must be adjusted.
-            # main_msg.leftslope = left_slope / LaneFollower.PIXELS_TO_METERS
-            # main_msg.rightslope = right_slope / LaneFollower.PIXELS_TO_METERS
-            if self._Left_Lane:
-                self.heading_error = left_heading
-                # main_msg.leftlane = True
-                # main_msg.he = left_heading
-            else:
-                self.heading_error = right_heading
-                # main_msg.leftlane = False
-                # main_msg.he = right_heading
+            
+            self.heading_error = left_heading if self._Left_Lane else right_heading
 
         else:
             self._tolerance += 1