diff --git a/components/lane_follower/src/Follower_Consts.toml b/components/lane_follower/src/Follower_Consts.toml new file mode 100644 index 000000000..0c60b0047 --- /dev/null +++ b/components/lane_follower/src/Follower_Consts.toml @@ -0,0 +1,27 @@ +# Constants for the individual follower module + +[Individual_Follower] +NWINDOWS = 9 +SEARCH_WIDTH = 100 +MINPIXELS = 40 +LANE_POLY_SIZE = 2 +BOX_COLOR = [0, 255, 0] +LANE_COLOR = [255, 0, 0] +DRAW_THICKNESS = 2 + +[Lane_Follower] +GUI = true +# TODO: Move these into a conf +KERNEL = 31 +LANE_TOLERANCE = 10 +MISSING_IMAGE_TOLERANCE = 100 +EMPTY_WINDOWS_THRESHOLD = 3 +OVERFLOW = 1000.0 +FORMAT = [640, 480] +TOLERANCE = 100 +PIXELS_TO_METERS = 260.8269125 +lower_hsv = [100, 0, 220] +upper_hsv = [255, 255, 255] +# Alignment points +bottom_coordinates = [[12, 472], [499, 475]] +top_coordinates = [[90, 8], [435, 24]] diff --git a/components/lane_follower/src/individual_follower.py b/components/lane_follower/src/individual_follower.py new file mode 100644 index 000000000..69937b864 --- /dev/null +++ b/components/lane_follower/src/individual_follower.py @@ -0,0 +1,150 @@ +import math + +import cv2 +import numpy as np +import toml +from einops import repeat + + +class ifResult: + def __init__(self): + self.result_img = None + self.empty_windows = 0 + self.heading = 0 + self.slope = 0 + + pass + + +class IndividualFollower: + + def __init__(self, toml_path='Follower_Consts.toml'): + + self.consts = toml.load(toml_path)["Individual_Follower"] + # self.fit is set in Plot_Line + self.fit = None + # These two are set below + self._binary_warped = None + self._histogram = None + + # Need to determine if these should remain properties + # Or if they should be passed as arguments to Plot_Line + @property + def _binary_warped(self, value: np.ndarray): + self._binary_warped = value + self._histogram = np.sum( + self.binary_warped[self.binary_warped.shape[0] // 2 :, :], axis=0 + ) + + def get_white_pixels(self): + # Returns an output image + NWINDOWS = self.consts["NWINDOWS"] + SEARCH_WIDTH = self.consts['SEARCH_WIDTH'] + + window_height = np.int32(self._binary_warped.shape[0] / NWINDOWS) + lane_inds = [] + + nonzero_pixels = self._binary_warped.nonzero() + nonzero_y = np.array(nonzero_pixels[0]) + nonzero_x = np.array(nonzero_pixels[1]) + + empty_windows = 0 + lane_base = np.argmax(self.histogram[:]) + + # Image to visualize output + out_img = repeat(self._binary_warped, 'h w -> h w c', repeat=3) * 255 + ##These outputs need to be confirmed compatible + # out_img = ( + # np.dstack( + # self._binary_warped, self._binary_warped, self._binary_warped + # ) + # * 255 + # ) + + for window in range(NWINDOWS): + window_dims = window * window_height + win_y_upper = self._binary_warped.shape[0] - window_dims + # One window height lower than win_y_higher + win_y_lower = win_y_upper - window_height + win_x_lower = lane_base - SEARCH_WIDTH + win_x_upper = lane_base + SEARCH_WIDTH + + lower_coords = (win_x_lower, win_y_lower) + upper_coords = (win_x_upper, win_y_lower) + cv2.rectangle( + out_img, + lower_coords, + upper_coords, + self.consts["BOX_COLOR"], + self.consts["DRAW_THICKNESS"], + ) + + white_pix_inds = ( + (nonzero_y >= win_y_lower) + & (nonzero_y < win_y_upper) + & (nonzero_x >= win_x_lower) + & (nonzero_x >= win_x_upper) + ).nonzero_pixels()[0] + + # This should likely be moved into the if statement: leaving for now + lane_inds.append(white_pix_inds) + if len(white_pix_inds) > self.consts["MINPIXELS"]: + # np.mean will return a float: We need an exact value + lane_base = np.int32(np.mean(nonzero_x[white_pix_inds])) + else: + empty_windows += 1 + if len(lane_inds) == 0: + return None + lane_array = np.concatenate(lane_inds) + lane_x_pos = nonzero_x[lane_array] + lane_y_pos = nonzero_y[lane_array] + # TODO: test if this statement is necessary + if lane_x_pos.any() and lane_y_pos.any(): + self._fit = np.polyfit( + lane_y_pos, lane_x_pos, self.consts["LANE_POLY_SIZE"] + ) + + out_img[nonzero_y[lane_array], nonzero_x[lane_array]] = ( + self.consts["LANE_COLOR"] + ) + return out_img + + def plot_line(self) -> ifResult: + if not self._binary_warped: + raise Exception("no binary warp specified") + + result = ifResult() + + NWINDOWS = self.consts["NWINDOWS"] + window_height = np.int32(self._binary_warped.shape[0] / NWINDOWS) + out_img = self.get_white_pixels() + if out_img is None: + return result + + ##Generates the search window area + window_img = np.zeros_like(out_img) + # Create linearly spaced points at each height, evaluate polynomial, create coordinates + x_val = np.arange(0, window_height, step=5).T + y_val = np.polyval(self.fit, x_val) + coords = np.concatenate((x_val, y_val), axis=0) + ##TODO: Test if this approach works + cv2.polylines( + out_img, + coords, + isClosed=False, + color=self.consts["LANE_COLOR"], + thickness=self.consts["DRAW_THICKNESS"], + ) + + # Calculating heading error by converting lane polynomial into line + ##TODO: Make this use the furthest found box. This way, we'll use the most accurate heading + y_lower = 0 + y_upper = (NWINDOWS + 1) * window_height + + result.slope = np.polyval(self.fit, y_lower) - np.polyval( + self.fit, y_upper + ) / (y_upper - y_lower) + result.result_img = cv2.addWeighted(out_img, 1, window_img, 0.3, 0) + result.heading = math.atan(result.slope) + + return result diff --git a/components/lane_follower/src/lane_follower.py b/components/lane_follower/src/lane_follower.py new file mode 100644 index 000000000..429ca8901 --- /dev/null +++ b/components/lane_follower/src/lane_follower.py @@ -0,0 +1,20 @@ +import toml +import cv2 + +# import numpy as np + + +class LaneFollower: + + def __init__( + self, odom_sub, cam_left, cam_right, toml_path='Follower_Consts.toml' + ): + self.vidcap_left = cv2.VideoCapture(cam_left) + self.vidcap_right = cv2.VideoCapture(cam_right) + self.const = toml.load(toml_path)["Lane_Follower"] + # LOWER = np.array(self.const['lower_hsv']) + # UPPER = np.array(self.const['upper_hsv']) + # PTS1 = np.float32([tl, bl, tr, br]) + # PTS2 = np.float32([[0, 0], [0, 480], [640, 0], [640, 480]]) + # # Matrix to warp the image for birdseye window + # UNWARP = cv2.getPerspectiveTransform(pts1, pts2) diff --git a/components/lane_follower/src/pyproject.toml b/components/lane_follower/src/pyproject.toml new file mode 100644 index 000000000..15528d718 --- /dev/null +++ b/components/lane_follower/src/pyproject.toml @@ -0,0 +1,8 @@ +[project] +version = '1.0' +name = "individual-follower" +authors = [ + {name = "Vaibhav Hariani"}, + {name = "Isaiah Rivera"}, +] +description = "IGVC lane follower module for a single camera"