def getmeanpoint(self, data): #print(data) new_image = np.frombuffer(data.data, dtype=np.uint8) new_image = cv2.imdecode(new_image, cv2.IMREAD_COLOR) img = cv2.resize(new_image, (424, 408)) wrap_img = perspective_transform(img, M, img_size=(444, 343)) wrap_img3 = get_tag_mask(wrap_img) binary = abs_sobel_thresh(wrap_img3, orient='y', sobel_kernel=3, thresh=(20, 255)) left_list, right_list, mean_x, mean_y, out_img2 = find_line_fit(binary, midpoint=car_mid_point, margin=100) self.planning_path = Trajectory() self.planning_path_left = Trajectory() self.planning_path_right = Trajectory() if len(mean_y) > 0: mean_x_real, mean_y_real = translation_view(np.asarray(mean_x), np.asarray(mean_y)) left_list_real, left_y_real = translation_view(np.asarray(left_list), np.asarray(mean_y)) right_list_real, right_y_real = translation_view(np.asarray(right_list), np.asarray(mean_y)) # print(mean_x_real, mean_y_real) #print(right_y_real) for i, point in enumerate(mean_y_real): point_xy = Point() point_xy.x = mean_x_real[i] point_xy.y = point self.planning_path.point.append(point_xy) point_xy = Point() point_xy.x = left_list_real[i] point_xy.y = left_y_real[i] self.planning_path_left.point.append(point_xy) point_xy = Point() point_xy.x = right_list_real[i] point_xy.y = right_y_real[i] self.planning_path_right.point.append(point_xy) print("left:", np.asarray(left_list_real).mean(), "right:", np.asarray(right_list_real).mean(), "mean:", np.asarray(mean_x_real).mean())
def __init__(self, node): self.node = node self.speed = 0 self.target_speed = 0 self.lateral_error = 0 self.cmd = Control_Command() self.cmd.steer_angle = 0 self.cmd.throttle = 0 self.trajectory = Trajectory() self.sum_error_longi = 0 self.node.create_reader("/chassis", Chassis, self.chassiscallback) self.node.create_reader("/control_reference", Control_Reference, self.speedrefcallback) # /planning/trajectory\ # /planning/control_trajectory self.node.create_reader("/perception/road_mean_point", Trajectory, self.trajectorycallback) self.writer = self.node.create_writer("/control", Control_Command) signal.signal(signal.SIGINT, self.sigint_handler) signal.signal(signal.SIGHUP, self.sigint_handler) signal.signal(signal.SIGTERM, self.sigint_handler) self.is_sigint_up = False while True: time.sleep(0.05) self.lateral_controller(self.trajectory, self.lateral_error) self.longitude_controller(self.target_speed, self.speed) self.writer.write(self.cmd) if self.is_sigint_up: print("Exit") self.is_sigint_up = False return
def getmeanpoint(self, data): # TODO e new_image = np.frombuffer(data.data, dtype=np.uint8) new_image = cv2.imdecode(new_image, cv2.IMREAD_COLOR) wrap_img = perspective_transform(new_image, M, (580, 560)) yellow_line = apply_yellow_white_mask(wrap_img) # TODO I line_list, mean_x, mean_y = find_line_fit(yellow_line, midpoint=car_mid_point, nwindows=10, margin=100) self.planning_path = Trajectory() #print("point size:",str(len(mean_y))) if len(mean_y) > 0: mean_x_real, mean_y_real = translation_view( np.asarray(mean_x), np.asarray(mean_y)) for i, point in enumerate(mean_y_real): point_xy = Point() point_xy.x = mean_x_real[i] point_xy.y = point self.planning_path.point.append(point_xy)
def getmeanpoint(self, data): # TODO e # TODO e END wrap_img = perspective_transform(new_image, M, (580, 560)) yellow_line = apply_yellow_white_mask(wrap_img) # TODO I # TODO I END self.planning_path = Trajectory() #print("point size:",str(len(mean_y))) if len(mean_y) > 0: mean_x_real, mean_y_real = translation_view( np.asarray(mean_x), np.asarray(mean_y)) for i, point in enumerate(mean_y_real): point_xy = Point() point_xy.x = mean_x_real[i] point_xy.y = point self.planning_path.point.append(point_xy)
def __init__(self, node): self.node = node self.speed = 0 self.target_speed = 0 self.lateral_error = 0 self.cmd = Control_Command() self.trajectory = Trajectory() self.node.create_reader("/chassis", Chassis, self.chassiscallback) self.node.create_reader("/control_reference", Control_Reference, self.speedrefcallback) self.node.create_reader("/planning/dwa_trajectory", Trajectory, self.trajectorycallback) self.writer = self.node.create_writer("/control", Control_Command) signal.signal(signal.SIGINT, self.sigint_handler) signal.signal(signal.SIGHUP, self.sigint_handler) signal.signal(signal.SIGTERM, self.sigint_handler) self.is_sigint_up = False while True: try: time.sleep(0.05) self.lateral_controller(self.trajectory, self.lateral_error) self.longitude_controller(self.target_speed, self.speed) self.writer.write(self.cmd) if self.is_sigint_up: print("Exit") self.is_sigint_up = False return except Exception: break
def __init__(self, node): self.node = node self.start_x = 0 self.start_y = 0 self.goal_x = 0 self.goal_y = 0 self.global_path = Trajectory() self.node.create_reader("/geek/uwb/localization", pos, self.localizationcallback) self.node.create_reader("/planning/mission_point", Point, self.missioncallback) self.writer = self.node.create_writer("/planning/global_trajectory", Trajectory) signal.signal(signal.SIGINT, self.sigint_handler) signal.signal(signal.SIGHUP, self.sigint_handler) signal.signal(signal.SIGTERM, self.sigint_handler) self.is_sigint_up = False while True: time.sleep(0.05) if not cyber.is_shutdown() and self.global_path: self.writer.write(self.global_path) if self.is_sigint_up: print("Exit!") self.is_sigint_up = False sys.exit()
def __init__(self, node): self.node = node self.planning_path = Trajectory() self.planning_path_left = Trajectory() self.planning_path_right = Trajectory() # TODO create reader self.node.create_reader("/realsense/compressed_image", Image, self.callback) # TODO create writer self.writer = self.node.create_writer( "/perception/road_mean_point", Trajectory) self.writer_left = self.node.create_writer( "/perception/road_left_point", Trajectory) self.writer_right = self.node.create_writer( "/perception/road_right_point", Trajectory)
def __init__(self, node): self.node = node self.planning_path = Trajectory() # TODO create reader self.node.create_reader("/perception/get_point", Trajectory, self.callback) # TODO create writer self.writer = self.node.create_writer("/perception/translation_point", Trajectory)
def reshape(self, data): point_array = data.point self.planning_path = Trajectory() for i, point in enumerate(point_array): point_xy = Point() new_x, new_y = translation_view(point.x, point.y) point_xy.x = new_x point_xy.y = new_y self.planning_path.point.append(point_xy)
def plan(self): print(hasattr(self, 'data')) while not cyber.is_shutdown(): print("in plan ********************************************") if not hasattr(self, 'data'): time.sleep(0.2) print("no data sleep firstly") continue sx = self.data.start_point.x # start x position [m] sy = self.data.start_point.y # start y positon [m] gx = self.data.end_point.x # goal x position [m] gy = self.data.end_point.y # goal y position [m] grid_size = 0.051 # potential grid size [m] robot_radius = 0.0725 # robot radius [m] print('start point,{},{} goal point,{}, {}'.format(sx, sy, gx, gy)) ox, oy = [], [] for obstacle in self.data.obs_points: ox.append(obstacle.x) oy.append(obstacle.y) print('obstacle information, ox:{}, oy:{} '.format(ox, oy)) # ox = [-0.03464780002832413] # obstacle x position list [m] # oy = [0.6250196099281311] # obstacle y position list [m] # if show_animation: # plt.grid(True) # plt.axis("equal") # path generation rx, ry = [], [] start = time.time() rx, ry = potential_field_planning( sx, sy, gx, gy, ox, oy, grid_size, robot_radius) end = time.time() print('time cost: {}'.format(end - start)) print('rx,{}, ry.{}'.format(rx, ry)) self.planning_path = Trajectory() if not rx: print("Failed to find a path") else: point = Point() for i, _ in enumerate(rx): point.x = rx[i] point.y = ry[i] self.planning_path.point.append(point) print('trajectory,{}'.format(self.planning_path)) self.write_to_channel()
def missioncallback(self, Point): self.goal_x = int(Point.x) self.goal_y = int(Point.y) pathList = self.start(self.start_x, self.start_y, self.goal_x, self.goal_y) self.planning_path = Trajectory() if not pathList: print("Failed to find a path") else: for path_point in pathList: point_xy.x = path_point[0] point_xy.y = path_point[1] self.planning_path.point.append(point_xy) if not cyber.is_shutdown() and self.planning_path: self.writer.write(self.planning_path)
def callback(self, data): global obslist, planning_path, best_trajectory obslist = [] print(" start!!") # initial state [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)] x = np.array([0.0, 0.0, math.pi / 2.0, 0.3, 0.0]) # goal position [x(m), y(m)] goal = np.array([data.end_point.x, data.end_point.y]) # obstacles [x(m) y(m), ....] for point in data.obs_points: obslist.append([point.x, point.y]) ob = np.matrix(obslist) # input [forward speed, yawrate] u = np.array([0.3, 0.0]) config = Config() best_trajectory = np.array(x) u, best_trajectory = dwa_control(x, u, config, goal, ob) x = motion(x, u, config.dt) self.planning_path = Trajectory() if not best_trajectory.any(): print("Failed to find a path") else: for path_point in best_trajectory: point_xy.x = path_point[0] point_xy.y = path_point[1] self.planning_path.point.append(point_xy) if not cyber.is_shutdown() and self.planning_path: self.writer.write(self.planning_path) print("Done")
def callback(self, data): global obslist, map_w, map_h, map_x_min, map_x_max, map_y_min, map_y_max obslist = [] pStart_x = int(20 * data.start_point.x) pStart_y = int(20 * data.start_point.y) pEnd_x = int(20 * data.end_point.x) pEnd_y = int(20 * data.end_point.y - 1) pStart = Point(pStart_x, pStart_y) pEnd = Point(pEnd_x, pEnd_y) for point in data.obs_points: obslist.append([int(20 * point.x), int(20 * point.y)]) for i in range(0): obslist.append( [random.uniform(2, pEnd.y), random.uniform(2, pEnd.y)]) #time_start = time.time() # 创建AStar对象,并设置起点终点 aStar = AStar(pStart, pEnd) aStar.expansion(offset=9) # 开始寻路 pathList = aStar.start() #time_end = time.time() #print('totally cost', time_end - time_start) self.planning_path = Trajectory() if not pathList: print("Failed to find a path") else: for path_point in pathList: point_xy.x = path_point.x * 0.05 point_xy.y = path_point.y * 0.05 self.planning_path.point.append(point_xy) if not cyber.is_shutdown() and self.planning_path: self.writer.write(self.planning_path)
def callback(self, pos): global obslist, planning_path, best_trajectory start_x = int(pos.x * scale) start_y = int(pos.y * scale) obslist = [] num_point = 0 minr = float("inf") for i, point in enumerate(self.pathList): r = math.sqrt((point[0] - start_x)**2 + (point[1] - start_y)**2) if minr >= r: minr = r num_point = i if (num_point - 8) <= 0: num_point = 8 #规划预瞄点 f_point = self.pathList[num_point - 8] self.goal = f_point print("goal:", self.goal) #将预瞄点坐标从地图坐标系转换到车身坐标系 f_point = [(f_point[0] - start_x) / scale, -((f_point[1] - start_y) / scale)] yaw = math.pi - pos.yaw x_f = f_point[0] * math.cos(yaw) + f_point[1] * math.sin(yaw) y_f = f_point[1] * math.cos(yaw) - f_point[0] * math.sin(yaw) print("trans-goal:", [x_f, y_f]) #将预设障碍物坐标从地图坐标系转换到车身坐标系 ob_point = [643, 353] ob_point = [(ob_point[0] - start_x) / scale, -((ob_point[1] - start_y) / scale)] x_ob = ob_point[0] * math.cos(yaw) + ob_point[1] * math.sin(yaw) y_ob = ob_point[1] * math.cos(yaw) - ob_point[0] * math.sin(yaw) print("ob:", [x_ob, y_ob]) print(" planning start!") # initial state [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)] x = np.array([0.0, 0.0, 0, 0.3, 0.0]) # goal position [x(m), y(m)] goal = np.array([x_f, y_f]) ob = np.matrix(self.obstacleList) ob = np.array([[x_ob, y_ob, 0.24 / scale]]) # input [forward speed, yawrate] u = np.array([0.3, 0.0]) config = Config() best_trajectory = np.array(x) u, best_trajectory = dwa_control(x, u, config, goal, ob) x = motion(x, u, config.dt) #接近终点减速 dist_car_goal = ((((start_x - self.pathList[0][0])**2 + (start_y - self.pathList[0][1])**2)**0.5) / scale) if dist_car_goal <= 0.2: u[0] = 0 self.planning_path = Trajectory() self.speed = Control_Reference() self.speed.vehicle_speed = u[0] if not best_trajectory.any(): print("Failed to find a path") else: for path_point in best_trajectory: point_xy.x = path_point[0] point_xy.y = path_point[1] self.planning_path.point.append(point_xy) point_xy.x = self.goal[0] point_xy.y = self.goal[1] self.planning_path.point.append(point_xy) if not cyber.is_shutdown() and self.planning_path: self.writer.write(self.planning_path) self.vwriter.write(self.speed) print("planning done")
def getmeanpoint(self, data): new_image = np.frombuffer(data.data, dtype=np.uint8) new_image = cv2.imdecode(new_image, cv2.IMREAD_COLOR) img = cv2.resize(new_image, (424, 408)) wrap_img = perspective_transform(img, M, img_size=(444, 343)) gray_d = cv2.cvtColor(wrap_img, cv2.COLOR_BGR2GRAY) wrap_img_2 = cv2.threshold(gray_d, 100, 220, cv2.THRESH_BINARY_INV)[1] # TODO e begin kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3)) img_d = cv2.erode(wrap_img_2, kernel, iterations=2) wrap_img_2 = cv2.dilate(img_d, kernel, iterations=3) # TODO e end cv2.fillPoly(img_d, mask_right_cor, 0) # TODO e begin image, contours, hierarchy = cv2.findContours( wrap_img_2, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_TC89_KCOS) # TODO e end # TODO f begin wrap_img3 = findMaxContour(wrap_img, wrap_img_2, contours) # TODO f end # TODO g begin binary = abs_sobel_thresh(wrap_img3, orient='x', sobel_kernel=3, thresh=(20, 255)) left_list, right_list, mean_x, mean_y, out_img2 = find_line_fit( binary, midpoint=car_mid_point, margin=100) # TODO g end self.planning_path = Trajectory() self.planning_path_left = Trajectory() self.planning_path_right = Trajectory() if len(mean_y) > 0: mean_x_real, mean_y_real = translation_view( np.asarray(mean_x), np.asarray(mean_y)) left_list_real, left_y_real = translation_view( np.asarray(left_list), np.asarray(mean_y)) right_list_real, right_y_real = translation_view( np.asarray(right_list), np.asarray(mean_y)) for i, point in enumerate(mean_y_real): point_xy = Point() point_xy.x = mean_x_real[i] point_xy.y = point self.planning_path.point.append(point_xy) point_xy = Point() point_xy.x = left_list_real[i] point_xy.y = left_y_real[i] self.planning_path_left.point.append(point_xy) point_xy = Point() point_xy.x = right_list_real[i] point_xy.y = right_y_real[i] self.planning_path_right.point.append(point_xy) print("left:", np.asarray(left_list_real).mean(), "right:", np.asarray(right_list_real).mean(), "mean:", np.asarray(mean_x_real).mean())