From d6563ccbbf56c285862e205605df5a504991a51f Mon Sep 17 00:00:00 2001 From: Konstantin Chaika Date: Wed, 22 Dec 2021 17:58:31 +0300 Subject: [PATCH 01/17] changes to FLI stable drive --- .../launch/indefinite_navigation2.launch | 23 +- .../apriltag/src/apriltag_detector_node.py | 242 ++++++++++++++ .../src/coordinator_node.py | 302 ++++++++++++++++++ 3 files changed, 553 insertions(+), 14 deletions(-) create mode 100644 packages/dt-core/packages/apriltag/src/apriltag_detector_node.py create mode 100644 packages/dt-core/packages/explicit_coordinator/src/coordinator_node.py diff --git a/packages/circle_drive/launch/indefinite_navigation2.launch b/packages/circle_drive/launch/indefinite_navigation2.launch index b1a7f53f..2ca229f9 100644 --- a/packages/circle_drive/launch/indefinite_navigation2.launch +++ b/packages/circle_drive/launch/indefinite_navigation2.launch @@ -4,30 +4,25 @@ - - - - - - + - + - - - + + + @@ -38,10 +33,10 @@ - - - - + + + + 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..deb0ffbf --- /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("/autobot05/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 From 38d50762e0c8ae365f99394e592289037ca5b873 Mon Sep 17 00:00:00 2001 From: light5551 Date: Fri, 24 Dec 2021 20:12:47 +0300 Subject: [PATCH 02/17] working version of left turn, i hope --- .../config/unicorn_intersection_node/default.yaml | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) 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..6ed386d5 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: 5 time_straight_turn: 13 time_right_turn: 2.5 -ff_left: 0.018 +#ff_left: 0.0001 +ff_left: 0 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 } LFparams_straight: {"red_to_white": True} LFparams_right: {phi_max: 0.27, phi_min: 0.0, range_est: 0.3, red_to_white: false} From 877583c98d966145fe6e939df09d38e4a0663429 Mon Sep 17 00:00:00 2001 From: light5551 Date: Sat, 25 Dec 2021 20:49:47 +0300 Subject: [PATCH 03/17] last upd --- .../src/random_april_tag_turns_node.py | 21 +++++++++---------- .../unicorn_intersection_node/default.yaml | 6 +++--- 2 files changed, 13 insertions(+), 14 deletions(-) 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 6ed386d5..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,17 +1,17 @@ #time_left_turn: 5.0 -time_left_turn: 5 +time_left_turn: 4 time_straight_turn: 13 time_right_turn: 2.5 #ff_left: 0.0001 -ff_left: 0 +ff_left: 0.03 ff_straight: 0 ff_right: -0.061 #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 } +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} From 6c6e25f6a3e1286bbc885d6a97f1160616583b80 Mon Sep 17 00:00:00 2001 From: Konstantin Chaika Date: Sat, 25 Dec 2021 21:41:04 +0300 Subject: [PATCH 04/17] add any bot support --- .../launch/apriltag_detector_node.launch | 23 +++++++++++++++++++ .../apriltag/src/apriltag_detector_node.py | 2 +- 2 files changed, 24 insertions(+), 1 deletion(-) create mode 100644 packages/dt-core/packages/apriltag/launch/apriltag_detector_node.launch 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 index deb0ffbf..5a272335 100644 --- a/packages/dt-core/packages/apriltag/src/apriltag_detector_node.py +++ b/packages/dt-core/packages/apriltag/src/apriltag_detector_node.py @@ -63,7 +63,7 @@ def __init__(self): "~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("/autobot05/fsm_node/mode", FSMState, self.cbFSMState) + self.sub_fsm = rospy.Subscriber("~fsm_node/mode", FSMState, self.cbFSMState) # create publisher self._tag_pub = rospy.Publisher( "~detections", From eae55eecae7f5760c944160c094a19fbc331d02a Mon Sep 17 00:00:00 2001 From: Konstantin Chaika Date: Fri, 11 Mar 2022 12:12:41 +0300 Subject: [PATCH 05/17] add template for new yellow detector --- launch.sh | 6 +- .../launch/indefinite_navigation2.launch | 47 +-- packages/custom_line_detector/CMakeLists.txt | 20 ++ packages/custom_line_detector/__init__.py | 0 .../config/line_detector_node/default.yaml | 16 +- .../custom_line_detector/include/__init__.py | 0 .../include/custom_line_detector/__init__.py | 29 ++ .../baseline_daffy.line_detector.yaml | 9 + .../custom_line_detector/color_range.py | 111 ++++++ .../custom_line_detector/detections.py | 15 + .../custom_line_detector/line_detector.py | 329 +++++++++++++++++ .../custom_line_detector/plot_detections.py | 76 ++++ .../launch/line_detector_node.launch | 12 + packages/custom_line_detector/package.xml | 20 ++ packages/custom_line_detector/setup.py | 12 + .../src/line_detector_node.py | 330 ++++++++++++++++++ 16 files changed, 980 insertions(+), 52 deletions(-) create mode 100644 packages/custom_line_detector/CMakeLists.txt create mode 100644 packages/custom_line_detector/__init__.py rename packages/{dt-core/packages/line_detector => custom_line_detector}/config/line_detector_node/default.yaml (57%) create mode 100644 packages/custom_line_detector/include/__init__.py create mode 100644 packages/custom_line_detector/include/custom_line_detector/__init__.py create mode 100644 packages/custom_line_detector/include/custom_line_detector/baseline_daffy.line_detector.yaml create mode 100644 packages/custom_line_detector/include/custom_line_detector/color_range.py create mode 100644 packages/custom_line_detector/include/custom_line_detector/detections.py create mode 100644 packages/custom_line_detector/include/custom_line_detector/line_detector.py create mode 100644 packages/custom_line_detector/include/custom_line_detector/plot_detections.py create mode 100644 packages/custom_line_detector/launch/line_detector_node.launch create mode 100644 packages/custom_line_detector/package.xml create mode 100644 packages/custom_line_detector/setup.py create mode 100755 packages/custom_line_detector/src/line_detector_node.py diff --git a/launch.sh b/launch.sh index bc03a901..a6e4ce42 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 indefinite_navigation2.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 2ca229f9..ade81e60 100644 --- a/packages/circle_drive/launch/indefinite_navigation2.launch +++ b/packages/circle_drive/launch/indefinite_navigation2.launch @@ -5,49 +5,14 @@ - + + + + - + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + 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..34e8ef8d --- /dev/null +++ b/packages/custom_line_detector/include/custom_line_detector/line_detector.py @@ -0,0 +1,329 @@ +import cv2 +import numpy as np + +from line_detector_interface import LineDetectorInterface +from .detections import Detections + + +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 + + """ + + 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 + + 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 detectYellowLines(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 = np.full((80, 160), 255, dtype=int) + lines = np.asarray([[10,10,10,50], + [20,20,20,60], + [60,20,40,60], + [40,30,20,40] + ]) + + centers, normals = self.findNormal(map, lines) + return Detections(lines=lines, normals=normals, map=map, centers=centers) + +''' +lines = [[86 10 86 4]] normals = [[ 1. -0.]] map = [[0 0 0 ... 0 0 0] + [0 0 0 ... 0 0 0] + [0 0 0 ... 0 0 0] + ... + [0 0 0 ... 0 0 0] + [0 0 0 ... 0 0 0] + [0 0 0 ... 0 0 0]] centers = [[86. 7.]] +lines = [[ 7 14 17 13] + [18 12 22 12] + [ 4 21 4 15] + [31 10 39 10] + [ 5 26 6 29] + [24 1 28 3]][autobot01/stop_line_filter_node-8] killing on exit +[autobot01/lane_filter_node-7] killing on exit +[INFO] [1646920190.060340]: [/autobot01/stop_line_filter_node] Received shutdown request. +[INFO] [1646920190.067721]: [/autobot01/lane_filter_node] Received shutdown request. + normals = [[ 0.09950372 0.99503719] + [-0. 1. ] + [ 1. -0. ] + [ 0. -1. ] + [-0.9486833 0.31622777] + [-0.4472136 0.89442719]] map = [[255 255 255 ... 255 255 255] + [255 255 255 ... 255 255 255] + [255 255 255 ... 255 255 255] + ... + [ 0 0 0 ... 0 0 0] + [ 0 0 0 ... 0 0 0] + [ 0 0 0 ... 0 0 0]][autobot01/ground_projection_node-6] killing on exit + centers = [[12. 13.5] + [20. 12. ] + [ 4. 18. ] + [35. 10. ] + [ 5.5 27.5] + [26. 2. ]] +[INFO] [1646920190.105539]: [/autobot01/ground_projection_node] Received shutdown request. +[autobot01/line_detector_node-5] killing on exit +lines = [] normals = [] map = [[0 0 0 ... 0 0 0] + [0 0 0 ... 0 0 0] + [0 0 0 ... 0 0 0] + ... + [0 0 0 ... 0 0 0] + [0 0 0 ... 0 0 0] + [0 0 0 ... 0 0 0]] centers = [] +{'RED': , 'WHITE': , 'YELLOW': } +[INFO] [1646920190.133156]: [/autobot01/line_detector_node] Received shutdown request. +lines = [[86 10 86 4]] normals = [[ 1. -0.]] map = [[0 0 0 ... 0 0 0] + [0 0 0 ... 0 0 0] + [0 0 0 ... 0 0 0] + ... + [0 0 0 ... 0 0 0] + [0 0 0 ... 0 0 0] + [0 0 0 ... 0 0 0]] centers = [[86. 7.]] +lines = [[35 10 41 10] + [ 4 21 4 15] + [10 14 15 13] + [ 5 26 5 22] + [ 1 55 4 62] + [25 11 29 11] + [22 12 24 9]] normals = [[ 0. -1. ] + [ 1. -0. ] + [ 0.19611614 0.98058068] + [ 1. -0. ] + [-0.91914503 0.3939193 ] + [-0. 1. ] + [ 0.83205029 0.5547002 ]] map = [[255 255 255 ... 255 255 255] + [255 255 255 ... 255 255 255] + [255 255 255 ... 255 255 255] + ... + [ 0 0 0 ... 0 0 0] + [ 0 0 0 ... 0 0 0] + [ 0 0 0 ... 0 0 0]] centers = [[38. 10. ] + [ 4. 18. ] + [12.5 13.5] + [ 5. 24. ] + [ 2.5 58.5] + [27. 11. ] + [23. 10.5]] +lines = [] normals = [] map = [[0 0 0 ... 0 0 0] + [0 0 0 ... 0 0 0] + [0 0 0 ... 0 0 0] + ... + [0 0 0 ... 0 0 0] + [0 0 0 ... 0 0 0] + [0 0 0 ... 0 0 0]] centers = [] +{'RED': , 'WHITE': , 'YELLOW': } + +''' \ No newline at end of file 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..ea218dc8 --- /dev/null +++ b/packages/custom_line_detector/src/line_detector_node.py @@ -0,0 +1,330 @@ +#!/usr/bin/env python3 + +import numpy as np +import cv2 +import rospy +from cv_bridge import CvBridge +from sensor_msgs.msg import CompressedImage, Image +from duckietown_msgs.msg import Segment, SegmentList, AntiInstagramThresholds +from custom_line_detector import LineDetector, ColorRange, plotSegments, plotMaps +from image_processing.anti_instagram import AntiInstagram + +from duckietown.dtros import DTROS, NodeType, TopicType + + +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.bridge = CvBridge() + + # 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 + ) + + # 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 + ) + + 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 + ) + + # Resize the image to the desired dimensions + 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 :, :, :] + + # Extract the line segments for every color + self.detector.setImage(image) + 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.detectYellowLines(ranges) + + #print(detections) + + # 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) + + 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) + + @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() From c04d39ac910ef2e1c56243e19c7c5b6b24c8d629 Mon Sep 17 00:00:00 2001 From: TatyanaBerlenko Date: Thu, 14 Apr 2022 14:07:43 +0300 Subject: [PATCH 06/17] tmp yellow line detection --- .../custom_line_detector/line_detector.py | 195 +++++++++++++++++- .../src/line_detector_node.py | 77 +++++-- 2 files changed, 249 insertions(+), 23 deletions(-) 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 index 34e8ef8d..95005d48 100644 --- a/packages/custom_line_detector/include/custom_line_detector/line_detector.py +++ b/packages/custom_line_detector/include/custom_line_detector/line_detector.py @@ -1,9 +1,18 @@ import cv2 import numpy as np - +from math import sqrt from line_detector_interface import LineDetectorInterface from .detections import Detections +MIN_PIXEL = 3 +MAX_PIXEL = 200 +PART_OF_INTEREST = 0.7 +EPS = 8 +CAMERA_MATRIX = np.array( + [[278.79547761007365, 0.0, 314.29374336264345], [0.0, 280.52395701002115, 228.59132685202135], [0.0, 0.0, 1.0]]) +DISTORTION_COEFFFICIENTS = np.array( + [-0.23670917420627122, 0.03455456424406622, 0.0037778674941860426, 0.0020245279929775382, 0.0]) + class LineDetector(LineDetectorInterface): """ @@ -38,6 +47,14 @@ class LineDetector(LineDetectorInterface): """ + class Element: + def __init__(self, ncenter, ndx, ndy, rad): + self.center = ncenter + self.dx = int(ndx) + self.dy = int(ndy) + self.rad = int(rad) + + def __init__( self, canny_thresholds=[80, 200], @@ -59,6 +76,162 @@ def __init__( 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_dash_line = [] + + @staticmethod + def get_max_dist_between_elements(img, y): + height, width, _ = img.shape + max_dist = 170 + step = 100 + if height - step <= y <= height: + return max_dist + if height - step * 2 <= y: + return (max_dist >> 1) + 10 + elif height - step * 2.5 <= y: + return max_dist >> 2 + else: + return max_dist >> 5 + + @staticmethod + def _max_radius(h, img_len): + if h > img_len * PART_OF_INTEREST: + return 0 + alpha = (1 - (MIN_PIXEL + EPS) / MAX_PIXEL) / (img_len * PART_OF_INTEREST) + return MAX_PIXEL * (1 - alpha * h) + + @staticmethod + def _dist_pixels(pix1, pix2): + return int(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.04 * 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 = ((radius[0][0] - center[0][0]) ** 2 + (radius[0][1] - center[0][1]) ** 2) ** (0.5) + + if not MIN_PIXEL < radius_length < MAX_PIXEL: + continue + + if len(approx) in (3, 4): + dx = 0 + dy = 0 + for el in approx: + x1 = int(el[0][0]) + y1 = int(el[0][1]) + for el2 in approx: + x2 = int(el2[0][0]) + y2 = int(el2[0][1]) + dx = max(dx, abs(x2 - x1)) + dy = max(dy, abs(y2 - y1)) + dy = max(dy, int(dx * 0.4)) + dx //= 2 + dy //= 2 + ans = [] + max_dist = 0 + + for el in approx: + for el2 in approx: + if self._dist_pixels(el[0], el2[0]) > max_dist: + max_dist = self._dist_pixels(el[0], el2[0]) + ans = [el[0], el2[0]] + ncent = [(ans[0][0] + ans[1][0]) // 2, (ans[0][1] + ans[1][1]) // 2] + + if np.array_equal(img[int(center[0][1]), int(center[0][0])], [0, 0, 0]): + + continue + filtered_contours.append(self.Element(ncent, dx, dy, radius_length)) + + return filtered_contours + + @staticmethod + def sort_contours(contours): + return sorted(contours, key=lambda contour: (contour.center[1], contour.center[0]), reverse=True) + + def detect_dash_line_for_pub(self, img): + gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) + ret, threshold_image = cv2.threshold(gray, 145, 255, cv2.THRESH_BINARY) # 150, 255, 0) + contours, _ = cv2.findContours(threshold_image, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + contours = self._filter_contours(contours, threshold_image) + contours = self.sort_contours(contours) + for i, contour in enumerate(contours): + cv2.circle(threshold_image, (int(contour.center[0]), int(contour.center[1])), int(contour.rad), (255, 255, 0), + thickness=5) + # next_contour = False + # ans_dash_line = [] + # prev_contour = contours[0] + # cv2.circle(threshold_image, (int(prev_contour.center[0]), int(prev_contour.center[1])), int(prev_contour.rad), (255, 255, 0), + # thickness=3) + return threshold_image + + def _detect_dash_line(self, img): + + gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) + ret, threshold_image = cv2.threshold(gray, 145, 255, cv2.THRESH_BINARY) # 150, 255, 0) + contours, h = cv2.findContours(threshold_image, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + threshold_image = cv2.cvtColor(threshold_image, cv2.COLOR_GRAY2RGB) + + contours = self._filter_contours(contours, threshold_image) + contours = self.sort_contours(contours) + next_contour = False + ans_dash_line = [] + + prev_contour = contours[0] + cv2.circle(threshold_image, (int(prev_contour.center[0]), int(prev_contour.center[1])), int(prev_contour.rad), (255, 255, 0), + thickness=1) + + # cv2.putText(threshold_image, str(0), (int(prev_contour.center[0]), int(prev_contour.center[1])), + # cv2.FONT_HERSHEY_SIMPLEX, 0.42, (0, 8, 230), thickness=1) + for i, contour in enumerate(contours[1:]): + cv2.circle(threshold_image, (int(contour.center[0]), int(contour.center[1])), int(contour.rad), (255, 255, 0), + thickness=1) + max_dist = self.get_max_dist_between_elements(threshold_image, prev_contour.center[1]) + # # cv2.putText(threshold_image, str(i), (int(contour.center[0]), int(contour.center[1])), + # # cv2.FONT_HERSHEY_SIMPLEX, 0.42, (0, 8, 230), thickness=1) + dist = self._dist_pixels(contour.center, prev_contour.center) + if dist <= max_dist and not self.is_inside(contour, prev_contour): + ans_dash_line.append(prev_contour) + next_contour = True + else: + if next_contour: + ans_dash_line.append(prev_contour) + next_contour = False + + prev_contour = contour + + if next_contour: + ans_dash_line.append(prev_contour) + return threshold_image, ans_dash_line + + @staticmethod + def is_inside(first_line, second_line): + if first_line.center == second_line.center and first_line.rad == second_line.rad: # переопределить метод сравнения + return False + big_cont = first_line if first_line.rad > second_line.rad else second_line + small_cont = first_line if first_line.rad < second_line.rad 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 + + @staticmethod + def is_cross(first_line, second_line): + dist_between_cent = LineDetector._dist_pixels(first_line.center, second_line.center) + e = 5 # зависимость от координат + if first_line.rad + second_line.rad < dist_between_cent + e: + return False + return True + + @staticmethod + def _make_undistorted_image(img): + return cv2.undistort(img, CAMERA_MATRIX, DISTORTION_COEFFFICIENTS) + + @staticmethod + def _sum(pix1, pix2): + return [pix1[0] + pix2[0], pix1[1] + pix2[1]] + def setImage(self, image): """ @@ -217,7 +390,7 @@ def detectLines(self, color_range): centers, normals = self.findNormal(map, lines) return Detections(lines=lines, normals=normals, map=map, centers=centers) - def detectYellowLines(self, color_range): + def detectYellowLines(self, color_range, 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. @@ -228,14 +401,18 @@ def detectYellowLines(self, color_range): 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, """ - + print('!!') + img = self._make_undistorted_image(img) + _, lines = self._detect_dash_line(img) + print(lines) + lines = np.asarray(lines) + + # lines = np.asarray([[10,10,10,50], + # [20,20,20,60], + # [60,20,40,60], + # [40,30,20,40] + # ]) map = np.full((80, 160), 255, dtype=int) - lines = np.asarray([[10,10,10,50], - [20,20,20,60], - [60,20,40,60], - [40,30,20,40] - ]) - centers, normals = self.findNormal(map, lines) return Detections(lines=lines, normals=normals, map=map, centers=centers) diff --git a/packages/custom_line_detector/src/line_detector_node.py b/packages/custom_line_detector/src/line_detector_node.py index ea218dc8..68f2b14a 100755 --- a/packages/custom_line_detector/src/line_detector_node.py +++ b/packages/custom_line_detector/src/line_detector_node.py @@ -4,13 +4,14 @@ import cv2 import rospy from cv_bridge import CvBridge -from sensor_msgs.msg import CompressedImage, Image +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): """ @@ -58,8 +59,9 @@ def __init__(self, node_name): 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 @@ -97,6 +99,9 @@ def __init__(self, node_name): 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( @@ -107,6 +112,38 @@ def __init__(self, node_name): "~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 + self.K = msg.K + self.D = msg.D + # 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 @@ -144,23 +181,18 @@ def image_cb(self, image_msg): self.anti_instagram_thresholds["lower"], self.anti_instagram_thresholds["higher"], image ) - # Resize the image to the desired dimensions - 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 :, :, :] + # 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.detectYellowLines(ranges) + # for color, ranges in list(self.color_ranges.items()): + # if color == 'YELLOW': + # detections['YELLOW'] = self.detector.detectYellowLines(ranges, image) - #print(detections) # Construct a SegmentList segment_list = SegmentList() @@ -213,6 +245,23 @@ def image_cb(self, image_msg): 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: + print(np.array(self.K).reshape((3, 3))) + print() + image = cv2.undistort(image, np.array(self.K).reshape((3, 3)), np.array(self.D)) + # image = cv2.undistort(image, self.camera_model.K, self.camera_model.D) + # Resize the image to the desired dimensions + 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 :, :, :] + # 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}") From 6ba2000635515b3cb90dd2a06b21fed43952ddc2 Mon Sep 17 00:00:00 2001 From: TatyanaBerlenko Date: Thu, 21 Apr 2022 12:39:51 +0300 Subject: [PATCH 07/17] add detection --- .../custom_line_detector/line_detector.py | 77 ++++++++++++------- .../src/line_detector_node.py | 40 +++++----- 2 files changed, 68 insertions(+), 49 deletions(-) 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 index 95005d48..a557a733 100644 --- a/packages/custom_line_detector/include/custom_line_detector/line_detector.py +++ b/packages/custom_line_detector/include/custom_line_detector/line_detector.py @@ -1,3 +1,5 @@ +import functools + import cv2 import numpy as np from math import sqrt @@ -104,18 +106,23 @@ def _dist_pixels(pix1, pix2): return int(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.04 * 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 = ((radius[0][0] - center[0][0]) ** 2 + (radius[0][1] - center[0][1]) ** 2) ** (0.5) + if radius_length > 60: + continue if not MIN_PIXEL < radius_length < MAX_PIXEL: continue if len(approx) in (3, 4): + # cv2.drawContours(img, contours, k, (0, 100, 100), thickness=2) dx = 0 dy = 0 for el in approx: @@ -142,29 +149,31 @@ def _filter_contours(self, contours, img): if np.array_equal(img[int(center[0][1]), int(center[0][0])], [0, 0, 0]): continue + # cv2.drawContours(img, contours, k, (100, 0, 100), thickness=1) + filtered_contours.append(self.Element(ncent, dx, dy, radius_length)) return filtered_contours - @staticmethod - def sort_contours(contours): - return sorted(contours, key=lambda contour: (contour.center[1], contour.center[0]), reverse=True) - def detect_dash_line_for_pub(self, img): - gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) - ret, threshold_image = cv2.threshold(gray, 145, 255, cv2.THRESH_BINARY) # 150, 255, 0) - contours, _ = cv2.findContours(threshold_image, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) - contours = self._filter_contours(contours, threshold_image) - contours = self.sort_contours(contours) - for i, contour in enumerate(contours): - cv2.circle(threshold_image, (int(contour.center[0]), int(contour.center[1])), int(contour.rad), (255, 255, 0), - thickness=5) - # next_contour = False - # ans_dash_line = [] - # prev_contour = contours[0] - # cv2.circle(threshold_image, (int(prev_contour.center[0]), int(prev_contour.center[1])), int(prev_contour.rad), (255, 255, 0), - # thickness=3) - return threshold_image + def sort_contours(self, contours): + return sorted(contours, key=functools.cmp_to_key(lambda contour1, contour2: self._dist_pixels(contour1.center, contour2.center)), reverse=True) + # int(sqrt((pix1[0] - pix2[0]) ** 2 + (pix1[1] - pix2[1]) ** 2)) + # def detect_dash_line_for_pub(self, img): + # gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) + # ret, threshold_image = cv2.threshold(gray, 145, 255, cv2.THRESH_BINARY) # 150, 255, 0) + # contours, _ = cv2.findContours(threshold_image, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + # contours = self._filter_contours(contours, threshold_image) + # contours = self.sort_contours(contours) + # for i, contour in enumerate(contours): + # cv2.circle(threshold_image, (int(contour.center[0]), int(contour.center[1])), int(contour.rad), (255, 255, 0), + # thickness=5) + # # next_contour = False + # # ans_dash_line = [] + # # prev_contour = contours[0] + # # cv2.circle(threshold_image, (int(prev_contour.center[0]), int(prev_contour.center[1])), int(prev_contour.rad), (255, 255, 0), + # # thickness=3) + # return threshold_image def _detect_dash_line(self, img): @@ -179,30 +188,42 @@ def _detect_dash_line(self, img): ans_dash_line = [] prev_contour = contours[0] - cv2.circle(threshold_image, (int(prev_contour.center[0]), int(prev_contour.center[1])), int(prev_contour.rad), (255, 255, 0), - thickness=1) + # cv2.circle(threshold_image, (int(prev_contour.center[0]), int(prev_contour.center[1])), int(prev_contour.rad), (255, 255, 0), + # thickness=1) - # cv2.putText(threshold_image, str(0), (int(prev_contour.center[0]), int(prev_contour.center[1])), - # cv2.FONT_HERSHEY_SIMPLEX, 0.42, (0, 8, 230), thickness=1) + cv2.putText(threshold_image, str(0), (int(prev_contour.center[0]), int(prev_contour.center[1])), + cv2.FONT_HERSHEY_SIMPLEX, 0.42, (0, 8, 230), thickness=1) for i, contour in enumerate(contours[1:]): - cv2.circle(threshold_image, (int(contour.center[0]), int(contour.center[1])), int(contour.rad), (255, 255, 0), - thickness=1) + # cv2.circle(threshold_image, (int(contour.center[0]), int(contour.center[1])), int(contour.rad), (255, 255, 0), + # thickness=1) max_dist = self.get_max_dist_between_elements(threshold_image, prev_contour.center[1]) - # # cv2.putText(threshold_image, str(i), (int(contour.center[0]), int(contour.center[1])), - # # cv2.FONT_HERSHEY_SIMPLEX, 0.42, (0, 8, 230), thickness=1) + print(i) + print(max_dist) + + cv2.putText(threshold_image, str(i+1), (int(contour.center[0]), int(contour.center[1])), + cv2.FONT_HERSHEY_SIMPLEX, 0.42, (0, 8, 230), thickness=1) dist = self._dist_pixels(contour.center, prev_contour.center) + print(dist) + print(self.is_inside(contour, prev_contour)) if dist <= max_dist and not self.is_inside(contour, prev_contour): ans_dash_line.append(prev_contour) + cv2.circle(threshold_image, (int(prev_contour.center[0]), int(prev_contour.center[1])), int(prev_contour.rad), (255, 255, 0), + thickness=1) next_contour = True else: if next_contour: ans_dash_line.append(prev_contour) + cv2.circle(threshold_image, (int(prev_contour.center[0]), int(prev_contour.center[1])), int(prev_contour.rad), (255, 255, 0), + thickness=1) next_contour = False prev_contour = contour + print('-'*80) if next_contour: ans_dash_line.append(prev_contour) + cv2.circle(threshold_image, (int(prev_contour.center[0]), int(prev_contour.center[1])), int(prev_contour.rad), (255, 255, 0), + thickness=1) return threshold_image, ans_dash_line @staticmethod @@ -390,7 +411,7 @@ def detectLines(self, color_range): centers, normals = self.findNormal(map, lines) return Detections(lines=lines, normals=normals, map=map, centers=centers) - def detectYellowLines(self, color_range, img): + def detectYellowLines(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. @@ -402,7 +423,7 @@ def detectYellowLines(self, color_range, img): :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, """ print('!!') - img = self._make_undistorted_image(img) + _, lines = self._detect_dash_line(img) print(lines) lines = np.asarray(lines) diff --git a/packages/custom_line_detector/src/line_detector_node.py b/packages/custom_line_detector/src/line_detector_node.py index 68f2b14a..2441863f 100755 --- a/packages/custom_line_detector/src/line_detector_node.py +++ b/packages/custom_line_detector/src/line_detector_node.py @@ -119,23 +119,22 @@ def __init__(self, node_name): def _cinfo_cb(self, msg): # create mapx and mapy H, W = msg.height, msg.width - self.K = msg.K - self.D = msg.D + # 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 - # ) + 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 @@ -247,17 +246,16 @@ def image_cb(self, image_msg): self.pub_d_maps.publish(debug_image_msg) if self.pub_contours.get_num_connections() > 0: - print(np.array(self.K).reshape((3, 3))) - print() - image = cv2.undistort(image, np.array(self.K).reshape((3, 3)), np.array(self.D)) - # image = cv2.undistort(image, self.camera_model.K, self.camera_model.D) + # image = cv2.undistort(image, np.array(self.K).reshape((3, 3)), np.array(self.D)) + image = cv2.undistort(image, self.camera_model.K, self.camera_model.D) + # Resize the image to the desired dimensions 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 :, :, :] - # 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 From 57e7d4cd18380426d1745d60d301c455e38858ad Mon Sep 17 00:00:00 2001 From: TatyanaBerlenko Date: Thu, 21 Apr 2022 13:16:38 +0300 Subject: [PATCH 08/17] add detection --- .../custom_line_detector/line_detector.py | 39 +++++++------------ 1 file changed, 13 insertions(+), 26 deletions(-) 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 index a557a733..c2d21a4b 100644 --- a/packages/custom_line_detector/include/custom_line_detector/line_detector.py +++ b/packages/custom_line_detector/include/custom_line_detector/line_detector.py @@ -109,17 +109,19 @@ def _filter_contours(self, contours, img): filtered_contours = [] for k, contour in enumerate(contours): - approx = cv2.approxPolyDP(contour, 0.04 * cv2.arcLength(contour, True), True) + # cv2.drawContours(img, contours, k, (0, 100, 100), thickness=1) + approx = cv2.approxPolyDP(contour, 0.045 * cv2.arcLength(contour, True), True) center = sum(approx) / approx.shape[0] - + cv2.putText(img, str(len(approx)), (int(center[0][0]), int(center[0][1])), + cv2.FONT_HERSHEY_SIMPLEX, 0.32, (0, 8, 230), thickness=1) radius = max(contour, key=lambda x: abs(center[0][0] - x[0][0]) ** 2 + abs(center[0][1] - x[0][1]) ** 2) radius_length = ((radius[0][0] - center[0][0]) ** 2 + (radius[0][1] - center[0][1]) ** 2) ** (0.5) if radius_length > 60: continue - if not MIN_PIXEL < radius_length < MAX_PIXEL: - continue + # if not MIN_PIXEL < radius_length < MAX_PIXEL: + # continue if len(approx) in (3, 4): # cv2.drawContours(img, contours, k, (0, 100, 100), thickness=2) @@ -158,22 +160,7 @@ def _filter_contours(self, contours, img): def sort_contours(self, contours): return sorted(contours, key=functools.cmp_to_key(lambda contour1, contour2: self._dist_pixels(contour1.center, contour2.center)), reverse=True) - # int(sqrt((pix1[0] - pix2[0]) ** 2 + (pix1[1] - pix2[1]) ** 2)) - # def detect_dash_line_for_pub(self, img): - # gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) - # ret, threshold_image = cv2.threshold(gray, 145, 255, cv2.THRESH_BINARY) # 150, 255, 0) - # contours, _ = cv2.findContours(threshold_image, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) - # contours = self._filter_contours(contours, threshold_image) - # contours = self.sort_contours(contours) - # for i, contour in enumerate(contours): - # cv2.circle(threshold_image, (int(contour.center[0]), int(contour.center[1])), int(contour.rad), (255, 255, 0), - # thickness=5) - # # next_contour = False - # # ans_dash_line = [] - # # prev_contour = contours[0] - # # cv2.circle(threshold_image, (int(prev_contour.center[0]), int(prev_contour.center[1])), int(prev_contour.rad), (255, 255, 0), - # # thickness=3) - # return threshold_image + def _detect_dash_line(self, img): @@ -191,8 +178,8 @@ def _detect_dash_line(self, img): # cv2.circle(threshold_image, (int(prev_contour.center[0]), int(prev_contour.center[1])), int(prev_contour.rad), (255, 255, 0), # thickness=1) - cv2.putText(threshold_image, str(0), (int(prev_contour.center[0]), int(prev_contour.center[1])), - cv2.FONT_HERSHEY_SIMPLEX, 0.42, (0, 8, 230), thickness=1) + # cv2.putText(threshold_image, str(0), (int(prev_contour.center[0]), int(prev_contour.center[1])), + # cv2.FONT_HERSHEY_SIMPLEX, 0.42, (0, 8, 230), thickness=1) for i, contour in enumerate(contours[1:]): # cv2.circle(threshold_image, (int(contour.center[0]), int(contour.center[1])), int(contour.rad), (255, 255, 0), # thickness=1) @@ -200,8 +187,8 @@ def _detect_dash_line(self, img): print(i) print(max_dist) - cv2.putText(threshold_image, str(i+1), (int(contour.center[0]), int(contour.center[1])), - cv2.FONT_HERSHEY_SIMPLEX, 0.42, (0, 8, 230), thickness=1) + # cv2.putText(threshold_image, str(i+1), (int(contour.center[0]), int(contour.center[1])), + # cv2.FONT_HERSHEY_SIMPLEX, 0.42, (0, 8, 230), thickness=1) dist = self._dist_pixels(contour.center, prev_contour.center) print(dist) print(self.is_inside(contour, prev_contour)) @@ -213,7 +200,7 @@ def _detect_dash_line(self, img): else: if next_contour: ans_dash_line.append(prev_contour) - cv2.circle(threshold_image, (int(prev_contour.center[0]), int(prev_contour.center[1])), int(prev_contour.rad), (255, 255, 0), + cv2.circle(threshold_image, (int(prev_contour.center[0]), int(prev_contour.center[1])), int(prev_contour.rad), (255, 0, 220), thickness=1) next_contour = False @@ -222,7 +209,7 @@ def _detect_dash_line(self, img): if next_contour: ans_dash_line.append(prev_contour) - cv2.circle(threshold_image, (int(prev_contour.center[0]), int(prev_contour.center[1])), int(prev_contour.rad), (255, 255, 0), + cv2.circle(threshold_image, (int(prev_contour.center[0]), int(prev_contour.center[1])), int(prev_contour.rad), (0, 255, 2000), thickness=1) return threshold_image, ans_dash_line From 519fd3acbdb24e8d77bdaa5f278a99c0a4fc326d Mon Sep 17 00:00:00 2001 From: TatyanaBerlenko Date: Thu, 21 Apr 2022 14:00:12 +0300 Subject: [PATCH 09/17] change launch file --- launch.sh | 2 +- .../circle_drive/launch/lane_following.launch | 45 +++++++++++++++++++ 2 files changed, 46 insertions(+), 1 deletion(-) create mode 100644 packages/circle_drive/launch/lane_following.launch diff --git a/launch.sh b/launch.sh index a6e4ce42..7262d8ad 100755 --- a/launch.sh +++ b/launch.sh @@ -7,7 +7,7 @@ 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 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 From 05658cc0a9fb71df39581e793746d6d78b8ca759 Mon Sep 17 00:00:00 2001 From: TatyanaBerlenko Date: Fri, 22 Apr 2022 13:47:28 +0300 Subject: [PATCH 10/17] logging output --- .../custom_line_detector/line_detector.py | 38 ++++++++++--------- .../src/line_detector_node.py | 6 +++ 2 files changed, 27 insertions(+), 17 deletions(-) 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 index c2d21a4b..23f87810 100644 --- a/packages/custom_line_detector/include/custom_line_detector/line_detector.py +++ b/packages/custom_line_detector/include/custom_line_detector/line_detector.py @@ -6,8 +6,8 @@ from line_detector_interface import LineDetectorInterface from .detections import Detections -MIN_PIXEL = 3 -MAX_PIXEL = 200 +MIN_PIXEL = 2 +MAX_PIXEL = 30 PART_OF_INTEREST = 0.7 EPS = 8 CAMERA_MATRIX = np.array( @@ -109,21 +109,22 @@ def _filter_contours(self, contours, img): filtered_contours = [] for k, contour in enumerate(contours): + print('-'*80) + # cv2.drawContours(img, contours, k, (0, 100, 100), thickness=1) approx = cv2.approxPolyDP(contour, 0.045 * cv2.arcLength(contour, True), True) center = sum(approx) / approx.shape[0] - cv2.putText(img, str(len(approx)), (int(center[0][0]), int(center[0][1])), - cv2.FONT_HERSHEY_SIMPLEX, 0.32, (0, 8, 230), thickness=1) + # cv2.putText(img, str(len(approx)), (int(center[0][0]), int(center[0][1])), + # cv2.FONT_HERSHEY_SIMPLEX, 0.32, (0, 8, 230), thickness=1) radius = max(contour, key=lambda x: abs(center[0][0] - x[0][0]) ** 2 + abs(center[0][1] - x[0][1]) ** 2) radius_length = ((radius[0][0] - center[0][0]) ** 2 + (radius[0][1] - center[0][1]) ** 2) ** (0.5) - if radius_length > 60: - continue - - # if not MIN_PIXEL < radius_length < MAX_PIXEL: - # continue + if MAX_PIXEL < radius_length or radius_length < MIN_PIXEL: + continue + print(k, radius_length, len(approx)) if len(approx) in (3, 4): + # cv2.drawContours(img, contours, k, (0, 100, 100), thickness=2) dx = 0 dy = 0 @@ -154,7 +155,7 @@ def _filter_contours(self, contours, img): # cv2.drawContours(img, contours, k, (100, 0, 100), thickness=1) filtered_contours.append(self.Element(ncent, dx, dy, radius_length)) - + print('='*80) return filtered_contours @@ -163,7 +164,7 @@ def sort_contours(self, contours): def _detect_dash_line(self, img): - + print('!'*80) gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) ret, threshold_image = cv2.threshold(gray, 145, 255, cv2.THRESH_BINARY) # 150, 255, 0) contours, h = cv2.findContours(threshold_image, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) @@ -178,19 +179,22 @@ def _detect_dash_line(self, img): # cv2.circle(threshold_image, (int(prev_contour.center[0]), int(prev_contour.center[1])), int(prev_contour.rad), (255, 255, 0), # thickness=1) - # cv2.putText(threshold_image, str(0), (int(prev_contour.center[0]), int(prev_contour.center[1])), - # cv2.FONT_HERSHEY_SIMPLEX, 0.42, (0, 8, 230), thickness=1) + cv2.putText(threshold_image, str(0), (int(prev_contour.center[0]), int(prev_contour.center[1])), + cv2.FONT_HERSHEY_SIMPLEX, 0.3, (0, 80, 230), thickness=1) for i, contour in enumerate(contours[1:]): # cv2.circle(threshold_image, (int(contour.center[0]), int(contour.center[1])), int(contour.rad), (255, 255, 0), # thickness=1) max_dist = self.get_max_dist_between_elements(threshold_image, prev_contour.center[1]) - print(i) - print(max_dist) - # cv2.putText(threshold_image, str(i+1), (int(contour.center[0]), int(contour.center[1])), - # cv2.FONT_HERSHEY_SIMPLEX, 0.42, (0, 8, 230), thickness=1) + + cv2.putText(threshold_image, str(i+1), (int(contour.center[0]), int(contour.center[1])), + cv2.FONT_HERSHEY_SIMPLEX, 0.32, (0, 80, 230), thickness=1) dist = self._dist_pixels(contour.center, prev_contour.center) + print(i+1) + print(max_dist) print(dist) + print(contour.rad, prev_contour.rad, dist) + print(contour.dx, contour.dy, prev_contour.dx, prev_contour.dy) print(self.is_inside(contour, prev_contour)) if dist <= max_dist and not self.is_inside(contour, prev_contour): ans_dash_line.append(prev_contour) diff --git a/packages/custom_line_detector/src/line_detector_node.py b/packages/custom_line_detector/src/line_detector_node.py index 2441863f..e4e2b3c4 100755 --- a/packages/custom_line_detector/src/line_detector_node.py +++ b/packages/custom_line_detector/src/line_detector_node.py @@ -227,6 +227,12 @@ def image_cb(self, image_msg): # 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: + 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 :, :, :] + 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) From e3f768ed8b3140f92079a9ed18f7cc5ef68159e4 Mon Sep 17 00:00:00 2001 From: TatyanaBerlenko Date: Tue, 7 Jun 2022 15:16:13 +0300 Subject: [PATCH 11/17] add normals --- .../custom_line_detector/line_detector.py | 222 ++++++++++++------ .../src/line_detector_node.py | 34 ++- 2 files changed, 160 insertions(+), 96 deletions(-) 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 index 23f87810..961f1b0a 100644 --- a/packages/custom_line_detector/include/custom_line_detector/line_detector.py +++ b/packages/custom_line_detector/include/custom_line_detector/line_detector.py @@ -6,7 +6,7 @@ from line_detector_interface import LineDetectorInterface from .detections import Detections -MIN_PIXEL = 2 +MIN_PIXEL = 3 MAX_PIXEL = 30 PART_OF_INTEREST = 0.7 EPS = 8 @@ -50,21 +50,21 @@ class LineDetector(LineDetectorInterface): """ class Element: - def __init__(self, ncenter, ndx, ndy, rad): + def __init__(self, ncenter, ndx, ndy, rad, approx): self.center = ncenter self.dx = int(ndx) self.dy = int(ndy) self.rad = int(rad) - + self.approx = approx 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=[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 @@ -81,18 +81,19 @@ def __init__( self.last_dash_line = [] @staticmethod - def get_max_dist_between_elements(img, y): - height, width, _ = img.shape - max_dist = 170 - step = 100 - if height - step <= y <= height: - return max_dist - if height - step * 2 <= y: - return (max_dist >> 1) + 10 - elif height - step * 2.5 <= y: - return max_dist >> 2 - else: - return max_dist >> 5 + def get_polinom(): + x = [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): + print('y:', y) + pol = LineDetector.get_polinom() + result = round(pol(y)) + + return 0 if result <= 5 else result @staticmethod def _max_radius(h, img_len): @@ -109,33 +110,51 @@ def _filter_contours(self, contours, img): filtered_contours = [] for k, contour in enumerate(contours): - print('-'*80) + # print('-' * 80) # cv2.drawContours(img, contours, k, (0, 100, 100), thickness=1) approx = cv2.approxPolyDP(contour, 0.045 * cv2.arcLength(contour, True), True) center = sum(approx) / approx.shape[0] - # cv2.putText(img, str(len(approx)), (int(center[0][0]), int(center[0][1])), - # cv2.FONT_HERSHEY_SIMPLEX, 0.32, (0, 8, 230), thickness=1) radius = max(contour, key=lambda x: abs(center[0][0] - x[0][0]) ** 2 + abs(center[0][1] - x[0][1]) ** 2) radius_length = ((radius[0][0] - center[0][0]) ** 2 + (radius[0][1] - center[0][1]) ** 2) ** (0.5) + # cv2.putText(img, str(round(radius_length)), (int(center[0][0]), int(center[0][1])), + # cv2.FONT_HERSHEY_SIMPLEX, 0.22, (0, 8, 230), thickness=1) if MAX_PIXEL < radius_length or radius_length < MIN_PIXEL: continue - print(k, radius_length, len(approx)) + if len(approx) in (3, 4): + el_contour = [] # cv2.drawContours(img, contours, k, (0, 100, 100), thickness=2) dx = 0 dy = 0 + + el_0 = approx[0] + x1 = int(el_0[0][0]) + y1 = int(el_0[0][1]) + tmp = [x1, y1] + + for el in approx[1:]: + x2 = int(el[0][0]) + y2 = int(el[0][1]) + tmp.extend([x2, y2]) + el_contour.append(tmp) + tmp = [x2, y2] + tmp = [x1, y1] + el_contour.append(tmp) + for el in approx: x1 = int(el[0][0]) y1 = int(el[0][1]) for el2 in approx: + x2 = int(el2[0][0]) y2 = int(el2[0][1]) dx = max(dx, abs(x2 - x1)) dy = max(dy, abs(y2 - y1)) + dy = max(dy, int(dx * 0.4)) dx //= 2 dy //= 2 @@ -150,71 +169,61 @@ def _filter_contours(self, contours, img): ncent = [(ans[0][0] + ans[1][0]) // 2, (ans[0][1] + ans[1][1]) // 2] if np.array_equal(img[int(center[0][1]), int(center[0][0])], [0, 0, 0]): - continue # cv2.drawContours(img, contours, k, (100, 0, 100), thickness=1) - filtered_contours.append(self.Element(ncent, dx, dy, radius_length)) - print('='*80) - return filtered_contours - + filtered_contours.append(self.Element(ncent, dx, dy, radius_length, el_contour)) + # print('=' * 80) + return filtered_contours, img def sort_contours(self, contours): - return sorted(contours, key=functools.cmp_to_key(lambda contour1, contour2: self._dist_pixels(contour1.center, contour2.center)), reverse=True) - + return sorted(contours, key=functools.cmp_to_key( + lambda contour1, contour2: self._dist_pixels(contour1.center, contour2.center)), reverse=True) def _detect_dash_line(self, img): - print('!'*80) + gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) ret, threshold_image = cv2.threshold(gray, 145, 255, cv2.THRESH_BINARY) # 150, 255, 0) - contours, h = cv2.findContours(threshold_image, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + contours, h = cv2.findContours(threshold_image, 1, 2) threshold_image = cv2.cvtColor(threshold_image, cv2.COLOR_GRAY2RGB) - - contours = self._filter_contours(contours, threshold_image) + contours, threshold_image = self._filter_contours(contours, threshold_image) contours = self.sort_contours(contours) next_contour = False ans_dash_line = [] - + if not len(contours): + return threshold_image, [] prev_contour = contours[0] - # cv2.circle(threshold_image, (int(prev_contour.center[0]), int(prev_contour.center[1])), int(prev_contour.rad), (255, 255, 0), - # thickness=1) - cv2.putText(threshold_image, str(0), (int(prev_contour.center[0]), int(prev_contour.center[1])), - cv2.FONT_HERSHEY_SIMPLEX, 0.3, (0, 80, 230), thickness=1) + # cv2.circle(threshold_image, (int(prev_contour.center[0]), int(prev_contour.center[1])), int(prev_contour.rad), + # (255, 255, 0), + # thickness=1) + # cv2.putText(threshold_image, str(0), (int(prev_contour.center[0]), int(prev_contour.center[1])), + # cv2.FONT_HERSHEY_SIMPLEX, 0.42, (0, 8, 230), thickness=1) for i, contour in enumerate(contours[1:]): - # cv2.circle(threshold_image, (int(contour.center[0]), int(contour.center[1])), int(contour.rad), (255, 255, 0), - # thickness=1) - max_dist = self.get_max_dist_between_elements(threshold_image, prev_contour.center[1]) - - - cv2.putText(threshold_image, str(i+1), (int(contour.center[0]), int(contour.center[1])), - cv2.FONT_HERSHEY_SIMPLEX, 0.32, (0, 80, 230), thickness=1) + print(i + 1) + max_dist = self.get_max_dist_between_elements(prev_contour.center[1]) + print('max_dist', max_dist) + print('rad', contour.rad) + # cv2.putText(threshold_image, str(i + 1), (int(contour.center[0]), int(contour.center[1])), + # cv2.FONT_HERSHEY_SIMPLEX, 0.42, (0, 8, 230), thickness=1) dist = self._dist_pixels(contour.center, prev_contour.center) - print(i+1) - print(max_dist) - print(dist) - print(contour.rad, prev_contour.rad, dist) - print(contour.dx, contour.dy, prev_contour.dx, prev_contour.dy) - print(self.is_inside(contour, prev_contour)) + # cv2.circle(threshold_image, (int(contour.center[0]), int(contour.center[1])), int(contour.rad), + # (255, 255, 0), + # thickness=1) + print('dist', dist) if dist <= max_dist and not self.is_inside(contour, prev_contour): ans_dash_line.append(prev_contour) - cv2.circle(threshold_image, (int(prev_contour.center[0]), int(prev_contour.center[1])), int(prev_contour.rad), (255, 255, 0), - thickness=1) next_contour = True else: if next_contour: ans_dash_line.append(prev_contour) - cv2.circle(threshold_image, (int(prev_contour.center[0]), int(prev_contour.center[1])), int(prev_contour.rad), (255, 0, 220), - thickness=1) next_contour = False prev_contour = contour - print('-'*80) if next_contour: ans_dash_line.append(prev_contour) - cv2.circle(threshold_image, (int(prev_contour.center[0]), int(prev_contour.center[1])), int(prev_contour.rad), (0, 255, 2000), - thickness=1) + return threshold_image, ans_dash_line @staticmethod @@ -231,7 +240,7 @@ def is_inside(first_line, second_line): @staticmethod def is_cross(first_line, second_line): dist_between_cent = LineDetector._dist_pixels(first_line.center, second_line.center) - e = 5 # зависимость от координат + e = 5 # зависимость от координат if first_line.rad + second_line.rad < dist_between_cent + e: return False return True @@ -244,7 +253,6 @@ def _make_undistorted_image(img): def _sum(pix1, pix2): return [pix1[0] + pix2[0], pix1[1] + pix2[1]] - def setImage(self, image): """ Sets the :py:attr:`bgr` attribute to the provided image. Also stores @@ -402,6 +410,34 @@ def detectLines(self, color_range): centers, normals = self.findNormal(map, lines) return Detections(lines=lines, normals=normals, map=map, centers=centers) + def formatted_answer(self, img): + # img = self._make_undistorted_image(img) + + img, dashline = self._detect_dash_line(img) + result = [] + lenvector = 6 + + for el in dashline: + min_el = self.Element([0, 0], 0, 0, 0,0) + for el2 in dashline: + if 1 < self._dist_pixels(el2.center, el.center) < self._dist_pixels(el.center, min_el.center): + min_el = el2 + vector = [min_el.center[0] - el.center[0], min_el.center[1] - el.center[1]] + len_line = sqrt(vector[0] ** 2 + vector[1] ** 2) + + vector = [(vector[0] / len_line * lenvector), (vector[1] / len_line * lenvector)] + + point1 = self._sum(el.center, vector) # считаем концы линии элемента разметки + point2 = self._sum(el.center, [vector[0] * -1, vector[1] * -1]) + + normal = [vector[1] / lenvector, + vector[0] / lenvector] # считаем нормаль длины 1 (это вектор из начала координат) + result.append([point1, point2, normal]) + + for line in result: + cv2.line(img, (int(line[0][0]), int(line[0][1])), (int(line[1][0]), int(line[1][1])), (0, 255, 0), thickness=2) + return img + def detectYellowLines(self, img): """ Detects the line segments in the currently set image that occur in and the edges of the regions of the image @@ -413,20 +449,54 @@ def detectYellowLines(self, img): 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, """ - print('!!') + print('!!!') + + tr_img, lines = self._detect_dash_line(img) + contours = [] + for line in lines: + contours.extend(line.approx) + print(contours) + if not contours: + return None - _, lines = self._detect_dash_line(img) - print(lines) - lines = np.asarray(lines) + contours = np.asarray(contours) - # lines = np.asarray([[10,10,10,50], - # [20,20,20,60], - # [60,20,40,60], - # [40,30,20,40] - # ]) map = np.full((80, 160), 255, dtype=int) - centers, normals = self.findNormal(map, lines) - return Detections(lines=lines, normals=normals, map=map, centers=centers) + centers, normals = self.findNormal(map, contours) + print('='*80) + return Detections(lines=contours, normals=normals, map=map, centers=centers) + +'''[array([[[10, 73]], + + [[11, 78]], + + [[15, 79]]], dtype=int32), array([[[22, 57]], + + [[14, 66]], + + [[18, 72]], + + [[27, 60]]], dtype=int32), array([[[41, 43]], + + [[36, 42]], + + [[28, 51]], + + [[33, 53]]], dtype=int32), array([[[52, 32]], + + [[46, 32]], + + [[41, 37]], + + [[46, 38]]], dtype=int32), array([[[51, 28]], + + [[55, 28]], + + [[59, 25]], + + [[54, 25]]], dtype=int32)] +''' + ''' lines = [[86 10 86 4]] normals = [[ 1. -0.]] map = [[0 0 0 ... 0 0 0] @@ -515,4 +585,4 @@ def detectYellowLines(self, img): [0 0 0 ... 0 0 0]] centers = [] {'RED': , 'WHITE': , 'YELLOW': } -''' \ No newline at end of file +''' diff --git a/packages/custom_line_detector/src/line_detector_node.py b/packages/custom_line_detector/src/line_detector_node.py index e4e2b3c4..4181576e 100755 --- a/packages/custom_line_detector/src/line_detector_node.py +++ b/packages/custom_line_detector/src/line_detector_node.py @@ -180,7 +180,7 @@ def image_cb(self, image_msg): 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) @@ -188,9 +188,9 @@ def image_cb(self, image_msg): 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.detectYellowLines(ranges, image) + for color, ranges in list(self.color_ranges.items()): + if color == 'YELLOW': + detections['YELLOW'] = self.detector.detectYellowLines(image) # Construct a SegmentList @@ -227,12 +227,6 @@ def image_cb(self, image_msg): # 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: - 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 :, :, :] - 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) @@ -253,16 +247,8 @@ def image_cb(self, image_msg): if self.pub_contours.get_num_connections() > 0: # image = cv2.undistort(image, np.array(self.K).reshape((3, 3)), np.array(self.D)) - image = cv2.undistort(image, self.camera_model.K, self.camera_model.D) - - # Resize the image to the desired dimensions - 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 :, :, :] - - image, _ = self.detector._detect_dash_line(image) + # image = cv2.undistort(image, self.camera_model.K, self.camera_model.D) + image = self.detector.formatted_answer(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) @@ -275,6 +261,14 @@ def image_cb(self, image_msg): 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): """ From a5c981e8f0a3b8a6c7f2565ec237c82d7999772d Mon Sep 17 00:00:00 2001 From: TatyanaBerlenko Date: Tue, 7 Jun 2022 16:18:16 +0300 Subject: [PATCH 12/17] add normals --- .../custom_line_detector/line_detector.py | 23 ++++--------------- .../src/line_detector_node.py | 1 - 2 files changed, 5 insertions(+), 19 deletions(-) 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 index 961f1b0a..fcb9428d 100644 --- a/packages/custom_line_detector/include/custom_line_detector/line_detector.py +++ b/packages/custom_line_detector/include/custom_line_detector/line_detector.py @@ -89,7 +89,6 @@ def get_polinom(): @staticmethod def get_max_dist_between_elements(y): - print('y:', y) pol = LineDetector.get_polinom() result = round(pol(y)) @@ -110,7 +109,6 @@ def _filter_contours(self, contours, img): filtered_contours = [] for k, contour in enumerate(contours): - # print('-' * 80) # cv2.drawContours(img, contours, k, (0, 100, 100), thickness=1) approx = cv2.approxPolyDP(contour, 0.045 * cv2.arcLength(contour, True), True) @@ -125,7 +123,6 @@ def _filter_contours(self, contours, img): continue if len(approx) in (3, 4): - el_contour = [] # cv2.drawContours(img, contours, k, (0, 100, 100), thickness=2) dx = 0 @@ -142,7 +139,7 @@ def _filter_contours(self, contours, img): tmp.extend([x2, y2]) el_contour.append(tmp) tmp = [x2, y2] - tmp = [x1, y1] + tmp.extend([x1, y1]) el_contour.append(tmp) for el in approx: @@ -173,7 +170,6 @@ def _filter_contours(self, contours, img): # cv2.drawContours(img, contours, k, (100, 0, 100), thickness=1) filtered_contours.append(self.Element(ncent, dx, dy, radius_length, el_contour)) - # print('=' * 80) return filtered_contours, img def sort_contours(self, contours): @@ -200,17 +196,13 @@ def _detect_dash_line(self, img): # cv2.putText(threshold_image, str(0), (int(prev_contour.center[0]), int(prev_contour.center[1])), # cv2.FONT_HERSHEY_SIMPLEX, 0.42, (0, 8, 230), thickness=1) for i, contour in enumerate(contours[1:]): - print(i + 1) max_dist = self.get_max_dist_between_elements(prev_contour.center[1]) - print('max_dist', max_dist) - print('rad', contour.rad) # cv2.putText(threshold_image, str(i + 1), (int(contour.center[0]), int(contour.center[1])), # cv2.FONT_HERSHEY_SIMPLEX, 0.42, (0, 8, 230), thickness=1) dist = self._dist_pixels(contour.center, prev_contour.center) # cv2.circle(threshold_image, (int(contour.center[0]), int(contour.center[1])), int(contour.rad), # (255, 255, 0), # thickness=1) - print('dist', dist) if dist <= max_dist and not self.is_inside(contour, prev_contour): ans_dash_line.append(prev_contour) next_contour = True @@ -449,22 +441,17 @@ def detectYellowLines(self, img): 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, """ - print('!!!') - + map_ = np.full((80, 160), 255, dtype=int) tr_img, lines = self._detect_dash_line(img) contours = [] for line in lines: contours.extend(line.approx) - print(contours) if not contours: - return None + return Detections(lines=contours, normals=[], map=map_, centers=[]) contours = np.asarray(contours) - - map = np.full((80, 160), 255, dtype=int) - centers, normals = self.findNormal(map, contours) - print('='*80) - return Detections(lines=contours, normals=normals, map=map, centers=centers) + centers, normals = self.findNormal(map_, contours) + return Detections(lines=contours, normals=normals, map=map_, centers=centers) '''[array([[[10, 73]], diff --git a/packages/custom_line_detector/src/line_detector_node.py b/packages/custom_line_detector/src/line_detector_node.py index 4181576e..1303dc99 100755 --- a/packages/custom_line_detector/src/line_detector_node.py +++ b/packages/custom_line_detector/src/line_detector_node.py @@ -192,7 +192,6 @@ def image_cb(self, image_msg): if color == 'YELLOW': detections['YELLOW'] = self.detector.detectYellowLines(image) - # Construct a SegmentList segment_list = SegmentList() segment_list.header.stamp = image_msg.header.stamp From ab4f189f09399fdc729e893da7dd931baac0aec0 Mon Sep 17 00:00:00 2001 From: TatyanaBerlenko Date: Wed, 8 Jun 2022 15:34:10 +0300 Subject: [PATCH 13/17] fix --- packages/custom_line_detector/src/line_detector_node.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/packages/custom_line_detector/src/line_detector_node.py b/packages/custom_line_detector/src/line_detector_node.py index 1303dc99..6f82ccd5 100755 --- a/packages/custom_line_detector/src/line_detector_node.py +++ b/packages/custom_line_detector/src/line_detector_node.py @@ -188,9 +188,9 @@ def image_cb(self, image_msg): 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.detectYellowLines(image) + # for color, ranges in list(self.color_ranges.items()): + # if color == 'YELLOW': + # detections['YELLOW'] = self.detector.detectYellowLines(image) # Construct a SegmentList segment_list = SegmentList() From c4f11ab0dfd2c5dd3f3b004ba8cd155191769b11 Mon Sep 17 00:00:00 2001 From: TatyanaBerlenko Date: Wed, 15 Jun 2022 15:05:31 +0300 Subject: [PATCH 14/17] split normal --- packages/circle_drive/scripts/circle_drive.py | 0 .../custom_line_detector/line_detector.py | 47 ++++++++++++++++++- .../src/line_detector_node.py | 6 +-- 3 files changed, 48 insertions(+), 5 deletions(-) mode change 100755 => 100644 packages/circle_drive/scripts/circle_drive.py 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/include/custom_line_detector/line_detector.py b/packages/custom_line_detector/include/custom_line_detector/line_detector.py index fcb9428d..4e918ba8 100644 --- a/packages/custom_line_detector/include/custom_line_detector/line_detector.py +++ b/packages/custom_line_detector/include/custom_line_detector/line_detector.py @@ -5,6 +5,8 @@ from math import sqrt from line_detector_interface import LineDetectorInterface from .detections import Detections +import numpy as np + MIN_PIXEL = 3 MAX_PIXEL = 30 @@ -123,6 +125,7 @@ def _filter_contours(self, contours, img): continue if len(approx) in (3, 4): + el_contour = [] # cv2.drawContours(img, contours, k, (0, 100, 100), thickness=2) dx = 0 @@ -131,16 +134,56 @@ def _filter_contours(self, contours, img): el_0 = approx[0] x1 = int(el_0[0][0]) y1 = int(el_0[0][1]) + tmp = [x1, y1] + contour = [] + d = [] for el in approx[1:]: x2 = int(el[0][0]) y2 = int(el[0][1]) + tmp.extend([x2, y2]) - el_contour.append(tmp) + d.append(self._dist_pixels(tmp[:2], tmp[2:])) + contour.append(tmp) tmp = [x2, y2] + tmp.extend([x1, y1]) - el_contour.append(tmp) + contour.append(tmp) + d.append(self._dist_pixels(tmp[:2], tmp[2:])) + + max_dist = max(d) + max_dist_index = d.index(max_dist) + + c = contour[max_dist_index] + x_list = list(map(int, np.linspace(c[0], c[2], 5))) + y_list = list(map(int, np.linspace(c[1], c[3], 5))) + print('x', x_list) + print('y', y_list) + el_contour = [] + tmp = [] + for x, y in zip(x_list, y_list): + if tmp: + tmp.extend([x, y]) + el_contour.append(tmp) + tmp = [x, y] + + d.pop(max_dist_index) + contour.pop(max_dist_index) + + max_dist = max(d) + max_dist_index = d.index(max_dist) + + c = contour[max_dist_index] + x_list = list(map(int, np.linspace(c[0], c[2], 5))) + y_list = list(map(int, np.linspace(c[1], c[3], 5))) + tmp = [x_list[0], y_list[0]] + for x, y in zip(x_list, y_list): + tmp.extend([x, y]) + el_contour.append(tmp) + tmp = [x, y] + + print(el_contour) for el in approx: x1 = int(el[0][0]) diff --git a/packages/custom_line_detector/src/line_detector_node.py b/packages/custom_line_detector/src/line_detector_node.py index 6f82ccd5..1303dc99 100755 --- a/packages/custom_line_detector/src/line_detector_node.py +++ b/packages/custom_line_detector/src/line_detector_node.py @@ -188,9 +188,9 @@ def image_cb(self, image_msg): 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.detectYellowLines(image) + for color, ranges in list(self.color_ranges.items()): + if color == 'YELLOW': + detections['YELLOW'] = self.detector.detectYellowLines(image) # Construct a SegmentList segment_list = SegmentList() From a9e6d685c62a281402f881e2d9dbc4bed60b0974 Mon Sep 17 00:00:00 2001 From: TatyanaBerlenko Date: Wed, 22 Jun 2022 12:53:42 +0300 Subject: [PATCH 15/17] add cos checking --- .../custom_line_detector/line_detector.py | 205 ++++++++++++------ .../config/lane_filter_node/default.yaml | 23 -- 2 files changed, 133 insertions(+), 95 deletions(-) delete mode 100644 packages/dt-core/packages/lane_filter/config/lane_filter_node/default.yaml 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 index 4e918ba8..adc033e0 100644 --- a/packages/custom_line_detector/include/custom_line_detector/line_detector.py +++ b/packages/custom_line_detector/include/custom_line_detector/line_detector.py @@ -16,6 +16,7 @@ [[278.79547761007365, 0.0, 314.29374336264345], [0.0, 280.52395701002115, 228.59132685202135], [0.0, 0.0, 1.0]]) DISTORTION_COEFFFICIENTS = np.array( [-0.23670917420627122, 0.03455456424406622, 0.0037778674941860426, 0.0020245279929775382, 0.0]) +BLACK_PIXEL = [0, 0, 0] class LineDetector(LineDetectorInterface): @@ -80,7 +81,7 @@ def __init__( 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_dash_line = [] + self.last_correct_edge = [] @staticmethod def get_polinom(): @@ -109,81 +110,35 @@ def _dist_pixels(pix1, pix2): def _filter_contours(self, contours, img): + self.last_correct_edge = [] filtered_contours = [] for k, contour in enumerate(contours): - - # cv2.drawContours(img, contours, k, (0, 100, 100), thickness=1) 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 = ((radius[0][0] - center[0][0]) ** 2 + (radius[0][1] - center[0][1]) ** 2) ** (0.5) - # cv2.putText(img, str(round(radius_length)), (int(center[0][0]), int(center[0][1])), - # cv2.FONT_HERSHEY_SIMPLEX, 0.22, (0, 8, 230), thickness=1) - if MAX_PIXEL < radius_length or radius_length < MIN_PIXEL: continue - - if len(approx) in (3, 4): - - el_contour = [] - # cv2.drawContours(img, contours, k, (0, 100, 100), thickness=2) + if len(approx) == 4: + print('='*80) dx = 0 dy = 0 + contour, distances = self.create_contour_from_approx(approx) + max_dist_index = self.get_edge_index(contour, distances, img.shape[0]-1) - el_0 = approx[0] - x1 = int(el_0[0][0]) - y1 = int(el_0[0][1]) - - tmp = [x1, y1] - contour = [] - d = [] - - for el in approx[1:]: - x2 = int(el[0][0]) - y2 = int(el[0][1]) - - tmp.extend([x2, y2]) - d.append(self._dist_pixels(tmp[:2], tmp[2:])) - contour.append(tmp) - tmp = [x2, y2] - - tmp.extend([x1, y1]) - contour.append(tmp) - d.append(self._dist_pixels(tmp[:2], tmp[2:])) - - max_dist = max(d) - max_dist_index = d.index(max_dist) - - c = contour[max_dist_index] - x_list = list(map(int, np.linspace(c[0], c[2], 5))) - y_list = list(map(int, np.linspace(c[1], c[3], 5))) - print('x', x_list) - print('y', y_list) - el_contour = [] - tmp = [] - for x, y in zip(x_list, y_list): - if tmp: - tmp.extend([x, y]) - el_contour.append(tmp) - tmp = [x, y] - - d.pop(max_dist_index) - contour.pop(max_dist_index) - - max_dist = max(d) - max_dist_index = d.index(max_dist) - - c = contour[max_dist_index] - x_list = list(map(int, np.linspace(c[0], c[2], 5))) - y_list = list(map(int, np.linspace(c[1], c[3], 5))) - tmp = [x_list[0], y_list[0]] - for x, y in zip(x_list, y_list): - tmp.extend([x, y]) - el_contour.append(tmp) - tmp = [x, y] - - print(el_contour) + edge = contour[max_dist_index] + # cv2.line(img, (edge[0], edge[1]), (edge[2], edge[3]), (0, 205, 90), 1) + # print('max edge:', edge,'\nmax_dist_index:', max_dist_index) + + contours = LineDetector.get_contour_by_piece(edge) + tmp_d = {0: 2, 2: 0, 1: 3, 3: 1} # может быть 3точечный + max_dist_index_2 = tmp_d[max_dist_index] + edge = contour[max_dist_index_2] + # print('max edge:', edge,'\nmax_dist_index:', max_dist_index) + # cv2.line(img, (edge[0], edge[1]), (edge[2], edge[3]), (233, 0, 0), 1) + contours += LineDetector.get_contour_by_piece(edge) + + # print('FIN CONT:', contours, 'LEN F C', len(contours)) for el in approx: x1 = int(el[0][0]) @@ -208,21 +163,127 @@ def _filter_contours(self, contours, img): ans = [el[0], el2[0]] ncent = [(ans[0][0] + ans[1][0]) // 2, (ans[0][1] + ans[1][1]) // 2] - if np.array_equal(img[int(center[0][1]), int(center[0][0])], [0, 0, 0]): + if np.array_equal(img[int(center[0][1]), int(center[0][0])], BLACK_PIXEL): continue - # cv2.drawContours(img, contours, k, (100, 0, 100), thickness=1) - - filtered_contours.append(self.Element(ncent, dx, dy, radius_length, el_contour)) + filtered_contours.append(self.Element(ncent, dx, dy, radius_length, [])) return filtered_contours, img + def get_edge_index(self, contour, distances, y): + # print('analysis') + # print('dist', distances) + # for edge in contour: + # print('x1-x2', abs(edge[0]-edge[2])) + # print('y1-y2', abs(edge[1]-edge[3])) + if self.last_correct_edge: + inv = False + last = self.last_correct_edge + else: + inv = True + last = (0, y, contour[0][0], y) + edge_index = self.get_index_by_cos(contour[0], contour[1], distances[0], distances[1], + last, inv) + if edge_index != -1: + self.last_correct_edge = contour[edge_index] + print('by cos') + 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] + print('by length') + return max_dist_index + print('by 50%') + self.last_correct_edge = contour[0] + return 0 + + def get_index_by_cos(self, a, b, d_a, d_b, last, inv=False): + D_E = 0.5 + E = 0.05 + + cos_a = abs(self.get_cos(last, a, d_a)) + cos_b = abs(self.get_cos(last, b, d_b)) + print('last correct', last) + print('a', a) + print('b', b) + print('cos_a', cos_a) + print('cos_b', cos_b) + print('inv', inv) + + if abs(cos_a - 1) <= E: + if not inv: + return 0 + return 1 + if abs(cos_b - 1) <= E: + if not inv: + return 1 + return 0 + + if abs(cos_a - cos_b) > D_E: + + if not inv: + return 0 if cos_a > cos_b else 1 + else: + return 1 if cos_a > cos_b else 0 + + else: + return -1 + + def get_cos(self, last, current, d_current): + d_last = self._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 (last[0] * current[0] + last[1] * current[1]) / (d_last * d_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))) + el_contour = [] + tmp = [] + for x, y in zip(x_list, y_list): + if tmp: + tmp.extend([x, y]) + el_contour.append(tmp) + tmp = [x, y] + return el_contour + + def create_contour_from_approx(self, approx): + el_0 = approx[0] + x1 = int(el_0[0][0]) + y1 = int(el_0[0][1]) + + tmp = [x1, y1] + contour = [] + distances = [] + + for el in approx[1:]: + x2 = int(el[0][0]) + y2 = int(el[0][1]) + + tmp.extend([x2, y2]) + distances.append(self._dist_pixels(tmp[:2], tmp[2:])) + contour.append(tmp) + tmp = [x2, y2] + + tmp.extend([x1, y1]) + contour.append(tmp) + distances.append(self._dist_pixels(tmp[:2], tmp[2:])) + return contour, distances + def sort_contours(self, contours): return sorted(contours, key=functools.cmp_to_key( lambda contour1, contour2: self._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, 145, 255, cv2.THRESH_BINARY) # 150, 255, 0) + 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) @@ -485,7 +546,7 @@ def detectYellowLines(self, img): :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((80, 160), 255, dtype=int) - tr_img, lines = self._detect_dash_line(img) + _, lines = self._detect_dash_line(img) contours = [] for line in lines: contours.extend(line.approx) 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 From 94760ac3d7f998006b6b9662062293bb34e94c5b Mon Sep 17 00:00:00 2001 From: TatyanaBerlenko Date: Fri, 1 Jul 2022 14:47:25 +0300 Subject: [PATCH 16/17] refactoring --- .../custom_line_detector/line_detector.py | 539 ++++++------------ .../src/line_detector_node.py | 5 +- 2 files changed, 178 insertions(+), 366 deletions(-) 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 index adc033e0..72d48e26 100644 --- a/packages/custom_line_detector/include/custom_line_detector/line_detector.py +++ b/packages/custom_line_detector/include/custom_line_detector/line_detector.py @@ -1,22 +1,14 @@ import functools import cv2 -import numpy as np from math import sqrt from line_detector_interface import LineDetectorInterface from .detections import Detections import numpy as np -MIN_PIXEL = 3 -MAX_PIXEL = 30 -PART_OF_INTEREST = 0.7 -EPS = 8 -CAMERA_MATRIX = np.array( - [[278.79547761007365, 0.0, 314.29374336264345], [0.0, 280.52395701002115, 228.59132685202135], [0.0, 0.0, 1.0]]) -DISTORTION_COEFFFICIENTS = np.array( - [-0.23670917420627122, 0.03455456424406622, 0.0037778674941860426, 0.0020245279929775382, 0.0]) BLACK_PIXEL = [0, 0, 0] +EDGES_NUM_IN_RECT = 4 class LineDetector(LineDetectorInterface): @@ -52,14 +44,21 @@ class LineDetector(LineDetectorInterface): """ - class Element: - def __init__(self, ncenter, ndx, ndy, rad, approx): - self.center = ncenter - self.dx = int(ndx) - self.dy = int(ndy) + 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], @@ -84,271 +83,235 @@ def __init__( self.last_correct_edge = [] @staticmethod - def get_polinom(): - x = [74, 69, 65, 48, 45, 40, 29, 0] + 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) + # print('degrees_a', degrees_a, 'degrees_b', degrees_b) + if abs(degrees_a - degrees_b) > DEGREES_DIST: + # print('abs(degrees_a - degrees_b) > DEGREES_E4', abs(cos_a - cos_b) > DEGREES_DIST, 'DEGREES_E', 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: + # print('abs(degrees_a - RIGHT_ANGLE) <= RIGHT_ANGLE_E', abs(degrees_a - RIGHT_ANGLE) <= RIGHT_ANGLE_DIST) + if not inv: + return 0 + return 1 + if abs(degrees_b - RIGHT_ANGLE) <= RIGHT_ANGLE_DIST: + # print('abs(degrees_b - RIGHT_ANGLE) <= RIGHT_ANGLE_E', 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): - pol = LineDetector.get_polinom() - result = round(pol(y)) - - return 0 if result <= 5 else result - - @staticmethod - def _max_radius(h, img_len): - if h > img_len * PART_OF_INTEREST: - return 0 - alpha = (1 - (MIN_PIXEL + EPS) / MAX_PIXEL) / (img_len * PART_OF_INTEREST) - return MAX_PIXEL * (1 - alpha * h) + 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 int(sqrt((pix1[0] - pix2[0]) ** 2 + (pix1[1] - pix2[1]) ** 2)) + return sqrt((pix1[0] - pix2[0]) ** 2 + (pix1[1] - pix2[1]) ** 2) def _filter_contours(self, contours, img): - - self.last_correct_edge = [] 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 = ((radius[0][0] - center[0][0]) ** 2 + (radius[0][1] - center[0][1]) ** 2) ** (0.5) - if MAX_PIXEL < radius_length or radius_length < MIN_PIXEL: - continue - if len(approx) == 4: - print('='*80) - dx = 0 - dy = 0 - contour, distances = self.create_contour_from_approx(approx) - max_dist_index = self.get_edge_index(contour, distances, img.shape[0]-1) - - edge = contour[max_dist_index] - # cv2.line(img, (edge[0], edge[1]), (edge[2], edge[3]), (0, 205, 90), 1) - # print('max edge:', edge,'\nmax_dist_index:', max_dist_index) - - contours = LineDetector.get_contour_by_piece(edge) - tmp_d = {0: 2, 2: 0, 1: 3, 3: 1} # может быть 3точечный - max_dist_index_2 = tmp_d[max_dist_index] - edge = contour[max_dist_index_2] - # print('max edge:', edge,'\nmax_dist_index:', max_dist_index) - # cv2.line(img, (edge[0], edge[1]), (edge[2], edge[3]), (233, 0, 0), 1) - contours += LineDetector.get_contour_by_piece(edge) - - # print('FIN CONT:', contours, 'LEN F C', len(contours)) - - for el in approx: - x1 = int(el[0][0]) - y1 = int(el[0][1]) - for el2 in approx: - - x2 = int(el2[0][0]) - y2 = int(el2[0][1]) - dx = max(dx, abs(x2 - x1)) - dy = max(dy, abs(y2 - y1)) - - dy = max(dy, int(dx * 0.4)) - dx //= 2 - dy //= 2 - ans = [] - max_dist = 0 - - for el in approx: - for el2 in approx: - if self._dist_pixels(el[0], el2[0]) > max_dist: - max_dist = self._dist_pixels(el[0], el2[0]) - ans = [el[0], el2[0]] - ncent = [(ans[0][0] + ans[1][0]) // 2, (ans[0][1] + ans[1][1]) // 2] - - if np.array_equal(img[int(center[0][1]), int(center[0][0])], BLACK_PIXEL): + 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.Element(ncent, dx, dy, radius_length, [])) + + filtered_contours.append(self.Contour(cont_center, radius_length, approx)) return filtered_contours, img - def get_edge_index(self, contour, distances, y): - # print('analysis') - # print('dist', distances) - # for edge in contour: - # print('x1-x2', abs(edge[0]-edge[2])) - # print('y1-y2', abs(edge[1]-edge[3])) + 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: - inv = False + has_last = False last = self.last_correct_edge else: - inv = True - last = (0, y, contour[0][0], y) - edge_index = self.get_index_by_cos(contour[0], contour[1], distances[0], distances[1], - last, inv) - if edge_index != -1: - self.last_correct_edge = contour[edge_index] - print('by cos') - return edge_index + 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] - print('by length') return max_dist_index - print('by 50%') - self.last_correct_edge = contour[0] - return 0 - - def get_index_by_cos(self, a, b, d_a, d_b, last, inv=False): - D_E = 0.5 - E = 0.05 - - cos_a = abs(self.get_cos(last, a, d_a)) - cos_b = abs(self.get_cos(last, b, d_b)) - print('last correct', last) - print('a', a) - print('b', b) - print('cos_a', cos_a) - print('cos_b', cos_b) - print('inv', inv) - - if abs(cos_a - 1) <= E: - if not inv: - return 0 - return 1 - if abs(cos_b - 1) <= E: - if not inv: - return 1 - return 0 - - if abs(cos_a - cos_b) > D_E: - - if not inv: - return 0 if cos_a > cos_b else 1 - else: - return 1 if cos_a > cos_b else 0 + 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: - return -1 + edge_index = 1 if cos_a > cos_b else 0 + self.last_correct_edge = contour[edge_index] + return edge_index - def get_cos(self, last, current, d_current): - d_last = self._dist_pixels((last[0], last[1]), (last[2], last[3])) + @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 (last[0] * current[0] + last[1] * current[1]) / (d_last * d_current) + 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): + 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))) - el_contour = [] - tmp = [] + contour = [] + current_piece = [] for x, y in zip(x_list, y_list): - if tmp: - tmp.extend([x, y]) - el_contour.append(tmp) - tmp = [x, y] - return el_contour - - def create_contour_from_approx(self, approx): - el_0 = approx[0] - x1 = int(el_0[0][0]) - y1 = int(el_0[0][1]) - - tmp = [x1, y1] + 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 el in approx[1:]: - x2 = int(el[0][0]) - y2 = int(el[0][1]) + for vertex in approx[1:]: + x_1 = int(vertex[0][0]) + y_1 = int(vertex[0][1]) - tmp.extend([x2, y2]) - distances.append(self._dist_pixels(tmp[:2], tmp[2:])) - contour.append(tmp) - tmp = [x2, y2] + 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] - tmp.extend([x1, y1]) - contour.append(tmp) - distances.append(self._dist_pixels(tmp[:2], tmp[2:])) + 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 - def sort_contours(self, contours): + @staticmethod + def _sort_contours(contours): return sorted(contours, key=functools.cmp_to_key( - lambda contour1, contour2: self._dist_pixels(contour1.center, contour2.center)), reverse=True) + lambda contour1, contour2: LineDetector._dist_pixels(contour1.center, contour2.center)), reverse=True) + + def detect_dash_line(self, img): - 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) - contours = self.sort_contours(contours) - next_contour = False - ans_dash_line = [] if not len(contours): return threshold_image, [] + contours = LineDetector._sort_contours(contours) + + next_contour = False + dash_lines = [] prev_contour = contours[0] - # cv2.circle(threshold_image, (int(prev_contour.center[0]), int(prev_contour.center[1])), int(prev_contour.rad), - # (255, 255, 0), - # thickness=1) - # cv2.putText(threshold_image, str(0), (int(prev_contour.center[0]), int(prev_contour.center[1])), - # cv2.FONT_HERSHEY_SIMPLEX, 0.42, (0, 8, 230), thickness=1) for i, contour in enumerate(contours[1:]): - max_dist = self.get_max_dist_between_elements(prev_contour.center[1]) - # cv2.putText(threshold_image, str(i + 1), (int(contour.center[0]), int(contour.center[1])), - # cv2.FONT_HERSHEY_SIMPLEX, 0.42, (0, 8, 230), thickness=1) - dist = self._dist_pixels(contour.center, prev_contour.center) - # cv2.circle(threshold_image, (int(contour.center[0]), int(contour.center[1])), int(contour.rad), - # (255, 255, 0), - # thickness=1) - if dist <= max_dist and not self.is_inside(contour, prev_contour): - ans_dash_line.append(prev_contour) + + 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: - ans_dash_line.append(prev_contour) + dash_lines.append(prev_contour) next_contour = False prev_contour = contour if next_contour: - ans_dash_line.append(prev_contour) - - return threshold_image, ans_dash_line + dash_lines.append(prev_contour) + return threshold_image, dash_lines @staticmethod - def is_inside(first_line, second_line): - if first_line.center == second_line.center and first_line.rad == second_line.rad: # переопределить метод сравнения + def _is_inside(first_line, second_line): + if first_line == second_line: return False - big_cont = first_line if first_line.rad > second_line.rad else second_line - small_cont = first_line if first_line.rad < second_line.rad else second_line + 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 - @staticmethod - def is_cross(first_line, second_line): - dist_between_cent = LineDetector._dist_pixels(first_line.center, second_line.center) - e = 5 # зависимость от координат - if first_line.rad + second_line.rad < dist_between_cent + e: - return False - return True - - @staticmethod - def _make_undistorted_image(img): - return cv2.undistort(img, CAMERA_MATRIX, DISTORTION_COEFFFICIENTS) - - @staticmethod - def _sum(pix1, pix2): - return [pix1[0] + pix2[0], pix1[1] + pix2[1]] - def setImage(self, image): """ Sets the :py:attr:`bgr` attribute to the provided image. Also stores @@ -506,174 +469,24 @@ def detectLines(self, color_range): centers, normals = self.findNormal(map, lines) return Detections(lines=lines, normals=normals, map=map, centers=centers) - def formatted_answer(self, img): - # img = self._make_undistorted_image(img) - - img, dashline = self._detect_dash_line(img) - result = [] - lenvector = 6 - - for el in dashline: - min_el = self.Element([0, 0], 0, 0, 0,0) - for el2 in dashline: - if 1 < self._dist_pixels(el2.center, el.center) < self._dist_pixels(el.center, min_el.center): - min_el = el2 - vector = [min_el.center[0] - el.center[0], min_el.center[1] - el.center[1]] - len_line = sqrt(vector[0] ** 2 + vector[1] ** 2) - - vector = [(vector[0] / len_line * lenvector), (vector[1] / len_line * lenvector)] - - point1 = self._sum(el.center, vector) # считаем концы линии элемента разметки - point2 = self._sum(el.center, [vector[0] * -1, vector[1] * -1]) - - normal = [vector[1] / lenvector, - vector[0] / lenvector] # считаем нормаль длины 1 (это вектор из начала координат) - result.append([point1, point2, normal]) - - for line in result: - cv2.line(img, (int(line[0][0]), int(line[0][1])), (int(line[1][0]), int(line[1][1])), (0, 255, 0), thickness=2) - return img - - def detectYellowLines(self, img): + 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: - color_range (:py:class:`ColorRange`): A :py:class:`ColorRange` object specifying the desired colors. + 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((80, 160), 255, dtype=int) - _, lines = self._detect_dash_line(img) - contours = [] - for line in lines: - contours.extend(line.approx) - if not contours: - return Detections(lines=contours, normals=[], map=map_, centers=[]) - - contours = np.asarray(contours) - centers, normals = self.findNormal(map_, contours) - return Detections(lines=contours, normals=normals, map=map_, centers=centers) - -'''[array([[[10, 73]], - - [[11, 78]], - - [[15, 79]]], dtype=int32), array([[[22, 57]], - - [[14, 66]], - - [[18, 72]], - - [[27, 60]]], dtype=int32), array([[[41, 43]], - - [[36, 42]], - - [[28, 51]], - - [[33, 53]]], dtype=int32), array([[[52, 32]], - - [[46, 32]], - - [[41, 37]], - - [[46, 38]]], dtype=int32), array([[[51, 28]], - - [[55, 28]], - - [[59, 25]], - - [[54, 25]]], dtype=int32)] -''' - - -''' -lines = [[86 10 86 4]] normals = [[ 1. -0.]] map = [[0 0 0 ... 0 0 0] - [0 0 0 ... 0 0 0] - [0 0 0 ... 0 0 0] - ... - [0 0 0 ... 0 0 0] - [0 0 0 ... 0 0 0] - [0 0 0 ... 0 0 0]] centers = [[86. 7.]] -lines = [[ 7 14 17 13] - [18 12 22 12] - [ 4 21 4 15] - [31 10 39 10] - [ 5 26 6 29] - [24 1 28 3]][autobot01/stop_line_filter_node-8] killing on exit -[autobot01/lane_filter_node-7] killing on exit -[INFO] [1646920190.060340]: [/autobot01/stop_line_filter_node] Received shutdown request. -[INFO] [1646920190.067721]: [/autobot01/lane_filter_node] Received shutdown request. - normals = [[ 0.09950372 0.99503719] - [-0. 1. ] - [ 1. -0. ] - [ 0. -1. ] - [-0.9486833 0.31622777] - [-0.4472136 0.89442719]] map = [[255 255 255 ... 255 255 255] - [255 255 255 ... 255 255 255] - [255 255 255 ... 255 255 255] - ... - [ 0 0 0 ... 0 0 0] - [ 0 0 0 ... 0 0 0] - [ 0 0 0 ... 0 0 0]][autobot01/ground_projection_node-6] killing on exit - centers = [[12. 13.5] - [20. 12. ] - [ 4. 18. ] - [35. 10. ] - [ 5.5 27.5] - [26. 2. ]] -[INFO] [1646920190.105539]: [/autobot01/ground_projection_node] Received shutdown request. -[autobot01/line_detector_node-5] killing on exit -lines = [] normals = [] map = [[0 0 0 ... 0 0 0] - [0 0 0 ... 0 0 0] - [0 0 0 ... 0 0 0] - ... - [0 0 0 ... 0 0 0] - [0 0 0 ... 0 0 0] - [0 0 0 ... 0 0 0]] centers = [] -{'RED': , 'WHITE': , 'YELLOW': } -[INFO] [1646920190.133156]: [/autobot01/line_detector_node] Received shutdown request. -lines = [[86 10 86 4]] normals = [[ 1. -0.]] map = [[0 0 0 ... 0 0 0] - [0 0 0 ... 0 0 0] - [0 0 0 ... 0 0 0] - ... - [0 0 0 ... 0 0 0] - [0 0 0 ... 0 0 0] - [0 0 0 ... 0 0 0]] centers = [[86. 7.]] -lines = [[35 10 41 10] - [ 4 21 4 15] - [10 14 15 13] - [ 5 26 5 22] - [ 1 55 4 62] - [25 11 29 11] - [22 12 24 9]] normals = [[ 0. -1. ] - [ 1. -0. ] - [ 0.19611614 0.98058068] - [ 1. -0. ] - [-0.91914503 0.3939193 ] - [-0. 1. ] - [ 0.83205029 0.5547002 ]] map = [[255 255 255 ... 255 255 255] - [255 255 255 ... 255 255 255] - [255 255 255 ... 255 255 255] - ... - [ 0 0 0 ... 0 0 0] - [ 0 0 0 ... 0 0 0] - [ 0 0 0 ... 0 0 0]] centers = [[38. 10. ] - [ 4. 18. ] - [12.5 13.5] - [ 5. 24. ] - [ 2.5 58.5] - [27. 11. ] - [23. 10.5]] -lines = [] normals = [] map = [[0 0 0 ... 0 0 0] - [0 0 0 ... 0 0 0] - [0 0 0 ... 0 0 0] - ... - [0 0 0 ... 0 0 0] - [0 0 0 ... 0 0 0] - [0 0 0 ... 0 0 0]] centers = [] -{'RED': , 'WHITE': , 'YELLOW': } - -''' + 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/src/line_detector_node.py b/packages/custom_line_detector/src/line_detector_node.py index 1303dc99..3c6ef50d 100755 --- a/packages/custom_line_detector/src/line_detector_node.py +++ b/packages/custom_line_detector/src/line_detector_node.py @@ -190,7 +190,7 @@ def image_cb(self, image_msg): } for color, ranges in list(self.color_ranges.items()): if color == 'YELLOW': - detections['YELLOW'] = self.detector.detectYellowLines(image) + detections['YELLOW'] = self.detector.detect_yellow_lines(image) # Construct a SegmentList segment_list = SegmentList() @@ -245,9 +245,8 @@ def image_cb(self, image_msg): self.pub_d_maps.publish(debug_image_msg) if self.pub_contours.get_num_connections() > 0: - # image = cv2.undistort(image, np.array(self.K).reshape((3, 3)), np.array(self.D)) # image = cv2.undistort(image, self.camera_model.K, self.camera_model.D) - image = self.detector.formatted_answer(image) + 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) From b6fc51e4f0477cf2199b091696df8ec7f7d603d8 Mon Sep 17 00:00:00 2001 From: TatyanaBerlenko Date: Fri, 1 Jul 2022 15:08:33 +0300 Subject: [PATCH 17/17] refactoring --- .../include/custom_line_detector/line_detector.py | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) 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 index 72d48e26..53286462 100644 --- a/packages/custom_line_detector/include/custom_line_detector/line_detector.py +++ b/packages/custom_line_detector/include/custom_line_detector/line_detector.py @@ -88,21 +88,17 @@ def _check_cos(cos_a, cos_b, inv=False): 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) - # print('degrees_a', degrees_a, 'degrees_b', degrees_b) if abs(degrees_a - degrees_b) > DEGREES_DIST: - # print('abs(degrees_a - degrees_b) > DEGREES_E4', abs(cos_a - cos_b) > DEGREES_DIST, 'DEGREES_E', 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: - # print('abs(degrees_a - RIGHT_ANGLE) <= RIGHT_ANGLE_E', abs(degrees_a - RIGHT_ANGLE) <= RIGHT_ANGLE_DIST) if not inv: return 0 return 1 if abs(degrees_b - RIGHT_ANGLE) <= RIGHT_ANGLE_DIST: - # print('abs(degrees_b - RIGHT_ANGLE) <= RIGHT_ANGLE_E', abs(degrees_b - RIGHT_ANGLE) <= RIGHT_ANGLE_DIST) if not inv: return 1 return 0 @@ -118,9 +114,9 @@ def _get_polynomial(): @staticmethod def _get_max_dist_between_elements(y): min_dist_lim = 5 - # pol = LineDetector._get_polynomial() - # result = round(pol(y)) - result = y / 2 + pol = LineDetector._get_polynomial() + result = round(pol(y)) + # result = y / 2 return 0 if result <= min_dist_lim else result @staticmethod