def gpsCallback(self, data):
gps = GeoPointStamped()
gps.header = data.header
gps.position.latitude = data.latitude
gps.position.longitude = data.longitude
gps.position.altitude = data.altitude
self.position_pub.publish(gps)
def PointCloudCall(self,data): # 20 Hz As this function has highest frequency, good for update
pointCloud_ = PointCloud2()
pointCloud_ = data
self.graphslam_PointCloud_pub.publish(pointCloud_) # for Graph slam
self.nlosExclusionF_ = puNlosExclusion.nlosExclusion(self.calAngNNLOS)
self.nlosExclusionF_.nlosExclusion_(self.posArr, self.GNSS_, 'statManu', self.excluSatLis) # statManu
self.puGNSSPosCalF_prop = puGNSSPosCal.GNSSPosCal()
if((len(self.GNSSNRAWlosDel.GNSS_Raws) > 1) and (self.GNSSNRAWlosDel.GNSS_Raws[-1].GNSS_time != self.GNSSNRAWlosDelTimeFlag)): # only if there is a change, there will conduct the calculation
# print 'self.GNSSNRAWlosDel',len(self.GNSSNRAWlosDel.GNSS_Raws)
self.puGNSSPosCalF_prop.iterPosCal(self.GNSSNRAWlosDel, 'WLSGNSS')
self.GNSSNRAWlosDelTimeFlag = self.GNSSNRAWlosDel.GNSS_Raws[-1].GNSS_time
navfix_ = NavSatFix()
llh_ = []
llh_ = ecef2llh.xyz2llh(self.puGNSSPosCalF_prop.ecef_)
navfix_.header.stamp.secs = self.GNSSNRAWlosDel.GNSS_Raws[-1].GNSS_time
navfix_.latitude = float(llh_[0])
navfix_.longitude = float(llh_[1])
navfix_.altitude = float(llh_[2])
self.propGNSS_Navfix_pub.publish(navfix_)
# for Graph slam
graphNavfix_ = NavSatFix()
graphNavfix_ = navfix_
graphNavfix_.header = pointCloud_.header
self.graphslam_Navfix_pub.publish(graphNavfix_)
# for Graph slam
geopoint = GeoPointStamped()
geopoint.header = graphNavfix_.header
geopoint.position.latitude = float(llh_[0])
geopoint.position.longitude = float(llh_[1])
geopoint.position.altitude = float(llh_[2])
self.graphslam_GeoPoint_pub.publish(geopoint)
def PointCloudCall(
self, data
): # 20 Hz As this function has highest frequency, good for update
self.pointCloud_ = PointCloud2()
self.pointCloud_ = data
# self.graphslam_PointCloud_pub.publish(self.pointCloud_) # for Graph slam
self.nlosExclusionF_ = puNlosExclusion.nlosExclusion(self.calAngNNLOS)
self.nlosExclusionF_.nlosExclusion_(self.posArr, self.GNSS_,
'statManu_ExclusionOpenLoop',
self.excluSatLis) # statManu
self.puGNSSPosCalF_prop = puGNSSPosCal.GNSSPosCal()
if ((len(self.GNSSNRAWlosDel.GNSS_Raws) > 1)
and (self.GNSSNRAWlosDel.GNSS_Raws[-1].GNSS_time !=
self.GNSSNRAWlosDelTimeFlag)
): # only if there is a change, there will conduct the calculation
# print 'self.GNSSNRAWlosDel',len(self.GNSSNRAWlosDel.GNSS_Raws)
self.puGNSSPosCalF_prop.iterPosCal(self.GNSSNRAWlosDel, 'WLSGNSS')
self.GNSSNRAWlosDelTimeFlag = self.GNSSNRAWlosDel.GNSS_Raws[
-1].GNSS_time
navfix_ = NavSatFix()
llh_ = []
llh_ = ecef2llh.xyz2llh(self.puGNSSPosCalF_prop.ecef_)
navfix_.header.stamp.secs = self.GNSSNRAWlosDel.GNSS_Raws[
-1].GNSS_time
navfix_.latitude = float(llh_[0])
navfix_.longitude = float(llh_[1])
navfix_.altitude = float(llh_[2])
self.propGNSS_Navfix_pub.publish(navfix_)
# for Graph slam
graphNavfix_ = NavSatFix()
graphNavfix_ = navfix_
graphNavfix_.header = self.pointCloud_.header
self.graphslam_Navfix_pub.publish(graphNavfix_)
# for Graph slam
geopoint = GeoPointStamped()
geopoint.header = graphNavfix_.header
# calculate the ENU coordiantes initialLLH
enu_ = pugeomath.transformEcefToEnu(self.initialLLH,
self.puGNSSPosCalF_prop.ecef_)
print 'enu_ ->: ', enu_
geopoint.position.latitude = float(enu_[0])
geopoint.position.longitude = float(enu_[1])
# geopoint.position.altitude = float(llh_[2]) # use this to save the covariance in altitude component
geopoint.position.altitude = self.GNSSNRAWlosDel.GNSS_Raws[
-1].snr * self.HDOPProp # save the covariance
self.graphslam_GeoPoint_pub.publish(geopoint)
def gps2navsat(filename, bag):
with open(filename, 'r') as file:
try:
while True:
data = struct.unpack('qddd', file.read(8 * 4))
time = data[0]
local = data[1:]
lat_lon_el_theta = struct.unpack('dddd', file.read(8 * 4))
gps_cov = struct.unpack('dddddddddddddddd', file.read(8 * 16))
if abs(lat_lon_el_theta[0]) < 1e-1:
continue
navsat = NavSatFix()
navsat.header.frame_id = 'gps'
navsat.header.stamp = rospy.Time.from_sec(time * 1e-6)
navsat.status.status = NavSatStatus.STATUS_FIX
navsat.status.service = NavSatStatus.SERVICE_GPS
navsat.latitude = lat_lon_el_theta[0]
navsat.longitude = lat_lon_el_theta[1]
navsat.altitude = lat_lon_el_theta[2]
navsat.position_covariance = numpy.array(gps_cov).reshape(
4, 4)[:3, :3].flatten().tolist()
navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_KNOWN
bag.write('/gps/fix', navsat, navsat.header.stamp)
geopoint = GeoPointStamped()
geopoint.header = navsat.header
geopoint.position.latitude = lat_lon_el_theta[0]
geopoint.position.longitude = lat_lon_el_theta[1]
geopoint.position.altitude = lat_lon_el_theta[2]
bag.write('/gps/geopoint', geopoint, geopoint.header.stamp)
except:
print 'done'
def init_uav(self, arm=False, takeoff=False, verbose=False):
if verbose:
print("Starting init_uav(). Atm we have to manualy accept every new command, but just remove the `input()` from the code to do this faster.")
res_setMode = self.setmode_srv(0,'GUIDED')
if verbose:
print("Res of setmode:", res_setMode)
res_set_ref_mode = self.set_ref_frame_srv(1) # FRAME_BODY_NED = 8 works for velocities
if verbose:
print("Res of set_ref_frame", res_set_ref_mode)
rospy.sleep(1)
print("Waiting for ekf initialisation")
origin = GeoPointStamped()
origin.header = Header()
origin.header.stamp = rospy.Time.now()
origin.position = GeoPoint()
origin.position.latitude = self.OPERATION_POSITION[0]
origin.position.longitude = self.OPERATION_POSITION[1]
origin.position.altitude = self.OPERATION_POSITION[2]
self.set_origin_pub.publish(origin)
if verbose:
print("Published ",origin)
while(True):
res = self.arm()
if res.success:
break
rospy.sleep(1)
self.disarm()
# input("Ekf initialized?")
#time.sleep(40) # For some reason, it does not work if we use rospy.sleep(...)
if arm or takeoff:
self.arm()
if takeoff:
self.takeoff()
print("Uav is now initialized. We are done with waiting.")
def __get_point_msg(cls, data, frame):
"""
Deserializes a location to a message of type PointStamped.
Args:
data: A dictionary containing the location.
frame: Frame for the point.
Returns:
A message of type PointStamped with the location.
"""
header = Header()
header.stamp = rospy.get_rostime()
header.frame_id = frame
msg = GeoPointStamped()
msg.header = header
msg.position.latitude = data["latitude"]
msg.position.longitude = data["longitude"]
return msg
def gps2navsat(filename, bag):
with open(filename, 'r') as file:
try:
while True:
data = struct.unpack('qddd', file.read(8*4))
time = data[0]
local = data[1:]
lat_lon_el_theta = struct.unpack('dddd', file.read(8*4))
gps_cov = struct.unpack('dddddddddddddddd', file.read(8*16))
if abs(lat_lon_el_theta[0]) < 1e-1:
continue
navsat = NavSatFix()
navsat.header.frame_id = 'gps'
navsat.header.stamp = rospy.Time.from_sec(time * 1e-6)
navsat.status.status = NavSatStatus.STATUS_FIX
navsat.status.service = NavSatStatus.SERVICE_GPS
navsat.latitude = lat_lon_el_theta[0]
navsat.longitude = lat_lon_el_theta[1]
navsat.altitude = lat_lon_el_theta[2]
navsat.position_covariance = numpy.array(gps_cov).reshape(4, 4)[:3, :3].flatten().tolist()
navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_KNOWN
bag.write('/gps/fix', navsat, navsat.header.stamp)
geopoint = GeoPointStamped()
geopoint.header = navsat.header
geopoint.position.latitude = lat_lon_el_theta[0]
geopoint.position.longitude = lat_lon_el_theta[1]
geopoint.position.altitude = lat_lon_el_theta[2]
bag.write('/gps/geopoint', geopoint, geopoint.header.stamp)
except:
print 'done'