diff --git a/ros2/src/lanefollowingtest/LaneDetection.py b/ros2/src/lanefollowingtest/LaneDetection.py index ece87be09..a1c73ece8 100644 --- a/ros2/src/lanefollowingtest/LaneDetection.py +++ b/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() \ No newline at end of file