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