def talker():
print 'in talker'
pub = rospy.Publisher('GPS', NavSatFix)
rospy.init_node('GPStalker')
while not rospy.is_shutdown():
#Assuming that parse will return these values
time.sleep(1)
parse()
msg = NavSatFix()
Fix = NavSatStatus()
Fix.status = GPS.mode
Fix.service = GPS.numSat
msg.header.stamp = rospy.Time.now()
msg.status = Fix
msg.latitude = GPS.lat
msg.longitude = GPS.lon
msg.altitude = GPS.alt
msg.position_covariance = (0, 0, 0, 0, 0, 0, 0, 0, 0)
msg.position_covariance_type = 0
#covariance_type = 0 unknown
# = 1 approximated
# = 2 diagonal known
# = 3 known
pub.publish(msg)
def parse_gps(self, data):
'''
Given raw data from Jaguar GPS socket, parse and return NavSatFix message.
If bad/incomplete data, return None
'''
# use regex to parse
gpgga_hit = re.search('^\$(GPGGA.+?)\r\n', data)
gprmc_hit = re.search('^\$(GPRMC.+?)\r\n', data)
if gprmc_hit:
# Get estimated heading (not part of standard ROS navsatfix message)
gprmc = gprmc_hit.group(0).split(",")
try:
heading = float(gprmc[8])
except:
heading = float("NaN")
else:
while heading < -180.0:
heading += 360.0
while heading > 180.0:
heading -= 360.0
finally:
self.current_est_heading = heading
if gpgga_hit:
gpgga = gpgga_hit.group(0).split(",")
nav_msg = NavSatFix()
# Set header information
time = gpgga[1]
hrs = float(time[0:1])
mins = float(time[2:3])
secs = float(time[4:5])
nav_msg.header.stamp = rospy.Time.from_sec(hrs * 3600 + mins * 60 + secs)
nav_msg.header.frame_id = "gps"
# Set GPS Status
status = NavSatStatus()
status.status = -1 if int(gpgga[6]) == 0 else 0
nav_msg.status = status
# Set longitude and latitude
lat_str = gpgga[2]
lon_str = gpgga[4]
lat_degs = float(lat_str[:2]) + (float(lat_str[2:]) / 60.0)
lon_degs = float(lon_str[:3]) + (float(lon_str[3:]) / 60.0)
nav_msg.latitude = -1 * lat_degs if gpgga[3] == "S" else lat_degs
nav_msg.longitude = -1 * lon_degs if gpgga[5] == "W" else lon_degs
# Set altitude (Positive is above the WGS 84 ellipsoid)
try:
nav_msg.altitude = float(gpgga[9])
except:
nav_msg.altitude = float("NaN")
# Set covariance type to unknown
nav_msg.position_covariance_type = 0
return nav_msg
else:
return None
def gps_sensor():
# initialize node, transform listener, and rate of tf lookup
rospy.init_node("gps_sensor", anonymous=True)
tfBuffer = tf2_ros.Buffer()
gps_sensor = tf2_ros.TransformListener(tfBuffer)
rate = rospy.Rate(1.0)
# create publisher objects for two topics, establish publish rate
pub_ship = rospy.Publisher("zed_f9p/base/fix",
sensor_msgs.msg.NavSatFix,
queue_size=10)
pub_UAV = rospy.Publisher("zed_f9p/rover/fix",
sensor_msgs.msg.NavSatFix,
queue_size=10)
# look up the NED to ship and NED to UAV transforms
while not rospy.is_shutdown():
try:
tship = tfBuffer.lookup_transform("NED", "boat", rospy.Time(0))
tUAV = tfBuffer.lookup_transform("NED", "UAV", rospy.Time(0))
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
rate.sleep()
continue
trans_ship = tship.transform.translation
trans_UAV = tUAV.transform.translation
# create the object for the ship which will be published and the tuple with converted NED to lat, long, alt numbers
NSF_ship = NavSatFix()
ship_lat_long_alt_coords = convert_NED_latlong(rospy.get_param("gps_sensor/rlat"), rospy.get_param("gps_sensor/rlong"), rospy.get_param("gps_sensor/ralt"),\
trans_ship.x, trans_ship.y, trans_ship.z)
# fill in the NavSatFix object with the correct numbers
NSF_ship.latitude = ship_lat_long_alt_coords[0]
NSF_ship.longitude = ship_lat_long_alt_coords[1]
NSF_ship.altitude = ship_lat_long_alt_coords[2]
# create the object for the ship which will be published and the tuple with converted NED to lat, long, alt numbers
NSF_UAV = NavSatFix()
UAV_lat_long_alt_coords = convert_NED_latlong(rospy.get_param("gps_sensor/rlat"), rospy.get_param("gps_sensor/rlong"), rospy.get_param("gps_sensor/ralt"),\
trans_UAV.x, trans_UAV.y, trans_UAV.z)
# fill in the NavSatFix object with the correct numbers
NSF_UAV.latitude = UAV_lat_long_alt_coords[0]
NSF_UAV.longitude = UAV_lat_long_alt_coords[1]
NSF_UAV.altitude = UAV_lat_long_alt_coords[2]
# publish to the two topics
# rospy.loginfo(NSF_ship)
pub_ship.publish(NSF_ship)
# rospy.loginfo(NSF_UAV)
pub_UAV.publish(NSF_UAV)
# wait at publishing rate
rate.sleep()
def callback(sensorstr):
data = sensorstr.data
if data[0] == 'Z':
data = data.replace("Z","").replace("\n","").replace("\r","")
data_list = data.split(',')
if len(data_list) == 4:
try:
##data_list structure: lat, lon, heading, vel
float_list = [float(i) for i in data_list]
poseGPS = NavSatFix()
poseGPS.header.frame_id = "world"
poseGPS.header.stamp = rospy.Time.now()
poseGPS.status.status = 0
poseGPS.status.service = 1
poseGPS.latitude = float_list[0]
poseGPS.longitude = float_list[1]
poseGPS.altitude = 0.0
poseGPS.position_covariance = [3.24, 0, 0, 0, 3,24, 0, 0, 0, 0]
poseGPS.position_covariance_type = 2
try:
pubGPS.publish(poseGPS)
log = "GPS Pose Data: %f %f Publish at Time: %s" % (float_list[0], float_list[1], rospy.get_time())
except:
log = "poseGPS Publisher Error"
except:
log = "GPS Data Error! Data : %s" % data
rospy.loginfo(log)
rospy.loginfo(log)
def get_and_pub_packet(self):
data, addr = self.sock.recvfrom(8308)
line = data[206:278]
my_fix = NavSatFix()
my_fix.header.stamp = rospy.Time.now()
latitude_degrees = float(line[16:18])
latitude_minutes = float(line[18:25])
latitude = self.decimal_degrees(degs=latitude_degrees,
mins=latitude_minutes)
if (line[26] == 'S'):
latitude = -1 * latitude
longitude_degrees = float(line[28:31])
longitude_minutes = float(line[31:38])
longitude = self.decimal_degrees(degs=longitude_degrees,
mins=longitude_minutes)
if (line[39] == 'W'):
longitude = -1 * longitude
my_fix.latitude = latitude
my_fix.longitude = longitude
my_fix.altitude = 0.0
self.fix_pub.publish(my_fix)
def publishGPS():
ser = serial.Serial(port='/dev/ttyACM0', baudrate=4800, timeout=5)
pub = rospy.Publisher('/tunnelGPS_fix', NavSatFix, queue_size=1024)
rospy.init_node('tunnel_gps_node', anonymous=True)
rate = rospy.Rate(1) # 1Hz
NavStat_msg = NavSatFix()
while not rospy.is_shutdown():
line_raw = ser.readline()
line = line_raw.split(',')
if line[0] == "GPGGA":
NavStat_msg.time = line[1]
if line[2] == '':
print "No GPS Signal"
else:
if line[3] == 'N':
NavStat_msg.latitude = float(line[2])
else:
NavStat_msg.latitude = - float(line[2])
if line[5] == 'E':
NavStat_msg.longitude = float(line[4])
else:
NavStat_msg.longitude = - float(line[4])
NavStat_msg.altitude = line[9]
pub.publish(NavStat_msg)
print "Publishing: lat = " + NavStat_msg.latitude + " long = " + NavStat_msg.longitude + " alt = " + NavStat_msg.altitude
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 gps_talker():
pub = rospy.Publisher('navsat', NavSatFix, queue_size=10)
rospy.init_node('gps_talker', anonymous=True)
msg = NavSatFix()
seq = 0
while not rospy.is_shutdown():
line = s.readline()
data = pynmea2.parse(line)
if line[1:5] == 'GPRM':
seq += 1
msg.header.seq = seq
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = 'world'
if data.status == 'A':
msg.status.status = int(1)
else:
msg.status.status = int(0)
msg.status.service = int(0)
msg.latitude = float(data.lat)
msg.longitude = float(data.lon)
# data.true_course=142.46
# data.spd_over_grnd
if line[1:5] == 'GPGG':
seq += 1
msg.header.seq = seq
msg.latitude = float(data.lat)
msg.longitude = float(data.lon)
msg.altitude = float(data.altitude) * 3.28084
pub.publish(msg)
def rtkgps(portname, baudrate):
pub = rospy.Publisher('rtk_fix', NavSatFix, queue_size=1)
rospy.init_node('rtk_gps_node', anonymous=True)
rate = rospy.Rate(10)
ser = serial.Serial(portname, baudrate, timeout=2)
while True:
line = ser.readline()
if line.startswith('$GNGGA'):
ls = line.split(",")
lat = ls[2]
lat1 = ls[2].strip()
latf = float(lat1)
latfinal = latf / 100
longitude = ls[4]
longitude1 = ls[4].strip()
longitudef = float(longitude1)
longitudefinal = longitudef / 100
altd = ls[9].strip()
altdf = float(altd)
print latfinal
print longitudefinal
print altdf
rtk_fix = NavSatFix()
#include header and time
rtk_fix.latitude = latfinal
rtk_fix.longitude = longitudefinal
rtk_fix.altitude = altdf
pub.publish(rtk_fix)
def on_message(client, userdata, msg):
global pub
global last_lat
global last_lon
y = json.loads(msg.payload)
navsat = NavSatFix()
navsat.header.stamp = rospy.Time.now()
navsat.header.frame_id = 'gps'
navsat.status.status = 0
navsat.status.service = 1
latitude = y["latitude"]
longitude = y["longitude"]
navsat.latitude = float(latitude[:2]) + float(latitude[2:]) / 60
navsat.longitude = float(longitude[:3]) + float(longitude[3:]) / 60
if y["lat"] == "S":
navsat.latitude = -1 * navsat.latitude
if y["lon"] == "W":
navsat.longitude = -1 * navsat.longitude
navsat.altitude = y["altitude"]
if last_lat != latitude or last_lon != longitude:
print("lat:" + navsat.latitude)
print("lon:" + navsat.longitude)
last_lat = latitude
last_lon = longitude
pub.publish(navsat)
def __init__(self):
rospy.init_node('add_test_points')
self.waypoint_pub = rospy.Publisher('/waypoints', WaypointsArray, queue_size=10, latch=True)
f = open ("points.txt", "r")
p_array = []
for line in f:
l = NavSatFix()
items = line.split(",")
l.latitude = (float) (items[0])
l.longitude = (float) (items[1])
l.altitude = (float) (items[2].split("\n")[0])
p_array.append(l)
msg = WaypointsArray()
msg.waypoints = p_array
#for i in range(100):
self.waypoint_pub.publish(msg)
#rospy.sleep(.1)
#print msg
print "done"
rospy.spin()
def _on_publish_pose(self, msg):
try:
point, quat = self._tfl.lookupTransform(self._world_frame, self._robot_frame, rospy.Time(0))
gps_msg = NavSatFix()
# result.header.frame_id = self._world_frame
gps_msg.status.status = 0
gps_msg.status.service = 1
gps_msg.header.stamp = rospy.Time.now()
utm_point = utm.UTMPoint(point[0], point[1], point[2], zone=self.UTM_ZONE, band=self.UTM_BAND)
latlon = utm_point.toMsg()
gps_msg.latitude = latlon.latitude
gps_msg.longitude = latlon.longitude
gps_msg.altitude = latlon.altitude
imu_msg = Imu()
imu_msg.header.stamp = rospy.Time.now()
imu_msg.header.frame_id = self._world_frame
roll, pitch, yaw = euler_from_quaternion(quat)
quat = quaternion_from_euler(roll, pitch, (yaw * self.ORI_INVERT) + self.ORI_OFFSET)
imu_msg.orientation.x = quat[0]
imu_msg.orientation.y = quat[1]
imu_msg.orientation.z = quat[2]
imu_msg.orientation.w = quat[3]
#print "EULER", math.degrees(roll), math.degrees(pitch), math.degrees(yaw), "mit offset:", math.degrees(yaw + self.ORI_OFFSET)
self._pub_gps.publish(gps_msg)
self._pub_imu.publish(imu_msg)
self._print_transform_error = True
except tf.LookupException as le:
if self._print_transform_error:
self._print_transform_error = False
rospy.logwarn("Error while looup transform: %s" % le)
except Exception as ee:
if self._print_transform_error:
self._print_transform_error = False
rospy.logwarn("Error while transform: %s" % ee)
def got_position(xx, yy, aa, stamp):
global EP
fix = NavSatFix()
fix.header.stamp = stamp
fix.header.frame_id = "sim_gps"
fix.status.status = 0
fix.status.service = 1
utm_e = base_xx + base_utm_e + xx + CONFIG['SIM_ERROR'] * cos(
(yy + time()) / EP)
utm_n = base_yy + base_utm_n + yy + CONFIG['SIM_ERROR'] * cos(
(utm_e / 3.14) / EP)
(lon, lat) = conv(utm_e, utm_n, inverse=True)
fix.latitude = lat
fix.longitude = lon
fix.altitude = 1.3
#print "got_position: %f %f" % (lat, lon)
gps.publish(fix)
pos.publish(Pose2D(utm_e, utm_n, aa))
tfBr.sendTransform((utm_e - base_xx, utm_n - base_yy, 0),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(), "/map", "/gps")
def talker():
pub = rospy.Publisher('gps/fix', NavSatFix, queue_size=10)
rospy.init_node('fake_gps', anonymous=True)
pub2 = rospy.Publisher('vel', Twist, queue_size=10)
rate = rospy.Rate(2) # 10hz
loc_index = 0
while not rospy.is_shutdown():
#print(loc_index)
fake_localization = NavSatFix()
fake_vel = Twist()
#fake_vel.frame_id = "/gps"
#fake_localization.header.stamp = time.time()
fake_localization.header.frame_id = '/gps'
fake_localization.status.status = 1
fake_localization.status.service = 1
#print(gps_localizations[loc_index][0], gps_localizations[loc_index][1])
fake_localization.latitude = gps_localizations[loc_index][0]
fake_localization.longitude = gps_localizations[loc_index][1]
print(fake_localization.latitude, fake_localization.longitude)
fake_localization.altitude = 0.0
fake_localization.position_covariance = [
1.3224999999999998, 0.0, 0.0, 0.0, 1.3224999999999998, 0.0, 0.0,
0.0, 5.289999999999999
]
fake_localization.position_covariance_type = 1
#fake_vel.header.frame_id = "/gps"
pub.publish(fake_localization)
pub2.publish(fake_vel)
if (loc_index == len(gps_localizations) - 1):
loc_index = 0
else:
loc_index = loc_index + 1
rate.sleep()
def step(self):
stamp = super().step()
if not stamp:
return
if self.__gps_publisher.get_subscription_count() > 0 or \
self.__speed_publisher.get_subscription_count() > 0 or \
self._always_publish:
self._wb_device.enable(self._timestep)
msg = Float32()
msg.data = self._wb_device.getSpeed()
self.__speed_publisher.publish(msg)
if self.__coordinate_system == GPS.WGS84:
msg = NavSatFix()
msg.header.stamp = stamp
msg.header.frame_id = self._frame_id
msg.latitude = self._wb_device.getValues()[0]
msg.longitude = self._wb_device.getValues()[1]
msg.altitude = self._wb_device.getValues()[2]
msg.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
msg.status.service = NavSatStatus.SERVICE_GPS
self.__gps_publisher.publish(msg)
else:
msg = PointStamped()
msg.header.stamp = stamp
msg.header.frame_id = self._frame_id
msg.point.x = self._wb_device.getValues()[0]
msg.point.y = self._wb_device.getValues()[1]
msg.point.z = self._wb_device.getValues()[2]
self.__gps_publisher.publish(msg)
else:
self._wb_device.disable()
def spin(self):
#############################################################################
r = rospy.Rate(self.rate)
while not rospy.is_shutdown():
current_time = rospy.get_rostime()
frame_id = 'gps'
latitude = 40.444539
longitude = -79.940777
current_fix = NavSatFix()
current_fix.header.stamp = current_time
current_fix.header.frame_id = frame_id
current_time_ref = TimeReference()
current_time_ref.header.stamp = current_time
current_time_ref.header.frame_id = frame_id
current_time_ref.source = frame_id
current_fix.status.status = NavSatStatus.STATUS_FIX
current_fix.status.service = NavSatStatus.SERVICE_GPS
current_fix.latitude = latitude
current_fix.longitude = longitude
current_fix.position_covariance[0] = 0.1
current_fix.position_covariance[4] = 0.1
current_fix.position_covariance[8] = 0.25
current_fix.position_covariance_type = \
NavSatFix.COVARIANCE_TYPE_APPROXIMATED
current_fix.altitude = 0.0
self.fix_pub.publish(current_fix)
self.update()
r.sleep()
def run(self):
while True:
#Reading acceleration data
report = self.session.next()
if self.debug: print(report)
if report["class"] == "TPV":
if "time" in report:
gpsMSG = NavSatFix()
gpsMSG.latitude = report["lat"]
gpsMSG.longitude = report["lon"]
try:
gpsMSG.status = report["status"]
except:
pass
try:
gpsMSG.altitude = report["alt"]
except:
pass
current_time = modf(time.time())
# gpsMSG.header.sec = int(current_time[1])
# gpsMSG.header.nanosec = int(current_time[0] * 1000000000) & 0xffffffff
self.gpsPublisher.publish(gpsMSG)
else:
if self.debug: print("GPS has not locked onto sattelites")
def navToPylon(self):
self.towerRequest.publish(True)
while not (self.received):
self.rate.sleep()
if len(self.pylonList) > 0:
utmDronePos = utm.from_latlon(self.dronePos.latitude,
self.dronePos.longitude)
nearidx, nextWP = self.findNearestPylon(utmDronePos)
orientation, wpOffN, wpOffE = self.calcOrientation(nearidx)
utmNxtWP = utm.from_latlon(nextWP.lat, nextWP.lon)
nxtWPAdjN = utmNxtWP[0] + wpOffN
nxtWPAdjE = utmNxtWP[1] + wpOffE
WPADJ = utm.to_latlon(nxtWPAdjN, nxtWPAdjE, utmNxtWP[2],
utmNxtWP[3])
print("Waypoint to pylon is:", WPADJ)
wpTarget = NavSatFix()
wpTarget.latitude = WPADJ[0]
wpTarget.longitude = WPADJ[1]
wpTarget.altitude = 25.0
self.wpPub.publish(wpTarget)
print("coordSent")
self.coordSent = True
tmp = NavSatFix()
else:
# print(self.dronePos)
print("Warning: no pylons found! loitering...")
self.sendState(False)
def callback(self): #Publish data at 1 Hz rate = rospy.Rate(1) #Define the RTKRCV server ports corresponding to each of the processing #Modes port = 5801 sock = socket.socket() host = socket.gethostname() sock.connect((host, port)) #WGS84 Parameters e2 = 6.69437999014e-3 a = 6378137.0 #At a rate of 1 hz, create an rtklib message for each socket and publish #the data over while not rospy.is_shutdown(): #Create and RTKLIB and a NavSatFix data structure rtk = rtklib() navsat = NavSatFix() #Set the data structure header time to the current system time navsat.header.stamp = rospy.Time.now() rtk.header.stamp = rospy.Time.now() #Get the position message from the RTKRCV server msgStr = socket.recv(1024) #Split the message msg = msgStr.split() #Save the latitude, longitude and ellipsoid height navsat.latitude = float(msg[2]) navsat.longitude = float(msg[3]) navsat.altitude = float(msg[4]) #Save the position covariance navsat.position_covariance = [float(msg[7]),float(msg[10]),float(msg[12]),float(msg[10]),float(msg[8]),float(msg[11]),float(msg[12]),float(msg[11]),float(msg[9])] navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_KNOWN #Compute the radius of curvature in the N = 1.0*a/np.sqrt(1-e2*(np.sin(float(msg[2])*np.pi/180.0)**2)) #Compute and store the ECEF position rtk.x = (N+float(msg[4]))*np.cos(float(msg[2])*np.pi/180.0)*np.cos(float(msg[3])*np.pi/180.0) rtk.y = (N+float(msg[4]))*np.cos(float(msg[2])*np.pi/180.0)*np.sin(float(msg[3])*np.pi/180.0) rtk.z = (N*(1-e2)+float(msg[4]))*np.sin(float(msg[2])*np.pi/180.0) rtk.delay = float(msg[13]) rtk.ftest = float(msg[14]) #Store the NavSatFix data rtk.state = navsat pubs[i].publish(rtk) rate.sleep()
def init_path():
# logitude, latitute, altitute, orientation (+deg)
gpsPub = rospy.Publisher("/fix_path_init", NavSatFix, queue_size=10)
wpSub = rospy.Subscriber("/gps_path_init", Odometry, utm_cb)
navStatus = NavSatStatus()
navStatus.service = 1
navStatus.status = 1
# waypoints hardcoded for now, may want to spec with argument in future
num_wp = gps_waypoints.shape[0]
hdop = 1.5
position_covariance_type = 1
global wp_num
for i in range(0, num_wp):
# send gps waypoint for conversion
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = 'base_link'
msgOut = NavSatFix()
msgOut.position_covariance[0] = hdop**2
msgOut.position_covariance[4] = hdop**2
msgOut.position_covariance[8] = (2 * hdop)**2
msgOut.longitude = gps_waypoints[i][0]
msgOut.latitude = gps_waypoints[i][1]
msgOut.altitude = gps_waypoints[i][2]
msgOut.position_covariance_type = position_covariance_type
msgOut.status = navStatus
msgOut.header = header
wp_num = i
gpsPub.publish(msgOut)
def make_fix(t, data, args):
fix = NavSatFix()
fix.header.stamp = t
fix.header.frame_id = args.frame_id
if data.Q in (1, 2, 4):
# Fix, Float, GPS
fix.status.status = NavSatStatus.STATUS_GBAS_FIX
elif data.Q == 3:
# SBAS
fix.status.status = NavSatStatus.STATUS_SBAS_FIX
elif data.Q == 5:
# Single
fix.status.status = NavSatStatus.STATUS_FIX
else:
fix.status.status = NavSatStatus.STATUS_NO_FIX
fix.latitude = data.lat
fix.longitude = data.lon
fix.altitude = data.height
fix.position_covariance = [
covariance(data.sde),
covariance(data.sdne),
covariance(data.sdeu),
covariance(data.sdne),
covariance(data.sdn),
covariance(data.sdun),
covariance(data.sdeu),
covariance(data.sdun),
covariance(data.sdu),
]
fix.position_covariance_type = NavSatFix.COVARIANCE_TYPE_KNOWN
return fix
def read_rtk_gps():
s = socket.socket() # Create a socket object
# these are parameters loaded onto the ros parameter servers
host = rospy.get_param('~host', '172.20.10.5')
# these are parameters loaded onto the ros parameter servers
port = rospy.get_param('~port', 3000)
s.connect((host, port))
rospy.init_node('rtk_gps_node')
pub_navsatfix = rospy.Publisher("/rtk_fix", NavSatFix, queue_size=0)
navsatfix = NavSatFix()
while True:
GPS_msgs = s.recv(1024)
msgs = re.split('\r\n', GPS_msgs)
for msg in msgs:
if msg[:6] == "$GPGGA":
print msg
data = msg.split(',')
navsatfix.status = NavSatStatus(status=int(data[6]))
navsatfix.latitude = float(data[2][:2])
navsatfix.longitude = float(data[4][:3])
navsatfix.altitude = float(data[9])
navsatfix.header.stamp = rospy.Time.now()
pub_navsatfix.publish(navsatfix)
def rtk_gps_node():
# queue sine set up to large avoid message lost from buffer
# define pub publish to rtk_fix rostopic
pub = rospy.Publisher('rtk_fix', NavSatFix, queue_size=100000)
rospy.init_node('rtk_gps_node', anonymous=True)
# 100hz for more serial message to PC
rate = rospy.Rate(100)
# getting serial message from /dev/ttyACM0 baud rate as 115200
ser = serial.Serial('/dev/ttyACM0', '115200')
navs = NavSatFix()
header = Header()
while not rospy.is_shutdown():
readinstring = ser.readline()
oristr = readinstring.split(',')
# when the serial message title as GNGGA, stor in navs(NavaSatFix) structure
if oristr[0] == "$GNGGA":
if oristr[3] == 'N':
navs.latitude = float(float(oristr[2]) /
100) + float(float(oristr[2]) % 100) / 60
elif oristr[3] == 'S':
navs.latitude = -(float(float(oristr[2]) / 100) +
float(float(oristr[2]) % 100) / 60)
if oristr[5] == 'E':
navs.longitude = float(float(oristr[4]) / 100) + float(
float(oristr[4]) % 100) / 60
elif oristr[5] == 'W':
navs.longitude = -(float(float(oristr[4]) / 100) +
float(float(oristr[4]) % 100) / 60)
navs.altitude = float(oristr[9])
#publish navs to rtk_fix
pub.publish(navs)
print(readinstring)
rate.sleep()
def bestpos_handler(self, bestpos):
navsat = NavSatFix()
# TODO: The timestamp here should come from SPAN, not the ROS system time.
navsat.header.stamp = rospy.Time.now()
navsat.header.frame_id = self.odom_frame
# Assume GPS - this isn't exposed
navsat.status.service = NavSatStatus.SERVICE_GPS
position_type_to_status = {
BESTPOS.POSITION_TYPE_NONE: NavSatStatus.STATUS_NO_FIX,
BESTPOS.POSITION_TYPE_FIXED: NavSatStatus.STATUS_FIX,
BESTPOS.POSITION_TYPE_FIXEDHEIGHT: NavSatStatus.STATUS_FIX,
BESTPOS.POSITION_TYPE_FLOATCONV: NavSatStatus.STATUS_FIX,
BESTPOS.POSITION_TYPE_WIDELANE: NavSatStatus.STATUS_FIX,
BESTPOS.POSITION_TYPE_NARROWLANE: NavSatStatus.STATUS_FIX,
BESTPOS.POSITION_TYPE_DOPPLER_VELOCITY: NavSatStatus.STATUS_FIX,
BESTPOS.POSITION_TYPE_SINGLE: NavSatStatus.STATUS_FIX,
BESTPOS.POSITION_TYPE_PSRDIFF: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_WAAS: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_PROPAGATED: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_OMNISTAR: NavSatStatus.STATUS_SBAS_FIX,
BESTPOS.POSITION_TYPE_L1_FLOAT: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_IONOFREE_FLOAT: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_NARROW_FLOAT: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_L1_INT: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_WIDE_INT: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_NARROW_INT: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_RTK_DIRECT_INS: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_INS_SBAS: NavSatStatus.STATUS_SBAS_FIX,
BESTPOS.POSITION_TYPE_INS_PSRSP: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_INS_PSRDIFF: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_INS_RTKFLOAT: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_INS_RTKFIXED: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_INS_OMNISTAR: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_INS_OMNISTAR_HP: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_INS_OMNISTAR_XP: NavSatStatus.STATUS_GBAS_FIX,
BESTPOS.POSITION_TYPE_OMNISTAR_HP: NavSatStatus.STATUS_SBAS_FIX,
BESTPOS.POSITION_TYPE_OMNISTAR_XP: NavSatStatus.STATUS_SBAS_FIX,
BESTPOS.POSITION_TYPE_PPP_CONVERGING: NavSatStatus.STATUS_SBAS_FIX,
BESTPOS.POSITION_TYPE_PPP: NavSatStatus.STATUS_SBAS_FIX,
BESTPOS.POSITION_TYPE_INS_PPP_CONVERGING: NavSatStatus.STATUS_SBAS_FIX,
BESTPOS.POSITION_TYPE_INS_PPP: NavSatStatus.STATUS_SBAS_FIX,
}
navsat.status.status = position_type_to_status.get(bestpos.position_type, NavSatStatus.STATUS_NO_FIX)
# Position in degrees.
navsat.latitude = bestpos.latitude
navsat.longitude = bestpos.longitude
# Altitude in metres.
navsat.altitude = bestpos.altitude
navsat.position_covariance[0] = pow(2, bestpos.latitude_std)
navsat.position_covariance[4] = pow(2, bestpos.longitude_std)
navsat.position_covariance[8] = pow(2, bestpos.altitude_std)
navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_DIAGONAL_KNOWN
# Ship ito
self.pub_navsatfix.publish(navsat)
def got_position(xx, yy, aa, stamp):
global EP
fix = NavSatFix()
fix.header.stamp = stamp
fix.header.frame_id = "sim_gps"
fix.status.status = 0
fix.status.service = 1
utm_e = base_xx + base_utm_e + xx + CONFIG["SIM_ERROR"] * cos((yy + time()) / EP)
utm_n = base_yy + base_utm_n + yy + CONFIG["SIM_ERROR"] * cos((utm_e / 3.14) / EP)
(lon, lat) = conv(utm_e, utm_n, inverse=True)
fix.latitude = lat
fix.longitude = lon
fix.altitude = 1.3
# print "got_position: %f %f" % (lat, lon)
gps.publish(fix)
pos.publish(Pose2D(utm_e, utm_n, aa))
tfBr.sendTransform(
(utm_e - base_xx, utm_n - base_yy, 0),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(),
"/map",
"/gps",
)
def publish_navsatfix(msg, num):
global i_navsatfix
if msg.name() != 'NAV_HPPOSLLH':
print "error: invalid type of message!!!!"
return
navsatfix = NavSatFix()
navsatfix.header.stamp = rospy.Time.now()
if num == 1:
navsatfix.header.frame_id = "/left_gnss"
elif num == 2:
navsatfix.header.frame_id = "/right_gnss"
navsatfix.header.seq = i_navsatfix[num - 1]
navsatfix.latitude = msg.Latitude * 1e-7 + msg.latHp * 1e-9
navsatfix.longitude = msg.Longitude * 1e-7 + msg.lonHp * 1e-9
navsatfix.altitude = msg.height * 1e-3 + msg.heightHp * 1e-4
navsatfix.position_covariance = [(msg.hAcc * 1e-4) / math.sqrt(2.), 0, 0,
0, (msg.hAcc * 1e-4) / math.sqrt(2.), 0,
0, 0, (msg.vAcc * 1e-4)]
navsatfix.status.status = fix_status[num - 1]
if num == 1:
pub_leftnavsatfix.publish(navsatfix)
elif num == 2:
pub_rightnavsatfix.publish(navsatfix)
i_navsatfix[num - 1] += 1
def wecar_callback(wecar_msg):
navsat_msg = NavSatFix()
navsat_msg.header.frame_id = 'map'
navsat_msg.header.stamp = rospy.Time.now()
navsat_msg.latitude = wecar_msg.latitude
navsat_msg.longitude = wecar_msg.longitude
navsat_msg.altitude = wecar_msg.altitude
wecar_pub.publish(navsat_msg)
def publish_active_way_point(self):
if self.active_directions:
if len(self.current_way_points) > 0:
way_point_msg = NavSatFix()
way_point_msg.longitude = self.current_way_points[0][0]
way_point_msg.latitude = self.current_way_points[0][1]
way_point_msg.altitude = self.current_way_points[0][2]
self.ros_pub_gps_way_point.publish(way_point_msg)
def publish_gps(self):
if self.drone.positionGPS!=None:
gps_data = NavSatFix()
gps_data.latitude = self.drone.positionGPS[0]
gps_data.longitude = self.drone.positionGPS[1]#maybe reversed?
gps_data.altitude = self.drone.positionGPS[2]
gps_data.header.frame_id = self.unique_id
self.gps_pub.publish(gps_data)
def onPositionChange(self, latitude, longitude, altitude):
fix = NavSatFix()
fix.header.stamp = rospy.Time.now()
fix.header.frame_id = "odom"
fix.latitude = latitude
fix.longitude = longitude
fix.altitude = altitude
global_pose_pub.publish(fix)
def gps():
print utm.conversion.R
import roslib; roslib.load_manifest('ardrone_control')
import rospy; global rospy
from sensor_msgs.msg import NavSatFix
# rospy.init_node('test')
gps = GPS()
#print gps
m = NavSatFix()
m.latitude = 45.0
m.altitude = utm.conversion.R
m.longitude = 20.0
gps.set_zero(m)
gps.measure(m)
print gps.get_state()
print gps.Z
m.latitude = 45.1
m.altitude = utm.conversion.R
m.longitude = 20.0
gps.measure(m)
print gps.get_state()
print gps.Z
gps.set_zero_yaw( 30 * pi/180 )
gps.measure(m)
print gps.get_state()
print gps.get_measurement()
print gps.Z
for i in range(10000):
gps.measure(m)
gps.Broadcast()
rospy.spin()
def publish_gps(gps_pub, imu_data): gps = NavSatFix() gps.header.frame_id = FRAME_ID gps.header.stamp = rospy.Time.now() gps.latitude = imu_data.lat gps.longitude = imu_data.lon gps.altitude = imu_data.alt gps_pub.publish(gps)
def publish_gps(self, data):
G = NavSatFix()
gps_fmt = 'c3f'
read_data = struct.unpack(gps_fmt, data)
G.latitude = read_data[1]
G.longitude = read_data[2]
G.altitude = read_data[3]
self.gps_pub.publish(G)
rospy.loginfo("Published GPS MSG")
def navsat_callback(gps_msg):
navsat_msg = NavSatFix()
navsat_msg.header.frame_id = 'map'
navsat_msg.latitude = gps_msg.latitude
navsat_msg.altitude = gps_msg.altitude
navsat_msg.longitude = gps_msg.longitude
navsat_msg.header.stamp = rospy.Time.now()
navsat_msg.status.status = 1
navsat_pub.publish(navsat_msg)
def default(self, ci='unused'):
gps = NavSatFix()
gps.header = self.get_ros_header()
gps.latitude = self.data['x']
gps.longitude = self.data['y']
gps.altitude = self.data['z']
self.publish(gps)
def gps():
# If using I2C, we'll create an I2C interface to talk to using default pins
i2c = board.I2C()
# Create a GPS module instance.
gps = adafruit_gps.GPS_GtopI2C(i2c, debug=False) # Use I2C interface
# Turn on the basic GGA and RMC info (what you typically want)
gps.send_command(b"PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0")
# Set update rate to once a second (1hz)
gps.send_command(b"PMTK220,1000")
rospy.loginfo("Initializing gps publisher")
gps_pub = rospy.Publisher('/gps', NavSatFix, queue_size=5)
rospy.loginfo("Publishing NavSatFix at: " + gps_pub.resolved_name)
rospy.init_node('gps_node')
rate = rospy.Rate(1) # 50hz
while not rospy.is_shutdown():
gps.update()
if gps.has_fix:
nav = NavSatFix()
nav.header.stamp = rospy.Time.now()
nav.header.frame_id = 'Adafruit Mini GPS PA1010D'
nav.latitude = gps.latitude
nav.longitude = gps.longitude
nav.altitude = gps.altitude_m
rospy.loginfo("=" * 40)
rospy.loginfo("Latitude: {0:.6f} degrees".format(gps.latitude))
rospy.loginfo("Longitude: {0:.6f} degrees".format(gps.longitude))
rospy.loginfo("Fix quality: {}".format(gps.fix_quality))
# Some attributes beyond latitude, longitude and timestamp are optional
# and might not be present. Check if they're None before trying to use!
if gps.satellites is not None:
rospy.loginfo("# satellites: {}".format(gps.satellites))
if gps.altitude_m is not None:
rospy.loginfo("Altitude: {} meters".format(gps.altitude_m))
if gps.speed_knots is not None:
rospy.loginfo("Speed: {} knots".format(gps.speed_knots))
if gps.track_angle_deg is not None:
rospy.loginfo("Track angle: {} degrees".format(gps.track_angle_deg))
if gps.horizontal_dilution is not None:
rospy.loginfo("Horizontal dilution: {}".format(gps.horizontal_dilution))
if gps.height_geoid is not None:
rospy.loginfo("Height geoid: {} meters".format(gps.height_geoid))
gps_pub.publish(nav)
else:
# Try again if we don't have a fix yet.
rospy.loginfo("Waiting for fix...")
rate.sleep()
def publishWaypoint(self):
msg = NavSatFix()
msg.longitude = self.longitude
msg.latitude = self.latitude
msg.altitude = self.height
outstr = "Drone is publishing waypoint: objective is at (%f|%f|%f)" % (
self.longitude, self.latitude, self.height)
rospy.loginfo(outstr)
self.pub.publish(msg)
def getMsg():
pub_navsat = rospy.Publisher('/rtklib/rover', NavSatFix)
pub_pose = rospy.Publisher('/rtklib/pose', PoseStamped)
#Initialize the RTKLIB ROS node
rospy.init_node('rtklib_messages', anonymous=True)
#Define the publishing frequency of the node
rate = rospy.Rate(10)
#Create a socket
sock = socket.socket()
#Get the address of the local host
host = socket.gethostname()
#Connect to the RTKRCV server that is bound to port xxxx
port = 5801
sock.connect((host,port))
e2 = 6.69437999014e-3
a = 6378137.0
while not rospy.is_shutdown():
navsat = NavSatFix()
ecef_xyz = Point()
ecef_pose = Pose()
ecef_stampedPose = PoseStamped()
ecef_stampedPose =
navsat.header.stamp = rospy.Time.now()
#Get the position message from the RTKRCV server
msgStr = sock.recv(1024)
#Split the message
msg = msgStr.split()
navsat.latitude = float(msg[2])
navsat.longitude = float(msg[3])
navsat.altitude = float(msg[4])
N = 1.0*a/np.sqrt(1-e2*(np.sin(float(msg[2])*np.pi/180.0)**2))
ecef_xyz.x = (N+float(msg[4]))*np.cos(float(msg[2])*np.pi/180.0)*np.cos(float(msg[3])*np.pi/180.0)
ecef_xyz.y = (N+float(msg[4]))*np.cos(float(msg[2])*np.pi/180.0)*np.sin(float(msg[3])*np.pi/180.0)
ecef_xyz.z = (N*(1-e2)+float(msg[4]))*np.sin(float(msg[2])*np.pi/180.0)
ecef_pose.position = ecef_xyz
ecef_stampedPose.pose = ecef_pose
pub_navsat.publish(navsat)
pub_pose.publish(ecef_stampedPose)
rate.sleep()
def callback_sbp_pos(self, msg, **metadata):
if self.debug:
rospy.loginfo("Received SBP_MSG_POS_LLH (Sender: %d): %s" % (msg.sender, repr(msg)))
out = NavSatFix()
out.header.frame_id = self.frame_id
out.header.stamp = rospy.Time.now()
out.status.service = NavSatStatus.SERVICE_GPS
out.latitude = msg.lat
out.longitude = msg.lon
out.altitude = msg.height
out.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED
if msg.flags & 0x03:
self.last_rtk_time = time.time()
self.rtk_fix_mode = msg.flags & 0x03
out.status.status = NavSatStatus.STATUS_GBAS_FIX
# TODO this should probably also include covariance of base fix?
out.position_covariance[0] = self.rtk_h_accuracy**2
out.position_covariance[4] = self.rtk_h_accuracy**2
out.position_covariance[8] = self.rtk_v_accuracy**2
pub = self.pub_rtk_fix
self.last_rtk_pos = msg
# If we are getting this message, RTK is our best fix, so publish this as our best fix.
self.pub_fix.publish(out)
else:
self.last_spp_time = time.time()
out.status.status = NavSatStatus.STATUS_FIX
# TODO hack, piksi should provide these numbers
if self.last_dops:
out.position_covariance[0] = (self.last_dops.hdop * 1E-2)**2
out.position_covariance[4] = (self.last_dops.hdop * 1E-2)**2
out.position_covariance[8] = (self.last_dops.vdop * 1E-2)**2
else:
out.position_covariance[0] = COV_NOT_MEASURED
out.position_covariance[4] = COV_NOT_MEASURED
out.position_covariance[8] = COV_NOT_MEASURED
pub = self.pub_spp_fix
self.last_pos = msg
# Check if SPP is currently our best available fix
if self.rtk_fix_mode <= 0:
self.pub_fix.publish(out)
pub.publish(out)
def recieve_gps(self, data ): msg = NavSatFix() msg.latitude = data.vector.x msg.longitude = data.vector.y msg.altitude = data.vector.z msg.header.stamp = data.header.stamp msg.header.frame_id = data.header.frame_id self.publisher['gps'].publish(msg)
def default(self, ci='unused'):
""" Publish the data of the gps sensor as a custom ROS NavSatFix message """
gps = NavSatFix()
gps.header = self.get_ros_header()
# TODO ros.org/doc/api/sensor_msgs/html/msg/NavSatFix.html
gps.latitude = self.data['x']
gps.longitude = self.data['y']
gps.altitude = self.data['z']
self.publish(pose)
def ExtFix2Fix(data):
fix = NavSatFix()
fix.header = data.header
fix.status.status = data.status.status
fix.status.service = data.status.position_source
fix.latitude = data.latitude
fix.longitude = data.longitude
fix.altitude = data.altitude
fix.position_covariance = data.position_covariance
fix.position_covariance_type = data.position_covariance_type
return fix
def callback(self): rate = rospy.Rate(10) ports = [5801,5802,5803] sockets = [] pubs = [self.static_pub, self.rtkStatic_pub, self.rtkDynamic_pub] for i in ports: sock = socket.socket() host = socket.gethostname() sock.connect((host, i)) sockets.append(sock) e2 = 6.69437999014e-3 a = 6378137.0 while not rospy.is_shutdown(): for i in range(len(sockets)): navsat = NavSatFix() ecef_xyz = Point() ecef_pose = Pose() #ecef_stampedPose = PoseStamped() navsat.header.stamp = rospy.Time.now() #Get the position message from the RTKRCV server msgStr = sockets[i].recv(1024) #Split the message msg = msgStr.split() navsat.latitude = float(msg[2]) navsat.longitude = float(msg[3]) navsat.altitude = float(msg[4]) navsat.position_covariance = [float(msg[7]),float(msg[10]),float(msg[12]),float(msg[10]),float(msg[8]),float(msg[11]),float(msg[12]),float(msg[11]),float(msg[9])] navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_KNOWN N = 1.0*a/np.sqrt(1-e2*(np.sin(float(msg[2])*np.pi/180.0)**2)) ecef_xyz.x = (N+float(msg[4]))*np.cos(float(msg[2])*np.pi/180.0)*np.cos(float(msg[3])*np.pi/180.0) ecef_xyz.y = (N+float(msg[4]))*np.cos(float(msg[2])*np.pi/180.0)*np.sin(float(msg[3])*np.pi/180.0) ecef_xyz.z = (N*(1-e2)+float(msg[4]))*np.sin(float(msg[2])*np.pi/180.0) ecef_pose.position = ecef_xyz #ecef_stampedPose.pose = ecef_pose pubs[i].publish(navsat) rate.sleep()
def gps_fix_sim():
pub=rospy.Publisher('sensor_msgs/NavSatFix', NavSatFix, queue_size=10)
rospy.init_node('gps_sim', anonymous=True)
rate = rospy.Rate(2)
while not rospy.is_shutdown():
gps_fix = NavSatFix()
gps_fix.latitude = 36.6
gps_fix.longitude = -121.1
gps_fix.altitude = 1.0
pub.publish(gps_fix)
rate.sleep()
def pub_gps(msg_type, msg, bridge):
pub = bridge.get_ros_pub("gps", NavSatFix, queue_size=1)
fix = NavSatFix()
fix.header.stamp = bridge.project_ap_time(msg)
fix.header.frame_id = 'base_footprint'
fix.latitude = msg.lat/1e07
fix.longitude = msg.lon/1e07
# NOTE: absolute (MSL) altitude
fix.altitude = msg.alt/1e03
fix.status.status = NavSatStatus.STATUS_FIX
fix.status.service = NavSatStatus.SERVICE_GPS
fix.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
pub.publish(fix)
def testAutoOriginFromNavSatFix(self):
rospy.init_node('test_initialize_origin')
nsf_pub = rospy.Publisher('fix', NavSatFix, queue_size=2)
origin_sub = self.subscribeToOrigin()
self.test_stamp = True
nsf_msg = NavSatFix()
nsf_msg.latitude = swri['latitude']
nsf_msg.longitude = swri['longitude']
nsf_msg.altitude = swri['altitude']
nsf_msg.header.frame_id = "/far_field"
nsf_msg.header.stamp = msg_stamp
r = rospy.Rate(10.0)
while not rospy.is_shutdown():
nsf_pub.publish(nsf_msg)
r.sleep()
def test_telemetry_serializer(self):
"""Tests telemetry serializer."""
# Set up test data.
navsat = NavSatFix()
navsat.latitude = 38.149
navsat.longitude = -76.432
navsat.altitude = 30.48
compass = Float64(90.0)
json = serializers.TelemetrySerializer.from_msg(navsat, compass)
altitude_msl = serializers.meters_to_feet(navsat.altitude)
# Compare.
self.assertEqual(json["latitude"], navsat.latitude)
self.assertEqual(json["longitude"], navsat.longitude)
self.assertEqual(json["altitude_msl"], altitude_msl)
self.assertEqual(json["uas_heading"], compass.data)
def publish(self, data):
msg = NavSatFix()
msg.header.frame_id = self._frameId
msg.header.stamp = rospy.get_rostime()
msg.latitude = data.getLat()
msg.longitude = data.getLng()
msg.altitude = data.getMeters()
if data.getFix() == 1:
msg.status.status = 0
else:
msg.status.status = -1
msg.position_covariance_type = 1
msg.position_covariance[0] = data.getHDOP() * data.getHDOP()
msg.position_covariance[4] = data.getHDOP() * data.getHDOP()
msg.position_covariance[8] = 4 * data.getHDOP() * data.getHDOP()
msg.status.service = 1
self._pub.publish(msg)
def subCB(msg_in):
global pub
msg_out = NavSatFix()
msg_out.header = msg_in.header
msg_out.status.status = NavSatStatus.STATUS_FIX # TODO - fix this
msg_out.status.service = NavSatStatus.SERVICE_GPS
msg_out.latitude = msg_in.LLA.x
msg_out.longitude = msg_in.LLA.y
msg_out.altitude = msg_in.LLA.z
msg_out.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED
msg_out.position_covariance[1] = msg_in.PosUncerainty
pub.publish(msg_out)
def sub_insCB(msg_in):
global pub_imu
global pub_gps
global msg_imu
msg_imu.header.stamp = msg_in.header.stamp
msg_imu.header.frame_id = msg_in.header.frame_id
# Convert the RPY data from the Vectornav into radians!
roll = (math.pi * msg_in.RPY.x) / 180.0
pitch = (math.pi * msg_in.RPY.y) / 180.0
yaw = (math.pi * msg_in.RPY.z) / 180.0
q = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
msg_imu.orientation.x = q[0]
msg_imu.orientation.y = q[1]
msg_imu.orientation.z = q[2]
msg_imu.orientation.w = q[3]
pub_imu.publish(msg_imu)
msg_gps = NavSatFix()
msg_gps.header = msg_in.header
msg_gps.status.status = NavSatStatus.STATUS_FIX # TODO - fix this
msg_gps.status.service = NavSatStatus.SERVICE_GPS
msg_gps.latitude = msg_in.LLA.x
msg_gps.longitude = msg_in.LLA.y
msg_gps.altitude = msg_in.LLA.z
msg_gps.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED
msg_gps.position_covariance[0] = msg_in.PosUncerainty
pub_gps.publish(msg_gps)
msg_time = TimeReference()
msg_time.header.stamp = msg_in.header.stamp
msg_time.header.frame_id = msg_in.header.frame_id
unix_time = 315964800 + (msg_in.Week * 7 * 24 * 3600) + msg_in.Time
msg_time.time_ref = rospy.Time.from_sec(unix_time)
pub_time.publish(msg_time)
def talker():
pub = rospy.Publisher("/gps/fix", NavSatFix, queue_size=10)
rospy.init_node('talker', anonymous=True)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
#gps config
gpsd.next()
os.system('clear')
gps_data = NavSatFix()
#status of gps
# gps_data.status.status = 0
# gps_data.status.service = 1
#time
gps_data.header.stamp.secs = round(time.time())
#position
gps_data.latitude = gpsd.fix.latitude
gps_data.longitude = gpsd.fix.longitude
gps_data.altitude = 0
# gps_data.position_covariance_type = 0
#publication ros
pub.publish(gps_data)
rate.sleep()
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 publish(self, data):
now = int(round(time.time() * 1000))
msg = NavSatFix()
msg.header.frame_id = self._frameId
msg.header.stamp = rospy.get_rostime()
msg.latitude = data.getLat()
msg.longitude = data.getLng()
msg.altitude = data.getMeters()
cur_fix = data.getFix()
#print cur_fix
if (cur_fix != self._old_fix):
msg.status.status = 0
self._old_fix = cur_fix
self._wd = now
elif (now - self._wd) < GPS_WD_TIMEOUT:
msg.status.status = 0
else:
msg.status.status = -1
msg.position_covariance_type = 1
msg.position_covariance[0] = data.getHDOP() * data.getHDOP()
msg.position_covariance[4] = data.getHDOP() * data.getHDOP()
msg.position_covariance[8] = 4 * data.getHDOP() * data.getHDOP()
msg.status.service = 1
self._pub.publish(msg)
def add_sentence(self, nmea_string, frame_id, timestamp=None):
if not check_nmea_checksum(nmea_string):
rospy.logwarn("Received a sentence with an invalid checksum. " +
"Sentence was: %s" % repr(nmea_string))
return False
parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence(nmea_string)
if not parsed_sentence:
rospy.logdebug("Failed to parse NMEA sentence. Sentece was: %s" % nmea_string)
return False
if timestamp:
current_time = timestamp
else:
current_time = rospy.get_rostime()
current_fix = NavSatFix()
current_fix.header.stamp = current_time
current_fix.header.frame_id = frame_id
current_time_ref = TimeReference()
current_time_ref.header.stamp = current_time
current_time_ref.header.frame_id = frame_id
if self.time_ref_source:
current_time_ref.source = self.time_ref_source
else:
current_time_ref.source = frame_id
if 'B' in parsed_sentence:
data = parsed_sentence['B']
# Only publish a fix from RMC if the use_RMC flag is set.
if True:
current_fix.status.status = NavSatStatus.STATUS_FIX
current_fix.status.service = NavSatStatus.SERVICE_GPS
current_fix.latitude = data['latitude']
current_fix.longitude = data['longitude']
current_fix.altitude = float('NaN')
current_fix.position_covariance_type = \
NavSatFix.COVARIANCE_TYPE_UNKNOWN
self.fix_pub.publish(current_fix)
if not math.isnan(data['utc_time']):
current_time_ref.time_ref = rospy.Time.from_sec(data['utc_time'])
self.time_ref_pub.publish(current_time_ref)
# Publish velocity from RMC regardless, since GGA doesn't provide it.
if True:
current_vel = TwistStamped()
current_vel.header.stamp = current_time
current_vel.header.frame_id = frame_id
current_vel.twist.linear.x = data['speed'] * \
math.sin(data['true_course'])
current_vel.twist.linear.y = data['speed'] * \
math.cos(data['true_course'])
self.vel_pub.publish(current_vel)
else:
return False
def navigation_handler(self, data):
""" Rebroadcasts navigation data in the following formats:
1) /odom => /base footprint transform (if enabled, as per REP 105)
2) Odometry message, with parent/child IDs as in #1
3) NavSatFix message, for systems which are knowledgeable about GPS stuff
4) IMU messages
"""
rospy.logdebug("Navigation received")
# If we don't have a fix, don't publish anything...
if self.nav_status.status == NavSatStatus.STATUS_NO_FIX:
return
orient = PyKDL.Rotation.RPY(RAD(data.roll), RAD(data.pitch), RAD(data.heading)).GetQuaternion()
# UTM conversion
(zone, easting, northing) = LLtoUTM(23, data.latitude, data.longitude)
# Initialize starting point if we haven't yet
# TODO: Do we want to follow UTexas' lead and reinit to a nonzero point within the same UTM grid?
if not self.init and self.zero_start:
self.origin.x = easting
self.origin.y = northing
self.init = True
# Publish origin reference for others to know about
p = Pose()
p.position.x = self.origin.x
p.position.y = self.origin.y
self.pub_origin.publish(p)
#
# Odometry
# TODO: Work out these covariances properly from DOP
#
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = self.odom_frame
odom.child_frame_id = self.base_frame
odom.pose.pose.position.x = easting - self.origin.x
odom.pose.pose.position.y = northing - self.origin.y
odom.pose.pose.position.z = data.altitude
odom.pose.pose.orientation = Quaternion(*orient)
odom.pose.covariance = POSE_COVAR
# Twist is relative to /vehicle frame
odom.twist.twist.linear.x = data.speed
odom.twist.twist.linear.y = 0
odom.twist.twist.linear.z = -data.down_vel
odom.twist.twist.angular.x = RAD(data.ang_rate_long)
odom.twist.twist.angular.y = RAD(-data.ang_rate_trans)
odom.twist.twist.angular.z = RAD(-data.ang_rate_down)
odom.twist.covariance = TWIST_COVAR
self.pub_odom.publish(odom)
#
# Odometry transform (if required)
#
if self.publish_tf:
self.tf_broadcast.sendTransform(
(odom.pose.pose.position.x, odom.pose.pose.position.y,
odom.pose.pose.position.z), Quaternion(*orient),
odom.header.stamp,odom.child_frame_id, odom.frame_id)
#
# NavSatFix
# TODO: Work out these covariances properly from DOP
#
navsat = NavSatFix()
navsat.header.stamp = rospy.Time.now()
navsat.header.frame_id = self.odom_frame
navsat.status = self.nav_status
navsat.latitude = data.latitude
navsat.longitude = data.longitude
navsat.altitude = data.altitude
navsat.position_covariance = NAVSAT_COVAR
navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
self.pub_navsatfix.publish(navsat)
#
# IMU
# TODO: Work out these covariances properly
#
imu = Imu()
imu.header.stamp == rospy.Time.now()
imu.header.frame_id = self.base_frame
# Orientation
imu.orientation = Quaternion(*orient)
imu.orientation_covariance = IMU_ORIENT_COVAR
# Angular rates
imu.angular_velocity.x = RAD(data.ang_rate_long)
imu.angular_velocity.y = RAD(-data.ang_rate_trans)
imu.angular_velocity.y = RAD(-data.ang_rate_down)
imu.angular_velocity_covariance = IMU_VEL_COVAR
# Linear acceleration
imu.linear_acceleration.x = data.long_accel
imu.linear_acceleration.y = data.trans_accel
imu.linear_acceleration.z = data.down_accel
imu.linear_acceleration_covariance = IMU_ACCEL_COVAR
self.pub_imu.publish(imu)
pass
def add_sentence(self, nmea_string, frame_id, timestamp=None):
if not check_nmea_checksum(nmea_string):
rospy.logwarn("Received a sentence with an invalid checksum. " +
"Sentence was: %s" % repr(nmea_string))
return False
parsed_sentence = libjavad_navsat_driver.parser.parse_nmea_sentence(nmea_string)
if not parsed_sentence:
rospy.logdebug("Failed to parse NMEA sentence. Sentece was: %s" % nmea_string)
return False
if timestamp:
current_time = timestamp
else:
current_time = rospy.get_rostime()
current_fix = NavSatFix()
current_fix.header.stamp = current_time
current_fix.header.frame_id = frame_id
current_time_ref = TimeReference()
current_time_ref.header.stamp = current_time
current_time_ref.header.frame_id = frame_id
if self.time_ref_source:
current_time_ref.source = self.time_ref_source
else:
current_time_ref.source = frame_id
if not self.use_RMC and 'GGA' in parsed_sentence:
data = parsed_sentence['GGA']
gps_qual = data['fix_type']
if gps_qual == 0:
current_fix.status.status = NavSatStatus.STATUS_NO_FIX
elif gps_qual == 1:
current_fix.status.status = NavSatStatus.STATUS_FIX
elif gps_qual == 2:
current_fix.status.status = NavSatStatus.STATUS_SBAS_FIX
elif gps_qual in (4, 5):
current_fix.status.status = NavSatStatus.STATUS_GBAS_FIX
else:
current_fix.status.status = NavSatStatus.STATUS_NO_FIX
current_fix.status.service = NavSatStatus.SERVICE_GPS
current_fix.header.stamp = current_time
latitude = data['latitude']
if data['latitude_direction'] == 'S':
latitude = -latitude
current_fix.latitude = latitude
longitude = data['longitude']
if data['longitude_direction'] == 'W':
longitude = -longitude
current_fix.longitude = longitude
hdop = data['hdop']
current_fix.position_covariance[0] = hdop ** 2
current_fix.position_covariance[4] = hdop ** 2
current_fix.position_covariance[8] = (2 * hdop) ** 2 # FIXME
current_fix.position_covariance_type = \
NavSatFix.COVARIANCE_TYPE_APPROXIMATED
# Altitude is above ellipsoid, so adjust for mean-sea-level
#altitude = data['altitude'] + data['mean_sea_level']
altitude = data['altitude']
current_fix.altitude = altitude
self.fix_pub.publish(current_fix)
if not math.isnan(data['utc_time']):
current_time_ref.time_ref = rospy.Time.from_sec(data['utc_time'])
self.time_ref_pub.publish(current_time_ref)
elif 'RMC' in parsed_sentence:
data = parsed_sentence['RMC']
# Only publish a fix from RMC if the use_RMC flag is set.
if self.use_RMC:
if data['fix_valid']:
current_fix.status.status = NavSatStatus.STATUS_FIX
else:
current_fix.status.status = NavSatStatus.STATUS_NO_FIX
current_fix.status.service = NavSatStatus.SERVICE_GPS
latitude = data['latitude']
if data['latitude_direction'] == 'S':
latitude = -latitude
current_fix.latitude = latitude
longitude = data['longitude']
if data['longitude_direction'] == 'W':
longitude = -longitude
current_fix.longitude = longitude
current_fix.altitude = float('NaN')
current_fix.position_covariance_type = \
NavSatFix.COVARIANCE_TYPE_UNKNOWN
self.fix_pub.publish(current_fix)
if not math.isnan(data['utc_time']):
current_time_ref.time_ref = rospy.Time.from_sec(data['utc_time'])
self.time_ref_pub.publish(current_time_ref)
# Publish velocity from RMC regardless, since GGA doesn't provide it.
if data['fix_valid']:
current_vel = TwistStamped()
current_vel.header.stamp = current_time
current_vel.header.frame_id = frame_id
current_vel.twist.linear.x = data['speed'] * \
math.sin(data['true_course'])
current_vel.twist.linear.y = data['speed'] * \
math.cos(data['true_course'])
current_vel.twist.angular.z = data['true_course']
self.vel_pub.publish(current_vel)
else:
return False
def talker():
ser.flush()
pub = rospy.Publisher('gps', NavSatFix, queue_size=100)
pub_n = rospy.Publisher('comb', comb, queue_size=100)
rospy.init_node('gps_node', anonymous=True)
rate = rospy.Rate(100) # 10hz
GPSmsg = NavSatFix()
combMsg = comb()
seq = 0
while not rospy.is_shutdown():
if (ser.read() != '$'):
continue
line0 = ser.readline()
cs = line0[-4:-2]
cs1 = int(cs, 16)
cs2 = checksum(line0)
print("cs1,cs2", cs1, cs2)
if (cs1 != cs2):
continue
line = line0.replace("GPFPD,", "")
line = line.replace("\r\n", "")
if ("\x00" in line):
continue
if (not string.find('*', line)):
continue
line = line.replace("*", ",")
words = string.split(line, ",")
GPSWeek = string.atoi(words[0])
GPSTime = string.atof(words[1])
Heading = string.atof(words[2])
Pitch = string.atof(words[3])
Roll = string.atof(words[4])
Lattitude = string.atof(words[5])
Longitude = string.atof(words[6])
Altitude = string.atof(words[7])
Ve = string.atof(words[8])
Vn = string.atof(words[9])
Vu = string.atof(words[10])
Baseline = string.atof(words[11])
NSV1 = string.atoi(words[12])
NSV2 = string.atoi(words[13])
Status = words[14]
combMsg.GPSWeek = GPSWeek
combMsg.GPSTime = GPSTime
combMsg.Heading = Heading
combMsg.Pitch = Pitch
combMsg.Roll = Roll
combMsg.Lattitude = Lattitude
combMsg.Longitude = Longitude
combMsg.Altitude = Altitude
combMsg.Ve = Ve
combMsg.Vn = Vn
combMsg.Vu = Vu
combMsg.Baseline = Baseline
combMsg.NSV1 = NSV1
combMsg.NSV2 = NSV2
combMsg.Status = Status
GPSmsg.latitude = Lattitude
GPSmsg.longitude = Longitude
GPSmsg.altitude = Altitude
#GPSmsg.status.status = Status
GPSmsg.header.stamp = rospy.Time.now()
GPSmsg.header.frame_id = 'gps_node'
GPSmsg.header.seq = seq
#rospy.loginfo(hello_str)
pub.publish(GPSmsg)
print("pub GPS")
pub_n.publish(combMsg)
print("pub comb")
seq = seq + 1
rate.sleep()
def talker():
pubGPS = rospy.Publisher('poseGPS', NavSatFix, queue_size=1000)
pubIMU = rospy.Publisher('imuBoat', Imu, queue_size=1000)
pubTwist = rospy.Publisher('twistGPS', TwistWithCovarianceStamped, queue_size=1000)
rospy.init_node('imugpspublisher', anonymous=True)
rate = rospy.Rate(100) # 100hz
while not rospy.is_shutdown():
data = ser.readline()
if data[0] == 'Z':
data = data.replace("Z","").replace("\n","").replace("\r","")
data_list = data.split(',')
if len(data_list) == 4:
try:
##data_list structure: lat, lon, heading, vel
float_list = [float(i) for i in data_list]
poseGPS = NavSatFix()
poseGPS.header.frame_id = "world"
poseGPS.header.stamp = rospy.Time.now()
poseGPS.status.status = 0
poseGPS.status.service = 1
poseGPS.latitude = float_list[0]
poseGPS.longitude = float_list[1]
poseGPS.altitude = 0.0
poseGPS.position_covariance = [3.24, 0, 0, 0, 3,24, 0, 0, 0, 0]
poseGPS.position_covariance_type = 2
pubGPS.publish(poseGPS)
twistGPS = TwistWithCovarianceStamped()
twistGPS.header = poseGPS.header
twistGPS.twist.twist.linear.x = float_list[3]*knots*math.cos(latest_yaw)
twistGPS.twist.twist.linear.y = float_list[3]*knots*math.sin(latest_yaw)
twistGPS.twist.twist.linear.z = 0.0
##angular data not used here
twistGPS.twist.covariance = [0.01, 0.01, 0, 0, 0, 0]
pubTwist.publish(twistGPS)
log = "GPS Data: %f %f %f %f Publish at Time: %s" % (float_list[0], float_list[1], float_list[2], float_list[3], rospy.get_time())
except: log = "GPS Data Error! Data : %s" % data
else:
log = "GPS Data Error! Data : %s" % data
rospy.loginfo(log)
elif data[0] == 'Y':
data = data.replace("Y","").replace("\n","").replace("\r","")
data_list = data.split(',')
if len(data_list) == 9:
try:
##data_list structure: accel, magni, gyro
float_list = [float(i) for i in data_list]
imuData = Imu()
imuData.header.frame_id = "base_link"
imuData.header.stamp = rospy.Time.now()
latest_yaw = float_list[3]
##data form in yaw, pitch, roll
quat = tf.transformations.quaternion_from_euler(float_list[3], float_list[4], float_list[5], 'rzyx')
imuData.orientation.x = quat[0]
imuData.orientation.y = quat[1]
imuData.orientation.z = quat[2]
imuData.orientation.w = quat[3]
imuData.angular_velocity.x = math.radians(float_list[6]*gyro_scale)
imuData.angular_velocity.y = math.radians(-float_list[7]*gyro_scale)
imuData.angular_velocity.z = math.radians(-float_list[8]*gyro_scale)
imuData.linear_acceleration.x = float_list[0]*accel_scale
imuData.linear_acceleration.y = -float_list[1]*accel_scale
imuData.linear_acceleration.z = -float_list[2]*accel_scale
imuData.orientation_covariance = [1.5838e-6, 0, 0, 0, 1.49402e-6, 0, 0, 0, 1.88934e-6]
imuData.angular_velocity_covariance = [7.84113e-7, 0, 0, 0, 5.89609e-7, 0, 0, 0, 6.20293e-7]
imuData.linear_acceleration_covariance = [9.8492e-4, 0, 0, 0, 7.10809e-4, 0, 0, 0, 1.42516e-3]
pubIMU.publish(imuData)
log = "IMU Data: %f %f %f %f %f %f %f %f %f Publish at Time: %s" \
% (imuData.linear_acceleration.x, imuData.linear_acceleration.y, imuData.linear_acceleration.z,
float_list[3], float_list[4], float_list[5],
imuData.angular_velocity.x, imuData.angular_velocity.y, imuData.angular_velocity.z, rospy.get_time())
except: log = "IMU Data Error! Data : %s" % data
else: log = "Data Error! Data : %s" % data
rospy.loginfo(log)
else:
log = "Data Error or Message: %s" % data
rospy.loginfo(log)
rate.sleep()
