def test_is_equals_lat(self): loc = Location() self.assertTrue(loc.is_equals_lat(0)) self.assertFalse(loc.is_equals_lat(1)) self.assertFalse(loc.is_equals_lat(0.05)) self.assertFalse(loc.is_equals_lat(-0.05)) self.assertFalse(loc.is_equals_lat(-0.06)) self.assertTrue(loc.is_equals_lat(-0.04)) self.assertTrue(loc.is_equals_lat(-0.03)) self.assertTrue(loc.is_equals_lat(-0.02)) self.assertTrue(loc.is_equals_lat(-0.01)) self.assertTrue(loc.is_equals_lat(+0.04)) self.assertTrue(loc.is_equals_lat(+0.03)) self.assertTrue(loc.is_equals_lat(+0.02)) self.assertTrue(loc.is_equals_lat(+0.01)) self.assertFalse(loc.is_right_alt(CHARACTER)) self.assertFalse(loc.is_right_alt(CHAR_NUMBER)) loc.set_latitude(LIMIT_NUMBER_LATITUDE) self.assertTrue(loc.is_equals_lat(LIMIT_NUMBER_LATITUDE)) self.assertFalse(loc.is_equals_lat(LIMIT_NUMBER_LATITUDE+1)) self.assertFalse(loc.is_equals_lat(LIMIT_NUMBER_LATITUDE+0.05)) self.assertTrue(loc.is_equals_lat(LIMIT_NUMBER_LATITUDE-0.05)) self.assertFalse(loc.is_equals_lat(LIMIT_NUMBER_LATITUDE-0.06)) self.assertTrue(loc.is_equals_lat(LIMIT_NUMBER_LATITUDE-0.04)) self.assertTrue(loc.is_equals_lat(LIMIT_NUMBER_LATITUDE-0.03)) self.assertTrue(loc.is_equals_lat(LIMIT_NUMBER_LATITUDE-0.02)) self.assertTrue(loc.is_equals_lat(LIMIT_NUMBER_LATITUDE-0.01)) self.assertFalse(loc.is_equals_lat(LIMIT_NUMBER_LATITUDE+0.04)) self.assertFalse(loc.is_equals_lat(LIMIT_NUMBER_LATITUDE+0.03)) self.assertFalse(loc.is_equals_lat(LIMIT_NUMBER_LATITUDE+0.02)) self.assertFalse(loc.is_equals_lat(LIMIT_NUMBER_LATITUDE+0.01)) loc.set_latitude(-LIMIT_NUMBER_LATITUDE) self.assertTrue(loc.is_equals_lat(-LIMIT_NUMBER_LATITUDE)) self.assertFalse(loc.is_equals_lat(-LIMIT_NUMBER_LATITUDE+1)) self.assertFalse(loc.is_equals_lat(-LIMIT_NUMBER_LATITUDE-0.05)) self.assertTrue(loc.is_equals_lat(-LIMIT_NUMBER_LATITUDE+0.05)) self.assertFalse(loc.is_equals_lat(-LIMIT_NUMBER_LATITUDE+0.06)) self.assertTrue(loc.is_equals_lat(-LIMIT_NUMBER_LATITUDE+0.04)) self.assertTrue(loc.is_equals_lat(-LIMIT_NUMBER_LATITUDE+0.03)) self.assertTrue(loc.is_equals_lat(-LIMIT_NUMBER_LATITUDE+0.02)) self.assertTrue(loc.is_equals_lat(-LIMIT_NUMBER_LATITUDE+0.01)) self.assertFalse(loc.is_equals_lat(-LIMIT_NUMBER_LATITUDE-0.04)) self.assertFalse(loc.is_equals_lat(-LIMIT_NUMBER_LATITUDE-0.03)) self.assertFalse(loc.is_equals_lat(-LIMIT_NUMBER_LATITUDE-0.02)) self.assertFalse(loc.is_equals_lat(-LIMIT_NUMBER_LATITUDE-0.01))
def right(self,move_in_meter): loc = Location() loc1 = Location() head = self.vehicle.heading loc.setFromVehicleLocation(self.vehicle.location.global_frame) loc1.setFromVehicleLocation(self.vehicle.location.global_frame) loc.set_longitude(round(loc.get_longitude()+move_in_meter/100000.0*math.cos(self.vehicle.heading+90),7)) ############maybe 90 or 45 because of the angle loc.set_latitude(round(loc.get_latitude()+move_in_meter/100000.0*math.sin(self.vehicle.heading+90),7)) ############maybe 90 or 45 because of the angle location_global = LocationGlobal(loc.latitude,loc.longitude,loc.altitude) self.vehicle.simple_goto(location_global) while True: time.sleep(DELAY) if abs(self.vehicle.location.global_relative_frame.lat-location_global.lat) < 0.0000001 and abs(self.vehicle.location.global_relative_frame.lon-location_global.lon) < 0.000001: break; self.condition_yaw(head) if move_in_meter > 0: print "complete function right" else: print "complete function left"
def forward(self,move_forward_in_meter): dest_location = Location() loc1 = Location() head = self.vehicle.heading loc1.setFromVehicleLocation(self.vehicle.location.global_relative_frame) dest_location.setFromVehicleLocation(self.vehicle.location.global_relative_frame) add_lat = math.sin(self.vehicle.heading)*(move_forward_in_meter/100000.0) add_lon = math.cos(self.vehicle.heading)*(move_forward_in_meter/100000.0) dest_location.set_latitude(round(dest_location.get_latitude()+ add_lat,7)) dest_location.set_longitude(round(dest_location.get_longitude()+add_lon,7)) location_global = LocationGlobalRelative(dest_location.get_latitude(),dest_location.get_longitude(),dest_location.get_altitude()) self.vehicle.simple_goto(location_global,groundspeed=100) while True: time.sleep(DELAY) if abs(self.vehicle.location.global_relative_frame.lat-location_global.lat) < 0.0000001 and abs(self.vehicle.location.global_relative_frame.lon-location_global.lon) < 0.000001 : break; self.condition_yaw(head) if move_forward_in_meter > 0: print "complete function forward\n" else : print "complete function backwards\n"
def test_set_latitude(self): #create object location loc = Location() #set the default location loc.set_latitude(0) self.assertTrue(loc.get_latitude() == 0) #set latitude limit positive number loc.set_latitude(LIMIT_NUMBER_LATITUDE) self.assertTrue(loc.get_latitude() == LIMIT_NUMBER_LATITUDE) #check is success the set on the latitude the underscore limit loc.set_latitude(-LIMIT_NUMBER_LATITUDE) self.assertTrue(loc.get_latitude() == -LIMIT_NUMBER_LATITUDE) #check if the number that not in the limits set the closest number limit. loc.set_latitude(LIMIT_NUMBER_LATITUDE+1) self.assertEqual(loc.get_latitude(),LIMIT_NUMBER_LATITUDE) loc.set_latitude(-LIMIT_NUMBER_LATITUDE-1) self.assertEqual(loc.get_latitude(),-LIMIT_NUMBER_LATITUDE) #check if the latitude set a character when it's character - set zero loc.set_latitude(CHARACTER) self.assertEqual(loc.get_latitude(),0) #when it's number - set the number as float loc.set_latitude(CHAR_NUMBER) self.assertEqual(loc.get_latitude(),float(CHAR_NUMBER))
def test_equalsTo(self): loc = Location() loc1 = Location() #---------------latitude--------------------# self.assertTrue(loc.equalsTo(loc1)) loc1.set_latitude(LIMIT_NUMBER_LATITUDE) self.assertFalse(loc.equalsTo(loc1)) loc.set_latitude(LIMIT_NUMBER_LATITUDE) self.assertTrue(loc.equalsTo(loc1)) loc.set_latitude(LIMIT_NUMBER_LATITUDE+1) self.assertTrue(loc.equalsTo(loc1)) loc1.set_latitude(LIMIT_NUMBER_LATITUDE+1) self.assertTrue(loc.equalsTo(loc1)) loc.set_latitude(-LIMIT_NUMBER_LATITUDE) self.assertFalse(loc.equalsTo(loc1)) loc1.set_latitude(-LIMIT_NUMBER_LATITUDE) self.assertTrue(loc.equalsTo(loc1)) loc.set_latitude(-LIMIT_NUMBER_LATITUDE-1) self.assertTrue(loc.equalsTo(loc1)) loc1.set_latitude(-LIMIT_NUMBER_LATITUDE-1) self.assertTrue(loc.equalsTo(loc1)) loc.set_latitude(CHARACTER) self.assertFalse(loc.equalsTo(loc1)) loc1.set_latitude(CHARACTER) self.assertTrue(loc.equalsTo(loc1)) loc.set_latitude(CHAR_NUMBER) self.assertFalse(loc.equalsTo(loc1)) loc1.set_latitude(CHAR_NUMBER) self.assertTrue(loc.equalsTo(loc1)) #print "%f %f " % ( loc.longitude, loc.latitude) #print "%f %f " % ( loc1.longitude, loc1.latitude) #---------------longitude-------------------------# loc1.set_longitude(LIMIT_NUMBER_LONGITUDE) self.assertFalse(loc.equalsTo(loc1)) loc.set_longitude(LIMIT_NUMBER_LONGITUDE) self.assertTrue(loc.equalsTo(loc1)) loc.set_longitude(LIMIT_NUMBER_LONGITUDE+1) #print "%f %f " % ( loc.longitude, loc.latitude) #print "%f %f " % ( loc1.longitude, loc1.latitude) self.assertTrue(loc.equalsTo(loc1)) loc1.set_longitude(LIMIT_NUMBER_LONGITUDE+1) self.assertTrue(loc.equalsTo(loc1)) loc.set_longitude(-LIMIT_NUMBER_LONGITUDE) self.assertFalse(loc.equalsTo(loc1)) loc1.set_longitude(-LIMIT_NUMBER_LONGITUDE) self.assertTrue(loc.equalsTo(loc1)) loc.set_longitude(-LIMIT_NUMBER_LONGITUDE-1) self.assertTrue(loc.equalsTo(loc1)) loc1.set_longitude(-LIMIT_NUMBER_LONGITUDE-1) self.assertTrue(loc.equalsTo(loc1)) loc.set_longitude(CHARACTER) self.assertTrue(loc.equalsTo(loc1)) loc1.set_longitude(CHARACTER) self.assertTrue(loc.equalsTo(loc1)) loc.set_longitude(CHAR_NUMBER) self.assertFalse(loc.equalsTo(loc1)) loc1.set_longitude(CHAR_NUMBER) self.assertTrue(loc.equalsTo(loc1)) #-------------altitude--------------------# loc.set_altitude(CHARACTER) self.assertTrue(loc.equalsTo(loc1)) loc.set_altitude(CHAR_NUMBER) self.assertFalse(loc.equalsTo(loc1)) loc1.set_altitude(CHAR_NUMBER) self.assertTrue(loc.equalsTo(loc1))