├── .gitignore ├── README.md ├── a_star.py ├── a_star_path_log.csv ├── drone_path_log.csv ├── fly_sequence.py ├── main.py ├── map_objects.py ├── media ├── astar_path_2d.png ├── astar_path_3d.png └── planned_vs_actual.png ├── plot_paths.py └── test_flight.py /.gitignore: -------------------------------------------------------------------------------- 1 | cache/ 2 | 3 | # Byte-compiled / optimized / DLL files 4 | __pycache__/ 5 | *.py[cod] 6 | *$py.class 7 | 8 | # C extensions 9 | *.so 10 | 11 | # Distribution / packaging 12 | .Python 13 | build/ 14 | develop-eggs/ 15 | dist/ 16 | downloads/ 17 | eggs/ 18 | .eggs/ 19 | lib/ 20 | lib64/ 21 | parts/ 22 | sdist/ 23 | var/ 24 | wheels/ 25 | pip-wheel-metadata/ 26 | share/python-wheels/ 27 | *.egg-info/ 28 | .installed.cfg 29 | *.egg 30 | MANIFEST 31 | 32 | # PyInstaller 33 | # Usually these files are written by a python script from a template 34 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 35 | *.manifest 36 | *.spec 37 | 38 | # Installer logs 39 | pip-log.txt 40 | pip-delete-this-directory.txt 41 | 42 | # Unit test / coverage reports 43 | htmlcov/ 44 | .tox/ 45 | .nox/ 46 | .coverage 47 | .coverage.* 48 | .cache 49 | nosetests.xml 50 | coverage.xml 51 | *.cover 52 | *.py,cover 53 | .hypothesis/ 54 | .pytest_cache/ 55 | 56 | # Translations 57 | *.mo 58 | *.pot 59 | 60 | # Django stuff: 61 | *.log 62 | local_settings.py 63 | db.sqlite3 64 | db.sqlite3-journal 65 | 66 | # Flask stuff: 67 | instance/ 68 | .webassets-cache 69 | 70 | # Scrapy stuff: 71 | .scrapy 72 | 73 | # Sphinx documentation 74 | docs/_build/ 75 | 76 | # PyBuilder 77 | target/ 78 | 79 | # Jupyter Notebook 80 | .ipynb_checkpoints 81 | 82 | # IPython 83 | profile_default/ 84 | ipython_config.py 85 | 86 | # pyenv 87 | .python-version 88 | 89 | # pipenv 90 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 91 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 92 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 93 | # install all needed dependencies. 94 | #Pipfile.lock 95 | 96 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 97 | __pypackages__/ 98 | 99 | # Celery stuff 100 | celerybeat-schedule 101 | celerybeat.pid 102 | 103 | # SageMath parsed files 104 | *.sage.py 105 | 106 | # Environments 107 | .env 108 | .venv 109 | env/ 110 | venv/ 111 | ENV/ 112 | env.bak/ 113 | venv.bak/ 114 | 115 | # Spyder project settings 116 | .spyderproject 117 | .spyproject 118 | 119 | # Rope project settings 120 | .ropeproject 121 | 122 | # mkdocs documentation 123 | /site 124 | 125 | # mypy 126 | .mypy_cache/ 127 | .dmypy.json 128 | dmypy.json 129 | 130 | # Pyre type checker 131 | .pyre/ 132 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Crazyflie Trajectory Planning and Obstacle Avoidance 2 | ## Hardware Setup 3 | ## Conceptual Approach 4 | ## Code Architecture 5 | ## A-Star 6 | ## Demons -------------------------------------------------------------------------------- /a_star.py: -------------------------------------------------------------------------------- 1 | class Astar: 2 | """ 3 | This class implements the A* algorithm to find paths on a given map 4 | 5 | Attributes: 6 | map (Map) : the environment for A* 7 | start (Tuple) : the starting coordinates of the drone 8 | target (Tuple) : the desired end coordinates of the drone 9 | nodes_found (List) : A list of nodes discovered by A* 10 | nodes_checked (List) : A list of nodes explored by A* 11 | Methods: 12 | __init__ (map, start, target): initializes the Astar class 13 | calculate_score(parent,child,target) : Calculates the heuristic value of a node 14 | pos_to_node(pos) : Finds the closest node to the given position 15 | a_star_search : implements the A* algorithm 16 | """ 17 | def __init__(self, map, start, target): 18 | self.map = map 19 | self.start = start 20 | self.target = target 21 | self.nodes_found = [] 22 | self.nodes_checked = [] 23 | 24 | def calculate_score(self,parent,child,target): 25 | """ 26 | Calculates the heuristic value of a node 27 | Args: 28 | Parent(Node):The previous node being explored 29 | Child(Node):The node being scored 30 | Target(tuple): The tuple containing the x and y position of the target 31 | Returns: 32 | Score (Double):The heuristic value of the node 33 | """ 34 | # multi-agent note: implement score adjustment for idx in node.intersections 35 | _current_pos=child.coords 36 | _target_pos=self.target 37 | _dist_to_target=((_current_pos[0]-_target_pos[0])**2+(_current_pos[1]-_target_pos[1])**2)**0.5 38 | score=_dist_to_target+parent.heuristic+self.map.drone_dim 39 | 40 | return score 41 | 42 | def pos_to_node(self, pos): 43 | """ 44 | Finds the closest node to the given position (localization) 45 | Args: 46 | pos(tuple): The tuple containing an x and y position in the map frame 47 | map(Map): the map of nodes for the position coords 48 | Returns: 49 | closest_node (Node):The node closest to the position 50 | """ 51 | node_x = int(pos[0]//self.map.drone_dim) 52 | node_y = int(pos[1]//self.map.drone_dim) 53 | closest_node = self.map.array[node_y][node_x] 54 | return closest_node 55 | 56 | def a_star_search(self): 57 | """ 58 | Implements the A* algorithm 59 | Args: 60 | None 61 | Returns: 62 | trajectory (List):The list of tuples defining the drone path 63 | """ 64 | 65 | # creating a property that tells you where path has been 66 | #Starts A* with the drone starting position 67 | self.nodes_found.append(self.pos_to_node(self.start)) 68 | 69 | while True: 70 | #If there are no nodes left to search, end the function 71 | if len(self.nodes_found) == 0: 72 | print("Tough luck, we couldn't find a solution") 73 | break 74 | 75 | #Find the next node to check (it has the lowest f_cost) 76 | self.nodes_found = sorted(self.nodes_found, key=lambda n: n.f_cost) 77 | node_to_check = self.nodes_found.pop(0) 78 | self.nodes_checked.append(node_to_check) 79 | 80 | #TARGET NODE FOUND! after check, enter into path assembly. 81 | if node_to_check == self.pos_to_node(self.target): 82 | #Creates a node path for the drone to follow, starting at ending point 83 | path = [node_to_check] 84 | node = node_to_check.parent 85 | while True: 86 | # inserting the parent node before 1st node in list to form the path backwards 87 | path.insert(0, node) 88 | # once you reach the starting node, you're done making the path! 89 | if node == self.pos_to_node(self.start): 90 | break 91 | node = node.parent 92 | #Convert the path in nodes to the path in coordinates 93 | trajectory = [] 94 | for traj_node in path: 95 | trajectory.append(traj_node.coords) 96 | return trajectory 97 | 98 | #Checks nodes around the node being explored 99 | new_nodes = self.map.get_neighbors(node_to_check) 100 | for node in new_nodes: 101 | #Score each node 102 | node.set_heuristic(self.calculate_score(node_to_check,node,self.target)) 103 | #Add the node if it hasn't been seen before 104 | if node not in self.nodes_found and node not in self.nodes_checked: 105 | node.parent=node_to_check 106 | self.nodes_found.append(node) 107 | #If the node has been checked before but has a different parent, use the parent with a lower score 108 | elif node in self.nodes_found and node.parent != node_to_check: 109 | old_score = node.heuristic 110 | old_parent = node.parent 111 | node.set_parent(node_to_check) 112 | new_score = node.heuristic 113 | if old_score < new_score: 114 | node.set_parent(old_parent) 115 | else: 116 | self.nodes_found.append(node) -------------------------------------------------------------------------------- /a_star_path_log.csv: -------------------------------------------------------------------------------- 1 | x,y,z 2 | -------------------------------------------------------------------------------- /drone_path_log.csv: -------------------------------------------------------------------------------- 1 | x,y,z 2 | -------------------------------------------------------------------------------- /fly_sequence.py: -------------------------------------------------------------------------------- 1 | import math 2 | import time 3 | 4 | from cflib.crazyflie.log import LogConfig 5 | from cflib.crazyflie.syncLogger import SyncLogger 6 | 7 | def wait_for_position_estimator(scf): 8 | print('Waiting for estimator to find position...') 9 | 10 | log_config = LogConfig(name='Kalman Variance', period_in_ms=500) 11 | log_config.add_variable('kalman.varPX', 'float') 12 | log_config.add_variable('kalman.varPY', 'float') 13 | log_config.add_variable('kalman.varPZ', 'float') 14 | 15 | var_y_history = [1000] * 10 16 | var_x_history = [1000] * 10 17 | var_z_history = [1000] * 10 18 | 19 | threshold = 0.001 20 | 21 | with SyncLogger(scf, log_config) as logger: 22 | for log_entry in logger: 23 | data = log_entry[1] 24 | 25 | var_x_history.append(data['kalman.varPX']) 26 | var_x_history.pop(0) 27 | var_y_history.append(data['kalman.varPY']) 28 | var_y_history.pop(0) 29 | var_z_history.append(data['kalman.varPZ']) 30 | var_z_history.pop(0) 31 | 32 | min_x = min(var_x_history) 33 | max_x = max(var_x_history) 34 | min_y = min(var_y_history) 35 | max_y = max(var_y_history) 36 | min_z = min(var_z_history) 37 | max_z = max(var_z_history) 38 | 39 | # print("{} {} {}". 40 | # format(max_x - min_x, max_y - min_y, max_z - min_z)) 41 | 42 | if (max_x - min_x) < threshold and ( 43 | max_y - min_y) < threshold and ( 44 | max_z - min_z) < threshold: 45 | break 46 | 47 | def reset_estimator(scf): 48 | cf = scf.cf 49 | cf.param.set_value('kalman.resetEstimation', '1') 50 | time.sleep(0.1) 51 | cf.param.set_value('kalman.resetEstimation', '0') 52 | 53 | wait_for_position_estimator(cf) 54 | 55 | def get_pose(scf): 56 | log_pos = LogConfig(name='Position', period_in_ms=200) 57 | log_pos.add_variable('kalman.stateX', 'float') 58 | log_pos.add_variable('kalman.stateY', 'float') 59 | log_pos.add_variable('kalman.stateZ', 'float') 60 | scf.cf.log.add_config(log_pos) 61 | 62 | with SyncLogger(scf, log_pos) as logger: 63 | for log_entry in logger: 64 | data = log_entry[1] 65 | x = data['kalman.stateX'] 66 | y = data['kalman.stateY'] 67 | z = data['kalman.stateZ'] 68 | return x, y, z 69 | 70 | def run_sequence(scf, sequence, delta_t): 71 | cf = scf.cf 72 | 73 | for position in sequence: 74 | print('Setting position {}'.format(position)) 75 | for i in range(int(delta_t * 10)): 76 | cf.commander.send_position_setpoint(position[0], 77 | position[1], 78 | position[2], 79 | position[3]) 80 | time.sleep(0.1) 81 | 82 | cf.commander.send_stop_setpoint() 83 | # Make sure that the last packet leaves before the link is closed 84 | # since the message queue is not flushed before closing 85 | time.sleep(0.1) 86 | -------------------------------------------------------------------------------- /main.py: -------------------------------------------------------------------------------- 1 | import os 2 | import cflib.crtp 3 | import numpy as np 4 | import matplotlib.pyplot as plt 5 | from cflib.crazyflie import Crazyflie 6 | from cflib.crazyflie.log import LogConfig 7 | from cflib.crazyflie.syncCrazyflie import SyncCrazyflie 8 | from cflib.utils import uri_helper 9 | from fly_sequence import reset_estimator, run_sequence, get_pose 10 | from map_objects import Obstacle, Map 11 | from a_star import Astar 12 | 13 | # URI to the Crazyflie to connect to COMMENTED 3/9/23 14 | # uri = uri_helper.uri_from_env(default="radio://0/60/2M/E7E7E7E7E7") 15 | 16 | path_log = open( 17 | os.path.join(os.path.realpath(os.path.dirname(__file__)), "drone_path_log.csv"), 18 | "w+", 19 | ) 20 | path_log.write("x,y,z\n") 21 | path_log.flush() 22 | a_star_log = open( 23 | os.path.join(os.path.realpath(os.path.dirname(__file__)), "a_star_path_log.csv"), 24 | "w+", 25 | ) 26 | a_star_log.write("x,y,z\n") 27 | a_star_log.flush() 28 | 29 | # this may cause problems because now the file is just a path generator 30 | def start_position_printing(scf, file=None): 31 | log_conf = LogConfig(name="Position", period_in_ms=500) 32 | log_conf.add_variable("kalman.stateX", "float") 33 | log_conf.add_variable("kalman.stateY", "float") 34 | log_conf.add_variable("kalman.stateZ", "float") 35 | 36 | scf.cf.log.add_config(log_conf) 37 | log_conf.data_received_cb.add_callback(position_callback) 38 | log_conf.start() 39 | 40 | 41 | def position_callback(timestamp, data, logconf): 42 | x = data["kalman.stateX"] 43 | y = data["kalman.stateY"] 44 | z = data["kalman.stateZ"] 45 | print("pos: ({}, {}, {})".format(x, y, z)) 46 | path_log.write(f"{x},{y},{z}\n") 47 | path_log.flush() 48 | 49 | # if __name__ == "__main__": 50 | # COMMENTED 3/9/23 above/below commenting out code to run crazyflie 51 | # cflib.crtp.init_drivers() 52 | 53 | # with SyncCrazyflie(uri, cf=Crazyflie(rw_cache="./cache")) as scf: 54 | # initializing the points that will be commanded 55 | 56 | 57 | drop_loc = { 58 | "D1": (1.8, .6), 59 | "D2": (1.8, -0.3) 60 | } 61 | 62 | start_loc = { 63 | "S1": (-.6, 0.9), 64 | "S2": (-.6, .3), 65 | "S3":(-.6, -.3) 66 | } 67 | 68 | pick_loc = { 69 | "P1": (.4, .7), 70 | "P2": (.3, -.2), 71 | "P3": (1.0, .2), 72 | "P4": (.2, -.7), 73 | } 74 | 75 | class MultiPlanner(): 76 | def __init__(self, drop_loc, start_loc, pick_loc): 77 | # Initialize Point of interest dictionaries 78 | self.drop_loc = drop_loc 79 | self.start_loc = start_loc 80 | self.pick_loc = pick_loc 81 | 82 | self.min_hover_height = 0.45 83 | 84 | # Hover buffer for drones 85 | self.hover_height = 0.1 86 | 87 | # z_level counter, increments with each run of create_multi_capable_path 88 | self.z_level = 0 89 | 90 | # Lighthouse origin relative to bottom left of map 91 | self.map_origin = (1, 1) 92 | 93 | map_dim_x = 3.5 94 | 95 | map_dim_y = 2 96 | 97 | obstacles = [ 98 | [(0, 0.3), (0, 0.605), (1.22, 0.605), (1.22, 0.3)], 99 | [(0, -0.3), (0, -0.605), (1.22, -0.605), (1.22, -0.3)] 100 | ] 101 | 102 | obstacle_list = [Obstacle(ob) for ob in obstacles] 103 | 104 | self.map = Map( 105 | origin=self.map_origin, 106 | dim_x=map_dim_x, 107 | dim_y=map_dim_y, 108 | obstacles=obstacle_list, 109 | drone_dim=0.1, 110 | prev_map = None 111 | ) 112 | 113 | def framed_pos(self, position): 114 | """function to create the framed pos values taken by A*""" 115 | framed_position = ( 116 | position[0]+self.map_origin[0], 117 | position[1]+self.map_origin[1] 118 | ) 119 | return framed_position 120 | 121 | def create_multi_capable_path(self, droneID, preprocessed_tasks, processed_map=None): 122 | """ 123 | run A* alg for a drone that will output a new map with drone intersections and a commandable path 124 | 125 | args: 126 | droneID (str): name that you are calling the drone in your code for consistency. Used 127 | mostly in the node drone intersection tuple 128 | map (map class): map that A* is run in, has all attributes of map class 129 | preprocessed_tasks (list): list of the tasks expected of this drone, in order. 130 | note: this should BEGIN with the starting location of the drone, i.e. 1 131 | 132 | returns: 133 | map (map class): same map as original with new updated intersections. Will be passed in to the next 134 | time this func runs as the processed_map variable 135 | sequence (list?): list of points commanded to the drone in (x,y,z) tuples. Advances with timesteps 136 | """ 137 | 138 | # Note: this script does NOT touch down or change altitude at all exc for very beginning and very end. 139 | # Didn't have time to do anything other than basic pathing 140 | 141 | 142 | # starting_point = "2" 143 | # hover_bin = "Bin 1a" 144 | # drop_loc = "B" 145 | 146 | 147 | 148 | # debugging statements for map 149 | # print(f"the size of the map array is{(map.array[2][13])}") 150 | # map.visualize_map() 151 | 152 | # creating list to store the meters lighthouse-frame coords of each task in task list 153 | task_loc = [] 154 | for strings in preprocessed_tasks: 155 | print(str(strings)) 156 | if str(strings) in start_loc: 157 | task_loc.append((start_loc[strings])) 158 | if str(strings) in pick_loc: 159 | task_loc.append((pick_loc[strings])) 160 | if str(strings) in drop_loc: 161 | task_loc.append((drop_loc[strings])) 162 | 163 | print(f"the task_loc is {task_loc}") 164 | print(f"len of task loc for all except start is {len(task_loc[1:][:])}") 165 | 166 | # COMMENTED 3/9/23 moving towards func format, no more start and end 167 | # Select start, hover, & end points 168 | # start = start_loc[preprocesed_tasks[0]] 169 | # start_map_frame = (start[0] + map_origin[0], start[1] + map_origin[1]) 170 | # hover_point = pick_loc[hover_bin] 171 | # hover_point_map_frame = ( 172 | # hover_point[0] + map_origin[0], 173 | # hover_point[1] + map_origin[1], 174 | # ) 175 | # end = drop_loc[preprocessed_tasks[-1]] 176 | # end_map_frame = (end[0] + map_origin[0], end[1] + map_origin[1]) 177 | 178 | # Add takeoff to sequence 179 | # x, y, z = start[0], start[1], 0.0 180 | 181 | # COMMENTED 3/9/23 might cause problems, commenting any ref to crazyflie pose estimation for now. 182 | # x, y, z = get_pose(scf) 183 | # sequence.append((x, y, hover_height / 2, 0.0)) 184 | 185 | 186 | # performing a* on all points that are not the first point in the task list bc its the start 187 | # needs to initialize and then populate astar with functions Astar and Astar.a_star_search 188 | 189 | # test the Astar 190 | # test_astar = Astar( 191 | # map=map, start=framed_pos((0,0)), 192 | # target=framed_pos(0,1)) 193 | # print(f"test generated astar between known pts {test_astar.a_star_search()}") 194 | 195 | seq_to_hover = [] 196 | for idx, locations in enumerate(task_loc[:-1]): 197 | # print(f"index of the task loc enumeration is {idx} which leads to {framed_pos(task_loc[idx])} when framed") 198 | 199 | print(f"Finding path between {self.framed_pos(task_loc[idx])} and {self.framed_pos(task_loc[idx+1])}") 200 | 201 | next_goal_astar = Astar( 202 | map=self.map, start=self.framed_pos(task_loc[idx]), 203 | target=self.framed_pos(task_loc[idx+1]) 204 | # map=map, start=start_map_frame, target=hover_point_map_frame 205 | ) 206 | 207 | new_traj = next_goal_astar.a_star_search() 208 | 209 | if new_traj: 210 | print("adding traj: ", new_traj) 211 | seq_to_hover.extend(new_traj) 212 | 213 | if len(seq_to_hover)>1: 214 | print("Generated hover sequence") 215 | else: 216 | print("Failed to generate hover sequence") 217 | 218 | # Add hover flying sequence(accomplish goals) 219 | path_to_hover = np.array(self.map.sequence_to_path(seq=seq_to_hover)) 220 | 221 | height = self.min_hover_height + self.hover_height * self.z_level 222 | self.z_level += 1 223 | 224 | HeightYaw_matrix = np.vstack([[height, 0.0]] * path_to_hover.shape[0]) 225 | 226 | path_to_hover = np.append(path_to_hover, HeightYaw_matrix, axis=1) 227 | 228 | 229 | # Add Plotting sequence 230 | path_to_plot = np.array(seq_to_hover) 231 | 232 | Height_matrix = np.vstack([height] * path_to_plot.shape[0]) 233 | 234 | path_to_plot = np.append(path_to_plot, Height_matrix, axis=1) 235 | 236 | print(path_to_plot) 237 | 238 | # COMMENTED BEFORE PREZ not working 239 | # Add takeoff and landing to flight sequence 240 | # sequence.insert(0, (path_to_hover[0][0], path_to_hover[0][1], hover_height / 2, 0.0)) 241 | # sequence.insert(1, (path_to_hover[0][0], path_to_hover[0][1], hover_height, 0.0)) 242 | 243 | # sequence.append((path_to_hover[-1][0], path_to_hover[-1][1], hover_height, 0.0)) 244 | # sequence.append((path_to_hover[-1][0], path_to_hover[-1][1], hover_height / 2, 0.0)) 245 | # sequence.append((path_to_hover[-1][0], path_to_hover[-1][1], 0.0, 0.0)) 246 | 247 | 248 | # initializing the A* object to be searched 249 | # astar_to_drop = Astar( 250 | # map=map, start=hover_point_map_frame, target=end_map_frame 251 | # ) 252 | # seq_to_drop = astar_to_drop.a_star_search() 253 | # print("Generated drop sequence") 254 | 255 | # delay so that the drone stays at a certain point 256 | # sequence.extend([(hover_point[0], hover_point[1], hover_height, 0.0)] * 20) 257 | 258 | # # Add second flight sequence 259 | # path_to_drop = map.sequence_to_path(seq=seq_to_drop) 260 | # for i, p in enumerate(path_to_drop): 261 | # path_to_drop[i] = (p[0], p[1], hover_height, 0.0) 262 | 263 | # sequence.extend(path_to_drop) 264 | 265 | 266 | # print(sequence) 267 | 268 | a_star_seq = path_to_plot 269 | 270 | world_seq = path_to_hover 271 | 272 | return world_seq, a_star_seq 273 | 274 | 275 | # Initialize Multi Agent Planner Node with key locations and map 276 | Planner = MultiPlanner(drop_loc, start_loc, pick_loc) 277 | 278 | # Define tasks and agents. # of tasks must equal # of agents 279 | task_strings = [["S1","P1","D1"], ["S2","P2","D2"]] 280 | agents = ["kevin", "bob", 'charley'] 281 | 282 | # initializing sequence that will be sent 283 | sequence = [] 284 | for i in range(len(task_strings)): 285 | seq, plot_seq = Planner.create_multi_capable_path(agents[i], task_strings[i]) 286 | sequence.append(seq) 287 | 288 | Planner.map.visualize_map( 289 | path=plot_seq, waypoint_names=task_strings[i] 290 | ) 291 | 292 | plt.show() 293 | print("Total hover sequence: ", sequence) 294 | 295 | # for s in sequence: 296 | # a_star_log.write(f"{s[0]},{s[1]},{s[2]}\n") 297 | # a_star_log.flush() 298 | 299 | # fig = plt.figure() 300 | # ax = plt.axes(projection="3d") 301 | # ax.scatter3D( 302 | # [s[0] for s in sequence], [s[1] for s in sequence], [s[2] for s in sequence] 303 | # ) 304 | # ax.set_xlim([-1, 2.5]) 305 | # ax.set_ylim([-1, 1]) 306 | # ax.set_zlim([0, 1]) 307 | # ax.set_xlabel("x") 308 | # ax.set_ylabel("y") 309 | # ax.set_zlabel("z") 310 | # ax.set_title( 311 | # f"Planned Crazyflie Path\nTakeoff Loc {task[0]}, Hover @ {task[1:-2]}, Drop Loc {task[-1]}" 312 | # ) 313 | # plt.show() 314 | 315 | 316 | 317 | # COMMENTED 3/9/23 commenting out code to run crazyflie 318 | # reset_estimator(scf) 319 | # start_position_printing(scf) 320 | # run_sequence(scf, sequence, 0.5) 321 | -------------------------------------------------------------------------------- /map_objects.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | from mpl_toolkits.mplot3d import Axes3D 3 | import numpy as np 4 | 5 | 6 | 7 | class Obstacle: 8 | def __init__(self, vertices): 9 | """ 10 | vertices are inputted as a list of (x, y) tuples in meters. 11 | 12 | This class only works for rectangular obstacles that are in line with x and y axes 13 | """ 14 | self.vertices = vertices 15 | self.top = max([vertex[1] for vertex in vertices]) 16 | self.bottom = min([vertex[1] for vertex in vertices]) 17 | self.left = min([vertex[0] for vertex in vertices]) 18 | self.right = max([vertex[0] for vertex in vertices]) 19 | 20 | 21 | 22 | class Map: 23 | # here we give our start and end, and this class will calculate 24 | # obstacles is a list of obstacle objects 25 | # prev_map is a way that map can take previous map objects and incorporate the intersections into a new map 26 | def __init__(self, origin=(0, 0), dim_x=0, dim_y=0, obstacles=[], drone_dim=0.1, prev_map = None): 27 | self.origin = origin 28 | self.dim_x = dim_x 29 | self.dim_y = dim_y 30 | self.drone_dim = drone_dim 31 | self.obstacles = obstacles 32 | self.prev_map = prev_map 33 | 34 | fig = plt.figure() 35 | self.ax = fig.add_subplot(projection='3d') 36 | 37 | self.extents = [ 38 | (0, self.dim_y), 39 | (self.dim_x, self.dim_y), 40 | (0, 0), 41 | (self.dim_x, 0), 42 | ] 43 | 44 | # creating map based on the length of x and y edges of the map - x=0 y=0 does NOT represent origin. to get origin use map.origin 45 | # because drone_dim is .1 meter, division makes each meter 10 nodes 46 | self.array = [ 47 | [Node(coords=(x, y)) for x in range(int(dim_x / drone_dim))] 48 | for y in range(int(dim_y / drone_dim)) 49 | ] 50 | 51 | self.create_obstacles(obstacles) 52 | 53 | # if there is a previous map inputted, grab its drone intersections so that you can populate the new map with higher priority drones 54 | if self.prev_map is not None: 55 | for y in range(len(self.prev_map.array)): 56 | for x in range(len(self.prev_map.array[0])): 57 | if self.prev_map.array[y][x].intersections is not None: 58 | self.array[y][x].set_intersection(self.prev_map.array[y][x].intersections) 59 | 60 | def create_obstacles(self, obstacles): 61 | for o in obstacles: 62 | # note that we are finding obstacle coords based on origin set above!! 63 | top = int((o.top + self.origin[1]) / self.drone_dim) 64 | bottom = int((o.bottom + self.origin[1]) / self.drone_dim) 65 | left = int((o.left + self.origin[0]) / self.drone_dim) 66 | right = int((o.right + self.origin[0]) / self.drone_dim) 67 | # print(f"{top=}, {bottom=}, {left=}, {right=}") 68 | 69 | # adjusting node obstacle property for all coords in obstacle 70 | for y in range(bottom, top + 1): 71 | for x in range(left, right + 1): 72 | self.array[y][x].set_obstacle(True) 73 | 74 | def get_neighbors(self, node): 75 | # grabbing position of node (meters) 76 | x_pos = node.coords[0] 77 | y_pos = node.coords[1] 78 | 79 | # initializing number of nodes in x and y dimensions of map 80 | array_y = len(self.array) 81 | array_x = len(self.array[0]) 82 | 83 | # Creating neighbor nodes based on no diagonal just horiz/vert movement 84 | # because this is relative location of nodes, we don't need to ref origin 85 | top = None if y_pos + 1 >= array_y else self.array[y_pos + 1][x_pos] 86 | bottom = None if y_pos - 1 < 0 else self.array[y_pos - 1][x_pos] 87 | left = None if x_pos - 1 < 0 else self.array[y_pos][x_pos - 1] 88 | right = None if x_pos + 1 >= array_x else self.array[y_pos][x_pos + 1] 89 | neighbors = [top, bottom, left, right] 90 | neighbors_filtered = [ 91 | n for n in neighbors if n is not None and not n.is_obstacle 92 | ] 93 | return neighbors_filtered 94 | 95 | def sequence_to_path(self, seq): 96 | path = [] 97 | # seq is a list of tuples indicating drone position in "coords" frame, not true origin relative to lighthouse frame 98 | for s in seq: 99 | if s is not None: 100 | path.append( 101 | ( 102 | (s[0] * self.drone_dim) + 0.5 * self.drone_dim - self.origin[0], 103 | (s[1] * self.drone_dim) + 0.5 * self.drone_dim - self.origin[1], 104 | ) 105 | ) 106 | return path 107 | 108 | def visualize_map(self, path=None, waypoint_names=None): 109 | obstacles_x = [] 110 | obstacles_y = [] 111 | for y in range(len(self.array)): 112 | for x in range(len(self.array[0])): 113 | if self.array[y][x].is_obstacle: 114 | obstacles_x.append(x) 115 | obstacles_y.append(y) 116 | 117 | for z in np.linspace(0, 0.5, 20): 118 | self.ax.scatter(obstacles_x, obstacles_y, [z]*len(obstacles_x), color="black") 119 | 120 | if path.all(): 121 | self.ax.scatter(path[:,0], path[:,1], path[:,2]) 122 | 123 | self.ax.set_xlim([0, len(self.array[0])]) 124 | self.ax.set_ylim([0, len(self.array)]) 125 | self.ax.set_zlim([0, 2]) 126 | self.ax.set_xlabel("x") 127 | self.ax.set_ylabel("y") 128 | title = "Planned Crazyflie Path" 129 | if waypoint_names is not None: 130 | title += f"\nTakeoff Loc {waypoint_names[0]}, Hover @ {waypoint_names[1]}, Drop Loc {waypoint_names[2]}" 131 | self.ax.set_title(title) 132 | 133 | # plt.show() 134 | 135 | class Node: 136 | """ 137 | Object representing a node in the map, which is a unit square the 138 | size of the Crazyflie drone. 139 | 140 | Attributes: 141 | _coords (2 double tuple): Coordinates of the node in the map 142 | _is_obstacle (bool): Whether the node is an obstacle 143 | parent (Node): Parent of this node for A* 144 | _cost (double): Cost value for distance from start to node position 145 | _heuristic (double): Heuristic value for distance to end from node position 146 | _f_cost (double): Total cost of node 147 | """ 148 | 149 | def __init__(self, coords=(0.0, 0.0)): 150 | """Creates a new Node with coordinates""" 151 | self._coords = coords 152 | self._is_obstacle = False 153 | self.parent = None 154 | self._cost = 0 155 | self._heuristic = 0 156 | self._f_cost = self.heuristic + self.cost 157 | self._intersections = [] 158 | # empty list where drone intersections at time indeces will be recorded 159 | 160 | @property 161 | def coords(self): 162 | return self._coords 163 | 164 | @property 165 | def is_obstacle(self): 166 | return self._is_obstacle 167 | 168 | @property 169 | def cost(self): 170 | return self._cost 171 | 172 | @property 173 | def heuristic(self): 174 | return self._heuristic 175 | 176 | @property 177 | def f_cost(self): 178 | self._f_cost = self._cost + self._heuristic 179 | return self._f_cost 180 | 181 | @property 182 | def intersections(self): 183 | return self.intersections 184 | # use this property to access points where drones intersect a node 185 | # will return a list of tuples [("droneID", time_idx), ("ID", idx)...] 186 | 187 | def get_parent(self): 188 | return self.parent 189 | 190 | def set_obstacle(self, val): 191 | self._is_obstacle = val 192 | 193 | def set_heuristic(self, val): 194 | self._heuristic = val 195 | self._f_cost = self._cost + self._heuristic 196 | 197 | def set_cost(self, val): 198 | self._cost = val 199 | self._f_cost = self._cost + self._heuristic 200 | 201 | def set_parent(self, val): 202 | self.parent = val 203 | 204 | def set_intersections(self, val): 205 | self.intersections.extend(val) 206 | # when appending intersections they are in the format ("drone ID", time idx) 207 | # where time idx is an int representing the step of A* that this intersection is 208 | 209 | def __sort__(self, other_node): 210 | return self.f_cost < other_node.f_cost 211 | 212 | def __repr__(self): 213 | return f"F_cost: {self.f_cost}" 214 | 215 | def __eq__(self, n): 216 | return self.coords == n.coords 217 | -------------------------------------------------------------------------------- /media/astar_path_2d.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RajivPerera-Robotics/UAV-Multi-Agent-Coordination/fe56256a2c24d5be8fcb53e801e9494ef291ffcb/media/astar_path_2d.png -------------------------------------------------------------------------------- /media/astar_path_3d.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RajivPerera-Robotics/UAV-Multi-Agent-Coordination/fe56256a2c24d5be8fcb53e801e9494ef291ffcb/media/astar_path_3d.png -------------------------------------------------------------------------------- /media/planned_vs_actual.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RajivPerera-Robotics/UAV-Multi-Agent-Coordination/fe56256a2c24d5be8fcb53e801e9494ef291ffcb/media/planned_vs_actual.png -------------------------------------------------------------------------------- /plot_paths.py: -------------------------------------------------------------------------------- 1 | import os 2 | import matplotlib.pyplot as plt 3 | import pandas as pd 4 | 5 | if __name__ == "__main__": 6 | drone_path = pd.read_csv(os.path.join(os.path.dirname(os.path.abspath(__file__)), "drone_path_log.csv")) 7 | a_star_path = pd.read_csv(os.path.join(os.path.dirname(os.path.abspath(__file__)), "a_star_path_log.csv")) 8 | 9 | fig = plt.figure() 10 | ax = plt.axes(projection="3d") 11 | ax.scatter3D(a_star_path['x'], a_star_path['y'], a_star_path['z']) 12 | ax.scatter3D(drone_path['x'], drone_path['y'], drone_path['z']) 13 | ax.set_xlim([-1, 2.5]) 14 | ax.set_ylim([-1, 1]) 15 | ax.set_zlim([0, 1]) 16 | ax.set_xlabel('x') 17 | ax.set_ylabel('y') 18 | ax.set_zlabel('z') 19 | ax.set_aspect('equal') 20 | ax.legend(['Planned Path', 'Actual Path']) 21 | ax.set_title("Crazyflie Planned Trajectory vs. Actual Trajectory") 22 | plt.show() 23 | -------------------------------------------------------------------------------- /test_flight.py: -------------------------------------------------------------------------------- 1 | """ 2 | This script shows a simple scripted flight path using the MotionCommander class. 3 | 4 | Simple example that connects to the crazyflie at `URI` and runs a 5 | sequence. Change the URI variable to your Crazyflie configuration. 6 | """ 7 | import logging 8 | import time 9 | 10 | import cflib.crtp 11 | from cflib.crazyflie.syncCrazyflie import SyncCrazyflie 12 | from cflib.positioning.motion_commander import MotionCommander 13 | 14 | URI = 'radio://0/60/2M' 15 | 16 | # Only output errors from the logging framework 17 | logging.basicConfig(level=logging.ERROR) 18 | 19 | 20 | if __name__ == '__main__': 21 | # Initialize the low-level drivers (don't list the debug drivers) 22 | cflib.crtp.init_drivers(enable_debug_driver=False) 23 | 24 | with SyncCrazyflie(URI) as scf: 25 | # We take off when the commander is created 26 | with MotionCommander(scf) as mc: 27 | print('Taking off!') 28 | time.sleep(1) 29 | 30 | # There is a set of functions that move a specific distance 31 | # We can move in all directions 32 | print('Moving forward 0.5m') 33 | mc.forward(0.2) 34 | # Wait a bit 35 | time.sleep(1) 36 | 37 | print('Moving up 0.2m') 38 | mc.up(0.2) 39 | # Wait a bit 40 | time.sleep(1) 41 | 42 | print('Doing a 270deg circle'); 43 | mc.circle_right(0.2, velocity=0.5, angle_degrees=270) 44 | 45 | print('Moving down 0.2m') 46 | mc.down(0.2) 47 | # Wait a bit 48 | time.sleep(1) 49 | 50 | # print('Rolling left 0.2m at 0.6m/s') 51 | # mc.left(0.2, velocity=0.6) 52 | # # Wait a bit 53 | # time.sleep(1) 54 | 55 | # print('Moving forward 0.5m') 56 | # mc.forward(0.5) 57 | 58 | # We land when the MotionCommander goes out of scope 59 | print('Landing!') --------------------------------------------------------------------------------