def __init__(self): self.scale = 4000 # initialize Qt gui application and window self.app = pg.QtGui.QApplication([]) # initialize QT self.window = gl.GLViewWidget() # initialize the view object self.window.setWindowTitle('Path Viewer') self.window.setGeometry( 0, 0, 1500, 1500) # args: upper_left_x, upper_right_y, width, height grid = gl.GLGridItem() # make a grid to represent the ground grid.scale(self.scale / 20, self.scale / 20, self.scale / 20) # set the size of the grid (distance between each line) self.window.addItem(grid) # add grid to viewer self.window.setCameraPosition(distance=self.scale, elevation=50, azimuth=-90) self.window.setBackgroundColor('k') # set background color to black self.window.show() # display configured window self.window.raise_() # bring window to the front self.plot_initialized = False # has the mav been plotted yet? # get points that define the non-rotated, non-translated mav and the mesh colors self.mav_points, self.mav_meshColors = self.get_mav_points() # dubins path parameters self.dubins_path = dubins_parameters() self.mav_body = []
def __init__(self): self.waypoints = msg_waypoints() self.segmentLength = 550 # standard length of path segments self.waypoints.type = 'dubins' self.dubins_path = dubins_parameters() self.dubins_path.radius = np.inf self.tree_courses = np.array([0.0])
def collision(self, startNode, endNode, map, radius): dubinPath = dubins_parameters() dubinPath.update(startNode[0:3], startNode.item(3), endNode[0:3], endNode.item(3), radius) if np.isnan(dubinPath.r1.item(1)): return True N, E, D = self.pointsAlongDubinsPath(dubinPath, self.pointsAlongPathSpacing) # spacing = map.city_width / map.num_city_blocks # N += (spacing * .5) # E += (spacing * .5) # N = np.remainder(N, spacing) # E = np.remainder(E, spacing) #Check for within circle of square obstacle for i in range(0, len(map.building_north)): norths = np.abs(N - map.building_north[i]) withinNorth = (norths < (map.building_width / 2 + self.clearance)) if np.any(withinNorth, axis=0): for j in range(0, len(map.building_east)): easts = np.abs(E[withinNorth] - map.building_east[j]) withinEast = (easts < (map.building_width / 2 + self.clearance)) downSub = D[withinNorth] if np.any(withinEast, axis=0): if np.any(-downSub[withinEast] < ( map.building_height[j, i] + self.clearance)): return True return False
def __init__(self, map, voronoi=None): self.scale = 4000 # initialize Qt gui application and window self.app = pg.QtGui.QApplication([]) # initialize QT self.window = gl.GLViewWidget() # initialize the view object self.window.setWindowTitle('World Viewer') #self.window.setGeometry(0, 0, 1500, 1500) # args: upper_left_x, upper_right_y, width, height sg = QtWidgets.QDesktopWidget().availableGeometry() self.window.setGeometry(sg.width() / 2., 0, sg.width() / 2., sg.height()) grid = gl.GLGridItem() # make a grid to represent the ground grid.scale(self.scale / 20, self.scale / 20, self.scale / 20) # set the size of the grid (distance between each line) self.window.addItem(grid) # add grid to viewer self.window.setCameraPosition(distance=self.scale, elevation=50, azimuth=-90) self.window.setBackgroundColor('k') # set background color to black self.window.show() # display configured window self.window.raise_() # bring window to the front self.plot_initialized = False # has the mav been plotted yet? # get points that define the non-rotated, non-translated mav and the mesh colors self.mav_points, self.mav_meshColors = self.get_mav_points() # dubins path parameters self.dubins_path = dubins_parameters() self.mav_body = [] if voronoi != None: self.voronoi = voronoi vm_all_pts = self.voronoi.E_inf self.vm_all = gl.GLLinePlotItem(pos=vm_all_pts, color=pg.glColor('w'), width=1.0, mode='lines') self.window.addItem(self.vm_all) vm_pts = self.voronoi.E self.vm = gl.GLLinePlotItem(pos=vm_pts, color=pg.glColor('y'), width=1.0, mode='lines') self.window.addItem(self.vm) #vm_path_pts = self.voronoi.path_pts #self.vm_path = gl.GLLinePlotItem(pos=vm_path_pts,color=pg.glColor('m'),width=4.0,mode='lines') #self.window.addItem(self.vm_path) # draw map self.drawMap(map) self.initialized_RRT = False self.RRT_iteration = 0 self.RRT_colors = [ pg.glColor('y'), pg.glColor('g'), pg.glColor('b'), pg.glColor('w'), pg.glColor('r'), pg.glColor('m') ]
def __init__(self): # message sent to path follower self.path = msg_path() # pointers to previous, current, and next waypoints self.ptr_previous = 0 self.ptr_current = 1 self.ptr_next = 2 # flag that request new waypoints from path planner self.flag_need_new_waypoints = True self.num_waypoints = 0 self.halfspace_n = np.inf * np.ones((3,1)) self.halfspace_r = np.inf * np.ones((3,1)) # state of the manager state machine self.manager_state = 1 # dubins path parameters self.dubins_path = dubins_parameters()
def planDubinsPath(self, wpp_start, wpp_end, map, wp_msg, min_radius): """ Dubins dubins path planner """ self.dubins_path = dubins_parameters() # desired down position is down position of end node self.pd = wpp_end.item(2) self.end = wpp_end self.segmentLength = 3.0 * min_radius # create Tree class of start node start_node = Tree(wpp_start.item(0), wpp_start.item(1), self.pd) while len(self.path_options) < self.num_paths: # initialize paths each run through self.node_iteration = 0 # establish tree starting with the start node self.tree = [start_node] self.pts = [] while self.tree[-1].goal == False: new_info = self.generateRandomDubinsNode(min_radius) if new_info != None: new_node = Tree(new_info[0], new_info[1], new_info[2], new_info[3], new_info[4], new_info[5], new_info[6]) self.tree.append(new_node) path_objects = [] cost = 0.0 # last index of tree self.index = len(self.tree) - 1 # keeps appending to path while there's a parent while self.index != None: path_objects.append(self.tree[self.index]) self.index = self.tree[self.index].parent # smooth the path path, cost = self.smoothDubinsPath(path_objects) # append path and cost to path/cost options self.path_options.append(path) self.cost_options.append(cost) # choose the best path options lowest_index = np.argmin(np.array(self.cost_options)) path = self.path_options[lowest_index] wp_msg.num_waypoints = len(path) for ii in range(len(path)): # reverses directiona and adds wp_msg wp_msg.ned[:, ii] = np.array([path[ii][1], path[ii][0], path[ii][2]]).T # if dubins, calculate course if wp_msg.type == 'dubins': for ii in range(len(path) - 1): wp_msg.course[0, ii] = np.arctan2( wp_msg.ned[1, ii + 1] - wp_msg.ned[1, ii], wp_msg.ned[0, ii + 1] - wp_msg.ned[0, ii]) return wp_msg