def test_a_car_should_stop_when_there_is_a_car_ahead(self):
road = build_road(10, Orientation.NORTH)
for c in road:
self.assertIsNone(c.current_car)
p = Path([0] * 10)
for i in range(4, 10):
road[i].current_car = Car(p, road[0])
road[1].current_car = Car(p, road[0])
compute_next(road)
apply_next(road)
self.assertIsNone(road[1].current_car)
self.assertIsNotNone(road[2].current_car)
self.assertIsNone(road[3].current_car)
compute_next(road)
apply_next(road)
self.assertIsNone(road[2].current_car)
self.assertIsNotNone(road[3].current_car)
self.assertIsNotNone(road[4].current_car)
compute_next(road)
apply_next(road)
self.assertIsNone(road[2].current_car)
self.assertIsNotNone(road[3].current_car)
self.assertIsNotNone(road[4].current_car)
def test_go_if_no_right_priority(self):
rp = RightPriorityNode()
r1 = RoadNode(Orientation.NORTH)
r2 = RoadNode(Orientation.WEST)
r3 = RoadNode(Orientation.EAST)
nodes = [rp, r1, r2, r3]
p = Path([0] * 3)
r1.current_car = Car(deepcopy(p), r1)
r2.current_car = Car(deepcopy(p), r2)
link(r1, rp)
link(r2, rp)
link(rp, r3)
compute_next(nodes)
apply_next(nodes)
self.assertIsNotNone(r1.current_car)
self.assertIsNone(r2.current_car)
self.assertIsNotNone(rp.current_car)
self.assertIsNone(r3.current_car)
compute_next(nodes)
apply_next(nodes)
self.assertIsNotNone(r1.current_car)
self.assertIsNone(r2.current_car)
self.assertIsNone(rp.current_car)
self.assertIsNotNone(r3.current_car)
def __init__(self, map="rally_map2.png", fps=10):
self.script_dir = os.path.dirname(__file__)
# 맵 정보 불러오기
map_path = os.path.join(self.script_dir, "map", map)
self.map = cv.imread(map_path)
self.car = Car()
self.fps = fps
self.DRIVE = self.car.DRIVE
self.REVERSE = self.car.REVERSE
self.BREAK = self.car.BREAK
self.max_steering_deg = self.car.max_steering_deg
self.reward_domain = get_reward_domain(self.map, self.car)
self.is_done = False
def test_dont_go_if_someone_in_junction_and_no_one_in_priority_road(self):
rp = RightPriorityNode()
r1 = RoadNode(Orientation.NORTH)
r2 = RoadNode(Orientation.WEST)
r3 = RoadNode(Orientation.EAST)
nodes = [rp, r1, r2, r3]
p = Path([0] * 3)
r1.current_car = Car(p, r1)
rp.current_car = Car(p, rp)
link(r1, rp)
link(r2, rp)
link(rp, r3)
compute_next(nodes)
apply_next(nodes)
self.assertIsNotNone(r1.current_car)
self.assertIsNone(r2.current_car)
self.assertIsNone(rp.current_car)
self.assertIsNotNone(r3.current_car)
def test_for_3_input_for_the_priority(self):
rp = RightPriorityNode()
r1 = RoadNode(Orientation.NORTH)
r2 = RoadNode(Orientation.WEST)
r3 = RoadNode(Orientation.SOUTH)
r4 = RoadNode(Orientation.EAST)
nodes = [rp, r1, r2, r3, r4]
p = Path([0] * 3)
r1.current_car = Car(p, r1)
r2.current_car = Car(p, r2)
r3.current_car = Car(p, r3)
link(r1, rp)
link(r2, rp)
link(r3, rp)
link(rp, r4)
compute_next(nodes)
apply_next(nodes)
self.assertIsNotNone(r1.current_car)
self.assertIsNotNone(r2.current_car)
self.assertIsNone(r3.current_car)
self.assertIsNotNone(rp.current_car)
self.assertIsNone(r4.current_car)
compute_next(nodes)
apply_next(nodes)
self.assertIsNotNone(r1.current_car)
self.assertIsNotNone(r2.current_car)
self.assertIsNone(r3.current_car)
self.assertIsNone(rp.current_car)
self.assertIsNotNone(r4.current_car)
compute_next(nodes)
apply_next(nodes)
self.assertIsNotNone(r1.current_car)
self.assertIsNone(r2.current_car)
self.assertIsNone(r3.current_car)
self.assertIsNotNone(rp.current_car)
self.assertIsNone(r4.current_car)
def compute_next(self):
super().compute_next()
if random.random() <= self.rate:
self.to_spawn += 1
if self.to_spawn > 0 and self.successors[0].current_car is None:
probas = sorted(self.paths.keys())
draw = random.random()
for p in probas:
if draw <= p / 100:
self.next_car = Car(deepcopy(self.paths[p]), self)
self.to_spawn -= 1
break
def test_a_car_should_wait_before_go_forward_when_there_is_a_car_ahead(
self):
road = build_road(3, Orientation.NORTH)
p = Path([0] * 3)
road[0].current_car = Car(p, road[0])
road[1].current_car = Car(p, road[1])
road[2].current_car = Car(p, road[2])
compute_next(road)
apply_next(road)
self.assertIsNotNone(road[0].current_car)
self.assertIsNotNone(road[1].current_car)
self.assertIsNone(road[2].current_car)
compute_next(road)
apply_next(road)
self.assertIsNotNone(road[0].current_car)
self.assertIsNone(road[1].current_car)
self.assertIsNotNone(road[2].current_car)
compute_next(road)
apply_next(road)
self.assertIsNone(road[0].current_car)
self.assertIsNotNone(road[1].current_car)
self.assertIsNone(road[2].current_car)
compute_next(road)
apply_next(road)
self.assertIsNone(road[0].current_car)
self.assertIsNone(road[1].current_car)
self.assertIsNotNone(road[2].current_car)
compute_next(road)
apply_next(road)
self.assertIsNone(road[0].current_car)
self.assertIsNone(road[1].current_car)
self.assertIsNone(road[2].current_car)
def test_a_car_should_go_forward_when_it_is_alone(self):
road = build_road(10, Orientation.NORTH)
for n in road:
self.assertIsNone(n.current_car)
p = Path([0] * 11)
road[0].current_car = Car(p, road[0])
for i in range(10):
# Empty before the car
for j in range(i):
self.assertIsNone(road[j].current_car)
# Check if the car is at the right place
self.assertIsNotNone(road[i].current_car)
# Empty after the car
for j in range(i + 1, len(road)):
self.assertIsNone(road[j].current_car)
compute_next(road)
apply_next(road)
def create_car(self, urdf_name, init_pose=None, noise=None, props=None):
if urdf_name in self.urdf_paths:
zeroPos = [0.0, 0.0, 0.0]
zeroOrient = p.getQuaternionFromEuler([0.0, 0.0, 0.0])
self._carID = p.loadURDF(self.urdf_paths[urdf_name], zeroPos,
zeroOrient)
if init_pose:
if "pos" in init_pose:
carPos = init_pose["pos"]
else:
carPos = zeroPos
if "orient" in init_pose:
carOrient = init_pose["orient"]
else:
carOrient = zeroOrient
self.initCarPos = carPos
self.initCarOrient = carOrient
else:
carPos = [
random.uniform(self.spawn_bounds[0][0],
self.spawn_bounds[0][1]),
random.uniform(self.spawn_bounds[1][0],
self.spawn_bounds[1][1]),
random.uniform(self.spawn_bounds[2][0],
self.spawn_bounds[2][1]),
]
carOrient = [0.0, 0.0, random.uniform(0, 2 * math.pi)]
self.initCarPos = None
self.initCarOrient = None
self._cars[self._carID] = Car(self._carID, carPos, carOrient,
props)
self.car_noise = noise
return self._carID
else:
return None
import time
from simulator.car import Car
from simulator.recorder import Recorder
from simulator.simulator import Simulator
simulator = Simulator()
car = Car(simulator)
simulator.connect()
simulator.start()
print car.set_throttle(.3223)
print car.get_throttle()
print car.set_steering_angle(.1)
print car.get_steering_angle()
duration = time.time() + 10
recorder = Recorder(car)
try:
recorder.start()
time.sleep(5)
finally:
recorder.stop()
simulator.stop()
class Simulator(object):
"""
map_type: "label", "image"
"""
def __init__(self, map="rally_map2.png", fps=10):
self.script_dir = os.path.dirname(__file__)
# 맵 정보 불러오기
map_path = os.path.join(self.script_dir, "map", map)
self.map = cv.imread(map_path)
self.car = Car()
self.fps = fps
self.DRIVE = self.car.DRIVE
self.REVERSE = self.car.REVERSE
self.BREAK = self.car.BREAK
self.max_steering_deg = self.car.max_steering_deg
self.reward_domain = get_reward_domain(self.map, self.car)
self.is_done = False
def render(self, fps=None):
background = self.map.copy()
background = draw_reward_domain(background, self.reward_domain)
background = draw_car(background, self.car)
background = draw_ultrasonic(background, self.car, self.map)
cv.imshow("Simulator", background)
if fps:
cv.waitKey(int(1000/fps))
else:
cv.waitKey(int(1000/self.fps))
def step(self, gear, steering_deg):
self.car.update(1/self.fps, gear, steering_deg)
distances_meter = self._get_ultrasonics_distances()
is_done, reward = is_episode_done(self.map, self.car, self.reward_domain)
if is_done or np.min(distances_meter) <= 10:
self.is_done = True
reward = -1
else:
self.is_done = False
if reward != 0:
self.reward_domain = get_reward_domain(self.map, self.car)
return distances_meter, reward, self.is_done
def reset(self):
self.is_done = False
self.car.reset()
self._set_random_start_pos()
distances_meter = self._get_ultrasonics_distances()
self.reward_domain = get_reward_domain(self.map, self.car)
return distances_meter, 0, self.is_done
def _set_random_start_pos(self):
map_height, map_width = self.map.shape[:2]
while True:
self.car.position = (np.random.randint(map_height), np.random.randint(map_width))
self.car.yaw = np.random.uniform(-np.pi, np.pi)
distances_meter = self._get_ultrasonics_distances()
is_done, _ = is_episode_done(self.map, self.car)
if not (is_done or np.min(distances_meter) <= 15):
break
def _get_ultrasonics_distances(self):
ultrasonic_start_points, ultrasonic_end_points, _ = get_ultrasonic_distance(self.map, self.car)
distances = np.sqrt(np.sum((ultrasonic_start_points - ultrasonic_end_points)**2, axis=1))
distances_meter = rint(distances * self.car.meter_per_pixel * 100)
return distances_meter
def createCarInPosition(self, thinker, streetId, position):
self.cars.append(Car(thinker, streetId, position, self.map))
def createCar(self, thinker, streetId):
self.cars.append(Car(thinker, streetId, self.map.streets[streetId].start, self.map))
from threading import Thread
import cv2
from simulator import renderer
from simulator.car import Car
from simulator.scene import Scene
from carla_sim.coneTest import ConeTest
# Set up the basic scene
from simulator.track import Track
track = Track("tracks/brands_hatch.json")
car = Car(pos=track.car_pos)
scene = Scene()
scene.set_track(track)
scene.add_car(car)
scene.gen_pedestrians(200)
def run_rl():
cv2.imshow("", renderer.render_scene(scene) / 255)
cv2.waitKey(0)
def run_carla_simulator():
simulator = ConeTest()
simulator.set_scene(scene)