From 233047ddc5f421b7ea4cc80e6be743f474ebf560 Mon Sep 17 00:00:00 2001 From: "Vaibhav.hariani@gmail.com" Date: Tue, 16 Apr 2024 11:13:57 -0400 Subject: [PATCH] Blind attempt of Lane Determination: Will need to characterize, tune, and test (ESPECIALLY AROUND TURNS). --- ros2/src/lanefollowingtest/LaneDetection.py | 88 +++++++++++++-------- 1 file changed, 53 insertions(+), 35 deletions(-) diff --git a/ros2/src/lanefollowingtest/LaneDetection.py b/ros2/src/lanefollowingtest/LaneDetection.py index a1c73ece8..6931ecd88 100644 --- a/ros2/src/lanefollowingtest/LaneDetection.py +++ b/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() \ No newline at end of file + main()