if m[int(pts[1]),int(pts[0])]<0.5:
collision = True
car.redo()
car.v = -0.8*car.v
print("gg")
break
if collision:
collision_count = 1
print("\rState: "+car.state_str(), "| Goal:", nav_pos, end="\t")
pos = (car.x, car.y, car.yaw)
sdata = lmodel.measure(pos)
plist = EndPoint(pos, [sensor_size,-120,120], sdata)
# Particle Filter
pf.feed((car.v, car.w, car.dt), sdata)
print(pf.neff)
if pf.neff < particle_size/2:
print("re")
pf.resampling()
pmimg = pf.particle_list[5].gmap.adaptive_get_map_prob()
pmimg = (255*pmimg).astype(np.uint8)
pmimg = cv2.cvtColor(pmimg, cv2.COLOR_GRAY2RGB)
pmimg_ = cv2.flip(pmimg,0)
cv2.imshow("pmap", pmimg_)
# Map
gm.update_map(pos, [sensor_size,-120,120,max_dist], sdata)
mimg = gm.adaptive_get_map_prob()
mimg = (255*mimg).astype(np.uint8)
