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 trajectory_kappa(father_node, son_node, efficients): begin = [father_node.x, father_node.y, 0.0 * math.pi / 180.0] end = [son_node.x, son_node.y, 0.0 * math.pi / 180.0] s, rho, thetaRho = cubic.Polynomial(begin, end) tmp_kappa = [] for i in range(0, len(s), 5): x0, y0, theta0 = ftc.frenetToXY(s[i], rho[i], thetaRho[i], efficients) x1, y1, theta1 = ftc.frenetToXY(s[i + 1], rho[i + 1], thetaRho[i + 1], efficients) x2, y2, theta2 = ftc.frenetToXY(s[i + 2], rho[i + 2], thetaRho[i + 2], efficients) k1 = (x1 - x0) * (y2 - 2 * y1 + y0) k2 = (y1 - y0) * (x2 - 2 * x1 + x0) k3 = ((x1 - x0)**2 + (y1 - y0)**2)**(3.0 / 2.0) if (k3 == 0.0): tmp_kappa.append(0) else: tmp_kappa.append((k1 - k2) / k3) sum_kappa = 0 for i in range(len(tmp_kappa)): sum_kappa = sum_kappa + tmp_kappa[i]**2 mean_kappa = sum_kappa / len(tmp_kappa) return mean_kappa
def boundValue(efficients): # 等式约束 # 注意,这里生成的坐标位置是最右侧车道线的,不是参考线的 xr0, yr0, thetar0 = ftc.search_in_coefficients(s0, efficients) s1 = s0 + reso_s # xr1, yr1, thetar1 = ftc.search_in_coefficients(s1, efficients) s2 = s0 + 2 * reso_s xr2, yr2, thetar2 = ftc.frenetToXY(s2, refLineRho, 0, efficients) x0, y0, theta0 = ftc.frenetToXY(start_SRho[0], start_SRho[1], start_SRho[2], efficients) print("x0, y0, theta0:", x0, y0, theta0) rho_1 = reso_s * (math.tan(theta0 - thetar0)) + start_SRho[1] x1, y1, theta1 = ftc.frenetToXY(s1, rho_1, 0, efficients) print("x1, y1, theta1:", x1, y1, theta1) m = kappa0 * ((x1 - x0)**2 + (y1 - y0)**2)**(3 / 2) n = (x1 - x0) * (yr2 - 2 * y1 + y0) + (y1 - y0) * (xr2 - 2 * x1 + x0) q = (x1 - x0) * math.sin(thetar2 + math.pi / 2.0) - ( y1 - y0) * math.cos(thetar2 + math.pi / 2.0) rho_2 = (m - n) / q # 本来求出的是到参考线的距离 # 加上参考线到最右侧车道线的距离,是真正的偏移量, # 这个问题debug了很久,一定要将方程设在参考线处,可以免去很多麻烦 rho_2 = rho_2 + refLineRho print("rho1, rho2:", rho_1, rho_2) return rho_1, rho_2
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 inequality_cons(rho, s, optimization, efficients): # 曲率约束 for i in range(n_s): x0, y0, theta0 = ftc.frenetToXY(s[i], rho[i], 0, efficients) x1, y1, theta1 = ftc.frenetToXY(s[i + 1], rho[i + 1], 0, efficients) x2, y2, theta2 = ftc.frenetToXY(s[i + 2], rho[i + 2], 0, efficients) k1 = (x1 - x0) * (y2 - 2 * y1 + y0) k2 = (y1 - y0) * (x2 - 2 * x1 + x0) k3 = ((x1 - x0)**2 + (y1 - y0)**2)**(3.0 / 2.0) kappa1 = -k1 + k2 + k3 * kappa_max kappa2 = k1 - k2 + k3 * kappa_max optimization.subject_to(kappa1 >= 0) optimization.subject_to(kappa2 >= 0) # 障碍物约束 for i in range(n_s + 3): # the first obs if ((s[i] - static_obs[0][0])**2 < 9): c1_obs = (s[i] - static_obs[0][0])**2 + (rho[i] - static_obs[0][1])**2 c2_obs = (s[i] + d_circle - static_obs[0][0])**2 + (rho[i] - static_obs[0][1])**2 c3_obs = (s[i] + 2 * d_circle - static_obs[0][0])**2 + (rho[i] - static_obs[0][1])**2 optimization.subject_to(c1_obs >= safe_distance**2) optimization.subject_to(c2_obs >= safe_distance**2) optimization.subject_to(c3_obs >= safe_distance**2) # the second obs if ((s[i] - static_obs[1][0])**2 < 9): c1_obs2 = (s[i] - static_obs[1][0])**2 + (rho[i] - static_obs[1][1])**2 c2_obs2 = (s[i] + d_circle - static_obs[1][0])**2 + (rho[i] - static_obs[1][1])**2 c3_obs2 = (s[i] + 2 * d_circle - static_obs[1][0])**2 + (rho[i] - static_obs[1][1])**2 optimization.subject_to(c1_obs2 >= safe_distance**2) optimization.subject_to(c2_obs2 >= safe_distance**2) optimization.subject_to(c3_obs2 >= safe_distance**2) # the second obs if ((s[i] - static_obs[2][0])**2 < 9): c1_obs3 = (s[i] - static_obs[2][0])**2 + (rho[i] - static_obs[2][1])**2 c2_obs3 = (s[i] + d_circle - static_obs[2][0])**2 + (rho[i] - static_obs[2][1])**2 c3_obs3 = (s[i] + 2 * d_circle - static_obs[2][0])**2 + (rho[i] - static_obs[2][1])**2 optimization.subject_to(c1_obs3 >= safe_distance**2) optimization.subject_to(c2_obs3 >= safe_distance**2) optimization.subject_to(c3_obs3 >= safe_distance**2)
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 ToPathOptimizer2(efficients): 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.4, alpha=1) return tmp_s, tmp_rho, tmp_thetaRho
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 path_kappa(rho, s, efficients): kappaVec = [] for i in range(len(rho) - 2): x0, y0, theta0 = ftc.frenetToXY(s[i], rho[i], 0, efficients) x1, y1, theta1 = ftc.frenetToXY(s[i + 1], rho[i + 1], 0, efficients) x2, y2, theta2 = ftc.frenetToXY(s[i + 2], rho[i + 2], 0, efficients) k1 = (x1 - x0) * (y2 - 2 * y1 + y0) k2 = (y1 - y0) * (x2 - 2 * x1 + x0) k3 = ((x1 - x0)**2 + (y1 - y0)**2)**(3.0 / 2.0) if (k3 == 0.0): kappaVec.append(0) else: kappaVec.append((k1 - k2) / k3) return kappaVec
def generate_lattice(efficients): end_set = sampling(longitudinal_num, lateral_num, lateral_step, longitudinal_step) # print(end_set) end_size = end_set.shape # print("end_size:") # print(end_size) # 生成车辆起点到第一列采样点的图 for i in range(end_size[1]): s, rho, thetaRho = cubic.Polynomial(start_SRho, end_set[0, i]) x = [] y = [] # print(s) for j in range(len(s)): tmpX, tmpY, tmpTheta = ftc.frenetToXY(s[j], rho[j], thetaRho[j], efficients) x.append(tmpX) y.append(tmpY) # plt.scatter(end_set[0, i][0], end_set[0, i][1], color='b', s=2, alpha=0.8) plt.plot(x, y, c='b', linewidth=0.2, alpha=1.0) # 采样点之间的图 for i in range(end_size[0] - 1): for j in range(end_size[1]): # print([i, j]) for q in range(end_size[1]): # mptg.test_optimize_trajectory(end_set[1, 0], end_set[0, 1]) s, rho, thetaRho = cubic.Polynomial(end_set[i, q], end_set[i + 1, j]) x = [] y = [] # print(s) for q in range(len(s)): tmpX, tmpY, tmpTheta = ftc.frenetToXY( s[q], rho[q], thetaRho[q], efficients) x.append(tmpX) y.append(tmpY) # plt.scatter(end_set[i + 1, j][0], end_set[i + 1, j][1], color='b', s=2, alpha=0.8) plt.plot(x, y, c='b', linewidth=0.1, alpha=0.80) return None
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 ToMPC(): 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) kappa_list = calKappa.path_kappa(rho_vector, s, efficients) return x[0:n_s + 1], y[0:n_s + 1], theta[0:n_s + 1], kappa_list
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()
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(): # 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()
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 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()