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)