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))
