diff --git a/ros2/UI/pages/Lane_Detection.py b/ros2/UI/pages/Lane_Detection.py index 07712feae..d8196f2c0 100644 --- a/ros2/UI/pages/Lane_Detection.py +++ b/ros2/UI/pages/Lane_Detection.py @@ -11,7 +11,7 @@ from ui_generics import * import ros2_numpy as rnp from geometry_msgs.msg import Transform - +from streamlit_image_coordinates import streamlit_image_coordinates as st_images # This class currently subscribes to some topics in cam_publisher, which I used to test. Please rewrite for lane Detection. @@ -192,14 +192,16 @@ def input_handler(context): img = sub.reference_img if img is not None: - cols = st.columns(3) - cols[0].image(img, "Left reference image for transformation") + pixel_values_ref = st_images(img) + st.write(pixel_values_ref) transformed_left = cv2.warpPerspective(img, matrix, (640, 480)) hsv_transformed_left = cv2.cvtColor( transformed_left, cv2.COLOR_BGR2HSV) mask = cv2.inRange(hsv_transformed_left, hsv[:][0], hsv[:][1]) - cols[1].image(transformed_left, "Left Birds Eye View Preview") - cols[2].image(mask, "Left Mask Preview") + transformed_vales = st_images(transformed_left) + st.write(transformed_vales) + # # st.image(mask, "Left Mask Preview") + # st.write(values) # TODO: Make this button publish the custom ROS2 Message :) # TODO: Make this button do soemthing (Read and write to file) if(st.button("Publish")): diff --git a/ros2/src/lanefollowingtest/LaneDetection.py b/ros2/src/lanefollowingtest/LaneDetection.py index 179afe397..13598519e 100644 --- a/ros2/src/lanefollowingtest/LaneDetection.py +++ b/ros2/src/lanefollowingtest/LaneDetection.py @@ -10,12 +10,13 @@ from std_msgs.msg import String # Inputs from both cameras -vidcap_left = cv2.VideoCapture("/dev/video3") +vidcap_left = cv2.VideoCapture("/dev/video0") vidcap_left.set(3, 640) vidcap_left.set(4, 480) -vidcap_right = cv2.VideoCapture("/dev/video1") -vidcap_right.set(3, 640) -vidcap_right.set(4, 480) +vidcap_right = vidcap_left +# vidcap_right = cv2.VideoCapture("/dev/video1") +# vidcap_right.set(3, 640) +# vidcap_right.set(4, 480) # These are constants @@ -107,7 +108,7 @@ def Plot_Line(self, smoothen=False, prevFrameCount=6): # And recast the x and y points into usable format for cv2.fillPoly() line_window1 = np.array( [np.transpose(np.vstack([fitx-margin, ploty]))]) - line_window2 = np.array([np.flipud(np.transpose(np.vstack([fitx+margin, + line_window2 = np.array([(np.transpose(np.vstack([fitx+margin, ploty])))]) line_pts = np.hstack((line_window1, line_window2)) @@ -136,10 +137,10 @@ def Plot_Line(self, smoothen=False, prevFrameCount=6): upper = np.array([u_h, u_s, u_v]) # Coordinates for the 4 alignment points: again, should be handled by the UI -bl = (12, 355) -tl = (66, 304) -br = (635, 344) -tr = (595, 308) +bl = (12, 472) +tl = (90, 8) +br = (499, 475) +tr = (435, 24) # Aplying perspective transformation pts1 = np.float32([tl, bl, tr, br]) pts2 = np.float32([[0, 0], [0, 480], [640, 0], [640, 480]]) @@ -158,7 +159,8 @@ def __init__(self): # Determine which lane we're in: Left lane means the right image is dashed self._Left_Lane = False - + ###The ratio from pixels to inches + conversion = 6.625 timer_period = 0.01 # seconds self.timer = self.create_timer(timer_period, self.timer_callback) @@ -229,6 +231,7 @@ def determine_lane_size(self, img): def timer_callback(self): success_l, image_l = vidcap_left.read() success_r, image_r = vidcap_right.read() + image_r = cv2.flip(image_r,0) images = [(image_l, "left"), (image_r, "right")] left_buffer = -1 right_buffer = -1 @@ -242,8 +245,8 @@ def timer_callback(self): 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)) + transformed_frame = cv2.rotate(cv2.warpPerspective( + frame, matrix, (640, 480)), cv2.ROTATE_90_CLOCKWISE) # Object Detection # Image Thresholding hsv_transformed_frame = cv2.cvtColor(