def get_switch_back_course(dl):
ax = [0.0, 30.0, 6.0, 20.0, 35.0]
ay = [0.0, 0.0, 20.0, 35.0, 20.0]
cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(
ax, ay, ds=dl)
ax = [35.0, 10.0, 0.0, 0.0]
ay = [20.0, 30.0, 5.0, 0.0]
cx2, cy2, cyaw2, ck2, s2 = cubic_spline_planner.calc_spline_course(
ax, ay, ds=dl)
cyaw2 = [i - math.pi for i in cyaw2]
cx.extend(cx2)
cy.extend(cy2)
cyaw.extend(cyaw2)
ck.extend(ck2)
return cx, cy, cyaw, ck
def get_forward_course(dl):
ax = [0.0, 60.0, 125.0, 50.0, 75.0, 30.0, -10.0]
ay = [0.0, 0.0, 50.0, 65.0, 30.0, 50.0, -20.0]
cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(
ax, ay, ds=dl)
return cx, cy, cyaw, ck
def get_straight_course(dl):
ax = [0.0, 5.0, 10.0, 20.0, 30.0, 40.0, 50.0]
ay = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(
ax, ay, ds=dl)
return cx, cy, cyaw, ck
def get_straight_course2(dl):
ax = [0.0, -10.0, -20.0, -40.0, -50.0, -60.0, -70.0]
ay = [0.0, -1.0, 1.0, 0.0, -1.0, 1.0, 0.0]
cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(
ax, ay, ds=dl)
return cx, cy, cyaw, ck
def get_switch_back_course(dl):
ax = [0.0, 30.0, 6.0, 20.0, 35.0]
ay = [0.0, 0.0, 20.0, 35.0, 20.0]
cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(ax,
ay,
ds=dl)
ax = [35.0, 10.0, 0.0, 0.0]
ay = [20.0, 30.0, 5.0, 0.0]
cx2, cy2, cyaw2, ck2, s2 = cubic_spline_planner.calc_spline_course(ax,
ay,
ds=dl)
cyaw2 = [i - math.pi for i in cyaw2]
cx.extend(cx2)
cy.extend(cy2)
cyaw.extend(cyaw2)
ck.extend(ck2)
return cx, cy, cyaw, ck
def get_straight_course3(dl):
ax = [0.0, -10.0, -20.0, -40.0, -50.0, -60.0, -70.0]
ay = [0.0, -1.0, 1.0, 0.0, -1.0, 1.0, 0.0]
cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(
ax, ay, ds=dl)
cyaw = [i - math.pi for i in cyaw]
return cx, cy, cyaw, ck
def get_straight_course3(dl):
ax = [0.0, -10.0, -20.0, -40.0, -50.0, -60.0, -70.0]
ay = [0.0, -1.0, 1.0, 0.0, -1.0, 1.0, 0.0]
cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(ax,
ay,
ds=dl)
cyaw = [i - math.pi for i in cyaw]
return cx, cy, cyaw, ck
def get_lane_change_scenario4(course, dl):
ax = [-200, -210.0, -220.0, -230.0, -240.0, -250.0, -260, -270, -280]
ay = [-16, -16, -16.5, -17, -18, -19, -20, -20, -20]
cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(ax,
ay,
ds=dl)
cyaw = [i - math.pi for i in cyaw]
return cx, cy, cyaw, ck
def path_plan(dl):
# Insert path plan coordinates here:
ax = [10.0, 20.0]
ay = [10.0, 20.0]
cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(ax,
ay,
ds=dl)
return cx, cy, cyaw, ck
def main():
print("LQR steering control tracking start!!")
#ax = [0.0, 6.0, 12.5, 10.0, 17.5, 20.0, 25.0]
#ay = [0.0, -3.0, -5.0, 6.5, 3.0, 0.0, 0.0]
#ax = [15.0, 11.0, 10.5, 6.0, 1.5, -10.0, -15.0]
#ay = [15.0, 15.0, 12.0, 7.5, 3.0, 0.0, 0.0]
x_crds = np.array([-8, 1, 10, 19, 28.0, 28, 28, 28, 28])
y_crds = np.array([1.0, 1.0, 1.0, 1.0, 1, 10, 19, 28, 37])
u_path = np.array([-1, 2, 2, 2, 2, 3, 3, 3, 3, 0])
ax = list(x_crds)
ay = list(y_crds)
goal = [ax[-1], ay[-1]]
cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(ax,
ay,
ds=0.1)
primi = lattice_based_planner.Primitives(4.5, 0.1)
cx, cy, cyaw, ck = lattice_based_planner.calc_lattice_course(
primi, x_crds, y_crds, u_path)
target_speed = 10.8 / 3.6 # simulation parameter km/h -> m/s
sp = calc_speed_profile(cyaw, target_speed)
t, x, y, yaw, v = do_simulation(cx, cy, cyaw, ck, sp, goal)
if show_animation: # pragma: no cover
plt.close()
plt.subplots(1)
plt.plot(ax, ay, "xb", label="waypoints")
plt.plot(cx, cy, "-r", label="target course")
plt.plot(x, y, "-g", label="tracking")
plt.grid(True)
plt.axis("equal")
plt.xlabel("x[m]")
plt.ylabel("y[m]")
plt.legend()
plt.subplots(1)
plt.plot(s, [np.rad2deg(iyaw) for iyaw in cyaw], "-r", label="yaw")
plt.grid(True)
plt.legend()
plt.xlabel("line length[m]")
plt.ylabel("yaw angle[deg]")
plt.subplots(1)
plt.plot(s, ck, "-r", label="curvature")
plt.grid(True)
plt.legend()
plt.xlabel("line length[m]")
plt.ylabel("curvature [1/m]")
plt.show()
def pathCallback(msg):
global cx, cy, cyaw, ck, s
global global_path_x, global_path_y
global x, y, yaw, v, t
global state
global last_idx
global gpsvel
global state
# global header_time
global history_x_mid, history_y_mid
global updateIdx, updateRate
### Compare Header
# 0821 Add if path is same as prev one, pass
# start = time.time()
if updateIdx > updateRate:
global_path_x = msg.position
global_path_y = msg.velocity
updateIdx = 0
# print("global_path_x: ", global_path_x)
'''
if header_time == msg.header.stamp:
print("************************SAME PATH ")
#return 0
'''
if len(global_path_x) < 30:
print("ERROR Rogue Path data", len(global_path_x))
return 0
dist = np.sqrt(np.square(history_x_mid - global_path_x[4]) + np.square(history_y_mid -global_path_y[4] ))
# print("************************* DIST", dist)
history_x_mid = global_path_x[4]
history_y_mid = global_path_y[4]
# header_time = msg.header.stamp
cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(
global_path_x, global_path_y, ds=0.1)
# Initial state
state = State(x=global_path_x[0], y=global_path_y[0])
last_idx = len(cx) - 1
x = [state.x]
y = [state.y]
yaw = [state.yaw]
v = [state.v]
t = [0.0]
# target_idx, _ = calc_target_index(state, cx, cy)
updateIdx = 1 + updateIdx
def get_astar_course(dl):
df = pd.read_csv('route.csv', delim_whitespace=True)
ax = df.X
ay = df.Y
#ax = [0.0, 1.0, 2.0, 2.5, 3.0, 4.5, 6.0, 7.5, 9.0, 11.0, 12.0, 13.0, 15.0, 17.0]
#ay = [0.0, 1.0, 2.0, 4.0, 6.0, 8.0, 8.5, 10.5, 12.0, 13.0, 14.0, 15.0, 15.5, 16.0]
cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(ax,
ay,
ds=dl)
return cx, cy, cyaw, ck, s
def get_path():
waypoints_x = []
waypoints_y = []
x_path, y_path = Path.generate_path(True)
waypoints_x = x_path
waypoints_y = y_path
final_goal = [waypoints_x[-1], waypoints_y[-1]]
course_x, course_y, course_yaw, course_k, course_s = cubic_spline_planner.calc_spline_course(
waypoints_x, waypoints_y, ds=0.1)
target_velocity = 10.0 / 3.6
return waypoints_x, waypoints_y, final_goal, course_x, course_y, course_yaw, course_k, course_s, target_velocity
def main():
print("LQR steering control tracking start!!")
ax = [0.0, 6.0, 12.5, 10.0, 7.5, 3.0, -1.0]
ay = [0.0, -3.0, -5.0, 6.5, 3.0, 5.0, -2.0]
goal = [ax[-1], ay[-1]]
cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(ax,
ay,
ds=0.1)
target_speed = 10.0 / 3.6 # simulation parameter km/h -> m/s
sp = calc_speed_profile(cx, cy, cyaw, target_speed)
t, x, y, yaw, v, eigVals = closed_loop_prediction(cx, cy, cyaw, ck, sp,
goal)
if show_animation: # pragma: no cover
plt.close()
plt.subplots(1)
plt.plot(ax, ay, "xb", label="input")
plt.plot(cx, cy, "-r", label="spline")
plt.plot(x, y, "-g", label="tracking")
plt.grid(True)
plt.axis("equal")
plt.xlabel("x[m]")
plt.ylabel("y[m]")
plt.legend()
plt.subplots(1)
plt.plot(s, [np.rad2deg(iyaw) for iyaw in cyaw], "-r", label="yaw")
plt.grid(True)
plt.legend()
plt.xlabel("line length[m]")
plt.ylabel("yaw angle[deg]")
plt.subplots(1)
plt.plot(s, ck, "-r", label="curvature")
plt.grid(True)
plt.legend()
plt.xlabel("line length[m]")
plt.ylabel("curvature [1/m]")
plt.subplots(1)
plt.plot(t, eigVals[:, 0], "xb", label="lateral error eigval")
plt.plot(t, eigVals[:, 1], "cs", label="lateral error rate eigval")
plt.plot(t, eigVals[:, 2], "mo", label="heading error eigval")
plt.plot(t, eigVals[:, 3], "r*", label="heading error rate eigval")
plt.grid(True)
plt.legend()
plt.xlabel("time")
plt.ylabel("eigvalues")
plt.show()
def get_straight_course2(dl, y_coords, x_coords):
#import pandas as pd
#df = pd.read_csv("route.csv")
#ax = [0.0, -10.0, -20.0, -40.0, -50.0, -60.0, -70.0,-80]
#ay = [0.0, -1.0, 1.0, 0.0, -1.0, 1.0, 0.0,-1.0]
ax = y_coords
ay = x_coords
cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(ax,
ay,
ds=dl)
return cx, cy, cyaw, ck
def main():
print("LQR steering control tracking start!!")
df = pd.read_csv('route.csv', delim_whitespace=True)
ax = df.X
ay = df.Y
#ax = [0.0, 6.0, 12.5, 10.0, 17.5, 20.0, 25.0]
#ay = [0.0, -3.0, -5.0, 6.5, 3.0, 0.0, 0.0]
goal = [df["X"].iloc[-1], df["Y"].iloc[-1]]
cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(ax,
ay,
ds=0.1)
target_speed = 7.2 / 3.6 # simulation parameter km/h -> m/s
sp = calc_speed_profile(cyaw, target_speed)
t, x, y, yaw, v = do_simulation(cx, cy, cyaw, ck, sp, goal)
if show_animation: # pragma: no cover
plt.close()
plt.subplots(1)
plt.plot(ax, ay, "xb", label="waypoints")
plt.plot(cx, cy, "-r", label="target course")
plt.plot(x, y, "-g", label="tracking")
plt.grid(True)
plt.axis("equal")
plt.xlabel("x[m]")
plt.ylabel("y[m]")
plt.legend()
plt.subplots(1)
#plt.plot(s, [np.rad2deg(iyaw) for iyaw in cyaw], "-r", label="yaw")
plt.plot(t, np.rad2deg(yaw), "-r", label="yaw")
plt.grid(True)
plt.legend()
plt.xlabel("Time [s]")
plt.ylabel("Yaw angle[deg]")
plt.subplots(1)
plt.plot(s, ck, "-r", label="curvature")
plt.grid(True)
plt.legend()
plt.xlabel("line length[m]")
plt.ylabel("curvature [1/m]")
plt.subplots()
plt.plot(t, v, "-r", label="speed")
plt.grid(True)
plt.xlabel("Time [s]")
plt.ylabel("Speed [kmh]")
plt.show()
def main():
print("LQR steering control tracking start!!")
ax = [0.0, 6.0, 12.5, 10.0, 10.5, 3.0, -1.0]
ay = [0.0, -3.0, -5.0, 6.5, 3.0, 10.0, -2.0]
goal = [ax[-1], ay[-1]]
cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(ax,
ay,
ds=0.1)
target_speed = 10.0 / 3.6 # simulation parameter km/h -> m/s
sp = calc_speed_profile(cx, cy, cyaw, target_speed)
t, x, y, yaw, v = closed_loop_prediction(cx, cy, cyaw, ck, sp, goal)
def get_circular_course(dl):
x = np.array([0])
y = np.array([0])
theta = np.array([0])
for t in np.linspace(0, 2 * math.pi, 50):
x = np.append(x, 5 * np.sin(t))
y = np.append(y, 5 * np.sin(t + (math.pi / 2)))
theta = np.append(theta, t)
ax = x[1:]
ay = y[1:]
cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(ax,
ay,
ds=dl)
return cx, cy, cyaw, ck
def main():
print("rear wheel feedback tracking start!!")
#ax = [0.0, 6.0, 12.5, 5.0, 7.5, 3.0, -1.0]
#ay = [0.0, 0.0, 5.0, 6.5, 3.0, 5.0, -2.0]
ax = [0.0, 100.0, 100.0, 50.0, 60.0]
ay = [0.0, 0.0, -30.0, -20.0, 0.0]
goal = [ax[-1], ay[-1]]
cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(
ax, ay, ds=0.1)
target_speed = 10.0 / 3.6
sp = calc_speed_profile(cx, cy, cyaw, target_speed)
t, x, y, yaw, v, goal_flag = closed_loop_prediction(
cx, cy, cyaw, ck, sp, goal)
# Test
assert goal_flag, "Cannot goal"
if show_animation: # pragma: no cover
plt.close()
plt.subplots(1)
plt.plot(ax, ay, "xb", label="input")
plt.plot(cx, cy, "-r", label="spline")
plt.plot(x, y, "-g", label="tracking")
plt.grid(True)
plt.axis("equal")
plt.xlabel("x[m]")
plt.ylabel("y[m]")
plt.legend()
plt.subplots(1)
plt.plot(s, [np.rad2deg(iyaw) for iyaw in cyaw], "-r", label="yaw")
plt.grid(True)
plt.legend()
plt.xlabel("line length[m]")
plt.ylabel("yaw angle[deg]")
plt.subplots(1)
plt.plot(s, ck, "-r", label="curvature")
plt.grid(True)
plt.legend()
plt.xlabel("line length[m]")
plt.ylabel("curvature [1/m]")
plt.show()
def cars_planners_setup(cord_cars_xy_cords):
"""
"""
car_num = len(cord_cars_xy_cords)
cars_plannars = {}
for i_c in range(car_num):
cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(
cord_cars_xy_cords[i_c][0], cord_cars_xy_cords[i_c][1], ds=0.1)
target_speed = 3 # simulation parameter km/h -> m/s
sp = lqr_speed_steer_control.calc_speed_profile(cyaw, target_speed)
cars_plannars[i_c] = Planner(cx, cy, cyaw, ck, s, sp)
return cars_plannars
def main():
ax = [0.0, 6.0, 12.5, 10.0, 7.5, 3.0, -1.0]
ay = [0.0, -3.0, -5.0, 6.5, 3.0, 5.0, -2.0]
goal = [ax[-1], ay[-1]]
cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(ax, ay, ds=0.1)
veh_state = VehicleState(x=0.5, y=0.0, yaw=0.0, v=0.0)
e, th_e = 0.0, 0.0
steering_lqr_controller = SteeringLQR()
dl, target_ind, e, th_e = steering_lqr_controller.lqr_steering_control(veh_state,
cx, cy, cyaw, ck, e, th_e)
print(np.rad2deg(dl))
def main():
print("LQR steering control tracking start!!")
ax = [7.5, 12.836819813353078, 58]
#
#
ay = [2, 26.423725233465163, 38.0]
goal = [ax[-1], ay[-1]]
cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(ax,
ay,
ds=0.1)
target_speed = 20.0 / 3.6 # simulation parameter km/h -> m/s
sp = calc_speed_profile(cx, cy, cyaw, target_speed)
t, x, y, yaw, v = closed_loop_prediction(cx, cy, cyaw, ck, sp, goal, ax[0],
ay[0])
if show_animation: # pragma: no cover
plt.close()
plt.subplots(1)
plt.plot(ax, ay, "xb", label="input")
plt.plot(cx, cy, "-r", label="spline")
plt.plot(x, y, "-g", label="tracking")
plt.grid(True)
plt.axis("equal")
plt.xlabel("x[m]")
plt.ylabel("y[m]")
plt.legend()
plt.subplots(1)
plt.plot(s, [np.rad2deg(iyaw) for iyaw in cyaw], "-r", label="yaw")
plt.grid(True)
plt.legend()
plt.xlabel("line length[m]")
plt.ylabel("yaw angle[deg]")
plt.subplots(1)
plt.plot(s, ck, "-r", label="curvature")
plt.grid(True)
plt.legend()
plt.xlabel("line length[m]")
plt.ylabel("curvature [1/m]")
plt.show()
def main():
print(__file__ + " start!!")
# Chose course by global variable
if LANE_CHANGE:
ax, ay, course_name = getLaneChange()
elif FIGURE_EIGHT:
ax, ay, course_name = getFigureEight()
elif ROAD:
ax, ay, course_name = getRoadPath()
cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(ax, ay, ds=DL)
sp = calc_speed_profile(cx, cy, cyaw, TARGET_SPEED)
initial_state = State(x=cx[0], y=0.5, yaw=cyaw[0], v=TARGET_SPEED)
t, x, y, yaw, v, d, a, station, e_ct_arr = do_simulation(
cx, cy, cyaw, ck, sp, initial_state, course_name)
if SHOW_PLOTS: # pragma: no cover
# plt.close("all")
plt.figure(1)
plt.plot(cx, cy, "-r", label="spline")
# plt.scatter(cx, cy, "r", s=0.9, label="spline")
plt.plot(x, y, "-g", label="tracking")
plt.grid(True)
plt.axis("equal")
plt.xlabel("x[m]")
plt.ylabel("y[m]")
plt.legend()
# plt.subplots()
# plt.plot(t, v, "-r", label="speed")
# plt.grid(True)
# plt.xlabel("Time [s]")
# plt.ylabel("Speed [kmh]")
# Plot relevant errors
plt.figure(0)
plt.plot(station, e_ct_arr, c=COLOR, label="Q={}".format(Q[0, 0]))
plt.xlabel("Station[m]")
plt.ylabel("Cross-Track Error[m]")
plt.title("MPC at {}m/s on {}".format(TARGET_SPEED, course_name))
plt.grid(True)
def main():
print("rear wheel feedback tracking start!!")
ax = [0.0, 6.0, 12.5, 5.0, 7.5, 3.0, -1.0]
ay = [0.0, 0.0, 5.0, 6.5, 3.0, 5.0, -2.0]
goal = [ax[-1], ay[-1]]
cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(
ax, ay, ds=0.1)
target_speed = 10.0 / 3.6
sp = calc_speed_profile(cx, cy, cyaw, target_speed)
t, x, y, yaw, v, goal_flag = closed_loop_prediction(
cx, cy, cyaw, ck, sp, goal)
# Test
assert goal_flag, "Cannot goal"
if show_animation: # pragma: no cover
plt.close()
plt.subplots(1)
plt.plot(ax, ay, "xb", label="input")
plt.plot(cx, cy, "-r", label="spline")
plt.plot(x, y, "-g", label="tracking")
plt.grid(True)
plt.axis("equal")
plt.xlabel("x[m]")
plt.ylabel("y[m]")
plt.legend()
plt.subplots(1)
plt.plot(s, [np.rad2deg(iyaw) for iyaw in cyaw], "-r", label="yaw")
plt.grid(True)
plt.legend()
plt.xlabel("line length[m]")
plt.ylabel("yaw angle[deg]")
plt.subplots(1)
plt.plot(s, ck, "-r", label="curvature")
plt.grid(True)
plt.legend()
plt.xlabel("line length[m]")
plt.ylabel("curvature [1/m]")
plt.show()
def main():
print("LQR steering control tracking start!!")
#ax = [0.0, 6.0, 12.5, 10.0, 7.5, 3.0, -1.0]
#ay = [0.0, -3.0, -5.0, 6.5, 3.0, 5.0, -2.0]
ax = [4.368, -0.2, -3.564, -4.310]
ay = [0, 1.189, 5.362, 9.536]
goal = [ax[-1], ay[-1]]
cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(ax,
ay,
ds=0.1)
target_speed = 3 # simulation parameter km/h -> m/s
sp = calc_speed_profile(cx, cy, cyaw, target_speed)
t, x, y, yaw, v = closed_loop_prediction(cx, cy, cyaw, ck, sp, goal)
if show_animation: # pragma: no cover
plt.close()
plt.subplots(1)
#plt.plot(ax, ay, "xb", label="input")
plt.plot(cx, cy, "-r", label="reference path")
plt.plot(x, y, "-g", label="tracking")
plt.grid(False)
plt.axis("equal")
plt.xlabel("x[m]")
plt.ylabel("y[m]")
#plt.legend()
plt.savefig('path.png', transparent=True)
plt.subplots(1)
plt.plot(s, [np.rad2deg(iyaw) for iyaw in cyaw], "-r", label="yaw")
plt.grid(True)
plt.legend()
plt.xlabel("line length[m]")
plt.ylabel("yaw angle[deg]")
plt.subplots(1)
plt.plot(s, ck, "-r", label="curvature")
plt.grid(True)
plt.legend()
plt.xlabel("line length[m]")
plt.ylabel("curvature [1/m]")
plt.show()
def generate_path(is_default=True):
x_path = []
y_path = []
if is_default == True:
path_obj = Path()
x_raw, y_raw = path_obj.default_raw_path()
x_refacto, y_refacto = path_obj.default_refactor()
path_properties = path_obj.path_props(x_refacto, y_refacto)
x_path, y_path = path_obj.remove_singular(x_refacto, y_refacto,
path_properties)
waypoints_x = x_path
waypoints_y = y_path
final_goal = path_properties[1]
course_x, course_y, course_yaw, course_k, course_s = cubic_spline_planner.calc_spline_course(
waypoints_x, waypoints_y, ds=0.1)
else:
print "No Other than default"
return x_path, y_path, course_x, course_y, course_yaw, course_k, course_s, final_goal
def main():
show_animation = True
print("LQR steering control tracking start!!")
# ax = [0.0, 6.0, 12.5, 10.0, 7.5, 3.0, -1.0]
# ay = [0.0, -3.0, -5.0, 6.5, 3.0, 5.0, -2.0]
ax = [0.0, 100.0, 100.0, 50.0, 60.0]
ay = [0.0, 0.0, -30.0, -20.0, 0.0]
goal = [ax[-1], ay[-1]]
cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(
ax, ay, ds=1.0)
target_speed = 30.0 / 3.6 # simulation parameter km/h -> m/s
sp = calc_speed_profile(cx, cy, cyaw, target_speed)
t, x, y, v, yaw = control(cx, cy, cyaw, ck, sp, goal)
if show_animation:
plt.close()
flg, _ = plt.subplots(1)
plt.plot(ax, ay, "xb", label="input")
plt.plot(cx, cy, "-r", label="spline")
plt.plot(x, y, "-g", label="tracking")
plt.grid(True)
plt.axis("equal")
plt.xlabel("x[m]")
plt.ylabel("y[m]")
plt.legend()
flg, ax = plt.subplots(1)
plt.plot(s, [math.degrees(iyaw) for iyaw in cyaw], "-r", label="yaw")
plt.grid(True)
plt.legend()
plt.xlabel("line length[m]")
plt.ylabel("yaw angle[deg]")
flg, ax = plt.subplots(1)
plt.plot(s, ck, "-r", label="curvature")
plt.grid(True)
plt.legend()
plt.xlabel("line length[m]")
plt.ylabel("curvature [1/m]")
plt.show()
def main():
print("LQR steering control tracking start!!")
ax = [0.0, 6.0, 12.5, 10.0, 17.5, 20.0, 25.0]
ay = [0.0, -3.0, -5.0, 6.5, 3.0, 0.0, 0.0]
goal = [ax[-1], ay[-1]]
cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(ax,
ay,
ds=0.1)
target_speed = 10.0 / 3.6 # simulation parameter km/h -> m/s
sp = calc_speed_profile(cyaw, target_speed)
t, x, y, yaw, v = do_simulation(cx, cy, cyaw, ck, sp, goal)
if show_animation: # pragma: no cover
plt.close()
plt.subplots(1)
plt.plot(ax, ay, "xb", label="waypoints")
plt.plot(cx, cy, "-r", label="target course")
plt.plot(x, y, "-g", label="tracking")
plt.grid(True)
plt.axis("equal")
plt.xlabel("x[m]")
plt.ylabel("y[m]")
plt.legend()
plt.subplots(1)
plt.plot(s, [np.rad2deg(iyaw) for iyaw in cyaw], "-r", label="yaw")
plt.grid(True)
plt.legend()
plt.xlabel("line length[m]")
plt.ylabel("yaw angle[deg]")
plt.subplots(1)
plt.plot(s, ck, "-r", label="curvature")
plt.grid(True)
plt.legend()
plt.xlabel("line length[m]")
plt.ylabel("curvature [1/m]")
plt.show()
def main():
print("LQR steering control tracking start!!")
ax = [0.0, 6.0, 12.5, 10.0, 7.5, 3.0, -1.0]
ay = [0.0, -3.0, -5.0, 6.5, 3.0, 5.0, -2.0]
goal = [ax[-1], ay[-1]]
cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(
ax, ay, ds=0.1)
target_speed = 10.0 / 3.6 # simulation parameter km/h -> m/s
sp = calc_speed_profile(cx, cy, cyaw, target_speed)
t, x, y, yaw, v = closed_loop_prediction(cx, cy, cyaw, ck, sp, goal)
if show_animation: # pragma: no cover
plt.close()
plt.subplots(1)
plt.plot(ax, ay, "xb", label="input")
plt.plot(cx, cy, "-r", label="spline")
plt.plot(x, y, "-g", label="tracking")
plt.grid(True)
plt.axis("equal")
plt.xlabel("x[m]")
plt.ylabel("y[m]")
plt.legend()
plt.subplots(1)
plt.plot(s, [np.rad2deg(iyaw) for iyaw in cyaw], "-r", label="yaw")
plt.grid(True)
plt.legend()
plt.xlabel("line length[m]")
plt.ylabel("yaw angle[deg]")
plt.subplots(1)
plt.plot(s, ck, "-r", label="curvature")
plt.grid(True)
plt.legend()
plt.xlabel("line length[m]")
plt.ylabel("curvature [1/m]")
plt.show()
def closed_loop_prediction(ax, ay):
cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(ax, ay, ds=0.1)
rospy.init_node("path_tracking")
listener = tf.TransformListener()
listener.waitForTransform('/map', '/base_link', rospy.Time(), rospy.Duration(20.0))
pub = rospy.Publisher('drive_parameters', drive_param, queue_size=10)
index = 0
pe = 0
pth_e = 0
gazebo_pose = Pose_listener()
while not rospy.is_shutdown():
t = listener.getLatestCommonTime('/map', '/base_link')
if listener.canTransform('/map', '/base_link', t):
position, quaternion = listener.lookupTransform('/map', '/base_link', t)
rpy = tf.transformations.euler_from_quaternion(quaternion)
x = position[0]
y = position[1]
yaw = rpy[2]
#x = gazebo_pose.pose.x
#y = gazebo_pose.pose.y
#yaw = gazebo_pose.pose.theta
delta, e, th_e, index = lqr_steering_control(x, y, yaw, cx, cy, cyaw, ck, pe, pth_e)
if delta > max_steer:
delta = max_steer
elif delta < -max_steer:
delta = -max_steer
pe = e
pth_e = th_e
msg = drive_param()
if np.linalg.norm([x - waypoints[-1][0], y - waypoints[-1][1]]) < 0.3:
msg.velocity = 0
rospy.signal_shutdown("the car has reach the goal")
else:
msg.velocity = velocity
msg.angle = -delta
pub.publish(msg)
def set_path(self, pose, vel, ax=[0, 1], ay=[0, 1]):
self.state.update(pose.x, pose.y, pose.theta, vel)
self.start_th = pose.theta
self.cx, self.cy, self.cyaw, ck, s = cubic_spline_planner.calc_spline_course(
ax, ay, ds=0.1)
# target course
# self.cx = ax
# self.cy = ay
# self.cx.append(self.cx[len(self.cx)-1])
# self.cy.append(self.cy[len(self.cy)-1])
# self.cyaw.append(self.cyaw[len(self.cyaw)-1]
self.last_idx = len(self.cx) - 1
# target course
self.target_course = TargetCourse(self.cx, self.cy)
self.target_idx, _ = self.target_course.search_target_index(
self.state, self.k, self.linear_tolerance_outer, self.look_ahead)
def __init__(self, initialState):
self.k = 0.5 # control gain
self.Kp = 1.0 # speed proportional gain
self.dt = 0.1 # [s] time difference
self.L = 0.29 # [m] Wheel base of vehicle
self.max_steer = np.radians(30.0) # [rad] max steering angle
self.show_animation = True
gpsMath = GpsMath()
ax = [4300738.591950053, 4300735.762529756, 4300731.881422988, 4300725.061867644, 4300716.428532064]
ay = [687530.1153441871, 687527.1996543248, 687525.1935662497, 687519.4075912425, 687510.02154688]
ax = list(map(lambda x: gpsMath.locationToPoint(x, 9.083533, 480)["x"], [47.031944, 47.031745, 47.031807, 47.031965, 47.031944]))
ay = list(map(lambda x: gpsMath.locationToPoint(47.031944, x, 480)["y"], [9.083533, 9.083629, 9.083816, 9.083692, 9.083533]))
#ax = [4300796.650611158, 4300812.637012742, 4300812.637012742, 4300794.963600908, 4300796.650611158]
#ay = [687608.2953105057, 687615.5013634935, 687615.5013634935, 687620.2303347178, 687608.2953105057]
print(ax)
print(ay)
ds = 0.1
self.cx, self.cy, self.cyaw, self.ck, self.s = cubic_spline_planner.calc_spline_course(
ax, ay, ds=ds)
self.target_speed = 3.0 / 3.6 # [m/s]
self.max_simulation_time = 100.0
# Initial state
self.state = initialState
self.last_idx = len(self.cx) - 1
self.target_idx, _ = self.calc_target_index(self.state, self.cx, self.cy)
self.time = 0.0
self.x = [self.state.x]
self.y = [self.state.y]
self.yaw = [self.state.yaw]
self.v = [self.state.v]
self.t = [0.0]
self.LOG.info("on {}/{}".format(self.x, self.y))
def generate_path(is_default=True):
x_path = []
y_path = []
# csp = imp.load_source('cubic_spline_planner.py', '/home/oks/catkin_ws/src/framework_sim/clrn_gplanner/scripts/cubic_spline_planner.py')
if is_default == True:
path_obj = Path()
x_raw, y_raw = path_obj.default_raw_path()
path_properties = path_obj.path_props(x_raw, y_raw)
x_path, y_path = path_obj.remove_singular(x_raw, y_raw,
path_properties)
x_refacto, y_refacto = path_obj.remove_initpoint(x_path, y_path)
path_properties = path_obj.path_props(x_refacto, y_refacto)
waypoints_x = x_path
waypoints_y = y_path
final_goal = path_properties[1]
course_x, course_y, course_yaw, course_k, course_s = csp.calc_spline_course(
waypoints_x, waypoints_y, ds=0.1)
ref_speed = path_obj.get_ref_speed()
speed_profile = calc_speed_profile(course_x, course_y, course_yaw,
ref_speed)
else:
print "No Other than default"
return x_path, y_path, course_x, course_y, course_yaw, course_k, course_s, final_goal, speed_profile
def main():
ax = [0.0, 100.0, 100.0, 50.0, 60.0]
ay = [0.0, 0.0, -30.0, -20.0, 0.0]
cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(ax,
ay,
ds=0.1)
target_speed = 30.0 / 3.6 # [m/s]
max_simulation_time = 100.0
# Initial state
state = State(x=-0.0, y=5.0, yaw=np.radians(0.0), v=2.0)
last_idx = len(cx) - 1
time = 0.0
x = [state.x]
y = [state.y]
yaw = [state.yaw]
v = [state.v]
t = [0.0]
target_idx, _ = calc_target_index(state, cx, cy)
show_animation = True
dt = 0.1
while max_simulation_time >= time and last_idx > target_idx:
ai = longitude_control(target_speed, state.v)
di, target_idx = latitude_control(state, cx, cy, cyaw, target_idx)
state.update(ai, di)
time += dt
x.append(state.x)
y.append(state.y)
yaw.append(state.yaw)
v.append(state.v)
t.append(time)
if show_animation:
plt.cla()
plt.plot(cx, cy, ".r", label="course")
plt.plot(x, y, "-b", label="trajectory")
plt.plot(cx[target_idx], cy[target_idx], "xg", label="target")
plt.axis("equal")
plt.grid(True)
plt.title("Speed[km/h]:" + str(state.v * 3.6)[:4])
plt.pause(0.001)
# Test
assert last_idx >= target_idx, "Cannot reach goal"
if show_animation:
plt.plot(cx, cy, ".r", label="course")
plt.plot(x, y, "-b", label="trajectory")
plt.legend()
plt.xlabel("x[m]")
plt.ylabel("y[m]")
plt.axis("equal")
plt.grid(True)
flg, ax = plt.subplots(1)
plt.plot(t, [iv * 3.6 for iv in v], "-r")
plt.xlabel("Time[s]")
plt.ylabel("Speed[km/h]")
plt.grid(True)
plt.show()
cv2.circle(traj, (draw_kalman_x, draw_kalman_y), 1, (255, 255, 255), 1)
cv2.rectangle(traj, (10, 20), (600, 60), (0, 0, 0), -1)
text = "Coordinates: x=%2fm y=%2fm z=%2fm" % (x, y, z)
cv2.putText(traj, text, (20, 40), cv2.FONT_HERSHEY_PLAIN, 1,
(255, 255, 255), 1, 8)
cv2.imshow('Road facing camera', img)
cv2.imshow('Trajectory', traj)
cv2.waitKey(1)
print(kalman_y.x)
if counter > N_STEPS:
break
counter += 1
trajectory = np.array(trajectory)
cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(
list(trajectory[:, 0][::2]), list(trajectory[:, 1][::2]), ds=0.1)
plt.scatter(trajectory[:, 0], trajectory[:, 1], label='Filtered', s=0.5)
plt.scatter(trajectory[:, 2], trajectory[:, 3], label='Bicycle Model', s=0.5)
plt.scatter(trajectory[:, 4], trajectory[:, 5], label='GNSS', s=0.5)
plt.scatter(trajectory[:, 6],
trajectory[:, 7],
label='Visual Odometery',
s=0.5)
plt.scatter(cx, cy, label='Spline', s=0.5)
plt.xticks([-3, -2, -1, 0, 1, 2, 3])
plt.legend(loc='best')
plt.show()
def get_Switch_back_course2(dl):
ax = [24.899999618530273, 24.900005340576172, 24.90001106262207,\
24.900014877319336, 24.900020599365234, 24.900026321411133,\
24.90003204345703, 24.89870262145996, 24.889429092407227,\
24.864276885986328, 24.815370559692383, 24.735021591186523,\
24.615943908691406, 24.29094886779785, 23.830982208251953,\
23.252336502075195, 22.575511932373047, 21.824485778808594,\
21.385210037231445, 20.937171936035156, 20.484573364257812,\
20.03000259399414, 19.574806213378906, 19.1195068359375,\
18.233142852783203, 17.346778869628906, 16.46041488647461,\
15.574051856994629, 14.687687873840332, 13.801324844360352,\
12.914960861206055, 12.028596878051758, 11.142233848571777,\
10.25586986541748, 9.369505882263184, 8.93588638305664,\
8.50233268737793, 8.069173812866211, 7.637269020080566,\
7.208261489868164, 6.784831523895264, 6.3709259033203125,\
6.020498275756836, 5.684054851531982, 5.282763481140137,\
4.910910606384277, 4.570590019226074, 4.262977123260498,\
3.9884414672851562, 3.7466609477996826, 3.5367300510406494,\
3.357264518737793, 3.2064950466156006, 3.082350254058838,\
2.9825286865234375, 2.9045591354370117, 2.8458502292633057,\
2.803729772567749, 2.775472640991211, 2.75832462310791,\
2.7495148181915283, 2.7462656497955322, 2.7457969188690186,\
2.745792865753174, 2.745788812637329, 2.7457847595214844,\
2.747112989425659, 2.7563867568969727, 2.7815396785736084,\
2.8304450511932373, 2.910794734954834, 3.0298728942871094,\
3.354867458343506, 3.8148345947265625, 4.393479824066162,\
5.0703043937683105, 5.82133150100708, 6.260606288909912,\
6.708644866943359, 7.161242961883545, 7.615814685821533,\
8.07101058959961, 8.526309967041016, 9.351309776306152,\
10.176310539245605, 11.001310348510742, 11.826310157775879,\
12.651309967041016, 13.476309776306152, 14.411310195922852,\
15.34631061553955, 16.28131103515625, 17.216310501098633,\
18.151309967041016, 19.0863094329834, 20.021310806274414,\
20.956310272216797, 21.89130973815918, 22.826311111450195]
ay = [14.199999809265137, 15.033333778381348, 15.866666793823242,\
16.700000762939453, 17.53333282470703, 18.366666793823242,\
19.200000762939453, 19.65530014038086, 20.110496520996094,\
20.565067291259766, 21.01766586303711, 21.46570587158203,\
21.90498161315918, 22.65601348876953, 23.332843780517578,\
23.911497116088867, 24.371471405029297, 24.696475982666016,\
24.81555938720703, 24.895915985107422, 24.944826126098633,\
24.969985961914062, 24.979265213012695, 24.98059844970703,\
24.98060417175293, 24.980609893798828, 24.980615615844727,\
24.980621337890625, 24.980627059936523, 24.980632781982422,\
24.98063850402832, 24.98064422607422, 24.980648040771484,\
24.980653762817383, 24.98065948486328, 24.97962760925293,\
24.9724178314209, 24.95285987854004, 24.914812088012695,\
24.852230072021484, 24.75927734375, 24.630525588989258,\
24.485376358032227, 24.310237884521484, 24.048620223999023,\
23.74658203125, 23.409381866455078, 23.042070388793945,\
22.649396896362305, 22.235727310180664, 21.80500030517578,\
21.36069679260254, 20.905839920043945, 20.442995071411133,\
19.974287033081055, 19.50144386291504, 19.025814056396484,\
18.54842185974121, 18.070003509521484, 17.59105110168457,\
17.11186981201172, 16.63261604309082, 16.153350830078125,\
15.52001667022705, 14.886683464050293, 14.253350257873535,\
13.798049926757812, 13.342854499816895, 12.88828182220459,\
12.435683250427246, 11.98764419555664, 11.548367500305176,\
10.79733657836914, 10.120506286621094, 9.541853904724121,\
9.081877708435059, 8.756874084472656, 8.637789726257324,\
8.557435035705566, 8.508523941040039, 8.483365058898926,\
8.474085807800293, 8.47275161743164, 8.472745895385742,\
8.47274112701416, 8.472735404968262, 8.47273063659668,\
8.472725868225098, 8.4727201461792, 8.4727144241333,\
8.472708702087402, 8.472702026367188, 8.472696304321289,\
8.47269058227539, 8.472684860229492, 8.472679138183594,\
8.472672462463379, 8.47266674041748, 8.472661018371582]
cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(ax,
ay,
ds=dl)
ax = [22.826311111450195, 22.418312072753906, 22.010366439819336,\
21.602720260620117, 21.196029663085938, 20.791549682617188,\
20.39132308959961, 19.998374938964844, 19.213205337524414,\
18.5056095123291, 17.90065574645996, 17.419771194458008,\
17.079994201660156, 16.961217880249023, 16.878517150878906,\
16.82509994506836, 16.793806076049805, 16.77730941772461,\
16.768220901489258, 16.762981414794922, 16.75774383544922]
ay = [8.472661018371582, 8.471787452697754, 8.465682029724121,\
8.449118614196777, 8.416891098022461, 8.363858222961426,\
8.285024642944336, 8.175670623779297, 7.835904121398926,\
7.355029106140137, 6.750082015991211, 6.042492389678955,\
5.257328033447266, 4.823906421661377, 4.38212776184082,\
3.9358205795288086, 3.4873881340026855, 3.0381479263305664,\
2.58868670463562, 2.2887322902679443, 1.9887781143188477]
cx2, cy2, cyaw2, ck2, s2 = cubic_spline_planner.calc_spline_course(ax,
ay,
ds=dl)
cyaw2 = [i - math.pi for i in cyaw2]
cx.extend(cx2)
cy.extend(cy2)
cyaw.extend(cyaw2)
ck.extend(ck2)
return cx, cy, cyaw, ck
def main():
"""Plot an example of Stanley steering control on a cubic spline."""
# target course
ax = [0.0, 100.0, 100.0, 50.0, 60.0]
ay = [0.0, 0.0, -30.0, -20.0, 0.0]
cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(ax,
ay,
ds=0.1)
target_speed = 30.0 / 3.6 # [m/s]
max_simulation_time = 100.0
# Initial state
state = State(x=-0.0, y=5.0, yaw=np.radians(20.0), v=0.0)
last_idx = len(cx) - 1
time = 0.0
x = [state.x]
y = [state.y]
yaw = [state.yaw]
v = [state.v]
t = [0.0]
target_idx, _ = calc_target_index(state, cx, cy)
while max_simulation_time >= time and last_idx > target_idx:
ai = pid_control(target_speed, state.v)
di, target_idx = stanley_control(state, cx, cy, cyaw, target_idx)
state.update(ai, di)
time += dt
x.append(state.x)
y.append(state.y)
yaw.append(state.yaw)
v.append(state.v)
t.append(time)
if show_animation: # pragma: no cover
plt.cla()
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
plt.plot(cx, cy, ".r", label="course")
plt.plot(x, y, "-b", label="trajectory")
plt.plot(cx[target_idx], cy[target_idx], "xg", label="target")
plt.axis("equal")
plt.grid(True)
plt.title("Speed[km/h]:" + str(state.v * 3.6)[:4])
plt.pause(0.001)
# Test
assert last_idx >= target_idx, "Cannot reach goal"
if show_animation: # pragma: no cover
plt.plot(cx, cy, ".r", label="course")
plt.plot(x, y, "-b", label="trajectory")
plt.legend()
plt.xlabel("x[m]")
plt.ylabel("y[m]")
plt.axis("equal")
plt.grid(True)
plt.subplots(1)
plt.plot(t, [iv * 3.6 for iv in v], "-r")
plt.xlabel("Time[s]")
plt.ylabel("Speed[km/h]")
plt.grid(True)
plt.show()
def main():
"""Plot an example of Stanley steering control on a cubic spline."""
# target course
ax = [0.0, 100.0, 100.0, 50.0, 60.0]
ay = [0.0, 0.0, -30.0, -20.0, 0.0]
cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(
ax, ay, ds=0.1)
target_speed = 30.0 / 3.6 # [m/s]
max_simulation_time = 100.0
# Initial state
state = State(x=-0.0, y=5.0, yaw=np.radians(20.0), v=0.0)
last_idx = len(cx) - 1
time = 0.0
x = [state.x]
y = [state.y]
yaw = [state.yaw]
v = [state.v]
t = [0.0]
target_idx, _ = calc_target_index(state, cx, cy)
while max_simulation_time >= time and last_idx > target_idx:
ai = pid_control(target_speed, state.v)
di, target_idx = stanley_control(state, cx, cy, cyaw, target_idx)
state.update(ai, di)
time += dt
x.append(state.x)
y.append(state.y)
yaw.append(state.yaw)
v.append(state.v)
t.append(time)
if show_animation: # pragma: no cover
plt.cla()
plt.plot(cx, cy, ".r", label="course")
plt.plot(x, y, "-b", label="trajectory")
plt.plot(cx[target_idx], cy[target_idx], "xg", label="target")
plt.axis("equal")
plt.grid(True)
plt.title("Speed[km/h]:" + str(state.v * 3.6)[:4])
plt.pause(0.001)
# Test
assert last_idx >= target_idx, "Cannot reach goal"
if show_animation: # pragma: no cover
plt.plot(cx, cy, ".r", label="course")
plt.plot(x, y, "-b", label="trajectory")
plt.legend()
plt.xlabel("x[m]")
plt.ylabel("y[m]")
plt.axis("equal")
plt.grid(True)
plt.subplots(1)
plt.plot(t, [iv * 3.6 for iv in v], "-r")
plt.xlabel("Time[s]")
plt.ylabel("Speed[km/h]")
plt.grid(True)
plt.show()