def gbfs(self, problem):
q = [Path([problem.getInitial()])]
visited = []
i = 0
while q:
i = i + 1
currentPath = q.pop(0)
currentState = currentPath.getLast()
if currentState == problem.getFinal():
return currentPath, i
visited.append(currentState)
aux = []
for child in currentState.allNextStates():
if child not in visited:
aux.append(child)
aux = self.orderStates(aux)
for state in aux:
p = Path(deepcopy(currentPath.getValues()))
p.add(State(deepcopy(state.getValues())))
q.append(p)
return None, i
class BestFirst:
"""Class representing Best First algorithm
It returns a path depending on which country has the highest number of cases on the next day"""
def __init__(self, starting_country, starting_date, alg_graph, goal_function):
"""Initializes BestFirst Algorithm object
You specify the country and date you start with and a graph to work with"""
self.path = Path(starting_country, starting_date)
self.graph = alg_graph
self.goal_function = goal_function
def finding_path(self):
"""Main function to find a path according to BestFirst algorithm
It returns a path object"""
start_country, start_date = self.path.countries[0], self.path.start_date
stats = self.graph.get_stats(start_country) # Here is a country which has latest data
last_date = datetime.date(stats[0, 2], stats[0, 1], stats[0, 0])
current_country_neighbors = self.graph.get_neighbours(start_country)
current_country, current_date = start_country, start_date
while current_date != last_date:
current_country = self.choose_neighbor_with_max_cases(current_country_neighbors)
current_country_neighbors = self.graph.get_neighbours(current_country)
current_date += datetime.timedelta(days=1)
self.path.add(current_country)
self.path.eval(self.graph, self.goal_function)
def choose_neighbor_with_max_cases(self, countries_list=None):
"""Function which chooses the best country to go to next
It looks for the biggest number of cases from countries list"""
countries_list = countries_list if countries_list else []
try:
if not countries_list:
raise Exception
except Exception as e:
print("There are no neighbors to choose from: {0}".format(e))
max_cases = -1
country_with_max_cases = ''
for country in countries_list:
if self.path.get_profit(self.graph, country, self.goal_function) > max_cases:
max_cases = self.path.get_profit(self.graph, country, self.goal_function)
country_with_max_cases = country
return country_with_max_cases
def work(self):
"""Method which initializes every method essential to find a path"""
self.finding_path()
def initialize_frontier(self):
path = Path()
path.add(self.start)
heapq.heappush(self.frontier, path)
return self.frontier
class CarFree2D:
def __init__(self, id: int, spawn_x, spawn_y, start, start_dir, end_dir,
size_x, size_y, angle, max_vel, max_acc, color, min_latency,
max_latency, errorrate):
self.ghost = False
# PHYSICAL PROPERTIES
self.color = color # color code
self.id = id # unique car id
self.spawn = [spawn_x, spawn_y] # spawnpoint
self.destination = 0
self.position_x = [spawn_x] # not used for EventQueue
self.position_y = [spawn_y]
self.direction = start_dir # direction of the car
self.start_direction = angle # start-direction of the car
self.last_position = [spawn_x,
spawn_y] # position after last control input
self.length = size_x # in m
self.width = size_y # in m
self.start_dir = start_dir
self.end_dir = end_dir
self.end = None
self.min_latency = min_latency
self.max_latency = max_latency
self.errorrate = errorrate
self.start_time = start
self.wheelbase = self.length - 0.5 * self.width
# VELOCITY
self.last_velocity = 0
self.last_velocity_x = 0 # velocity after last control input
self.last_velocity_y = 0
self.velocity = [] # not used for EventQueue
self.max_velocity = max_vel # absolute limit of velocity
# ACCELERATION
self.acceleration = 0 # acceleration given by last control input
self.acceleration_y = 0
self.acceleration_x = 0
self.max_acceleration = max_acc # absolute limit of acceleration
self.acceleration_x_c = 0
self.acceleration_y_c = 0
self.acceleration_controller = 0
# STEERING
self.steering = 0 # steering angle given by last control input
self.direction = 0 # direction angle given by last control input
self.steering_control = 0
# PATH
self.shape = [] # shape of the planned path (without exact timestamp)
self.path = Path(self.spawn) # "shape" with exact timestamp
self.waypoints = [] # list for the given points with the
self.planner = None
# CONTROLS
self.time_last_control = start - lib.dt # timestamp of last control input
self.time_last_update = start - lib.dt # timestamp of last given acceleration
self.time_last_step = start - lib.dt
self.stop = False # False: car drives, True: car stops (or will stop within the next time (t < ts)
self.stop_time = 0 # timestamp of stop (velocitiy 0 reached)
self.controller = Controller(self, lib.k_p, lib.k_d)
# DEBUGGING
self.debugging1 = []
self.debugging2 = []
self.debugging3 = []
self.debugging4 = []
self.dc_pos = []
dim = len(lib.statespace.A)
self.old_state = [
np.zeros(dim).reshape(-1, 1),
np.zeros(dim).reshape(-1, 1)
]
self.state = [np.zeros(dim), np.zeros(dim)]
self.full_arc = 0
self.counter = 0
self.min_dist = self.make_min_dist()
self.distances = []
# GETTER
# currenttly not used
def get_speed(
self, absolute
): # absolute is boolean - true returns absolute value, false vectorial speed
if absolute:
return sqrt(self.velocity[0] * self.velocity[0] +
self.velocity[1] * self.velocity[1])
else:
return self.velocity
# currenttly not used
def get_acceleration(self):
return self.acceleration
def set_first_position(self):
self.position_x = [self.spawn[0]]
self.position_y = [self.spawn[1]]
self.last_position = self.spawn
# sets waypoint (given by *.json file) for the car
def set_waypoint(self, x, y):
p = Point(x, y)
self.path.add(p)
self.waypoints.append(p)
# raise Exception('The point (' + str(p.x) + '|' + str(p.y) + ') is too far away. Skipped.')
def make_min_dist(self):
edges = [
[self.spawn[0] - self.length / 2, self.spawn[1] - self.width / 2],
[self.spawn[0] + self.length / 2, self.spawn[1] - self.width / 2],
[self.spawn[0] + self.length / 2, self.spawn[1] + self.width / 2],
[self.spawn[0] - self.length / 2, self.spawn[1] + self.width / 2]
]
out = lib.dist(self.spawn, edges[0])
for e in edges:
dist = lib.dist(self.spawn, e)
if dist > out:
out = dist
return out
# creates spline for the given waypoints (from the *.json file)
def create_spline(self):
self.planner = PathPlanner(self, self.max_velocity,
self.max_acceleration)
self.planner.make_path(self.path.points)
self.write_path()
if not self.ghost:
pass
#self.make_controls()
self.make_controls()
self.planner.t_equi_in_t = (np.array(self.planner.t_equi_in_t) +
self.start_time).tolist()
self.stop_time = self.planner.t_equi_in_t[-1]
def write_path(self):
for point in self.planner.path_from_v_equi_in_t:
self.shape.append([np.real(point), np.imag(point)])
pass
def test_dc_motor(self, ax, ay):
self.acceleration_x = ax
self.acceleration_y = ay
self.counter += 1
# x direction:
self.state[0] = lib.statespace.A.A.dot(
self.old_state[0]) + lib.statespace.B.A.dot(ax)
x = lib.statespace.C.A.dot(
self.old_state[0]) + lib.statespace.D.A.dot(ax)
# y direction:
self.state[1] = lib.statespace.A.A.dot(
self.old_state[1]) + lib.statespace.B.A.dot(ay)
y = lib.statespace.C.A.dot(
self.old_state[1]) + lib.statespace.D.A.dot(ay)
self.old_state = self.state
return x[0][0], y[0][0]
def steer(self, t, ax, ay, stop):
t = round(t, 5)
ax -= self.acceleration_x_c
ay -= self.acceleration_y_c
self.acceleration_x = ax
self.acceleration_y = ay
lp = self.last_position[:]
# x direction
self.state[0] = lib.statespace.A.A.dot(
self.old_state[0]) + lib.statespace.B.A.dot(ax)
x = lib.statespace.C.A.dot(
self.old_state[0]) + lib.statespace.D.A.dot(ax)
x = x[0][0]
# y direction:
self.state[1] = lib.statespace.A.A.dot(
self.old_state[1]) + lib.statespace.B.A.dot(ay)
y = lib.statespace.C.A.dot(
self.old_state[1]) + lib.statespace.D.A.dot(ay)
y = y[0][0]
self.position_x.append(self.spawn[0] + x)
self.position_y.append(self.spawn[1] + y)
self.last_position = [self.spawn[0] + x, self.spawn[1] + y]
self.old_state = self.state
if stop:
self.stop_time = t
else:
comp_vel = complex(self.last_velocity_x, self.last_velocity_y)
self.direction = np.angle([comp_vel])[0]
self.stop = stop
try:
self.direction = lib.angle(
Point(self.position_x[-2], self.position_y[-2]),
Point(self.position_x[-1], self.position_y[-1]))
except IndexError:
pass
self.time_last_step = t
def control(self, t, ax, ay):
self.acceleration_x_c = ax
self.acceleration_y_c = ay
# CONVERTING CONTROL_PREP INTO CONTROLS FOR CAR
# [timestamp, estimated x, estimated y, abs(acceleration), direction to drive]
def make_controls(self):
stop_time = self.planner.t_equi_in_t[-1]
stop = False
t = self.start_time
for acc in self.planner.acceleration_from_v_equi_in_t:
if acc == self.planner.acceleration_from_v_equi_in_t[-1]:
stop = True
ev = Event(t, self,
(t, self, np.real(acc), np.imag(acc), stop, "steering"),
lambda: lib.eventqueue.car_steering)
lib.eventqueue.add_event(ev)
t += lib.pt
# fills the self.controls list with acceleration values
self.controller.set_path(self.planner.path_from_v_equi_in_t)
# used with EventQueue
# returns position of the car at specific time t
# also more information available (but certainly not needed),f.e. acceleration, direction, steering, angle, ...
def get_data(self, t):
if self.ghost:
try:
if (t - self.start_time) > 0:
index = int((t - self.start_time) / lib.pt)
else:
index = 0
point = self.planner.path_from_v_equi_in_t[index]
vel = self.planner.velocity_from_v_equi_in_t[index]
dir = np.angle([vel])
except IndexError:
point = self.planner.path_from_v_equi_in_t[-1]
vel = self.planner.velocity_from_v_equi_in_t[-1]
vel_dir = self.planner.velocity_from_v_equi_in_t[-2]
dir = np.angle([vel_dir])
vel = abs(vel)
x = point.real
y = point.imag
else:
if not self.stop:
dt = (t - self.time_last_control)
else:
dt = (self.stop_time - self.time_last_control)
x = self.last_position[0]
y = self.last_position[1]
dir = round(self.direction, 5)
vel = self.last_velocity
if t == 0:
dir = self.start_direction
return [t, self.id, round(x, 4), round(y, 4), vel, dir]
def initialize_frontier(self):
path = Path()
path.add(self.start)
self.frontier.insert(0, path)
return self.frontier
class CarAckermann:
def __init__(self, id: int, spawn_x, spawn_y, start, start_dir, end_dir,
size_x, size_y, angle, max_vel, max_acc, max_steering_angle,
color, min_latency, max_latency, errorrate, seg_len,
channel_properties):
self.ghost = False
# PHYSICAL PROPERTIES
self.color = color # color code
self.id = id # unique car id
self.spawn = [spawn_x, spawn_y] # spawnpoint
self.destination = 0
self.position_x = [spawn_x] # not used for EventQueue
self.position_y = [spawn_y]
self.direction = start_dir # direction of the car
self.start_direction = angle # start-direction of the car
self.last_position = [spawn_x,
spawn_y] # position after last control input
self.length = size_x # in m
self.width = size_y # in m
self.start_dir = start_dir
self.end_dir = end_dir
self.end = None
self.min_latency = min_latency
self.max_latency = max_latency
self.errorrate = errorrate
self.start_time = start
self.wheelbase = self.length - 0.5 * self.width
self.rearbase = 0.5 * self.width
self.wheelangles = [0, 0]
self.stored_a = []
# VELOCITY
self.last_velocity = 0
self.velocity = [] # not used for EventQueue
self.max_velocity = max_vel # absolute limit of velocity
# ACCELERATION
self.acceleration = 0 # acceleration given by last control input
self.max_acceleration = max_acc # absolute limit of acceleration
self.acceleration_controller = 0
self.acc_debug = []
self.emergency_brake = False
# STEERING
self.steering = 0 # steering angle given by last control input
self.direction = 0 # direction angle given by last control input
self.steering_control = 0
self.max_steering_angle = max_steering_angle
# PATH
self.shape = [] # shape of the planned path (without exact timestamp)
self.path = Path(self.spawn) # "shape" with exact timestamp
self.path_buffer = []
self.segment_length = seg_len # length of segments (path is divided in segments which are then
# sent to the AGV
self.last_segment_time = 0 # start-time of the last received path-segment
self.waypoints = [] # list for the given points with the
self.planner = None
# CONTROLS
self.time_last_control = start - lib.dt # timestamp of last control input
self.time_last_update = start - lib.dt # timestamp of last given acceleration
self.time_last_step = start - lib.dt
self.stop_time = 0 # timestamp of stop (velocity 0 reached)
self.controller = Controller(self, lib.k_p, lib.k_d)
dim = len(lib.statespace.A)
self.old_state = np.zeros(dim).reshape(-1, 1)
self.state = None
self.full_arc = 0
self.min_dist = self.make_min_dist()
self.distances = []
self.lat_distances = []
#CHANNEL
self.channel = Channel(self.errorrate, channel_properties)
# DEBUGGING
self.debugging = []
self.arc_debug = []
def set_first_position(self):
self.position_x = [self.spawn[0]]
self.position_y = [self.spawn[1]]
self.last_position = self.spawn
# sets waypoint (given by *.json file) for the car
def set_waypoint(self, x, y):
p = Point(x, y)
self.path.add(p)
self.waypoints.append(p)
# raise Exception('The point (' + str(p.x) + '|' + str(p.y) + ') is too far away. Skipped.')
# used for collision control
def make_min_dist(self):
edges = [
[self.spawn[0] - self.length / 2, self.spawn[1] - self.width / 2],
[self.spawn[0] + self.length / 2, self.spawn[1] - self.width / 2],
[self.spawn[0] + self.length / 2, self.spawn[1] + self.width / 2],
[self.spawn[0] - self.length / 2, self.spawn[1] + self.width / 2]
]
out = lib.dist(self.spawn, edges[0])
for e in edges:
dist = lib.dist(self.spawn, e)
if dist > out:
out = dist
return out
# creates spline for the given waypoints (from the *.json file)
def create_spline(self):
self.planner = PathPlanner(self, self.max_velocity,
self.max_acceleration)
self.planner.make_path(self.path.points)
self.stop_time = self.planner.t_equi_in_t[-1]
self.write_path()
self.planner.t_equi_in_t = (np.array(self.planner.t_equi_in_t) +
self.start_time).tolist()
self.update_path()
def write_path(self):
for point in self.planner.path_from_v_equi_in_t:
self.shape.append([np.real(point), np.imag(point)])
def steer(self, t, a, angle):
if self.emergency_brake:
a = -self.max_acceleration # braking as fast as possible
else:
a += self.acceleration_controller # controlling the current acceleration
self.acc_debug.append(a)
angle = (angle + self.steering_control)
self.debugging.append(angle)
# angle = min(self.max_steering_angle, angle)
# angle = max(-self.max_steering_angle, angle)
if (self.last_velocity <= 0) & self.emergency_brake:
self.stop_time = t
if t < self.stop_time:
start_angle = self.direction - np.pi / 2
pos = np.array(self.last_position) - np.array(
[0, 0.5 * cos(self.direction) * self.wheelbase])
# DC-Motor
self.state = lib.statespace.A.dot(
self.old_state) + lib.statespace.B.dot(a)
new_arc = (lib.statespace.C.dot(self.old_state) +
lib.statespace.D.dot(a))[0, 0]
# State Transition
self.old_state = self.state
# Saving attributes
arc = new_arc - self.full_arc
self.full_arc = new_arc
self.last_velocity = arc / lib.pt
self.velocity.append(self.last_velocity)
self.arc_debug.append(arc)
try:
radius = self.wheelbase / tan(angle)
self.wheelangles = [
atan2(self.wheelbase, radius - 0.5 * self.rearbase),
atan2(self.wheelbase, radius + 0.5 * self.rearbase)
]
phi = arc / radius
center_of_turning_cycle = np.array(pos) - np.array([radius, 0])
point = np.array([radius * cos(phi), radius * sin(phi)])
point = point + center_of_turning_cycle - pos
point = np.dot(
np.array([[np.cos(start_angle), -np.sin(start_angle)],
[np.sin(start_angle),
np.cos(start_angle)]]), point.reshape(-1, 1))
point = point + np.array(pos).reshape(-1, 1)
self.direction = (phi + self.direction) % (2 * pi)
self.last_position = (
point.reshape(1, -1) +
np.array([0, 0.5 * cos(self.direction) * self.wheelbase
])).tolist()[0]
x = self.last_position[0]
y = self.last_position[1]
self.position_x.append(x)
self.position_y.append(y)
# in case of a straight movement:
except ZeroDivisionError:
x = self.last_position[0] + arc * cos(self.direction)
y = self.last_position[1] + arc * sin(self.direction)
self.last_position = [x, y]
self.position_x.append(x)
self.position_y.append(y)
self.wheelangles = [0, 0]
def control(self, acc):
self.acceleration_controller = acc
def make_controls(self):
t = self.last_segment_time
for i in range(len(self.stored_a)):
acc = self.stored_a[i]
prio = 1 # didsdsdsdfs
ev = Event(t, self, (round(t, 7), self, acc, "steering"),
lambda: lib.eventqueue.car_steering_ackermann, prio)
lib.eventqueue.add_event(ev)
t += lib.pt
# used with EventQueue
# returns position of the car at specific time t
# also more information available (but certainly not needed),f.e. acceleration, direction, steering, angle, ...
def get_data(self, t):
if self.ghost:
try:
if (t - self.start_time) > 0:
index = round((t - self.start_time) / lib.pt)
else:
index = 0
point = self.planner.path_from_v_equi_in_t[index]
vel = self.planner.velocity_from_v_equi_in_t[index]
dir = np.angle([vel])
except IndexError:
point = self.planner.path_from_v_equi_in_t[-1]
vel = self.planner.velocity_from_v_equi_in_t[-1]
vel_dir = self.planner.velocity_from_v_equi_in_t[-2]
dir = np.angle([vel_dir])
wheel_angles = [0, 0]
vel = abs(vel)
x = point.real
y = point.imag
else:
x = self.last_position[0]
y = self.last_position[1]
x = self.position_x[-1]
y = self.position_y[-1]
dir = round(self.direction, 5)
vel = self.last_velocity
wheel_angles = self.wheelangles
if t == 0:
dir = self.start_direction
return [t, self.id, round(x, 7), round(y, 7), vel, wheel_angles, dir]
def update_path(self):
print('update', self.last_segment_time)
self.stored_a = self.planner.a_from_v_equi_in_t[round(
self.last_segment_time /
lib.pt):round((self.last_segment_time + self.segment_length) /
lib.pt)]
# fills the self.controls list with acceleration values
self.controller.set_path(self.planner.path_from_v_equi_in_t)
self.make_controls()
t = self.last_segment_time
self.last_segment_time += self.segment_length
if self.last_segment_time < self.stop_time:
ev = Event(self.last_segment_time - self.segment_length / 3, self,
(
t,
self,
), lambda: lib.eventqueue.update_path)
lib.eventqueue.add_event(ev)
def brake(self, t):
for e in lib.eventqueue.events:
if e.object == self:
del e
self.emergency_brake = True
class BruteAlgorithm:
"""Class representing Brute Algorithm
It takes every possible path in the graph and finds the one,
on which the doctor cures the highest number of people"""
def __init__(self, starting_country, starting_date, alg_graph, goal_function):
"""Method which initializes BruteAlgorithm object
It takes start country and date plus the graph on which it should work on"""
self.path = Path(starting_country, starting_date)
self.graph = alg_graph
self.possible_paths = [tuple([starting_country])]
stats = self.graph.get_stats(starting_country) # Here is a country which has latest data
self.last_date = datetime.date(stats[0, 2], stats[0, 1], stats[0, 0])
self.days_difference = int((self.last_date - starting_date).days)
self.goal_function = goal_function
def generate_paths(self):
"""Method which generates all available paths"""
for path_list in self.possible_paths:
if int(self.days_difference) < len(path_list):
break
for neighbor in self.graph.get_neighbours(path_list[len(path_list)-1]):
new_path = list(path_list)
new_path.append(neighbor)
self.possible_paths.append(tuple(new_path))
def choose_best_path(self):
"""Method chooses the best possible path (which has the highest number of cured poeple)"""
longest_paths = [path for path in self.possible_paths if len(path) == self.days_difference + 1]
best_path = []
if self.goal_function == 'recovered':
best_score = -1
elif self.goal_function == 'deaths':
best_score = 1000000000
for path in longest_paths:
self.clear_path()
self.add_whole_path(path)
self.path.eval(self.graph, self.goal_function)
if self.goal_function == 'recovered' and self.path.score > best_score:
best_score = self.path.score
best_path = path
elif self.goal_function == 'deaths' and self.path.score < best_score:
best_score = self.path.score
best_path = path
self.clear_path()
self.add_whole_path(best_path)
self.path.eval(self.graph, self.goal_function)
def clear_path(self):
"""Method clears the variable self.path leaving only the starting country"""
for i in range(len(self.path.countries)-1):
self.path.countries.pop()
self.path.score = 0
def add_whole_path(self, path):
"""Method adds countries which are in the argument 'path'
(execpt the first one which is already in the variable self.path"""
for country_index in range(len(path)):
if country_index == 0:
continue
self.path.add(path[country_index])
def work(self):
"""Method which initializes every method essential to find a path"""
self.generate_paths()
self.choose_best_path()
