window_name = "Known Map Navigation Demo"
# Read Image
img = cv2.flip(cv2.imread("Maps/map.png"),0)
img[img>128] = 255
img[img<=128] = 0
m = np.asarray(img)
m = cv2.cvtColor(m, cv2.COLOR_RGB2GRAY)
m = m.astype(float) / 255.
m_dilate = 1-cv2.dilate(1-m, np.ones((40,40))) # Configuration-Space
img = img.astype(float)/255.
# Simulation Model
from bicycle_model import KinematicModel
car = KinematicModel(l=20, d=5, wu=5, wv=2, car_w=14, car_f=25, car_r=5)
car.init_state(init_pos)
# Path Tracking Controller
if control_type == 0:
from PathTracking.bicycle_pid import PidControl
controller = PidControl(kp=0.03, ki=0.00005, kd=0.08)
elif control_type == 1:
from PathTracking.bicycle_pure_pursuit import PurePursuitControl
controller = PurePursuitControl(kp=0.7,Lfc=10)
elif control_type == 2:
from PathTracking.bicycle_stanley import StanleyControl
controller = StanleyControl(kp=0.5)
elif control_type == 3:
from PathTracking.bicycle_lqr import LQRControl
controller = LQRControl()
path = path_generator.path2()
print("\rpath=")
print(path)
img_path = np.ones((600, 600, 3))
print("\rimg_path 1=")
print(img_path)
for i in range(path.shape[0] - 1):
cv2.line(img_path, (int(path[i, 0]), int(path[i, 1])),
(int(path[i + 1, 0]), int(path[i + 1, 1])), (1.0, 0.5, 0.5),
1)
print("\rimg_path 2=")
print(img_path)
# Initialize Car
car = KinematicModel()
start = (50, 300, 0)
car.init_state(start)
controller = PurePursuitControl(kp=1, Lfc=10)
controller.set_path(path)
while (True):
print("\rState: " + car.state_str(), end="\t")
# ================= Control Algorithm =================
# PID Longitude Control
#print("\rpath=")
#print(path)
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