diff --git a/launch.sh b/launch.sh index bc03a901..7262d8ad 100755 --- a/launch.sh +++ b/launch.sh @@ -7,9 +7,9 @@ set -e echo "This is an empty launch script. Update it to launch your application." #roslaunch circle_drive circle_drive.launch #roslaunch duckietown_demos lane_following.launch -roslaunch --wait circle_drive indefinite_navigation2.launch & +roslaunch --wait circle_drive lane_following.launch #roslaunch --wait launchfile.launch & -sleep 5 +#sleep 5 # we put a short sleep in here because rostopic will fail if there's no roscore yet -rostopic pub /$VEHICLE_NAME/fsm_node/mode duckietown_msgs/FSMState '{header: {}, state: "LANE_FOLLOWING"}' +#rostopic pub /$VEHICLE_NAME/fsm_node/mode duckietown_msgs/FSMState '{header: {}, state: "LANE_FOLLOWING"}' diff --git a/packages/circle_drive/launch/indefinite_navigation2.launch b/packages/circle_drive/launch/indefinite_navigation2.launch index b1a7f53f..ade81e60 100644 --- a/packages/circle_drive/launch/indefinite_navigation2.launch +++ b/packages/circle_drive/launch/indefinite_navigation2.launch @@ -4,55 +4,15 @@ - - - - - - + + + + - + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + diff --git a/packages/circle_drive/launch/lane_following.launch b/packages/circle_drive/launch/lane_following.launch new file mode 100644 index 00000000..ca45411f --- /dev/null +++ b/packages/circle_drive/launch/lane_following.launch @@ -0,0 +1,45 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/packages/circle_drive/scripts/circle_drive.py b/packages/circle_drive/scripts/circle_drive.py old mode 100755 new mode 100644 diff --git a/packages/custom_line_detector/CMakeLists.txt b/packages/custom_line_detector/CMakeLists.txt new file mode 100644 index 00000000..1077df4a --- /dev/null +++ b/packages/custom_line_detector/CMakeLists.txt @@ -0,0 +1,20 @@ +cmake_minimum_required(VERSION 2.8.3) +project(custom_line_detector) + +find_package(catkin REQUIRED COMPONENTS + roscpp + rospy + duckietown_msgs # Every duckietown packages should use this. + cv_bridge + # XXX add anti_instagram? +) + +catkin_python_setup() + + +catkin_package() + + +include_directories( + ${catkin_INCLUDE_DIRS} +) diff --git a/packages/custom_line_detector/__init__.py b/packages/custom_line_detector/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/packages/dt-core/packages/line_detector/config/line_detector_node/default.yaml b/packages/custom_line_detector/config/line_detector_node/default.yaml similarity index 57% rename from packages/dt-core/packages/line_detector/config/line_detector_node/default.yaml rename to packages/custom_line_detector/config/line_detector_node/default.yaml index 3fa655ab..9795434a 100644 --- a/packages/dt-core/packages/line_detector/config/line_detector_node/default.yaml +++ b/packages/custom_line_detector/config/line_detector_node/default.yaml @@ -3,16 +3,16 @@ top_cutoff: 40 colors: RED: - low_1: [0,30,100] - high_1: [17,255,255] - low_2: [165,40,100] + low_1: [0,140,100] + high_1: [15,255,255] + low_2: [165,140,100] high_2: [180,255,255] WHITE: - low: [41,0,100] - high: [164,100,255] + low: [0,0,150] + high: [180,100,255] YELLOW: - low: [23,40,100] - high: [40,255,255] + low: [25,140,100] + high: [45,255,255] line_detector_parameters: canny_thresholds: [80,200] @@ -20,4 +20,4 @@ line_detector_parameters: dilation_kernel_size: 3 hough_threshold: 2 hough_min_line_length: 3 - hough_max_line_gap: 1 + hough_max_line_gap: 1 \ No newline at end of file diff --git a/packages/custom_line_detector/include/__init__.py b/packages/custom_line_detector/include/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/packages/custom_line_detector/include/custom_line_detector/__init__.py b/packages/custom_line_detector/include/custom_line_detector/__init__.py new file mode 100644 index 00000000..0a3cfa9a --- /dev/null +++ b/packages/custom_line_detector/include/custom_line_detector/__init__.py @@ -0,0 +1,29 @@ +""" + + custom_line_detector + ------------- + + The ``custom_line_detector`` library packages classes and tools for handling line section extraction from images. The + main functionality is in the :py:class:`LineDetector` class. :py:class:`Detections` is the output data class for + the results of a call to :py:class:`LineDetector`, and :py:class:`ColorRange` is used to specify the colour ranges + in which :py:class:`LineDetector` is looking for line segments. + + There are two plotting utilities also included: :py:func:`plotMaps` and :py:func:`plotSegments` + + .. autoclass:: custom_line_detector.Detections + + .. autoclass:: custom_line_detector.ColorRange + + .. autoclass:: custom_line_detector.LineDetector + + .. autofunction:: custom_line_detector.plotMaps + + .. autofunction:: custom_line_detector.plotSegments + + +""" + +from .line_detector import LineDetector +from .detections import Detections +from .color_range import ColorRange +from .plot_detections import plotSegments, plotMaps diff --git a/packages/custom_line_detector/include/custom_line_detector/baseline_daffy.line_detector.yaml b/packages/custom_line_detector/include/custom_line_detector/baseline_daffy.line_detector.yaml new file mode 100644 index 00000000..4514d858 --- /dev/null +++ b/packages/custom_line_detector/include/custom_line_detector/baseline_daffy.line_detector.yaml @@ -0,0 +1,9 @@ +description: This is the Daffy baseline line detector (2020). +constructor: custom_line_detector.LineDetector +parameters: + canny_thresholds: [80,200] + canny_aperture_size: 3 + dilation_kernel_size: 3 + hough_threshold: 2 + hough_min_line_length: 3 + hough_max_line_gap: 1 diff --git a/packages/custom_line_detector/include/custom_line_detector/color_range.py b/packages/custom_line_detector/include/custom_line_detector/color_range.py new file mode 100644 index 00000000..ba572846 --- /dev/null +++ b/packages/custom_line_detector/include/custom_line_detector/color_range.py @@ -0,0 +1,111 @@ +import numpy as np +import cv2 + + +class ColorRange: + """ + The Color Range class holds one or multiple color ranges. It can easily be generated with + the :py:meth:`fromDict` class method and extends the `OpenCV's inRange `_ + method to work with multiple color ranges. + + All colours must be given in ``HSV`` space. + + Args: + low (:obj:`numpy array`): An ``Nx3`` array with the low ends of ``N`` colour ranges. + high (:obj:`numpy array`): An ``Nx3`` array with the high ends of ``N`` colour ranges. + """ + + def __init__(self, low, high): + self.low = low + self.high = high + + @classmethod + def fromDict(cls, dictionary): + """ + + Generates a :py:class:`ColorRange` object from a dictionary. Expects the colors to be given in ``HSV`` space. + If multi-entry ranges are provided (e.g. if you are interested in yellow and white), then each should have + keys ``high_X`` and ``low_X``, see example bellow: + + Examples: + + Single-entry color range:: + + { 'low': [0,0,150], 'high': [180,60,255] } + + Multi-entry color range:: + + { 'low_1': [0,0,150], 'high_1': [180,60,255], 'low_2': [165,140,100], 'high_2': [180,255,255] } + + + Args: + dictionary (:obj:`dict`): The yaml dictionary describing the color ranges. + + Returns: + :obj:`ColorRange`: the generated ColorRange object + """ + + # if only two entries: single-entry, if more: multi-entry + if len(dictionary) == 2: + assert "low" in dictionary, "Key 'low' must be in dictionary" + assert "high" in dictionary, "Key 'high' must be in dictionary" + low = np.array(dictionary["low"]).reshape((1, 3)) + high = np.array(dictionary["high"]).reshape((1, 3)) + + elif len(dictionary) % 2 == 0: + + # make the keys tuples with `low` or `high` and the id of the entry + dictionary = {tuple(k.split("_")): v for k, v in list(dictionary.items())} + entry_indices = set([k[1] for k, _ in list(dictionary.items())]) + + assert len(entry_indices) == len(dictionary) / 2, ( + "The multi-entry definition doesn't " "follow the requirements" + ) + + # build an array for the low and an array for the high range bounds + low = np.zeros((len(entry_indices), 3)) + high = np.zeros((len(entry_indices), 3)) + for idx, entry in enumerate(entry_indices): + low[idx] = dictionary[("low", entry)] + high[idx] = dictionary[("high", entry)] + + else: + raise ValueError( + "The input dictionary has two have an even number of " + "entries: a low and high value for each color range." + ) + + return cls(low=low, high=high) + + def inRange(self, image): + """ + Applies the `OpenCV inRange `_ + method to every color range entry. Returns the bitwise OR of the results. + In other words, returns a binary map with 1 for the pixels of the input image that fall in at least one of + the color ranges. + + Args: + image (:obj:`numpy array`): an ``HSV`` image + + Returns: + :obj:`numpy array`: a two-dimensional binary map + """ + + selection = cv2.inRange(image, self.low[0], self.high[0]) + for i in range(1, len(self.low)): + current = cv2.inRange(image, self.low[i], self.high[i]) + selection = cv2.bitwise_or(current, selection) + + return selection + + @property + def representative(self): + """ + Provides an representative color for this color range. This is the average color of the first range (if more + than one ranges are set). + + Returns: + :obj:`list`: a list with 3 entries representing an HSV color + """ + + return list(0.5 * (self.high[0] + self.low[0]).astype(int)) diff --git a/packages/custom_line_detector/include/custom_line_detector/detections.py b/packages/custom_line_detector/include/custom_line_detector/detections.py new file mode 100644 index 00000000..6f84b139 --- /dev/null +++ b/packages/custom_line_detector/include/custom_line_detector/detections.py @@ -0,0 +1,15 @@ +class Detections: + """ + This is a data class that can be used to store the results of the line detection procedure performed + by :py:class:`LineDetector`. + """ + + def __init__(self, lines, normals, centers, map): + self.lines = lines #: An ``Nx4`` array with every row representing a line ``[x1, y1, x2, y2]`` + self.normals = normals #: An ``Nx2`` array with every row representing the normal of a line ``[nx, + # ny]`` + + self.centers = centers #: An ``Nx2`` array with every row representing the center of a line ``[cx, + # cy]`` + + self.map = map #: A binary map of the area from which the line segments were extracted diff --git a/packages/custom_line_detector/include/custom_line_detector/line_detector.py b/packages/custom_line_detector/include/custom_line_detector/line_detector.py new file mode 100644 index 00000000..53286462 --- /dev/null +++ b/packages/custom_line_detector/include/custom_line_detector/line_detector.py @@ -0,0 +1,488 @@ +import functools + +import cv2 +from math import sqrt +from line_detector_interface import LineDetectorInterface +from .detections import Detections +import numpy as np + + +BLACK_PIXEL = [0, 0, 0] +EDGES_NUM_IN_RECT = 4 + + +class LineDetector(LineDetectorInterface): + """ + The Line Detector can be used to extract line segments from a particular color range in an image. It combines + edge detection, color filtering, and line segment extraction. + + This class was created for the goal of extracting the white, yellow, and red lines in the Duckiebot's camera stream + as part of the lane localization pipeline. It is setup in a way that allows efficient detection of line segments in + different color ranges. + + In order to process an image, first the :py:meth:`setImage` method must be called. In makes an internal copy of the + image, converts it to `HSV color space `_, which is much better for + color segmentation, and applies `Canny edge detection `_. + + Then, to do the actual line segment extraction, a call to :py:meth:`detectLines` with a :py:class:`ColorRange` + object must be made. Multiple such calls with different colour ranges can be made and these will reuse the + precomputed HSV image and Canny edges. + + Args: + + canny_thresholds (:obj:`list` of :obj:`int`): a list with two entries that specify the thresholds for the hysteresis procedure, details `here `__, default is ``[80, 200]`` + + canny_aperture_size (:obj:`int`): aperture size for a Sobel operator, details `here `__, default is 3 + + dilation_kernel_size (:obj:`int`): kernel size for the dilation operation which fills in the gaps in the color filter result, default is 3 + + hough_threshold (:obj:`int`): Accumulator threshold parameter. Only those lines are returned that get enough votes, details `here `__, default is 2 + + hough_min_line_length (:obj:`int`): Minimum line length. Line segments shorter than that are rejected, details `here `__, default is 3 + + hough_max_line_gap (:obj:`int`): Maximum allowed gap between points on the same line to link them, details `here `__, default is 1 + + """ + + class Contour: + def __init__(self, center, rad, approx): + self.center = center + self.rad = int(rad) + self.approx = approx + + def __lt__(self, other): + return self.rad < other.rad + + def __eq__(self, other): + return self.center == other.center and self.rad == other.rad + + def __gt__(self, other): + return self.rad > other.rad + + def __init__( + self, + canny_thresholds=[80, 200], + canny_aperture_size=3, + dilation_kernel_size=3, + hough_threshold=2, + hough_min_line_length=3, + hough_max_line_gap=1, + ): + + self.canny_thresholds = canny_thresholds + self.canny_aperture_size = canny_aperture_size + self.dilation_kernel_size = dilation_kernel_size + self.hough_threshold = hough_threshold + self.hough_min_line_length = hough_min_line_length + self.hough_max_line_gap = hough_max_line_gap + + # initialize the variables that will hold the processed images + self.bgr = np.empty(0) #: Holds the ``BGR`` representation of an image + self.hsv = np.empty(0) #: Holds the ``HSV`` representation of an image + self.canny_edges = np.empty(0) #: Holds the Canny edges of an image + self.last_correct_edge = [] + + @staticmethod + def _check_cos(cos_a, cos_b, inv=False): + DEGREES_DIST = 20 + RIGHT_ANGLE_DIST = 5 + RIGHT_ANGLE = 90 + degrees_a, degrees_b = LineDetector._get_degrees_from_cos(cos_a), LineDetector._get_degrees_from_cos(cos_b) + if abs(degrees_a - degrees_b) > DEGREES_DIST: + if not inv: + return 0 if cos_a > cos_b else 1 + else: + return 1 if cos_a > cos_b else 0 + else: + if abs(degrees_a - RIGHT_ANGLE) <= RIGHT_ANGLE_DIST: + if not inv: + return 0 + return 1 + if abs(degrees_b - RIGHT_ANGLE) <= RIGHT_ANGLE_DIST: + if not inv: + return 1 + return 0 + return -1 + + @staticmethod + def _get_polynomial(): + x = np.array([74, 69, 65, 48, 45, 40, 29, 0]) + y = np.array([36, 35, 28, 27, 26, 25, 11, 0]) + y = y + 5 + return np.poly1d(np.polyfit(x, y, 4)) + + @staticmethod + def _get_max_dist_between_elements(y): + min_dist_lim = 5 + pol = LineDetector._get_polynomial() + result = round(pol(y)) + # result = y / 2 + return 0 if result <= min_dist_lim else result + + @staticmethod + def _dist_pixels(pix1, pix2): + return sqrt((pix1[0] - pix2[0]) ** 2 + (pix1[1] - pix2[1]) ** 2) + + def _filter_contours(self, contours, img): + filtered_contours = [] + for k, contour in enumerate(contours): + approx = cv2.approxPolyDP(contour, 0.045 * cv2.arcLength(contour, True), True) + center = sum(approx) / approx.shape[0] + radius = max(contour, key=lambda x: abs(center[0][0] - x[0][0]) ** 2 + abs(center[0][1] - x[0][1]) ** 2) + radius_length = sqrt((radius[0][0] - center[0][0]) ** 2 + (radius[0][1] - center[0][1]) ** 2) + + if len(approx) == EDGES_NUM_IN_RECT: + diagonal = self._get_diagonal(approx) + cont_center = [int((diagonal[0][0] + diagonal[1][0]) // 2), int((diagonal[0][1] + diagonal[1][1]) // 2)] + + if np.array_equal(img[int(center[0][1]), int(center[0][0])], BLACK_PIXEL): + continue + + filtered_contours.append(self.Contour(cont_center, radius_length, approx)) + return filtered_contours, img + + def _get_diagonal(self, approx): + diagonal = [] + max_dist = 0 + for cortex_1 in approx: + for cortex_2 in approx: + if self._dist_pixels(cortex_1[0], cortex_2[0]) > max_dist: + max_dist = self._dist_pixels(cortex_1[0], cortex_2[0]) + diagonal = [cortex_1[0], cortex_2[0]] + return diagonal + + def get_edges_from_contours(self, img, contours): + self.last_correct_edge = [] + edges = [] + for contour in contours: + contour, distances = self._create_edges_from_approx(contour.approx) + max_edge_ind = self._get_max_edge_index(contour, distances, img.shape[0] - 1) + + edge = contour[max_edge_ind] + edges += LineDetector._get_contour_by_piece(edge) + + max_edge_ind = (max_edge_ind + 2) % EDGES_NUM_IN_RECT + edge = contour[max_edge_ind] + edges += LineDetector._get_contour_by_piece(edge) + return edges + + def _get_max_edge_index(self, contour, distances, y): + + if self.last_correct_edge: + has_last = False + last = self.last_correct_edge + else: + has_last = True + x = contour[0][0] if contour[0][0] != 0 else 10 + last = (0, y, x, y) + if not (contour[0][0] == 0 and contour[0][2] == 0 or contour[1][0] == 0 and contour[1][2] == 0): + + cos_a = LineDetector._get_cos(last, contour[0], distances[0]) + cos_b = LineDetector._get_cos(last, contour[1], distances[1]) + edge_index = LineDetector._check_cos(cos_a, cos_b, has_last) + if edge_index != -1: + self.last_correct_edge = contour[edge_index] + return edge_index + + max_dist_index = np.argmax(distances) + if abs(distances[0] - distances[1]) > 2 and abs(distances[2] - distances[3]) > 2: + if distances[max_dist_index] > 6: + self.last_correct_edge = contour[max_dist_index] + return max_dist_index + cos_a = LineDetector._get_cos(last, contour[2], distances[2]) + cos_b = LineDetector._get_cos(last, contour[3], distances[3]) + + edge_index = LineDetector._check_cos(cos_a, cos_b, has_last) + if edge_index != -1: + # print('by third cos') + self.last_correct_edge = contour[edge_index] + return edge_index + # print('by random') + if not has_last: + edge_index = 0 if cos_a > cos_b else 1 + else: + edge_index = 1 if cos_a > cos_b else 0 + self.last_correct_edge = contour[edge_index] + return edge_index + + @staticmethod + def _get_degrees_from_cos(cos): + return np.degrees(np.arccos(cos)) + + @staticmethod + def _get_cos(last, current, dist_current): + dist_last = LineDetector._dist_pixels((last[0], last[1]), (last[2], last[3])) + last = [last[0] - last[2], last[1] - last[3]] + current = [current[0] - current[2], current[1] - current[3]] + try: + return abs((last[0] * current[0] + last[1] * current[1]) / (dist_last * dist_current)) + except ZeroDivisionError: + return 1 + + @staticmethod + def _get_contour_by_piece(edge): + if abs(edge[0] - edge[2]) <= 1 or abs(edge[1] - edge[3]) <= 1: + return [edge] + x_list = list(map(int, np.linspace(edge[0], edge[2], 3))) + y_list = list(map(int, np.linspace(edge[1], edge[3], 3))) + contour = [] + current_piece = [] + for x, y in zip(x_list, y_list): + if current_piece: + current_piece.extend([x, y]) + contour.append(current_piece) + current_piece = [x, y] + return contour + + @staticmethod + def _create_edges_from_approx(approx): + vertex_0 = approx[0] + x_0 = int(vertex_0[0][0]) + y_0 = int(vertex_0[0][1]) + + current_vertex = [x_0, y_0] + contour = [] + distances = [] + + for vertex in approx[1:]: + x_1 = int(vertex[0][0]) + y_1 = int(vertex[0][1]) + + current_vertex.extend([x_1, y_1]) + distances.append(LineDetector._dist_pixels(current_vertex[:2], current_vertex[2:])) + contour.append(current_vertex) + current_vertex = [x_1, y_1] + + current_vertex.extend([x_0, y_0]) + contour.append(current_vertex) + distances.append(LineDetector._dist_pixels(current_vertex[:2], current_vertex[2:])) + return contour, distances + + @staticmethod + def _sort_contours(contours): + return sorted(contours, key=functools.cmp_to_key( + lambda contour1, contour2: LineDetector._dist_pixels(contour1.center, contour2.center)), reverse=True) + + def detect_dash_line(self, img): + + gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) + ret, threshold_image = cv2.threshold(gray, 0, 255, + cv2.THRESH_BINARY | cv2.THRESH_OTSU) + contours, h = cv2.findContours(threshold_image, 1, 2) + threshold_image = cv2.cvtColor(threshold_image, cv2.COLOR_GRAY2RGB) + contours, threshold_image = self._filter_contours(contours, threshold_image) + if not len(contours): + return threshold_image, [] + contours = LineDetector._sort_contours(contours) + + next_contour = False + dash_lines = [] + prev_contour = contours[0] + + for i, contour in enumerate(contours[1:]): + + max_dist = LineDetector._get_max_dist_between_elements(prev_contour.center[1]) + dist = LineDetector._dist_pixels(contour.center, prev_contour.center) + if dist <= max_dist and not LineDetector._is_inside(contour, prev_contour): + dash_lines.append(prev_contour) + next_contour = True + else: + if next_contour: + dash_lines.append(prev_contour) + next_contour = False + + prev_contour = contour + + if next_contour: + dash_lines.append(prev_contour) + return threshold_image, dash_lines + + @staticmethod + def _is_inside(first_line, second_line): + if first_line == second_line: + return False + big_cont = first_line if first_line > second_line else second_line + small_cont = first_line if first_line < second_line else second_line + dist_between_cent = LineDetector._dist_pixels(first_line.center, second_line.center) + if small_cont.rad + dist_between_cent < big_cont.rad: + return True + return False + + def setImage(self, image): + """ + Sets the :py:attr:`bgr` attribute to the provided image. Also stores + an `HSV `_ representation of the image and the + extracted `Canny edges `_. This is separated from + :py:meth:`detectLines` so that the HSV representation and the edge extraction can be reused for multiple + colors. + + Args: + image (:obj:`numpy array`): input image + + """ + + self.bgr = np.copy(image) + self.hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) + self.canny_edges = self.findEdges() + + def getImage(self): + """ + Provides the image currently stored in the :py:attr:`bgr` attribute. + + Returns: + :obj:`numpy array`: the stored image + """ + return self.bgr + + def findEdges(self): + """ + Applies `Canny edge detection `_ to a ``BGR`` image. + + + Returns: + :obj:`numpy array`: a binary image with the edges + """ + edges = cv2.Canny( + self.bgr, + self.canny_thresholds[0], + self.canny_thresholds[1], + apertureSize=self.canny_aperture_size, + ) + return edges + + def houghLine(self, edges): + """ + Finds line segments in a binary image using the probabilistic Hough transform. Based on the OpenCV function + `HoughLinesP `_. + + Args: + edges (:obj:`numpy array`): binary image with edges + + Returns: + :obj:`numpy array`: An ``Nx4`` array where each row represents a line ``[x1, y1, x2, y2]``. If no lines + were detected, returns an empty list. + + """ + lines = cv2.HoughLinesP( + edges, + rho=1, + theta=np.pi / 180, + threshold=self.hough_threshold, + minLineLength=self.hough_min_line_length, + maxLineGap=self.hough_max_line_gap, + ) + if lines is not None: + lines = lines.reshape((-1, 4)) # it has an extra dimension + else: + lines = [] + + return lines + + def colorFilter(self, color_range): + """ + Obtains the regions of the image that fall in the provided color range and the subset of the detected Canny + edges which are in these regions. Applies a `dilation `_ + operation to smooth and grow the regions map. + + Args: + color_range (:py:class:`ColorRange`): A :py:class:`ColorRange` object specifying the desired colors. + + Returns: + + :obj:`numpy array`: binary image with the regions of the image that fall in the color range + + :obj:`numpy array`: binary image with the edges in the image that fall in the color range + """ + # threshold colors in HSV space + map = color_range.inRange(self.hsv) + + # binary dilation: fills in gaps and makes the detected regions grow + kernel = cv2.getStructuringElement( + cv2.MORPH_ELLIPSE, (self.dilation_kernel_size, self.dilation_kernel_size) + ) + map = cv2.dilate(map, kernel) + + # extract only the edges which come from the region with the selected color + edge_color = cv2.bitwise_and(map, self.canny_edges) + + return map, edge_color + + def findNormal(self, map, lines): + """ + Calculates the centers of the line segments and their normals. + + Args: + map (:obj:`numpy array`): binary image with the regions of the image that fall in a given color range + + lines (:obj:`numpy array`): An ``Nx4`` array where each row represents a line. If no lines were detected, + returns an empty list. + + Returns: + :obj:`tuple`: a tuple containing: + + * :obj:`numpy array`: An ``Nx2`` array where each row represents the center point of a line. If no lines were detected returns an empty list. + + * :obj:`numpy array`: An ``Nx2`` array where each row represents the normal of a line. If no lines were detected returns an empty list. + """ + normals = [] + centers = [] + if len(lines) > 0: + length = np.sum((lines[:, 0:2] - lines[:, 2:4]) ** 2, axis=1, keepdims=True) ** 0.5 + dx = 1.0 * (lines[:, 3:4] - lines[:, 1:2]) / length + dy = 1.0 * (lines[:, 0:1] - lines[:, 2:3]) / length + + centers = np.hstack([(lines[:, 0:1] + lines[:, 2:3]) / 2, (lines[:, 1:2] + lines[:, 3:4]) / 2]) + x3 = (centers[:, 0:1] - 3.0 * dx).astype("int") + y3 = (centers[:, 1:2] - 3.0 * dy).astype("int") + x4 = (centers[:, 0:1] + 3.0 * dx).astype("int") + y4 = (centers[:, 1:2] + 3.0 * dy).astype("int") + + np.clip(x3, 0, map.shape[1] - 1, out=x3) + np.clip(y3, 0, map.shape[0] - 1, out=y3) + np.clip(x4, 0, map.shape[1] - 1, out=x4) + np.clip(y4, 0, map.shape[0] - 1, out=y4) + + flag_signs = (np.logical_and(map[y3, x3] > 0, map[y4, x4] == 0)).astype("int") * 2 - 1 + normals = np.hstack([dx, dy]) * flag_signs + + return centers, normals + + def detectLines(self, color_range): + """ + Detects the line segments in the currently set image that occur in and the edges of the regions of the image + that are within the provided colour ranges. + + Args: + color_range (:py:class:`ColorRange`): A :py:class:`ColorRange` object specifying the desired colors. + + Returns: + :py:class:`Detections`: A :py:class:`Detections` object with the map of regions containing the desired colors, and the detected lines, together with their center points and normals, + """ + + map, edge_color = self.colorFilter(color_range) + lines = self.houghLine(edge_color) + centers, normals = self.findNormal(map, lines) + return Detections(lines=lines, normals=normals, map=map, centers=centers) + + def detect_yellow_lines(self, img): + """ + Detects the line segments in the currently set image that occur in and the edges of the regions of the image + that are within the provided colour ranges. + + Args: + img (:py:class:`numpy.ndarray`): A :py:class:`numpy.ndarray` object specifying the image from camera. + + Returns: + :py:class:`Detections`: A :py:class:`Detections` object with the map of regions containing the desired colors, and the detected lines, together with their center points and normals, + """ + map_ = np.full(img.shape[:-1], 255, dtype=int) + debug_threshold_image, contours = self.detect_dash_line(img) + edges = self.get_edges_from_contours(img, contours) + + if not edges: + return Detections(lines=edges, normals=[], map=map_, centers=[]) + + edges = np.asarray(edges) + centers, normals = self.findNormal(map_, edges) + return Detections(lines=edges, normals=normals, map=map_, centers=centers) diff --git a/packages/custom_line_detector/include/custom_line_detector/plot_detections.py b/packages/custom_line_detector/include/custom_line_detector/plot_detections.py new file mode 100644 index 00000000..e54a44f8 --- /dev/null +++ b/packages/custom_line_detector/include/custom_line_detector/plot_detections.py @@ -0,0 +1,76 @@ +import cv2 +import numpy as np + + +def plotSegments(image, detections): + """ + + Draws a set of line segment detections on an image. + + Args: + image (:obj:`numpy array`): an image + detections (`dict`): a dictionary that has keys :py:class:`ColorRange` and values :py:class:`Detection` + + Returns: + :obj:`numpy array`: the image with the line segments drawn on top of it. + + """ + + im = np.copy(image) + + for color_range, det in list(detections.items()): + + # convert HSV color to BGR + c = color_range.representative + c = np.uint8([[[c[0], c[1], c[2]]]]) + color = cv2.cvtColor(c, cv2.COLOR_HSV2BGR).squeeze().astype(int) + # plot all detected line segments and their normals + for i in range(len(det.normals)): + center = det.centers[i] + normal = det.normals[i] + im = cv2.line( + im, + tuple(center.astype(int)), + tuple((center + 10 * normal).astype(int)), + color=(0, 0, 0), + thickness=2, + ) + # im = cv2.circle(im, (center[0], center[1]), radius=3, color=color, thickness=-1) + for line in det.lines: + im = cv2.line(im, (line[0], line[1]), (line[2], line[3]), color=(0, 0, 0), thickness=5) + im = cv2.line( + im, (line[0], line[1]), (line[2], line[3]), color=tuple([int(x) for x in color]), thickness=2 + ) + return im + + +def plotMaps(image, detections): + """ + + Draws a set of color filter maps (the part of the images falling in a given color range) on an image. + + Args: + image (:obj:`numpy array`): an image + detections (`dict`): a dictionary that has keys :py:class:`ColorRange` and values :py:class:`Detection` + + Returns: + :obj:`numpy array`: the image with the line segments drawn on top of it. + + """ + + im = np.copy(image) + im = cv2.cvtColor(cv2.cvtColor(im, cv2.COLOR_BGR2GRAY), cv2.COLOR_GRAY2BGR) + + color_map = np.zeros_like(im) + + for color_range, det in list(detections.items()): + + # convert HSV color to BGR + c = color_range.representative + c = np.uint8([[[c[0], c[1], c[2]]]]) + color = cv2.cvtColor(c, cv2.COLOR_HSV2BGR).squeeze().astype(int) + color_map[np.where(det.map)] = color + + im = cv2.addWeighted(im, 0.3, color_map, 1 - 0.3, 0.0) + + return im diff --git a/packages/custom_line_detector/launch/line_detector_node.launch b/packages/custom_line_detector/launch/line_detector_node.launch new file mode 100644 index 00000000..3cd96a44 --- /dev/null +++ b/packages/custom_line_detector/launch/line_detector_node.launch @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/packages/custom_line_detector/package.xml b/packages/custom_line_detector/package.xml new file mode 100644 index 00000000..744d39ff --- /dev/null +++ b/packages/custom_line_detector/package.xml @@ -0,0 +1,20 @@ + + + custom_line_detector + 1.0.0 + custom_line_detector node + Aleksandar Petrov + GPLv3 + + catkin + duckietown_msgs + image_processing + rospy + cv_bridge + + duckietown_msgs + image_processing + rospy + cv_bridge + + diff --git a/packages/custom_line_detector/setup.py b/packages/custom_line_detector/setup.py new file mode 100644 index 00000000..f3334bd9 --- /dev/null +++ b/packages/custom_line_detector/setup.py @@ -0,0 +1,12 @@ +## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +# fetch values from package.xml +setup_args = generate_distutils_setup( + packages=["custom_line_detector"], + package_dir={"": "include"}, +) + +setup(**setup_args) diff --git a/packages/custom_line_detector/src/line_detector_node.py b/packages/custom_line_detector/src/line_detector_node.py new file mode 100755 index 00000000..3c6ef50d --- /dev/null +++ b/packages/custom_line_detector/src/line_detector_node.py @@ -0,0 +1,375 @@ +#!/usr/bin/env python3 + +import numpy as np +import cv2 +import rospy +from cv_bridge import CvBridge +from sensor_msgs.msg import CompressedImage, Image, CameraInfo +from duckietown_msgs.msg import Segment, SegmentList, AntiInstagramThresholds +from custom_line_detector import LineDetector, ColorRange, plotSegments, plotMaps +from image_processing.anti_instagram import AntiInstagram +import os +from image_geometry import PinholeCameraModel +from duckietown.dtros import DTROS, NodeType, TopicType +# from camera_info_manager import CameraInfoManager + +class LineDetectorNode(DTROS): + """ + The ``LineDetectorNode`` is responsible for detecting the line white, yellow and red line segment in an image and + is used for lane localization. + + Upon receiving an image, this node reduces its resolution, cuts off the top part so that only the + road-containing part of the image is left, extracts the white, red, and yellow segments and publishes them. + The main functionality of this node is implemented in the :py:class:`custom_line_detector.LineDetector` class. + + The performance of this node can be very sensitive to its configuration parameters. Therefore, it also provides a + number of debug topics which can be used for fine-tuning these parameters. These configuration parameters can be + changed dynamically while the node is running via ``rosparam set`` commands. + + Args: + node_name (:obj:`str`): a unique, descriptive name for the node that ROS will use + + Configuration: + ~line_detector_parameters (:obj:`dict`): A dictionary with the parameters for the detector. The full list can be found in :py:class:`custom_line_detector.LineDetector`. + ~colors (:obj:`dict`): A dictionary of colors and color ranges to be detected in the image. The keys (color names) should match the ones in the Segment message definition, otherwise an exception will be thrown! See the ``config`` directory in the node code for the default ranges. + ~img_size (:obj:`list` of ``int``): The desired downsized resolution of the image. Lower resolution would result in faster detection but lower performance, default is ``[120,160]`` + ~top_cutoff (:obj:`int`): The number of rows to be removed from the top of the image _after_ resizing, default is 40 + + Subscriber: + ~camera_node/image/compressed (:obj:`sensor_msgs.msg.CompressedImage`): The camera images + ~anti_instagram_node/thresholds(:obj:`duckietown_msgs.msg.AntiInstagramThresholds`): The thresholds to do color correction + + Publishers: + ~segment_list (:obj:`duckietown_msgs.msg.SegmentList`): A list of the detected segments. Each segment is an :obj:`duckietown_msgs.msg.Segment` message + ~debug/segments/compressed (:obj:`sensor_msgs.msg.CompressedImage`): Debug topic with the segments drawn on the input image + ~debug/edges/compressed (:obj:`sensor_msgs.msg.CompressedImage`): Debug topic with the Canny edges drawn on the input image + ~debug/maps/compressed (:obj:`sensor_msgs.msg.CompressedImage`): Debug topic with the regions falling in each color range drawn on the input image + ~debug/ranges_HS (:obj:`sensor_msgs.msg.Image`): Debug topic with a histogram of the colors in the input image and the color ranges, Hue-Saturation projection + ~debug/ranges_SV (:obj:`sensor_msgs.msg.Image`): Debug topic with a histogram of the colors in the input image and the color ranges, Saturation-Value projection + ~debug/ranges_HV (:obj:`sensor_msgs.msg.Image`): Debug topic with a histogram of the colors in the input image and the color ranges, Hue-Value projection + + """ + + def __init__(self, node_name): + # Initialize the DTROS parent class + super(LineDetectorNode, self).__init__(node_name=node_name, node_type=NodeType.PERCEPTION) + + # Define parameters + self._line_detector_parameters = rospy.get_param("~line_detector_parameters", None) + self._colors = rospy.get_param("~colors", None) + self._img_size = rospy.get_param("~img_size", None) + self._top_cutoff = rospy.get_param("~top_cutoff", None) + self.vehicle = os.getenv('HOSTNAME') + self.bridge = CvBridge() + self.rectify_alpha = rospy.get_param("~rectify_alpha", 0.0) + + # The thresholds to be used for AntiInstagram color correction + self.ai_thresholds_received = False + self.anti_instagram_thresholds = dict() + self.ai = AntiInstagram() + + # This holds the colormaps for the debug/ranges images after they are computed once + self.colormaps = dict() + + # Create a new LineDetector object with the parameters from the Parameter Server / config file + self.detector = LineDetector(**self._line_detector_parameters) + # Update the color ranges objects + self.color_ranges = {color: ColorRange.fromDict(d) for color, d in list(self._colors.items())} + + # Publishers + self.pub_lines = rospy.Publisher( + "~segment_list", SegmentList, queue_size=1, dt_topic_type=TopicType.PERCEPTION + ) + self.pub_d_segments = rospy.Publisher( + "~debug/segments/compressed", CompressedImage, queue_size=1, dt_topic_type=TopicType.DEBUG + ) + self.pub_d_edges = rospy.Publisher( + "~debug/edges/compressed", CompressedImage, queue_size=1, dt_topic_type=TopicType.DEBUG + ) + self.pub_d_maps = rospy.Publisher( + "~debug/maps/compressed", CompressedImage, queue_size=1, dt_topic_type=TopicType.DEBUG + ) + # these are not compressed because compression adds undesired blur + self.pub_d_ranges_HS = rospy.Publisher( + "~debug/ranges_HS", Image, queue_size=1, dt_topic_type=TopicType.DEBUG + ) + self.pub_d_ranges_SV = rospy.Publisher( + "~debug/ranges_SV", Image, queue_size=1, dt_topic_type=TopicType.DEBUG + ) + self.pub_d_ranges_HV = rospy.Publisher( + "~debug/ranges_HV", Image, queue_size=1, dt_topic_type=TopicType.DEBUG + ) + self.pub_contours = rospy.Publisher( + "~debug/contours/compressed", CompressedImage, queue_size=1, dt_topic_type=TopicType.DEBUG + ) + + # Subscribers + self.sub_image = rospy.Subscriber( + "~image/compressed", CompressedImage, self.image_cb, buff_size=10000000, queue_size=1 + ) + + self.sub_thresholds = rospy.Subscriber( + "~thresholds", AntiInstagramThresholds, self.thresholds_cb, queue_size=1 + ) + + self.sub_cam_info = rospy.Subscriber( + '/{}/camera_node/camera_info'.format(self.vehicle), CameraInfo, self._cinfo_cb, queue_size=1 + ) + + def _cinfo_cb(self, msg): + # create mapx and mapy + H, W = msg.height, msg.width + + # create new camera info + self.camera_model = PinholeCameraModel() + self.camera_model.fromCameraInfo(msg) + # find optimal rectified pinhole camera + with self.profiler("/cb/camera_info/get_optimal_new_camera_matrix"): + rect_K, _ = cv2.getOptimalNewCameraMatrix( + self.camera_model.K, self.camera_model.D, (W, H), self.rectify_alpha + ) + # store new camera parameters + self._camera_parameters = (rect_K[0, 0], rect_K[1, 1], rect_K[0, 2], rect_K[1, 2]) + # create rectification map + with self.profiler("/cb/camera_info/init_undistort_rectify_map"): + self._mapx, self._mapy = cv2.initUndistortRectifyMap( + self.camera_model.K, self.camera_model.D, None, rect_K, (W, H), cv2.CV_32FC1 + ) + # once we got the camera info, we can stop the subscriber + # self.loginfo("Camera info message received. Unsubscribing from camera_info topic.") + # noinspection PyBroadException + try: + self._cinfo_sub.shutdown() + except BaseException: + pass + + def thresholds_cb(self, thresh_msg): + self.anti_instagram_thresholds["lower"] = thresh_msg.low + self.anti_instagram_thresholds["higher"] = thresh_msg.high + self.ai_thresholds_received = True + + def image_cb(self, image_msg): + """ + Processes the incoming image messages. + + Performs the following steps for each incoming image: + + #. Performs color correction + #. Resizes the image to the ``~img_size`` resolution + #. Removes the top ``~top_cutoff`` rows in order to remove the part of the image that doesn't include the road + #. Extracts the line segments in the image using :py:class:`custom_line_detector.LineDetector` + #. Converts the coordinates of detected segments to normalized ones + #. Creates and publishes the resultant :obj:`duckietown_msgs.msg.SegmentList` message + #. Creates and publishes debug images if there is a subscriber to the respective topics + + Args: + image_msg (:obj:`sensor_msgs.msg.CompressedImage`): The receive image message + + """ + + # Decode from compressed image with OpenCV + try: + image = self.bridge.compressed_imgmsg_to_cv2(image_msg) + except ValueError as e: + self.logerr(f"Could not decode image: {e}") + return + + # Perform color correction + if self.ai_thresholds_received: + image = self.ai.apply_color_balance( + self.anti_instagram_thresholds["lower"], self.anti_instagram_thresholds["higher"], image + ) + + image = self.cut_image(image) + + # Extract the line segments for every color + self.detector.setImage(image) + # detections = {} + detections = { + color: self.detector.detectLines(ranges) for color, ranges in list(self.color_ranges.items()) + } + for color, ranges in list(self.color_ranges.items()): + if color == 'YELLOW': + detections['YELLOW'] = self.detector.detect_yellow_lines(image) + + # Construct a SegmentList + segment_list = SegmentList() + segment_list.header.stamp = image_msg.header.stamp + + # Remove the offset in coordinates coming from the removing of the top part and + arr_cutoff = np.array([0, self._top_cutoff, 0, self._top_cutoff]) + arr_ratio = np.array( + [ + 1.0 / self._img_size[1], + 1.0 / self._img_size[0], + 1.0 / self._img_size[1], + 1.0 / self._img_size[0], + ] + ) + + # Fill in the segment_list with all the detected segments + for color, det in list(detections.items()): + # Get the ID for the color from the Segment msg definition + # Throw and exception otherwise + if len(det.lines) > 0 and len(det.normals) > 0: + try: + color_id = getattr(Segment, color) + lines_normalized = (det.lines + arr_cutoff) * arr_ratio + segment_list.segments.extend( + self._to_segment_msg(lines_normalized, det.normals, color_id) + ) + except AttributeError: + self.logerr(f"Color name {color} is not defined in the Segment message") + + # Publish the message + self.pub_lines.publish(segment_list) + + # If there are any subscribers to the debug topics, generate a debug image and publish it + if self.pub_d_segments.get_num_connections() > 0: + colorrange_detections = {self.color_ranges[c]: det for c, det in list(detections.items())} + debug_img = plotSegments(image, colorrange_detections) + debug_image_msg = self.bridge.cv2_to_compressed_imgmsg(debug_img) + debug_image_msg.header = image_msg.header + self.pub_d_segments.publish(debug_image_msg) + + if self.pub_d_edges.get_num_connections() > 0: + debug_image_msg = self.bridge.cv2_to_compressed_imgmsg(self.detector.canny_edges) + debug_image_msg.header = image_msg.header + self.pub_d_edges.publish(debug_image_msg) + + if self.pub_d_maps.get_num_connections() > 0: + colorrange_detections = {self.color_ranges[c]: det for c, det in list(detections.items())} + debug_img = plotMaps(image, colorrange_detections) + debug_image_msg = self.bridge.cv2_to_compressed_imgmsg(debug_img) + debug_image_msg.header = image_msg.header + self.pub_d_maps.publish(debug_image_msg) + + if self.pub_contours.get_num_connections() > 0: + # image = cv2.undistort(image, self.camera_model.K, self.camera_model.D) + image, _ = self.detector.detect_dash_line(image) + debug_image_msg = self.bridge.cv2_to_compressed_imgmsg(image) + debug_image_msg.header = image_msg.header + self.pub_contours.publish(debug_image_msg) + + for channels in ["HS", "SV", "HV"]: + publisher = getattr(self, f"pub_d_ranges_{channels}") + if publisher.get_num_connections() > 0: + debug_img = self._plot_ranges_histogram(channels) + debug_image_msg = self.bridge.cv2_to_imgmsg(debug_img, encoding="bgr8") + debug_image_msg.header = image_msg.header + publisher.publish(debug_image_msg) + + def cut_image(self, image): + height_original, width_original = image.shape[0:2] + img_size = (self._img_size[1], self._img_size[0]) + if img_size[0] != width_original or img_size[1] != height_original: + image = cv2.resize(image, img_size, interpolation=cv2.INTER_NEAREST) + image = image[self._top_cutoff :, :, :] + return image + + @staticmethod + def _to_segment_msg(lines, normals, color): + """ + Converts line detections to a list of Segment messages. + + Converts the resultant line segments and normals from the line detection to a list of Segment messages. + + Args: + lines (:obj:`numpy array`): An ``Nx4`` array where each row represents a line. + normals (:obj:`numpy array`): An ``Nx2`` array where each row represents the normal of a line. + color (:obj:`str`): Color name string, should be one of the pre-defined in the Segment message definition. + + Returns: + :obj:`list` of :obj:`duckietown_msgs.msg.Segment`: List of Segment messages + + """ + segment_msg_list = [] + for x1, y1, x2, y2, norm_x, norm_y in np.hstack((lines, normals)): + segment = Segment() + segment.color = color + segment.pixels_normalized[0].x = x1 + segment.pixels_normalized[0].y = y1 + segment.pixels_normalized[1].x = x2 + segment.pixels_normalized[1].y = y2 + segment.normal.x = norm_x + segment.normal.y = norm_y + segment_msg_list.append(segment) + return segment_msg_list + + def _plot_ranges_histogram(self, channels): + """Utility method for plotting color histograms and color ranges. + + Args: + channels (:obj:`str`): The desired two channels, should be one of ``['HS','SV','HV']`` + + Returns: + :obj:`numpy array`: The resultant plot image + + """ + channel_to_axis = {"H": 0, "S": 1, "V": 2} + axis_to_range = {0: 180, 1: 256, 2: 256} + + # Get which is the third channel that will not be shown in this plot + missing_channel = "HSV".replace(channels[0], "").replace(channels[1], "") + + hsv_im = self.detector.hsv + # Get the pixels as a list (flatten the horizontal and vertical dimensions) + hsv_im = hsv_im.reshape((-1, 3)) + + channel_idx = [channel_to_axis[channels[0]], channel_to_axis[channels[1]]] + + # Get only the relevant channels + x_bins = np.arange(0, axis_to_range[channel_idx[1]] + 1, 2) + y_bins = np.arange(0, axis_to_range[channel_idx[0]] + 1, 2) + h, _, _ = np.histogram2d( + x=hsv_im[:, channel_idx[0]], y=hsv_im[:, channel_idx[1]], bins=[y_bins, x_bins] + ) + # Log-normalized histogram + np.log(h, out=h, where=(h != 0)) + h = (255 * h / np.max(h)).astype(np.uint8) + + # Make a color map, for the missing channel, just take the middle of the range + if channels not in self.colormaps: + colormap_1, colormap_0 = np.meshgrid(x_bins[:-1], y_bins[:-1]) + colormap_2 = np.ones_like(colormap_0) * (axis_to_range[channel_to_axis[missing_channel]] / 2) + + channel_to_map = {channels[0]: colormap_0, channels[1]: colormap_1, missing_channel: colormap_2} + + self.colormaps[channels] = np.stack( + [channel_to_map["H"], channel_to_map["S"], channel_to_map["V"]], axis=-1 + ).astype(np.uint8) + self.colormaps[channels] = cv2.cvtColor(self.colormaps[channels], cv2.COLOR_HSV2BGR) + + # resulting histogram image as a blend of the two images + im = cv2.cvtColor(h[:, :, None], cv2.COLOR_GRAY2BGR) + im = cv2.addWeighted(im, 0.5, self.colormaps[channels], 1 - 0.5, 0.0) + + # now plot the color ranges on top + for _, color_range in list(self.color_ranges.items()): + # convert HSV color to BGR + c = color_range.representative + c = np.uint8([[[c[0], c[1], c[2]]]]) + color = cv2.cvtColor(c, cv2.COLOR_HSV2BGR).squeeze().astype(int) + for i in range(len(color_range.low)): + cv2.rectangle( + im, + pt1=( + (color_range.high[i, channel_idx[1]] / 2).astype(np.uint8), + (color_range.high[i, channel_idx[0]] / 2).astype(np.uint8), + ), + pt2=( + (color_range.low[i, channel_idx[1]] / 2).astype(np.uint8), + (color_range.low[i, channel_idx[0]] / 2).astype(np.uint8), + ), + color=color, + lineType=cv2.LINE_4, + ) + # --- + return im + + +if __name__ == "__main__": + # Initialize the node + line_detector_node = LineDetectorNode(node_name="line_detector_node") + # Keep it spinning to keep the node alive + rospy.spin() diff --git a/packages/dt-core/packages/apriltag/launch/apriltag_detector_node.launch b/packages/dt-core/packages/apriltag/launch/apriltag_detector_node.launch new file mode 100644 index 00000000..9d3e2703 --- /dev/null +++ b/packages/dt-core/packages/apriltag/launch/apriltag_detector_node.launch @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/packages/dt-core/packages/apriltag/src/apriltag_detector_node.py b/packages/dt-core/packages/apriltag/src/apriltag_detector_node.py new file mode 100644 index 00000000..5a272335 --- /dev/null +++ b/packages/dt-core/packages/apriltag/src/apriltag_detector_node.py @@ -0,0 +1,242 @@ +#!/usr/bin/env python3 + +import cv2 +import rospy +import tf +import numpy as np + +from threading import Thread +from concurrent.futures import ThreadPoolExecutor +from turbojpeg import TurboJPEG, TJPF_GRAY +from image_geometry import PinholeCameraModel +from dt_apriltags import Detector + +from dt_class_utils import DTReminder +from duckietown.dtros import DTROS, NodeType, TopicType, DTParam, ParamType + +from duckietown_msgs.msg import AprilTagDetectionArray, AprilTagDetection, FSMState +from sensor_msgs.msg import CameraInfo, CompressedImage +from geometry_msgs.msg import Transform, Vector3, Quaternion + + +class AprilTagDetector(DTROS): + def __init__(self): + super(AprilTagDetector, self).__init__( + node_name="apriltag_detector_node", node_type=NodeType.PERCEPTION + ) + # get static parameters + self.family = rospy.get_param("~family", "tag36h11") + self.ndetectors = rospy.get_param("~ndetectors", 1) + self.nthreads = rospy.get_param("~nthreads", 1) + self.quad_decimate = rospy.get_param("~quad_decimate", 1.0) + self.quad_sigma = rospy.get_param("~quad_sigma", 0.0) + self.refine_edges = rospy.get_param("~refine_edges", 1) + self.decode_sharpening = rospy.get_param("~decode_sharpening", 0.25) + self.tag_size = rospy.get_param("~tag_size", 0.065) + self.rectify_alpha = rospy.get_param("~rectify_alpha", 0.0) + self.state = "JOYSTICK_CONTROL" + # dynamic parameter + self.detection_freq = DTParam( + "~detection_freq", default=-1, param_type=ParamType.INT, min_value=-1, max_value=30 + ) + self._detection_reminder = DTReminder(frequency=self.detection_freq.value) + # camera info + self._camera_parameters = None + self._mapx, self._mapy = None, None + # create detector object + self._detectors = [ + Detector( + families=self.family, + nthreads=self.nthreads, + quad_decimate=self.quad_decimate, + quad_sigma=self.quad_sigma, + refine_edges=self.refine_edges, + decode_sharpening=self.decode_sharpening, + ) + for _ in range(self.ndetectors) + ] + self._renderer_busy = False + # create a CV bridge object + self._jpeg = TurboJPEG() + # create subscribers + self._img_sub = rospy.Subscriber( + "~image", CompressedImage, self._img_cb, queue_size=1, buff_size="20MB" + ) + self._cinfo_sub = rospy.Subscriber("~camera_info", CameraInfo, self._cinfo_cb, queue_size=1) + self.sub_fsm = rospy.Subscriber("~fsm_node/mode", FSMState, self.cbFSMState) + # create publisher + self._tag_pub = rospy.Publisher( + "~detections", + AprilTagDetectionArray, + queue_size=1, + dt_topic_type=TopicType.PERCEPTION, + dt_help="Tag detections", + ) + self._img_pub = rospy.Publisher( + "~detections/image/compressed", + CompressedImage, + queue_size=1, + dt_topic_type=TopicType.VISUALIZATION, + dt_help="Camera image with tag publishs superimposed", + ) + # create thread pool + self._workers = ThreadPoolExecutor(self.ndetectors) + self._tasks = [None] * self.ndetectors + # create TF broadcaster + self._tf_bcaster = tf.TransformBroadcaster() + + def cbFSMState(self, msg): + self.state = msg.state + + def on_shutdown(self): + self.loginfo("Shutting down workers pool") + self._workers.shutdown() + + def _cinfo_cb(self, msg): + # create mapx and mapy + H, W = msg.height, msg.width + # create new camera info + self.camera_model = PinholeCameraModel() + self.camera_model.fromCameraInfo(msg) + # find optimal rectified pinhole camera + with self.profiler("/cb/camera_info/get_optimal_new_camera_matrix"): + rect_K, _ = cv2.getOptimalNewCameraMatrix( + self.camera_model.K, self.camera_model.D, (W, H), self.rectify_alpha + ) + # store new camera parameters + self._camera_parameters = (rect_K[0, 0], rect_K[1, 1], rect_K[0, 2], rect_K[1, 2]) + # create rectification map + with self.profiler("/cb/camera_info/init_undistort_rectify_map"): + self._mapx, self._mapy = cv2.initUndistortRectifyMap( + self.camera_model.K, self.camera_model.D, None, rect_K, (W, H), cv2.CV_32FC1 + ) + # once we got the camera info, we can stop the subscriber + self.loginfo("Camera info message received. Unsubscribing from camera_info topic.") + # noinspection PyBroadException + try: + self._cinfo_sub.shutdown() + except BaseException: + pass + + def _detect(self, detector_id, msg): + # turn image message into grayscale image + with self.profiler("/cb/image/decode"): + img = self._jpeg.decode(msg.data, pixel_format=TJPF_GRAY) + # run input image through the rectification map + with self.profiler("/cb/image/rectify"): + img = cv2.remap(img, self._mapx, self._mapy, cv2.INTER_NEAREST) + # detect tags + with self.profiler("/cb/image/detection"): + tags = self._detectors[detector_id].detect(img, True, self._camera_parameters, self.tag_size) + # pack detections into a message + tags_msg = AprilTagDetectionArray() + tags_msg.header.stamp = msg.header.stamp + tags_msg.header.frame_id = msg.header.frame_id + for tag in tags: + # turn rotation matrix into quaternion + q = _matrix_to_quaternion(tag.pose_R) + p = tag.pose_t.T[0] + # create single tag detection object + detection = AprilTagDetection( + transform=Transform( + translation=Vector3(x=p[0], y=p[1], z=p[2]), + rotation=Quaternion(x=q[0], y=q[1], z=q[2], w=q[3]), + ), + tag_id=tag.tag_id, + tag_family=str(tag.tag_family), + hamming=tag.hamming, + decision_margin=tag.decision_margin, + homography=tag.homography.flatten().astype(np.float32).tolist(), + center=tag.center.tolist(), + corners=tag.corners.flatten().tolist(), + pose_error=tag.pose_err, + ) + # add detection to array + tags_msg.detections.append(detection) + # publish tf + self._tf_bcaster.sendTransform( + p.tolist(), + q.tolist(), + msg.header.stamp, + "tag/{:s}".format(str(tag.tag_id)), + msg.header.frame_id, + ) + # publish detections + self._tag_pub.publish(tags_msg) + # update healthy frequency metadata + self._tag_pub.set_healthy_freq(self._img_sub.get_frequency()) + self._img_pub.set_healthy_freq(self._img_sub.get_frequency()) + # render visualization (if needed) + if self._img_pub.anybody_listening() and not self._renderer_busy: + self._renderer_busy = True + Thread(target=self._render_detections, args=(msg, img, tags)).start() + + def _img_cb(self, msg): + if self.state != "INTERSECTION_COORDINATION": + return + # make sure we have received camera info + if self._camera_parameters is None: + return + # make sure we have a rectification map available + if self._mapx is None or self._mapy is None: + return + # make sure somebody wants this + if (not self._img_pub.anybody_listening()) and (not self._tag_pub.anybody_listening()): + return + # make sure this is a good time to detect (always keep this as last check) + if not self._detection_reminder.is_time(frequency=self.detection_freq.value): + return + # make sure we are still running + if self.is_shutdown: + return + # --- + # find the first available worker (if any) + for i in range(self.ndetectors): + if self._tasks[i] is None or self._tasks[i].done(): + # submit this image to the pool + self._tasks[i] = self._workers.submit(self._detect, i, msg) + break + + def _render_detections(self, msg, img, detections): + with self.profiler("/publishs_image"): + # get a color buffer from the BW image + img = cv2.cvtColor(img, cv2.COLOR_GRAY2RGB) + # draw each tag + for detection in detections: + for idx in range(len(detection.corners)): + cv2.line( + img, + tuple(detection.corners[idx - 1, :].astype(int)), + tuple(detection.corners[idx, :].astype(int)), + (0, 255, 0), + ) + # draw the tag ID + cv2.putText( + img, + str(detection.tag_id), + org=(detection.corners[0, 0].astype(int) + 10, detection.corners[0, 1].astype(int) + 10), + fontFace=cv2.FONT_HERSHEY_SIMPLEX, + fontScale=0.8, + color=(0, 0, 255), + ) + # pack image into a message + img_msg = CompressedImage() + img_msg.header.stamp = msg.header.stamp + img_msg.header.frame_id = msg.header.frame_id + img_msg.format = "jpeg" + img_msg.data = self._jpeg.encode(img) + # --- + self._img_pub.publish(img_msg) + self._renderer_busy = False + + +def _matrix_to_quaternion(r): + T = np.array(((0, 0, 0, 0), (0, 0, 0, 0), (0, 0, 0, 0), (0, 0, 0, 1)), dtype=np.float64) + T[0:3, 0:3] = r + return tf.transformations.quaternion_from_matrix(T) + + +if __name__ == "__main__": + node = AprilTagDetector() + # spin forever + rospy.spin() \ No newline at end of file diff --git a/packages/dt-core/packages/explicit_coordinator/src/coordinator_node.py b/packages/dt-core/packages/explicit_coordinator/src/coordinator_node.py new file mode 100644 index 00000000..4b4346a6 --- /dev/null +++ b/packages/dt-core/packages/explicit_coordinator/src/coordinator_node.py @@ -0,0 +1,302 @@ +#!/usr/bin/env python3 + +from random import random +import rospy +from duckietown_msgs.msg import ( + CoordinationClearance, + FSMState, + BoolStamped, + Twist2DStamped, + AprilTagsWithInfos, +) +from duckietown_msgs.msg import SignalsDetection, CoordinationSignal, MaintenanceState +from std_msgs.msg import String +from time import time + +UNKNOWN = "UNKNOWN" + + +class State: + INTERSECTION_PLANNING = "INTERSECTION_PLANNING" + LANE_FOLLOWING = "LANE_FOLLOWING" + AT_STOP_CLEARING = "AT_STOP_CLEARING" + SACRIFICE = "SACRIFICE" + SOLVING_UNKNOWN = "SOLVING_UNKNOWN" + GO = "GO" + KEEP_CALM = "KEEP_CALM" + TL_SENSING = "TL_SENSING" + INTERSECTION_CONTROL = "INTERSECTION_CONTROL" + AT_STOP_CLEARING_AND_PRIORITY = "AT_STOP_CLEARING_AND_PRIORITY" + SACRIFICE_FOR_PRIORITY = "SACRIFICE_FOR_PRIORITY" + OBSTACLE_ALERT = "OBSTACLE_ALERT" + OBSTACLE_STOP = "OBSTACLE_STOP" + + +class VehicleCoordinator: + """The Vehicle Coordination Module for Duckiebot""" + + T_MAX_RANDOM = 5.0 # seconds + T_CROSS = 6.0 # seconds + T_SENSE = 2.0 # seconds + T_UNKNOWN = 1.0 # seconds + T_MIN_RANDOM = 2.0 # seconds + T_KEEP_CALM = 4.0 # seconds + + def __init__(self): + + self.node = rospy.init_node("veh_coordinator", anonymous=True) + + # We communicate that the coordination mode has started + rospy.loginfo("The Coordination Mode has Started") + + self.active = True + + # Determine the state of the bot + self.state = State.INTERSECTION_PLANNING + self.last_state_transition = time() + self.random_delay = 0 + self.priority = False + + # Node name + self.node_name = rospy.get_name() + + # Initialize flag + self.intersection_go_published = False + + # Parameters + self.traffic_light_intersection = UNKNOWN + + self.use_priority_protocol = True + if rospy.get_param("~use_priority_protocol") == False: + self.use_priority_protocol = False + + self.tl_timeout = 120 + rospy.set_param("~tl_timeout", self.tl_timeout) + + # Initialize detection + self.traffic_light = UNKNOWN + self.right_veh = UNKNOWN + self.opposite_veh = UNKNOWN + + # Initialize mode + self.mode = "INTERSECTION_PLANNING" + + # Subscriptions + self.sub_switch = rospy.Subscriber("~switch", BoolStamped, self.cbSwitch, queue_size=1) + + rospy.Subscriber("~mode", FSMState, lambda msg: self.set("mode", msg.state)) + rospy.Subscriber("~apriltags_out", AprilTagsWithInfos, self.set_traffic_light) + rospy.Subscriber("~signals_detection", SignalsDetection, self.process_signals_detection) + rospy.Subscriber("~maintenance_state", MaintenanceState, self.cbMaintenanceState) + + # Initialize clearance to go + self.clearance_to_go = CoordinationClearance.NA + + # Set the light to be off + self.roof_light = CoordinationSignal.OFF + + # Publishing + self.clearance_to_go_pub = rospy.Publisher("~clearance_to_go", CoordinationClearance, queue_size=10) + self.pub_intersection_go = rospy.Publisher("~intersection_go", BoolStamped, queue_size=1) + self.pub_coord_cmd = rospy.Publisher("~car_cmd", Twist2DStamped, queue_size=1) + self.roof_light_pub = rospy.Publisher("~change_color_pattern", String, queue_size=10) + self.coordination_state_pub = rospy.Publisher("~coordination_state", String, queue_size=10) + + # Update param timer + rospy.Timer(rospy.Duration.from_sec(1.0), self.updateParams) + + while not rospy.is_shutdown(): + self.loop() + rospy.sleep(0.1) + + def cbMaintenanceState(self, msg): + if msg.state == "WAY_TO_MAINTENANCE" and self.use_priority_protocol: + self.priority = True + rospy.loginfo(f"[{self.node_name}] Granted priority rights on intersections.") + else: + self.priority = False + + def set_traffic_light(self, msg): + # Save old traffic light + traffic_light_old = self.traffic_light_intersection + # New traffic light + # TODO: only consider two closest signs + for item in msg.infos: + if item.traffic_sign_type == 17: + self.traffic_light_intersection = True + break + else: + self.traffic_light_intersection = False + # If different from the one before, restart from lane following + if traffic_light_old != self.traffic_light_intersection: + self.set_state(State.INTERSECTION_PLANNING) + + # Print result + # if self.traffic_light_intersection != UNKNOWN: + # #TODO if tl but can't see the led's for too long, switch to april tag intersection + # # Print + # if self.traffic_light_intersection: + # rospy.loginfo('[%s] Intersection with traffic light' %(self.node_name)) + # else: + # rospy.loginfo('[%s] Intersection without traffic light' %(self.node_name)) + + def set_state(self, state): + # Update only when changing state + if self.state != state: + rospy.loginfo(f"[{self.node_name}] Transitioned from {self.state} to {state}") + self.last_state_transition = time() + self.state = state + + # Set roof light + if self.state == State.AT_STOP_CLEARING: + # self.reset_signals_detection() + self.roof_light = CoordinationSignal.SIGNAL_A + elif self.state == State.AT_STOP_CLEARING_AND_PRIORITY: + self.roof_light = CoordinationSignal.SIGNAL_PRIORITY + # Publish LEDs - priority interrupt + # self.roof_light_pub.publish(self.roof_light) + elif self.state == State.SACRIFICE_FOR_PRIORITY: + self.roof_light = CoordinationSignal.SIGNAL_SACRIFICE_FOR_PRIORITY + # Publish LEDs - priority interrupt + # self.roof_light_pub.publish(self.roof_light) + elif self.state == State.SACRIFICE: + self.roof_light = CoordinationSignal.OFF + elif self.state == State.KEEP_CALM: + if self.priority: + self.roof_light = CoordinationSignal.SIGNAL_PRIORITY + else: + self.roof_light = CoordinationSignal.SIGNAL_A + elif self.state == State.GO and not self.traffic_light_intersection: + self.roof_light = CoordinationSignal.SIGNAL_GREEN + elif self.state == State.INTERSECTION_PLANNING or self.state == State.TL_SENSING: + self.roof_light = CoordinationSignal.OFF + + # rospy.logdebug('[coordination_node] Transitioned to state' + self.state) + + # Define the time at this current state + def time_at_current_state(self): + return time() - self.last_state_transition + + def set(self, name, value): + + self.__dict__[name] = value + + # Initialization of the state and of the type of intersection + if name == "mode": + if value == "JOYSTICK_CONTROL" or value == "INTERSECTION_COORDINATION": + self.set_state(State.INTERSECTION_PLANNING) + self.traffic_light_intersection = UNKNOWN + + # Definition of each signal detection + def process_signals_detection(self, msg): + self.set("traffic_light", msg.traffic_light_state) + self.set("right_veh", msg.right) + self.set("opposite_veh", msg.front) + + # definition which resets everything we know + def reset_signals_detection(self): + self.traffic_light = UNKNOWN + self.right_veh = UNKNOWN + self.opposite_veh = UNKNOWN + + # publishing the topics + def publish_topics(self): + now = rospy.Time.now() + + # Publish LEDs + self.roof_light_pub.publish(self.roof_light) + + # Clearance to go + self.clearance_to_go_pub.publish(CoordinationClearance(status=self.clearance_to_go)) + + # Publish intersection_go flag + # rospy.loginfo("clearance_to_go is "+str(self.clearance_to_go) + " and CoordinationClearance.GO is "+str(CoordinationClearance.GO)) + if self.clearance_to_go == CoordinationClearance.GO and not self.intersection_go_published: + msg = BoolStamped() + msg.header.stamp = now + msg.data = True + self.pub_intersection_go.publish(msg) + self.intersection_go_published = True + + rospy.loginfo(f"[{self.node_name}] Go!") + + # Publish LEDs + # self.roof_light_pub.publish(self.roof_light) + + car_cmd_msg = Twist2DStamped(v=0.0, omega=0.0) + car_cmd_msg.header.stamp = now + self.pub_coord_cmd.publish(car_cmd_msg) + self.coordination_state_pub.publish(data=self.state) + + # definition of the loop + def loop(self): + + if not self.active: + return + + if self.traffic_light_intersection != UNKNOWN: + self.reconsider() + self.publish_topics() + + def reconsider(self): + + if self.state == State.INTERSECTION_PLANNING: + if self.mode == "INTERSECTION_COORDINATION": + self.set_state(State.AT_STOP_CLEARING) + + elif self.state == State.AT_STOP_CLEARING: + # No cars detected + self.set_state(State.KEEP_CALM) + + elif self.state == State.GO: + self.clearance_to_go = CoordinationClearance.GO + if self.mode == "INTERSECTION_PLANNING": + self.set_state(State.INTERSECTION_PLANNING) + + elif self.state == State.KEEP_CALM: + # No cars detected + if self.time_at_current_state() > self.T_KEEP_CALM: + self.set_state(State.GO) + + elif self.state == State.TL_SENSING: + rospy.loginfo( + "[%s] I have been waiting in traffic light for: %s", self.node_name, (time() - self.begin_tl) + ) + if self.traffic_light == "traffic_light_go": + rospy.loginfo("[%s] Traffic light is green. I have priority. GO!", self.node_name) + self.set_state(State.GO) + + # If a tl intersection april tag is present but tl is switched off, wait until tl_timeout then use led coordination + elif time() - self.begin_tl > self.tl_timeout: + if self.priority: + self.set_state(State.AT_STOP_CLEARING_AND_PRIORITY) + else: + self.set_state(State.AT_STOP_CLEARING) + + # If not GO, pusblish wait + if self.state != State.GO: + # Initialize intersection_go_published + self.intersection_go_published = False + # Publish wait + self.clearance_to_go = CoordinationClearance.WAIT + + def cbSwitch(self, switch_msg): + self.active = switch_msg.data + + def updateParams(self, event): + self.tl_timeout = rospy.get_param("~tl_timeout") + + # def onShutdown(self): + # rospy.loginfo("[CoordinatorNode] Shutdown.") + # self.clearance_to_go_pub.unregister() + # self.pub_intersection_go.unregister() + # self.pub_coord_cmd.unregister() + # self.roof_light_pub.unregister() + # self.coordination_state_pub.unregister() + # + + +if __name__ == "__main__": + car = VehicleCoordinator() + # rospy.on_shutdown(coordinator_node.onShutdown) + rospy.spin() \ No newline at end of file diff --git a/packages/dt-core/packages/lane_filter/config/lane_filter_node/default.yaml b/packages/dt-core/packages/lane_filter/config/lane_filter_node/default.yaml deleted file mode 100644 index cbc5b5e6..00000000 --- a/packages/dt-core/packages/lane_filter/config/lane_filter_node/default.yaml +++ /dev/null @@ -1,23 +0,0 @@ -use_propagation: True -debug : False -lane_filter_histogram_configuration: - mean_d_0: 0 - mean_phi_0: 0 - sigma_d_0: 0.1 - sigma_phi_0: 0.1 - delta_d: 0.02 - delta_phi: 0.1 - d_max: 0.3 - d_min: -0.15 - phi_min: -0.5 - phi_max: 0.5 - cov_v: 0.5 - linewidth_white: 0.05 - linewidth_yellow: 0.025 - lanewidth: 0.23 - min_max: 0.1 - sigma_d_mask: 1.0 - sigma_phi_mask: 2.0 - range_min: 0.2 - range_est: 0.33 - range_max: 0.6 diff --git a/packages/dt-core/packages/navigation/src/random_april_tag_turns_node.py b/packages/dt-core/packages/navigation/src/random_april_tag_turns_node.py index a51fe491..35e87f0e 100644 --- a/packages/dt-core/packages/navigation/src/random_april_tag_turns_node.py +++ b/packages/dt-core/packages/navigation/src/random_april_tag_turns_node.py @@ -75,27 +75,26 @@ def cbTag(self, tag_msgs): signType = taginfo.traffic_sign_type if signType == taginfo.NO_RIGHT_TURN or signType == taginfo.LEFT_T_INTERSECT: availableTurns = [ - 0, 1, ] # these mystical numbers correspond to the array ordering in open_loop_intersection_control_node (very bad) elif signType == taginfo.NO_LEFT_TURN or signType == taginfo.RIGHT_T_INTERSECT: availableTurns = [1, 2] elif signType == taginfo.FOUR_WAY: - availableTurns = [0, 1, 2] + availableTurns = [1, 2] elif signType == taginfo.T_INTERSECTION: - availableTurns = [0, 2] + availableTurns = [2] rospy.loginfo(f"[{self.node_name}] reports Available turns are: [{availableTurns}]") # now randomly choose a possible direction if len(availableTurns) > 0: # 3501: turn off right turns - # randomIndex = numpy.random.randint(len(availableTurns)) - # chosenTurn = availableTurns[randomIndex] - while True: - randomIndex = numpy.random.randint(len(availableTurns)) - chosenTurn = availableTurns[randomIndex] - rospy.loginfo("Turn type now: %i" %(chosenTurn)) - if chosenTurn != 2: - break + randomIndex = numpy.random.randint(len(availableTurns)) + chosenTurn = availableTurns[randomIndex] + #while True: + # randomIndex = numpy.random.randint(len(availableTurns)) + # chosenTurn = availableTurns[randomIndex] + # rospy.loginfo("Turn type now: %i" %(chosenTurn)) + # if chosenTurn != 2: + # break # end of fix self.turn_type = chosenTurn diff --git a/packages/dt-core/packages/unicorn_intersection/config/unicorn_intersection_node/default.yaml b/packages/dt-core/packages/unicorn_intersection/config/unicorn_intersection_node/default.yaml index c9813643..87b4f501 100644 --- a/packages/dt-core/packages/unicorn_intersection/config/unicorn_intersection_node/default.yaml +++ b/packages/dt-core/packages/unicorn_intersection/config/unicorn_intersection_node/default.yaml @@ -1,13 +1,17 @@ -time_left_turn: 7.0 +#time_left_turn: 5.0 +time_left_turn: 4 time_straight_turn: 13 time_right_turn: 2.5 -ff_left: 0.018 +#ff_left: 0.0001 +ff_left: 0.03 ff_straight: 0 ff_right: -0.061 -LFparams_left: {phi_max: 0.0, phi_min: -0.5, range_est: 0.3, red_to_white: true, use_yellow: true} +#LFparams_left: {phi_max: -0.05, phi_min: -0.1, range_est: 0.3, red_to_white: true, use_yellow: true} + +LFparams_left: {"red_to_white": True, phi_min: -0.2, phi_max: 0.0} LFparams_straight: {"red_to_white": True} LFparams_right: {phi_max: 0.27, phi_min: 0.0, range_est: 0.3, red_to_white: false}