Testing Changes for new camera positions.

UI/Homepage.py

@@ -14,16 +14,17 @@ def __init__(self):
         self.thread: threading.Thread
 
     def __enter__(self):
-        """ Updates the image """
+        """Updates the image"""
         self.checkbox = st.checkbox("Render State Machine")
+
         @with_streamlit_context
         def update_image() -> None:
             """
             Update the progress bar asynchronously.
             """
             container = st.empty()
             while self.checkbox:
-                container.image("/home/eeadmin/selfdrive/live_SM.png")
+                container.image("/home/autonomylab/selfdrive/live_SM.png")
 
         self.thread = threading.Thread(target=update_image)
         self.thread.start()
@@ -47,27 +48,32 @@ def stop(self) -> None:
 def runner():
 
     st.header("Welcome to the IGVC Homepage!")
-    st.markdown('''
-             Please select an option on the sidebar to the right: This specifies which UI file you'd like to run   
+    st.markdown(
+        '''
+             Please select an option on the sidebar to the right: This specifies which UI file you'd like to run
 
 
              Streamlit supports fast reloads: Just refresh the python script to get going!
 
 
              Docs are here: https://docs.streamlit.io/library/api-reference
-             ''')
+             '''
+    )
     st.divider()
     with Image_Handler():
         pass
 
+
 if __name__ == "__main__":
     if runtime.exists():
-        st.set_page_config(
-            page_title="IGVC Homepage",
-            page_icon="🚗")
+        st.set_page_config(page_title="IGVC Homepage", page_icon="🚗")
         sidebar()
         runner()
     else:
-        sys.argv = ["streamlit", "run",
-                    "--client.showSidebarNavigation=False", sys.argv[0]]
+        sys.argv = [
+            "streamlit",
+            "run",
+            "--client.showSidebarNavigation=False",
+            sys.argv[0],
+        ]
         sys.exit(stcli.main())

ros2/cmd/src/modules/State_Machine_Interface.py

@@ -88,7 +88,7 @@ def Lane_Follow_Action(self, args=None):
             self.car_sm.Emergency_Trigger()
             self.Run()  # ESTOP if we lose the lane lines in our vision
 
-        [steer_cmd, vel_cmd] = self.lane_follow.follow_lane(1 / 200)
+        [steer_cmd, vel_cmd] = self.lane_follow.follow_lane(1 / 40)
         # Publish Lane Following command
         cmd.data = [
             steer_cmd,
@@ -152,7 +152,8 @@ def Cstop_Action(self, args=None):
     # hard estop is HARD
     def Estop_Action(self, error="Entered Error State", args=None):
         print("ESTOP REACHED")
-        slope = 2.0
+        # slope = 2.0
+        slope = 1.32
         # Args[0] is a "soft" estop: We aren't in a physical emergency, but something has gone wrong.
         if args is not None and args[0]:
             slope = 1.32
@@ -267,7 +268,7 @@ def Run(self, args=None):
             "Cstop": self.Cstop_Action,
             "Estop": self.Estop_Action,
         }
-        function_dict[self.car_SM.current_state.id](args=args)
+        function_dict[self.car_sm.current_state.id](args=args)
 
 
 class State_Machine_Failure(Exception):

ros2/cmd/src/modules/lane_behaviors/controller/stanley.py

@@ -99,8 +99,11 @@ def get_steering_cmd(self, heading_error, cross_track_error, v):
 
         vel_sign = math.copysign(1, v)
 
-        steer_angle = scale * heading_error * vel_sign + np.arctan(
-            self.k_stanley * cross_track_error / v
+        steer_angle = (
+            scale * heading_error * vel_sign
+            + np.arctan(self.k_stanley * cross_track_error / v)
+            if v != 0
+            else 0
         )
         delta = np.clip(
             steer_angle, -self.max_steer_angle, self.max_steer_angle

ros2/cmd/src/modules/lane_behaviors/individual_follower.py

@@ -124,15 +124,15 @@ def Plot_Line(self, smoothen=False, prevFrameCount=6):
         # Evaluating heading error:
 
         # This is the first window coordinates
-        y1 = self._binary_warped.shape[0] - (nwindows) * window_height
+        y1 = 0
         # This is the second window coordinates
-        y2 = self._binary_warped.shape[0] - (nwindows + 1) * window_height
+        y2 = (nwindows + 1) * window_height
         # Returns polynomial values at a point
         x1 = np.polyval(self._fit, y1)
         x2 = np.polyval(self._fit, y2)
 
         slope = (x2 - x1) / (y2 - y1)
 
-        heading = math.atan((x2 - x1) / (y2 - y1))  # Radians
+        heading = math.atan(slope)  # Radians
         result = cv2.addWeighted(out_img, 1, window_img, 0.3, 0)
         return result, empty_windows, heading, slope

ros2/cmd/src/modules/lane_behaviors/lane_change.py

@@ -66,7 +66,7 @@ def __init__(
     def create_path(self, relative_x, relative_y, end_yaw):
 
         start_x = self.odom_sub.xpos
-        start_y = self.odom_sub.ypos
+        start_y = self.odom_sub.ypos - 1.5
         print(f'start_x: {start_x}, start_y: {start_y}')
         # Unsure about lead_axle, this position is relative to encoders
         start_yaw = self.odom_sub.yaw

ros2/cmd/src/modules/lane_behaviors/lane_follower.py

@@ -7,7 +7,7 @@
 from lane_behaviors.controller.stanley import StanleyController
 
 from sensor_msgs.msg import Image
-
+from lane_behaviors.odom_sub import OdomSubscriber
 from cv_bridge import CvBridge
 import cv2
 import numpy as np
@@ -19,17 +19,17 @@
 class LaneFollower(Node):
     GUI = True
     # These are upper HSV & lower HSV bounds, respectively
-    (l_h, l_s, l_v) = (120, 0, 240)
+    (l_h, l_s, l_v) = (89, 19, 33)
     (u_h, u_s, u_v) = (255, 255, 255)
 
     LOWER = np.array([l_h, l_s, l_v])
     UPPER = np.array([u_h, u_s, u_v])
 
     # Coordinates for the 4 alignment points: again, should be handled by the UI
-    bl = (12, 472)
-    tl = (90, 8)
-    br = (499, 475)
-    tr = (435, 24)
+    bl = (8, 416)
+    tl = (160, 62)
+    br = (508, 469)
+    tr = (628, 208)
     # Aplying perspective transformation
     pts1 = np.float32([tl, bl, tr, br])
     pts2 = np.float32([[0, 0], [0, 480], [640, 0], [640, 480]])
@@ -38,24 +38,22 @@ class LaneFollower(Node):
     UNWARP = cv2.getPerspectiveTransform(pts1, pts2)
     # Kernel for blurring & removing "salt and pepper" noise
     KERNEL = 23
-    LANE_TOLERANCE = 10
 
     # This is the lane follower Cstop/Estop trigger from crosstrack:
     # Effectively, it's an error beyond what our system is capable of returning, and we should trigger an Estop in the state machine if this value is ever read.
     MISSING_IMAGE_TOLERANCE = 100
-    EMPTY_WINDOWS_THRESHOLD = 6
-    OVERFLOW = 1000.0
+    EMPTY_WINDOWS_THRESHOLD = 6  # Higher == less likelihood we ignore
     FORMAT = (640, 480)
     TOLERANCE = 100
 
-    PIXELS_TO_METERS = 260.8269125
+    PIXELS_TO_METERS = 127
 
-    def __init__(self, odom_sub):
+    def __init__(self, odom_sub: OdomSubscriber):
         super().__init__('lane_detection_node')
 
         # Inputs from both cameras
-        self.vidcap_left = cv2.VideoCapture("/dev/video0")
-        self.vidcap_right = cv2.VideoCapture("/dev/video2")
+        self.vidcap_left = cv2.VideoCapture("/dev/video2")
+        self.vidcap_right = cv2.VideoCapture("/dev/video4")
         # Setting the format for the images: we use 640 x 480
         self.vidcap_left.set(3, LaneFollower.FORMAT[0])
         self.vidcap_left.set(4, LaneFollower.FORMAT[1])
@@ -109,7 +107,7 @@ def img_publish(self, label, img_raw):
             )
 
     def measure_position_meters(
-        self, left, right, ignore_left=False, ignore_right=False
+        self, left, right, Left_Lane, ignore_left=False, ignore_right=False
     ):
 
         left_x_pos = 0
@@ -122,27 +120,28 @@ def measure_position_meters(
             left_fit = self._left_follower._fit
             # This is the first window coordinates
             # This is the second window coordinates
-            left_x_pos = np.polyval(left_fit, y_max)
+            left_x_pos = np.polyval(left_fit, y_max // 2)
             self.img_publish("sliding_left", left)
 
         if right is not None:
             right_fit = self._right_follower._fit
-            right_x_pos = np.polyval(right_fit, y_max)
+            right_x_pos = np.polyval(right_fit, y_max // 2)
             self.img_publish("sliding_right", right)
+
         width = self._left_follower._binary_warped.shape[1]
 
-        if ignore_left:
+        if not ignore_right and ignore_left:
             # 5 Feet = 1.52 Meters
-            return 1.52 - right_x_pos * (LaneFollower.PIXELS_TO_METERS)
-        if ignore_right:
-            return 1.52 - left_x_pos * (LaneFollower.PIXELS_TO_METERS)
+            return 1 - right_x_pos / (LaneFollower.PIXELS_TO_METERS)
+        if not ignore_left and ignore_right:
+            return 1 - left_x_pos / (LaneFollower.PIXELS_TO_METERS)
 
-        center_lanes_x_pos = (left_x_pos + right_x_pos) // 2
+        center_lanes_x_pos = (left_x_pos + right_x_pos) / 2
         # Calculate the deviation between the center of the lane and the center of the picture
         # The car is assumed to be placed in the center of the picture
         # If the deviation is negative, the car is on the left hand side of the center of the lane
         veh_pos = (
-            (width // 2) - center_lanes_x_pos
+            (width / 2) - center_lanes_x_pos
         ) / LaneFollower.PIXELS_TO_METERS
 
         return veh_pos
@@ -153,14 +152,14 @@ def follow_lane(self, period=0.005):
         steer_cmd = self.stanley.get_steering_cmd(
             self.heading_error,
             self.cross_track_error,
-            # self.odom_sub.vel,
-            2.235,
+            self.odom_sub.vel,
         )  # comment this out when you want to use the actual velocity
 
         # vel_cmd = self.odom_sub.vel
         vel_cmd = (
-            2.235  # Unsure if the velocity command should always be the target
+            2.0  # Unsure if the velocity command should always be the target
         )
+
         time.sleep(period)
         return steer_cmd, vel_cmd
 
@@ -220,10 +219,10 @@ def timer_callback(self):
             self.img_publish("raw_" + image[1], frame)
             self.img_publish("tf_" + image[1], transformed_frame)
 
-        (result_left, empty_left, left_heading, left_slope) = (
+        result_left, empty_left, left_heading, left_slope = (
             self._left_follower.Plot_Line()
         )
-        (result_right, empty_right, right_heading, right_slope) = (
+        result_right, empty_right, right_heading, right_slope = (
             self._right_follower.Plot_Line()
         )
 
@@ -253,23 +252,34 @@ def timer_callback(self):
             )
 
             cross_track = self.measure_position_meters(
-                result_left, result_right, ignore_left, ignore_right
+                result_left,
+                result_right,
+                self._Left_Lane,
+                ignore_left=True,
+                ignore_right=False,
             )
 
             self.cross_track_error = cross_track
             self._Left_Lane = (
-                True if empty_left < empty_right - 2 else self._Left_Lane
+                True
+                if empty_left < empty_right - 2 or ignore_right
+                else self._Left_Lane
             )
             self._Left_Lane = (
-                False if empty_left - 2 > empty_right else self._Left_Lane
+                False
+                if empty_left - 2 > empty_right or ignore_left
+                else self._Left_Lane
             )
-
-            self.heading_error = left_heading if self._Left_Lane else right_heading
-
-        else:
-            self._tolerance += 1
-            if self._tolerance > LaneFollower.TOLERANCE:
-                self.empty_error = True
+            self.heading_error = (
+                -left_heading
+                if self._Left_Lane and not ignore_left
+                else -right_heading if not ignore_right else 0
+            )
+        # else:
+        #     self._tolerance += 1
+        #     if self._tolerance > LaneFollower.TOLERANCE:
+        #         self.empty_error = True
+        # print("Left Lane" if self._Left_Lane else "Right Lane")
 
 
 def main(args=None):

ros2/cmd/src/modules/lane_change_test.py

@@ -39,7 +39,7 @@ def main(args=None):
 
         args = parser.parse_args()
         max_dist_to_goal = 0.5
-        max_dist_to_path = 1.5
+        max_dist_to_path = 15
 
         odom_sub = OdomSubscriber()
         lane_change = LaneChange(odom_sub, max_dist_to_goal, max_dist_to_path)