Fixed a bug with slope in lane_follower, prevented individual followe…

…r from not returning a slope, and added a constant for the minimum safe braking distance.
CooperUnion · Jun 2, 2024 · 03d81c8 · 03d81c8
1 parent 1c49144
commit 03d81c8
Show file tree

Hide file tree

Showing 3 changed files with 8 additions and 14 deletions.
diff --git 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
@@ -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
@@ -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