def test_negative_kruskals_algo(self): vertices = [Vertex('A'), Vertex('B'), Vertex('C')] edges = [ Edge(Vertex('A'), Vertex('B'), 2), Edge(Vertex('B'), Vertex('C'), 1), Edge(Vertex('A'), Vertex('C'), 10) ] graph = Graph(vertices, edges) correct_path = graph.kruskals_algorithm() incorrect_path = [ Edge(Vertex('B'), Vertex('C'), 1), Edge(Vertex('A'), Vertex('B'), 10) ] self.assertNotEqual(correct_path, incorrect_path)
def test_positive_kruskals_algo(self): vertices = [Vertex('A'), Vertex('B'), Vertex('C')] edges = [ Edge(Vertex('A'), Vertex('B'), 2), Edge(Vertex('B'), Vertex('C'), 1), Edge(Vertex('A'), Vertex('C'), 10) ] graph = Graph(vertices, edges) expected_path = graph.kruskals_algorithm() correct_path = [ Edge(Vertex('B'), Vertex('C'), 1), Edge(Vertex('A'), Vertex('B'), 2) ] self.assertEqual(expected_path, correct_path)
def __init__(self): pygame.init() self.screen = pygame.display.set_mode((1080, 720)) self.algorithm = Astar(Graph(25, 25)) self.visual_algorithm = Visual_Algorithm(self.algorithm, self.screen) self.running = True self.events = []
def set_graph(self, withWeights=False): print "Please enter the number of vertices:\n" num_vertices = raw_input(">") print "Please enter the number of edges:\n" num_edges = raw_input(">") vertices = [] edges = [] for i in range(int(num_vertices)): print "Please enter the name of the vertex: ", i vertex_name = raw_input(">") vertices.append(Vertex(vertex_name)) for j in range(int(num_edges)): print "Please enter the name of the first vertex of edge\n" first_vertex = raw_input(">") print "Please enter the name of the second vertex of edge\n" second_vertex = raw_input(">") if withWeights: print "Please enter edge weight\n" edge_weight = raw_input(">") edges.append( Edge(Vertex(first_vertex), Vertex(second_vertex), edge_weight)) else: edges.append(Edge(Vertex(first_vertex), Vertex(second_vertex))) return Graph(vertices=vertices, edges=edges)
def createSampleGraph(self): vertices = [] a_vertex = Vertex('A') b_vertex = Vertex('B') c_vertex = Vertex('C') d_vertex = Vertex('D') e_vertex = Vertex('E') f_vertex = Vertex('F') vertices.append(a_vertex) vertices.append(b_vertex) vertices.append(c_vertex) vertices.append(d_vertex) vertices.append(e_vertex) vertices.append(f_vertex) edges = [] edges.append(Edge(a_vertex, c_vertex, 1)) edges.append(Edge(a_vertex, b_vertex, 3)) edges.append(Edge(b_vertex, d_vertex, 2)) edges.append(Edge(b_vertex, e_vertex, 2)) edges.append(Edge(c_vertex, f_vertex, 2)) edges.append(Edge(c_vertex, a_vertex, 2)) edges.append(Edge(d_vertex, b_vertex, 2)) edges.append(Edge(e_vertex, f_vertex, 2)) edges.append(Edge(e_vertex, b_vertex, 2)) edges.append(Edge(f_vertex, c_vertex, 2)) edges.append(Edge(f_vertex, e_vertex, 2)) graph = Graph(vertices, edges) return graph
def __init__(self, s, init_pmap=np.array([0.5 for i in range(M)]), network_range=50000, num_uavs=10, p=0.9, q=0.3): """ location: a list of x and y coordinate of UAV initially, converted to a position vector (numpy) Q: list/hashmap of P transforms of all cells in the region network_range: distance between 2 UAV which ensures proper data transfer, edge length of undirected graph n: number of vertices of the undirected graph/ number of UAVs velocity: velocity vector dij = wieght matrix, initialized to matrix of zeros p: probability of presence of target hashmap q: probability of absence of a target hashmap CM: centroid of voronoid region vector density: list of densities of each cell """ super(UAVSwarmBot, self).__init__() self.vehicle = dronekit.connect(s) self.g_list = [] self.p = p self.q = q self.init_pmap = init_pmap self.__Q = np.zeros(10201) self.__network_range = network_range self.num_uavs = num_uavs self.neighbors = Graph(num_uavs) # or just use list for simplicity self.__velocity = np.zeros(2) self.__dij = np.zeros((num_uavs, num_uavs)) self.Kn = 0.002 self.Ku = 1 self.__location = [ self.vehicle.location.global_frame.lat, self.vehicle.location.global_frame.lon ] self.__voronoi_part_map = [] self.density = np.ones(10201) self.A = 0 self.CM = [0, 0] self.wplist = [] self.id = int(self.vehicle.parameters['SYSID_THISMAV']) self.UAVID = 0
class UAVSwarmBot(Thread): def __init__(self, s, init_pmap=np.array([0.5 for i in range(M)]), network_range=50000, num_uavs=10, p=0.9, q=0.3): """ location: a list of x and y coordinate of UAV initially, converted to a position vector (numpy) Q: list/hashmap of P transforms of all cells in the region network_range: distance between 2 UAV which ensures proper data transfer, edge length of undirected graph n: number of vertices of the undirected graph/ number of UAVs velocity: velocity vector dij = wieght matrix, initialized to matrix of zeros p: probability of presence of target hashmap q: probability of absence of a target hashmap CM: centroid of voronoid region vector density: list of densities of each cell """ super(UAVSwarmBot, self).__init__() self.vehicle = dronekit.connect(s) self.g_list = [] self.p = p self.q = q self.init_pmap = init_pmap self.__Q = np.zeros(10201) self.__network_range = network_range self.num_uavs = num_uavs self.neighbors = Graph(num_uavs) # or just use list for simplicity self.__velocity = np.zeros(2) self.__dij = np.zeros((num_uavs, num_uavs)) self.Kn = 0.002 self.Ku = 1 self.__location = [ self.vehicle.location.global_frame.lat, self.vehicle.location.global_frame.lon ] self.__voronoi_part_map = [] self.density = np.ones(10201) self.A = 0 self.CM = [0, 0] self.wplist = [] self.id = int(self.vehicle.parameters['SYSID_THISMAV']) self.UAVID = 0 def run(self): logging.debug('running' + ' ' + str(self.UAVID)) def __str__(self): return "UAVSwarmBot is the Probability Map based control law generation for an individual UAV." def getQ(self): """ returns: list Q with P tranforms of each cell as per the UAV object """ return self.__Q def getlocation(self): """ returns current location of UAV""" self.__location = [ self.vehicle.location.global_frame.lat, self.vehicle.location.global_frame.lon ] return self.__location def updatelocation(self): """ updates: current location of UAV """ return [ self.vehicle.location.global_frame.lat, self.vehicle.location.global_frame.lon ] def transform_pmap(self, M): """linearizes given probability map""" for i in range(M): self.__Q[i] = math.log((1 / self.init_pmap[i]) - 1) def getrange(self): """ returns: network range required for communication between any two UAVs """ return self.__network_range def getneighbors(self): """ returns: string of the adjecency matrix of neighbors of the UAV """ return self.neighbors.adjmatrix def getvoro(self): """ returns voronoi point list """ return self.__voronoi_part_map def fusion_update(self, friend_Q_list): """ """ for j in range(self.num_uavs): if self.neighbors.containsEdge(self.UAVID, j): self.__Q = self.__dij[self.UAVID][j] * friend_Q_list[ j] # multiplied whole matrix def update_Q(self, Zk_list): """ """ for i in range(M): if Zk_list[i] == 1: v = math.log(self.q / self.p) elif Zk_list[i] == 0: v = math.log((1 - self.q) / (1 - self.p)) else: v = 0 self.__Q[i] += v def update_A_CM(self, area_cell, M, Zk_list): """ g_list: list of coordinates of centre of all points """ self.A = 0 self.CM = [0, 0] flag = 0 for i in range(M): self.density[i] = math.exp(-self.Kn * abs(self.__Q[i])) if contains_point(self.g_list[i], self.__voronoi_part_map): self.A += area_cell * self.density[i] self.CM[0] += (area_cell) * ( self.density[i]) * self.g_list[i][0] self.CM[1] += (area_cell) * ( self.density[i]) * self.g_list[i][1] flag = 1 if flag == 0: print("correct voronoi map") else: self.CM[0] = self.CM[0] / self.A self.CM[1] = self.CM[1] / self.A def turn(self): if (self.__location[0] < bounding_box[0] or self.__location[0] > bounding_box[1]) and (self.__location[1] < bounding_box[2] or self.__location[1] > bounding_box[3]): self.__velocity[0] = -self.__velocity[0] self.__velocity[1] = -self.__velocity[1] elif (self.__location[0] < bounding_box[0] or self.__location[0] > bounding_box[1]): self.__velocity[0] = -self.__velocity[0] elif (self.__location[1] < bounding_box[2] or self.__location[1] > bounding_box[3]): self.__velocity[1] = -self.__velocity[1] """ Move vehicle in direction based on specified velocity vectors. """ msg = self.vehicle.message_factory.set_position_target_global_int_encode( 0, # time_boot_ms (not used) 0, 0, # target system, target component mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, # frame 0b0000111111000111, # type_mask (only speeds enabled) 0, # lat_int - X Position in WGS84 frame in 1e7 * meters 0, # lon_int - Y Position in WGS84 frame in 1e7 * meters 0, # alt - Altitude in meters in AMSL altitude(not WGS84 if absolute or relative) # altitude above terrain if GLOBAL_TERRAIN_ALT_INT self.__velocity[0], # X velocity in NED frame in m/s self.__velocity[1], # Y velocity in NED frame in m/s 0, # Z velocity in NED frame in m/s 0, 0, 0, # afx, afy, afz acceleration (not supported yet, ignored in GCS_Mavlink) 0, 0) # yaw, yaw_rate (not supported yet, ignored in GCS_Mavlink) self.vehicle.send_mavlink(msg) def update_velocity(self, Ku): """ changes the velocity vector of the UAV """ #heuristic = self.forward_heuristic() vel_x = Ku * (self.CM[0] - self.__location[0]) vel_y = Ku * (self.CM[1] - self.__location[1]) new_vel = [vel_x, vel_y] if self.CM[0] > 0 and self.CM[1] > 0: mynorm = np.linalg.norm(new_vel) if mynorm > 5: new_vel[0] = new_vel[0] * 5 / mynorm new_vel[1] = new_vel[1] * 5 / mynorm else: new_vel = [5, 0] self.__velocity = new_vel """ Move vehicle in direction based on specified velocity vectors. """ msg = self.vehicle.message_factory.set_position_target_global_int_encode( 0, # time_boot_ms (not used) 0, 0, # target system, target component mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, # frame 0b0000111111000111, # type_mask (only speeds enabled) 0, # lat_int - X Position in WGS84 frame in 1e7 * meters 0, # lon_int - Y Position in WGS84 frame in 1e7 * meters 0, # alt - Altitude in meters in AMSL altitude(not WGS84 if absolute or relative) # altitude above terrain if GLOBAL_TERRAIN_ALT_INT self.__velocity[0], # X velocity in NED frame in m/s self.__velocity[1], # Y velocity in NED frame in m/s 0, # Z velocity in NED frame in m/s 0, 0, 0, # afx, afy, afz acceleration (not supported yet, ignored in GCS_Mavlink) 0, 0) # yaw, yaw_rate (not supported yet, ignored in GCS_Mavlink) self.vehicle.send_mavlink(msg) def forward_heuristic(self): location = self.getlocation() wp = [0.011, 0] #getvector = [location[0]-wp[0], location[1]] #dist = np.linalg.norm(getvector) #getvector[0] = getvector[0]/dist #getvector[1] = getvector[1]/dist return wp def getvel(self): return self.__velocity def add_voro_point(self, pt): """ add a voronoi partition coordinate """ self.__voronoi_part_map.append(pt) def update_location(self, newlocation): """ changes the current location of the UAV """ points = dronekit.LocationGlobalRelative(new_location[0], newlocation[1], self.altitude) self.vehicle.simple_goto(points) def updatevoronoi(self, point_list): """ returns: vertices of the voronoi region for a UAV object """ self.__voronoi_part_map = voronoi(point_list, self.__location)[1] def norm(self, target_coordinate): """ target_coordinate: a list of 2 elements containing x and y coordinate of target respectively returns float: norm of the eucledian distance vector rounded to 4 decimal places """ # initialize zero matrix with 2 elements temp = np.zeros(2) current_x = self.__location[0] current_y = self.__location[1] # add the x and y coordinate difference in the temp array respectively and calculate norm of the same temp[0] = target_coordinate[0] - current_x temp[1] = target_coordinate[1] - current_y return np.linalg.norm(temp) def inrange(self, target_coordinate): """ target_coordinate: a list of 2 elements containing x and y coordinate of target respectively returns bool: checks if the given coordinate is in communication range with the UAVSwarmBot """ return self.norm(target_coordinate) <= self.__network_range def connect_to_neighbor(self, friend): """ friend: Numpy vector of friend ID adds edge between 2 UAVs """ if self.inrange(friend.getlocation()): self.neighbors.addEdge(self.UAVID, friend.UAVID) def remove_neighbor(self, friend): """ friend: Numpy vector of friend ID deletes edge between 2 UAVs """ if not self.inrange(friend.getlocation): self.neighbors.removeEdge(self.UAVID, friend.UAVID) def construct_weight_matrix(self): """ numneighbors: number of neighbors of the uav at the particular instant constructs a weight matrix dij initialized to 0 """ count = 0 for i in range(self.num_uavs): if self.neighbors.containsEdge(self.UAVID, i): count += 1 numneighbors = count for i in range(self.num_uavs): for j in range(self.num_uavs): if self.neighbors.containsEdge(self.UAVID, j): if i != j: self.__dij[i][j] = 1 - (numneighbors - 1) / self.num_uavs else: self.__dij[i][j] = 1 / self.num_uavs def altitude(self): return self.vehicle.location.global_relative_frame.alt def arm_and_takeoff(self, aTargetAltitude): while not self.vehicle.is_armable: print(" Waiting for vehicle to initialise...", self.id) time.sleep(1) print("Arming motors", self.id) # Copter should arm in GUIDED mode self.vehicle.mode = dronekit.VehicleMode("GUIDED") self.vehicle.armed = True while not self.vehicle.armed: print(" Waiting for arming...", self.id) time.sleep(1) print("Taking off!", self.id) self.vehicle.simple_takeoff(aTargetAltitude) while True: print(" Altitude: ", self.vehicle.location.global_relative_frame.alt) if self.vehicle.location.global_relative_frame.alt >= aTargetAltitude * 0.90: print("Reached target altitude", self.id) break time.sleep(1) def land(self): self.vehicle.mode = dronekit.VehicleMode("LAND") def heading(self): self.head = self.vehicle.heading return self.head def transmitdata(self): pass def recievedata(self): pass
from GraphClass import Graph, Vertex """ Course: CPCS 331 Section: GE Group: Member 1: Turki Alzubaidi Member 2: Ammar Joharji Member 3: Mohammad Alghafli """ try: graph = Graph() numberOfVertices = int(input("How many Vertices: ")) # we loop to add each vertex into a list to to insert the vertices in the alphabetical order verticesList = [] i = 0 while i < numberOfVertices: verLabel = input("Enter vertex number %d: " % (i + 1)) if verLabel not in verticesList: verticesList.append(verLabel) i += 1 else: print("This vertex is already inserted") verticesList.sort() # to sort the list # adding all vertices into the graph for i in verticesList: graph.addVertex(Vertex(i)) print() print("All vertices added successfully...")