def plan_motions(index_sec): motion = [] v0, v1 = 0, 0 seg = discontinuities2[index_sec[0]:index_sec[1] + 1] seg = list(zip(seg[:-1], seg[1:])) extent = 0 for sec in seg: s0, s1 = sec[0].state, sec[1].state extent += np.abs( reeds_shepp.path_length(s0, s1, 1. / self.maximum_curvature)) samples = [] for sec in seg: s0, s1 = sec[0].state, sec[1].state samples.extend( reeds_shepp.path_sample(s0, s1, 1. / self.maximum_curvature, res)) acc = min([(v_max**2 - v1**2) / extent, a_cc]) vcc = np.sqrt(v1**2 + acc * extent) for i, sample in enumerate(samples): if i * res < extent / 2.: vt = min([np.sqrt(v0**2 + 2 * acc * (i * res)), vcc]) else: vt = min([ np.sqrt(v1**2 + 2 * acc * np.abs(extent - i * res)), vcc ]) motion.append( self.Configuration(sample[:3], k=sample[3], v=np.sign(sample[4]) * vt)) return motion
def trajectory_generation(self): # DEFINIZIONE DELLA COORDINATA INIZIALE E DI QUELLA FINALE coordinate = [(0.0,0.0,0.0), (0.0, 4.0, np.pi)] # PARAMETRI UTILI ALLA GENERAZIONE DELLE CURVE DI REEDSSHEPP step_size = 0.25 rho = 5.8 # GENERAZIONE DELLE CURVE DI REEDS SHEPP qs = reeds_shepp.path_sample(coordinate[0], coordinate[1], rho, step_size) # ESTRAZIONE DEI PARAMETRI LUNGO I DUE ASSI E ARROTONDAMENTO ALLA TERZA DECIMALE xs = np.round([coordinate[0] for coordinate in qs],3) ys = np.round([coordinate[1] for coordinate in qs],3) # INIZIALIZZAZIONE DEI VETTORI "segment_x" E "segment_y" PER DISTINGUERE LE VARIE CIRCONFERENZE v1,v2,v3,v4,v5,v6,v7,v8,v9,v10,v11,v12 = [],[],[],[],[],[],[],[],[],[],[],[] segment_x = [v1, v2, v3, v4, v5, v6] segment_y = [v7, v8, v9, v10, v11, v12] # INIZIALIZZAZIONE DEI VETTORI CONTENENTI GLI ELEMENTI INIZIALI E FINALI DI CIASCUN TRATTO i = 0 j = 0 initial_x = [] initial_y = [] final_x = [] final_y = [] # CONTROLLO SE IL PRIMO TRATTO PREVEDA O MENO UN INCREMENTO LUNGO X o LUNGO Y, DUNQUE AGGIORNO SLOPE_X e SLOPE Y case = 0 if xs[i+1] > xs[i] and ys[i+1] > ys[i]: case = 1
def main(): filepath, seq = './test_scenes', 0 source, target = read_task(filepath, seq) grid_map = read_grid(filepath, seq) start = center2rear(deepcopy(source).gcs2lcs(source.state)) goal = center2rear(deepcopy(target).gcs2lcs(source.state)) states = reeds_shepp.path_sample(start.state, goal.state, 5.0, 0.3) cons = [transform(contour(), state) for state in states] cons = [np.floor(con / 0.1 + 600 / 2.).astype(int) for con in cons] mask = np.zeros_like(grid_map, dtype=np.uint8) past = time.time() [cv2.fillPoly(mask, [con], 255) for con in cons] # mode = np.zeros((600 + 2, 600 + 2), np.uint8) # miss = mask.copy() # cv2.floodFill(miss, mode, (0, 0), 255) now = time.time() print((now - past) * 1000) # mask = mask | np.bitwise_not(miss) # result = np.bitwise_and(mask, grid_map) # print (np.any(result==127)) result = np.bitwise_and(mask, grid_map) print(grid_map.min()) print(np.all(result < 255)) cv2.imshow("Mask", mask) cv2.waitKey(0)
def collision_free(self, x_from, x_to): # type: (StateNode, StateNode) -> bool """check if the path from one state to another state collides with any obstacles or not.""" # making contours of the curve states = reeds_shepp.path_sample(x_from.state, x_to.state, 1. / self.maximum_curvature, 0.3) # states.append(tuple(x_to.state)) # include the end point contours = [ self.transform(self.check_poly, s[0], s[1], s[2]) for s in states ] contours = [ np.floor(con / self.grid_res + self.grid_map.shape[0] / 2.).astype(int) for con in contours ] # making mask mask = np.zeros_like(self.grid_map, dtype=np.uint8) [cv2.fillPoly(mask, [con], 255) for con in contours] # checking result = np.bitwise_and(mask, self.grid_map) Debugger().debug_collision_checking(states, self.check_poly, np.all(result < self.obstacle), switch=self.debug) return np.all(result < self.obstacle)
def reeds_shepp_path_planning(start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature): q0 = [start_x, start_y, start_yaw] q1 = [end_x, end_y, end_yaw] step_size = 0.1 qs = reeds_shepp.path_sample(q0, q1, curvature, step_size) xs = [q[0] for q in qs] ys = [q[1] for q in qs] yaw = [q[2] for q in qs] xs.append(end_x) ys.append(end_y) yaw.append(end_yaw) clen = reeds_shepp.path_length(q0, q1, curvature) pathtypeTuple = reeds_shepp.path_type(q0, q1, curvature) ptype = "" for t in pathtypeTuple: if t == 1: ptype += "L" elif t == 2: ptype += "S" elif t == 3: ptype += "R" return xs, ys, yaw, ptype, clen
def plot_path(q0, q1): qs = reeds_shepp.path_sample(q0, q1, rho, step_size) xs = [q[0] for q in qs] ys = [q[1] for q in qs] plt.plot(xs, ys, 'b-') plt.plot(xs, ys, 'r.') plot_car(q0) plot_car(q1) plt.axis('equal')
def extract_path(filepath, seq, rho=5.0, size=0.1): ps = np.loadtxt('{}/{}/{}_path.txt'.format(filepath, '../labels', seq), delimiter=',') path = [] for p in zip(ps[:-1], ps[1:]): path.extend(reeds_shepp.path_sample(p[0], p[1], rho, size)) path = [p[:3] for p in path] print path return path
def plot_path(path, rho): pp = zip(path[:-1], path[1:]) states = [] for p in pp: states.extend(reeds_shepp.path_sample(p[0].state, p[1].state, rho, 0.3)) xs = [state[0] for state in states] ys = [state[1] for state in states] actor = plt.plot(xs, ys, c='r', lw=3.0) plt.draw() return actor
def steer(self, p, q, eps): delta = 0.2 length = rp.path_length(p, q, self.turn_radius) points = rp.path_sample(p, q, self.turn_radius, delta) points.append(q) path = np.array(points) if length > eps: num_points = math.floor(eps / delta) path = path[0:num_points + 1] return Curve(path)
def draw_path(world, source, no=0): filepath = '{}/{}_path.txt'.format('scene_maker/scenes', no) if not os.path.isfile(filepath): print('no path: {}'.format(filepath)) return origin = (source['x'], -source['y'], -numpy.radians(source['yaw'])) path = numpy.loadtxt(filepath, delimiter=',') path = zip(path[:-1], path[1:]) for x0, x1 in path: states = reeds_shepp.path_sample(x0, x1, 5.0, 0.1) for state in states: arr = lcs2gcs(state, origin) arr = (arr[0], -arr[1], -arr[2]) draw_box_with_arrow2(world, state2transform(arr, z=0.3), carla.Color(r=0, b=0, g=255, a=127))
def collision_check(path, rho, grid_map, iov_threshold=0.125): sequence = map(deepcopy, path) sequence = map(list, sequence) sequence = zip(sequence[:-1], sequence[1:]) free, intersection_area = True, 0 for x_from, x_to in sequence: states = reeds_shepp.path_sample(x_from, x_to, rho, 0.1) cons = [transform(contour(), state) for state in states] cons = [np.floor(con / 0.1 + 600 / 2.).astype(int) for con in cons] mask = np.zeros_like(grid_map, dtype=np.uint8) [cv2.fillPoly(mask, [con], 255) for con in cons] intersection = np.bitwise_and(mask, grid_map) free *= np.all(intersection < 255) intersection_area += np.sum(intersection / 255) iov = 1. * intersection_area / calculate_vehicle_area() return (False, iov) if not free and iov > iov_threshold else (True, iov)
def plan_motions(sector): q0, v0, q1, v1 = sector[0].state, sector[0].v, sector[ 1].state, sector[1].v extent = reeds_shepp.path_length(q0, q1, 1. / self.maximum_curvature) acc = min([(v_max**2 - v1**2) / extent, a_cc]) vcc = np.sqrt(v1**2 + acc * extent) samples = reeds_shepp.path_sample(q0, q1, 1. / self.maximum_curvature, res) for i, sample in enumerate(samples): if i * res < extent / 2.: vt = min([np.sqrt(v0**2 + 2 * acc * (i * res)), vcc]) else: vt = min( [np.sqrt(v1**2 + 2 * acc * (extent - i * res)), vcc]) motions.append( self.Configuration(sample[:3], k=sample[3], v=np.sign(sample[4]) * vt))
def draw_path(self, path, source, lift_time=0.01, z=0.26, with_box=True, sample_color=carla.Color(r=255, b=0, g=0, a=255), path_color=carla.Color(r=0, b=0, g=255, a=255)): if with_box: self.draw_states_with_box_and_arrow(path[1:-1], color=sample_color, z=z) origin = self.carla_transform_to_state(source) path = zip(path[:-1], path[1:]) for x0, x1 in path: states = reeds_shepp.path_sample(x0, x1, 5.0, 0.1) for state in states: arr = self.lcs2gcs(state, origin) self.draw_box(self.state2transform(arr, z=z), path_color, lift_time=lift_time)
def trajectory_generation(self): # DEFINIZIONE DELLA COORDINATA INIZIALE E DI QUELLA FINALE coordinate = [(0.0,0.0,0.0),(3.0,-4.0,np.pi)] # PARAMETRI UTILI ALLA GENERAZIONE DELLE CURVE DI REEDSSHEPP step_size = 0.01823 rho = 5.8 # GENERAZIONE DELLE CURVE DI REEDS SHEPP qs = reeds_shepp.path_sample(coordinate[0], coordinate[1], rho, step_size) tmax = np.round(reeds_shepp.path_length(coordinate[0], coordinate[1], rho),2) # ESTRAZIONE DEI PARAMETRI LUNGO I DUE ASSI E ARROTONDAMENTO ALLA TERZA DECIMALE xs = np.round([coordinate[0] for coordinate in qs],3) ys = np.round([coordinate[1] for coordinate in qs],3) theta = np.round([coordinate[2] for coordinate in qs],3) self.t = np.linspace(0,tmax,len(xs)) # Richiamo il metodo "trajectory" per l'estrazione dei segmenti rappresentanti le curve di Reeds Shepp a partire dalle circonferenze calcolate v_d_val = 0.5 # m/s element = 0 u1 = np.zeros(len(xs)) while element < len(theta)-1: if theta[element] < np.pi/2 and theta[element] > -np.pi/2: if xs[element] < xs[element+1]: u1[element] = 1 else: u1[element] = -1 else: if xs[element] > xs[element+1]: u1[element] = 1 else: u1[element] = -1 element = element+1 self.dotx_d = u1*np.cos(theta) self.doty_d = u1*np.sin(theta) self.v_d = np.sqrt(self.dotx_d**2 + self.doty_d**2) self.theta_d = np.arctan2(self.doty_d, self.dotx_d) self.x_d = xs self.y_d = ys
def get_path(self, present_pos, target): # Generates a Reeds-Shepp path target.append(math.pi / 2) path = reeds_shepp.path_sample(present_pos, target, self.rho, self.step_size) return path
if bigGap > 7 and obstacles[-1] == 1: if (mode == 'Looking' or mode == 'Stopping') and curSpeed > 0: accumDist += curSpeed * timestep robot.setCruisingSpeed(0) robot.setBrakeIntensity(1) mode = 'Stopping' elif curSpeed == 0 and mode == 'Stopping': print(accumDist) print(toTheRight) mode = 'Parking' errors = 0.25 spotwidth = 2.75 planleng = distSinceObs - 2 * errors goal = [accumDist + planleng / 2, toTheRight + spotwidth / 2] path = reeds_shepp.path_sample( (carleng / 2, 0, 0), (goal[0], goal[1], 0), rho, step_size) xs = np.array([q[0] for q in path]) ys = np.array([q[1] for q in path]) thetas = np.array([q[2] for q in path]) target_speed = 2.5 # [m/s] max_simulation_time = 200.0 state = State(x=0, y=0.0, yaw=0, v=0.0) last_idx = len(xs) - 1 time = 0.0 x = [state.x] y = [state.y] yaw = [state.yaw]
def plot_curve(x_from, x_to, rho, color='g'): states = reeds_shepp.path_sample(x_from.state, x_to.state, rho, 0.3) x, y = [state[0] for state in states], [state[1] for state in states] actor = plt.plot(x, y, c=color) plt.draw() return actor
def plot_curve(x_from, x_to, rho, color='g', lw=3., zorder=10): states = reeds_shepp.path_sample(x_from, x_to, rho, 0.3) x, y = [state[0] for state in states], [state[1] for state in states] actor = plt.plot(x, y, c=color, lw=lw, zorder=zorder) plt.draw() return actor
import numpy as np import reeds_shepp import json q0 = [0.0, 0.0, 0.] test_cases = [] for i in range(10000): print(i) q1 = np.random.randn(3) * 2 q_seq = np.array(reeds_shepp.path_sample(q0, q1, 0.2, 0.01)) test_cases.append([q1.tolist(), q_seq.tolist()]) with open('/tmp/reeds_shepp_test_cases.json', 'w') as f: data = {'cases': test_cases} json.dump(test_cases, f)
def plot_polygons(x_from, x_to, rho, color='y'): states = reeds_shepp.path_sample(x_from, x_to, rho, 0.3) [plot_polygon(transform(contour(), s), color) for s in states]
def waypoints_to_path(waypoints, r=1, step=0.1, r_step=0.2, model=Vehicle.DUBINS, FIX_ANGLES=False): path = [] for wp1, wp2 in zip(waypoints[:-1], waypoints[1:]): if model == Vehicle.DUBINS: dbp = dubins.shortest_path(wp1, wp2, r) path = path + dbp.sample_many(step)[0] elif model == Vehicle.RTR or model == Vehicle.STRAIGHT: # rotate (1) dist = np.linalg.norm(np.array(wp1[:-1]) - np.array(wp2[:-1])) x = wp1[0] y = wp1[1] phi = wp1[2] % (2 * np.pi) if dist > step: dx = wp2[0] - wp1[0] dy = wp2[1] - wp1[1] # as per https://docs.scipy.org/doc/numpy/reference/generated/numpy.arctan2.html # Note the role reversal: the “y-coordinate” is the first function parameter, the “x-coordinate” is the second. phi_goal = np.arctan2(dy, dx) else: phi_goal = wp2[2] if model == Vehicle.RTR: for a in short_angle_range(phi, phi_goal, r_step=r_step): path.append((x, y, a)) # translate steps = dist / step if steps < 1: steps = 1 dx = (wp2[0] - wp1[0]) / steps dy = (wp2[1] - wp1[1]) / steps for _ in range(int(steps)): x += dx y += dy path.append((x, y, phi_goal)) if model == Vehicle.RTR: # rotate (2) x = wp2[0] y = wp2[1] phi = phi_goal phi_goal = wp2[2] % (2 * np.pi) for a in short_angle_range(phi, phi_goal, r_step=r_step): path.append((x, y, a)) # if len(path) < 3: # print("OH NO") # print(f"{wp1}, {wp2}, {path}, {phi}") elif model == Vehicle.REEDS_SHEPP: part = [] sample = reeds_shepp.path_sample(wp1, wp2, r, step) for s in sample: part.append((s[0], s[1], s[-1])) # cleanup angles if FIX_ANGLES: for i, xy in enumerate(part[:-1]): dx = xy[0] - part[i + 1][0] dy = xy[1] - part[i + 1][1] phi = np.arctan2(dy, dx) path.append((xy[0], xy[1], phi)) path.append(wp2) else: path = path + part elif model == Vehicle.BEZIER: control_point1 = wp1[0] + np.sin(wp1[2]) * r, wp1[1] + np.cos( wp1[2]) * r control_point2 = wp2[0] - np.sin(wp2[2]) * r, wp2[1] - np.cos( wp2[2]) * r nodes = np.asfortranarray( [[wp1[0], control_point1[0], control_point2[0], wp2[0]], [wp1[1], control_point1[1], control_point2[1], wp2[1]]]) curve = bezier.Curve(nodes, degree=3) l = np.linspace(0.0, 1.0, num=int(curve.length / step)) points = curve.evaluate_multi(l) angles = [curve.evaluate_hodograph(i) for i in l] angles = [np.arctan2(x[1], x[0])[0] for x in angles] for i, (x, y) in enumerate(points.transpose()): path.append((x, y, angles[i])) else: print("NO VEHICLE MODEL!") if waypoints[-1] != path[-1]: path.append(waypoints[-1]) return path
def plot_polygons(x_from, x_to, rho, color='y', lw=3., zorder=10): states = reeds_shepp.path_sample(x_from, x_to, rho, 0.2) [ plot_polygon(transform(contour(), s), color, lw=lw, zorder=zorder) for s in states ]
import matplotlib.pyplot as plt import numpy as np import reeds_shepp import json q0 = [0.0, 0.0, 0.] q1 = np.random.randn(3) * 0.2 q1[2] += 1 eps = 0.0001 q_seq = np.array(reeds_shepp.path_sample(q0, q1, 0.2, eps))[:, :3] q_seq_back = np.array(reeds_shepp.path_sample(q1, q0, 0.2, eps))[:, :3] a = len(q_seq) b = len(q_seq_back) plt.scatter(q_seq[:, 0], q_seq[:, 1], c='r', s=0.01) plt.scatter(q_seq_back[:, 0], q_seq_back[:, 1], c='b', s=0.01) plt.show()