def ToPathPlanner2(efficients): # show sampling path generate_lattice(efficients) if showVehicleStart: tmpx, tmpy, tmptheta = ftc.frenetToXY(start_SRho[0], start_SRho[1], obstacleHeading, efficients) car.simVehicle([tmpx, tmpy], tmptheta, 'b', 1)
def ToPathPlanner(efficients): # show sampling path # show sampling path s0 = 400 offset = 0.3 refLineRho = lane_width * 0.5 laneChaneRefLine = lane_width * 1.5 refline = refLineRho start_SRho, static_obs=parameters(s0, offset, refline) generate_lattice(efficients, refLineRho, start_SRho) generate_lattice(efficients, laneChaneRefLine, start_SRho) if showVehicleStart: tmpx, tmpy, tmptheta = ftc.frenetToXY(start_SRho[0], start_SRho[1], obstacleHeading, efficients) car.simVehicle([tmpx, tmpy], tmptheta, 'b', 1) if show_obstacle: c = ['lightseagreen', 'orange', 'green'] for i in range(len(static_obs)): tmpx, tmpy, tmptheta = ftc.frenetToXY(static_obs[i][0], static_obs[i][1], obstacleHeading, efficients) car.simVehicle([tmpx, tmpy], tmptheta, c[i], 1) s0 = 480 offset = 0.0 refline = laneChaneRefLine start_SRho, static_obs=parameters(s0, offset, refline) generate_lattice(efficients, refLineRho, start_SRho) generate_lattice(efficients, laneChaneRefLine, start_SRho) return None
def closed_loop_prediction(cx, cy, cyaw, ck, speed_profile, goal): T = 500.0 # max simulation time goal_dis = 0.3 stop_speed = 0.05 state = State(x=cx[0], y=cy[0], yaw=cyaw[0], v=0.0) time = 0.0 x = [state.x] y = [state.y] yaw = [state.yaw] v = [state.v] t = [0.0] target_ind = calc_nearest_index(state, cx, cy, cyaw) e, e_th = 0.0, 0.0 while T >= time: dl, target_ind, e, e_th, ai = lqr_steering_control( state, cx, cy, cyaw, ck, e, e_th, speed_profile) state = update(state, ai, dl) if abs(state.v) <= stop_speed: target_ind += 1 time = time + dt # check goal dx = state.x - goal[0] dy = state.y - goal[1] if math.sqrt(dx**2 + dy**2) <= goal_dis: print("Goal") break x.append(state.x) y.append(state.y) yaw.append(state.yaw) v.append(state.v) t.append(time) if target_ind % 1 == 0 and show_animation: plt.cla() plt.plot(cx, cy, "-r", label="course") plt.plot(x, y, c="b", ls='-', label="trajectory") simModel.simVehicle(center_point=[x[-1], y[-1]], heading=yaw[-1], color='b', timestamp=1) # plt.plot(cx[target_ind], cy[target_ind], "xg", label="target") plt.axis("equal") plt.grid(True) # plt.xlim(-10, 100) # plt.ylim(-10, 100) plt.title("speed[km/h]:" + str(round(state.v * 3.6, 2)) + ",target index:" + str(target_ind)) plt.pause(0.0001) return t, x, y, yaw, v
def plotGraph(): # plt.style.use('ggplot') plt.figure(figsize=(3.5, 3.5 * 0.62)) # 单位英寸, 3.5 plt.axes([0.2, 0.2, 0.7, 0.7]) plt.axis("equal") plt.grid(linestyle="--", linewidth=0.5, alpha=1) # plt.rcParams['font.sans-serif'] = ['Times New Roman'] # 如果要显示中文字体,则在此处设为:SimHei # plt.rcParams['axes.unicode_minus'] = False # 显示负号 # # show multilane # multiLane.curvePath() # # 计算多车道环境的弧长参数曲线的系数 # efficients = multiLane.saveEfficients() # 计算回环环境的弧长参数曲线的系数 efficients = cubicSpline.saveEfficients() # show sampling path generate_lattice(efficients) if show_obstacle: c = ['r', 'r', 'r', 'darkorange'] for i in range(len(static_obs)): tmpx, tmpy, tmptheta = ftc.frenetToXY(static_obs[i][0], static_obs[i][1], obstacleHeading, efficients) car.simVehicle([tmpx, tmpy], tmptheta, c[i], 1) if showVehicleStart: tmpx, tmpy, tmptheta = ftc.frenetToXY(start_SRho[0], start_SRho[1], obstacleHeading, efficients) car.simVehicle([tmpx, tmpy], tmptheta, 'b', 1) font1 = {'family': 'Times New Roman', 'weight': 'normal', 'size': 10, } plt.xlabel("x (m)", font1) plt.ylabel('y (m)', font1) # 设置坐标刻度值的大小以及刻度值的字体 plt.tick_params(labelsize=10) # x = np.array([0.0, 10.0, 20.0, 30.0, 40.0, 50.0]) # x = np.array([0.0, 20.0, 40.0, 60.0, 80.0, 100.0]) # y = np.array([-2.0, -1, 0.0, 1.0, 2.0]) # y = np.array([-2.0, -1, 0.0, 1.0, 2.0]) # xgroup_labels = ['0.0', '20.0', '40.0', '60.0', '80.0', '100.0'] # x轴刻度的标识 # ygroup_labels = ['-2.0', '-1.0', '0.0', '1.0', '2.0'] # y轴刻度的标识 # plt.xticks(x, xgroup_labels, fontproperties='Times New Roman', fontsize=10) # 默认字体大小为10 # plt.yticks(y, ygroup_labels, fontproperties='Times New Roman', fontsize=10) plt.xticks(fontproperties='Times New Roman', fontsize=10) plt.yticks(fontproperties='Times New Roman', fontsize=10) plt.xlim(120, 200) plt.ylim(90, 140) plt.savefig('../SimGraph/pathLatticeXY4_061202.tiff', dpi=600) plt.show()
def plotGraph(): # plt.rcParams['font.sans-serif'] = ['Times New Roman'] # 如果要显示中文字体,则在此处设为:SimHei # plt.rcParams['axes.unicode_minus'] = False # 显示负号 # # show multilane # multiLane.curvePath() # # 计算多车道环境的弧长参数曲线的系数 # efficients = multiLane.saveEfficients() # 计算回环环境的弧长参数曲线的系数 efficients = cubicSpline.saveEfficients() # show sampling path generate_lattice(efficients) if show_obstacle: c = ['r', 'gold', 'darkorange'] for i in range(len(static_obs)): tmpx, tmpy, tmptheta = ftc.frenetToXY(static_obs[-(i + 1)][0], static_obs[-(i + 1)][1], obstacleHeading, efficients) car.simVehicle([tmpx, tmpy], tmptheta, c[i], 0.8) if showVehicleStart: tmpx, tmpy, tmptheta = ftc.frenetToXY(start_SRho[0], start_SRho[1], obstacleHeading, efficients) car.simVehicle([tmpx, tmpy], tmptheta, 'b', 0.8) font1 = { 'family': 'Times New Roman', 'weight': 'normal', 'size': 10, } plt.xlabel("x (m)", font1) plt.ylabel('y (m)', font1) # 设置坐标刻度值的大小以及刻度值的字体 plt.tick_params(labelsize=10) # x = np.array([0.0, 10.0, 20.0, 30.0, 40.0, 50.0]) # x = np.array([0.0, 20.0, 40.0, 60.0, 80.0, 100.0]) # y = np.array([-2.0, -1, 0.0, 1.0, 2.0]) # y = np.array([-2.0, -1, 0.0, 1.0, 2.0]) # xgroup_labels = ['0.0', '20.0', '40.0', '60.0', '80.0', '100.0'] # x轴刻度的标识 # ygroup_labels = ['-2.0', '-1.0', '0.0', '1.0', '2.0'] # y轴刻度的标识 # plt.xticks(x, xgroup_labels, fontproperties='Times New Roman', fontsize=10) # 默认字体大小为10 # plt.yticks(y, ygroup_labels, fontproperties='Times New Roman', fontsize=10) plt.xticks(fontproperties='Times New Roman', fontsize=10) plt.yticks(fontproperties='Times New Roman', fontsize=10) plt.xlim(30, 140) plt.ylim(80, 130) plt.savefig('../SimGraph/pathLatticeXY3_053002.tiff', dpi=600)
def plotGraph(): efficients = cubicSpline.saveEfficients() # show sampling path generate_lattice(efficients) if show_obstacle: for i in range(len(obstacle)): tmpx, tmpy, tmptheta = ftc.frenetToXY(obstacle[i][0], obstacle[i][1], obstacleHeading, efficients) car.simVehicle([tmpx, tmpy], tmptheta, 'r', 1) if showVehicleStart: tmpx, tmpy, tmptheta = ftc.frenetToXY(start_SRho[0], start_SRho[1], obstacleHeading, efficients) car.simVehicle([tmpx, tmpy], tmptheta, 'b', 1) font1 = {'family': 'Times New Roman', 'weight': 'normal', 'size': 10, } plt.xlabel("x (m)", font1) plt.ylabel('y (m)', font1) # 设置坐标刻度值的大小以及刻度值的字体 plt.tick_params(labelsize=10) # x = np.array([0.0, 10.0, 20.0, 30.0, 40.0, 50.0]) # x = np.array([0.0, 20.0, 40.0, 60.0, 80.0, 100.0]) # y = np.array([-2.0, -1, 0.0, 1.0, 2.0]) # y = np.array([-2.0, -1, 0.0, 1.0, 2.0]) # xgroup_labels = ['0.0', '20.0', '40.0', '60.0', '80.0', '100.0'] # x轴刻度的标识 # ygroup_labels = ['-2.0', '-1.0', '0.0', '1.0', '2.0'] # y轴刻度的标识 # plt.xticks(x, xgroup_labels, fontproperties='Times New Roman', fontsize=10) # 默认字体大小为10 # plt.yticks(y, ygroup_labels, fontproperties='Times New Roman', fontsize=10) plt.xticks(fontproperties='Times New Roman', fontsize=10) plt.yticks(fontproperties='Times New Roman', fontsize=10) plt.xlim(-10, 30) plt.ylim(-5, 80)
def plotGraph(): # plot graph plt.figure(figsize=(3.5, 3.5 * 0.618)) # 单位英寸, 3.5 font1 = {'family': 'Times New Roman', 'weight': 'normal', 'size': 10} plt.rcParams['font.sans-serif'] = ['Times New Roman'] # 如果要显示中文字体,则在此处设为:SimHei p1 = [0.2, 0.2, 0.7, 0.6] plt.axes(p1) rho_vector = ipoptSolver() # plot trajectory plt.plot(s, rho_vector, c='b', linestyle="-", linewidth=0.5, alpha=1) # plot obstacles car.simVehicle([static_obs[0][0], static_obs[0][1]], 0 * math.pi / 180, 'r', 1) car.simVehicle([static_obs[1][0], static_obs[1][1]], 0 * math.pi / 180, 'r', 1) car.simVehicle([static_obs[2][0], static_obs[2][1]], 0 * math.pi / 180, 'r', 1) plt.grid(linestyle="--", linewidth=0.5, alpha=1) plt.title('x-y Graph', font1) plt.xlabel('x (m)', font1) plt.ylabel('y (m)', font1) plt.xticks(fontproperties='Times New Roman', fontsize=10) plt.yticks(fontproperties='Times New Roman', fontsize=10) # plt.xlim(-1, 110) # plt.ylim(-4, 4) plt.savefig('../SimGraph/pathOptimizationPath060414.svg') plt.figure(figsize=(3.5, 3.5 * 0.618)) # 单位英寸, 3.5 font1 = {'family': 'Times New Roman', 'weight': 'normal', 'size': 10} plt.rcParams['font.sans-serif'] = ['Times New Roman'] # 如果要显示中文字体,则在此处设为:SimHei p2 = [0.2, 0.2, 0.7, 0.6] plt.axes(p2) kappa_list = trajectory_kappa(rho_vector, s) plt.plot(s[0:n_s + 1], kappa_list, c= 'b', linestyle="-", linewidth=0.5, alpha=1) y = [max(kappa_list) for i in range(n_s + 1)] y2 = [min(kappa_list) for i in range(n_s + 1)] plt.plot(s[0:n_s + 1], y, c= 'r', linestyle="--", linewidth=0.5, alpha=1) plt.plot(s[0:n_s + 1], y2, c= 'r', linestyle="--", linewidth=0.5, alpha=1) plt.title('Curvature Profile', font1) plt.grid(linestyle="--", linewidth=0.5, alpha=1) plt.xlabel('s (m)', font1) plt.ylabel('kappa (1/m)', font1) plt.xlim(-1, 110) plt.ylim(-0.2, 0.2) plt.savefig('../SimGraph/pathOptimizationKappa060413.svg') plt.show()
def do_simulation(cx, cy, cyaw, ck, sp, dl, initial_state): """ Simulation cx: course x position list cy: course y position list cy: course yaw position list ck: course curvature list sp: speed profile dl: course tick [m] """ goal = [cx[-1], cy[-1]] state = initial_state # initial yaw compensation if state.yaw - cyaw[0] >= math.pi: state.yaw -= math.pi * 2.0 elif state.yaw - cyaw[0] <= -math.pi: state.yaw += math.pi * 2.0 time = 0.0 x = [state.x] y = [state.y] yaw = [state.yaw] v = [state.v] t = [0.0] d = [0.0] a = [0.0] target_ind, _ = calc_nearest_index(state, cx, cy, cyaw, 0) odelta, oa = None, None cyaw = smooth_yaw(cyaw) while MAX_TIME >= time: xref, target_ind, dref = calc_ref_trajectory(state, cx, cy, cyaw, ck, sp, dl, target_ind) x0 = [state.x, state.y, state.v, state.yaw] # current state oa, odelta, ox, oy, oyaw, ov = iterative_linear_mpc_control( xref, x0, dref, oa, odelta) if odelta is not None: di, ai = odelta[0], oa[0] state = update_state(state, ai, di) time = time + DT x.append(state.x) y.append(state.y) yaw.append(state.yaw) v.append(state.v) t.append(time) d.append(di) a.append(ai) if check_goal(state, goal, target_ind, len(cx)): print("Goal") break if showVehicle: # pragma: no cover plt.cla() time_stamp = time / 100 + 0.3 plt.plot(cx, cy, "-r", label="course") # plt.plot(x, y, c="b", label="trajectory", ls = '-') # plt.xlim(-10, 80) plt.axis("equal") plt.grid(True) # plt.ylim(-10, 80) simModel.simVehicle(center_point=[x[-1], y[-1]], heading=yaw[-1], color='b', timestamp=1) # plt.plot(xref[0, :], xref[1, :], "xk", label="xref") # plt.plot(cx[target_ind], cy[target_ind], "xg", label="target") # plot_car(state.x, state.y, state.yaw, steer=di) # plt.xlim(-10, 100) # plt.ylim(-10, 100) plt.title("Time[s]:" + str(round(time, 2)) + ", speed[km/h]:" + str(round(state.v * 3.6, 2))) plt.pause(0.0001) return t, x, y, yaw, v, d, a
def plotGraph(): # plot graph plt.figure(figsize=(3.5, 3.0)) # 单位英寸, 3.5 font1 = {'family': 'Times New Roman', 'weight': 'normal', 'size': 10} plt.rcParams['font.sans-serif'] = ['Times New Roman'] # 如果要显示中文字体,则在此处设为:SimHei p1 = [0.15, 0.15, 0.80, 0.8] plt.axes(p1) efficients = cubicSpline.saveEfficients() rho_vector = ipoptSolver(efficients) theta_rho = [] for i in range(len(s) - 1): state0 = [s[i], rho_vector[i]] state1 = [s[i + 1], rho_vector[i + 1]] tmp_theta = heading.calHeadingFrenet(state0, state1) theta_rho.append(tmp_theta) theta_rho.append(theta_rho[-1]) x = [] y = [] theta = [] # print(s) for j in range(len(s)): tmpX, tmpY, tmpTheta = ftc.frenetToXY(s[j], rho_vector[j], theta_rho[j], efficients) x.append(tmpX) y.append(tmpY) theta.append(tmpTheta) tmp_s = [] tmp_rho = [] tmp_thetaRho = [] if showRoughPath: tmp_s, tmp_rho, tmp_thetaRho = pathPlanner.ToPathOptimizer2(efficients) if showRefinedPath: plt.plot(x, y, c='cyan', linewidth=0.6, alpha=1, label='Refined path') # plot obstacles if show_obstacle: for i in range(len(static_obs)): tmpx, tmpy, tmptheta = ftc.frenetToXY(static_obs[i][0], static_obs[i][1], obstacleHeading, efficients) car.simVehicle([tmpx, tmpy], tmptheta, 'r', 1) if show_vehicle_start: x0, y0, theta0 = ftc.frenetToXY(start_SRho[0], start_SRho[1], start_SRho[2], efficients) car.simVehicle([x0, y0], theta0, 'blue', 1) if show_vehicle_animation: for i in range(0, n_s, 1): time_stamp = i / n_s + 0.3 if time_stamp > 1.0: time_stamp = 1.0 if i <= 5: car.simVehicle([x[i], y[i]], theta[i], 'b', time_stamp) if 5 <= i <= 10: if (i % 2) == 0: car.simVehicle([x[i], y[i]], theta[i], 'b', time_stamp) if 10 <= i <= n_s: if (i % 3) == 0: car.simVehicle([x[i], y[i]], theta[i], 'b', time_stamp) # plt.pause(0.001) plt.grid(linestyle="--", linewidth=0.5, alpha=1) # plt.title('x-y Graph', font1) plt.xlabel('x (m)', font1) plt.ylabel('y (m)', font1) plt.xticks(fontproperties='Times New Roman', fontsize=10) plt.yticks(fontproperties='Times New Roman', fontsize=10) # plt.legend(loc=0) # 图例位置自动 plt.xlim(-10, 30) plt.ylim(-5, 80) plt.savefig('../SimGraph/pathOptimization2Path060406.svg') plt.savefig('../SimGraph/pathOptimization2Path060406.tiff', dpi=600) plt.figure(figsize=(3.5, 1.8)) # 单位英寸, 3.5 font1 = {'family': 'Times New Roman', 'weight': 'normal', 'size': 10} plt.rcParams['font.sans-serif'] = ['Times New Roman'] # 如果要显示中文字体,则在此处设为:SimHei p2 = [0.2, 0.25, 0.75, 0.60] plt.axes(p2) if showRoughKappa: rough_kappa = calKappa.path_kappa(tmp_rho, tmp_s, efficients) print("--------------") print(len(rough_kappa)) print(len(tmp_s)) plt.plot(tmp_s[0:-2], rough_kappa, c='magenta', linestyle="-", linewidth=0.5, alpha=1,label='Rough Path Curvature Profile') if showRefinedKappa: kappa_refined = calKappa.path_kappa(rho_vector, s, efficients) plt.plot(s[0:n_s + 1], kappa_refined, c='cyan', linestyle="-", linewidth=0.5, alpha=1, label='Refined Path Curvature Profile') y = [max(kappa_refined) for i in range(n_s + 1)] y2 = [min(kappa_refined) for i in range(n_s + 1)] # plt.plot(s[0:n_s + 1], y, c='k', linestyle="--", linewidth=0.5, alpha=1) # plt.plot(s[0:n_s + 1], y2, c='k', linestyle="--", linewidth=0.5, alpha=1) plt.title('Curvature Profile', font1) plt.grid(linestyle="--", linewidth=0.5, alpha=1) plt.xlabel('s (m)', font1) plt.ylabel('kappa (1/m)', font1) plt.xlim(-1, 110) plt.ylim(-0.1, 0.2) plt.legend(loc=0) # 图例位置自动 plt.savefig('../SimGraph/pathOptimization2Kappa060406.svg') plt.savefig('../SimGraph/pathOptimization2Kappa060406.tiff', dpi=600) plt.show() return None
def dynamic_vehicle(): for station in range(100): time_stamp = 0.0 + station / 100.0 vehicle.simVehicle([station, 3.75 / 2.0], 0, 'b', time_stamp) plt.pause(0.01)
def plotGraph(): # plot graph plt.figure(figsize=(3.5, 3.5 * 0.62)) # 单位英寸, 3.5 font1 = {'family': 'Times New Roman', 'weight': 'normal', 'size': 10} plt.rcParams['font.sans-serif'] = ['Times New Roman' ] # 如果要显示中文字体,则在此处设为:SimHei p1 = [0.2, 0.2, 0.7, 0.7] plt.axes(p1) efficients = cubicSpline.saveEfficients() rho_vector = ipoptSolver(efficients) theta_rho = [] for i in range(len(s) - 1): state0 = [s[i], rho_vector[i]] state1 = [s[i + 1], rho_vector[i + 1]] tmp_theta = heading.calHeadingFrenet(state0, state1) theta_rho.append(tmp_theta) theta_rho.append(theta_rho[-1]) x = [] y = [] theta = [] # print(s) for j in range(len(s)): tmpX, tmpY, tmpTheta = ftc.frenetToXY(s[j], rho_vector[j], theta_rho[j], efficients) x.append(tmpX) y.append(tmpY) theta.append(tmpTheta) tmp_s = [] tmp_rho = [] tmp_thetaRho = [] if showRoughPath: tmp_s, tmp_rho, tmp_thetaRho = pathPlanner.ToPathOptimizer5(efficients) if showRefinedPath: plt.plot(x[0:-3], y[0:-3], c='cyan', linewidth=0.6, alpha=1, label='Refined path') if showVehicleStart: tmpx, tmpy, tmptheta = ftc.frenetToXY(start_SRho[0], start_SRho[1], obstacleHeading, efficients) car.simVehicle([tmpx, tmpy], tmptheta, 'b', 1) # print(len(x), len(y), len(theta)) # print(x[0], y[0], theta[0]) # plot obstacles if showObsDyn: c = ['r', 'gold', 'lightseagreen'] for i in range(len(static_obs)): obs_s = [ j for j in np.arange(static_obs[-(i + 1)][0], static_obs[-(i + 1)][0] + 100, 1) ] obs_rho = [ static_obs[-(i + 1)][1] for q in np.arange( static_obs[-(i + 1)][0], static_obs[-(i + 1)][0] + 100, 1) ] for m in range(0, len(obs_s), 2): time_stamp = m / len(obs_s) + 0.05 if time_stamp > 1.0: time_stamp = 1.0 tmpx, tmpy, tmptheta = ftc.frenetToXY(obs_s[m], obs_rho[m], obstacleHeading, efficients) car.simVehicle([tmpx, tmpy], tmptheta, c[i], time_stamp) if showStaticObstacle: for i in range(len(static_obs)): c = ['lightseagreen', 'orange', 'green'] tmpx, tmpy, tmptheta = ftc.frenetToXY(static_obs[i][0], static_obs[i][1], obstacleHeading, efficients) car.simVehicle([tmpx, tmpy], tmptheta, c[i], 1) if showVehicleAnimation: for i in range(0, 6, 1): time_stamp = i / n_s + 0.1 if time_stamp > 1.0: time_stamp = 1.0 car.simVehicle([x[i], y[i]], theta[i], 'b', time_stamp) for i in range(6, 30, 2): time_stamp = i / n_s + 0.1 if time_stamp > 1.0: time_stamp = 1.0 car.simVehicle([x[i], y[i]], theta[i], 'b', time_stamp) for i in range(30, 60, 4): time_stamp = i / n_s + 0.1 if time_stamp > 1.0: time_stamp = 1.0 car.simVehicle([x[i], y[i]], theta[i], 'b', time_stamp) # plt.pause(0.001) for i in range(60, 80, 5): time_stamp = i / n_s + 0.1 if time_stamp > 1.0: time_stamp = 1.0 car.simVehicle([x[i], y[i]], theta[i], 'b', time_stamp) for i in range(80, 120, 4): time_stamp = i / n_s + 0.1 if time_stamp > 1.0: time_stamp = 1.0 car.simVehicle([x[i], y[i]], theta[i], 'b', time_stamp) for i in range(120, 160, 3): time_stamp = i / n_s + 0.1 if time_stamp > 1.0: time_stamp = 1.0 car.simVehicle([x[i], y[i]], theta[i], 'b', time_stamp) plt.grid(linestyle="--", linewidth=0.5, alpha=1) # plt.title('x-y Graph', font1) plt.xlabel('x (m)', font1) plt.ylabel('y (m)', font1) plt.xticks(fontproperties='Times New Roman', fontsize=10) plt.yticks(fontproperties='Times New Roman', fontsize=10) # plt.legend(loc=0) # 图例位置自动 plt.xlim(300, 500) plt.ylim(60, 190) # plt.axis('equal') plt.savefig('../SimGraph/pathOptimizer_5Path060406.svg') plt.savefig('../SimGraph/pathOptimizer_5Path060406.tiff', dpi=600) plt.figure(figsize=(3.5, 1.8)) # 单位英寸, 3.5 font1 = {'family': 'Times New Roman', 'weight': 'normal', 'size': 10} plt.rcParams['font.sans-serif'] = ['Times New Roman' ] # 如果要显示中文字体,则在此处设为:SimHei p2 = [0.2, 0.25, 0.7, 0.6] plt.axes(p2) if showRoughKappa: print("==================") print("len:tmp_rho,tmp_s", len(tmp_rho), len(tmp_s)) rough_kappa = calKappa.path_kappa(tmp_rho, tmp_s, efficients) print("--------------") print(len(rough_kappa)) plt.plot(tmp_s[0:-2], rough_kappa, c='magenta', linestyle="-", linewidth=0.5, alpha=1, label='Rough Path Curvature Profile') if showRefinedKappa: kappa_refined = calKappa.path_kappa(rho_vector, s, efficients) plt.plot(s[0:-2], kappa_refined, c='cyan', linestyle="-", linewidth=0.5, alpha=1, label='Refined Path Curvature Profile') # y = [kappa_max for i in range(n_s + 1)] # y2 = [-kappa_max for i in range(n_s + 1)] # plt.plot(s[0:n_s + 1], y, c= 'k', linestyle="--", linewidth=0.5, alpha=1) # plt.plot(s[0:n_s + 1], y2, c= 'k', linestyle="--", linewidth=0.5, alpha=1) plt.title('Curvature Profile', font1) plt.grid(linestyle="--", linewidth=0.5, alpha=1) plt.xlabel('s (m)', font1) plt.ylabel('kappa (1/m)', font1) plt.xlim(400, 580) plt.ylim(-0.1, 0.3) plt.legend(loc=0) # 图例位置自动 plt.savefig('../SimGraph/pathOptimization5Kappa060406.svg') plt.savefig('../SimGraph/pathOptimization5Kappa060406.tiff', dpi=600) plt.show()
x3 = a + (r + 3.75) * np.cos(theta) y3 = b + (r + 3.75) * np.sin(theta) # ===================================== # 画出移动障碍物图 heading = np.arange(-np.pi, -np.pi * 3 / 2, -0.01) center_x = a + (r + 3.75 / 2) * np.cos(heading) center_y = b + (r + 3.75 / 2) * np.sin(heading) real_heading = heading + math.pi * 3 / 2 fig = plt.figure() ax = fig.add_subplot(1, 1, 1) ax.plot(x, y, color='black', linewidth=2, linestyle='--') # axes.plot(x2_center, y2_center, color='r', linewidth='0.5', linestyle='--') ax.plot(x2, y2, color='black', linewidth=1.5, linestyle='-') # axes.plot(x3_center, y3_center, color='r', linewidth='0.5', linestyle='--') ax.plot(x3, y3, color='black', linewidth=2, linestyle='-') for i in range(len(real_heading)): time_stamp = 0.0 + i / len(real_heading) vehicle.simVehicle([center_x[i], center_y[i]], real_heading[i], 'b', time_stamp) plt.pause(0.001) if __name__ == '__main__': plt.axis("equal") plt.grid(True) plt.show()
def plotGraph(): # plot graph plt.figure(figsize=(3.5, 1.2)) # 单位英寸, 3.5 font1 = {'family': 'Times New Roman', 'weight': 'normal', 'size': 10} plt.rcParams['font.sans-serif'] = ['Times New Roman' ] # 如果要显示中文字体,则在此处设为:SimHei p1 = [0.15, 0.4, 0.8, 0.4] plt.axes(p1) # 调用pathplanner的环境 pp.plotGraph() rho_vector = ipoptSolver() # plot trajectory plt.plot(s, rho_vector, c='cyan', linestyle="-", linewidth=1, alpha=1, label='Refined path') car.simVehicle([x_init[2], x_init[3]], 0 * math.pi / 180, 'b', 1) # plot obstacles car.simVehicle([static_obs[0][0], static_obs[0][1]], 0 * math.pi / 180, 'r', 1) car.simVehicle([static_obs[1][0], static_obs[1][1]], 0 * math.pi / 180, 'r', 1) car.simVehicle([static_obs[2][0], static_obs[2][1]], 0 * math.pi / 180, 'r', 1) plt.grid(linestyle="--", linewidth=0.5, alpha=1) plt.title('x-y Graph', font1) plt.xlabel('x (m)', font1) plt.ylabel('y (m)', font1) plt.xticks(fontproperties='Times New Roman', fontsize=10) plt.yticks(fontproperties='Times New Roman', fontsize=10) plt.xlim(-1, 102) plt.ylim(-4, 4) # plt.legend(loc=0) # 图例位置自动 plt.savefig('../SimGraph/pathOptimizationTest_060404.svg') plt.savefig('../SimGraph/pathOptimizationTest_060404.tiff', dpi=600) plt.figure(figsize=(3.5, 1.2)) # 单位英寸, 3.5 font1 = {'family': 'Times New Roman', 'weight': 'normal', 'size': 10} plt.rcParams['font.sans-serif'] = ['Times New Roman' ] # 如果要显示中文字体,则在此处设为:SimHei p2 = [0.2, 0.35, 0.75, 0.6] plt.axes(p2) kappa_list = trajectory_kappa(rho_vector, s) plt.plot(s[0:n_s + 1], kappa_list, c='cyan', linestyle="-", linewidth=0.8, alpha=1, label='Refined Path') # plot rough path kappa pp.plotkappaGraph() y = [kappa_max for i in range(n_s + 1)] y2 = [-kappa_max for i in range(n_s + 1)] # plt.plot(s[0:n_s + 1], y, c= 'r', linestyle="--", linewidth=0.5, alpha=1, label='Maximum Curvature Limit') # plt.plot(s[0:n_s + 1], y2, c= 'orange', linestyle="--", linewidth=0.5, alpha=1, label='Minimum Curvature Limit') plt.title('Curvature Profile', font1) plt.grid(linestyle="--", linewidth=0.5, alpha=1) plt.xlabel('s (m)', font1) plt.ylabel('kappa (1/m)', font1) plt.xlim(-1, 102) plt.ylim(-0.12, 0.12) # plt.legend(loc=0) # 图例位置自动 plt.savefig('../SimGraph/pathOptimizationTestKappa_060404.svg') plt.savefig('../SimGraph/pathOptimizationTestKappa_060404.tiff', dpi=600) plt.show()
def plotGraph(): print(__file__ + " start!!") plt.figure(figsize=(3.5, 3.5 * 0.62)) # 单位英寸, 3.5 plt.axes([0.2, 0.2, 0.7, 0.7]) plt.axis("equal") plt.grid(linestyle="--", linewidth=0.5, alpha=1) # 计算回环环境的弧长参数曲线的系数 efficients = cubicSpline.saveEfficients() pathlattice.ToPathPlanner(efficients) if show_obstacle: c = ['r', 'r', 'r', 'darkorange'] for i in range(len(obstacle)): tmpx, tmpy, tmptheta = ftc.frenetToXY(obstacle[i][0], obstacle[i][1], obstacleHeading, efficients) car.simVehicle([tmpx, tmpy], tmptheta, c[i], 0.8) closed_set = dijkstra_planning(start_SRho, longi_step, latera_step, lateral_num, efficients) # print('----------------------------------') # print('closed_set:', len(closed_set)) goal = determine_goal(closed_set) rx, ry = calc_final_path(goal, closed_set) # print("rx, ry: %s" % rx, ry) tmp_s = [] tmp_rho = [] tmp_thetaRho = [] for i in range(len(rx) - 1): point_s = [rx[-(i + 1)], ry[-(i + 1)], 0.0 * math.pi / 180.0] point_e = [rx[-(i + 2)], ry[-(i + 2)], 0.0 * math.pi / 180.0] s, rho, thetaRho = cubic.Polynomial(point_s, point_e) tmp_s.extend(s) tmp_rho.extend(rho) tmp_thetaRho.extend(thetaRho) x = [] y = [] theta = [] if show_rough_path: # print(s) for j in range(len(tmp_s)): tmpX, tmpY, tmpTheta = ftc.frenetToXY(tmp_s[j], tmp_rho[j], tmp_thetaRho[j], efficients) x.append(tmpX) y.append(tmpY) theta.append(tmpTheta) # plt.scatter(end_set[0, i][0], end_set[0, i][1], color='b', s=2, alpha=0.8) plt.plot(x, y, 'magenta', linewidth=0.6, alpha=1) if show_vehicle_animation: x0, y0, theta0 = ftc.frenetToXY(start_SRho[0], start_SRho[1], start_SRho[2], efficients) car.simVehicle([x0, y0], theta0, 'b', 1) for j in range(0, len(tmp_s), 50): time_stamp = tmp_s[j] / s_end car.simVehicle([x[j], y[j]], theta[j], 'b', time_stamp) plt.pause(0.001) font1 = { 'family': 'Times New Roman', 'weight': 'normal', 'size': 10, } plt.xlabel("x (m)", font1) plt.ylabel('y (m)', font1) plt.xticks(fontproperties='Times New Roman', fontsize=10) plt.yticks(fontproperties='Times New Roman', fontsize=10) plt.xlim(120, 200) plt.ylim(90, 140) # plt.savefig('/home/ming/桌面/PTPSim/SimGraph/pathPlannerXY_5_30_8_48.svg') plt.savefig('../SimGraph/pathPlanner4_061102.tiff', dpi=600) plt.show()
if __name__ == '__main__': print(__file__ + " start!!") plt.figure(figsize=(3.5, 3.5 * 0.9)) # 单位英寸, 3.5 plt.axes([0.15, 0.15, 0.75, 0.75]) plt.axis("equal") plt.grid(linestyle="--", linewidth=0.5, alpha=1) efficients = mlane.saveEfficients() if show_lane: # show multilane mlane.curvePath() if show_obstacle: for i in range(len(obstacle)): x, y, theta = mlane.frenetToXY(obstacle[i][0], obstacle[i][1], obstacleHeading, efficients) car.simVehicle([x, y], theta, 'r', 0.8) # sampling_point = PathLattice.sampling(longitudinal_num, lateral_num, latera_step, longitudinal_step) # 用来显示lattice图像 # PathLattice.generate_lattice() closed_set = dijkstra_planning(start_SRho, longi_step, latera_step, lateral_num, efficients) # print('----------------------------------') # print('closed_set:', len(closed_set)) goal = determine_goal(closed_set) rx, ry = calc_final_path(goal, closed_set) # print("rx, ry: %s" % rx, ry) tmp_s = [] tmp_rho = []
def plotGraph(): print(__file__ + " start!!") plt.figure(figsize=(3.5, 3.0)) # 单位英寸, 3.5 p1 = [0.15, 0.15, 0.80, 0.8] plt.axes(p1) plt.axis("equal") plt.grid(linestyle="--", linewidth=0.5, alpha=1) # # 计算多车道环境的弧长参数曲线的系数 # efficients = multiLane.saveEfficients() # 计算回环环境的弧长参数曲线的系数 efficients = cubicSpline.saveEfficients() # if show_lane: # # show multilane # mlane.curvePath() if show_obstacle: for i in range(len(obstacle)): x, y, theta = ftc.frenetToXY(obstacle[i][0], obstacle[i][1], obstacleHeading, efficients) car.simVehicle([x, y], theta, 'r', 0.8) # sampling_point = PathLattice.sampling(longitudinal_num, lateral_num, latera_step, longitudinal_step) # 用来显示lattice图像 # PathLattice.generate_lattice() closed_set = dijkstra_planning(start_SRho, longi_step, latera_step, lateral_num, efficients) # print('----------------------------------') # print('closed_set:', len(closed_set)) goal = determine_goal(closed_set) rx, ry = calc_final_path(goal, closed_set) # print("rx, ry: %s" % rx, ry) tmp_s = [] tmp_rho = [] tmp_thetaRho = [] for i in range(len(rx) - 1): point_s = [rx[-(i + 1)], ry[-(i + 1)], 0.0 * math.pi / 180.0] point_e = [rx[-(i + 2)], ry[-(i + 2)], 0.0 * math.pi / 180.0] s, rho, thetaRho = cubic.Polynomial(point_s, point_e) tmp_s.extend(s) tmp_rho.extend(rho) tmp_thetaRho.extend(thetaRho) if showPathLattice: pathlattice.ToPathPlanner2(efficients) x = [] y = [] theta = [] if show_rough_path: # print(s) for j in range(len(tmp_s)): tmpX, tmpY, tmpTheta = ftc.frenetToXY(tmp_s[j], tmp_rho[j], tmp_thetaRho[j], efficients) x.append(tmpX) y.append(tmpY) theta.append(tmpTheta) # plt.scatter(end_set[0, i][0], end_set[0, i][1], color='b', s=2, alpha=0.8) plt.plot(x, y, 'magenta', linewidth=0.5, alpha=1) if show_vehicle_animation: x0, y0, theta0 = ftc.frenetToXY(start_SRho[0], start_SRho[1], start_SRho[2], efficients) car.simVehicle([x0, y0], theta0, 'g', 1) time_stamp = 0 for j in range(0, len(tmp_s), 50): time_stamp = tmp_s[j] / s_max car.simVehicle([x[j], y[j]], theta[j], 'b', time_stamp) plt.pause(0.01) font1 = {'family': 'Times New Roman', 'weight': 'normal', 'size': 10, } plt.xlabel("x (m)", font1) plt.ylabel('y (m)', font1) plt.xticks(fontproperties='Times New Roman', fontsize=10) plt.yticks(fontproperties='Times New Roman', fontsize=10) plt.xlim(-10, 30) plt.ylim(-5, 80) # plt.savefig('/home/ming/桌面/PTPSim/SimGraph/pathPlannerXY_5_30_8_48.svg') plt.savefig('../SimGraph/pathPlanner2_061101.tiff', dpi=600) plt.show()
def plotGraph(): if showLane: lane.lane() if showObstacle: for i in range(len(obstacle)): car.simVehicle([obstacle[i][0], obstacle[i][1]], 0 * math.pi / 180, 'r', 1) if showRefLine: x = [i for i in range(100)] refline = [refLineRho for i in range(100)] plt.plot(x, refline, c='green', linestyle="--", linewidth=1.2, alpha=1, label='Reference Line') # plt.legend(loc = 1) # sampling_point = PathLattice.sampling(longitudinal_num, lateral_num, latera_step, longitudinal_step) # 用来显示lattice图像 # PathLattice.generate_lattice() efficients = cubicSpline.saveEfficients() closed_set = dijkstra_planning(begin, longi_step, latera_step, lateral_num, efficients) # print('----------------------------------') # print('closed_set:', len(closed_set)) goal = determine_goal(closed_set) rx, ry = calc_final_path(goal, closed_set) # print("rx, ry: %s" % rx, ry) tmp_s = [] tmp_rho = [] tmp_theta = [] for i in range(len(rx) - 1): point_s = [rx[-(i + 1)], ry[-(i + 1)], 0.0 * math.pi / 180.0] point_e = [rx[-(i + 2)], ry[-(i + 2)], 0.0 * math.pi / 180.0] s, rho, theta = cubic.Polynomial(point_s, point_e) tmp_s.extend(s) tmp_rho.extend(rho) tmp_theta.extend(theta) plt.plot(tmp_s, tmp_rho, c='b', linewidth=0.8, label='Rough Path') # plt.legend(loc=2) if showVehicleAnimation: car.simVehicle([begin[0], begin[1]], 0 * math.pi / 180, 'g', 1) time_stamp = 0 for j in range(0, len(tmp_s), 30): time_stamp = tmp_s[j] / s_max car.simVehicle([tmp_s[j], tmp_rho[j]], tmp_theta[j], 'b', time_stamp) plt.pause(0.001) font1 = { 'family': 'Times New Roman', 'weight': 'normal', 'size': 10, } plt.xlabel("x (m)", font1) plt.ylabel('y (m)', font1) plt.xticks(fontproperties='Times New Roman', fontsize=10) plt.yticks(fontproperties='Times New Roman', fontsize=10) plt.xlim(-1, 110) plt.ylim(-4, 20) plt.axis("equal") plt.savefig('../SimGraph/pathPlanner060502.tiff', dpi=600)
def plotGraph(): # plot graph plt.figure(figsize=(3.5, 3.5 * 0.62)) # 单位英寸, 3.5 font1 = {'family': 'Times New Roman', 'weight': 'normal', 'size': 10} plt.rcParams['font.sans-serif'] = ['Times New Roman' ] # 如果要显示中文字体,则在此处设为:SimHei p1 = [0.2, 0.2, 0.7, 0.7] plt.axes(p1) efficients = cubicSpline.saveEfficients() rho_vector = ipoptSolver(efficients) theta_rho = [] for i in range(len(s) - 1): state0 = [s[i], rho_vector[i]] state1 = [s[i + 1], rho_vector[i + 1]] tmp_theta = heading.calHeadingFrenet(state0, state1) theta_rho.append(tmp_theta) theta_rho.append(theta_rho[-1]) x = [] y = [] theta = [] # print(s) for j in range(len(s)): tmpX, tmpY, tmpTheta = ftc.frenetToXY(s[j], rho_vector[j], theta_rho[j], efficients) x.append(tmpX) y.append(tmpY) theta.append(tmpTheta) tmp_s = [] tmp_rho = [] tmp_thetaRho = [] if showRoughPath: tmp_s, tmp_rho, tmp_thetaRho = pathPlanner.ToPathOptimizer3(efficients) if showRefinedPath: plt.plot(x, y, c='cyan', linewidth=0.6, alpha=1, label='Refined path') if showVehicleStart: tmpx, tmpy, tmptheta = ftc.frenetToXY(start_SRho[0], start_SRho[1], obstacleHeading, efficients) car.simVehicle([tmpx, tmpy], tmptheta, 'b', 0.8) # print(len(x), len(y), len(theta)) # print(x[0], y[0], theta[0]) # plot obstacles # if show_obstacle: # c=['r', 'gold', 'darkorange'] # for i in range(len(dyn_obs)): # obs_s = [j for j in np.arange(dyn_obs[-(i+1)][0], dyn_obs[-(i+1)][0]+ 50, 1)] # obs_rho = [dyn_obs[-(i+1)][1] for q in np.arange(dyn_obs[-(i+1)][0], dyn_obs[-(i+1)][0]+ 50, 1)] # for m in range(0, len(obs_s), 1): # time_stamp = m / len(obs_s) + 0.08 # if time_stamp > 1.0: # time_stamp = 1.0 # tmpx, tmpy, tmptheta = ftc.frenetToXY(obs_s[m], obs_rho[m], obstacleHeading, efficients) # car.simVehicle([tmpx, tmpy], tmptheta, c[i], time_stamp) if show_obstacle: c = ['r', 'gold', 'darkorange'] for i in range(len(static_obs)): tmpx, tmpy, tmptheta = ftc.frenetToXY(static_obs[-(i + 1)][0], static_obs[-(i + 1)][1], obstacleHeading, efficients) car.simVehicle([tmpx, tmpy], tmptheta, c[i], 0.8) if show_vehicle_animation: for i in range(0, n_s, 2): time_stamp = i / n_s + 0.1 if time_stamp > 1.0: time_stamp = 1.0 car.simVehicle([x[i], y[i]], theta[i], 'b', time_stamp) # plt.pause(0.001) plt.grid(linestyle="--", linewidth=0.5, alpha=1) # plt.title('x-y Graph', font1) plt.xlabel('x (m)', font1) plt.ylabel('y (m)', font1) plt.xticks(fontproperties='Times New Roman', fontsize=10) plt.yticks(fontproperties='Times New Roman', fontsize=10) # plt.legend(loc=0) # 图例位置自动 plt.xlim(30, 140) plt.ylim(80, 130) # plt.axis('equal') plt.savefig('../SimGraph/pathOptimization3Path060404.svg') plt.savefig('../SimGraph/pathOptimization3Path060404.tiff', dpi=600) plt.figure(figsize=(3.5, 1.8)) # 单位英寸, 3.5 font1 = {'family': 'Times New Roman', 'weight': 'normal', 'size': 10} plt.rcParams['font.sans-serif'] = ['Times New Roman' ] # 如果要显示中文字体,则在此处设为:SimHei p2 = [0.2, 0.25, 0.7, 0.6] plt.axes(p2) if showRoughKappa: rough_kappa = calKappa.path_kappa(tmp_rho, tmp_s, efficients) print("--------------") print(len(rough_kappa)) plt.plot(tmp_s[0:-2], rough_kappa, c='magenta', linestyle="-", linewidth=0.5, alpha=1, label='Rough Path Curvature Profile') if showRefinedKappa: kappa_list = calKappa.path_kappa(rho_vector, s, efficients) plt.plot(s[0:n_s + 1], kappa_list, c='cyan', linestyle="-", linewidth=0.5, alpha=1, label='Refined Path Curvature Profile') y = [max(kappa_list) for i in range(n_s + 1)] y2 = [min(kappa_list) for i in range(n_s + 1)] plt.plot(s[0:n_s + 1], y, c='k', linestyle="--", linewidth=0.5, alpha=1) plt.plot(s[0:n_s + 1], y2, c='k', linestyle="--", linewidth=0.5, alpha=1) plt.title('Curvature Profile', font1) plt.grid(linestyle="--", linewidth=0.5, alpha=1) plt.xlabel('s (m)', font1) plt.ylabel('kappa (1/m)', font1) plt.xlim(90, 210) plt.ylim(-0.1, 0.3) plt.legend(loc=0) # 图例位置自动 plt.savefig('../SimGraph/pathOptimization3Kappa060404.svg') plt.savefig('../SimGraph/pathOptimization3Kappa060404.tiff', dpi=600) plt.show()