pos = init_pos
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()
collision_count = 0
# 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((30, 30))) # Configuration-Space
img = img.astype(float) / 255.
# Lidar
lmodel = LidarModel(m)
#car = KinematicModel()
car = KinematicModel(l=20, d=5, wu=5, wv=2, car_w=14, car_f=25, car_r=5)
control_type = 2
if control_type == 0:
controller = PidControl()
elif control_type == 1:
controller = PurePursuitControl(kp=0.7, Lfc=10)
elif control_type == 2:
controller = StanleyControl(kp=0.5)
elif control_type == 3:
controller = LQRControl()
else:
controller = PurePursuitControl(kp=0.7, Lfc=10)
pos = (100, 200, 0)
car.x = 100
car.y = 200
from bicycle_model import KinematicModel
from grid_map import GridMap
from utils import *
# 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.
img = img.astype(float) / 255.
# Lidar
lmodel = LidarModel(m)
car = KinematicModel()
pos = (100, 200, 0)
car.x = 100
car.y = 200
car.yaw = 0
gm = GridMap([0.5, -0.5, 5.0, -5.0], gsize=3)
while (True):
print("\rState: " + car.state_str(), end="\t")
car.update()
pos = (car.x, car.y, car.yaw)
sdata = lmodel.measure(pos)
plist = EndPoint(pos, [61, -120, 120], sdata)
# Map
# Path
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)
import cv2
import path_generator
import sys
sys.path.append("../")
from bicycle_model import KinematicModel
# Path
path = path_generator.path2()
img_path = np.ones((600, 600, 3))
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)
# Initialize Car
car = KinematicModel()
start = (50, 300, 0)
car.init_state(start)
controller = StanleyControl(kp=0.5)
controller.set_path(path)
while (True):
#print("\rState: "+car.state_str(), end="\t")
# 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)
# Stanley Lateral Control
state = {
