Custom yellow line detector

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 <package> 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"}'

packages/circle_drive/launch/indefinite_navigation2.launch

@@ -4,55 +4,15 @@
   <arg name="demo_name" value="indefinite_navigation"/>
   <arg name="verbose" default="true"/>
 
-	<group ns="$(arg veh)">
-                <remap from="circle_drive/car_cmd" to="joy_mapper_node/car_cmd"/>
-                <node name="circle_drive" pkg="circle_drive" type="circle_drive.py" output="screen" required="true"/>
-	</group>
-
 	<!-- start basic args -->
-	<include file="$(find duckietown_demos)/launch/master.launch">
+    <remap from="line_detector_node/thresholds" to="anti_instagram_node/thresholds"/>
+    <remap from="line_detector_node/image/compressed" to="camera_node/image/compressed"/>
+
+	<include file="$(find custom_line_detector)/launch/line_detector_node.launch">
 		<!-- Basic parameters -->
 		<arg name="veh" value="$(arg veh)"/>
-        <arg name="demo_name" value="$(arg demo_name)"/>
+<!--        <arg name="demo_name" value="$(arg demo_name)"/>-->
 		<arg name="param_file_name" default="default" />
-		<arg name="visualization" value="true" />
-
-        <!-- Finite state machine -->
-        <arg name="fsm" value="true"/>
-        <arg name="/fsm/logic_gate" value="true"/>
-		<!-- Basic functionalities -->
-		<arg name="line_detector_param_file_name" default="default" />
-		<arg name="anti_instagram" default="true" />
-		<arg name="/camera/raw" value="true"/>
-		<arg name="/camera/rect" value="true"/>
-		<arg name="LED" value="true"/>
-		<arg name="/LED/detector" value="true"/>
-		<arg name="/LED/pattern_switch" value="true"/>
-
-        <!-- Lane Following stack -->
-        <arg name="lane_following" value="true"/>
-        <arg name="/lane_following/line_detection" value="true"/>
-        <arg name="/lane_following/ground_projection" value="true"/>
-        <arg name="/lane_following/lane_filter" value="true"/>
-        <arg name="/lane_following/lane_controller" value="true"/>
-        <arg name="/lane_following/stop_line_filter" value="true"/>
-
-        <!-- Vehicle avoidance stack -->
-        <arg name="vehicle_avoidance" value="true"/>
-        <arg name="/vehicle_avoidance/detection" value="true" />
-        <arg name="/vehicle_avoidance/filter" value="true" />
-        <arg name="/vehicle_avoidance/control" value="true"/>
-
-		<!-- Intersection arguments -->
-		<arg name="unicorn_intersection" value="true"/>
-		<arg name="apriltags" value="true" />
-		<arg name="apriltags_random" value="true" />
-		<arg name="intersectionType" value="trafficLight"/>
-
-		<!-- Multi-bot behaviours -->
-		<arg name="coordination" value="true"/>
-		<arg name="/coordination/implicit_coordination" value="false"/>
-		<arg name="/coordination/explicit_coordination" value="true"/>
-
+<!--		<arg name="visualization" value="false" />-->
 	</include>
 </launch>

packages/circle_drive/launch/lane_following.launch

@@ -0,0 +1,45 @@
+<?xml version="1.0" encoding="utf-8"?>
+<launch>
+	<arg name="veh" default="$(env VEHICLE_NAME)"/>
+    <arg name="demo_name" value="lane_following"/>
+	<!-- start basic args -->
+	    <remap from="line_detector_node/thresholds" to="anti_instagram_node/thresholds"/>
+        <remap from="line_detector_node/image/compressed" to="camera_node/image/compressed"/>
+
+    	<include file="$(find custom_line_detector)/launch/line_detector_node.launch">
+    		<!-- Basic parameters -->
+    		<arg name="veh" value="$(arg veh)"/>
+    <!--        <arg name="demo_name" value="$(arg demo_name)"/>-->
+    		<arg name="param_file_name" default="default" />
+    <!--		<arg name="visualization" value="false" />-->
+    	</include>
+	<include file="$(find duckietown_demos)/launch/master.launch">
+	    <!-- Basic arguments -->
+		<arg name="veh" value="$(arg veh)"/>
+        <arg name="demo_name" value="$(arg demo_name)"/>
+		<arg name="param_file_name" value="default" />
+		<arg name="visualization" value="true" />
+
+	    <!-- Finite state machine -->
+	    <arg name="fsm" value="true"/>
+	    <arg name="/fsm/logic_gate" value="false"/>
+
+		<!-- Camera and anti intagram -->
+		<arg name="anti_instagram" value="true" />
+
+		<!-- Deadreckoning (aka. Odometry)	-->
+<!--		<arg name="/localization" value="true"/>
+		<arg name="/localization/deadreckoning" value="true"/>
+LP removing for now
+-->
+	    <!-- Lane Following stack -->
+	    <arg name="lane_following" value="true"/>
+	    <arg name="/lane_following/line_detection" value="false"/>
+	    <arg name="line_detector_param_file_name" value="default" />
+	    <arg name="/lane_following/ground_projection" value="true"/>
+	    <arg name="/lane_following/lane_filter" value="true"/>
+	    <arg name="/lane_following/lane_controller" value="true"/>
+
+  	</include>
+
+</launch>

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}
+)

packages/dt-core/packages/line_detector/config/line_detector_node/default.yaml → packages/custom_line_detector/config/line_detector_node/default.yaml

@@ -3,21 +3,21 @@ 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]
   canny_aperture_size: 3
   dilation_kernel_size: 3
   hough_threshold: 2
   hough_min_line_length: 3
-  hough_max_line_gap: 1
+  hough_max_line_gap: 1

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

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

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 <https://docs.opencv.org/3.4/d2/de8/group__core__array.html#ga48af0ab51e36436c5d04340e036ce981>`_
+    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 <https://docs.opencv.org/3.4/d2/de8/group__core__array.html#ga48af0ab51e36436c5d04340e036ce981>`_
+        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))

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