def __init__(self):
self.score = 0
self.rows = 4
self.columns = 4
self.empty_tiles = []
self.board = [[0 for column in range(self.columns)]
for row in range(self.rows)]
self.collision_detector = CollisionDetector(self)
def test_convert_position_to_coordinate_sw_equals_to_0_0(self):
cd = CollisionDetector(logger_handler=sys.stdout,
send_warning_callback=None,
send_intersection_info_callback=None,
deltatime=None)
sw = Position()
sw.lat = cd.sw[0]
sw.lon = cd.sw[1]
self.assertEqual(cd.position_to_coordinate(sw), (0, 0))
def test_position_coordinate_inversion(self):
cd = CollisionDetector(logger_handler=sys.stdout,
send_warning_callback=None,
send_intersection_info_callback=None,
deltatime=None)
coordinate = cd.position_to_coordinate(self._bus_position)
convertedPosition = cd.coordinate_to_position(coordinate)
self.assertEqual(
(round(self._bus_position.lat, 4), round(self._bus_position.lon,
4)),
(round(convertedPosition.lat, 4), round(convertedPosition.lon, 4)))
def test_convert_position_to_coordinate_ne_equals_to_distance(self):
cd = CollisionDetector(logger_handler=sys.stdout,
send_warning_callback=None,
send_intersection_info_callback=None,
deltatime=None)
ne = Position()
ne.lat = cd._ne[0]
ne.lon = cd.ne[1]
self.assertEqual(cd.position_to_coordinate(ne),
(round(cd._distance_between_sw_and_se),
round(cd._distance_between_sw_and_nw)))
def test_projected_position(self):
cd = CollisionDetector(logger_handler=sys.stdout,
send_warning_callback=None,
send_intersection_info_callback=None,
deltatime=None)
cp = VehiclePositionProjector()
projected_bike_position = cp.projected_position(
5, 30, cd.position_to_coordinate, self._bike_position)
print(projected_bike_position.polygon.exterior.coords)
bike_exterior_positions = map(
cd.coordinate_to_position,
projected_bike_position.polygon.exterior.coords)
print("Bike's projected position")
for pos in bike_exterior_positions:
print("{" + "lat: {0}, lng: {1}".format(pos.lat, pos.lon) + "},")
projected_bus_position = cp.projected_position(
5, 315, cd.position_to_coordinate, self._bus_position)
bus_exterior_positions = map(
cd.coordinate_to_position,
projected_bus_position.polygon.exterior.coords)
print("Bus's projected position")
for pos in bus_exterior_positions:
print("{" + "lat: {0}, lng: {1}".format(pos.lat, pos.lon) + "},")
self.assertFalse(
projected_bus_position.intersects(projected_bike_position))
def __init__(self):
self.__game_settings = GameSettings()
self.__screen = Screen(self.__game_settings.screen_settings)
self.__ship = Ship(self.__game_settings.ship_settings, self.__screen)
self.__bullets = Bullets(self.__game_settings.bullets_settings,
self.__ship, self.__screen)
self.__dvd_logos = DVDLogos(self.__game_settings.dvd_logos_settings,
self.__ship, self.__screen)
self.__collison_detector = CollisionDetector(
self.__game_settings.collison_detector_settings, self.__ship,
self.__bullets, self.__dvd_logos)
self.__event_checker = EventChecker(self.__game_settings, self.__ship,
self.__bullets)
def start(self):
ship = Ship(self)
ship.move(self.get_window_center_x(), self.get_window_center_y())
ShipController(self, ship)
collision_detector = CollisionDetector()
for _ in range(50):
bubble = self.bubble_factory.create_random(self)
bubble.move(self.get_random_window_x(), self.get_random_window_y())
self.bubble_controller.register(bubble)
while 1:
for bubble in self.bubble_controller.bubbles:
if collision_detector.are_colliding(ship, bubble):
self.delete_bubble(bubble)
self.remove_if_outside(bubble)
self.bubble_controller.move_all()
self.update()
sleep(0.01)
def __init__(self):
# Load configuration
cfg = Configuration('config.yaml')
self._cfg = cfg
loglevel = self._name_to_loglevel(cfg.data['loglevel'])
self._configure_logger(loglevel)
self._logger = logging.Logger("Application")
deltatime = cfg.data['deltatime']
vehicle_type_a = cfg.data['vehicle_type_a']
vehicle_type_b = cfg.data['vehicle_type_b']
# TODO: Figure out why this is necessary
self._logger.addHandler(stdout)
self._collision_detector = CollisionDetector(logger_handler=stdout, loglevel=loglevel, send_warning_callback=self.send_warning,
send_intersection_info_callback=self.send_intersection_info, deltatime=deltatime)
self._position_handler = PositionHandler(self._collision_detector._group_a, self._collision_detector._group_b, vehicle_type_a, vehicle_type_b, logger_handler=stdout, loglevel=loglevel)
self.loop = asyncio.get_event_loop()
self.loop.set_debug(True)
class SimpleGameWrittenInPygame():
def __init__(self):
self.__game_settings = GameSettings()
self.__screen = Screen(self.__game_settings.screen_settings)
self.__ship = Ship(self.__game_settings.ship_settings, self.__screen)
self.__bullets = Bullets(self.__game_settings.bullets_settings,
self.__ship, self.__screen)
self.__dvd_logos = DVDLogos(self.__game_settings.dvd_logos_settings,
self.__ship, self.__screen)
self.__collison_detector = CollisionDetector(
self.__game_settings.collison_detector_settings, self.__ship,
self.__bullets, self.__dvd_logos)
self.__event_checker = EventChecker(self.__game_settings, self.__ship,
self.__bullets)
def run_game(self):
pygame.init()
to_update = [
self.__collison_detector, self.__ship, self.__bullets,
self.__dvd_logos
]
to_draw = [self.__ship, self.__bullets, self.__dvd_logos]
screen_surface = self.__screen.surface
self.__dvd_logos.create(15)
while True:
self.__event_checker.check_events()
self.__screen.update()
for x in to_update:
x.update()
for x in to_draw:
x.draw(screen_surface)
self.__screen.flip()
if self.__dvd_logos.is_any_logo() != True:
print("YOU WIN! :D")
return 0
if self.__collison_detector.ship_was_hit():
print("YOU LOSE! :<")
return 0
def test_initialization(self):
bus = Vehicle(Position(), "bus1")
bike = Vehicle(Position(), "bike1")
self.assertEqual(bus.id, "bus1")
self.assertEqual(bike.id, "bike1")
cd = CollisionDetector(logger_handler=sys.stdout,
send_warning_callback=None,
send_intersection_info_callback=None,
deltatime=None)
cd.add_to_group_B(bike)
cd.add_to_group_A(bus)
self.assertEqual(bike, cd.group_B["bike1"])
self.assertEqual(bus, cd.group_A["bus1"])
class Board:
# initialize the board as a 4x4 matrix filled with 0's to indicate unoccupied tiles
def __init__(self):
self.score = 0
self.rows = 4
self.columns = 4
self.empty_tiles = []
self.board = [[0 for column in range(self.columns)]
for row in range(self.rows)]
self.collision_detector = CollisionDetector(self)
# prints the board to the terminal
def print(self):
for row in range(self.rows):
for column in range(self.columns):
print('|', end=' ')
if self.board[row][column] == 0:
print('-', end=' ')
else:
print(self.board[row][column], end=' ')
print('|', end=' ')
print()
print('score: ' + str(self.score))
# clear the board
def clear(self):
for row in range(self.rows):
for column in range(self.columns):
self.board[row][column] = 0
self.empty_tiles.append((row, column))
# determines if the board is completely filled with tiles
def is_board_full(self):
if len(self.empty_tiles) == 0:
return True
else:
return False
# insert a tile into a position on the board
def insert_tile(self, row, column, tile):
if not self.is_tile_occupied(row, column):
self.board[row][column] = tile
self.empty_tiles.remove((row, column))
else:
print('tile is occupied')
# insert a random 2 tile or a 4 tile on any unoccupied tile of the boardS
def insert_random_tile(self):
if not self.is_board_full():
tile = self.empty_tiles[random.randint(0,
len(self.empty_tiles) - 1)]
# 90% chance of inserting a 2 tile, 10% chance of inserting a 4 tile
if random.random() < 0.9:
self.insert_tile(tile[0], tile[1], 2)
else:
self.insert_tile(tile[0], tile[1], 4)
# clears the board and inserts two random tiles
def new_game(self):
self.score = 0
self.clear()
for i in range(2):
self.insert_random_tile()
# checks to see if a tile is occupied at a given position
def is_tile_occupied(self, row, column):
if self.board[row][column] == 0:
return False
else:
return True
# moves a tile in a given direction with the correct collision logic
def move_tile(self, row, column, direction):
if direction == 'up':
new_row = self.collision_detector.detect_up_collision(row, column)
if self.board[new_row][column] == self.board[row][
column] and new_row != row:
self.score += 2 * self.board[row][column]
self.board[new_row][column] = 2 * self.board[row][column]
self.board[row][column] = 0
self.empty_tiles.append((row, column))
elif new_row != row:
self.board[new_row][column] = self.board[row][column]
self.board[row][column] = 0
self.empty_tiles.append((row, column))
self.empty_tiles.remove((new_row, column))
if direction == 'down':
new_row = self.collision_detector.detect_down_collision(
row, column)
if self.board[new_row][column] == self.board[row][
column] and new_row != row:
self.score += 2 * self.board[row][column]
self.board[new_row][column] = 2 * self.board[row][column]
self.board[row][column] = 0
self.empty_tiles.append((row, column))
elif new_row != row:
self.board[new_row][column] = self.board[row][column]
self.board[row][column] = 0
self.empty_tiles.append((row, column))
self.empty_tiles.remove((new_row, column))
if direction == 'left':
new_column = self.collision_detector.detect_left_collision(
row, column)
if self.board[row][new_column] == self.board[row][
column] and new_column != column:
self.score += 2 * self.board[row][column]
self.board[row][new_column] = 2 * self.board[row][column]
self.board[row][column] = 0
self.empty_tiles.append((row, column))
elif new_column != column:
self.board[row][new_column] = self.board[row][column]
self.board[row][column] = 0
self.empty_tiles.append((row, column))
self.empty_tiles.remove((row, new_column))
if direction == 'right':
new_column = self.collision_detector.detect_right_collision(
row, column)
if self.board[row][new_column] == self.board[row][
column] and new_column != column:
self.score += 2 * self.board[row][column]
self.board[row][new_column] = 2 * self.board[row][column]
self.board[row][column] = 0
self.empty_tiles.append((row, column))
elif new_column != column:
self.board[row][new_column] = self.board[row][column]
self.board[row][column] = 0
self.empty_tiles.append((row, column))
self.empty_tiles.remove((row, new_column))
# moves the tiles of the board in a given direction with the correct collision logic
def move(self, direction):
if direction == 'up':
for row in range(self.rows):
for column in range(self.columns):
if self.is_tile_occupied(row, column):
self.move_tile(row, column, direction)
elif direction == 'down':
for row in range(self.rows - 1, -1, -1):
for column in range(self.columns):
if self.is_tile_occupied(row, column):
self.move_tile(row, column, direction)
elif direction == 'left':
for column in range(self.columns):
for row in range(self.rows):
if self.is_tile_occupied(row, column):
self.move_tile(row, column, direction)
elif direction == 'right':
for column in range(self.columns - 1, -1, -1):
for row in range(self.rows):
if self.is_tile_occupied(row, column):
self.move_tile(row, column, direction)
# determines whether a given move is valid or not, aka it changes the board state
def valid_move(self, direction):
board_copy = deepcopy(self)
board_copy.move(direction)
if self.different_boards(board_copy):
return True
else:
return False
# compares two board states to determine if there is a difference or not
def different_boards(self, board):
for row in range(self.rows):
for column in range(self.columns):
if self.board[row][column] != board.board[row][column]:
return True
return False
# returns a list of the valid moves given the current board state
def valid_moves(self):
valid_moves = []
for direction in ['up', 'down', 'left', 'right']:
board_copy = deepcopy(self)
if board_copy.valid_move(direction):
valid_moves.append(direction)
return valid_moves
# manually sets the tile of the board
def set_board(self):
for row in range(self.rows):
for column in range(self.columns):
tile = int(input(''))
self.board[row][column] = tile
def clone(self):
return deepcopy(self)
class Appplication:
"""Main application class"""
@property
def COLLISION_DETECTOR_VERSION(self):
"Collision detector version"
return "1"
def __init__(self):
# Load configuration
cfg = Configuration('config.yaml')
self._cfg = cfg
loglevel = self._name_to_loglevel(cfg.data['loglevel'])
self._configure_logger(loglevel)
self._logger = logging.Logger("Application")
deltatime = cfg.data['deltatime']
vehicle_type_a = cfg.data['vehicle_type_a']
vehicle_type_b = cfg.data['vehicle_type_b']
# TODO: Figure out why this is necessary
self._logger.addHandler(stdout)
self._collision_detector = CollisionDetector(logger_handler=stdout, loglevel=loglevel, send_warning_callback=self.send_warning,
send_intersection_info_callback=self.send_intersection_info, deltatime=deltatime)
self._position_handler = PositionHandler(self._collision_detector._group_a, self._collision_detector._group_b, vehicle_type_a, vehicle_type_b, logger_handler=stdout, loglevel=loglevel)
self.loop = asyncio.get_event_loop()
self.loop.set_debug(True)
def _name_to_loglevel (self, loglevel_name):
if(loglevel_name == logging.getLevelName(logging.DEBUG)) :
return logging.DEBUG
else :
return logging.INFO
def _configure_logger(self, loglevel):
root = logging.getLogger()
root.setLevel(loglevel)
global stdout
stdout = logging.StreamHandler(sys.stdout)
stdout.setLevel(loglevel)
formatter = logging.Formatter(
'%(asctime)s - %(name)s - %(levelname)s - %(message)s')
stdout.setFormatter(formatter)
# TODO: Why is it not enough to add the handler to the root logger??
root.addHandler(stdout)
def _get_position_projector(self, vehicle_type ):
vehicle_type_to_projector = {
"bus" : VehiclePositionProjector(),
"bike" : VehiclePositionProjector(),
"bicycle" : VehiclePositionProjector(),
"car" : VehiclePositionProjector(),
"smv" : VehiclePositionProjector(),
"person" : PersonPositionProjector()
}
return vehicle_type_to_projector.get(vehicle_type)
async def run(self):
"""Starts the application"""
log = self._logger
log.info("Starting collision_detector...")
atexit.register(self.clean_up) # Register clean-up
cfg = self._cfg
log.info("Configuration parsed " + yaml.dump(self._cfg.data))
# Start NATS async IO
nats = NatsHandler(
cfg.data['nats-connection-string'],
position_handler=self._position_handler,
logger_handler=stdout,
loop=self.loop)
if cfg.data['nats-enabled']:
nats_connect_task = self.loop.create_task(nats.connect())
else:
log.warning("NATS disabled in configuration")
self._nats = nats
# Register collision monitoring loop
if cfg.data['collision_detector-enabled']:
projector_a = self._get_position_projector(cfg.data['vehicle_type_a'])
projector_b = self._get_position_projector(cfg.data['vehicle_type_b'])
cd_task = self.loop.create_task(self._collision_detector.monitor_vehicles(cfg.data["stale-position-time"], projector_a, projector_b, self.loop))
else:
log.warning("Collision detector disabled in configuration")
await nats_connect_task
await cd_task
def clean_up(self):
self._logger.info("Terminating the application")
self._nats.close()
self._logger.info("Termination complete")
def send_warning(self, distance, direction, threat_id, vehicle_id):
"""Sends a collision warning to the intersecting vehicle, wraps it and sends it to NATS"""
self._logger.debug("Send warning")
message = Warning()
message.threat_direction = direction
message.threat_distance = distance
message.threat_identifier = threat_id
self._nats.send_message(message, vehicle_id)
def send_intersection_info(self, distance, midpoint, bus, bike):
"""Send intersection information"""
self._logger.debug("Send intersection info")
message = IntersectionInfo(distance, midpoint, bus, bike )
self._nats.send_intersection_info(message)
)
sphere_manager = SphereManager(cfg.n_spheres, cfg.spheres_pos, cfg.spheres_r,
cfg.dronehitbox_r, cfg.safety_margin)
prism_manager = PrismManager(cfg.n_prisms, cfg.prisms, cfg.prisms_pos,
cfg.dronehitbox_r, cfg.safety_margin)
beam_manager = BeamManager(cfg.n_beams, cfg.beams, cfg.beams_pos,
cfg.dronehitbox_r, cfg.safety_margin)
#create obstacle objects
sphere_array = sphere_manager.create_spheres()
prism_array = prism_manager.create_prisms()
beam_array = beam_manager.create_beams()
collision_detector = CollisionDetector(cfg.safety_margin, sphere_array,
prism_array, beam_array,
cfg.dronehitbox_r)
controller = Controller(drone)
seed = 1
iter = 600
rrt = RRT_star(
start=np.asarray(cfg.start),
goal=np.asarray(cfg.goal),
search_range=0.5,
domain=(xs, ys, zs),
collision_manager=collision_detector,
controller=controller,
informed=False,
kinodynamic=