def plot_rwy(rwy_coords, color):
for r in rwy_coords:
a, b = r[0], r[1]
start_x, start_y, _ = pm.geodetic2enu(a[0], a[1], airport_altitude,
airport_lat, airport_lon,
airport_altitude)
end_x, end_y, _ = pm.geodetic2enu(b[0], b[1], airport_altitude,
airport_lat, airport_lon,
airport_altitude)
pl.plot([start_x * M_TO_NM, end_x * M_TO_NM],
[start_y * M_TO_NM, end_y * M_TO_NM],
'-',
c=color,
lw=2)
def plot_rwy(rwy_coords, color):
for r in rwy_coords:
a, b = r[0], r[1]
start_x, start_y, _ = pm.geodetic2enu(a[0], a[1], airport_altitude,
airport_lat, airport_lon,
airport_altitude)
end_x, end_y, _ = pm.geodetic2enu(b[0], b[1], airport_altitude,
airport_lat, airport_lon,
airport_altitude)
plt.plot([start_x * M_TO_NM, end_x * M_TO_NM],
[start_y * M_TO_NM, end_y * M_TO_NM],
'--',
c=color,
lw=1)
plt.scatter(0, 0, s=4, c=color, marker='*')
def callback_gnss(msg_gnss):
global RUN, RUN_imu, RUN_gnss
global arr_pos, arr_cov
global dydx_last
gnss_pos = np.array(
pm.geodetic2enu(msg_gnss.latitude, msg_gnss.longitude,
msg_gnss.altitude, lat0, lon0, h0))
if not RUN:
# Initial State
ekf.p[0] = gnss_pos[0]
ekf.p[1] = gnss_pos[1]
arr_pos[-1] = np.copy(arr_pos[0])
arr_pos[0] = np.array([gnss_pos[0], gnss_pos[1]])
arr_cov[-1] = np.copy(arr_cov[0])
arr_cov[0] = msg_gnss.position_covariance[0]
else:
ekf.correct_gnss_2d(gnss_pos[:-1], J_gnss)
if np.linalg.norm(ekf.v) >= gnss_compass_vel_min: # m/s
n = 0
diff = np.array([gnss_pos[0], gnss_pos[1]]) - arr_pos[n]
gnss_compass = np.arctan2(diff[-1], diff[0])
if np.abs(gnss_compass -
dydx_last) <= 25. / 180. * np.pi: # Cancel the noise
ekf.correct_compass(gnss_compass, J_gnss_compass)
dydx_last = np.copy(gnss_compass) + 0.0
elif np.linalg.norm(ekf.v) <= stop_vel_max: # m/s
ekf.correct_compass(ekf.yaw, J_yaw_stop)
arr_pos[-1] = np.copy(arr_pos[0])
arr_pos[0] = np.array([gnss_pos[0], gnss_pos[1]])
arr_cov[-1] = np.copy(arr_cov[0])
arr_cov[0] = msg_gnss.position_covariance[0]
RUN_gnss = True
def NavSat2PoseGPS(self, GPSfix):
gps_e, gps_n, gps_u = pm.geodetic2enu(GPSfix.latitude,
GPSfix.longitude, 0,
self.lat_mean, self.long_mean, 0)
delta_z_th = math.atan2((-gps_e - self.z_y), (gps_n - self.z_x))
#print(str(delta_z_th))
self.z_th = delta_z_th
self.z_x = gps_n
self.z_y = -gps_e
self.z_cov = GPSfix.position_covariance[
0] # Original value of covariance from Automower
self.z_cov = self.z_cov / 4.5 # Scale value of HDOP (Averaging among n&e covariances and removing 1.5*1.5 scale)
self.z_cov = self.z_cov / 10 # Trying to lower the cov
self.R = np.diag([self.z_cov, self.z_cov, self.z_th_cov
]) # Observation x,y,th position covariance
#self.R = np.diag([self.z_cov, self.z_cov]) # Observation x,y position covariance
# Save that a new measure has been made
self.new_measure = True
# since all odometry is 6DOF we'll need a quaternion created from yaw
odom_quat = tf.transformations.quaternion_from_euler(0, 0, self.z_th)
# next, we'll publish the pose message over ROS
pose = Pose(Point(self.z_x, self.z_y, 0.), Quaternion(*odom_quat))
# publish the message
self.pose_gps_pub.publish(pose)
def getENUTrackDict(start,stop,hour_window = 12,flights_of_interest=[]):
'''
This will return a dict with the trajectories of each plane observed in the
period of time specified (given in UTC timestamps).
'''
unique_flights,all_vals = getTracks(start,stop,hour_window=hour_window)
if numpy.size(flights_of_interest) != 0:
unique_flights = [u.replace(' ','') for u in unique_flights]
all_vals[:,0] = [u.replace(' ','') for u in all_vals[:,0]]
unique_flights = unique_flights[numpy.isin(unique_flights,flights_of_interest)]
all_vals = all_vals[numpy.isin(all_vals[:,0],flights_of_interest),:]
if numpy.size(all_vals) != 0:
lat = all_vals[:,2].astype(float)
lon = all_vals[:,3].astype(float)
alt = all_vals[:,4].astype(float)*0.3048 #Expressing alt in meters now.
timestamps = all_vals[:,1].astype(float)
flight_tracks_ENU = {}
origin = info.loadAntennaZeroLocation(deploy_index = 1) #Antenna 0
for unique_flight in unique_flights:
flight_cut = all_vals[:,0] == unique_flight
enu = pm.geodetic2enu(lat[flight_cut],lon[flight_cut],alt[flight_cut],origin[0],origin[1],origin[2]) #converts to ENU
ts = timestamps[flight_cut]
# x, y, z, t
flight_tracks_ENU[unique_flight.replace(' ','')] = numpy.vstack((numpy.asarray(enu),ts[None,:])).T
return flight_tracks_ENU, all_vals
else:
return [], []
def callback(self, data):
# Decompose the message into known data types.
lat = data.latitude
lon = data.longitude
alt = data.altitude
# If this is the first reading, save it to normnalize
# any subsequent readings.
if not self.init:
self.init_lat = lat
self.init_lon = lon
self.init_alt = alt
self.init = True
x, z, y = pm.geodetic2enu(lat, lon, alt,
self.init_lat, self.init_lon, self.init_alt)
# In NumPy, indexing starts from the top-left. We therefore invert E
# to get a WNU reading, which more closely approximates NumPy indexing.
x = -x
# Normalize the measurements according to the UAV's diameter,
# to have it such that every one unit of the xyz data represents
# a cube of dimension UAV_diameter**3
self.position = np.array([x, y, z]) / self.UAV_diameter
self.x = self.position[0]
self.y = self.position[1]
self.z = self.position[2]
def geodetic2enu(lat, lon, alt):
# x, y, z = pm.geodetic2ecef(lat, lon, alt)
# x, y ,z = ecef2enu(x, y, z, 45.90414414, 11.02845385, 227.5819)
x, y, z = pm.geodetic2enu(lat, lon, alt, 45.90414414, 11.02845385,
227.5819)
return x, y, z
def NavSat2PoseENU(self, GPSfix):
self.current_time = GPSfix.header.stamp
# print("Status: " + str(GPSfix.status.status) + " - Service: " + str(GPSfix.status.service) ) 5 - 1
gps_e , gps_n, gps_u = pm.geodetic2enu(GPSfix.latitude,GPSfix.longitude,0,self.lat_mean,self.long_mean,0)
self.z_x = - gps_e
self.z_y = gps_n
# since all odometry is 6DOF we'll need a quaternion created from yaw
odom_quat = tf.transformations.quaternion_from_euler(0, 0, self.th)
# first, we'll publish the transform over tf
self.pose_broadcaster.sendTransform(
(self.z_x, self.z_x, 0.),
odom_quat,
self.current_time,
"base_link",
"odom"
)
# next, we'll publish the pose message over ROS
pose = Pose(Point(self.z_x, self.z_y, 0.), Quaternion(*odom_quat))
# publish the message
self.pose_enu_Check_pub.publish(pose)
last_time = self.current_time
def updateENUFromLatlonel(data, verbose=True):
'''
Given data, this will take the latlonel data and use it to update the ENU data in a deep copy of the original dict.
Parameters
----------
data : dict
The dict generated when loading a configuration from a json file.
'''
data = copy.deepcopy(data) #So as to not edit the original.
origin = data['origin'][
'latlonel'] #Lat Lon Elevation, use for generating ENU from other latlonel values.
for mast in range(4):
for key in ['physical', 'hpol', 'vpol']:
if numpy.asarray(data['antennas']['ant%i' %
mast][key]['latlonel']).size > 0:
data['antennas']['ant%i' % mast][key]['enu'] = numpy.asarray(
pm.geodetic2enu(
data['antennas']['ant%i' % mast][key]['latlonel'][0],
data['antennas']['ant%i' % mast][key]['latlonel'][1],
data['antennas']['ant%i' % mast][key]['latlonel'][2],
origin[0], origin[1], origin[2]))
else:
if verbose:
print(key + ' does not contain ENU data for mast %i %s.' %
(mast, key))
return data
def set_nav(self, datagram, counter=0, add_enu=True):
"""
Read positioning information from a datagram of type 'P' and updates the navigation variable in the class.
"""
if (self._positioning_system is None):
self._positioning_system = datagram.Descriptor
if self._origin is None:
self._origin = np.array([datagram.Latitude, datagram.Longitude])
if self._previous_latitude is None:
self._previous_latitude = datagram.Latitude
self._previous_longitude = datagram.Longitude
r, _, _ = utils.geodetic.calculateRangeBearingFromGeographicals(
self._previous_longitude, self._previous_latitude,
datagram.Longitude, datagram.Latitude)
self._distance_travelled += r
nav = [
counter,
self._reader.currentRecordDateTime(), datagram.Heading,
datagram.Latitude, datagram.Longitude
]
if add_enu:
east, north, _ = pm3.geodetic2enu(datagram.Latitude,
datagram.Longitude, 0,
self._origin[0], self._origin[1],
0)
self._navigation.append(np.array(nav + [east, north]))
self._previous_latitude = datagram.Latitude
self._previous_longitude = datagram.Longitude
def GPS_call(self,data):
self.t_GPS_call = t.time()
lon = float(data.longitude) #x
lat = float(data.latitude) #y
alt = float(data.altitude) #z
self.status = data.status.status
self.e_gps,self.n_gps,self.u_gps = pm.geodetic2enu(lat,lon,alt,base_lat,base_lon,base_alt)
if self.dis_gps_flag == 0:
self.a = self.add_displacement
self.b = self.add_displacement
self.dis_gps_flag = 1
else:
self.a = self.b
self.b = self.add_displacement
self.dis_gps_enc = self.b-self.a
self.gps_check(self.dis_gps_enc)
self.e_gps_pre = self.e_gps
self.n_gps_pre = self.n_gps
def getENUTrackDict(start,
stop,
min_approach_cut_km,
hour_window=12,
flights_of_interest=[],
origin=None,
deploy_index=None):
'''
This will return a dict with the trajectories of each plane observed in the
period of time specified (given in UTC timestamps).
origin should be a tuple in the same format as returned by loadAntennaZeroLocation (latitude,longtidue,elevation)
'''
try:
if deploy_index is None:
deploy_index = info.returnDefaultDeploy()
unique_flights, all_vals = getTracks(start,
stop,
min_approach_cut_km,
hour_window=hour_window)
if numpy.size(flights_of_interest) != 0:
unique_flights = unique_flights[numpy.isin(unique_flights,
flights_of_interest)]
all_vals = all_vals[numpy.isin(all_vals['names'],
flights_of_interest)]
if numpy.size(all_vals) != 0:
flight_tracks_ENU = {}
if origin is None:
origin = info.loadAntennaZeroLocation(
deploy_index=deploy_index) #Antenna 0
for unique_flight in unique_flights:
flight_cut = numpy.where(all_vals['names'] == unique_flight)[0]
flight_cut = flight_cut[numpy.unique(
all_vals['timestamps'][flight_cut], return_index=True
)[1]] #Removing repeated timestamps per flight
ts = all_vals['timestamps'][flight_cut]
#cut = numpy.logical_and(cut,numpy.isin(numpy.arange(len(cut)),numpy.unique(file['timestamps'][...],return_index=True)[1])) #bool cut of first occurance of each timestamp to remove repeated.
enu = pm.geodetic2enu(all_vals['lat'][flight_cut],
all_vals['lon'][flight_cut],
all_vals['alt'][flight_cut], origin[0],
origin[1], origin[2]) #converts to ENU
sorted_track_indices = numpy.argsort(ts)
# x, y, z, t
flight_tracks_ENU[unique_flight] = numpy.vstack(
(numpy.asarray(enu), ts[None, :])).T[sorted_track_indices]
return flight_tracks_ENU, all_vals
else:
return {}, []
except Exception as e:
print('Error in getENUTrackDict.')
print(e)
exc_type, exc_obj, exc_tb = sys.exc_info()
fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1]
print(exc_type, fname, exc_tb.tb_lineno)
def test_ned():
xyz = pm.aer2ecef(*aer0, *lla0)
enu = pm.aer2enu(*aer0)
ned = (enu[1], enu[0], -enu[2])
lla = pm.aer2geodetic(*aer0, *lla0)
assert pm.aer2ned(*aer0) == approx(ned0)
with pytest.raises(ValueError):
pm.aer2ned(aer0[0], aer0[1], -1)
assert pm.enu2aer(*enu) == approx(aer0)
assert pm.enu2aer(*enu, deg=False) == approx(raer0)
assert pm.ned2aer(*ned) == approx(aer0)
assert pm.ecef2ned(*xyz, *lla0) == approx(ned)
assert pm.ned2ecef(*ned, *lla0) == approx(xyz)
# %%
assert pm.ecef2enuv(vx, vy, vz, *lla0[:2]) == approx((ve, vn, vu))
assert pm.ecef2nedv(vx, vy, vz, *lla0[:2]) == approx((vn, ve, -vu))
# %%
enu3 = pm.geodetic2enu(*lla, *lla0)
ned3 = (enu3[1], enu3[0], -enu3[2])
assert pm.geodetic2ned(*lla, *lla0) == approx(ned3)
assert pm.enu2geodetic(*enu3, *lla0) == approx(lla)
assert pm.ned2geodetic(*ned3, *lla0) == approx(lla)
def test_ned():
xyz = pm.aer2ecef(*aer0, *lla0)
enu = pm.aer2enu(*aer0)
ned = (enu[1], enu[0], -enu[2])
lla = pm.aer2geodetic(*aer0, *lla0)
assert pm.aer2ned(*aer0) == approx(ned0)
with pytest.raises(ValueError):
pm.aer2ned(aer0[0], aer0[1], -1)
assert pm.enu2aer(*enu) == approx(aer0)
assert pm.enu2aer(*enu, deg=False) == approx(raer0)
assert pm.ned2aer(*ned) == approx(aer0)
assert pm.ecef2ned(*xyz, *lla0) == approx(ned)
assert pm.ned2ecef(*ned, *lla0) == approx(xyz)
# %%
assert pm.ecef2enuv(vx, vy, vz, *lla0[:2]) == approx((ve, vn, vu))
assert pm.ecef2nedv(vx, vy, vz, *lla0[:2]) == approx((vn, ve, -vu))
# %%
enu3 = pm.geodetic2enu(*lla, *lla0)
ned3 = (enu3[1], enu3[0], -enu3[2])
assert pm.geodetic2ned(*lla, *lla0) == approx(ned3)
assert pm.enu2geodetic(*enu3, *lla0) == approx(lla)
assert pm.ned2geodetic(*ned3, *lla0) == approx(lla)
def get_line_points(ref, line: ET.ElementTree):
points_elements = line.findall('./PNT')
if len(points_elements) == 0:
return []
point_data = [pnt_to_pair(pelement) for pelement in points_elements]
points = [pymap3d.geodetic2enu(p[0], p[1], 0, ref[0], ref[1], 0, ell=ell_wgs84, deg=True) for p in
point_data]
return points
def c(points):
points = CoordinateConverter.__getPyprojFunc(
fromCrs.getProjStr(),
Geodetic().getProjStr())(points)
e, n, u = p3d.geodetic2enu(points[:, 1], points[:, 0],
points[:, 2], toCrs.lat, toCrs.lon,
toCrs.alt)
return np.column_stack((e, n, u))
def get_xy(self, lat, lon, alt):
e, n, u = pymap3d.geodetic2enu(lat, lon, alt,
self.control_data['base_lat'],
self.control_data['base_lon'],
self.control_data['base_alt'])
# print ('lat : ', lat, self.control_data['base_lat'])
# print ('lon : ', lon, self.control_data['base_lon'])
# print ('alt : ', alt, self.control_data['base_alt'])
return e, n
def GPS_call(self, data):
print("dede?")
lon = float(data.longitude) #x
lat = float(data.latitude) #y
alt = float(data.altitude) #z
self.e, self.n, self.u = pm.geodetic2enu(lat, lon, alt, base_lat,
base_lon, base_alt)
self.msg.x = float(n)
self.msg.y = float(e)
def callback_gps(msg_gps):
global state
global RUN_gps
gps_pos = pm.geodetic2enu(msg_gps.latitude, msg_gps.longitude,
msg_gps.altitude, lat0, lon0, h0)
state['x'] = gps_pos[0]
state['y'] = gps_pos[1]
RUN_gps = True
def compute_local_roation(self):
h = self.h0
lat = self.east[0]
lon = self.east[1]
lat0 = self.origin[0]
lon0 = self.origin[1]
x,y,z = pm.geodetic2enu(lat, lon, h, lat0, lon0, self.h0)
self.local_rot = atan2(y, x)
rospy.logwarn("Local roation = %s", self.local_rot)
return self.local_rot
def geo2enu(self, lat, lon):
x, y, _ = p3d.geodetic2enu(lat,
lon,
0,
self.lat0,
self.lon0,
0,
deg=True,
ell=p3d.Ellipsoid('wgs84'))
return x, y
def mergefov(w0: xarray.Dataset, w1: xarray.Dataset, projalt: float = 110e3, method: str = None):
"""
inputs:
-------
w0: wide FOV data, particularly az/el
w1: other camera FOV data contained in w0
projalt: projection altitude METERS
ofn: plot filename
fovrow: to vastly speedup intiial overlap exploration, pick row(s) of pixels to examing--it could take half an hour + otherwise.
find the ECEF x,y,z, at 110km altitude for narrow camera outer pixel
boundary, then find the closest pixels in the wide FOV to those points.
Remember, it can be (much) faster to brute force this calculation than to use
k-d tree.
"""
if projalt < 1e3:
logging.warning(f"this function expects meters, you picked projection altitude {projalt/1e3} km")
# %% print distance from wide camera to narrow camera (just for information)
print(f"intercamera distance with {w0.site}: {vdist(w0.lat,w0.lon, w1.lat,w1.lon)[0]/1e3:.1f} kilometers")
# %% ENU projection from cam0 to cam1
e1, n1, u1 = pm.geodetic2enu(w1.lat, w1.lon, w1.alt_m, w0.lat, w0.lon, w0.alt_m)
# %% find the ENU of narrow FOV pixels at 110km from narrow FOV
w1 = pixelmask(w1, method)
if method is not None and method.lower() == "mzslice":
w0 = pixelmask(w0, method)
azSlice0, elSlice0, rSlice0 = pm.ecef2aer(w1.x2mz, w1.y2mz, w1.z2mz, w0.lat, w0.lon, w0.alt_m)
azSlice1, elSlice1, rSlice1 = pm.ecef2aer(w0.x2mz, w0.y2mz, w0.z2mz, w1.lat, w1.lon, w1.alt_m)
# find image indices (mask) corresponding to slice az,el
w0["rows"], w0["cols"] = fnd.findClosestAzel(
w0["az"].where(w0["fovmask"]), w0["el"].where(w0["fovmask"]), azSlice0, elSlice0
)
w0.attrs["Brow"], w0.attrs["Bcol"] = fnd.findClosestAzel(w0["az"], w0["el"], w0.Baz, w0.Bel)
w1["rows"], w1["cols"] = fnd.findClosestAzel(
w1["az"].where(w1["fovmask"]), w1["el"].where(w1["fovmask"]), azSlice1, elSlice1
)
w1.attrs["Brow"], w1.attrs["Bcol"] = fnd.findClosestAzel(w1["az"], w1["el"], w1.Baz, w1.Bel)
else:
# csc(x) = 1/sin(x)
slantrange = projalt / np.sin(np.radians(np.ma.masked_invalid(w1["el"].where(w1["fovmask"]))))
assert (slantrange >= projalt).all(), "slantrange must be >= projection altitude"
e0, n0, u0 = pm.aer2enu(w1["az"], w1["el"], slantrange)
# %% find az,el to narrow FOV from ASI FOV
az0, el0, _ = pm.enu2aer(e0 - e1, n0 - n1, u0 - u1)
assert (el0 >= 0).all(), "FOVs may not overlap, negative elevation from cam0 to cam1"
# %% nearest neighbor brute force
w0["rows"], w0["cols"] = fnd.findClosestAzel(w0["az"], w0["el"], az0, el0)
return w0, w1
def geo2desiredENU(self, lat, lon, h): """Converts from LLA to desired local ENU coordinates. It requires origin LLA and local_rot """ lat0 = self.origin[0] lon0 = self.origin[1] x,y,z = pm.geodetic2enu(lat, lon, h, lat0, lon0, self.h0) x_L = cos(self.local_rot)*x + sin(self.local_rot)*y y_L = -1*sin(self.local_rot)*x + cos(self.local_rot)*y z = self.curr_z_enu - self.GND_ALT return x_L, y_L, z
def callback_gps(msg_gps):
global RUN_gps
global state_x
global state_y
global time_now
if not RUN_gps:
RUN_gps = True
time_now = time.time()
else:
gps_pos = pm.geodetic2enu(msg_gps.latitude, msg_gps.longitude,
msg_gps.altitude, lat0, lon0, h0)
state_x.append(gps_pos[0])
state_y.append(gps_pos[1])
def checkConfigConsistency(data, decimals=6, verbose=True):
'''
Given the data loaded from the configuration data file, this will check for defined ENU and latlonel coordinates,
and confirm if they agree with eachother.
Parameters
----------
data : dict
The dict generated when loading a configuration from a json file.
decimals : int
Values are rounded before compared. This chooses the precision using the numpy.around funciton.
'''
data = copy.deepcopy(data) #So as to not edit the original.
origin = data['origin'][
'latlonel'] #Lat Lon Elevation, use for generating ENU from other latlonel values.
for mast in range(4):
for key in ['hpol', 'vpol']:
if numpy.asarray(data['antennas'][
'ant%i' % mast][key]['enu']).size > 0 and numpy.asarray(
data['antennas']['ant%i' %
mast][key]['latlonel']).size > 0:
enu_from_latlonel = numpy.asarray(
pm.geodetic2enu(
data['antennas']['ant%i' % mast][key]['latlonel'][0],
data['antennas']['ant%i' % mast][key]['latlonel'][1],
data['antennas']['ant%i' % mast][key]['latlonel'][2],
origin[0], origin[1], origin[2]))
enu_from_json = numpy.asarray(
data['antennas']['ant%i' % mast][key]['enu'])
match = numpy.all(
numpy.around(enu_from_latlonel, decimals=decimals) ==
numpy.around(enu_from_json, decimals=decimals))
max_precision_to_check = 10
decimals = max_precision_to_check
while numpy.all(
numpy.around(enu_from_latlonel, decimals=decimals) !=
numpy.around(enu_from_json, decimals=decimals)
) or decimals == 0:
decimals -= 1
print(
'Checking mast %i %s coordinates: Match up to %i decimals'
% (mast, key, decimals) + ['', ' (Max precision checked)'][
decimals == max_precision_to_check])
else:
print(key + ' does not contain both latlonel and ENU data.')
def calculateRelativeCoordinatesToStation(self, station):
'''
Given a station this will determine relative coordinates of the source to the
station and store them in a dict using the station key.
'''
try:
self.relative_enu[station.key] = pm.geodetic2enu(
self.latitude, self.longitude, self.elevation,
station.latitude, station.longitude, station.elevation)
except Exception as e:
print('Error in Source.calculateRelativeCoordinatesToStation().')
print(e)
exc_type, exc_obj, exc_tb = sys.exc_info()
fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1]
print(exc_type, fname, exc_tb.tb_lineno)
def gpsSpCb(self, msg):
# lat = msg.x; lon = msg.y;
# msg.z is the altitude setpoint in ENU
if msg is not None:
target_lat = msg.x
target_lon = msg.y
target_alt = msg.z # in ENU
target_gps_alt = self.current_alt
delta_x, delta_y, delta_z = pm.geodetic2enu(
target_lat, target_lon, target_gps_alt, self.current_lat,
self.current_lon, self.current_alt)
self.mavros_sp.position.x = self.current_local_x + delta_x
self.mavros_sp.position.y = self.current_local_y + delta_y
self.mavros_sp.position.z = target_alt
def gps_callback(msg):
global datum_lat
global datum_lon
global i
if i == 0:
datum_lat = msg.latitude
datum_lon = msg.longitude
i = i + 1
current_lat = msg.latitude
current_lon = msg.longitude
x, y, _ = pm.geodetic2enu(current_lat, current_lon, 0, datum_lat,
datum_lon, 0)
position = Gps()
position.x = x
position.y = y
pub.publish(position)
def cg_pos(data):
'''Convers gps data to be in terms of local frame
Assuming data is in dict defined in getData.gpsData.dataClean
'''
x, y, _ = pm.geodetic2enu(data['latitude'],
data['longitude'],
data['el'],
lat0,
long0,
h0,
ell=wgs84)
xC, yC = (data["x_gps_cg"] * np.cos(data["Yaw"]) -
data["y_gps_cg"] * np.sin(data["Yaw"]),
data["x_gps_cg"] * np.sin(data["Yaw"]) +
data["y_gps_cg"] * np.cos(data["Yaw"]))
return x + xC, y + yC
def test_ned_geodetic():
lla = pm.aer2geodetic(*aer0, *lla0)
enu3 = pm.geodetic2enu(*lla, *lla0)
ned3 = (enu3[1], enu3[0], -enu3[2])
assert pm.geodetic2ned(*lla, *lla0) == approx(ned3)
lat, lon, alt = pm.enu2geodetic(*enu3, *lla0)
assert lat == approx(lla[0])
assert lon == approx(lla[1])
assert alt == approx(lla[2])
lat, lon, alt = pm.ned2geodetic(*ned3, *lla0)
assert lat == approx(lla[0])
assert lon == approx(lla[1])
assert alt == approx(lla[2])
def desiredENU2localSp(self, xd, yd, zd): """Converts locaion in desired ENU to setpoint in robot's ENU """ if self.INDOOR: self.mavros_sp.position.x = xd self.mavros_sp.position.y = yd self.mavros_sp.position.z = zd else: lat, lon, alt = self.desiredENU2geo(xd, yd, zd) # converts to differences in robot's ENU x,y,z = pm.geodetic2enu(lat, lon, alt, self.curr_lat, self.curr_lon, self.gpsAlt) # Add the differences to robot's current location self.mavros_sp.position.x = self.local_pose.pose.position.x + x self.mavros_sp.position.y = self.local_pose.pose.position.y + y self.mavros_sp.position.z = zd + self.GND_ALT
