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()
Exemple #5
0
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)
Exemple #6
0
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)
Exemple #7
0
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
Exemple #8
0
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
Exemple #10
0
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
Exemple #11
0
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)
Exemple #12
0
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
Exemple #13
0
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()
Exemple #14
0
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()
Exemple #15
0
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()
Exemple #16
0
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
Exemple #17
0
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()