controller = PurePursuitControl()
controller.set_path(path)
while(True):
print("\rState: "+car.state_str(), end="\t")
# ================= Control Algorithm =================
# PID Longitude Control
end_dist = np.hypot(path[-1,0]-car.x, path[-1,1]-car.y)
target_v = 40 if end_dist > 265 else 0
next_a = 0.1*(target_v - car.v)
# Pure Pursuit Lateral Control
state = {"x":car.x, "y":car.y, "yaw":car.yaw, "v":car.v}
next_delta, target = controller.feedback(state)
car.control(next_a,next_delta)
# =====================================================
# Update & Render
car.update()
img = img_path.copy()
cv2.circle(img,(int(target[0]),int(target[1])),3,(1,0.3,0.7),2) # target points
img = car.render(img)
img = cv2.flip(img, 0)
cv2.imshow("Pure-Pursuit Control Test", img)
k = cv2.waitKey(1)
if k == ord('r'):
car.init_state(start)
if k == 27:
print()
break
pos = (100, 200, 0)
car.x = pos[0]
car.y = pos[1]
car.yaw = pos[2]
while (True):
print("\rState: " + car.state_str(), end="\t")
car.update()
pos = (car.x, car.y, car.yaw)
sdata, plist = lmodel.measure_2d(pos)
img_ = img.copy()
for pts in plist:
cv2.line(img_, (int(1 * pos[0]), int(1 * pos[1])),
(int(1 * pts[0]), int(1 * pts[1])), (0.0, 1.0, 0.0), 1)
img_ = car.render(img_)
img_ = cv2.flip(img_, 0)
#Collision
p1, p2, p3, p4 = car.car_box
l1 = Bresenham(p1[0], p2[0], p1[1], p2[1])
l2 = Bresenham(p2[0], p3[0], p2[1], p3[1])
l3 = Bresenham(p3[0], p4[0], p3[1], p4[1])
l4 = Bresenham(p4[0], p1[0], p4[1], p1[1])
check = l1 + l2 + l3 + l4
collision = False
for pts in check:
if m[int(pts[1]), int(pts[0])] < 0.5:
collision = True
car.redo()
car.v = -0.5 * car.v
def main():
# Parameters
N_PARTICLES = 40 # Number of Particles
N_LANDMARKS = 80 # Number of Landmarks
PERCEPTUAL_RANGE = 120 # Landmark Detection Range
MOTION_NOISE = np.array([1e-5, 1e-2, 1e-5, 1e-2, 1e-5,
1e-2]) # Motion Noise
Qt_sim = np.diag([4, np.deg2rad(4)])**2 # Observation Noise
# Create landmarks
landmarks = []
for i in range(N_LANDMARKS):
rx = np.random.randint(10, 490)
ry = np.random.randint(10, 490)
landmarks.append((rx, ry))
# Initialize environment
window_name = "Fast-SLAM Demo"
cv2.namedWindow(window_name)
img = np.ones((500, 500, 3))
init_pos = (250, 100, 0)
car = KinematicModel(motion_noise=MOTION_NOISE)
car.init_state(init_pos)
car.v = 24
car.w = np.deg2rad(10)
path_odometry = [(init_pos)]
# Create particle filter
pf = ParticleFilter((car.x, car.y, car.yaw),
Qt_sim,
MOTION_NOISE,
psize=N_PARTICLES)
while (True):
###################################
# Simulate Controlling
###################################
u = (car.v, car.w, car.dt)
car.update()
# TODO(Lab-4): Remove the comment after complete the motion model.
#'''
pos_odometry = motion_model(path_odometry[-1], u)
path_odometry.append(pos_odometry)
#'''
print("\rState: " + car.state_str() + " | Neff:" + str(pf.Neff)[0:7],
end="\t")
###################################
# Simulate Observation
###################################
rvec = np.array(landmarks) - np.array((car.x, car.y))
dist = np.hypot(rvec[:, 0], rvec[:, 1])
landmarks_id = np.where(
dist < PERCEPTUAL_RANGE)[0] # Detected Landmark ids
landmarks_detect = np.array(landmarks)[
landmarks_id] # Detected Landmarks
z = []
for i in range(landmarks_detect.shape[0]):
lm = landmarks_detect[i]
r = dist[landmarks_id[i]]
phi = np.arctan2(lm[1] - car.y, lm[0] - car.x) - car.yaw
# Add Observation Noise
r = r + np.random.randn() * Qt_sim[0, 0]**0.5
phi = phi + np.random.randn() * Qt_sim[1, 1]**0.5
phi = normalize_angle(phi)
z.append((r, phi))
###################################
# SLAM Algorithm
###################################
# TODO(Lab-0): Remove the comment after complete the class.
#'''
pf.sample(u)
pf.update(z, landmarks_id)
pf.resample()
#'''
###################################
# Render Canvas
###################################
img_ = img.copy()
# Draw landmark
for lm in landmarks:
cv2.circle(img_, lm, 3, (0.1, 0.7, 0.1), 1)
for i in range(landmarks_detect.shape[0]):
lm = landmarks_detect[i]
cv2.line(img_, (int(car.x), int(car.y)), (int(lm[0]), int(lm[1])),
(0, 1, 0), 1)
# Draw path
for i in range(N_PARTICLES):
draw_path(img_, pf.particles[i].path, 100, (1, 0.7, 0.7))
bid = np.argmax(np.array(pf.weights))
draw_path(img_, pf.particles[bid].path, 1000,
(1, 0, 0)) # Draw Best Path
draw_path(img_, path_odometry, 1000, (0, 0, 0)) # Draw Odometry Path
# Draw particle pose
for i in range(N_PARTICLES):
cv2.circle(
img_,
(int(pf.particles[i].pos[0]), int(pf.particles[i].pos[1])), 2,
(1, 0, 0), 1)
# Draw maps of particles
'''
for lm in pf.particles[i].landmarks:
lx = pf.particles[i].landmarks[lm]["mu"][0,0]
ly = pf.particles[i].landmarks[lm]["mu"][1,0]
cv2.circle(img_, (int(lx),int(ly)), 2, (1,0,0), 1)
'''
# Draw map of best particle
for lid in pf.particles[bid].landmarks:
mean = pf.particles[bid].landmarks[lid]["mu"].reshape(2)
cov = pf.particles[bid].landmarks[lid]["sig"]
draw_ellipse(img_, mean, cov, (0, 0, 1))
cv2.circle(img_, (int(car.x), int(car.y)), PERCEPTUAL_RANGE, (0, 1, 0),
1) # Draw Detect Range
img_ = car.render(img_) # Render Car
img_ = cv2.flip(img_, 0)
cv2.imshow(window_name, img_)
###################################
# Keyboard
###################################
k = cv2.waitKey(1) #1
if k == ord("w"):
car.v += 4
elif k == ord("s"):
car.v += -4
if k == ord("a"):
car.w += 5
elif k == ord("d"):
car.w += -5
elif k == ord("r"):
car.init_state(init_pos)
pf = ParticleFilter((car.x, car.y, car.yaw),
Qt_sim,
MOTION_NOISE,
psize=N_PARTICLES)
path_odometry = [(init_pos)]
print("Reset!!")
if k == 27:
print()
break
def main():
window_name = "Fast-SLAM Demo"
cv2.namedWindow(window_name)
img = np.ones((500, 500, 3))
init_pos = (100, 200, 0)
lsize = 60
detect_dist = 150
psize = 30
landmarks = []
for i in range(lsize):
rx = np.random.randint(10, 490)
ry = np.random.randint(10, 490)
landmarks.append((rx, ry))
# Simulation Model
car = KinematicModel()
car.init_state(init_pos)
pf = ParticleFilter((car.x, car.y, car.yaw), psize=psize)
while (True):
###################################
# Simulate Control
###################################
u = (car.v, car.w, car.dt)
# Add Control Noise
car.v += np.random.randn() * 0.00002 * u[0]**2 + 0.00002 * u[1]**2
car.w += np.random.randn() * 0.00002 * u[0]**2 + 0.00002 * u[1]**2
car.update()
print("\rState: " + car.state_str() + "| Neff:" + str(pf.Neff),
end="\t")
###################################
# Simulate Observation
###################################
r = np.array(landmarks) - np.array((car.x, car.y))
dist = np.hypot(r[:, 0], r[:, 1])
detect_ids = np.where(dist < detect_dist)[0]
detect_lms = np.array(landmarks)[detect_ids]
obs = []
for i in range(detect_lms.shape[0]):
lm = detect_lms[i]
r = np.sqrt((car.x - lm[0])**2 + (car.y - lm[1])**2)
phi = np.rad2deg(np.arctan2(lm[1] - car.y,
lm[0] - car.x)) - car.yaw
# Add Observaation Noise
r = r + np.random.randn() * R_sim[0, 0]**0.5
phi = phi + np.random.randn() * R_sim[1, 1]**0.5
# Normalize Angle
phi = phi % 360
if phi > 180:
phi -= 360
obs.append((r, phi))
###################################
# SLAM Algorithm
###################################
pf.prediction(u)
pf.update_obs(obs, detect_ids)
pf.resample()
###################################
# Render Canvas
###################################
img_ = img.copy()
# Draw Landmark
for lm in landmarks:
cv2.circle(img_, lm, 3, (0.2, 0.5, 0.2), 1)
for i in range(detect_lms.shape[0]):
lm = detect_lms[i]
cv2.line(img_, (int(car.x), int(car.y)), (int(lm[0]), int(lm[1])),
(0, 1, 0), 1)
# Draw Trajectory
start_cut = 100
for j in range(psize):
path_size = len(pf.particles[j].path)
start = 0 if path_size < start_cut else path_size - start_cut
for i in range(start, path_size - 1):
cv2.line(img_, (int(pf.particles[j].path[i][0]),
int(pf.particles[j].path[i][1])),
(int(pf.particles[j].path[i + 1][0]),
int(pf.particles[j].path[i + 1][1])), (1, 0.7, 0.7),
1)
# Draw Best Trajectory
start_cut = 1000
bid = np.argmax(np.array(pf.weights))
path_size = len(pf.particles[bid].path)
start = 0 if path_size < start_cut else path_size - start_cut
for i in range(start, path_size - 1):
cv2.line(img_, (int(pf.particles[bid].path[i][0]),
int(pf.particles[bid].path[i][1])),
(int(pf.particles[bid].path[i + 1][0]),
int(pf.particles[bid].path[i + 1][1])), (1, 0, 0), 1)
# Draw Particle
for j in range(psize):
cv2.circle(
img_,
(int(pf.particles[j].pos[0]), int(pf.particles[j].pos[1])), 2,
(1, 0.0, 0.0), 1)
cv2.circle(img_, (int(car.x), int(car.y)), detect_dist, (0, 1, 0), 1)
# Render Car
img_ = car.render(img_)
img_ = cv2.flip(img_, 0)
cv2.imshow(window_name, img_)
###################################
# Keyboard
###################################
k = cv2.waitKey(1)
if k == ord("w"):
car.v += 4
elif k == ord("s"):
car.v += -4
if k == ord("a"):
car.w += 5
elif k == ord("d"):
car.w += -5
elif k == ord("r"):
car.init_state(init_pos)
pf.init_pf(init_pos)
print("Reset!!")
if k == 27:
print()
break
class NavigationEnv:
def __init__(self, path="Maps/map.png"):
# Read Map
self.img = cv2.flip(cv2.imread(path), 0)
self.img[self.img > 128] = 255
self.img[self.img <= 128] = 0
self.m = np.asarray(self.img)
self.m = cv2.cvtColor(self.m, cv2.COLOR_RGB2GRAY)
self.m = self.m.astype(float) / 255.
self.img = self.img.astype(float) / 255.
self.lmodel = LidarModel(self.m)
def initialize(self):
# Set Mobile Car
self.car = KinematicModel(d=12,
wu=12,
wv=4,
car_w=18,
car_f=14,
car_r=10,
dt=0.1)
#self.car = KinematicModel()
self.car.x, self.car.y = self._search_target()
self.car.yaw = 360 * np.random.random()
self.pos = (self.car.x, self.car.y, self.car.yaw)
# Set Navigation Target
self.target = self._search_target()
self.target_dist = np.sqrt((self.car.x - self.target[0])**2 +
(self.car.y - self.target[1])**2)
target_orien = np.arctan2(self.target[1] - self.car.y, self.target[0] -
self.car.x) - np.deg2rad(self.car.yaw)
target_rel = [
self.target_dist * np.cos(target_orien),
self.target_dist * np.sin(target_orien)
]
# Initialize Measurement
self.sdata, self.plist = self.lmodel.measure_2d(self.pos)
state = self._construct_state(self.sdata, target_rel)
return state
def step(self, action):
# Control and Update
self.car.control((action[0] + 1) / 2 * self.car.v_range,
action[1] * self.car.w_range)
self.car.update()
# Collision Handling
p1, p2, p3, p4 = self.car.car_box
l1 = Bresenham(p1[0], p2[0], p1[1], p2[1])
l2 = Bresenham(p1[0], p3[0], p1[1], p3[1])
l3 = Bresenham(p3[0], p4[0], p3[1], p4[1])
l4 = Bresenham(p4[0], p2[0], p4[1], p2[1])
check = l1 + l2 + l3 + l4
collision = False
for pts in check:
if self.m[int(pts[1]), int(pts[0])] < 0.5:
collision = True
self.car.redo()
self.car.v = -0.5 * self.car.v
break
# Measure Lidar
self.pos = (self.car.x, self.car.y, self.car.yaw)
self.sdata, self.plist = self.lmodel.measure_2d(self.pos)
# TODO(Lab-01): Reward Design
# Distance Reward
curr_target_dist = np.sqrt((self.car.x - self.target[0])**2 +
(self.car.y - self.target[1])**2)
reward_dist = self.target_dist - curr_target_dist
# Orientation Reward
orien = np.rad2deg(
np.arctan2(self.target[1] - self.car.y,
self.target[0] - self.car.x))
err_orien = (orien - self.car.yaw) % 360
if err_orien > 180:
err_orien = 360 - err_orien
reward_orien = np.deg2rad(err_orien)
# Action Panelty
reward_act = 0.05 if action[0] < -0.5 else 0
# Total
reward = 0.6 * reward_dist - 0.3 * reward_orien - 0.1 * reward_act
# Terminal State
done = False
if collision:
reward = -1
done = True
if curr_target_dist < 20:
reward = 5
done = True
# Relative Coordinate of Target
self.target_dist = curr_target_dist
target_orien = np.arctan2(self.target[1] - self.car.y, self.target[0] -
self.car.x) - np.deg2rad(self.car.yaw)
target_rel = [
self.target_dist * np.cos(target_orien),
self.target_dist * np.sin(target_orien)
]
state_next = self._construct_state(self.sdata, target_rel)
return state_next, reward, done
def render(self, gui=True):
img_ = self.img.copy()
for pts in self.plist:
cv2.line(img_, (int(1 * self.pos[0]), int(1 * self.pos[1])),
(int(1 * pts[0]), int(1 * pts[1])), (0.0, 1.0, 0.0), 1)
cv2.circle(img_, (int(1 * self.target[0]), int(1 * self.target[1])),
10, (1.0, 0.5, 0.7), 3)
img_ = self.car.render(img_)
img_ = cv2.flip(img_, 0)
if gui:
cv2.imshow("Mapless Navigation", img_)
k = cv2.waitKey(1)
return img_.copy()
def _search_target(self):
im_h, im_w = self.m.shape[0], self.m.shape[1]
tx = np.random.randint(0, im_w)
ty = np.random.randint(0, im_h)
kernel = np.ones((10, 10), np.uint8)
m_dilate = 1 - cv2.dilate(1 - self.m, kernel, iterations=3)
while (m_dilate[ty, tx] < 0.5):
tx = np.random.randint(0, im_w)
ty = np.random.randint(0, im_h)
return tx, ty
def _construct_state(self, sensor, target):
state_s = [s / 200 for s in sensor]
state_t = [t / 500 for t in target]
return state_s + state_t
def main():
window_name = "EKF SLAM Demo"
cv2.namedWindow(window_name)
img = np.ones((500, 500, 3))
init_pos = (100, 200, 0)
landmarks = [(100, 80), (400, 100), (300, 450)]
# Simulation Model
car = KinematicModel()
car.init_state(init_pos)
#
slam = EkfSLAM()
slam.init_pos(init_pos)
pos_now = init_pos
path = [pos_now]
while (True):
car.update()
img_ = img.copy()
print("\rState: " + car.state_str(), end="\t")
# Simulate Observation
obs = []
for lm in landmarks:
r = np.sqrt((car.x - lm[0])**2 + (car.y - lm[1])**2)
phi = np.rad2deg(np.arctan2(lm[1] - car.y,
lm[0] - car.x)) - car.yaw
obs.append((r, phi))
slam.ekf_prediction(car.v, car.w, car.dt)
print(slam.x[0:3])
# Draw Landmark
for lm in landmarks:
cv2.circle(img_, lm, 3, (0.2, 0.2, 0.8), 2)
cv2.line(img_, (int(car.x), int(car.y)), lm, (0, 1, 0), 1)
# Draw Predict Path
for i in range(len(path) - 1):
cv2.line(img_, (int(path[i][0]), int(path[i][1])),
(int(path[i + 1][0]), int(path[i + 1][1])), (1, 0.5, 0.5),
1)
img_ = car.render(img_)
img_ = cv2.flip(img_, 0)
cv2.imshow(window_name, img_)
# Noise
x_noise = np.random.randn() * Q_sim[0, 0]**0.5
y_noise = np.random.randn() * Q_sim[1, 1]**0.5
yaw_noise = np.random.randn() * Q_sim[2, 2]**0.5
# Keyboard
k = cv2.waitKey(1)
car.x += x_noise
car.y += y_noise
car.yaw += yaw_noise
if k == ord("w"):
car.v += 4
elif k == ord("s"):
car.v += -4
if k == ord("a"):
car.w += 5
elif k == ord("d"):
car.w += -5
elif k == ord("r"):
car.init_state(init_pos)
nav_pos = None
path = None
print("Reset!!")
if k == 27:
print()
break
pos_now = velocity_motion_model(pos_now, car.v, car.w, car.dt)
path.append(pos_now)
