def __init__(self):
self.targetLocation = PositionVector()
self.vehicleLocation = PositionVector()
self.backgroundColor = (74, 88, 109)
#load target
filename = sc_config.config.get_string(
'simulator', 'target_location',
'/home/dannuge/SmartCamera/target.jpg')
target_size = sc_config.config.get_float('algorithm', 'outer_ring',
1.0)
self.load_target(filename, target_size)
#define camera
self.camera_width = sc_config.config.get_integer(
'camera', 'width', 640)
self.camera_height = sc_config.config.get_integer(
'camera', 'height', 640)
self.camera_vfov = sc_config.config.get_float('camera', 'vertical-fov',
72.42)
self.camera_hfov = sc_config.config.get_float('camera',
'horizontal-fov', 72.42)
self.camera_fov = math.sqrt(self.camera_vfov**2 + self.camera_hfov**2)
self.camera_frameRate = 30
def project_position(self, origin, pitch, yaw, distance):
cos_pitch = math.cos(pitch)
dx = distance * math.cos(yaw) * cos_pitch
dy = distance * math.sin(yaw) * cos_pitch
dz = distance * math.sin(pitch)
ret = PositionVector(origin.x + dx, origin.y + dy, origin.z + dz)
return ret
def put_location(self, timestamp, location, vel):
loc = PositionVector().get_from_location(location) # covert to meters
self.location_buffer[self.lb_index] = (timestamp, loc, vel)
if (self.lb_index == self.size - 1):
self.lb_index = 0
else:
self.lb_index += 1
def earthframe_rad_to_relative_copter(ef_angle_to_target,
location,
alt_above_terrain=None):
veh_pos = PositionVector().get_from_location(location)
#get current altitude (constrained to no lower than 50cm)
if alt_above_terrain is None:
alt_above_terrain = location.alt
alt = max(alt_above_terrain, 0.5)
#convert earth-frame angles to earth-frame position offset
x = alt * math.tan(ef_angle_to_target.x) #meters
y = alt * math.tan(ef_angle_to_target.y)
z = 0 #not used
target_pos = PositionVector(x, y, z)
return target_pos
def run(self):
while True:
if self.vehicle.mode.name !="GUIDED": #if we are no longer in guided mode stop receving messages
logging.info("follower vehicle aborted GUIDED mode")
self.recv_thread.stop()
break
try:
(message,address) = self.msg_queue.get()
self.haversine_distance((message.lat,message.lon),(self.vehicle.location.global_relative_frame.lat,self.vehicle.location.global_relative_frame.lon))
if not self.called:
#set home location to home location of master vehicle
PositionVector.set_home_location(LocationGlobal(message.home_location.lat,message.home_location.lon,message.home_location.alt))
logging.debug("master home location is...")
logging.debug(message.home_location)
self.called = True
logging.debug("message received %s %s %s" %(message.lat, message.lon,message.alt))
#altitude = 5 #in metres
separation = self.distance #distance between drones
original_pos = PositionVector.get_from_location(message)
logging.debug("position for leader vehicle -> %s heading -> %s" %(original_pos,message.heading))
logging.debug("leader vehicle groundspeed is %.3f" %message.groundspeed)
logging.debug("leader vehicle lidar altitude is %.4f m"%message.rangefinder)
new_pos = selection_coding(original_pos,message.heading,separation)
new_pos = PositionVector(new_pos[0],new_pos[1],new_pos[2])
logging.debug("position for follower vehicle is %s" %(new_pos))
lat = new_pos.get_location().lat
lon = new_pos.get_location().lon
alt = new_pos.get_location().alt - message.home_location.alt
dest = LocationGlobalRelative(lat,lon,alt)
logging.debug("gps locationglobal data for follower vehicle to procced to is %s" %new_pos.get_location())
logging.debug("gps locationglobalrelative data for follower vehicle to procced to is %s" %dest)
self.vehicle.simple_goto(dest)
set_yaw(self.vehicle,message.heading)
logging.debug("vehicle groundspeed is %.3f" %self.vehicle.groundspeed)
logging.debug("vehicle heading is %.3f" %self.vehicle.heading)
logging.debug("vehicle lidar altitude is %.4f m "%self.vehicle.rangefinder.distance)
actualpath = PositionVector.get_from_location(self.vehicle.location.global_frame)
logging.debug("vehicle actual path in X direction is %.5f m" %actualpath.x)
logging.debug("vehicle actual path in Y direction is %.5f m" %actualpath.y)
time.sleep(0.1)
logging.debug("**************")
self.count+=1
self.msg_queue.task_done()
except Queue.Empty:
print ("all messages processed")
def main(self):
# set home to tridge's home field (absolute alt = 270)
PositionVector.set_home_location(
LocationGlobal(-35.362938, 149.165085, 0))
# calculate balloon position
fake_balloon_pos = PositionVector.get_from_location(
self.fake_balloon_location)
# vehicle attitude and position
veh_pos = PositionVector(0, 0, fake_balloon_pos.z) # at home location
veh_roll = math.radians(0) # leaned right 10 deg
veh_pitch = math.radians(0) # pitched back at 0 deg
veh_yaw = PositionVector.get_bearing(
veh_pos, fake_balloon_pos) # facing towards fake balloon
# display positions from home
print "Vehicle %s" % veh_pos
print "Balloon %s" % fake_balloon_pos
# generate simulated frame of balloon 10m north, 2m above vehicle
img = self.get_simulated_frame(veh_pos, veh_roll, veh_pitch, veh_yaw)
while (True):
# move vehicle towards balloon
veh_pos = veh_pos + (fake_balloon_pos - veh_pos) * 0.01
# regenerate frame
img = self.get_simulated_frame(veh_pos, veh_roll, veh_pitch,
veh_yaw)
# look for balloon in image using blob detector
found_in_image, xpos, ypos, size = balloon_finder.analyse_frame(
img)
# display actual vs real distance
dist_actual = PositionVector.get_distance_xyz(
veh_pos, fake_balloon_pos)
dist_est = balloon_utils.get_distance_from_pixels(
size, balloon_finder.balloon_radius_expected)
print "Dist Est:%f Act:%f Size Est:%f Act:%f" % (
dist_est, dist_actual, size, self.last_balloon_radius)
# show image
cv2.imshow("fake balloon", img)
# wait for keypress
k = cv2.waitKey(5) & 0xFF
if k == 27:
break
# destroy windows
cv2.destroyAllWindows()
parser.add_argument(
'--connect',
help=
"vehicle connection target string. If not specified, SITL automatically started and used."
)
parser.add_argument(
'--baudrate',
type=int,
help=
"Vehicle baudrate settings specify 57600 when connecting with 3dr telemetry radio.",
default=115200)
args = parser.parse_args()
connection_string = args.connect
pos1 = PositionVector(10, 0, 5)
pos2 = PositionVector(10, -10, 5)
dist_tol = 1.0 #used to determine if uav has reached allocated position
if not args.connect:
print "vehicle path not specified"
sys.exit(1) #abort cause of error..
# Connect to the Vehicle.
# Set `wait_ready=True` to ensure default attributes are populated before `connect()` return
print "\nConnecting to vehicle on: %s" % connection_string
vehicle = connect(connection_string, wait_ready=True, baud=args.baudrate)
v = Vehicle_params()
param = Params(vehicle, v)
#get home position ...
home_position = v.home_location
#Python Imports import math import time import os #Dronekit Imports from dronekit import VehicleMode, Attitude, connect, LocationGlobalRelative from dronekit_sitl import SITL #Common Library Imports from position_vector import PositionVector import flight_assist #List of global variables targetLocation = PositionVector() vehicleLocation = PositionVector() vehicleAttitude = 0 backgroundColor = (74,88,109) filename = (os.path.dirname(os.path.realpath(__file__)))+"/Resources/target.PNG" target_size = 1.5 camera_width = 640 camera_height = 480 camera_vfov = 60 camera_hfov = 60 camera_fov = math.sqrt(camera_vfov**2 + camera_hfov**2) camera_frameRate = 30 current_milli_time = lambda: int(round(time.time() * 1000)) def load_target(filename, actualS=1.5): global target, target_width, target_height
def get_location(self, timestamp):
pnt = self._interpolate(self.location_buffer, timestamp)
if pnt is not None:
pos = PositionVector(pnt.x, pnt.y, pnt.z)
return pos.get_location()
return None
