def __init__(self): #node_name = "Lane Filter Tester" pub_fake_segment_list = rospy.Publisher("~segment_list", SegmentList, queue_size=1) rospy.sleep(1) seg = Segment() seg.points[0].x = rospy.get_param("~x1") seg.points[0].y = rospy.get_param("~y1") seg.points[1].x = rospy.get_param("~x2") seg.points[1].y = rospy.get_param("~y2") color = rospy.get_param("~color") if color == "white": seg.color = seg.WHITE elif color == "yellow": seg.color = seg.YELLOW elif color == "red": seg.color = seg.RED else: print("error no color specified") seg_list = SegmentList() seg_list.segments.append(seg) pub_fake_segment_list.publish(seg_list)
def toSegmentMsg(self, lines, normals, color): arr_cutoff = np.array((0, self.top_cutoff, 0, self.top_cutoff)) arr_ratio = np.array((1./self.image_size[1], 1./self.image_size[0], 1./self.image_size[1], 1./self.image_size[0])) segmentMsgList = [] for x1,y1,x2,y2,norm_x,norm_y in np.hstack((lines,normals)): ox= int((x1+x2)/2) oy= int((y1+y2)/2) cx = (ox+3*norm_x).astype('int') cy = (oy+3*norm_y).astype('int') ccx = (ox-3*norm_x).astype('int') ccy = (oy-3*norm_y).astype('int') if cx >158: cx = 158 elif cx <1: cx = 1 if ccx >158: ccx = 158 elif ccx <1: ccx = 1 if cy >=79: cy = 79 elif cy <1: cy = 1 if ccy >=79: ccy = 79 elif ccy <1: ccy = 1 if (self.blue[cy, cx] == 255 and self.yellow[ccy,ccx] ==255) or (self.yellow[cy, cx] == 255 and self.blue[ccy,ccx] ==255): [x1,y1,x2,y2] = (([x1,y1,x2,y2] + arr_cutoff) * arr_ratio) 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 segmentMsgList.append(segment) if self.time_switch == True: msgg = BoolStamped() msgg.data = True self.pub_lane.publish(msgg) self.time_switch = False self.count = 0 return segmentMsgList
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 lineseglist_cb(self, seglist_msg): """ Projects a list of line segments on the ground reference frame point by point by calling :py:meth:`pixel_msg_to_ground_msg`. Then publishes the projected list of segments. Args: seglist_msg (:obj:`duckietown_msgs.msg.SegmentList`): Line segments in pixel space from unrectified images """ if self.camera_info_received: seglist_out = SegmentList() seglist_out.header = seglist_msg.header for received_segment in seglist_msg.segments: new_segment = Segment() new_segment.points[0] = self.pixel_msg_to_ground_msg( received_segment.pixels_normalized[0]) new_segment.points[1] = self.pixel_msg_to_ground_msg( received_segment.pixels_normalized[1]) new_segment.color = received_segment.color # TODO what about normal and points seglist_out.segments.append(new_segment) self.pub_lineseglist.publish(seglist_out) if not self.first_processing_done: self.log("First projected segments published.") self.first_processing_done = True if self.pub_debug_img.get_num_connections() > 0: debug_image_msg = self.bridge.cv2_to_compressed_imgmsg( self.debug_image(seglist_out)) debug_image_msg.header = seglist_out.header self.pub_debug_img.publish(debug_image_msg) else: self.log("Waiting for a CameraInfo message", "warn")
def lineseglist_cb(self,seglist_msg): seglist_out = SegmentList() for received_segment in seglist_msg.segments: new_segment = Segment() new_segment.points[0] = self.gp.vector2ground(received_segment.pixels_normalized[0]) new_segment.points[1] = self.gp.vector2ground(received_segment.pixels_normalized[1]) new_segment.color = received_segment.color # TODO what about normal and points seglist_out.segments.append(new_segment) self.pub_lineseglist_.publish(seglist_out)
def fuzzy_segment(s1, intensity): s2 = Segment() def noise(): return np.random.randn() * intensity for i in range(2): s2.pixels_normalized[i].x = s1.pixels_normalized[i].x + noise() s2.pixels_normalized[i].y = s1.pixels_normalized[i].y + noise() s2.color = s1.color return s2
def toSegmentMsg(lines, normals, color): segmentMsgList = [] 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 segmentMsgList.append(segment) return segmentMsgList
def toSegmentMsg(self, lines, normals, color): segmentMsgList = [] 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 segmentMsgList.append(segment) return segmentMsgList
def storeSegments(self,in_segs): rec_seg_list = SegmentList() rec_seg_list.header = in_segs.header for in_seg in in_segs.segments: new_seg = Segment() new_seg.points[0].x = in_seg.points[0].x new_seg.points[0].y = in_seg.points[0].y new_seg.points[0].z = 0.0 new_seg.points[1].x = in_seg.points[1].x new_seg.points[1].y = in_seg.points[1].y new_seg.points[1].z = 0.0 new_seg.color = in_seg.color rec_seg_list.segments.append(new_seg) self.seg_list.append(rec_seg_list) self.pubMap()
def pointArrays2Marker(self, *pointArrays): marker = Marker() marker.header.frame_id = self.veh_name marker.ns = self.veh_name + "/line_seg" marker.id = 0 marker.action = Marker.ADD marker.lifetime = rospy.Duration.from_sec(5.0) marker.type = Marker.LINE_LIST marker.pose.orientation.w = 1.0 marker.scale.x = 0.02 for pointArray in pointArrays: arrayShape = np.shape(pointArray) print(pointArray) for lv1 in range(arrayShape[1] - 1): seg = Segment() seg.points[0].x = pointArray[lv1][0] seg.points[0].y = pointArray[lv1][1] seg.points[1].x = pointArray[lv1 + 1][0] seg.points[1].y = pointArray[lv1 + 1][1] marker.points.append(seg.points[0]) marker.points.append(seg.points[1]) color = ColorRGBA(r=1.0, g=0.0, b=0.0, a=1.0) marker.colors.append(color) marker.colors.append(color) return marker
def visualizeCurves(self, *pointArrays): marker_array = MarkerArray() marker = Marker() marker.header.frame_id = self.veh_name marker.ns = self.veh_name + "/line_seg" marker.id = 0 marker.action = Marker.ADD marker.lifetime = rospy.Duration.from_sec(5.0) marker.type = Marker.LINE_LIST marker.pose.orientation.w = 1.0 marker.scale.x = 0.02 for pointArray in pointArrays: for lv1 in range(len(pointArray) - 1): seg = Segment() seg.points[0].x = pointArray[lv1][0] seg.points[0].y = pointArray[lv1][1] seg.points[1].x = pointArray[lv1 + 1][0] seg.points[1].y = pointArray[lv1 + 1][1] marker.points.append(seg.points[0]) marker.points.append(seg.points[1]) color = ColorRGBA(r=1.0, g=0.0, b=0.0, a=1.0) marker.colors.append(color) marker.colors.append(color) marker_array.markers.append(marker) self.pub_lane.publish(marker_array)
def find_ground_coordinates( gpg: GroundProjectionGeometry, sl: SegmentList, skip_not_on_ground: bool = True ) -> SegmentList: """ Creates a new segment list with the ground coordinates set. """ cutoff = 0.01 sl2 = SegmentList() sl2.header = sl.header # Get ground truth of segmentList for s1 in sl.segments: g0 = gpg.vector2ground(s1.pixels_normalized[0]) g1 = gpg.vector2ground(s1.pixels_normalized[1]) if skip_not_on_ground: if g0.x < cutoff or g1.x < cutoff: continue points = [g0, g1] pixels_normalized = [s1.pixels_normalized[0], s1.pixels_normalized[1]] color = s1.color s2 = Segment(points=points, pixels_normalized=pixels_normalized, color=color) # TODO what about normal and points sl2.segments.append(s2) return sl2
def toSegmentMsg(self, lines, normals, color): segmentMsgList = [] #segmentMsgList = [Segment() for i in range(lines.shape[0])] #k = 0 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 segmentMsgList.append(segment) #segmentMsgList[k] = segment #k += 1 return segmentMsgList
def toSegmentMsg(self, lines, normals, color, wid, hei): segmentMsgList = [] for u1,v1,u2,v2,norm_u,norm_v in np.hstack((lines,normals)): segment = Segment() segment.color = color segment.pixels[0].u = int(u1) segment.pixels[0].v = int(v1) segment.pixels[1].u = int(u2) segment.pixels[1].v = int(v2) segment.pixels_normalized[0].x = u1/wid segment.pixels_normalized[0].y = v1/hei segment.pixels_normalized[1].x = u2/wid segment.pixels_normalized[1].y = v2/hei segment.normal.x = norm_u segment.normal.y = norm_v segmentMsgList.append(segment) return segmentMsgList
def toSegmentMsg(self, lines, normals, centers, color): segmentMsgList = [] 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 #if self.bw_2[y4,x4]>0 : # 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 segmentMsgList.append(segment) return segmentMsgList
def lineseglist_cb(self, seglist_msg): message_time = seglist_msg.header.stamp.sec + seglist_msg.header.stamp.nanosec*1e-9 current_time = time.time() delay = current_time - message_time #fname = "/dev/shm/segment" + str(message_time) #seglist_msg.segments = pickle.load(open(fname, "rb")) #os.unlink(fname) #print("message time: " + str(message_time)) #print("current time: " + str(current_time)) #print("delay: " + str(delay), "# segments: ", len(seglist_msg.segments)) seglist_out = SegmentList() seglist_out.header = seglist_msg.header for received_segment in seglist_msg.segments: new_segment = Segment() new_segment.points[0] = self.gp.vector2ground(received_segment.pixels_normalized[0]) new_segment.points[1] = self.gp.vector2ground(received_segment.pixels_normalized[1]) new_segment.color = received_segment.color # TODO what about normal and points seglist_out.segments.append(new_segment) self.pub_lineseglist_.publish(seglist_out)
def predict_segments(sm, gpg): """ Predicts what segments the robot would see. Assumes map is in FRAME_AXLE. """ x_frustum = +0.1 fov = np.deg2rad(150) res = [] for segment in sm.segments: p1 = segment.points[0] p2 = segment.points[1] # If we are both in FRAME_AXLE if ((sm.points[p1].id_frame != FRAME_AXLE) or (sm.points[p2].id_frame != FRAME_AXLE)): msg = "Cannot deal with points not in frame FRAME_AXLE" raise NotImplementedError(msg) w1 = sm.points[p1].coords w2 = sm.points[p2].coords coords_inside = clip_to_view([w1, w2], x_frustum, fov) if not coords_inside: continue w1 = coords_inside[0] w2 = coords_inside[1] point1 = Point(w1[0], w1[1], w1[2]) point2 = Point(w2[0], w2[1], w2[2]) pixel1 = gpg.ground2pixel(point1) pixel2 = gpg.ground2pixel(point2) normalized1 = gpg.pixel2vector(pixel1) normalized2 = gpg.pixel2vector(pixel2) pixels_normalized = [normalized1, normalized2] normal = Vector2D(0, 0) points = [point1, point2] # color = segment_color_constant_from_string(segment.color) assert segment.color in [Segment.RED, Segment.YELLOW, Segment.WHITE] s = Segment(pixels_normalized=pixels_normalized, normal=normal, points=points, color=segment.color) res.append(s) return SegmentList(segments=res)
def rectify_segment(rectify: Rectify, gpg: GroundProjectionGeometry, s1: Segment) -> Segment: pixels_normalized = [] for i in (0, 1): # normalized coordinates nc = s1.pixels_normalized[i] # get pixel coordinates pixels = gpg.vector2pixel(nc) # rectify pr = rectify.rectify_point(pixels) # recompute normalized coordinates # t = Pixel(pr[0], pr[1]) v = gpg.pixel2vector(ImageSpaceResdepPoint(pr)) pixels_normalized.append(v) s2 = Segment(color=s1.color, pixels_normalized=pixels_normalized) return s2
def rectify_segment(gpg, s1): pixels_normalized = [] for i in (0, 1): # normalized coordinates nc = s1.pixels_normalized[i] # get pixel coordinates pixels = gpg.vector2pixel(nc) uv = (pixels.u, pixels.v) # rectify pr = gpg.rectify_point(uv) # recompute normalized coordinates t = Pixel(pr[0], pr[1]) v = gpg.pixel2vector(t) pixels_normalized.append(v) s2 = Segment(color=s1.color, pixels_normalized=pixels_normalized) return s2
def toSegmentMsg(self, lines, normals, color): arr_cutoff = np.array((0, self.top_cutoff, 0, self.top_cutoff)) arr_ratio = np.array( (1. / self.image_size[1], 1. / self.image_size[0], 1. / self.image_size[1], 1. / self.image_size[0])) segmentMsgList = [] for x1, y1, x2, y2, norm_x, norm_y in np.hstack((lines, normals)): ox = int((x1 + x2) / 2) oy = int((y1 + y2) / 2) cx = (ox + 3 * norm_x).astype('int') cy = (oy + 3 * norm_y).astype('int') ccx = (ox - 3 * norm_x).astype('int') ccy = (oy - 3 * norm_y).astype('int') if cx > 158: cx = 158 elif cx < 1: cx = 1 if ccx > 158: ccx = 158 elif ccx < 1: ccx = 1 if cy >= 79: cy = 79 elif cy < 1: cy = 1 if ccy >= 79: ccy = 79 elif ccy < 1: ccy = 1 if (self.blue[cy, cx] == 255 and self.yellow[ccy, ccx] == 255) or ( self.yellow[cy, cx] == 255 and self.blue[ccy, ccx] == 255): [x1, y1, x2, y2] = (([x1, y1, x2, y2] + arr_cutoff) * arr_ratio) 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 segmentMsgList.append(segment) if self.time_switch == True: msgg = BoolStamped() msgg.data = True self.pub_lane.publish(msgg) self.time_switch = False self.count = 0 else: self.count += 1 if self.count > 5: print "****************count = ", self.count, " *******************" if self.time_switch == False: msgg = BoolStamped() msgg.data = False self.pub_lane.publish(msgg) self.time_switch = True car_control_msg = Twist2DStamped() car_control_msg.v = 0.0 car_control_msg.omega = 0.0 self.pub_car_cmd.publish(car_control_msg) return segmentMsgList
def lines_cb(self, image): img = self.bridge.compressed_imgmsg_to_cv2(image, "bgr8") imgCropped = img[240:480, 0:640] hsvimg = cv.cvtColor(imgCropped, cv.COLOR_BGR2HSV) lower = np.array([23, 157, 118]) upper = np.array([179, 255, 255]) white_lower = np.array([42, 8, 88]) white_upper = np.array([122, 99, 255]) arr_cutoff = np.array([0, 40, 0, 40]) arr_ratio = np.array([1/160, 1/120, 1/160, 1/120]) a = [] b = [] c = SegmentList() maskWhite = cv.inRange(hsvimg, white_lower, white_upper) maskYellow = cv.inRange(hsvimg, lower, upper) imgWhite = cv.bitwise_and(imgCropped, imgCropped, mask=maskWhite) imgYellow = cv.bitwise_and(imgCropped, imgCropped, mask=maskYellow) imgCanny_white = cv.Canny(imgWhite, 150, 200) imgCanny_yellow = cv.Canny(imgYellow, 150, 200) imgResized_white = cv.resize(imgCanny_white, (160, 120), interpolation=cv.INTER_NEAREST) imgResized_yellow = cv.resize(imgCanny_yellow, (160, 120), interpolation=cv.INTER_NEAREST) imgCut_white = imgResized_white[40:120, 0:160] imgCut_yellow = imgResized_yellow[40:120, 0:160] lines_white = cv.HoughLinesP(imgCut_white, 1, (3.14159 / 180), 1) lines_yellow = cv.HoughLinesP(imgCut_yellow, 1, (3.14159 / 180), 1) #rospy.logwarn(maskYellow) for i in range(len(lines_white)): lines_normalized_white = (lines_white[i] + arr_cutoff) * arr_ratio a.append(lines_normalized_white) for i in range(len(lines_yellow)): lines_normalized_yellow = (lines_yellow[i] + arr_cutoff) * arr_ratio b.append(lines_normalized_yellow) segments = SegmentList() white_seg = Segment() yellow_seg = Segment() for i in range(len(a)): lines_a = a[i][0] white_seg.color = 0 white_seg.pixels_normalized[0].x = lines_a[0] white_seg.pixels_normalized[0].y = lines_a[1] white_seg.pixels_normalized[1].x = lines_a[2] white_seg.pixels_normalized[1].y = lines_a[3] segments.segments.append(white_seg) for i in range(len(b)): lines_b = b[i][0] yellow_seg.color = 1 yellow_seg.pixels_normalized[0].x = lines_b[0] yellow_seg.pixels_normalized[0].y = lines_b[1] yellow_seg.pixels_normalized[1].x = lines_b[2] yellow_seg.pixels_normalized[1].y = lines_b[3] #yellow_seg.append([yellow_seg.pixels_normalized[0].x,yellow_seg.pixels_normalized[0].y,yellow_seg.pixels_normalized[1].x,yellow_seg.pixels_normalized[1].y]) segments.segments.append(yellow_seg) #rospy.logwarn(lines_yellow) self.pub.publish(segments)
def callback_lab5(self, msg): # convert to a ROS image using the bridge cv_img = self.bridge.compressed_imgmsg_to_cv2(msg, "bgr8") # crop the image using the method provided in lab5 pdf image_size = (160, 120) offset = 40 new_image = cv2.resize(cv_img, image_size, interpolation=cv2.INTER_NEAREST) cv_cropped = new_image[offset:, :] # convert colorspace from BGR to HSV hsv_img = cv2.cvtColor(cv_cropped, cv2.COLOR_BGR2HSV) # filter white lane white_filter = cv2.inRange(hsv_img, (0, 0, 225), (180, 40, 255)) # filter yellow lane yellow_filter = cv2.inRange(hsv_img, (25, 80, 120), (75, 255, 255)) # convert the cropped image to gray grey_img = cv2.cvtColor(cv_cropped, cv2.COLOR_BGR2GRAY) # perform canny edge detection edges = cv2.Canny(grey_img, 0, 255) # need to dilate incoming white and yellow lane images before doing bitwise and # implement the dilate function privided in lecture slide kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (7, 7)) # also create a unique kernel to dilate filtered edges kernel_for_edges = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (1, 1)) # dilate white and yellow filtered images dilated_white = cv2.dilate(white_filter, kernel) dilated_yellow = cv2.dilate(yellow_filter, kernel) # dilate the edges to account for lines that are too thin dilated_edges = cv2.dilate(edges, kernel_for_edges) # perform bitwise and for the white and yellow lines and_white = cv2.bitwise_and(dilated_edges, dilated_white, mask=None) and_yellow = cv2.bitwise_and(dilated_edges, dilated_yellow, mask=None) # apply Probabilistic Hough Transform on both white and yellow images' lines # white lines lines_white = cv2.HoughLinesP(and_white, 1, np.pi / 180, 5, None, 10, 5) # yellow lines lines_yellow = cv2.HoughLinesP(and_yellow, 1, np.pi / 180, 5, None, 2, 2) # draw blue line on the two images using function provided in homework9 pdf image_lines_white = self.output_lines(cv_cropped, lines_white) # merge the white and yellow line images by sending image_lines_white as argument to output_lines image_lines_combined = self.output_lines(image_lines_white, lines_yellow) # prepare imgmsg for the combined white/yellow lane detection ros_combined_output = self.bridge.cv2_to_imgmsg( image_lines_combined, "bgr8") # publish the combined result of white/yellow lane detector image self.pub_combined_img.publish(ros_combined_output) # values for normalization as provided in lab5 PDF arr_cutoff = np.array([0, offset, 0, offset]) arr_ratio = np.array([ 1. / image_size[1], 1. / image_size[0], 1. / image_size[1], 1. / image_size[0] ]) # stores the list of Segments and will be published to /segment_list resultList = SegmentList() #check to make sure white line is detected if (lines_white is not None): # normalize the list of white lines line_normalized_white = (lines_white + arr_cutoff) * arr_ratio w_list = [] #iterate through each white line for i in range(len(line_normalized_white)): #create a single segment to store values w_seg = Segment() w_seg.color = Segment.WHITE #store values x0, y0, x1, y1 to this segment w_seg.pixels_normalized[0].x = line_normalized_white[i][0][0] w_seg.pixels_normalized[0].y = line_normalized_white[i][0][1] w_seg.pixels_normalized[1].x = line_normalized_white[i][0][2] w_seg.pixels_normalized[1].y = line_normalized_white[i][0][3] # add the segment to list w_list.append(w_seg) # add the resulting list to the resulting List resultList.segments.extend(w_list) #check to make sure yellow line is detected if (lines_yellow is not None): # normalize the list of yellow lines line_normalized_yellow = (lines_yellow + arr_cutoff) * arr_ratio y_list = [] #iterate through each yellow line for i in range(len(line_normalized_yellow)): #create a single segment to store values y_seg = Segment() y_seg.color = Segment.YELLOW #store values x0, y0, x1, y1 to this segment y_seg.pixels_normalized[0].x = line_normalized_yellow[i][0][0] y_seg.pixels_normalized[0].y = line_normalized_yellow[i][0][1] y_seg.pixels_normalized[1].x = line_normalized_yellow[i][0][2] y_seg.pixels_normalized[1].y = line_normalized_yellow[i][0][3] # add this segment to the list of yellow segments y_list.append(y_seg) # finally, add the list to the resulting List resultList.segments.extend(y_list) # publish the resulting list of segments to a topic received by ground_detection_node self.pub_seglist.publish(resultList)
def toSegmentMsg(self, lines, normals, color): qtable = zeros(7,15,3) round1 = 0 segmentMsgList = [] for x1,y1,x2,y2,norm_x,norm_y in np.hstack((lines,normals)): segment = Segment() segment.color = color map_seg = zeros(7,15) for i in range(round(x1*15),round(x2*15)) map_seg(i,round(i*(y2-y1)/(x2-x1)))=1 t_pre = map_seg(6,1) for i in 15 t = map_seg(6,i) if (t < t_pre) s = i elif (t > t_pre) e = i t_pre = t gaol_x = 6 gaol_y = (s+e)/2 while round1<50 map_matrix = map_seg round1 = round1+1 position_x = 2 position_y = 8 count=0 while ~(position_x == gaol_x && position_y == gaol_y) a=0.9 b=0.8 t_pre = map_seg(position_x,1) for i in 15 t = map_seg(position_x,i) if (t < t_pre) s = i elif (t > t_pre) e = i t_pre = t dis = (position_y-s) - (e-position_y) if (dis <= 0) reward = 5*dis elif (dis > 0) reward = -5*dis count = count+1 rand_action = round( random.randint(1,3) ) max_q = max([qtable(position_x,position_y,1) qtable(position_x,position_y,2) qtable(position_x,position_y,3) ]) max_index = values.index(max([qtable(position_x,position_y,1) qtable(position_x,position_y,2) qtable(position_x,position_y,3) ])) if( qtable(position_x,position_y,rand_action) >= qtable(position_x,position_y,max_index) ) action = rand_action else action = max_index map_matrix(position_x,position_y) = count pre_position_x = position_x pre_position_y = position_y if action ==1 position_x = pre_position_x+1 elif action ==2 position_y = pre_position_y-1 elif action ==3 position_y = pre_position_y+1 if(map_seg(position_x,position_y) == 1) position_x = pre_position_x position_y = pre_position_y reward=-100 b=0 if(position_x == gaol_x && position_y == gaol_y) reward=100 b=0 max_qtable = max([qtable(position_x,position_y,1) qtable(position_x,position_y,2) qtable(position_x,position_y,3) ]) max_qtable_index = values.index(max([qtable(position_x,position_y,1) qtable(position_x,position_y,2) qtable(position_x,position_y,3) ])) old_q=qtable(pre_position_x,pre_position_y,action) new_q=old_q+a*(reward+b*max_qtable-old_q) qtable(pre_position_x,pre_position_y,action)=new_q for i in range(1,15) for j in range(1,7) if map_matrix(i,j)!=0 x1=i/15. y1=j/7. break for i in range(15,1)[::-1] for j in range(7,1)[::-1] if map_matrix(i,j)!=0 x2=i/15. y2=j/7. break 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 segmentMsgList.append(segment) return segmentMsgList