def plan_path(droneparameters, destinationWP):
above_takeoffWP = waypoint.Waypoint(droneparameters.X, droneparameters.Y, 300) # height of 30cm, change desired flight height here
above_destinationWP = waypoint.Waypoint(destinationWP.X, destinationWP.Y, 300) # height of 30cm, change desired flight height here
destinationWP = waypoint.Waypoint(destinationWP.X, destinationWP.Y, destinationWP.Z)
droneparameters.path = [above_takeoffWP, above_destinationWP, destinationWP] # creates the whole path of waypoints
droneparameters.targetWP = droneparameters.path[0] # sets the target waypoint to the first waypoint of the path
print("targetWP: " + str(droneparameters.targetWP.X) + " - " + str(droneparameters.targetWP.Y) + " - " + str(droneparameters.targetWP.Z))
droneparameters.onJob = True
def randomize_waypoint(generation):
global WAYPOINT
rand_x, rand_y = UNIT_SIZE * random.randint(1, NUM_UNITS - 2), UNIT_SIZE * random.randint(1, NUM_UNITS - 5)
# If waypoint would be too close to agent, re randomize
if abs(rand_x - AGENT_INITIAL_X) < NUM_UNITS // 4 * UNIT_SIZE and \
abs(rand_y - AGENT_INITIAL_Y) < NUM_UNITS // 4 * UNIT_SIZE:
randomize_waypoint(generation)
# Else, set waypoint to new coordinates
else:
WAYPOINT = waypoint.Waypoint(rand_x, rand_y)
def get_overpass_nodes(response, Pts, index_used, lat, lon, lim_dist):
i_name = 1
for node in response['features']:
lat2 = node['geometry']['coordinates'][1]
lon2 = node['geometry']['coordinates'][0]
match, near_lon, near_lat, index, min_dist = find_nearest(
lon, lat, lon2, lat2, lim_dist)
if match:
# log.debug('Distance to node: ' + '%.2f' % (min_dist * 1e3) + ' m')
has_name = False
ele = '' # set default in case proper tag not found
[lon_new, lat_new,
new_gpx_index] = add_new_coordinate(lon, lat, lon2, lat2, index)
tag_dict = node['properties']
query_name = get_wpt_type(tag_dict)
if 'ele' in tag_dict:
ele = tag_dict['ele']
if 'name' in tag_dict:
name = tag_dict['name']
has_name = True
else:
name = query_name + str(
i_name) # set by default in case proper tag not found
i_name += 1
# Because only 1 POI is possible per GPS point
if index not in index_used and new_gpx_index is not None:
# log.debug(query_name + " - " + name + " - " + ele)
Pt = waypoint.Waypoint(name,
'node',
lon_new,
lat_new,
ele,
node['id'],
index,
new_gpx_index,
query_name,
has_name,
min_dist,
tags=tag_dict)
Pts.append(Pt)
index_used.append(index)
else:
# log.debug('/!\ Node index already used: ' + query_name + " - " + name + " - " + ele)
pass
return Pts
def build(config_node):
global standby_route
standby_route = [] # clear standby route
for child_name in config_node.getChildren():
if child_name == 'name':
# ignore this for now
pass
elif child_name[:3] == 'wpt':
child = config_node.getChild(child_name)
wp = waypoint.Waypoint()
wp.build(child)
standby_route.append(wp)
elif child_name == 'enable':
# we do nothing on this tag right now, fixme: remove
# this tag from all routes?
pass
else:
print 'Unknown top level section:', child_name
return False
print 'loaded %d waypoints' % len(standby_route)
return True
def build_str(request):
global standby_route
tokens = request.split(',')
if len(tokens) < 4:
return False
standby_route = [] # clear standby
i = 0
while i + 4 <= len(tokens):
mode = int(tokens[i])
wp = waypoint.Waypoint()
if mode == 0:
wp.mode = 'relative'
wp.dist_m = float(tokens[i+1])
wp.hdg_deg = float(tokens[i+2])
else:
wp.mode = 'absolute'
wp.lon_deg = float(tokens[i+1])
wp.lat_deg = float(tokens[i+2])
standby_route.append(wp)
i += 4;
print 'Loaded %d waypoints' % len(standby_route)
return True
def process_job(self, client, userdata, msg):
# msg is of format: str(coord.x) + "," + str(coord.y) + "," + str(coord.z))
print("topic: " + str(env.mqttTopicJob) + "/" +
str(self.droneparameters.ID) + " - onjob: " +
str(self.droneparameters.onJob))
if not self.droneparameters.onJob:
print("Got job through MQTT, processing")
# Get the coordinates
coord = str(msg.payload).split(",")
# create a waypoint for the destination
destinationWP = waypoint.Waypoint(int(float(coord[0])),
int(float(coord[1])),
int(float(coord[2])))
print("destinationWP: " + str(destinationWP.X) + " - " +
str(destinationWP.Y) + " - " + str(destinationWP.Z))
pathplanner.plan_path(self.droneparameters, destinationWP)
print("pathplanning done")
self.pathfollower.check_position()
print("pathfollowing done")
self.job_client.publish(
env.mqttTopicJobdone + "/" + str(self.droneparameters.ID),
"done")
else:
print("Already on a job")
def next_path(current_tile, task_to_do):
list_of_paths = list()
node = waypoint.Waypoint(current_tile)
current_tile.set_node(node)
start_node = current_tile.get_node()
current_path = list()
all_paths = list()
if current_tile.get_coordiantes()[0] > task_to_do.get_coordinates()[0]:
prio_x = task_to_do.get_coordinates()[0] - current_tile.get_coordiantes()[0]
elif current_tile.get_coordiantes()[0] < task_to_do.get_coordinates()[0]:
prio_x = current_tile.get_coordiantes()[0] - task_to_do.get_coordinates()[0]
elif current_tile.get_coordiantes()[0] == task_to_do.get_coordinates()[0]:
prio_x = 0
if current_tile.get_coordiantes()[1] > task_to_do.get_coordinates()[1]:
prio_y = task_to_do.get_coordinates()[1] - current_tile.get_coordiantes()[1]
elif current_tile.get_coordiantes()[1] < task_to_do.get_coordinates()[1]:
prio_y = current_tile.get_coordiantes()[1] - task_to_do.get_coordinates()[1]
elif current_tile.get_coordiantes()[1] == task_to_do.get_coordinates()[1]:
prio_y = 0
if current_tile.get_coordiantes()[2] > task_to_do.get_coordinates()[2]:
prio_z = task_to_do.get_coordinates()[2] - current_tile.get_coordiantes()[2]
elif current_tile.get_coordiantes()[2] < task_to_do.get_coordinates()[2]:
prio_z = current_tile.get_coordiantes()[2] - task_to_do.get_coordinates()[2]
elif current_tile.get_coordiantes()[2] == task_to_do.get_coordinates()[2]:
prio_z = 0
# todo try ideal path
if abs(prio_y) > abs(prio_x):
if prio_y > 0:
if prio_x > 0:
# top is prio 1
# right is prio 2
while start_node is not None:
if 'top' in current_tile.get_node.get_pointer:
#top is möglich
current_tile = current_tile.get_top()
current_path.append('top')
if current_tile.get_node is None:
current_tile = current_tile.get_bot()
current_tile.get_node.remove('top')
current_path.pop()
continue
elif current_tile is task_to_do:
all_paths.append(current_path)
current_tile = current_tile.get_bot()
current_tile.get_node.remove('top')
current_path.pop()
continue
else:
node = waypoint.Waypoint(current_tile)
continue
elif 'right' in current_tile.get_node.get_pointer:
# top is nicht möglich aber right dafür
current_tile = current_tile.get_rgt()
current_path.append('right')
if current_tile.get_node is None:
current_tile = current_tile.get_lft()
current_tile.get_node.remove('right')
current_path.pop()
continue
elif current_tile is task_to_do:
all_paths.append(current_path)
current_tile = current_tile.get_lft()
current_tile.get_node.remove('right')
current_path.pop()
continue
else:
node = waypoint.Waypoint(current_tile)
continue
elif prio_x < 0:
# top is prio 1
# left is prio 2
elif prio_x == 0:
# top is prio 1 (and only prio)
elif prio_y < 0:
if prio_x > 0:
#bot is prio 1
# right is prio 2
elif prio_x < 0:
#bot is prio 1
# left is prio 2
elif prio_x == 0:
#bot is prio 1 (and only prio)
elif abs(prio_x) > abs(prio_y):
if prio_x > 0:
if prio_y > 0:
#right is prio 1
# top is prio 2
elif prio_y < 0:
#right is prio 1
# bot is prio 2
elif prio_y == 0:
#right is prio 1 (and only prio)
elif prio_x < 0:
if prio_y > 0:
#left is prio 1
# top is prio 2
elif prio_y < 0:
#left is prio 1
# bot is prio 2
elif prio_y == 0:
import airsim
import waypoint
import airsim_helper
client = airsim.MultirotorClient()
client.confirmConnection()
client.enableApiControl(True)
client.armDisarm(True)
path = [
waypoint.Waypoint(30.35315, 76.36064, "A"),
waypoint.Waypoint(30.35484, 76.36033, "B"),
]
airsim_helper.takeoffAndLandWithPath(path, client)
def get_segments_pts(response, Pts, index_used, lat, lon, lim_dist):
for segment in response:
# print segment.elev_difference
lat_start = segment.start_latlng.lat
lon_start = segment.start_latlng.lon
match, near_lon, near_lat, index, min_dist = find_nearest(
lon, lat, lon_start, lat_start, lim_dist)
lat_end = segment.end_latlng.lat
lon_end = segment.end_latlng.lon
match2, near_lon2, near_lat2, index2, min_dist = find_nearest(
lon, lat, lon_end, lat_end, lim_dist)
# print('index :' + str(index) + ' ' + str(index2))
if match and match2 and (index < index2):
# log.debug('Distance to node: ' + '%.2f' % (min_dist * 1e3) + ' m')
[lon_new, lat_new,
new_gpx_index] = add_new_coordinate(lon, lat, lon_start,
lat_start, index)
query_name = 'strava_start'
name = 'Start ' + segment.name
# Because only 1 POI is possible per GPS point
if index not in index_used and new_gpx_index is not None:
# log.debug(query_name + " - " + name)
Pt = waypoint.Waypoint(name,
'strava',
lon_new,
lat_new,
'',
segment.id,
index,
new_gpx_index,
query_name,
True,
min_dist,
tags=segment)
Pts.append(Pt)
index_used.append(index)
else:
# log.debug('/!\ Node index already used: ' + query_name + " - " + name)
pass
[lon_new, lat_new,
new_gpx_index] = add_new_coordinate(lon, lat, lon_end, lat_end,
index2)
query_name = 'strava_end'
name = 'End ' + segment.name
# Because only 1 POI is possible per GPS point
if index2 not in index_used and new_gpx_index is not None:
# log.debug(query_name + " - " + name)
Pt = waypoint.Waypoint(name,
'strava',
lon_new,
lat_new,
'',
segment.id,
index2,
new_gpx_index,
query_name,
True,
min_dist,
tags=segment)
Pts.append(Pt)
index_used.append(index2)
else:
# log.debug('/!\ Node index already used: ' + query_name + " - " + name)
pass
return Pts
import airsim
import waypoint
import math
# GLOBAL VARIABLES
CRUISE_VELOCITY = 10 # 10 m/s
CRUISE_ALTITUDE = -50 # 50 m
BASE_POSITION = waypoint.Waypoint(lat=30.35294, long=76.36183, name="BASE")
def connect(client):
"""
establishes connection to `airsim.MultirotorClient`
"""
client.confirmConnection()
client.enableApiControl(True)
client.armDisarm(True)
def gps2ned(waypoint):
bearing = BASE_POSITION.get_bearing(waypoint)
distance = BASE_POSITION.distance(waypoint)
y = math.sin(math.radians(bearing)) * distance
x = math.cos(math.radians(bearing)) * distance
return [x, y]
def traversePath(waypoints, client):
for point in waypoints:
ned = gps2ned(point)
print(ned, point.name)
def get_overpass_ways(response, Pts, index_used, lat, lon, lim_dist):
"""
Purpose: Return way features close to the gpx route from the overpass query
Inputs:
- response: overpass response (json format)
- Pts: The waypoints
- index_used: The gpx index point(s) linked to a waypoint
- lat
- lon
- lim_dist
Return: list of waypoints with attributes
"""
i_name = 1
for way in response['features']:
table = [(coord[0], coord[1])
for coord in way['geometry']['coordinates']]
lon2, lat2 = [x[0] for x in table], [x[1] for x in table]
match, near_lon, near_lat, index = find_nearest_way(
lon, lat, lon2, lat2, lim_dist)
if match == 1:
tag_dict = way['properties']
query_name = get_wpt_type(tag_dict)
ele = '' # set default in case proper tag not found
has_name = False
if 'name' in tag_dict:
name = tag_dict['name']
has_name = True
if 'ele' in tag_dict:
ele = tag_dict['ele']
[lon_new, lat_new,
new_gpx_index] = add_new_coordinate(lon, lat, near_lon, near_lat,
index)
if not has_name:
name = query_name + str(
i_name) # set by default in case proper tag not found
i_name += 1
# Because only 1 POI is possible per GPS point
if index not in index_used and new_gpx_index is not None:
# log.debug(query_name + " - " + name)
Pt = waypoint.Waypoint(name,
'way',
lon_new,
lat_new,
ele,
way['id'],
index,
new_gpx_index,
query_name,
has_name,
0,
tags=tag_dict) # todo lim dist
Pts.append(Pt)
index_used.append(index)
else:
# log.debug('/!\ Node index already used: ' + query_name + " - " + name + " - " + ele)
pass
return Pts