def sampling_paths_for_Stopping(l0, l0_v, l0_a, s0, s0_v, s0_a, ref_path):
PATHS = dict()
for s1_v in [-1.0, 0.0, 1.0, 2.0]:
for t1 in np.arange(1.0, 16.0, 1.0):
path_pre = Path()
path_lon = quintic_polynomial.QuinticPolynomial(
s0, s0_v, s0_a, 55.0, s1_v, 0.0, t1)
path_pre.t = list(np.arange(0.0, t1, C.T_STEP))
path_pre.s = [path_lon.calc_xt(t) for t in path_pre.t]
path_pre.s_v = [path_lon.calc_dxt(t) for t in path_pre.t]
path_pre.s_a = [path_lon.calc_ddxt(t) for t in path_pre.t]
path_pre.s_jerk = [path_lon.calc_dddxt(t) for t in path_pre.t]
for l1 in [0.0]:
path = copy.deepcopy(path_pre)
path_lat = quintic_polynomial.QuinticPolynomial(
l0, l0_v, l0_a, l1, 0.0, 0.0, t1)
path.l = [path_lat.calc_xt(t) for t in path_pre.t]
path.l_v = [path_lat.calc_dxt(t) for t in path_pre.t]
path.l_a = [path_lat.calc_ddxt(t) for t in path_pre.t]
path.l_jerk = [path_lat.calc_dddxt(t) for t in path_pre.t]
path.x, path.y = SL_2_XY(path.s, path.l, ref_path)
path.yaw, path.curv, path.ds = calc_yaw_curv(path.x, path.y)
if path.yaw is None:
continue
l_jerk_sum = sum(np.abs(path.l_jerk))
s_jerk_sum = sum(np.abs(path.s_jerk))
v_diff = (path.s_v[-1])**2
path.cost = C.K_JERK * (l_jerk_sum + s_jerk_sum) + \
C.K_V_DIFF * v_diff + \
C.K_TIME * t1 * 2 + \
C.K_OFFSET * abs(path.l[-1]) + \
50.0 * sum(np.abs(path.s_v))
PATHS[path] = path.cost
return PATHS
def sampling_paths_for_Cruising(l0, l0_v, l0_a, s0, s0_v, s0_a, ref_path):
PATHS = dict()
for s1_v in np.arange(C.TARGET_SPEED * 0.5, C.TARGET_SPEED * 1.2,
C.TARGET_SPEED * 0.2):
for t1 in np.arange(4.5, 5.5, 0.2):
path_pre = Path()
path_lon = quartic_polynomial.QuarticPolynomial(
s0, s0_v, s0_a, s1_v, 0.0, t1)
path_pre.t = list(np.arange(0.0, t1, C.T_STEP))
path_pre.s = [path_lon.calc_xt(t) for t in path_pre.t]
path_pre.s_v = [path_lon.calc_dxt(t) for t in path_pre.t]
path_pre.s_a = [path_lon.calc_ddxt(t) for t in path_pre.t]
path_pre.s_jerk = [path_lon.calc_dddxt(t) for t in path_pre.t]
for l1 in np.arange(-C.ROAD_WIDTH, C.ROAD_WIDTH,
C.ROAD_SAMPLE_STEP):
path = copy.deepcopy(path_pre)
path_lat = quintic_polynomial.QuinticPolynomial(
l0, l0_v, l0_a, l1, 0.0, 0.0, t1)
path.l = [path_lat.calc_xt(t) for t in path_pre.t]
path.l_v = [path_lat.calc_dxt(t) for t in path_pre.t]
path.l_a = [path_lat.calc_ddxt(t) for t in path_pre.t]
path.l_jerk = [path_lat.calc_dddxt(t) for t in path_pre.t]
path.x, path.y = SL_2_XY(path.s, path.l, ref_path)
path.yaw, path.curv, path.ds = calc_yaw_curv(path.x, path.y)
l_jerk_sum = sum(np.abs(path.l_jerk))
s_jerk_sum = sum(np.abs(path.s_jerk))
v_diff = abs(C.TARGET_SPEED - path.s_v[-1])
path.cost = C.K_JERK * (l_jerk_sum + s_jerk_sum) + \
C.K_V_DIFF * v_diff + \
C.K_TIME * t1 * 2 + \
C.K_OFFSET * abs(path.l[-1]) + \
C.K_COLLISION * is_path_collision(path)
PATHS[path] = path.cost
return PATHS