def generate_path(states, k0):
# x, y, yaw, s, km, kf
lookup_table = get_lookup_table()
result = []
for state in states:
bestp = search_nearest_one_from_lookuptable(state[0], state[1],
state[2], lookup_table)
target = motion_model.State(x=state[0], y=state[1], yaw=state[2])
init_p = np.matrix(
[math.sqrt(state[0]**2 + state[1]**2), bestp[4], bestp[5]]).T
x, y, yaw, p = planner.optimize_trajectory(target, k0, init_p)
if x is not None:
print("find good path")
result.append(
[x[-1], y[-1], yaw[-1],
float(p[0]),
float(p[1]),
float(p[2])])
print("finish path generation")
return result
def generate_path(target_states, k0):
# x, y, yaw, s, km, kf
lookup_table = get_lookup_table()
result = []
for state in target_states:
bestp = search_nearest_one_from_lookuptable(state[0], state[1],
state[2], lookup_table)
print("x,y,yaw,s,km,kf")
print("bestp : ", bestp)
target = motion_model.State(x=state[0], y=state[1],
yaw=state[2]) # State class constructor
init_p = np.array(
[math.sqrt(state[0]**2 + state[1]**2), bestp[4],
bestp[5]]).reshape(3, 1) # 거리, s, km
print("init_p : ", init_p)
x, y, yaw, p = planner.optimize_trajectory(target, k0, init_p)
if x is not None:
print("find good path")
result.append(
[x[-1], y[-1], yaw[-1],
float(p[0]),
float(p[1]),
float(p[2])])
print("finish path generation")
return result
def generate_lookup_table():
states = calc_states_list()
k0 = 0.0
# x, y, yaw, s, km, kf
lookuptable = [[1.0, 0.0, 0.0, 1.0, 0.0, 0.0]]
for state in states:
bestp = search_nearest_one_from_lookuptable(state[0], state[1],
state[2], lookuptable)
target = motion_model.State(x=state[0], y=state[1], yaw=state[2])
init_p = np.matrix(
[math.sqrt(state[0]**2 + state[1]**2), bestp[4], bestp[5]]).T
print("bestp", bestp)
print("initp", init_p)
if init_p[0] == 0.0:
print("hello")
init_p[0] = 0.1
cur_states = [0, 0, 0, 0]
x, y, yaw, p = planner.optimize_trajectory(cur_states, target, k0,
init_p)
if x is not None:
print("find good path")
lookuptable.append(
[x[-1], y[-1], yaw[-1],
float(p[0]),
float(p[1]),
float(p[2])])
print("finish lookup table generation")
save_lookup_table("lookuptables.csv", lookuptable)
# print("table[3]", table[4])
# print("table[4]", table[4])
# print("table[5]", table[5])
for table in lookuptable:
if table[3] != 0:
xc, yc, yawc = motion_model.generate_trajectory(
cur_states, table[3], table[4], table[5], k0)
plt.plot(xc, yc, "-r")
# xc, yc, yawc = motion_model.generate_trajectory(cur_states,
# table[3], -table[4], -table[5], k0)
# plt.plot(xc, yc, "-r")
plt.grid(True)
plt.axis("equal")
plt.show()
print("Done")
def generate_lookup_table():
states = calc_states_list() # calculate target states
k0 = 0.0
# target_x, target_y, target_yaw -> s, km, kf
lookuptable = [[1.0, 0.0, 0.0, 1.0, 0.0, 0.0]]
print("Lookup Table is being generated......")
for idx, state in enumerate(states):
bestp = search_nearest_one_from_lookuptable(state[0], state[1],
state[2], lookuptable)
target = motion_model.State(x=state[0], y=state[1], yaw=state[2])
# init_p = np.matrix(
# [math.sqrt(state[0] ** 2 + state[1] ** 2), bestp[4], bestp[5]]).T
init_p = np.matrix([bestp[3], bestp[4], bestp[5]]).T
x, y, yaw, p = planner.optimize_trajectory(target, k0, init_p)
if x is not None:
print("find good path")
lookuptable.append(
[x[-1], y[-1], yaw[-1],
float(p[0]),
float(p[1]),
float(p[2])])
## print percent of progress
percent = 100 * (idx + 1.0) / len(states)
print("Complete %{}...".format(percent))
print("finish lookup table generation")
save_lookup_table("lookuptable.csv", lookuptable)
for table in lookuptable:
xc, yc, yawc = motion_model.generate_trajectory(
table[3], table[4], table[5], k0)
plt.plot(xc, yc, "-r")
xc, yc, yawc = motion_model.generate_trajectory(
table[3], -table[4], -table[5], k0
) # symmetrical, this is why target_yaw inlcude only -30 degree and exclude +30 degree
# similar for target_y, note that not for target_x
# also note that here change the sign of steering make the sign of target_yaw
# and target_y change at the same time
plt.plot(xc, yc, "-r")
plt.grid(True)
plt.axis("equal")
plt.show()
print("Done")
def generate_lookup_table():
states = calc_states_list()
k0 = 0.0
# x, y, yaw, s, km, kf
lookuptable = [[1.0, 0.0, 0.0, 1.0, 0.0, 0.0]]
for state in states:
bestp = search_nearest_one_from_lookuptable(state[0], state[1],
state[2], lookuptable)
target = motion_model.State(x=state[0], y=state[1], yaw=state[2])
init_p = np.array(
[math.sqrt(state[0]**2 + state[1]**2), bestp[4],
bestp[5]]).reshape(3, 1)
x, y, yaw, p = planner.optimize_trajectory(target, k0, init_p)
if x is not None:
print("find good path")
lookuptable.append(
[x[-1], y[-1], yaw[-1],
float(p[0]),
float(p[1]),
float(p[2])])
print("finish lookup table generation")
save_lookup_table("lookuptable.csv", lookuptable)
for table in lookuptable:
xc, yc, yawc = motion_model.generate_trajectory(
table[3], table[4], table[5], k0)
plt.plot(xc, yc, "-r")
xc, yc, yawc = motion_model.generate_trajectory(
table[3], -table[4], -table[5], k0)
plt.plot(xc, yc, "-r")
plt.grid(True)
plt.axis("equal")
plt.show()
print("Done")
def generate_lookup_table():
states = calc_states_list()
k0 = 0.0
# x, y, yaw, s, km, kf
lookuptable = [[1.0, 0.0, 0.0, 1.0, 0.0, 0.0]]
for state in states:
bestp = search_nearest_one_from_lookuptable(
state[0], state[1], state[2], lookuptable)
target = motion_model.State(x=state[0], y=state[1], yaw=state[2])
init_p = np.array(
[math.sqrt(state[0] ** 2 + state[1] ** 2), bestp[4], bestp[5]]).reshape(3, 1)
x, y, yaw, p = planner.optimize_trajectory(target, k0, init_p)
if x is not None:
print("find good path")
lookuptable.append(
[x[-1], y[-1], yaw[-1], float(p[0]), float(p[1]), float(p[2])])
print("finish lookup table generation")
save_lookup_table("lookuptable.csv", lookuptable)
for table in lookuptable:
xc, yc, yawc = motion_model.generate_trajectory(
table[3], table[4], table[5], k0)
plt.plot(xc, yc, "-r")
xc, yc, yawc = motion_model.generate_trajectory(
table[3], -table[4], -table[5], k0)
plt.plot(xc, yc, "-r")
plt.grid(True)
plt.axis("equal")
plt.show()
print("Done")
def generate_path(target_states, k0):
# x, y, yaw, s, km, kf
lookup_table = get_lookup_table()
result = []
for state in target_states:
bestp = search_nearest_one_from_lookuptable(
state[0], state[1], state[2], lookup_table)
target = motion_model.State(x=state[0], y=state[1], yaw=state[2])
init_p = np.matrix(
[math.sqrt(state[0] ** 2 + state[1] ** 2), bestp[4], bestp[5]]).T
x, y, yaw, p = planner.optimize_trajectory(target, k0, init_p)
if x is not None:
print("find good path")
result.append(
[x[-1], y[-1], yaw[-1], float(p[0]), float(p[1]), float(p[2])])
print("finish path generation")
return result