Fixed Lane_Detection, publishing all images from the turtlebot

ros2/src/lanefollowingtest/LaneDetection.py

@@ -1,22 +1,22 @@
 
 # from irobot_create_msgs.msg import WheelTicks, WheelTicks
 from std_msgs.msg import Float64
+from sensor_msgs.msg import Image
 import rclpy
 from rclpy.node import Node
-from rclpy.qos import qos_profile_sensor_data
-from sensor_msgs.msg import Image
 from cv_bridge import CvBridge
 import cv2
 import numpy as np
-import LaneDetection as LD
+
 
 # Inputs from both cameras
 vidcap_left = cv2.VideoCapture("/dev/video0")
-vidcap_left.set(3, 640)
-vidcap_left.set(4, 480)
-vidcap_right = cv2.VideoCapture("/dev/video4")
-vidcap_right.set(3, 640)
-vidcap_right.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)
+
 
 # These are constants
 nwindows = 9
@@ -31,9 +31,9 @@
 def nothing(x):
     pass
 
-
 class Individual_Follower():
 
+
     def __init__(self):
         self._fit = None
         self._binary_warped = 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)
@@ -147,13 +147,10 @@ def Plot_Line(self, smoothen=False, prevFrameCount=6):
 matrix = cv2.getPerspectiveTransform(pts1, pts2)
 
 
-
-
 class Lane_Follower(Node):
     GUI = True
 
     def __init__(self):
-
         super().__init__('lane_detection_node')
         self._tolerance = 0
         self._left_follower = Individual_Follower()
@@ -165,42 +162,42 @@ def __init__(self):
         # Publisher for error from the lane lines relative to the camera FOV
         self.camData_publisher = self.create_publisher(
             Float64, '/cam_data', 10)
-
-        #GUI Controller Initializer
-        #/raw_left, /raw_right, /tf_left, /tf_right, /sliding_left, /sliding_right
         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}
 
+
     def img_publish(self, label, img_raw):
-        self._publishers[label].publish(self._bridge.cv2_to_imgmsg(img_raw,encoding="bgr8"))
+        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)
 
-            if(Lane_Follower.GUI):
-                self.img_publish("sliding_left", left)               
-            cv2.imshow("Result 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]
-            if(Lane_Follower.GUI):
-                self.img_publish("sliding_right", right)
-            cv2.imshow("Result 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
@@ -209,25 +206,27 @@ 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 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):
+        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))
             # Object Detection
@@ -239,18 +238,15 @@ def timer_callback(self):
                 self._left_follower.set_binwarp(binwarp=mask)
             else:
                 self._right_follower.set_binwarp(binwarp=mask)
-
-            if(Lane_Follower.GUI):
-                self.img_publish("raw_" +image[1],frame)
-                self.img_publish("tf_" + image[1],transformed_frame)            
-            cv2.imshow("Original " + image[1], frame)
-            cv2.imshow("Bird's Eye View " + image[1], transformed_frame)
+
+            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)
@@ -259,13 +255,14 @@ def timer_callback(self):
         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
@@ -277,4 +274,4 @@ def main(args=None):
 
 
 if __name__ == '__main__':
-    main()
+    main()