import cv2
import numpy as np
from utils import *
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.
lmodel = LidarModel(m)
car = KinematicModel()
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)
max_dist = 250
lmodel = LidarModel(m, sensor_size=sensor_size, max_dist=max_dist)
car = KinematicModel(d=9, wu=7, wv=2, car_w=16, car_f=13, car_r=7)
#car = KinematicModel(l=20, d=5, wu=5, wv=2, car_w=14, car_f=25, car_r=5)
control_type = 3
if control_type == 0:
controller = PidControl()
elif control_type == 1:
controller = PurePursuitControl(kp=0.5,Lfc=5)
elif control_type == 2:
controller = StanleyControl(kp=0.5)
else:
controller = PurePursuitControl(kp=0.5,Lfc=5)
pos = (100,200,0)
car.x = 100
car.y = 200
car.yaw = 0
rrt = RRTStar(m_dilate)
astar = AStar(m_dilate)
gm = GridMap([0.4, -0.4, 5.0, -5.0], gsize=grid_size)
# First Mapping
pos = (car.x, car.y, car.yaw)
sdata = lmodel.measure(pos)
gm.update_map(pos, [sensor_size,-120,120,max_dist], sdata)
# Init PF
pf = ParticleFilter((car.x, car.y, car.yaw), [sensor_size,-120,120,max_dist], gm, particle_size)
def mouse_click(event, x, y, flags, param):
global nav_pos, pos, path, m_dilate, way_points, controller