From 5cae060e380da42d7752f9b39baa437376eb6896 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Tue, 18 Jul 2017 21:35:53 -0700 Subject: [PATCH] code clean up --- PathPlanning/StateLatticePlanner/__init__.py | 0 .../state_lattice_planner.py | 62 ++++++++----------- 2 files changed, 27 insertions(+), 35 deletions(-) create mode 100644 PathPlanning/StateLatticePlanner/__init__.py diff --git a/PathPlanning/StateLatticePlanner/__init__.py b/PathPlanning/StateLatticePlanner/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/PathPlanning/StateLatticePlanner/state_lattice_planner.py b/PathPlanning/StateLatticePlanner/state_lattice_planner.py index d8293557..b930ae19 100644 --- a/PathPlanning/StateLatticePlanner/state_lattice_planner.py +++ b/PathPlanning/StateLatticePlanner/state_lattice_planner.py @@ -11,11 +11,11 @@ import model_predictive_trajectory_generator as planner import motion_model -def search_nearest_one_from_lookuptable(tx, ty, tyaw, lookuptable): +def search_nearest_one_from_lookuptable(tx, ty, tyaw, lookup_table): mind = float("inf") minid = -1 - for (i, table) in enumerate(lookuptable): + for (i, table) in enumerate(lookup_table): dx = tx - table[0] dy = ty - table[1] @@ -25,7 +25,7 @@ def search_nearest_one_from_lookuptable(tx, ty, tyaw, lookuptable): minid = i mind = d - return lookuptable[minid] + return lookup_table[minid] def get_lookup_table(): @@ -34,12 +34,12 @@ def get_lookup_table(): return np.array(data) -def generate_path(states, k0): +def generate_path(target_states, k0): # x, y, yaw, s, km, kf lookup_table = get_lookup_table() result = [] - for state in states: + for state in target_states: bestp = search_nearest_one_from_lookuptable( state[0], state[1], state[2], lookup_table) @@ -58,11 +58,11 @@ def generate_path(states, k0): return result -def calc_uniform_states(np, nh, d, a_min, a_max, p_min, p_max): +def calc_uniform_polar_states(nxy, nh, d, a_min, a_max, p_min, p_max): """ calc uniform state - :param np: number of position sampling + :param nxy: number of position sampling :param nh: number of heading sampleing :param d: distance of terminal state :param a_min: position sampling min angle @@ -71,24 +71,14 @@ def calc_uniform_states(np, nh, d, a_min, a_max, p_min, p_max): :param p_max: heading sampling max angle :return: states list """ - states = [] - for i in range(np): - a = a_min + (a_max - a_min) * i / (np - 1) - for j in range(nh): - xf = d * math.cos(a) - yf = d * math.sin(a) - if nh == 1: - yawf = (p_max - p_min) / 2 + a - else: - yawf = p_min + (p_max - p_min) * j / (nh - 1) + a - - states.append([xf, yf, yawf]) + angle_samples = [i / (nxy - 1) for i in range(nxy)] + states = sample_states(angle_samples, a_min, a_max, d, p_max, p_min, nh) return states -def calc_biased_states(goal_angle, ns, nxy, nh, d, a_min, a_max, p_min, p_max): +def calc_biased_polar_states(goal_angle, ns, nxy, nh, d, a_min, a_max, p_min, p_max): """ calc biased state @@ -105,29 +95,33 @@ def calc_biased_states(goal_angle, ns, nxy, nh, d, a_min, a_max, p_min, p_max): :return: states list """ - cnav = [] - for i in range(ns - 1): - asi = a_min + (a_max - a_min) * i / (ns - 1) - cnavi = math.pi - abs(asi - goal_angle) - cnav.append(cnavi) + asi = [a_min + (a_max - a_min) * i / (ns - 1) for i in range(ns - 1)] + cnav = [math.pi - abs(i - goal_angle) for i in asi] cnav_sum = sum(cnav) cnav_max = max(cnav) # normalize - for i in range(ns - 1): - cnav[i] = (cnav_max - cnav[i]) / (cnav_max * ns - cnav_sum) + cnav = [(cnav_max - cnav[i]) / (cnav_max * ns - cnav_sum) for i in range(ns - 1)] csumnav = np.cumsum(cnav) di = [] + li = 0 for i in range(nxy): - for ii in range(ns - 1): + for ii in range(li, ns - 1): if ii / ns >= i / (nxy - 1): di.append(csumnav[ii]) + li = ii - 1 break + states = sample_states(di, a_min, a_max, d, p_max, p_min, nh) + + return states + + +def sample_states(angle_samples, a_min, a_max, d, p_max, p_min, nh): states = [] - for i in di: + for i in angle_samples: a = a_min + (a_max - a_min) * i for j in range(nh): @@ -142,7 +136,6 @@ def calc_biased_states(goal_angle, ns, nxy, nh, d, a_min, a_max, p_min, p_max): return states - def uniform_terminal_state_sampling1(): k0 = 0.0 nxy = 5 @@ -152,7 +145,7 @@ def uniform_terminal_state_sampling1(): a_max = math.radians(45.0) p_min = - math.radians(45.0) p_max = math.radians(45.0) - states = calc_uniform_states(nxy, nh, d, a_min, a_max, p_min, p_max) + states = calc_uniform_polar_states(nxy, nh, d, a_min, a_max, p_min, p_max) result = generate_path(states, k0) for table in result: @@ -176,7 +169,7 @@ def uniform_terminal_state_sampling2(): a_max = math.radians(45.0) p_min = - math.radians(20.0) p_max = math.radians(20.0) - states = calc_uniform_states(nxy, nh, d, a_min, a_max, p_min, p_max) + states = calc_uniform_polar_states(nxy, nh, d, a_min, a_max, p_min, p_max) result = generate_path(states, k0) for table in result: @@ -202,7 +195,7 @@ def biased_terminal_state_sampling1(): p_max = math.radians(20.0) ns = 100 goal_angle = math.radians(0.0) - states = calc_biased_states(goal_angle, ns, nxy, nh, d, a_min, a_max, p_min, p_max) + states = calc_biased_polar_states(goal_angle, ns, nxy, nh, d, a_min, a_max, p_min, p_max) result = generate_path(states, k0) for table in result: @@ -226,7 +219,7 @@ def biased_terminal_state_sampling2(): p_max = math.radians(20.0) ns = 100 goal_angle = math.radians(30.0) - states = calc_biased_states(goal_angle, ns, nxy, nh, d, a_min, a_max, p_min, p_max) + states = calc_biased_polar_states(goal_angle, ns, nxy, nh, d, a_min, a_max, p_min, p_max) result = generate_path(states, k0) for table in result: @@ -239,7 +232,6 @@ def biased_terminal_state_sampling2(): plt.show() - def main(): # uniform_terminal_state_sampling1() # uniform_terminal_state_sampling2()