Blind attempt of Lane Determination: Will need to characterize, tune,…

… and test (ESPECIALLY AROUND TURNS).

ros2/src/lanefollowingtest/LaneDetection.py

@@ -11,11 +11,11 @@
 
 # Inputs from both cameras
 vidcap_left = cv2.VideoCapture("/dev/video0")
-vidcap_left.set(3,640)
-vidcap_left.set(4,480)
+vidcap_left.set(3, 640)
+vidcap_left.set(4, 480)
 vidcap_right = cv2.VideoCapture("/dev/video2")
-vidcap_right.set(3,640)
-vidcap_right.set(4,480)
+vidcap_right.set(3, 640)
+vidcap_right.set(4, 480)
 
 
 # These are constants
@@ -31,8 +31,8 @@
 def nothing(x):
     pass
 
-class Individual_Follower():
 
+class Individual_Follower():
 
     def __init__(self):
         self._fit = None
@@ -123,7 +123,7 @@ def Plot_Line(self, smoothen=False, prevFrameCount=6):
         return result
 
 
-#These are constants for the timer callback: Should be modified by UI
+# These are constants for the timer callback: Should be modified by UI
 l_h = 0
 l_s = 0
 l_v = 200
@@ -135,7 +135,7 @@ def Plot_Line(self, smoothen=False, prevFrameCount=6):
 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
+# Coordinates for the 4 alignment points: again, should be handled by the UI
 bl = (12, 355)
 tl = (66, 304)
 br = (635, 344)
@@ -155,6 +155,9 @@ def __init__(self):
         self._tolerance = 0
         self._left_follower = Individual_Follower()
         self._right_follower = Individual_Follower()
+        # Determine which lane we're in: Left lane means the right image is dashed
+        self._Left_Lane = False
+
 
         timer_period = 0.01  # seconds
         self.timer = self.create_timer(timer_period, self.timer_callback)
@@ -164,40 +167,36 @@ def __init__(self):
             Float64, '/cam_data', 10)
         if Lane_Follower.GUI:
             self._bridge = CvBridge()
-            image_labels = ("raw_left", "raw_right", "tf_left", "tf_right", "sliding_left", "sliding_right")
-            self._publishers = {label: self.create_publisher(Image, "/" + label, 10) for label in image_labels}
-
+            image_labels = ("raw_left", "raw_right", "tf_left",
+                            "tf_right", "sliding_left", "sliding_right")
+            self._publishers = {label: self.create_publisher(
+                Image, "/" + label, 10) for label in image_labels}
 
     def img_publish(self, label, img_raw):
-        if(self.GUI):
-            self._publishers[label].publish(self._bridge.cv2_to_imgmsg(img_raw,encoding="passthrough"))
+        if (self.GUI):
+            self._publishers[label].publish(
+                self._bridge.cv2_to_imgmsg(img_raw, encoding="passthrough"))
 #        cv2.imshow(label, img_raw)
-
 
-
-    def measure_position_meters(self,left,right):
+    def measure_position_meters(self, left, right):
         left_x_pos = 0
         right_x_pos = 0
 
-
         # Will be the same for left side & right side
         y_max = self._left_follower._binary_warped.shape[0]
-                
+
         # Calculate left and right line positions at the bottom of the image
-        if left is not None:        
+        if left is not None:
             left_fit = self._left_follower._fit
             left_x_pos = left_fit[0]*y_max**2 + \
                 left_fit[1]*y_max + left_fit[2]
-            self.img_publish("sliding_left",left)
-
+            self.img_publish("sliding_left", left)
 
         if right is not None:
             right_fit = self._right_follower._fit
             right_x_pos = right_fit[0]*y_max**2 + \
                 right_fit[1]*y_max + right_fit[2]
-            self.img_publish("sliding_right",right)
-
-
+            self.img_publish("sliding_right", right)
 
         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
@@ -206,26 +205,42 @@ def measure_position_meters(self,left,right):
         veh_pos = (
             (self._left_follower._binary_warped.shape[1]//2) - center_lanes_x_pos) * xm_per_pix
 
-        if(left is None):
+        if (left is None):
             veh_pos += 91
         elif (right is None):
             veh_pos -= 91
         return veh_pos / 100
 
+    def determine_lane_size(self, img):
+        # Taking in both warped images, determine which lane line is the longer one, and ergo the "solid line"
+        # This may struggle on turns, but might work depending: Will need to characterize
+        gray = cv2.split(img)[3]
+        # TODO, parametrize all constants here for tweaking in the U.I
+        edges = cv2.Canny(gray, 50, 150)
+        lines = cv2.HoughLinesP(edges, 1, np.pi/180, 100,
+                                minLineLength=100, MaxLineGap=5)
+        m_length = 0
+        for line in lines:
+            x1, y1, x2, y2 = line[0]
+            length = (x1 - x2) ^ 2 + (y1-y2) ^ 2
+            m_length = max(m_length, length)
+        return m_length
 
     def timer_callback(self):
         success_l, image_l = vidcap_left.read()
         success_r, image_r = vidcap_right.read()
         images = [(image_l, "left"), (image_r, "right")]
-        if not(success_l and success_r):
+        left_buffer = -1
+        right_buffer = -1
+        if not (success_l and success_r):
             return
 
         for image in images:
             frame = image[0]
             # frame = cv2.resize(image[0], (640, 480))
             # I might've cooked with this list comprehension
-            for point in (bl,tl,br,tr):
-                frame = cv2.circle(frame,point,5,(0,0,255),-1)
+            for point in (bl, tl, br, tr):
+                frame = cv2.circle(frame, point, 5, (0, 0, 255), -1)
 
             transformed_frame = cv2.warpPerspective(
                 frame, matrix, (640, 480))
@@ -236,33 +251,36 @@ def timer_callback(self):
             mask = cv2.inRange(hsv_transformed_frame, lower, upper)
             if image[1] == "left":
                 self._left_follower.set_binwarp(binwarp=mask)
+                left_buffer = self.determine_lane_size(mask)
             else:
                 self._right_follower.set_binwarp(binwarp=mask)
-
-            self.img_publish("raw_"+ image[1], frame)
+                right_buffer = self.determine_lane_size(mask)
+            self.img_publish("raw_" + image[1], frame)
             self.img_publish("tf_" + image[1], transformed_frame)
 
         result_left = self._left_follower.Plot_Line()
         result_right = self._right_follower.Plot_Line()
-
         msg_out = Float64()
-        #TODO: Is this the behavior we want? Or do we need it to do something else if one of the lines is invalid?  
+        # TODO: Is this the behavior we want? Or do we need it to do something else if one of the lines is invalid?
         if (result_left is not None or result_right is not None):
             pos = self.measure_position_meters(result_left, result_right)
             print(pos)
             msg_out.data = pos
             self.camData_publisher.publish(msg_out)
+            if(left_buffer > right_buffer and left_buffer > 0):
+                self._Left_Lane = True
+            elif (right_buffer > 0):
+                self._Left_Lane = False
         else:
             TOLERANCE = 100
             self._tolerance += 1
-            if(self._tolerance > TOLERANCE):
+            if (self._tolerance > TOLERANCE):
                 msg_out.data = 1000.0
                 self.camData_publisher.publish(msg_out)
 
-
         if cv2.waitKey(10) == 27:
             return
-        
+
 
 def main(args=None):
     rclpy.init(args=args)  # Initialize ROS2 program
@@ -274,4 +292,4 @@ def main(args=None):
 
 
 if __name__ == '__main__':
-    main()
+    main()