def enter_at_start(self, prob1, prob2):
for i in range(3):
if random.random() < prob1:
self.lanes[0].addCar(vehicle.Vehicle(base=4, id=5), 4*i)
for i in range(4):
if random.random() < prob2:
self.lanes[1].addCar(vehicle.Vehicle(base=4, id=6), 4*i)
def __init__(self, nodes: int = 0, edges: int = 0, mapa: map.Map = None, vehicles_no: int = 100, cycles_number=100,
slicing_type=SlicingType.MULTI_POINT_VISITED_EPSILON):
self.avgerage_ages = []
self.done_cycles = 0
self.charger_nums_of_best = []
self.kilometrages_of_best = []
self.visited_nodes_num_of_best = []
self.nodes_to_chargers_ratios_of_best = []
self.cycles_number = cycles_number
self.slicing_type = slicing_type
self.best_vehicle = None
self.slice_epsilon = 2
if mapa is None:
self.mapa = map.Map(nodes, edges, as_complete=False, tries=10000)
else:
self.mapa = copy.deepcopy(mapa)
self.map_solution = self.mapa
self.map_binary = None
self.vehicles = []
for i in range(vehicles_no):
self.vehicles.append(vehicle.Vehicle(self.mapa))
while self.get_vehicles_number() % 4:
self.vehicles.append(vehicle.Vehicle(self.mapa))
def test_moves_one_car_bottom_edge_vertical(self):
v = vehicle.Vehicle('X', 0, 4, 'V')
expected_v = vehicle.Vehicle('X', 0, 3, 'V')
r = rushhour.RushHour(set([v]))
expected_r = rushhour.RushHour(set([expected_v]))
next_moves = set([move for move in r.moves()])
self.assertEqual(1, len(next_moves))
self.assertIn(expected_r, next_moves)
def test_moves_one_car_right_edge_horizontal(self):
v = vehicle.Vehicle('X', 4, 0, 'H')
expected_v = vehicle.Vehicle('X', 3, 0, 'H')
r = rushhour.RushHour(set([v]))
expected_r = rushhour.RushHour(set([expected_v]))
next_moves = set([move for move in r.moves()])
self.assertEqual(1, len(next_moves))
self.assertIn(expected_r, next_moves)
def test_create_bad_y(self):
with self.assertRaises(ValueError):
vehicle.Vehicle('X', 0, None, 'H')
with self.assertRaises(ValueError):
vehicle.Vehicle('X', 0, -1, 'H')
with self.assertRaises(ValueError):
vehicle.Vehicle('X', 0, 6, 'H')
def test_inequality(self):
v1 = vehicle.Vehicle('X', 0, 0, 'H')
v2 = vehicle.Vehicle('A', 0, 0, 'H')
v3 = vehicle.Vehicle('X', 1, 0, 'H')
v4 = vehicle.Vehicle('X', 0, 1, 'H')
v5 = vehicle.Vehicle('X', 0, 0, 'V')
self.assertNotEqual(v1, v2)
self.assertNotEqual(v1, v3)
self.assertNotEqual(v1, v4)
self.assertNotEqual(v1, v5)
def test_moves_one_car_middle_vertical(self):
v = vehicle.Vehicle('X', 0, 1, 'V')
expected_v1 = vehicle.Vehicle('X', 0, 0, 'V')
expected_v2 = vehicle.Vehicle('X', 0, 2, 'V')
r = rushhour.RushHour(set([v]))
expected_r1 = rushhour.RushHour(set([expected_v1]))
expected_r2 = rushhour.RushHour(set([expected_v2]))
next_moves = set([move for move in r.moves()])
self.assertEqual(2, len(next_moves))
self.assertIn(expected_r1, next_moves)
self.assertIn(expected_r2, next_moves)
def test_create_bad_orientation(self):
with self.assertRaises(ValueError):
vehicle.Vehicle('X', 0, 0, None)
with self.assertRaises(ValueError):
vehicle.Vehicle('X', 0, 0, 'h')
with self.assertRaises(ValueError):
vehicle.Vehicle('X', 0, 0, 'v')
with self.assertRaises(ValueError):
vehicle.Vehicle('X', 0, 0, 'x')
def crossing(self):
for i in range(0, int(self.get_vehicles_number() / 2)):
v1 = self.vehicles[i * 2]
v2 = self.vehicles[i * 2 + 1]
v3 = vehicle.Vehicle(self.mapa)
v4 = vehicle.Vehicle(self.mapa)
self.offspring(v1, v2, v3)
self.offspring(v2, v1, v4)
self.vehicles.append(v3)
self.vehicles.append(v4)
def manualSet(self, modelList):
self.vehs = []
for i in range(self.N):
modelTag = modelList[i]
veh_temp = vehicle.Vehicle(self.trafficModel[modelTag], i * 10)
self.vehs.append(veh_temp)
while self.collisionCheck()[0]:
self.vehs.pop()
modelTag = random.randint(
0,
len(self.trafficModel) -
1) if self.laneNum == 1 else random.randint(0, 23)
veh_temp = vehicle.Vehicle(self.trafficModel[modelTag])
self.vehs.append(veh_temp)
def calcolateDistances(g, obstacles):
distances = np.zeros(input_sh)
# la variabile di offset serve perchè i sensori DEVONO sempre essere in numero dispari, ordinati
# da sinistra a destra (o il contrario, questo dipende da come è implementato il veicolo)
# il vettore ha come primo elemento la distanza del sensore di sinistra e come ultimo elemento la
# la distanza del sensore di destra (o viceversa)
offset = int(input_sh / 2)
j = -offset # -1 = -int(input_sh / 2)
gir_prov = gr.Vehicle((0, 0))
while j <= offset: # 1 = int(input_sh / 2)
action = g.old_action + j
if action > 0:
action = action % 8
if action < 0:
action = action + 8
gir_prov.pos = g.pos
for i in range(6): #calcola per una distanza 6
gir_prov.update_pos(action)
if checkCollision(gir_prov, obstacles):
distances[j + offset] = i + 1
break
if i == 5:
distances[j + offset] = 6
j += 1
return distances
def makeVehicle(theta, direc, r):
global info, vehR
radius = vehR
if (TWO_LANE):
tX = trackX
if (r <= outR + TRACK_WIDTH and r >= outR): radius += TRACK_WIDTH
elif (direc == CTR_CLK): tX = trackRightX
else: tX = trackLeftX
v = vehicle.Vehicle(tX, trackY, radius, theta, direc, len(vehicles))
vehCanvas = canvas.create_polygon(v.getVehPoints(), fill='red')
wheelsCanvas = []
for wP in v.getWheelPoints():
wheelsCanvas.append(canvas.create_polygon(wP, fill='black'))
dirCanvas = canvas.create_polygon(v.getDirPoints(), fill='yellow')
idCanvas = canvas.create_text(v.getX(), v.getY(), text=str(v.getID()))
detCanvas = canvas.create_oval(v.getX() - idm.DETECTION_DIST,
v.getY() - idm.DETECTION_DIST,
v.getX() + idm.DETECTION_DIST,
v.getY() + idm.DETECTION_DIST,
outline="DarkOrange2")
if (detectionRadius): canvas.itemconfig(detCanvas, state=tk.NORMAL)
else: canvas.itemconfig(detCanvas, state=tk.HIDDEN)
v.setCanvas([vehCanvas, wheelsCanvas, dirCanvas, idCanvas, detCanvas])
vehicles.append(v)
infoLabel.configure(text=infoListToText())
def inTrack(x, y):
global vehR
# Check for overlap
direc, tX, tY, tR = CLK, trackLeftX, trackY, vehR
if (TWO_LANE): tX = trackX
elif (x > CANVAS_WIDTH // 2): direc, tX = CTR_CLK, trackRightX
for v in vehicles:
x1 = x - tX
y1 = y - tY
r = math.sqrt((x1)**2 + (y1)**2)
a = toDegrees(math.acos(float(x1 / r)))
if (y1 > 0): a = (a * -1.0) % MAX_DEG
if (TWO_LANE and r > outR): tR += TRACK_WIDTH
checkV = vehicle.Vehicle(tX, tY, tR, a, direc, -1)
vehicleIntersectionCollide(v, checkV)
if (vehiclesCollide(v, checkV)): return (None, None)
# Check for 2 lane track first
r = math.sqrt((tX - x)**2 + (tY - y)**2)
if (TWO_LANE and ((r <= outR and r >= (outR - TRACK_WIDTH)) or
(r <= outR + TRACK_WIDTH and r >= outR))):
return (r, CLK)
elif (r <= outR and r >= (outR - TRACK_WIDTH)):
return (r, direc)
return (None, None)
def getVehicles(self, board):
"""Convert board into a list of vehicles."""
names = []
vehicles = []
# read in board
for i in range(len(board)):
# current coordinates
x = i % self.size
y = i // self.size
# make new vehicles
if board[i] != '.' and board[i] not in names:
vehicles.append(v.Vehicle(board[i], x, y, 1, 'N'))
names.append(board[i])
# change length if vehicle does exist
elif board[i] in names:
index = names.index(board[i])
car = vehicles[index]
car.length += 1
if x == car.xBegin:
car.orientation = 'V'
else:
car.orientation = 'H'
return vehicles
def drive(cfg, client_ip=None, to_control=False):
#Initialize car
V = vehicle.Vehicle()
client_ip = client_ip or cfg.CLIENT_IP
camA = CSICamera(image_w=cfg.IMAGE_W,
image_h=cfg.IMAGE_H,
image_d=cfg.IMAGE_DEPTH,
framerate=cfg.CAMERA_FRAMERATE,
gstreamer_flip=cfg.CSIC_CAM_GSTREAMER_FLIP_PARM,
client_ip=client_ip)
camB = RS_D435i(image_w=cfg.IMAGE_W,
image_h=cfg.IMAGE_H,
image_d=cfg.IMAGE_DEPTH,
framerate=cfg.CAMERA_FRAMERATE,
client_ip=client_ip)
V.add(camA, outputs=['cam/image_array_a'], threaded=True)
V.add(camB,
outputs=['cam/image_array_b', 'cam/image_array_c'],
threaded=True)
if to_control:
V.add(NaiveController(),
outputs=['throttle', 'steering'],
threaded=True)
V.add(Sender(), inputs=['throttle', 'steering'], threaded=True)
else:
V.add(Receiver(client_ip), threaded=True)
#run the vehicle for 20 seconds
V.start(rate_hz=cfg.DRIVE_LOOP_HZ, max_loop_count=cfg.MAX_LOOPS)
def createvehicle(sid):
vehicle_id = str(uuid.uuid4())
vehicles[vehicle_id] = vehicle.Vehicle()
setvehicle(sid, vehicle_id)
# sio.emit("vehicle_id", vehicle_id, to=sid)
print(f"User {sid} created {vehicle_id}")
return vehicle_id
def generatePath():
robot = vehicle.Vehicle(1, 50) #create a robot
robot.setOdometry(True) #configure its odometer
robot.setOdometryVariance([0.2, 1])
global speed
global angle
speed, angle = [], []
for a in xrange(4): #create a retangular path
for i in xrange(400):
angle.append(0)
for i in xrange(9):
angle.append(10)
for i in xrange(len(angle)): #set the speed to a constant along the path
speed.append(1)
robot.sim_Path(speed, angle) #run in a rectangular path
speed, angle = robot.readOdometry() #reads the sensors
cam = upper_cam.UpperCam([10, 10, 0.5],
robot) #create a cam upper the path
cam.readPath() #read the robot path
camPoses = cam.readPoses() #save the noise path
camPoses[2] = 0.01745 * np.array(camPoses[2]) #convert the angular to rad
real = robot.poses
real[2] = 0.01745 * np.array(real[2]) #convert the angular to rad
return real, camPoses
def initGame():
#inizializzo il veicolo
g = gr.Vehicle(
(startX, startY)) # inizializzo con la posizione prestabilita
obstacles = list() # creo gli ostacoli in posizione random
for i in range(numOst):
poz = (random.randint(1, N - 1), random.randint(1, N - 1))
if poz != (startX, startY):
obstacles.append(poz)
# inserisco i bordi
i = 1
while i <= N - 2:
poz = (0, i)
poz1 = (i, 0)
poz2 = (N - 1, i)
poz3 = (i, N - 1)
#poz3 = (i, 6)
obstacles.append(poz)
obstacles.append(poz1)
obstacles.append(poz2)
obstacles.append(poz3)
i += 1
# inserisco gli angoli
obstacles.append((0, 0))
obstacles.append((N - 1, N - 1))
obstacles.append((0, N - 1))
obstacles.append((N - 1, 0))
return g, obstacles
def test_iterate(self):
self.available_paths = []
start_node, stop_node = random.sample(range(0, self.map_solution.size - 1), 2)
print("Checking for available paths...")
self.check_available_paths(start_node, stop_node)
print("Paths found! Starting test...")
for path in self.available_paths:
veh_checker = vehicle.Vehicle(self.mapa)
veh_checker.start_node = path[0]
veh_checker.current_node = path[0]
veh_checker.chargers=self.map_solution.chargers
for i in range(1, len(path)):
if veh_checker.current_node in veh_checker.chargers:
veh_checker.charge()
if veh_checker.can_move_to(path[i]):
if path[i] == stop_node:
return True
veh_checker.move(path[i])
else:
break
return False
def create_vehicle(self, entity, position, chaser=False):
c = vehicle.Configuration()
dims = physics.PxVec3(6, 2, 12)
c.position = physics.PxVec3(position.x, position.y, position.z)
c.chassis_dimensions = dims
c.wheel_radius = 0.5
c.wheel_width = 0.4
c.wheel_mass = 10
c.chassis_mass = 1000
c.chassis_moi = physics.PxVec3(c.chassis_mass, c.chassis_mass / 2,
c.chassis_mass)
c.chassis_offset = physics.PxVec3(0, -dims.y, 0)
if chaser:
c.wheel_moi = 20
c.torque = 4500
c.max_speed = 1.0
c.steer_angle = math.pi * .15
c.max_omega = 2600
else:
c.wheel_moi = 20
c.torque = 3700
c.max_speed = 1.0
c.steer_angle = math.pi * .15
c.max_omega = 1800
self.vehicle = entity.add_component(
vehicle.Vehicle(self.physics, self.controller, c))
def enter_at_start(self, prob):
for i, lane in enumerate(self.lanes):
for j in range(4):
if lane.cells[j] == None:
if random.random() < prob:
lane.addCar(vehicle.Vehicle(base=0, id=i), j)
break
def add_vehicle(self,
length=4,
width=2,
v_change=config.car_v_change,
current=-1,
travelled=0,
slow_duration=config.car_slow_duration,
slowdown_probability=None):
'''Add a new vehicle to the lane'''
index = self.find_index(travelled)
if slowdown_probability is None:
slowdown_probability = config.car_slow_prob
if len(self.vehicles) != 0 and \
self.vehicles[index-1].travelled - self.vehicles[index-1].size["length"] <= \
-self.vehicles[index-1].size["length"]:
return
if current < 0:
current = self.starting_velocity / self.ticks_per_second
new_vehicle = vehicle.Vehicle(length, width,
self.speed_limit / self.ticks_per_second,
v_change / self.ticks_per_second,
current, slowdown_probability,
travelled - length, slow_duration,
self.ticks_per_second / 10)
self.vehicles.insert(index, new_vehicle)
def get_vehicle(self, vin: str) -> vehicle.Vehicle:
"""Retrieve a specific vehicle by its VIN.
Args:
vin: A VIN to query and return as a Vehicle object. In order to
succeed the corresponding vehicle must be registered in the
ConnectedDrive portal.
Returns:
A new Vehicle representing the requested vehicle.
Raises:
KeyError: No vehicle with a matching VIN is listed in this account.
"""
vehicle_data = None
api_response = self.call_endpoint('vehicles')
for vehicle_record in api_response['vehicles']:
if vehicle_record['vin'] == vin:
vehicle_data = vehicle_record
break
else:
raise KeyError(
'No vehicle with VIN {vin} is registered'.format(vin=vin)
)
api_response = self.call_endpoint(
'vehicles/{vin}/status'.format(vin=vin))
return vehicle.Vehicle(
self, vehicle_data, api_response['vehicleStatus']
)
def plot_single_run():
height = 10
width = 10
m = mapgrid.generate_map(width=width, height=height)
v = vehicle.Vehicle(m, 10)
plot = plotter.LocatorPlot(v)
plot.plot_map()
# The placement of the vehicle need not be shown
# plot.plot_vehicle()
log.info("Start location:", str(v.location()))
log.info("Start color:", v.get_color())
for i in range(10):
v.move_unbound()
plot.plot_vehicle()
# found, x, y = locator.locate(v.map(), v.history(), v)
num_matches, x, y, possible_loc = locator.locate(v.map(), v.history())
log.info("End location:", v.location())
log.info("End color:", v.get_color())
if num_matches == 1:
loc_x, loc_y, dir = v.location()
log.info(
"Found on location", x, y, " and its correctness is " + str(
(num_matches == 1) and (x == loc_x) and (y == loc_y)))
else:
log.warning("Location not found!")
# Graph testing
"""
history_graphs = []
for i in range(4):
history_graph = graph_utils.history_to_digraph(v.history(), i)
history_graphs.append(history_graph)
graph_utils.digraph_history_analyzer(history_graph)
map_graph = graph_utils.map_to_digraph(m)
for i in range(4):
match = graph_utils.graph_matcher(map_graph, history_graphs[i])
print("Start direction", direction.Direction(i), "graph match:", match)
"""
# History error handling test
locator.locate_with_one_possible_error(v.map(), v.history())
locator.locate_with_one_possible_error(v.map(), v.history_error_one())
locator.locate_with_error_fallback(v.map(), v.history())
locator.locate_with_error_fallback(v.map(), v.history_error_one())
# locator.locate_with_movement_and_error_fallback(v.map(), v.history(), v, retries=10)
# locator.locate_with_movement_and_error_fallback(v.map(), v.history_error_one(), v, retries=10)
plot.show()
def create_vehicle(self):
new_vehicle = vehicle.Vehicle(self.scene.addObject("VehicleRoot"))
trail = self.trail_manager.attach_trail(
new_vehicle.get_trail_attach_point(), 5.0, 10.0,
(1.0, 1.0, 0.0, 0.0))
new_vehicle.set_trail(trail)
return new_vehicle
async def put_new_vehicle(vehicle_name: str):
vehicle_id = str(uuid.uuid4())
# vehicles[vehicle_id] = vehicle.Vehicle()
vehicles[vehicle_name] = vehicle.Vehicle()
# vehicles[vehicle_id].id = vehicle_id
vehicles[vehicle_name].id = vehicle_name
vehicles[vehicle_name].name = vehicle_name
print("VEHICLE ADDED")
return vehicle_name
def test_history_error_generation(self):
m = mapgrid.generate_map(10, 10)
v = vehicle.Vehicle(m)
history_colors = v.history()[:, 1]
for i in range(10):
history_err_colors = v.history_error_one()[:, 1]
diff = np.sum(history_colors != history_err_colors)
self.assertEqual(diff, 1)
def test_create_from_invalid(self):
v = vehicle.Vehicle('X', 0, 0, 'H')
with self.assertRaises(TypeError):
r = rushhour.RushHour(v)
with self.assertRaises(TypeError):
r = rushhour.RushHour([v])
with self.assertRaises(TypeError):
r = rushhour.RushHour(tuple([v]))
def reStart(self):
self.vehs = []
state = []
for _ in range(self.N):
modelTag = random.randint(0, len(self.trafficModel) - 1)
veh_temp = vehicle.Vehicle(self.trafficModel[modelTag])
self.vehs.append(veh_temp)
while self.collisionCheck()[0]:
self.vehs.pop()
modelTag = random.randint(0, len(self.trafficModel) - 1)
veh_temp = vehicle.Vehicle(self.trafficModel[modelTag])
self.vehs.append(veh_temp)
for i in range(self.N):
pos = self.vehs[i].cen_x if (abs(self.vehs[i].cen_x) > abs(
self.vehs[i].cen_y)) else self.vehs[i].cen_y
state += [pos, self.vehs[i].trafficModel.id, self.vehs[i].vel]
return np.array(state)
def test_get_board_vertical_bottom_left(self):
v = vehicle.Vehicle('X', 0, 4, 'V')
r = rushhour.RushHour(set([v]))
expected_board = [[' ', ' ', ' ', ' ', ' ', ' '],
[' ', ' ', ' ', ' ', ' ', ' '],
[' ', ' ', ' ', ' ', ' ', ' '],
[' ', ' ', ' ', ' ', ' ', ' '],
['X', ' ', ' ', ' ', ' ', ' '],
['X', ' ', ' ', ' ', ' ', ' ']]
for line, expected_line in zip(r.get_board(), expected_board):
self.assertEqual(line, expected_line)
