def __init__(self,olat,olong):
self.outmsg = Odometry()
self.outpub = rospy.Publisher("nav_odom",Odometry,queue_size=1)
self.sub = rospy.Subscriber("p3d_odom",Odometry,self.p3dcallback)
rospy.loginfo("Subscribing to %s"%self.sub.name)
self.rxodom = False
self.originLat = olat
self.originLong = olong
utmy, utmx, utmzone = nc.LLtoUTM(self.originLat,self.originLong)
self.originX = utmx
self.originY = utmy
self.originZone = utmzone
rospy.loginfo("Origin at Lat/Lon %.6f/%.6f which is UTM X: %.4f Y: %.4f Zone: %s"%(self.originLat, self.originLong, self.originX, self.originY, self.originZone))
self.seq = 0
def get_xy_based_on_lat_long(lat, lon, name):
# Define a local orgin, latitude and longitude in decimal degrees
# GPS Origin
olat = 49.9
olon = 8.9
xg2, yg2 = gc.ll2xy(lat, lon, olat, olon)
utmy, utmx, utmzone = gc.LLtoUTM(lat, lon)
xa, ya = axy.ll2xy(lat, lon, olat, olon)
rospy.loginfo("######### " + name + " ###########")
rospy.loginfo("LAT COORDINATES ==>" + str(lat) + "," + str(lon))
rospy.loginfo("COORDINATES XYZ ==>" + str(xg2) + "," + str(yg2))
rospy.loginfo("COORDINATES AXY==>" + str(xa) + "," + str(ya))
rospy.loginfo("COORDINATES UTM==>" + str(utmx) + "," + str(utmy))
return xg2, yg2
def callback(data):
# Set Map Origin
# 30.174885, -96.512199
olat = 30.174885
olon = -96.512199
# Odom to lat/lon
xg2, yg2 = gc.xy2ll(data.pose.pose.position.x, data.pose.pose.position.y,
olat, olon)
# build navsat message
fake_gps = NavSatFix()
fake_gps.header.frame_id = rospy.get_param('~frame_id', 'gps')
fake_gps.header.stamp = rospy.Time.now()
fake_gps.status.status = 1
fake_gps.status.service = 1
fake_gps.latitude = xg2
fake_gps.longitude = yg2
fake_gps.altitude = 0
#compass = Float(90.0)
pub = rospy.Publisher('fake_gps', NavSatFix, queue_size=10)
pub.publish(fake_gps)
# Odom to UTM
utmy, utmx, utmzone = gc.LLtoUTM(xg2, yg2)
# build navsat message
fake_UTM = NavSatFix()
fake_UTM.header.frame_id = rospy.get_param('~frame_id', 'utm')
fake_UTM.header.stamp = rospy.Time.now()
fake_UTM.status.status = 1
fake_UTM.status.service = 1
fake_UTM.latitude = utmy
fake_UTM.longitude = utmx
fake_UTM.altitude = 0
#compass = Float(90.0)
pub2 = rospy.Publisher('fake_utm', NavSatFix, queue_size=10)
pub2.publish(fake_UTM)
Lon = linspace(-126.0,-120.0,100)
Lat = linspace(32.0,40.0,100)
# Define local origin at lower right of UTM zone
olon = -126.0
olat = 32.0
Xutm = []
Yutm = []
Xxy = []
Yxy = []
Xa = []
Ya = []
for lat in Lat:
lon = -126.0
utmy, utmx, utmzone = gc.LLtoUTM(lat,lon)
x, y = gc.ll2xy(lat,lon,olat,olon)
xa,ya = axy.ll2xy(lat,lon,olat,olon)
print utmzone
Xutm.append(utmx)
Yutm.append(utmy)
Xxy.append(x)
Yxy.append(y)
Xa.append(xa)
Ya.append(ya)
Xutm = array(Xutm)
Yutm = array(Yutm)
figure(1)
clf()
subplot(211)
reload(gc)
# Import AlvinXY transformation module
import alvinxy.alvinxy as axy
reload(axy)
# Define a local orgin, latitude and longitude in decimal degrees
olat = 37.
olon = -122.
# Pick a point not too far from the origin
lat = olat + 0.01
lon = olon + 0.01
# Convert to UTM
outmy, outmx, outmzone = gc.LLtoUTM(olat, olon)
utmy, utmx, utmzone = gc.LLtoUTM(lat, lon)
print('UTM Conversion Using geonav_tranform')
print(
'Convert to UTM, Lat: %.4f, Lon:%.4f >> UTMY: %.1f, UTMX: %.1f, Zone:%s' %
(lat, lon, utmy, utmx, utmzone))
latt, lonn = gc.UTMtoLL(utmy, utmx, utmzone)
print('Convert back to Lat/Lon, Lat: %.4f, Lon: %.4f' % (latt, lonn))
print('Delta, Lat: %.12f, Lon: %.12f [deg]' % (lat - latt, lon - lonn))
print(' ')
# Convert to X/Y and back
# Convert a lat/lon to a local x/y
print('Convert from lat/lon to x/y')
xg, yg = gc.ll2xy(lat, lon, olat, olon)
def handle(request):
xg, yg = gc.ll2xy(request.lat, request.lon, request.olat, request.olon)
utmy, utmx, utmzone = gc.LLtoUTM(request.lat, request.lon)
xa, ya = axy.ll2xy(request.lat, request.lon, request.olat, request.olon)
return gps_to_othersResponse(xg, yg, xa, ya, utmx, utmy)
