def findMinimumPath(self, tree, end_node):
# Find the leaves that connect to the end node
connectedNodes = []
for i in range(0, np.size(tree, 0)):
if tree[i, 5] == 1:
connectedNodes.append(i)
# Find the path with the shortest distance (could find a different heuristic for choosing which path to go with,
# especially because we are going to shorten the path anyway??). Choose shortest after smoothing?? Or choose for
# least turns.
minIndex = np.argmin(tree[connectedNodes, 3])
minIndex = connectedNodes[minIndex]
waypoints = msg_waypoints()
waypoints.ned[:, waypoints.num_waypoints] = end_node[0:3]
waypoints.num_waypoints += 1
waypoints.ned[:, waypoints.num_waypoints] = tree[minIndex, 0:3]
waypoints.num_waypoints += 1
parentNode = int(tree[minIndex, 4])
while parentNode > 0:
waypoints.ned[:, waypoints.num_waypoints] = tree[parentNode, 0:3]
waypoints.num_waypoints += 1
parentNode = int(tree[parentNode, 4])
waypoints.ned[:, waypoints.num_waypoints] = tree[parentNode, 0:3]
waypoints.num_waypoints += 1 # This adds the starting point
waypoints2 = msg_waypoints()
for i in range(0, waypoints.num_waypoints):
waypoints2.ned[:,
i] = waypoints.ned[:,
waypoints.num_waypoints - i - 1]
waypoints.ned = waypoints2.ned
return waypoints
def smoothPath(self, path, map):
waypoints = msg_waypoints()
waypoints.ned[:, waypoints.num_waypoints] = path.ned[:, waypoints.
num_waypoints]
waypoints.num_waypoints += 1
index = 1
while index < path.num_waypoints - 1:
if self.collision(waypoints.ned[:, waypoints.num_waypoints - 1],
path.ned[:, index + 1], map):
# self.flyablePath(smoothedPath[len(smoothedPath) - 1], path[index + 1], prev_chi, chi):
waypoints.ned[:, waypoints.num_waypoints] = path.ned[:, index]
waypoints.num_waypoints += 1
index += 1
waypoints.ned[:,
waypoints.num_waypoints] = path.ned[:,
path.num_waypoints -
1]
waypoints.num_waypoints += 1
waypoints.airspeed = path.airspeed
# smoothedPath.append(path[len(path) - 1])
# for i in range(0, len(smoothedPath) - 1): # Could add other things to this cost function if wanted
# cost += np.sqrt(
# (smoothedPath[i].n - smoothedPath[i + 1].n) ** 2 + (smoothedPath[i].e - smoothedPath[i + 1].e) ** 2 + \
# (smoothedPath[i].d - smoothedPath[i + 1].d) ** 2)
return waypoints
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 __init__(self, Ts, time_horizon):
# message sent to path follower
self.Ts = Ts
self.path = msg_path()
self.optimize = optimizer(Ts, time_horizon)
self.waypoints = msg_waypoints()
self.N = time_horizon
def planPath(self, wpp_start, wpp_end, R_min, map):
self.segmentLength = 3.5 * R_min #2.5 ??
# desired down position is down position of end node
pd = wpp_end.item(2)
numSolvedPaths = 0
solvedPaths = []
solvedPathsWeights = []
while numSolvedPaths < 4:
# specify start and end nodes from wpp_start and wpp_end
# format: N, E, D, chi, cost, parentIndex, connectsToGoalFlag,
start_node = np.array([
wpp_start.item(0),
wpp_start.item(1), pd,
self.mod(wpp_start.item(3)), 0, -1, 0
])
end_node = np.array([
wpp_end.item(0),
wpp_end.item(1), pd,
self.mod(wpp_end.item(3)), 0, 0, 0
])
# establish tree starting with the start node
tree = np.array([start_node])
# check to see if start_node connects directly to end_node
collision, weight = self.collision(start_node, end_node, map,
R_min)
if ((np.linalg.norm(start_node[0:3] - end_node[0:3]) <
self.segmentLength)
and np.linalg.norm(start_node[0:3] - end_node[0:3]) >=
3. * R_min and not collision):
waypointsPath = msg_waypoints()
waypointsPath.ned[:,
waypointsPath.num_waypoints] = end_node[0:3]
waypointsPath.airspeed[
waypointsPath.num_waypoints] = end_node[4]
waypointsPath.course[waypointsPath.num_waypoints] = end_node[3]
waypointsPath.num_waypoints = 1
return waypointsPath
else:
numPaths = 0
while numPaths < 1: #?? Change to do full trees multiple times. Then pick best path. Need to calculate dubin cost though to compare.
tree, flag = self.extendTree(tree, end_node, R_min, map,
pd)
numPaths = numPaths + flag
# find path with minimum cost to end_node
if flag == 2:
continue
minPath = self.findMinimumPath(tree, end_node)
smoothPath, newWeight = self.smoothPath(minPath, map, R_min)
smoothPath2, newWeight2 = self.smoothPath(smoothPath, map, R_min)
solvedPaths.append(smoothPath2)
solvedPathsWeights.append(newWeight2)
numSolvedPaths += 1
minIndex = solvedPathsWeights.index(min(solvedPathsWeights))
return solvedPaths[minIndex]
def __init__(self,map,world_view,initial_position):
# waypoints definition
self.waypoints = msg_waypoints()
self.end = np.array([PLAN.city_width/2.0, PLAN.city_width/2.0, P.pd0])#, p.u0])
self.start = np.array([-PLAN.city_width, -PLAN.city_width, P.pd0])#, P.u0])
self.map = map
self.rrt = planRRT(map,world_view)
self.initial_position = initial_position
def __init__(self, map):
self.waypoints = msg_waypoints()
#segment length must be greater than 3*R_min (right now 3*175) for dubins path
# I am just going to do two plus a little and hope that it works
self.segmentLength = 400 # standard length of path segments
#keep in mind the size of the buildings. Right now they are 72 wide
self.min_distance = 100 # minimum distance from building center to path
self.complete_path = []
self.reqPaths = 3
def createWaypoints(self, path, wp_type):
waypoints = msg_waypoints()
neds = np.zeros((path.size,3))
courses = np.zeros(path.size)
for i in range(path.size):
neds[i] = path[i].ned
if wp_type == 'dubins':
if i == path.size-1:
vec = path[i].ned - path[i-1].ned
else:
vec = path[i+1].ned - path[i].ned
courses[i] = np.arctan2(vec[0], vec[1])
waypoints.add_waypoints(wp_type, neds, courses)
return waypoints
def smoothPath(self, path, map, R_min):
waypoints = msg_waypoints()
waypoints.ned[:, waypoints.num_waypoints] = path.ned[:, waypoints.
num_waypoints]
waypoints.course[:,
waypoints.num_waypoints] = path.course[:, waypoints.
num_waypoints]
waypoints.num_waypoints += 1
index = 1
while index < path.num_waypoints - 1:
# chi = np.arctan2((path.ned[1,index + 1] - waypoints.ned[1,waypoints.num_waypoints-1]),
# (path.ned[0,index + 1] - waypoints.ned[0,waypoints.num_waypoints-1]))
if self.collision(np.concatenate((waypoints.ned[:,waypoints.num_waypoints-1],waypoints.course[:,waypoints.num_waypoints-1]),axis=0), np.concatenate((path.ned[:,index + 1],path.course[:,index + 1]),axis=0), map, R_min) or \
np.linalg.norm(waypoints.ned[:,waypoints.num_waypoints-1] - path.ned[:,index + 1]) <= 2.*R_min:
# self.flyablePath(smoothedPath[len(smoothedPath) - 1], path[index + 1], prev_chi, chi):
waypoints.ned[:, waypoints.num_waypoints] = path.ned[:, index]
waypoints.course[:,
waypoints.num_waypoints] = path.course[:,
index]
waypoints.airspeed[:, waypoints.
num_waypoints] = path.airspeed[:, index]
waypoints.num_waypoints += 1
index += 1
waypoints.ned[:,
waypoints.num_waypoints] = path.ned[:,
path.num_waypoints -
1]
waypoints.course[:, waypoints.
num_waypoints] = path.course[:,
path.num_waypoints - 1]
waypoints.airspeed[:, waypoints.
num_waypoints] = path.airspeed[:,
path.num_waypoints -
1]
waypoints.num_waypoints += 1
# smoothedPath.append(path[len(path) - 1])
# for i in range(0, len(smoothedPath) - 1): # Could add other things to this cost function if wanted
# cost += np.sqrt(
# (smoothedPath[i].n - smoothedPath[i + 1].n) ** 2 + (smoothedPath[i].e - smoothedPath[i + 1].e) ** 2 + \
# (smoothedPath[i].d - smoothedPath[i + 1].d) ** 2)
return waypoints
def planPath(self, wpp_start, wpp_end, map):
# desired down position is down position of end node
pd = wpp_end.item(2)
# specify start and end nodes from wpp_start and wpp_end
# format: N, E, D, cost, parentIndex, connectsToGoalFlag,
start_node = np.array(
[wpp_start.item(0),
wpp_start.item(1), pd, 0, -1, 0])
end_node = np.array([wpp_end.item(0), wpp_end.item(1), pd, 0, 0, 0])
# establish tree starting with the start node
tree = np.array([start_node])
# check to see if start_node connects directly to end_node
if ((np.linalg.norm(start_node[0:3] - end_node[0:3]) <
self.segmentLength)
and not self.collision(start_node, end_node, map)):
waypointsPath = msg_waypoints()
waypointsPath.ned[:, 0:2] = np.append([start_node[0:3]],
[end_node[0:3]],
axis=0).T
# waypointsPath.airspeed[0:2] = np.append(start_node[3],end_node[3],axis=0)
# np.append(tree, newNode, axis=0)
waypointsPath.num_waypoints = 2
return waypointsPath
else:
numPaths = 0
while numPaths < 3:
tree, flag = self.extendTree(tree, end_node, map, pd)
numPaths = numPaths + flag
# find path with minimum cost to end_node
path = self.findMinimumPath(tree, end_node)
return self.smoothPath(path, map)
from chap2.video_writer import video_writer
video = video_writer(video_name="chap11_video.avi",
bounding_box=(0, 0, 1000, 1000),
output_rate=SIM.ts_video)
# initialize elements of the architecture
wind = wind_simulation(SIM.ts_simulation)
mav = mav_dynamics(SIM.ts_simulation)
ctrl = autopilot(SIM.ts_simulation)
obsv = observer(SIM.ts_simulation)
path_follow = path_follower()
path_manage = path_manager()
# waypoint definition
from message_types.msg_waypoints import msg_waypoints
waypoints = msg_waypoints()
#waypoints.type = 'straight_line'
#waypoints.type = 'fillet'
waypoints.type = 'dubins'
waypoints.num_waypoints = 4
Va = PLAN.Va0
waypoints.ned[:, 0:waypoints.num_waypoints] \
= np.array([[0, 0, -100],
[1000, 0, -100],
[0, 1000, -100],
[1000, 1000, -100]]).T
waypoints.airspeed[:, 0:waypoints.num_waypoints] \
= np.array([[Va, Va, Va, Va]])
waypoints.course[:, 0:waypoints.num_waypoints] \
= np.array([[np.radians(0),
np.radians(45),
def __init__(self):
self.waypoints = msg_waypoints()
self.segmentLength = 200 # standard length of path segments
def __init__(self):
# waypoints definition
self.waypoints = msg_waypoints()
self.rrt = planRRT()
def __init__(self):
# waypoints definition
self.waypoints = msg_waypoints()
def main():
if DATA:
screen_pos = [2000, 0] # x, y position on screen
data_view = data_viewer(*screen_pos) # initialize view of data plots
# Holodeck Setup
env = holodeck.make("Ocean", show_viewport=True)
env.reset()
# wave intensity: 1-13, wave size: 1-8, wave dir: 0-360 deg
env.set_ocean_state(6, 3, 90)
env.set_day_time(5)
env.set_weather('Cloudy')
env.set_aruco_code(False)
alt = -PLAN.MAV.pd0*100
if DEBUG_HOLO:
env.set_control_scheme("uav0", ControlSchemes.UAV_ROLL_PITCH_YAW_RATE_ALT)
uav_cmd = np.array([0, -0.2, 0, alt])
uav_cmd = np.array([0,0,0,0])
boat_cmd = 0
env.act("uav0", uav_cmd)
env.act("boat0", boat_cmd)
# Just for getting UAV off the platform
state = env.set_state("uav0", [-1500, 0, 6000], [0,0,0], [0,0,0], [0,0,0])["uav0"]
state = env.set_state("uav0", [-1500, 0, 2000], [0,0,0], [0,0,0], [0,0,0])["uav0"]
env.teleport("boat0", location=[3500, 0, 0], rotation=[0, 0, 0])
# Initialization
pos = np.array([OFFSET_N, OFFSET_E, alt])
att = np.array([0, 0, 0])
vel = np.array([0, 0, 0]) * 100
angvel = np.array([0, 0, 0])
state = env.set_state("uav0", pos, att, vel, angvel)["uav0"]
# Camera
cam_alt = 300
env.teleport_camera([0, 0, cam_alt], [0, 0, -0.5])
if DEBUG_HOLO:
pos = state["LocationSensor"]
# env.teleport_camera([0, 0, cam_alt], [0, 0, -0.5])
for i in range(1):
state = env.tick()["uav0"]
pos = state["LocationSensor"]
# env.teleport_camera([0, 0, cam_alt], [0, 0, -0.5])
if SHOW_PIXELS:
pixels = state["RGBCamera"]
cv2.imshow('Camera', pixels)
cv2.waitKey(0)
print("READY FOR SIM")
# sys.exit(0)
# initialize elements of the architecture
wind = wind_simulation(SIM.ts_simulation)
mav = mav_dynamics(SIM.ts_simulation)
ctrl = autopilot(SIM.ts_simulation)
obsv = observer(SIM.ts_simulation)
path_follow = path_follower()
path_manage = path_manager()
# waypoint definition
waypoints = msg_waypoints()
waypoints.type = 'straight_line'
waypoints.type = 'fillet'
waypoints.type = 'dubins'
waypoints.num_waypoints = 4
Va = PLAN.Va0
d = 1000.0
h = -PLAN.MAV.pd0
waypoints.ned[:waypoints.num_waypoints] = np.array([[0, 0, -h],
[d, 0, -h],
[0, d, -h],
[d, d, -h]])
waypoints.airspeed[:waypoints.num_waypoints] = np.array([Va, Va, Va, Va])
waypoints.course[:waypoints.num_waypoints] = np.array([np.radians(0),
np.radians(45),
np.radians(45),
np.radians(-135)])
# initialize the simulation time
sim_time = SIM.start_time
delta = np.zeros(4)
mav.update(delta) # propagate the MAV dynamics
mav.update_sensors()
# main simulation loop
# print("Waiting for keypress")
# input()
print("Press Q to exit...")
while sim_time < SIM.end_time:
#-------observer-------------
measurements = mav.update_sensors() # get sensor measurements
# estimate states from measurements
estimated_state = obsv.update(measurements)
#-------path manager-------------
path = path_manage.update(waypoints, PLAN.R_min, estimated_state)
#-------path follower-------------
autopilot_commands = path_follow.update(path, estimated_state)
# autopilot_commands = path_follow.update(path, mav.true_state)
#-------controller-------------
delta, commanded_state = ctrl.update(autopilot_commands, estimated_state)
#-------physical system------j-------
current_wind = wind.update() # get the new wind vector
mav.update(delta, current_wind) # propagate the MAV dynamics
#-------update holodeck-------------
update_holodeck(env, mav.true_state)
draw_holodeck(env, waypoints)
#-------update viewer-------------
if DATA:
data_view.update(mav.true_state, # true states
estimated_state, # estimated states
commanded_state, # commanded states
SIM.ts_simulation)
#-------increment time-------------
sim_time += SIM.ts_simulation
def smoothPath(self, path, map, R_min):
waypoints = msg_waypoints()
waypoints.ned[:, waypoints.num_waypoints] = path.ned[:, waypoints.
num_waypoints]
waypoints.course[:,
waypoints.num_waypoints] = path.course[:, waypoints.
num_waypoints]
waypoints.num_waypoints += 1
weight = 0
index = 1
while index < path.num_waypoints - 1:
collision, pathWeight = self.collision(
np.concatenate(
(waypoints.ned[:, waypoints.num_waypoints - 1],
waypoints.course[:, waypoints.num_waypoints - 1]),
axis=0),
np.concatenate(
(path.ned[:, index + 1], path.course[:, index + 1]),
axis=0), map, R_min)
if collision or \
np.linalg.norm(waypoints.ned[:,waypoints.num_waypoints-1] - path.ned[:,index + 1]) <= 2.*R_min:
# self.flyablePath(smoothedPath[len(smoothedPath) - 1], path[index + 1], prev_chi, chi):
waypoints.ned[:, waypoints.num_waypoints] = path.ned[:, index]
waypoints.course[:,
waypoints.num_waypoints] = path.course[:,
index]
waypoints.airspeed[:, waypoints.
num_waypoints] = path.airspeed[:, index]
weight += pathWeight
waypoints.num_waypoints += 1
index += 1
collision, pathWeight = self.collision(
np.concatenate((waypoints.ned[:, waypoints.num_waypoints - 1],
waypoints.course[:, waypoints.num_waypoints - 1]),
axis=0),
np.concatenate((path.ned[:, index], path.course[:, index]),
axis=0), map, R_min)
waypoints.ned[:,
waypoints.num_waypoints] = path.ned[:,
path.num_waypoints -
1]
waypoints.course[:, waypoints.
num_waypoints] = path.course[:,
path.num_waypoints - 1]
waypoints.airspeed[:, waypoints.
num_waypoints] = path.airspeed[:,
path.num_waypoints -
1]
weight += pathWeight
waypoints.num_waypoints += 1
# #Loop through again, but jumping two
# waypoints2 = msg_waypoints()
# waypoints2.ned[:, waypoints.num_waypoints] = waypoints.ned[:, waypoints.num_waypoints]
# waypoints2.course[:, waypoints.num_waypoints] = waypoints.course[:, waypoints.num_waypoints]
# waypoints2.num_waypoints += 1
# weight = 0
# index = 2
# while index < waypoints.num_waypoints - 1:
# collision, pathWeight = self.collision(np.concatenate(
# (waypoints2.ned[:, waypoints2.num_waypoints - 1], waypoints2.course[:, waypoints2.num_waypoints - 1]),
# axis=0), np.concatenate((waypoints.ned[:, index + 1], waypoints.course[:, index + 1]), axis=0), map, R_min)
# if collision or \
# np.linalg.norm(
# waypoints2.ned[:, waypoints2.num_waypoints - 1] - waypoints.ned[:, index + 1]) <= 2. * R_min:
# # self.flyablePath(smoothedPath[len(smoothedPath) - 1], path[index + 1], prev_chi, chi):
# waypoints2.ned[:, waypoints2.num_waypoints] = waypoints.ned[:, index]
# waypoints2.course[:, waypoints2.num_waypoints] = waypoints.course[:, index]
# waypoints2.airspeed[:, waypoints2.num_waypoints] = waypoints.airspeed[:, index]
# weight += pathWeight
# waypoints2.num_waypoints += 1
# index += 2
# collision, pathWeight = self.collision(np.concatenate(
# (waypoints2.ned[:, waypoints2.num_waypoints - 1], waypoints2.course[:, waypoints2.num_waypoints - 1]), axis=0),
# np.concatenate((waypoints.ned[:, index], waypoints.course[:, index]),
# axis=0), map, R_min)
# waypoints2.ned[:, waypoints.num_waypoints] = waypoints.ned[:, waypoints.num_waypoints - 1]
# waypoints2.course[:, waypoints.num_waypoints] = waypoints.course[:, waypoints.num_waypoints - 1]
# waypoints2.airspeed[:, waypoints.num_waypoints] = waypoints.airspeed[:, waypoints.num_waypoints - 1]
# weight += pathWeight
# waypoints2.num_waypoints += 1
return waypoints, weight
def __init__(self):
# waypoints definition
self.waypoints = msg_waypoints()
self.rrt = planRRT(map)
self.rrtDubins = planRRTDubins(map)
self.rrtDubinsProj = planRRTDubinsProj(map)
def __init__(self, map):
self.map = map
self.num_closest_points = 3
self.end = np.array([[PLAN.city_width / 2., PLAN.city_width / 2.]])
start = np.array([[
P.pn0_corner,
P.pe0_corner,
]])
start = np.array([np.transpose([start[0, 1], start[0, 0]])])
north = self.map.building_north.reshape((PLAN.num_blocks**2, 1))
east = self.map.building_east.reshape((PLAN.num_blocks**2, 1))
self.points = np.hstack((east, north))
self.E = [] # edges array
vor = Voronoi(self.points)
self.ridge = vor.ridge_vertices
self.V = np.concatenate((vor.vertices, start, self.end), axis=0)
num_vertices = len(vor.vertices)
self.ridge_points = vor.ridge_points
# debugging
#voronoi_plot_2d(vor,line_colors='red',show_vertices=True)
#plt.show()
for vpair in self.ridge:
if vpair[0] >= 0 and vpair[1] >= 0:
v0 = vor.vertices[vpair[0]]
v1 = vor.vertices[vpair[1]]
self.E.append([[v0[0], v0[1]], [v1[0], v1[1]]])
if num_vertices < self.num_closest_points:
self.num_closest_points = num_vertices
closest = np.ones((self.num_closest_points, 2), dtype=float)
closest *= PLAN.city_width * 1e90
for ii in range(num_vertices):
distance = VT.calcDistance(vor.vertices[ii], start[0])
if distance < np.amax(closest[:, 0]):
closest[np.argmax(closest[:, 0]), 1] = ii
closest[np.argmax(closest[:, 0]), 0] = distance
start_index = num_vertices
for ii in range(self.num_closest_points):
closest_index = int(closest[ii, 1])
self.ridge.append([closest_index, start_index])
self.E.append([[
vor.vertices[int(closest[ii, 1]), 0],
vor.vertices[int(closest[ii, 1]), 1]
], [start[0][0], start[0][1]]])
closest = np.ones((self.num_closest_points, 2), dtype=float)
closest *= PLAN.city_width * 1e9
for ii in range(num_vertices):
distance = VT.calcDistance(vor.vertices[ii], self.end[0])
if distance < np.amax(closest[:, 0]):
closest[np.argmax(closest[:, 0]), 1] = ii
closest[np.argmax(closest[:, 0]), 0] = distance
end_index = num_vertices + 1
for ii in range(self.num_closest_points):
closest_index = int(closest[ii, 1])
self.ridge.append([closest_index, end_index])
self.E.append([[
vor.vertices[int(closest[ii, 1]), 0],
vor.vertices[int(closest[ii, 1]), 1]
], [self.end[0][0], self.end[0][1]]])
#self.E.append([[start[0][0],start[0][1]],[self.end[0][0],self.end[0][1]]])
self.E.append([[start[0][0], start[0][1]], [start[0][0], start[0][1]]])
self.E = np.asarray(self.E)
self.E_inf = []
center = self.points.mean(axis=0)
for pointidx, simplex in zip(self.ridge_points, self.ridge):
simplex = np.asarray(simplex)
if np.any(simplex < 0):
i = simplex[simplex >= 0][0] # finite end Voronoi vertex
t = self.points[pointidx[1]] - self.points[
pointidx[0]] # tangent
t = t / np.linalg.norm(t)
n = np.array([-t[1], t[0]]) # normal
midpoint = self.points[pointidx].mean(axis=0)
far_point = vor.vertices[i] + np.sign(
np.dot(midpoint - center, n)) * n * PLAN.city_width * 10.0
self.E_inf.append([[vor.vertices[i, 0], vor.vertices[i, 1]],
[far_point[0], far_point[1]]])
#plt.plot([vor.vertices[i,0], far_point[0]],[vor.vertices[i,1], far_point[1]], 'g--')
self.E_inf = np.asarray(self.E_inf)
# assign weight
weight = np.zeros((self.E.shape[0], 1))
self.ridge = list(filter(lambda x: x[0] >= 0, self.ridge))
for ii in range(len(self.ridge)):
D_prime = np.zeros((self.points.shape[0], 1))
for jj in range(self.points.shape[0]):
sigma_star = VT.calcSigmaStar(self.points[jj],
self.E[ii, 0, :], self.E[ii,
1, :])
if sigma_star < 0.0:
D_prime[jj] = np.linalg.norm(self.points[jj] -
self.E[ii, 0, :])
elif sigma_star > 1.0:
D_prime[jj] = np.linalg.norm(self.points[jj] -
self.E[ii, 1, :])
else:
D_prime[jj] = VT.calcWeight(self.points[jj],
self.E[ii, 0, :], self.E[ii,
1, :])
weight[ii] = VT.calcCost(self.E[ii, 0, :], self.E[ii, 1, :],
np.amin(D_prime))
if np.amin(D_prime) < 0:
print(np.amin(D_prime))
path = VT.dijkstraSearch(self.V, self.ridge, weight)
'''
path_weight = np.zeros((len(path),1))
for mm in range(len(path)):
path_weight[mm] = weight[mm]
print(path_weight)
'''
self.path_pts = []
for ii in range(len(path) - 1):
for jj in range(len(self.ridge)):
if path[ii] < path[ii + 1]:
a = path[ii]
b = path[ii + 1]
else:
a = path[ii + 1]
b = path[ii]
if [a, b] == self.ridge[jj]:
self.path_pts.append(self.E[jj, :, :])
break
self.path_pts = np.asarray(self.path_pts)
#return self.V[path[1]],start,np.amax(weight)
# waypoints return message
self.waypoints = msg_waypoints()
self.waypoints.type = 'fillet'
self.waypoints.num_waypoints = len(path)
Va = PLAN.Va0
self.waypoints.airspeed[:, 0:self.waypoints.num_waypoints] \
= Va*np.ones((1,self.waypoints.num_waypoints))
path_plan = np.zeros((3, len(path)))
for ii in range(len(path)):
path_plan[:, ii] = [
self.V[path[ii]].item(1), self.V[path[ii]].item(0), -100.
]
self.waypoints.ned[:, 0:self.waypoints.num_waypoints] \
= path_plan
course_plan = np.zeros((1, len(path)))
for ii in range(0, len(path) - 1):
#print(self.waypoints.ned[1,ii+1]-self.waypoints.ned[1,ii])
#print(np.arctan2(self.waypoints.ned[1,ii+1]-self.waypoints.ned[1,ii],self.waypoints.ned[0,ii+1]-self.waypoints.ned[0,ii]))
course_plan[0, ii] = np.arctan2(
self.waypoints.ned[1, ii + 1] - self.waypoints.ned[1, ii],
self.waypoints.ned[0, ii + 1] - self.waypoints.ned[0, ii])
self.waypoints.course[:, 0:self.waypoints.num_waypoints] \
= course_plan
