def recalculate_goal(self, goal_orginal_x, goal_orginal_y):
rospy.loginfo('Recalculating goal to include offset between gps->odom')
rospy.loginfo('({}, {})->({}, {})'.format(self.gps_fix[0],
self.gps_fix[1],
self.gps_filtered[0],
self.gps_filtered[1]))
goal_offset_x, goal_offset_y = gc.ll2xy(self.gps_filtered[0],
self.gps_filtered[1],
self.gps_fix[0],
self.gps_fix[1])
new_goal_x = goal_orginal_x - goal_offset_x
new_goal_y = goal_orginal_y - goal_offset_y
rospy.loginfo('Recalculated goal: ({}, {})'.format(
new_goal_x, new_goal_y))
return new_goal_x, new_goal_y
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 send_gps_goal(self, goal_lat, goal_lon, timeout=0):
self.mb_client.wait_for_server()
# Convert gps coordinates to odometry coordinates
rospy.loginfo('------------------------------------------')
rospy.loginfo('Making goal with (lat,lon): ({},{})'.format(
goal_lat, goal_lon))
goal_x, goal_y = gc.ll2xy(goal_lat, goal_lon, self.orig_lat,
self.orig_lon)
rospy.loginfo('(lat,lon): ({},{}) converts to \n(x,y): ({},{})'\
.format(goal_lat, goal_lon, goal_x, goal_y))
# With odom drift compensation
if self.recalc_goals:
while not rospy.is_shutdown():
# Check if we have gps readings
if self.gps_fix is None or self.gps_filtered is None:
time.sleep(.1)
continue
# Send goals while recalculating
rospy.loginfo('.................................')
new_goal = self.recalculate_goal(goal_x, goal_y)
move_base_goal = make_goal(*new_goal)
self.mb_client.send_goal(move_base_goal)
state = self.mb_client.wait_for_result(
rospy.Duration.from_sec(self.recalc_rate))
if state == actionlib.SimpleGoalState.DONE:
rospy.loginfo('Goal successfully reached')
break
# Normal gps waypoint to waypoint navigation
else:
move_base_goal = make_goal(goal_x, goal_y)
rospy.loginfo('Sending goal')
self.mb_client.send_goal(move_base_goal)
rospy.loginfo('Waiting for results')
state = self.mb_client.wait_for_result()
# if state == actionlib.SimpleGoalState.DONE:
# rospy.loginfo('Goal successfully reached')
# else:
# rospy.logwarn('Was not able to reach the goal in time')
return state
import geonav_transform.geonav_conversions as gc
reload(gc)
# Define a local orgin, latitude and longitude in decimal degrees
# This is the standard geonav origin for working at El Estero Lake
olat = 36.595
olon = -121.89
# This is somewhat confusing, but Gazebo also has a local coordinate system.
# In the elestero.launch file we define the gazebo origin relative to the
# heightmap used to represent the lake.
gaz_lat = 36.596524
gaz_lon = -121.888169
# So in geonav coordinates, the Gazebo origin is...
xg, yg = gc.ll2xy(gaz_lat,gaz_lon,olat,olon)
print('Geonav ll2xy, Lat: %.4f, Lon:%.4f >> X: %.1f, Y: %.1f'
%(gaz_lat,gaz_lon,xg,yg))
# This corresponds to where the vehicle is when we spawn it at 0,0 in Gazebo
# If we know the target location in the geonav coordinates
x = 137.6
y = 54.0
# Then the location in Gazebo coordinates is...
print('Target Gazebo coordinates, X: %.3f, Y: %.3f [m]'%(x-xg,y-yg))
# Similarly for the repulsive field location
x = 138.2
y = 75.9
# Then the location in Gazebo coordinates is...
print('Target Gazebo coordinates, X: %.3f, Y: %.3f [m]'%(x-xg,y-yg))
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)
plot(Lat,Xutm,'.')
def get_xy_from_lat_long(self):
xg2, yg2 = gc.ll2xy(self.current_wp.lat, self.current_wp.lon,
self.dest_wp.lat, self.dest_wp.lon)
return xg2, yg2
nx = 10
dx = 1.0
ny = 10
dy = 1.0
lonv, latv = np.meshgrid(np.linspace(0, dx, nx), np.linspace(0, dy, ny))
axv = np.zeros(lonv.shape)
ayv = np.zeros(latv.shape)
gxv = np.zeros(lonv.shape)
gyv = np.zeros(latv.shape)
for ii in range(nx):
for jj in range(ny):
lat = originlat + latv[ii, jj]
lon = originlon + lonv[ii, jj]
axv[ii, jj], ayv[ii, jj] = axy.ll2xy(lat, lon, originlat, originlon)
gxv[ii, jj], gyv[ii, jj] = gc.ll2xy(lat, lon, originlat, originlon)
figure(3)
clf()
plot(lonv.flatten(), latv.flatten(), '.')
grid(True)
figure(4)
clf()
plot(axv.flatten(), ayv.flatten(), 'r.', label='AlvinXY')
hold(True)
plot(gxv.flatten(), gyv.flatten(), 'g.', label='Geonav')
grid(True)
xlabel('Longitude [deg]')
ylabel('Latitude [m]')
title('Comparison of AlvinXY and Geonav for regular spaced Lat/Lon')
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)
xa, ya = axy.ll2xy(lat, lon, olat, olon)
print('Geonav ll2xy, Lat: %.4f, Lon:%.4f >> X: %.1f, Y: %.1f' %
(lat, lon, xg, yg))
print('AlvinXY ll2xy, Lat: %.4f, Lon:%.4f >> X: %.1f, Y: %.1f' %
(lat, lon, xa, ya))
# Back to lat/lon
glat, glon = gc.xy2ll(xg, yg, olat, olon)
alat, alon = axy.xy2ll(xg, yg, olat, olon)
print('Geonav xy2xy, X: %.1f, Y: %.1f >> Lat: %.4f, Lon:%.4f ' %
(xg, yg, glat, glon))
print('\t Delta, Lat: %.12f, Lon: %.12f [deg]' % (lat - glat, lon - glon))
print('AlvinXY xy2xy, X: %.1f, Y: %.1f >> Lat: %.4f, Lon:%.4f ' %
(xa, ya, alat, alon))
print('\t Delta, Lat: %.12f, Lon: %.12f [deg]' % (lat - alat, lon - alon))
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)
return client.get_result()
if __name__ == "__main__":
rospy.init_node('autonomous_twist')
points = []
# Read the files
if os.path.isfile(abs_path):
with open(abs_path, 'r') as f:
points = f.readlines()
print("Available number of manual points : ", len(points))
#Convert point to UTM
for point in points:
if len(point) < 10:
continue
coo = point.split(' ')
lat = float(coo[0].strip())
lon = float(coo[1].strip())
utmy, utmx, utmzone = gc.LLtoUTM(lat, lon)
#Convert UTM to odom frame
olat = 49.9
olon = 8.90
xg2, yg2 = gc.ll2xy(lat, lon, olat, olon)
#send as goal
result = movebase_client(xg2, yg2)
print("Result ", result)
