WaypointConvert.py
Go to the documentation of this file.
1 ################################################################################
2 #
3 # Copyright (c) 2017 Taylor Pool, BYU MAGICC Lab.
4 # All rights reserved.
5 #
6 # Redistribution and use in source and binary forms, with or without
7 # modification, are permitted provided that the following conditions are met:
8 #
9 # * Redistributions of source code must retain the above copyright notice, this
10 # list of conditions and the following disclaimer.
11 #
12 # * Redistributions in binary form must reproduce the above copyright notice,
13 # this list of conditions and the following disclaimer in the documentation
14 # and/or other materials provided with the distribution.
15 #
16 # * Neither the name of the copyright holder nor the names of its
17 # contributors may be used to endorse or promote products derived from
18 # this software without specific prior written permission.
19 #
20 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
21 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
22 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
23 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
24 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
25 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
26 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
27 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
28 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
29 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30 # POSSIBILITY OF SUCH DAMAGE.
31 #
32 ################################################################################
33 
34 # Taylor Pool
35 # December 1, 2016
36 #AUVSI Project
37 
38 # This script contains a function that converts given GPS coordinates into waypoints for ROS.
39 #The input supports two types of GPS coordinates
40 #1) Long Degree Decimal Format
41 # ex: -34.6830975
42 #2) Degrees-Minutes-Seconds Format
43 # ex: N78-45-76.23
44 #The function to be called for conversion is to_meters(originLat, originLong, originAlt, newLat, newLong, newAlt, flag)
45 #The two formats listed above can be used simultaeously in the same function call.
46 #NOTE: Degrees-Minutes-Seconds Format is the only format that uses NSEW directions in front.
47 #NOTE: Degrees-Minutes-Seconds Format needs to be surrounded with quotations when passed as an argument.
48 
49 from math import pi
50 from math import cos
51 from math import sqrt
52 
53 #constant
54 EARTH_RADIUS = 6371008.0 #in meters
55 
56 #Function decimal_degrees
57 #Pre: string is in format [N/E or S/W]DD-MM-SS.SS
58 #Post: Returns GPS component in long decimal format
59 def decimal_degrees (string):
60  a = 0
61 
62  firstLetter = string[0]
63  if firstLetter == 'N' or firstLetter == 'E':
64  a = 1
65  elif firstLetter == 'S' or firstLetter == 'W':
66  a = -1
67 
68  lessString = string.strip("NSEW ")
69 
70  values = lessString.split('-', 2)
71 
72  d = float(values[0])
73  m = float(values[1])
74  s = float(values[2])
75 
76  decimal = a*(d+(m/60.0)+(s/3600.0))
77 
78  return decimal
79 
80 
81 #Function meter_convert
82 #Takes in long decimal GPS format and outputs the destination in meters
83 #Pre: Lat and Long are given in decimal degrees, and altitude is in meters
84 #Post: Returns destination in tuple format [latChange, longChange, newAlt, distance]
85 def meter_convert(originLat, originLong, originAlt, newLat, newLong, newAlt):
86 
87  #Find CrossSectional Radius of Earth for Given Latitude Degree
88  crossRadius = cos(originLat*pi/180.0)*EARTH_RADIUS
89 
90  GPSdestination = [newLat, newLong, -newAlt] #Given in terms of N, E, D
91 
92  #Convert Change in GPS Coordinates to Change in Meters
93  latChange = ((newLat - originLat)*pi/180.0)*EARTH_RADIUS #in meters
94 
95  longChange = ((newLong - originLong)*pi/180.0)*crossRadius
96 
97  altitudeChange = newAlt - originAlt
98 
99  #Compute Total Distance to Fly
100  distance = sqrt(pow(latChange, 2.0) + pow(longChange, 2.0) + pow(altitudeChange, 2.0))
101 
102  #New Waypoint Given in Terms of [North, East, Down]. In meters.
103  destination = [latChange, longChange, newAlt, distance]
104 
105  return destination
106 
107 
108 #Function to_meters
109 #Pre: input of all the data needed to fly to a waypoint
110 #flag: tells whether altitude is in meters or feet
111 # 0: meters else: feet
112 #Post: outputs destination given in meters that ROS can understand.
113 def to_meters(originLat, originLong, originAlt, newLat, newLong, newAlt, flag):
114 
115  #Altitude in Feet needs to be converted to Meters
116  if (flag != 0):
117  originAlt = .305*originAlt
118  newAlt = .305*newAlt
119 
120  GPSorigin = [originLat, originLong, originAlt]
121  GPSdestination = [newLat, newLong, newAlt]
122 
123  values = [str(originLat), str(originLong), str(newLat), str(newLong)]
124  newValues = []
125 
126  for value in values:
127  if ("N" in value) or ("S" in value) or ("E" in value) or ("W" in value) == 1:
128  #print "Degrees Minutes Seconds Format"
129  #print value
130  newValues.append(decimal_degrees(value))
131  #print decimal_degrees(value)
132  else:
133  #print "Long Decimal Format"
134  newValues.append(float(value))
135 
136  destination = meter_convert(newValues[0], newValues[1], originAlt, newValues[2], newValues[3], newAlt)
137 
138  #Test Output
139  #print("Origin in GPS Coordinates: " + str(GPSorigin))
140  #print("\n")
141  #print("Destination in GPS Coordinates: " + str(GPSdestination))
142  #print("\n")
143  #print("Destination Coordinates (Meters) with Distance: " + str(destination))
144 
145  return destination
146 
147 
148 
149 #######################################################################################################
150 
151 
152 #Test
153 #test = to_meters("N90-90-76.45", "W45-67-23.54", 20.0, -40.257049176511316, 111.65421836078167, 20.0, 0)
154 
155 #if __name__ == __main__:
156 # toMeters(40.25787274333326, -111.65480308234692, -20.0, 40.257049176511316, -111.65421836078167, -20.0, 0)
157 
158 
159 #######################################################################################################################
160 
161 #Sample Output:
162 #tpool2@FB280-09:/media/tpool2/TAYLORPOOL/AUVSI/rosflight_utils/src$ python WaypointConvert.py
163 #Degrees Minutes Seconds Format
164 #N90-90-76.45
165 #91.5212361111
166 #Degrees Minutes Seconds Format
167 #W45-67-23.54
168 #-46.1232055556
169 #Long Decimal Format
170 #Long Decimal Format
171 #Origin in GPS Coordinates: ['N90-90-76.45', 'W45-67-23.54', 20.0]
172 
173 
174 #Destination in GPS Coordinates: [-40.257049176511316, 111.65421836078167, 20.0]
175 
176 
177 #Destination Coordinates (Meters) with Distance: [-14653095.165621338, -465750.5181201244, 20.0, 14660495.267141715]
def meter_convert(originLat, originLong, originAlt, newLat, newLong, newAlt)
def decimal_degrees(string)
def to_meters(originLat, originLong, originAlt, newLat, newLong, newAlt, flag)


rosflight_utils
Author(s):
autogenerated on Wed Jul 3 2019 20:00:31