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(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 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()
