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
