def callback(self, data):
'''
Callback function, <data> is the depth image
'''
try:
time1 = time.time()
try:
frame = self.bridge.imgmsg_to_cv2(data, desired_encoding="passthrough")
except CvBridgeError as err:
print err
return
mog2_res = self.mog2.run(False,
frame.astype(np.float32))
if mog2_res is None:
return
mask1 = cv2.morphologyEx(mog2_res.copy(), cv2.MORPH_OPEN, self.open_kernel)
check_sum = np.sum(mask1 > 0)
if not check_sum or check_sum == np.sum(frame > 0):
return
_, contours, _ = cv2.findContours(mask1, cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_NONE)
cont_ind = np.argmax([cv2.contourArea(contour) for
contour in contours])
final_mask = np.zeros_like(mask1)
cv2.drawContours(final_mask, contours, cont_ind, 1, -1)
#cv2.imshow('test',mask1*255)
#cv2.waitKey(10)
frame = frame * final_mask
scores_exist,_ = self.used_classifier.run_testing(frame,
online=True)
#DEBUGGING
#cv2.imshow('test',(frame%256).astype(np.uint8))
#cv2.waitKey(10)
time2 = time.time()
self.time.append(np.mean(self.time[-9:]+[(time2-time1)]))
if (self.used_classifier.recognized_classes is not None
and len(self.used_classifier.recognized_classes)>0
and scores_exist):
self.image_pub.publish(self.bridge.cv2_to_imgmsg(
self.used_classifier.frames_preproc.hand_img, "passthrough"))
msg = TimeReference()
try:
msg.source = self.used_classifier.train_classes[
self.used_classifier.recognized_classes[-1]]
except TypeError:
msg.source = self.used_classifier.recognized_classes[-1].name
msg.time_ref = Time()
msg.time_ref.secs = int(1/float(self.time[-1]) if self.time[-1]
else 0)
self.class_pub.publish(msg)
self.skel_pub.publish(self.bridge.cv2_to_imgmsg(
np.atleast_2d(np.array(self.used_classifier.
frames_preproc.skeleton.skeleton,np.int32))))
except Exception as e:
exc_type, exc_value, exc_traceback = sys.exc_info()
traceback.print_exception(exc_type,
exc_value,
exc_traceback, limit=10, file=sys.stdout)
def callback_sbp_gps_time(self, msg, **metadata):
if self.debug:
rospy.loginfo("Received SBP_MSG_GPS_TIME (Sender: %d): %s" % (msg.sender, repr(msg)))
out = TimeReference()
out.header.frame_id = self.frame_id
out.header.stamp = rospy.Time.now()
out.time_ref = ros_time_from_sbp_time(msg)
out.source = "gps"
self.pub_time.publish(out)
def posmv_listener():
position_pub = rospy.Publisher('/posmv/position', NavSatFix, queue_size=10)
timeref_pub = rospy.Publisher('/posmv/time_reference',
TimeReference,
queue_size=10)
orientation_pub = rospy.Publisher('/posmv/orientation',
NavEulerStamped,
queue_size=10)
rospy.init_node('posmv')
pos = posmv.Posmv()
gps_week = None
gps_utc_offset = None
timestamp = datetime.datetime.utcfromtimestamp(
rospy.Time.now().to_time()).isoformat()
bag = rosbag.Bag(
'nodes/posmv_' + ('-'.join(timestamp.split(':'))) + '.bag', 'w',
rosbag.Compression.BZ2)
while not rospy.is_shutdown():
data = pos.read((1, 3))
#print data
for d in data:
if d['group_id'] == 1:
now = rospy.get_rostime()
pos_now = decode_time(d, gps_week, gps_utc_offset)
if pos_now is not None:
tref = TimeReference()
tref.header.stamp = now
tref.time_ref = rospy.Time(
calendar.timegm(pos_now.timetuple()),
pos_now.microsecond * 1000)
tref.source = 'posmv'
timeref_pub.publish(tref)
bag.write('/posmv/time_reference', tref)
nsf = NavSatFix()
nsf.header.stamp = now
nsf.header.frame_id = 'posmv'
nsf.latitude = d['latitude']
nsf.longitude = d['longitude']
nsf.altitude = d['altitude']
position_pub.publish(nsf)
bag.write('/posmv/position', nsf)
nes = NavEulerStamped()
nes.header.stamp = now
nes.header.frame_id = 'posmv'
nes.orientation.roll = d['vessel_roll']
nes.orientation.pitch = d['vessel_pitch']
nes.orientation.heading = d['vessel_heading']
orientation_pub.publish(nes)
bag.write('/posmv/orientation', nes)
if d['group_id'] == 3:
gps_week = d['gps_utc_week_number']
gps_utc_offset = d['gps_utc_time_offset']
def callback_sbp_gps_time(self, msg, **metadata):
if self.debug:
rospy.loginfo("Received SBP_MSG_GPS_TIME (Sender: %d): %s" %
(msg.sender, repr(msg)))
out = TimeReference()
out.header.frame_id = self.frame_id
out.header.stamp = rospy.Time.now()
out.time_ref = ros_time_from_sbp_time(msg)
out.source = "gps"
self.pub_time.publish(out)
def timepulse_callback(self, channel):
self.get_logger().info(f"{time.time()} Timepulse trigger")
gps_msg = NavSatFix()
timeref_msg = TimeReference()
msg_hdr = Header()
system_time = self.get_clock().now().to_msg()
msg_hdr.frame_id = 'base_link' # center of the plane
try:
ubx = self.ubp.read()
except IOError:
self.get_logger().warning("GPS disconnected. Attempting to reconnect.")
self.ubp = GPSReader(self.port, self.baud,
self.TIMEOUT, self.UBXONLY)
return
while ubx:
if (ubx.msg_cls + ubx.msg_id) == b"\x01\x07": # NAV_PVT
# <UBX(NAV-PVT, iTOW=16:50:32, year=2015, month=10, day=25, hour=16, min=50, second=48, valid=b'\xf0', tAcc=4294967295, nano=0, fixType=0, flags=b'\x00', flags2=b'$', numSV=0, lon=0, lat=0, height=0, hMSL=-17000, hAcc=4294967295, vAcc=4294967295, velN=0, velE=0, velD=0, gSpeed=0, headMot=0, sAcc=20000, headAcc=18000000, pDOP=9999, reserved1=65034815406080, headVeh=0, magDec=0, magAcc=0)>
msg_hdr.stamp = self._gen_timestamp_from_utc(ubx)
fix_stat = NavSatStatus()
if ubx.fixType == 0:
self.get_logger().warning(f"No fix yet.")
break
fix_stat.service = SERVICE_GPS
gps_msg.status = fix_stat
gps_msg.header = msg_hdr
gps_msg.latitude = float(ubx.lat)/10000000
gps_msg.longitude = float(ubx.lon)/10000000
gps_msg.altitude = float(ubx.height)/1000
timeref_msg.header = msg_hdr
timeref_msg.time_ref = system_time
timeref_msg.source = "GPS"
self.fix_pub.publish(gps_msg)
self.time_pub.publish(timeref_msg)
self.get_logger().info(f"Publishing gps message: ({timeref_msg.header.stamp.sec}.{timeref_msg.header.stamp.nanosec}): ({gps_msg.latitude}, {gps_msg.longitude}, {gps_msg.altitude})")
return
else:
self.get_logger().info(f"Other GPS MSG: {(ubx.msg_cls + ubx.msg_id)}")
ubx = self.ubp.read()
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 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 nmea_depth_udp():
# Init node
rospy.init_node("nmea_depth_udp", anonymous=True)
rospy.loginfo("[nmea_depth_udp] Initializing node...")
rate = rospy.Rate(10) # 10 Hz
# Parameters
udp_addr = rospy.get_param('~address', '')
udp_port = rospy.get_param('~port', 12021) # Random palindrome port
device_frame_id = rospy.get_param('~frame_id', "")
# Start UDP socket (defaults to any IP and port 12021)
udp_in.bind((udp_addr, udp_port))
# NMEA Sentence publisher (to publish NMEA sentence regardless of content)
sentence_pub = rospy.Publisher("%s/nmea_sentence" % device_frame_id,
Sentence,
queue_size=10)
# GPS publishers
# GPGGA - Position
# GPVTG - Velocity Track Good (ground speed)
# GPZDA - Time reference (GPST)
# GPGSA - Active Satellites
# GPGSV - Satellites in View
position_pub = rospy.Publisher("%s/gps/fix" % device_frame_id,
NavSatFix,
queue_size=10)
vel_pub = rospy.Publisher("%s/gps/vel" % device_frame_id,
TwistStamped,
queue_size=10)
timeref_pub = rospy.Publisher("%s/gps/time_reference" % device_frame_id,
TimeReference,
queue_size=10)
active_sat_pub = rospy.Publisher("%s/gps/active_satellites" %
device_frame_id,
Gpgsa,
queue_size=10)
sat_view_pub = rospy.Publisher("%s/gps/satellites_in_view" %
device_frame_id,
Gpgsv,
queue_size=10)
# Sidescanner publishers
# SDDBT - Depth Below Transducer
# SDDPT - Depth of Water
# SDMTW - Mean Temperature of Water
# SDVHW - Velocity and heading in Water
# SDHDG - Magnetic heading
depth_below_trans_pub = rospy.Publisher(
"%s/scanner/water/depth_below_transducer" % device_frame_id,
DepthBelowTransducer,
queue_size=10)
depth_water_pub = rospy.Publisher("%s/scanner/water/depth" %
device_frame_id,
DepthOfWater,
queue_size=10)
temp_water_pub = rospy.Publisher("%s/scanner/water/temperature" %
device_frame_id,
Temperature,
queue_size=10)
water_heading_speed_pub = rospy.Publisher(
"%s/scanner/water/heading_and_speed" % device_frame_id,
WaterHeadingSpeed,
queue_size=10)
mag_heading_pub = rospy.Publisher("%s/scanner/magnetic_heading" %
device_frame_id,
MagneticHeading,
queue_size=10)
rospy.loginfo("[nmea_depth_udp] Initialization done.")
# rospy.loginfo("[nmea_depth_udp] Published topics:")
# rospy.loginfo("[nmea_depth_udp] Sentence:\t\t\t%s/nmea_sentence" % device_frame_id)
# rospy.loginfo("[nmea_depth_udp] GPS Fix:\t\t\t%s/fix" % device_frame_id)
# rospy.loginfo("[nmea_depth_udp] GPS Velocity:\t\t%s/vel" % device_frame_id)
# rospy.loginfo("[nmea_depth_udp] Time Reference:\t\t%s/time_reference" % device_frame_id)
# rospy.loginfo("[nmea_depth_udp] Depth of Water:\t\t%s/depth/water" % device_frame_id)
# rospy.loginfo("[nmea_depth_udp] Depth below transducer:\t%s/depth/below_transducer" % device_frame_id)
# Run node
while not rospy.is_shutdown():
try:
nmea_in, _ = udp_in.recvfrom(1024)
except socket.error:
pass
ros_now = rospy.Time().now()
nmea_parts = nmea_in.strip().split(',')
if len(nmea_parts):
#### GPS
# GPS Fix position
if nmea_parts[0] == '$GPGGA' and len(nmea_parts) >= 10:
latitude = cast_float(
nmea_parts[2][0:2]) + cast_float(nmea_parts[2][2:]) / 60.0
if nmea_parts[3] == 'S':
latitude = -latitude
longitude = cast_float(
nmea_parts[4][0:3]) + cast_float(nmea_parts[4][3:]) / 60.0
if nmea_parts[5] == 'W':
longitude = -longitude
altitude = cast_float(nmea_parts[9])
nsf = NavSatFix()
nsf.header.stamp = ros_now
nsf.header.frame_id = device_frame_id
nsf.latitude = latitude
nsf.longitude = longitude
nsf.altitude = altitude
position_pub.publish(nsf)
# Velocity
if nmea_parts[0] == '$GPVTG' and len(nmea_parts) >= 9:
vel = TwistStamped()
vel.header.frame_id = device_frame_id
vel.header.stamp = ros_now
vel.twist.linear.x = cast_float(
nmea_parts[7]) / 3.6 # Km/h to m/s
vel_pub.publish(vel)
# Time reference (GPST)
if nmea_parts[0] == '$GPZDA' and len(nmea_parts) >= 5:
tref = TimeReference()
tref.header.frame_id = device_frame_id
tref.header.stamp = ros_now
hour = cast_int(nmea_parts[1][0:2])
minute = cast_int(nmea_parts[1][2:4])
second = cast_int(nmea_parts[1][4:6])
try:
ms = int(float(nmea_parts[1][6:]) * 1000000)
except ValueError:
ms = 0
day = cast_int(nmea_parts[2])
month = cast_int(nmea_parts[3])
year = cast_int(nmea_parts[4])
try:
zda = datetime.datetime(year, month, day, hour, minute,
second, ms)
tref.time_ref = rospy.Time(
calendar.timegm(zda.timetuple()),
zda.microsecond * 1000)
except ValueError:
pass
tref.source = device_frame_id
timeref_pub.publish(tref)
# GPS DOP and active satellites
if nmea_parts[0] == '$GPGSA' and len(nmea_parts) >= 18:
gsa = Gpgsa()
gsa.header.frame_id = device_frame_id
gsa.header.stamp = ros_now
gsa.auto_manual_mode = nmea_parts[1]
gsa.fix_mode = cast_int(nmea_parts[2])
satellite_list = []
for x in nmea_parts[3:14]:
try:
satellite_list.append(int(x))
except ValueError:
break
gsa.sv_ids = satellite_list
gsa.pdop = cast_float(nmea_parts[15])
gsa.hdop = cast_float(nmea_parts[16])
gsa.vdop = cast_float(nmea_parts[17])
active_sat_pub.publish(gsa)
# GPS Satellites in View
if nmea_parts[0] == '$GPGSV' and len(nmea_parts) >= 7:
# Typically GPGSV messages come in sequences, run and obtain messages from UDP until last message in sequence arrives
gsv = Gpgsv()
gsv.header.frame_id = device_frame_id
gsv.header.stamp = ros_now
gsv.n_msgs = cast_int(nmea_parts[1])
gsv.msg_number = cast_int(nmea_parts[2])
gsv.n_satellites = cast_int(nmea_parts[3])
for i in range(4, len(nmea_parts), 4):
gsv_sat = GpgsvSatellite()
try:
gsv_sat.prn = int(nmea_parts[i])
except ValueError:
pass
try:
gsv_sat.elevation = int(nmea_parts[i + 1])
except ValueError:
pass
try:
gsv_sat.azimuth = int(nmea_parts[i + 2])
except ValueError:
pass
try:
gsv_sat.snr = int(nmea_parts[i + 3].split("*")[0])
except ValueError:
pass
gsv.satellites.append(gsv_sat)
sat_view_pub.publish(gsv)
#### Side-scanner
# Depth (DBT - Depth Below Transducer)
if nmea_parts[0] == '$SDDBT' and len(nmea_parts) >= 7:
d = DepthBelowTransducer()
d.header.frame_id = device_frame_id
d.header.stamp = ros_now
try:
d.feet = cast_float(nmea_parts[1]) # In feet
except ValueError:
pass
try:
d.meters = cast_float(nmea_parts[3]) # In meters
except ValueError:
pass
try:
d.fathoms = cast_float(nmea_parts[5]) # In fathoms
except ValueError:
pass
depth_below_trans_pub.publish(d)
# Depth (DPT - DePTh of water)
if nmea_parts[0] == '$SDDPT' and len(nmea_parts) >= 4:
d = DepthOfWater()
d.header.frame_id = device_frame_id
d.header.stamp = ros_now
try:
d.depth = cast_float(nmea_parts[1]) # In meters
except ValueError:
pass
try:
d.offset = cast_float(nmea_parts[2]) # In meters
except ValueError:
pass
try:
d.range = cast_float(nmea_parts[3])
except ValueError:
pass
depth_water_pub.publish(d)
# SDMTW - Mean Temperature of Water
if nmea_parts[0] == '$SDMTW' and len(nmea_parts) >= 3:
tempC = Temperature()
tempC.header.frame_id = device_frame_id
tempC.header.stamp = ros_now
tempC.temperature = cast_float(nmea_parts[1])
temp_water_pub.publish(tempC)
# SDVHW - Water Heading and Speed
if nmea_parts[0] == '$SDVHW' and len(nmea_parts) >= 9:
whs = WaterHeadingSpeed()
whs.header.frame_id = device_frame_id
whs.header.stamp = ros_now
whs.true_heading = cast_float(nmea_parts[1])
whs.mag_heading = cast_float(nmea_parts[3])
whs.knots = cast_float(nmea_parts[5])
whs.kmph = cast_float(nmea_parts[7])
whs.mps = whs.kmph / 3.6 # Km/h to m/s
water_heading_speed_pub.publish(whs)
# SDHDG - Magnetic heading
if nmea_parts[0] == '$SDHDG' and len(nmea_parts) >= 6:
hdg = MagneticHeading()
hdg.header.frame_id = device_frame_id
hdg.header.stamp = ros_now
hdg.heading = cast_float(nmea_parts[1])
hdg.mag_dev = cast_float(nmea_parts[2])
hdg.mag_dev_dir = nmea_parts[3]
hdg.mag_var = cast_float(nmea_parts[4])
hdg.mag_var_dir = nmea_parts[5].split('*')[0]
quat = quaternion_from_euler(0.0, 0.0, cast_float(hdg.heading))
hdg.quaternion.x = quat[0]
hdg.quaternion.y = quat[1]
hdg.quaternion.z = quat[2]
hdg.quaternion.w = quat[3]
mag_heading_pub.publish(hdg)
# NMEA Sentence (published regardless of content)
sentence_msg = Sentence()
sentence_msg.header.frame_id = device_frame_id
sentence_msg.header.stamp = ros_now
sentence_msg.sentence = nmea_in
sentence_pub.publish(sentence_msg)
# Node sleeps for 10 Hz
rate.sleep()
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
gpsfix = self.gpsfix
gpsfix.header.stamp = current_time
gpsfix.header.frame_id = 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
elif gps_qual == 9:
# Support specifically for NOVATEL OEM4 recievers which report WAAS fix as 9
# http://www.novatel.com/support/known-solutions/which-novatel-position-types-correspond-to-the-gga-quality-indicator/
current_fix.status.status = NavSatStatus.STATUS_SBAS_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']
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)
gpsfix.hdop = data['hdop']
gpsfix.time = data['utc_time']
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'])
self.vel_pub.publish(current_vel)
gpsfix.speed = data['speed']
gpsfix.track = math.degrees(data['true_course'])
elif 'GSV' in parsed_sentence:
data = parsed_sentence['GSV']
msg = Gpgsv()
msg.header.stamp = rospy.Time.now()
msg.n_msgs = data['n_msgs']
msg.msg_number = data['msg_number']
msg.n_satellites = data['n_satellites']
gpsfix.status.satellites_visible = msg.n_satellites
if data['msg_number'] == 1:
gpsfix.status.satellite_visible_prn = []
gpsfix.status.satellite_visible_z = []
gpsfix.status.satellite_visible_azimuth = []
gpsfix.status.satellite_visible_snr = []
for i in range(0, 4):
try:
sat = GpgsvSatellite(data['prn%d' % i],
data['elevation%d' % i],
data['azimuth%d' % i],
data['snr%d' % i])
msg.satellites.append(sat)
gpsfix.status.satellite_visible_prn.append(data['prn%d' %
i])
gpsfix.status.satellite_visible_z.append(
data['elevation%d' % i])
gpsfix.status.satellite_visible_azimuth.append(
data['azimuth%d' % i])
gpsfix.status.satellite_visible_snr.append(data['snr%d' %
i])
except:
pass
self.sat_pub.publish(msg)
elif 'GSA' in parsed_sentence:
data = parsed_sentence['GSA']
gpsfix.pdop = data['pdop']
gpsfix.hdop = data['hdop']
gpsfix.vdop = data['vdop']
gpsfix.status.satellites_used = len(data['prns'])
gpsfix.status.satellite_used_prn = data['prns']
else:
return False
if ('RMC' in parsed_sentence
and self.use_RMC) or ('GGA' in parsed_sentence
and not self.use_RMC):
gpsfix.latitude = current_fix.latitude
gpsfix.longitude = current_fix.longitude
gpsfix.altitude = current_fix.altitude
gpsfix.status.status = current_fix.status.status
gpsfix.status.position_source = gpsfix.status.orientation_source = gpsfix.status.motion_source = current_fix.status.service
self.gpsfix_pub.publish(gpsfix)
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 = GPS()
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 "RMC" in parsed_sentence:
data = parsed_sentence["RMC"]
if data["fix_valid"]:
self.speed = data["speed"]
if not math.isnan(data["true_course"]):
self.ground_course = data["true_course"]
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.fix = True
current_fix.NumSat = 4
# 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.covariance = 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"]
current_fix.altitude = altitude
current_fix.speed = self.speed
current_fix.ground_course = self.ground_course
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:
current_fix.fix = True
current_fix.NumSat = 4
# 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
if data["fix_valid"]:
current_fix.speed = data["speed"]
if not math.isnan(data["true_course"]):
current_fix.ground_course = data["true_course"]
else:
current_fix.ground_course = self.ground_course
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)
else:
return False
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
navData.speed = float(fields[7]) * 0.514444444444
#print fields
gpsVel.header.stamp = timeNow
gpsVel.twist.linear.x = navData.speed * math.sin(
navData.track)
gpsVel.twist.linear.y = navData.speed * math.cos(
navData.track)
publish_vel = True
navData.header.stamp = timeNow
navData.status.position_source = GPSStatus.SOURCE_GPS
navData.status.motion_source = GPSStatus.SOURCE_GPS
publish_time = True
gpstime.header.stamp = gpsVel.header.stamp
gpstime.time_ref = convertNMEATimeToROS(fields[1])
longitude = float(
fields[5][0:3]) + float(fields[5][3:]) / 60
if fields[6] == 'W':
longitude = -longitude
latitude = float(
fields[3][0:2]) + float(fields[3][2:]) / 60
if fields[4] == 'S':
latitude = -latitude
#publish data
navData.latitude = latitude
navData.longitude = longitude
navData.altitude = float('NaN')
def nmea_depth_tcp():
# Init node
system_name = socket.gethostname()
rospy.init_node("lowrance_sonar", anonymous=True)
rospy.loginfo("[nmea_depth_tcp] Initializing node...")
# Parameters
tcp_addr = rospy.get_param('~address')
tcp_port = rospy.get_param('~port', 10110) # Lowrance standard port
update_rate = rospy.get_param(
'~update_rate', 40) # Measurement comm rate for Lowrance (Hz)
# Connect TCP client to destination
try:
tcp_in.connect((tcp_addr, tcp_port))
except IOError as exp:
rospy.logerr("Socket error: %s" % exp.strerror)
rospy.signal_shutdown(reason="Socket error: %s" % exp.strerror)
# NMEA Sentence publisher (to publish NMEA sentence regardless of content)
sentence_pub = rospy.Publisher("%s/sonar/nmea_sentence" % system_name,
Sentence,
queue_size=10)
# GPS publishers
# GPGGA - Position
# GPVTG - Velocity Track Good (ground speed)
# GPZDA - Time reference (GPST)
# GPGSA - Active Satellites
# GPGSV - Satellites in View
position_pub = rospy.Publisher("%s/sonar/gps/fix" % system_name,
NavSatFix,
queue_size=10)
vel_pub = rospy.Publisher("%s/sonar/gps/vel" % system_name,
TwistStamped,
queue_size=10)
timeref_pub = rospy.Publisher("%s/sonar/gps/time_reference" % system_name,
TimeReference,
queue_size=10)
active_sat_pub = rospy.Publisher("%s/sonar/gps/active_satellites" %
system_name,
Gpgsa,
queue_size=10)
sat_view_pub = rospy.Publisher("%s/sonar/gps/satellites_in_view" %
system_name,
Gpgsv,
queue_size=10)
# Sidescanner publishers
# SDDBT - Depth Below Transducer
# SDDPT - Depth of Water
# SDMTW - Mean Temperature of Water
# SDVHW - Velocity and heading in Water
# SDHDG - Magnetic heading
depth_below_trans_pub = rospy.Publisher(
"%s/sonar/scanner/water/depth_below_transducer" % system_name,
DepthBelowTransducer,
queue_size=10)
depth_water_pub = rospy.Publisher("%s/sonar/scanner/water/depth" %
system_name,
DepthOfWater,
queue_size=10)
temp_water_pub = rospy.Publisher("%s/sonar/scanner/water/temperature" %
system_name,
Temperature,
queue_size=10)
water_heading_speed_pub = rospy.Publisher(
"%s/sonar/scanner/water/heading_and_speed" % system_name,
WaterHeadingSpeed,
queue_size=10)
mag_heading_pub = rospy.Publisher("%s/sonar/scanner/magnetic_heading" %
system_name,
MagneticHeading,
queue_size=10)
# Diagnostics publisher
diag_pub = rospy.Publisher("/diagnostics", DiagnosticArray, queue_size=10)
rate = rospy.Rate(update_rate) # Defines the publish rate
rospy.loginfo("[nmea_depth_tcp] Initialization done.")
# rospy.loginfo("[nmea_depth_tcp] Published topics:")
# rospy.loginfo("[nmea_depth_tcp] Sentence:\t\t\t%s/nmea_sentence" % system_name)
# rospy.loginfo("[nmea_depth_tcp] GPS Fix:\t\t\t%s/fix" % system_name)
# rospy.loginfo("[nmea_depth_tcp] GPS Velocity:\t\t%s/vel" % system_name)
# rospy.loginfo("[nmea_depth_tcp] Time Reference:\t\t%s/time_reference" % system_name)
# rospy.loginfo("[nmea_depth_tcp] Depth of Water:\t\t%s/depth/water" % system_name)
# rospy.loginfo("[nmea_depth_tcp] Depth below transducer:\t%s/depth/below_transducer" % system_name)
# Run node
last_update = 0
while not rospy.is_shutdown():
try:
nmea_in = tcp_in.makefile().readline()
except socket.error:
pass
nmea_parts = nmea_in.strip().split(',')
ros_now = rospy.Time().now()
diag_msg = DiagnosticArray()
diag_msg.status.append(DiagnosticStatus())
diag_msg.status[0].name = 'sonar'
diag_msg.status[0].hardware_id = '%s' % system_name
if len(nmea_parts):
#### GPS
# GPS Fix position
if nmea_parts[0] == '$GPGGA' and len(nmea_parts) >= 10:
latitude = cast_float(
nmea_parts[2][0:2]) + cast_float(nmea_parts[2][2:]) / 60.0
if nmea_parts[3] == 'S':
latitude = -latitude
longitude = cast_float(
nmea_parts[4][0:3]) + cast_float(nmea_parts[4][3:]) / 60.0
if nmea_parts[5] == 'W':
longitude = -longitude
altitude = cast_float(nmea_parts[9])
nsf = NavSatFix()
nsf.header.stamp = ros_now
nsf.header.frame_id = system_name
nsf.latitude = latitude
nsf.longitude = longitude
nsf.altitude = altitude
position_pub.publish(nsf)
# Velocity
if nmea_parts[0] == '$GPVTG' and len(nmea_parts) >= 9:
vel = TwistStamped()
vel.header.frame_id = system_name
vel.header.stamp = ros_now
vel.twist.linear.x = cast_float(
nmea_parts[7]) / 3.6 # Km/h to m/s
vel_pub.publish(vel)
# Time reference (GPST)
if nmea_parts[0] == '$GPZDA' and len(nmea_parts) >= 5:
tref = TimeReference()
tref.header.frame_id = system_name
tref.header.stamp = ros_now
hour = cast_int(nmea_parts[1][0:2])
minute = cast_int(nmea_parts[1][2:4])
second = cast_int(nmea_parts[1][4:6])
try:
ms = int(float(nmea_parts[1][6:]) * 1000000)
except ValueError:
ms = 0
day = cast_int(nmea_parts[2])
month = cast_int(nmea_parts[3])
year = cast_int(nmea_parts[4])
try:
zda = datetime.datetime(year, month, day, hour, minute,
second, ms)
tref.time_ref = rospy.Time(
calendar.timegm(zda.timetuple()),
zda.microsecond * 1000)
except ValueError:
pass
tref.source = system_name
timeref_pub.publish(tref)
# GPS DOP and active satellites
if nmea_parts[0] == '$GPGSA' and len(nmea_parts) >= 18:
gsa = Gpgsa()
gsa.header.frame_id = system_name
gsa.header.stamp = ros_now
gsa.auto_manual_mode = nmea_parts[1]
gsa.fix_mode = cast_int(nmea_parts[2])
satellite_list = []
for x in nmea_parts[3:14]:
try:
satellite_list.append(int(x))
except ValueError:
break
gsa.sv_ids = satellite_list
gsa.pdop = cast_float(nmea_parts[15])
gsa.hdop = cast_float(nmea_parts[16])
gsa.vdop = cast_float(nmea_parts[17])
active_sat_pub.publish(gsa)
# GPS Satellites in View
if nmea_parts[0] == '$GPGSV' and len(nmea_parts) >= 7:
gsv = Gpgsv()
gsv.header.frame_id = system_name
gsv.header.stamp = ros_now
gsv.n_msgs = cast_int(nmea_parts[1])
gsv.msg_number = cast_int(nmea_parts[2])
gsv.n_satellites = cast_int(nmea_parts[3])
for i in range(4, len(nmea_parts), 4):
gsv_sat = GpgsvSatellite()
try:
gsv_sat.prn = int(nmea_parts[i])
except ValueError:
pass
try:
gsv_sat.elevation = int(nmea_parts[i + 1])
except ValueError:
pass
try:
gsv_sat.azimuth = int(nmea_parts[i + 2])
except ValueError:
pass
try:
gsv_sat.snr = int(nmea_parts[i + 3].split("*")[0])
except ValueError:
pass
gsv.satellites.append(gsv_sat)
sat_view_pub.publish(gsv)
#### Side-scanner
# Depth (DBT - Depth Below Transducer)
if nmea_parts[0] == '$SDDBT' and len(nmea_parts) >= 7:
d = DepthBelowTransducer()
d.header.frame_id = system_name
d.header.stamp = ros_now
try:
d.feet = cast_float(nmea_parts[1]) # In feet
except ValueError:
pass
try:
d.meters = cast_float(nmea_parts[3]) # In meters
except ValueError:
pass
try:
d.fathoms = cast_float(nmea_parts[5]) # In fathoms
except ValueError:
pass
depth_below_trans_pub.publish(d)
# Depth (DPT - DePTh of water)
if nmea_parts[0] == '$SDDPT' and len(nmea_parts) >= 4:
d = DepthOfWater()
d.header.frame_id = system_name
d.header.stamp = ros_now
try:
d.depth = cast_float(nmea_parts[1]) # In meters
except ValueError:
pass
try:
d.offset = cast_float(nmea_parts[2]) # In meters
except ValueError:
pass
try:
d.range = cast_float(nmea_parts[3])
except ValueError:
pass
depth_water_pub.publish(d)
# SDMTW - Mean Temperature of Water
if nmea_parts[0] == '$SDMTW' and len(nmea_parts) >= 3:
tempC = Temperature()
tempC.header.frame_id = system_name
tempC.header.stamp = ros_now
tempC.temperature = cast_float(nmea_parts[1])
temp_water_pub.publish(tempC)
# SDVHW - Water Heading and Speed
if nmea_parts[0] == '$SDVHW' and len(nmea_parts) >= 9:
whs = WaterHeadingSpeed()
whs.header.frame_id = system_name
whs.header.stamp = ros_now
whs.true_heading = cast_float(nmea_parts[1])
whs.mag_heading = cast_float(nmea_parts[3])
whs.knots = cast_float(nmea_parts[5])
whs.kmph = cast_float(nmea_parts[7])
whs.mps = whs.kmph / 3.6 # Km/h to m/s
water_heading_speed_pub.publish(whs)
# SDHDG - Magnetic heading
if nmea_parts[0] == '$SDHDG' and len(nmea_parts) >= 6:
hdg = MagneticHeading()
hdg.header.frame_id = system_name
hdg.header.stamp = ros_now
hdg.heading = cast_float(nmea_parts[1])
hdg.mag_dev = cast_float(nmea_parts[2])
hdg.mag_dev_dir = nmea_parts[3]
hdg.mag_var = cast_float(nmea_parts[4])
hdg.mag_var_dir = nmea_parts[5].split('*')[0]
quat = quaternion_from_euler(0.0, 0.0, cast_float(hdg.heading))
hdg.quaternion.x = quat[0]
hdg.quaternion.y = quat[1]
hdg.quaternion.z = quat[2]
hdg.quaternion.w = quat[3]
mag_heading_pub.publish(hdg)
# NMEA Sentence (published regardless of content)
sentence_msg = Sentence()
sentence_msg.header.frame_id = system_name
sentence_msg.header.stamp = ros_now
sentence_msg.sentence = nmea_in
sentence_pub.publish(sentence_msg)
diag_msg.status[0].level = DiagnosticStatus.OK
diag_msg.status[0].message = 'OK'
diag_msg.status[0].values = [
KeyValue(key="Sentence", value=sentence_msg.sentence)
]
last_update = ros_now
# Check for stale status
elapsed = ros_now.to_sec() - last_update.to_sec()
if elapsed > 35:
diag_msg.status[0].level = DiagnosticStatus.STALE
diag_msg.status[0].message = 'Stale'
diag_msg.status[0].values = [
KeyValue(key="Update Status", value='Stale'),
KeyValue(key="Time Since Update", value=str(elapsed))
]
# Publish diagnostics message
diag_pub.publish(diag_msg)
# Node sleeps for some time
rate.sleep()
def add_sentence_original(self, nmea_string, frame_id, timestamp=None):
if not check_nmea_checksum(nmea_string):
self.get_logger().warn("Received a sentence with an invalid checksum. " +
"Sentence was: %s" % nmea_string)
return False
parsed_sentence = parser.parse_nmea_sentence(nmea_string)
if not parsed_sentence:
self.get_logger().debug("Failed to parse NMEA sentence. Sentence was: %s" % nmea_string)
return False
if timestamp:
current_time = timestamp
else:
current_time = self.get_clock().now().to_msg()
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:
current_fix.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED
data = parsed_sentence['GGA']
fix_type = data['fix_type']
if not (fix_type in self.gps_qualities):
fix_type = -1
gps_qual = self.gps_qualities[fix_type]
default_epe = gps_qual[0]
current_fix.status.status = gps_qual[1]
current_fix.position_covariance_type = gps_qual[2]
if current_fix.status.status > 0:
self.valid_fix = True
else:
self.valid_fix = False
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
# Altitude is above ellipsoid, so adjust for mean-sea-level
altitude = data['altitude'] + data['mean_sea_level']
current_fix.altitude = altitude
# use default epe std_dev unless we've received a GST sentence with epes
if not self.using_receiver_epe or math.isnan(self.lon_std_dev):
self.lon_std_dev = default_epe
if not self.using_receiver_epe or math.isnan(self.lat_std_dev):
self.lat_std_dev = default_epe
if not self.using_receiver_epe or math.isnan(self.alt_std_dev):
self.alt_std_dev = default_epe * 2
hdop = data['hdop']
current_fix.position_covariance[0] = (hdop * self.lon_std_dev) ** 2
current_fix.position_covariance[4] = (hdop * self.lat_std_dev) ** 2
current_fix.position_covariance[8] = (2 * hdop * self.alt_std_dev) ** 2 # FIXME
print("add sentence gga")
self.trustedFix_pub.publish(current_fix)
if not math.isnan(data['utc_time']):
current_time_ref.time_ref = rclpy.time.Time(seconds=data['utc_time']).to_msg()
self.last_valid_fix_time = current_time_ref
self.time_ref_pub.publish(current_time_ref)
elif not self.use_RMC and 'VTG' in parsed_sentence:
data = parsed_sentence['VTG']
# Only report VTG data when you've received a valid GGA fix as well.
if self.valid_fix:
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)
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.trustedFix_pub.publish(current_fix)
if not math.isnan(data['utc_time']):
current_time_ref.time_ref = rclpy.time.Time(seconds=data['utc_time']).to_msg()
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'])
self.vel_pub.publish(current_vel)
elif 'GST' in parsed_sentence:
data = parsed_sentence['GST']
# Use receiver-provided error estimate if available
self.using_receiver_epe = True
self.lon_std_dev = data['lon_std_dev']
self.lat_std_dev = data['lat_std_dev']
self.alt_std_dev = data['alt_std_dev']
elif 'HDT' in parsed_sentence:
data = parsed_sentence['HDT']
if data['heading']:
current_heading = QuaternionStamped()
current_heading.header.stamp = current_time
current_heading.header.frame_id = frame_id
q = quaternion_from_euler(0, 0, math.radians(data['heading']))
current_heading.quaternion.x = q[0]
current_heading.quaternion.y = q[1]
current_heading.quaternion.z = q[2]
current_heading.quaternion.w = q[3]
self.heading_pub.publish(current_heading)
else:
return False
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. Sentence 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:
current_fix.position_covariance_type = \
NavSatFix.COVARIANCE_TYPE_APPROXIMATED
data = parsed_sentence['GGA']
fix_type = data['fix_type']
if not (fix_type in self.gps_qualities):
fix_type = -1
gps_qual = self.gps_qualities[fix_type]
default_epe = gps_qual[0]
current_fix.status.status = gps_qual[1]
current_fix.position_covariance_type = gps_qual[2]
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
# Altitude is above ellipsoid, so adjust for mean-sea-level
altitude = data['altitude'] + data['mean_sea_level']
current_fix.altitude = altitude
# use default epe std_dev unless we've received a GST sentence with epes
if not self.using_receiver_epe or math.isnan(self.lon_std_dev):
self.lon_std_dev = default_epe
if not self.using_receiver_epe or math.isnan(self.lat_std_dev):
self.lat_std_dev = default_epe
if not self.using_receiver_epe or math.isnan(self.alt_std_dev):
self.alt_std_dev = default_epe * 2
hdop = data['hdop']
current_fix.position_covariance[0] = (hdop * self.lon_std_dev)**2
current_fix.position_covariance[4] = (hdop * self.lat_std_dev)**2
current_fix.position_covariance[8] = (2 * hdop *
self.alt_std_dev)**2 # FIXME
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'])
self.vel_pub.publish(current_vel)
elif 'GST' in parsed_sentence:
data = parsed_sentence['GST']
# Use receiver-provided error estimate if available
self.using_receiver_epe = True
self.lon_std_dev = data['lon_std_dev']
self.lat_std_dev = data['lat_std_dev']
self.alt_std_dev = data['alt_std_dev']
elif 'HDT' in parsed_sentence:
data = parsed_sentence['HDT']
if data['heading']:
self.heading = math.radians(data['heading'])
current_heading = QuaternionStamped()
current_heading.header.stamp = current_time
current_heading.header.frame_id = frame_id
q = quaternion_from_euler(0, 0, math.radians(data['heading']))
current_heading.quaternion.x = q[0]
current_heading.quaternion.y = q[1]
current_heading.quaternion.z = q[2]
current_heading.quaternion.w = q[3]
self.heading_pub.publish(current_heading)
elif 'HDG' in parsed_sentence:
data = parsed_sentence['HDG']
if data['heading']:
magnetic_current_heading = QuaternionStamped()
magnetic_current_heading.header.stamp = current_time
magnetic_current_heading.header.frame_id = frame_id
q = quaternion_from_euler(0, 0, math.radians(data['heading']))
magnetic_current_heading.quaternion.x = q[0]
magnetic_current_heading.quaternion.y = q[1]
magnetic_current_heading.quaternion.z = q[2]
magnetic_current_heading.quaternion.w = q[3]
self.magnetic_heading_pub.publish(magnetic_current_heading)
elif 'VTG' in parsed_sentence:
data = parsed_sentence['VTG']
if data['speed']:
current_vel = TwistStamped()
current_vel.header.stamp = current_time
current_vel.header.frame_id = frame_id
current_vel.twist.linear.x = data['speed']
self.vel_pub.publish(current_vel)
self.vel_pub.publish(current_vel)
else:
return False
navData.header.stamp = timeNow
navData.latitude = latitude
if latitude == 0 and longitude == 0:
navData.status.status = NavSatStatus.STATUS_NO_FIX
else:
navData.status.status = NavSatStatus.STATUS_GBAS_FIX
navData.position_covariance[0] = hdop**2
navData.position_covariance[4] = hdop**2
navData.position_covariance[8] = (2*hdop)**2 #FIX ME
navData.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED
#Altitude is above ellipsoid, so adjust for mean-sea-level
navData.altitude = altitude
gpstime.header.stamp = timeNow
timeref = "{0:02d}{1:02d}{2:02d}".format(int((weekseconds % 86400) / 3600), int((weekseconds % 3600)/60), int(weekseconds % 60))
print timeref
gpstime.time_ref = convertNMEATimeToROS(timeref)
gpspub.publish(navData)
gpstimePub.publish(gpstime)
except ValueError as e:
rospy.logwarn("Value error, likely due to missing fields in the NMEA messages. Error was: %s" % e)
except rospy.ROSInterruptException:
0
quality = int(fields[5])
nr_sats = int(fields[6])
ratio = float(fields[14])
# Pose
enu.header.stamp = time_now
enu.pose.position.x = float(fields[2])
enu.pose.position.y = float(fields[3])
enu.pose.position.z = float(fields[4])
# Timeref
# Expects time as UTC
gpstime.header.stamp = time_now
t = time.strptime(fields[0] + ' ' + fields[1],
"%Y/%m/%d %H:%M:%S.%f")
gpstime.time_ref = rospy.Time.from_sec(calendar.timegm(t))
posepub.publish(enu)
timepub.publish(gpstime)
if not publish_tf: continue
# Transform
if quality in accept_quality and nr_sats >= accept_num_sats and ratio >= accept_ratio:
try:
tflisten.waitForTransform(odom_frame_id, base_frame_id,
time_now, rospy.Duration(1.0))
(trans,
rot) = tflisten.lookupTransform(odom_frame_id,
base_frame_id, time_now)
except tf.Exception:
def nmea_depth_udp():
# Init node
rospy.init_node("nmea_depth_udp", anonymous=True)
rospy.loginfo("[nmea_depth_udp] Initializing node...")
rate = rospy.Rate(10) # 10 Hz
# Parameters
udp_addr = rospy.get_param('~address', '')
udp_port = rospy.get_param('~port', 12021) # Random palindrome port
device_frame_id = rospy.get_param('~frame_id', "")
# Start UDP socket (defaults to any IP and port 12021)
udp_in.bind((udp_addr, udp_port))
# Publishers
sentence_pub = rospy.Publisher("%s/nmea_sentence" % device_frame_id,
Sentence,
queue_size=10)
position_pub = rospy.Publisher("%s/fix" % device_frame_id,
NavSatFix,
queue_size=10)
vel_pub = rospy.Publisher("%s/vel" % device_frame_id,
TwistStamped,
queue_size=10)
timeref_pub = rospy.Publisher("%s/time_reference" % device_frame_id,
TimeReference,
queue_size=10)
depth_below_trans_pub = rospy.Publisher("%s/depth/below_transducer" %
device_frame_id,
DepthBelowTransducer,
queue_size=10)
depth_water_pub = rospy.Publisher("%s/depth/water" % device_frame_id,
DepthOfWater,
queue_size=10)
rospy.loginfo("[nmea_depth_udp] Initialization done.")
rospy.loginfo("[nmea_depth_udp] Published topics:")
rospy.loginfo("[nmea_depth_udp] Sentence:\t\t\t%s/nmea_sentence" %
device_frame_id)
rospy.loginfo("[nmea_depth_udp] GPS Fix:\t\t\t%s/fix" % device_frame_id)
rospy.loginfo("[nmea_depth_udp] GPS Velocity:\t\t%s/vel" % device_frame_id)
rospy.loginfo("[nmea_depth_udp] Time Reference:\t\t%s/time_reference" %
device_frame_id)
rospy.loginfo("[nmea_depth_udp] Depth of Water:\t\t%s/depth/water" %
device_frame_id)
rospy.loginfo(
"[nmea_depth_udp] Depth below transducer:\t%s/depth/below_transducer" %
device_frame_id)
# Run node
while not rospy.is_shutdown():
try:
nmea_in, _ = udp_in.recvfrom(1024)
except socket.error:
pass
ros_now = rospy.Time().now()
nmea_parts = nmea_in.strip().split(',')
if len(nmea_parts):
try:
# GPS Fix position
if nmea_parts[0] == '$GPGGA' and len(nmea_parts) >= 10:
latitude = int(
nmea_parts[2][0:2]) + float(nmea_parts[2][2:]) / 60.0
if nmea_parts[3] == 'S':
latitude = -latitude
longitude = int(
nmea_parts[4][0:3]) + float(nmea_parts[4][3:]) / 60.0
if nmea_parts[5] == 'W':
longitude = -longitude
# altitude = float(nmea_parts[9])
nsf = NavSatFix()
nsf.header.stamp = ros_now
nsf.header.frame_id = device_frame_id
nsf.latitude = latitude
nsf.longitude = longitude
# nsf.altitude = altitude
position_pub.publish(nsf)
# Velocity
if nmea_parts[0] == '$GPVTG' and len(nmea_parts) >= 9:
vel = TwistStamped()
vel.header.frame_id = device_frame_id
vel.header.stamp = ros_now
vel.twist.linear.x = float(
nmea_parts[7]) / 3.6 # Km/h to m/s
vel_pub.publish(vel)
# Time reference (GPST)
if nmea_parts[0] == '$GPZDA' and len(nmea_parts) >= 5:
tref = TimeReference()
tref.header.frame_id = device_frame_id
tref.header.stamp = ros_now
hour = int(nmea_parts[1][0:2])
minute = int(nmea_parts[1][2:4])
second = int(nmea_parts[1][4:6])
try:
ms = int(float(nmea_parts[1][6:]) * 1000000)
except ValueError:
ms = 0
day = int(nmea_parts[2])
month = int(nmea_parts[3])
year = int(nmea_parts[4])
zda = datetime.datetime(year, month, day, hour, minute,
second, ms)
tref.time_ref = rospy.Time(
calendar.timegm(zda.timetuple()),
zda.microsecond * 1000)
tref.source = device_frame_id
timeref_pub.publish(tref)
# Depth (DBT - Depth Below Transducer)
if nmea_parts[0] == '$SDDBT' and len(nmea_parts) >= 7:
d = DepthBelowTransducer()
d.header.frame_id = device_frame_id
d.header.stamp = ros_now
try:
d.feet = float(nmea_parts[1]) # In feet
except ValueError:
pass
try:
d.meters = float(nmea_parts[3]) # In meters
except ValueError:
pass
try:
d.fathoms = float(nmea_parts[5]) # In fathoms
except ValueError:
pass
depth_below_trans_pub.publish(d)
# Depth (DPT - DePTh of water)
if nmea_parts[0] == '$SDDPT' and len(nmea_parts) >= 4:
d = DepthOfWater()
d.header.frame_id = device_frame_id
d.header.stamp = ros_now
try:
d.depth = float(nmea_parts[1]) # In meters
except ValueError:
pass
try:
d.offset = float(nmea_parts[2]) # In meters
except ValueError:
pass
try:
d.range = float(nmea_parts[3])
except ValueError:
pass
depth_water_pub.publish(d)
#### Other possible parsings (from Heck's provided logs)
# GPGLL - Geographic Latitude and Longitude (legacy sentence, same info as contained in GPGGA)
# GPGSA - Gps dillution of position and active SAtellites
# GPGSV - Gps Satellites in View
# GPRMC - Recommendec Minimum navigation type C (includes latitude, longitude, speed in knots, date... all info already available on other messages)
# SDMTW - Mean Temperature of Water
# SDVHW - Velocity and Heading in Water (Water speed in knots/kilometers-per-hour and heading in magnetic degrees)
# SDHDG - magnetic HeaDinG (in degrees, with deviation and variation)
except ValueError:
pass
# NMEA Sentence (published regardless of content)
sentence_msg = Sentence()
sentence_msg.header.frame_id = device_frame_id
sentence_msg.header.stamp = ros_now
sentence_msg.sentence = nmea_in
sentence_pub.publish(sentence_msg)
# Node sleeps for 10 Hz
rate.sleep()
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 not self.use_RMC and 'GGA' in parsed_sentence:
#rospy.loginfo('GGA')
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
# covariances diagonals as rospy arguments
current_fix.position_covariance[0] = self.gps_covariance_pos[0]
current_fix.position_covariance[4] = self.gps_covariance_pos[1]
current_fix.position_covariance[8] = self.gps_covariance_pos[2]
current_fix.position_covariance_type =\
NavSatFix.COVARIANCE_TYPE_DIAGONAL_KNOWN
# Altitude is above ellipsoid, so adjust for mean-sea-level
altitude = data['altitude'] + data['mean_sea_level']
current_fix.altitude = altitude
if not math.isnan(data['utc_time']):
current_time_ref.time_ref = rospy.Time.from_sec(int(int(timestamp.to_sec())/(60*60))*(60*60)+data['utc_time'])
self.time_ref_pub.publish(current_time_ref)
if self.use_GPS_time:
self.gps_time = current_time_ref.time_ref
current_fix.header.stamp = current_time_ref.time_ref
current_fix.header.stamp = current_fix.header.stamp+rospy.Duration(self.time_delay)
#print(timestamp.to_sec())
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']
#rospy.loginfo('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'])
self.vel_pub.publish(current_vel)
elif 'VTG' in parsed_sentence:
data = parsed_sentence['VTG']
track_made_good_degrees_true = data['track_made_good_degrees_true']
track_made_good_degrees_magnetic = data['track_made_good_degrees_magnetic']
speed = data['speed']
SPEED_OVER_GROUND = data['speed_over_ground']
if not math.isnan(track_made_good_degrees_true):
DIRECTION_REFERENCE = "True"
COURSE_OVER_GROUND = track_made_good_degrees_true
elif not math.isnan(track_made_good_degrees_magnetic):
DIRECTION_REFERENCE = "Magnetic"
COURSE_OVER_GROUND = track_made_good_degrees_magnetic
else:
DIRECTION_REFERENCE = "Null"
COURSE_OVER_GROUND = float(0)
SPEED_OVER_GROUND = float('NaN')
current_vtg = Course_Speed()
current_vtg.header.stamp = current_time
current_vtg.header.frame_id = frame_id
current_vtg.DIRECTION_REFERENCE = DIRECTION_REFERENCE
current_vtg.COURSE_OVER_GROUND = COURSE_OVER_GROUND
current_vtg.SPEED_OVER_GROUND = SPEED_OVER_GROUND
self.vtg_pub.publish(current_vtg)
#rospy.loginfo(track_made_good_degrees_magnetic)
elif 'HDT' in parsed_sentence:
#rospy.loginfo('HDT')
data = parsed_sentence['HDT']
heading_degrees = data['heading_degrees']
current_hdt = GPHDT()
current_hdt.header.stamp = current_time
current_hdt.header.frame_id = frame_id
current_hdt.HEADING_DEGREES = heading_degrees
if self.use_GPS_time and not self.gps_time==None:
current_hdt.header.stamp = self.gps_time-rospy.Duration(0,1000000)
elif self.use_GPS_time:
current_hdt.header.stamp.secs = 0
current_hdt.header.stamp = current_hdt.header.stamp+rospy.Duration(self.time_delay)+rospy.Duration(self.time_delay_heading)
#print(current_hdt.header.stamp.to_sec())
self.hdt_pub.publish(current_hdt)
else:
rospy.loginfo('Not valid')
return False
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" % 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. Sentence 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_heading = Imu()
current_heading.header.stamp = current_time
current_heading.header.frame_id = 'base_footprint'
current_direction = String() # For testing
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
# Add capture/publishing heading info
if not self.use_RMC and 'HDT' in parsed_sentence:
#rospy.loginfo("HDT!")
data = parsed_sentence['HDT']
tempHeading = data['true_heading']
ccHeading = (2 * math.pi) - tempHeading
q = tf.transformations.quaternion_from_euler(0,0,ccHeading)
current_heading.orientation.x = q[0]
current_heading.orientation.y = q[1]
current_heading.orientation.z = q[2]
current_heading.orientation.w = q[3]
#current_time_ref.time_ref = rospy.Time.from_sec(data['utc_time'])
#if (current_heading.data < .3927): current_direction.data = "N"
#elif (current_heading.data < 1.178): current_direction.data = "NE"
#elif (current_heading.data < 1.9635): current_direction.data = "E"
#elif (current_heading.data < 2.74889): current_direction.data = "SE"
#elif (current_heading.data < 3.53429): current_direction.data = "S"
#elif (current_heading.data < 4.31969): current_direction.data = "SW"
#elif (current_heading.data < 5.10509): current_direction.data = "W"
#elif (current_heading.data < 5.89048): current_direction.data = "NW"
#else: current_direction.data = "N"
self.heading_pub.publish(current_heading)
#self.direction_pub.publish(current_direction)
#self.time_ref_pub.publish(current_time_ref)
elif 'GGA' in parsed_sentence:
#rospy.loginfo("GGA!")
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']
current_fix.altitude = altitude
current_time_ref.time_ref = rospy.Time.from_sec(data['utc_time'])
self.fix_pub.publish(current_fix)
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
current_time_ref.time_ref = rospy.Time.from_sec(data['utc_time'])
self.fix_pub.publish(current_fix)
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'])
self.vel_pub.publish(current_vel)
else:
return False
def add_sentence(self,
message,
frame_id,
timestamp=None,
sentence_type="NMEA",
ubx_message_definitions=None):
"""Public method to provide a new sentence to the driver.
Args:
message (str): NMEA or UBX sentence in string form.
frame_id (str): TF frame ID of the GPS receiver.
timestamp (rospy.Time, optional): Time the sentence was received.
If timestamp is not specified, the current time is used.
sentence_type (str, optional): NMEA or UBX
ubx_message_definitions (str, optional): Message definitions for UBX sentences
Return:
bool: True if the NMEA string is successfully processed, False if there is an error.
"""
if not "UBX" == sentence_type:
if not check_nmea_checksum(message):
rospy.logwarn(
"Received a sentence with an invalid checksum. " +
"Sentence was: %s" % repr(message))
return False
parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence(
message)
if not parsed_sentence:
rospy.logdebug(
"Failed to parse NMEA sentence. Sentence was: %s" %
message)
return False
elif "UBX" == sentence_type:
parsed_sentence = libnmea_navsat_driver.parser_ubx.parse_ubx_sentence(
message, ubx_message_definitions)
if not parsed_sentence:
rospy.logdebug(
"Failed to parse UBX sentence. Sentence was: %s" % message)
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
if not self.use_GNSS_time:
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:
current_fix.position_covariance_type = \
NavSatFix.COVARIANCE_TYPE_APPROXIMATED
data = parsed_sentence['GGA']
if self.use_GNSS_time:
if math.isnan(data['utc_time'][0]):
rospy.logwarn("Time in the NMEA sentence is NOT valid")
return False
current_fix.header.stamp = rospy.Time(data['utc_time'][0],
data['utc_time'][1])
fix_type = data['fix_type']
if not (fix_type in self.gps_qualities):
fix_type = -1
gps_qual = self.gps_qualities[fix_type]
default_epe = gps_qual[0]
current_fix.status.status = gps_qual[1]
current_fix.position_covariance_type = gps_qual[2]
self.valid_fix = (fix_type > 0)
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
# Altitude is above ellipsoid, so adjust for mean-sea-level
altitude = data['altitude'] + data['mean_sea_level']
current_fix.altitude = altitude
# use default epe std_dev unless we've received a GST sentence with
# epes
if not self.using_receiver_epe or math.isnan(self.lon_std_dev):
self.lon_std_dev = default_epe
if not self.using_receiver_epe or math.isnan(self.lat_std_dev):
self.lat_std_dev = default_epe
if not self.using_receiver_epe or math.isnan(self.alt_std_dev):
self.alt_std_dev = default_epe * 2
hdop = data['hdop']
current_fix.position_covariance[0] = (hdop * self.lon_std_dev)**2
current_fix.position_covariance[4] = (hdop * self.lat_std_dev)**2
current_fix.position_covariance[8] = (2 * hdop *
self.alt_std_dev)**2 # FIXME
self.fix_pub.publish(current_fix)
if not (math.isnan(data['utc_time'][0]) or self.use_GNSS_time):
current_time_ref.time_ref = rospy.Time(data['utc_time'][0],
data['utc_time'][1])
self.last_valid_fix_time = current_time_ref
self.time_ref_pub.publish(current_time_ref)
if self.use_extended_nmea_messages:
info = NavSatInfo()
info.header.stamp = current_time
info.header.frame_id = frame_id
info.num_satellites = data['num_satellites']
info.last_dgps_update = data['age_dgps']
self.age_dgps = data['age_dgps']
info.hdop = data['hdop']
self.info_pub.publish(info)
elif not self.use_RMC and 'VTG' in parsed_sentence:
data = parsed_sentence['VTG']
# Only report VTG data when you've received a valid GGA fix as
# well.
if self.valid_fix:
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)
elif 'RMC' in parsed_sentence:
data = parsed_sentence['RMC']
if self.use_GNSS_time:
if math.isnan(data['utc_time'][0]):
rospy.logwarn("Time in the NMEA sentence is NOT valid")
return False
current_fix.header.stamp = rospy.Time(data['utc_time'][0],
data['utc_time'][1])
# 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'][0]) or self.use_GNSS_time):
current_time_ref.time_ref = rospy.Time(
data['utc_time'][0], data['utc_time'][1])
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'])
self.vel_pub.publish(current_vel)
elif 'GRS' in parsed_sentence and self.use_extended_nmea_messages:
data = parsed_sentence['GRS']
msg = NavSatGRS()
msg.header.stamp = current_time
msg.header.frame_id = frame_id
msg.utc_time = data['utc_time']
msg.mode = data['mode']
msg.residual_01 = data['residual_01']
msg.residual_02 = data['residual_02']
msg.residual_03 = data['residual_03']
msg.residual_04 = data['residual_04']
msg.residual_05 = data['residual_05']
msg.residual_06 = data['residual_06']
msg.residual_07 = data['residual_07']
msg.residual_08 = data['residual_08']
msg.residual_09 = data['residual_09']
msg.residual_10 = data['residual_10']
msg.residual_11 = data['residual_11']
# The following two attributes have been introduced with NMEA 4.11
msg.system_id = data['system_id']
msg.signal_id = data['signal_id']
self.grs_pub.publish(msg)
elif 'GST' in parsed_sentence and self.use_extended_nmea_messages:
data = parsed_sentence['GST']
# Use receiver-provided error estimate if available
self.using_receiver_epe = True
self.lon_std_dev = data['lon_std_dev']
self.lat_std_dev = data['lat_std_dev']
self.alt_std_dev = data['alt_std_dev']
msg = NavSatGST()
msg.header.stamp = current_time
msg.header.frame_id = frame_id
msg.utc_time = data['utc_time']
msg.ranges_std_dev = data['ranges_std_dev']
msg.semi_major_ellipse_std_dev = data['semi_major_ellipse_std_dev']
msg.semi_minor_ellipse_std_dev = data['semi_minor_ellipse_std_dev']
msg.semi_major_orientation = data['semi_major_orientation']
msg.lat_std_dev = data['lat_std_dev']
msg.lon_std_dev = data['lon_std_dev']
msg.alt_std_dev = data['alt_std_dev']
self.gst_pub.publish(msg)
elif 'HDT' in parsed_sentence:
data = parsed_sentence['HDT']
if data['heading']:
current_heading = QuaternionStamped()
current_heading.header.stamp = current_time
current_heading.header.frame_id = frame_id
q = quaternion_from_euler(0, 0, math.radians(data['heading']))
current_heading.quaternion.x = q[0]
current_heading.quaternion.y = q[1]
current_heading.quaternion.z = q[2]
current_heading.quaternion.w = q[3]
self.heading_pub.publish(current_heading)
elif self.use_trimble_messages and 'PTNL,VHD' in parsed_sentence:
data = parsed_sentence['PTNL,VHD']
msg = NavSatTrimbleHeading()
msg.header.stamp = current_time
msg.header.frame_id = frame_id
msg.azimuth = data['azimuth']
msg.azimuth_rate = data['azimuth_rate']
msg.vertical_angle = data['vertical_angle']
msg.vertical_angle_rate = data['vertical_angle_rate']
msg.range = data['range']
msg.range_rate = data['range_rate']
# explanation of the status codes in message definition
msg.status = data['fix_type']
msg.num_satellites = data['num_satellites']
msg.pdop = data['pdop']
self.trimble_heading_pub.publish(msg)
elif self.use_trimble_messages and 'PTNL,AVR' in parsed_sentence:
data = parsed_sentence['PTNL,AVR']
msg = NavSatTrimbleMovingBase()
msg.header.stamp = current_time
msg.header.frame_id = frame_id
msg.yaw = data['yaw']
if self.ccw_heading:
msg.yaw = -msg.yaw
msg.yaw += self.heading_offset
# wrap yaw angle to [-pi, pi)
msg.yaw = (msg.yaw + math.pi) % (2 * math.pi) - math.pi
msg.tilt = data['tilt']
msg.roll = data['roll']
msg.range = data['range']
# explanation of the status codes in message definition
msg.status = data['fix_type']
msg.pdop = data['pdop']
msg.num_satellites = data['num_satellites']
self.trimble_moving_base_pub.publish(msg)
# An dual antenna system can only measure tow out of the
# three angles tilt, roll, yaw. Yaw is always measured.
# Assuming all not measured angles are zero.
if math.isnan(msg.tilt):
msg.tilt = 0.
if math.isnan(msg.roll):
msg.roll = 0.
msg2 = Imu()
msg2.header.stamp = current_time
msg2.header.frame_id = frame_id
q = quaternion_from_euler(msg.tilt, msg.roll, msg.yaw)
msg2.orientation.x = q[0]
msg2.orientation.y = q[1]
msg2.orientation.z = q[2]
msg2.orientation.w = q[3]
# Calculate the orientation error estimate based on the position
# error estimates and the baseline. Very conservative by taking the
# largest possible error at both ends in both directions. Set high
# error if there is no proper vector fix (msg.status is not 3).
if msg.status == 3:
total_position_error = math.sqrt(self.lat_std_dev**2 +
self.lon_std_dev**2)
angular_error_estimate = math.atan2(total_position_error * 2,
msg.range)
angular_error_cov = angular_error_estimate**2
else:
angular_error_cov = 1
msg2.orientation_covariance = [
angular_error_cov, 0.0, 0.0, 0.0, angular_error_cov, 0.0, 0.0,
0.0, angular_error_cov
]
msg2.angular_velocity_covariance = [
-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
]
msg2.linear_acceleration_covariance = [
-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
]
self.trimble_moving_base_imu_pub.publish(msg2)
# Documentation source:
# https://www.u-blox.com/sites/default/files/u-blox_ZED-F9P_InterfaceDescription_%28UBX-18010854%29.pdf
# Some comments have been directly copied from the documentation
elif self.use_ublox_messages and 'PUBX,00' in parsed_sentence:
data = parsed_sentence['PUBX,00']
msg = NavSatUbloxPubxPosition()
msg.header.stamp = current_time
msg.header.frame_id = frame_id
msg.utc_time = rospy.Time(data['utc_time'][0], data['utc_time'][1])
msg.latitude = data['latitude']
if data['latitude_direction'] == 'S':
msg.latitude = -msg.latitude
msg.longitude = data['longitude']
if data['longitude_direction'] == 'W':
msg.longitude = -msg.longitude
msg.altitude = data['altitude']
msg.nav_stat = data['nav_stat']
msg.h_acc = data['h_acc']
msg.v_acc = data['v_acc']
msg.sog = data['sog'] / 3.6
msg.cog = data['cog'] / 180.0 * math.pi
msg.v_vel = data['v_vel']
msg.diff_age = data['diff_age']
msg.hdop = data['hdop']
msg.vdop = data['vdop']
msg.tdop = data['tdop']
msg.num_svs = data['num_svs']
msg.dr = data['dr']
self.ublox_pubx_position_pub.publish(msg)
# Documentation source:
# https://www.u-blox.com/sites/default/files/u-blox_ZED-F9P_InterfaceDescription_%28UBX-18010854%29.pdf
# Some comments have been directly copied from the documentation
elif self.use_ublox_messages and 'UBX-NAV-RELPOSNED' in parsed_sentence:
data = parsed_sentence['UBX-NAV-RELPOSNED']
msg = NavSatUbloxRelPos()
msg.header.stamp = current_time
msg.header.frame_id = frame_id
msg.tow = data['iTOW'] * 0.001
msg.n = data['relPosN'] * 0.01 + data['relPosHPN'] * 0.0001
msg.e = data['relPosE'] * 0.01 + data['relPosHPE'] * 0.0001
msg.d = data['relPosD'] * 0.01 + data['relPosHPD'] * 0.0001
msg.length = data['relPosLength'] * 0.01 + data[
'relPosHPLength'] * 0.0001
msg.heading = data['relPosHeading'] * 0.00001 / 180.0 * math.pi
if self.ccw_heading:
msg.heading = -msg.heading
# only add offset if heading is valid - invalid heading should always be 0.0
data['flags'] = format(data['flags'], '032b')
if 1 == int(data['flags'][23], 2):
msg.heading += self.heading_offset
# wrap yaw angle to [-pi, pi)
msg.heading = (msg.heading + math.pi) % (2 * math.pi) - math.pi
msg.acc_n = data['accN'] * 0.0001
msg.acc_e = data['accE'] * 0.0001
msg.acc_d = data['accD'] * 0.0001
msg.acc_length = data['accLength'] * 0.0001
msg.acc_heading = data['accHeading'] * 0.00001 / 180.0 * math.pi
msg.rel_pos_heading_valid = int(data['flags'][23], 2)
msg.ref_obs_miss = int(data['flags'][24], 2)
msg.ref_pos_miss = int(data['flags'][25], 2)
msg.is_moving = int(data['flags'][26], 2)
msg.carr_soln = int(data['flags'][27:29], 2)
msg.rel_pos_valid = int(data['flags'][29], 2)
msg.diff_soln = int(data['flags'][30], 2)
msg.gnss_fix_ok = int(data['flags'][31], 2)
# accuracy estimates in non RTK fixed modes shouldn't be relied on
if msg.carr_soln == 2 and msg.rel_pos_valid == 1:
self.lat_std_dev = msg.acc_n
self.lon_std_dev = msg.acc_e
self.alt_std_dev = msg.acc_d
elif msg.carr_soln == 1 and msg.rel_pos_valid == 1:
self.lat_std_dev = self.default_epe_quality5
self.lon_std_dev = self.default_epe_quality5
self.alt_std_dev = self.default_epe_quality5
else:
self.lat_std_dev = self.default_epe_quality1
self.lon_std_dev = self.default_epe_quality1
self.alt_std_dev = self.default_epe_quality1
self.ublox_relpos_pub.publish(msg)
# report erroneous messages
if msg.carr_soln != 0:
if msg.ref_obs_miss == 1 \
or msg.ref_pos_miss == 1 \
or msg.rel_pos_valid == 0 \
or msg.gnss_fix_ok == 0:
rospy.logerr(
"GNSS receiver failed to calculate GNSS fix despite available correction data. "
"Consider lowering the frequency, receiver load might be too high."
)
return False
# publish odometry and IMU messages for fusion in case message is fine
if msg.carr_soln == 0:
if msg.diff_soln == 1:
rospy.logwarn_throttle(
10, "Unable to calculate RTK solution.")
return False
msg2 = Odometry()
msg2.header.stamp = current_time
msg2.header.frame_id = frame_id
# follow enu conventions
msg2.pose.pose.position.x = msg.e
msg2.pose.pose.position.y = msg.n
msg2.pose.pose.position.z = -msg.d
msg2.pose.covariance[0] = self.lon_std_dev**2
msg2.pose.covariance[7] = self.lat_std_dev**2
msg2.pose.covariance[14] = self.alt_std_dev**2
# output heading once in moving base mode
if msg.is_moving == 1:
# take the already inverted heading from above and not the raw
# value
q = quaternion_from_euler(0., 0., msg.heading)
msg2.pose.pose.orientation.x == q[0]
msg2.pose.pose.orientation.y == q[1]
msg2.pose.pose.orientation.z == q[2]
msg2.pose.pose.orientation.w == q[3]
# degrade estimated when not in RTK fixed mode
msg2.pose.covariance[21] = msg.acc_heading**2 + (2 -
msg.carr_soln)
msg2.pose.covariance[28] = msg.acc_heading**2 + (2 -
msg.carr_soln)
msg2.pose.covariance[35] = msg.acc_heading**2 + (2 -
msg.carr_soln)
else:
msg2.pose.covariance[21] = -1.0
msg2.pose.covariance[28] = -1.0
msg2.pose.covariance[35] = -1.0
self.ublox_relpos_odom_pub.publish(msg2)
# Publish IMU message if in moving base mode
if msg.is_moving == 1:
msg3 = Imu()
msg3.header.stamp = current_time
msg3.header.frame_id = frame_id
xyzw = quaternion_from_euler(0.0, 0.0, msg.heading)
msg3.orientation.x = xyzw[0]
msg3.orientation.y = xyzw[1]
msg3.orientation.z = xyzw[2]
msg3.orientation.w = xyzw[3]
if msg.carr_soln == 2:
angular_error_cov = msg.acc_heading**2
else:
angular_error_cov = msg.acc_heading**2 + (2 -
msg.carr_soln)
msg3.orientation_covariance = [
angular_error_cov, 0.0, 0.0, 0.0, angular_error_cov, 0.0,
0.0, 0.0, angular_error_cov
]
msg3.angular_velocity_covariance = [
-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
]
msg3.linear_acceleration_covariance = [
-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
]
self.ublox_relpos_imu_pub.publish(msg3)
elif self.use_ublox_messages and 'UBX-NAV-GEOFENCE' in parsed_sentence:
data = parsed_sentence['UBX-NAV-GEOFENCE']
msg = NavSatUbloxGeoFence()
msg.header.stamp = current_time
msg.header.frame_id = frame_id
msg.tow = data['iTOW'] * 0.001
msg.status = data['status']
msg.num_fences = data['numFences']
msg.comb_state = data['combState']
msg.status = data['status']
# The original message is variable length, so all fences are
# initialized with "deactivated" state.
msg.fence1 = NavSatUbloxGeoFence.DEACTIVATED
msg.fence2 = NavSatUbloxGeoFence.DEACTIVATED
msg.fence3 = NavSatUbloxGeoFence.DEACTIVATED
msg.fence4 = NavSatUbloxGeoFence.DEACTIVATED
try:
msg.fence1 = data['state'][0]
msg.fence2 = data['state'][1]
msg.fence3 = data['state'][2]
msg.fence4 = data['state'][3]
except KeyError:
pass
self.ublox_geofence_pub.publish(msg)
elif self.use_ublox_messages and 'UBX-NAV-PVT' in parsed_sentence:
data = parsed_sentence['UBX-NAV-PVT']
msg = NavSatUbloxPositionVelocityTime()
msg.header.stamp = current_time
msg.header.frame_id = frame_id
msg.tow = data['iTOW'] * 0.001
msg.year = data['year']
msg.month = data['month']
msg.day = data['day']
msg.hour = data['hour']
msg.min = data['min']
msg.sec = data['sec']
bitfield_valid = format(data['valid'], '08b')
msg.valid_date = int(bitfield_valid[7], 2)
msg.valid_time = int(bitfield_valid[6], 2)
msg.fully_resolved = int(bitfield_valid[5], 2)
msg.valid_mag = int(bitfield_valid[4], 2)
msg.t_acc = data['tAcc'] / 1.e9
msg.nano = data['nano']
msg.fix_type = data['fixType']
bitfield_flags = format(data['flags'], '08b')
msg.gnss_fix_ok = int(bitfield_flags[7], 2)
msg.diff_soln = int(bitfield_flags[6], 2)
msg.psm_state = int(bitfield_flags[3:6], 2)
msg.head_veh_valid = int(bitfield_flags[2], 2)
msg.carr_soln = int(bitfield_flags[0:2], 2)
bitfield_flags2 = format(data['flags2'], '08b')
msg.confirmed_avai = int(bitfield_flags2[2], 2)
msg.confirmed_date = int(bitfield_flags2[1], 2)
msg.confirmed_time = int(bitfield_flags2[0], 2)
msg.num_sv = data['numSV']
msg.lon = data['lon'] / 1.e7
msg.lat = data['lat'] / 1.e7
msg.height = data['height'] / 1.e3
msg.h_msl = data['hMSL'] / 1.e3
msg.h_acc = data['hAcc'] / 1.e3
msg.v_acc = data['vAcc'] / 1.e3
msg.vel_n = data['velN'] / 1.e3
msg.vel_e = data['velE'] / 1.e3
msg.vel_d = data['velD'] / 1.e3
msg.g_speed = data['gSpeed'] / 1.e3
msg.head_mot = data['headMot'] / 1.e5 / 180.0 * math.pi
msg.s_acc = data['sAcc'] / 1.e3
msg.head_acc = data['headAcc'] / 1.e5 / 180.0 * math.pi
msg.p_dop = data['pDOP'] / 1.e2
msg.head_veh = data['headVeh'] / 1.e5 / 180.0 * math.pi
msg.mag_dec = data['magDec'] / 1.e2 / 180.0 * math.pi
msg.mag_acc = data['magAcc'] / 1.e2 / 180.0 * math.pi
self.ublox_position_velocity_time_pub.publish(msg)
elif self.use_ublox_messages and 'UBX-RAW' in parsed_sentence:
data = parsed_sentence['UBX-RAW']
msg = String()
# transmitting binary data (bytes) directly is not possible anymore
# with ROS and Python3, therefore base64 encoding is applied
msg.data = base64.b64encode(data['raw']).decode('ascii')
self.ublox_raw.publish(msg)
else:
return False
#if not satellite lock message parse it separately
else:
if GPSLock == True:
if 'RMC' in fields[0]:
#print fields
gpsVel.header.stamp = timeNow
gpsVel.twist.linear.x = float(fields[7])*0.514444444444*math.sin(float(fields[8]))
gpsVel.twist.linear.y = float(fields[7])*0.514444444444*math.cos(float(fields[8]))
gpsVelPub.publish(gpsVel)
navData.status.status = NavSatStatus.STATUS_FIX
navData.header.stamp = gpsVel.header.stamp
navData.status.service = NavSatStatus.SERVICE_GPS
gpstime.header.stamp = gpsVel.header.stamp
gpstime.time_ref = convertNMEATimeToROS(fields[1])
longitude = float(fields[5][0:3]) + float(fields[5][3:])/60
if fields[6] == 'W':
longitude = -longitude
latitude = float(fields[3][0:2]) + float(fields[3][2:])/60
if fields[4] == 'S':
latitude = -latitude
#publish data
navData.latitude = latitude
navData.longitude = longitude
navData.altitude = float('NaN')
navData.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
gpspub.publish(navData)
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" % 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. Sentence 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_heading = Imu()
current_heading.header.stamp = current_time
current_heading.header.frame_id = 'base_footprint'
current_direction = String() # For testing
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
# Add capture/publishing heading info
if not self.use_RMC and 'HDT' in parsed_sentence:
#rospy.loginfo("HDT!")
data = parsed_sentence['HDT']
tempHeading = data['true_heading']
ccHeading = (2 * math.pi) - tempHeading
q = tf.transformations.quaternion_from_euler(0, 0, ccHeading)
current_heading.orientation.x = q[0]
current_heading.orientation.y = q[1]
current_heading.orientation.z = q[2]
current_heading.orientation.w = q[3]
#current_time_ref.time_ref = rospy.Time.from_sec(data['utc_time'])
#if (current_heading.data < .3927): current_direction.data = "N"
#elif (current_heading.data < 1.178): current_direction.data = "NE"
#elif (current_heading.data < 1.9635): current_direction.data = "E"
#elif (current_heading.data < 2.74889): current_direction.data = "SE"
#elif (current_heading.data < 3.53429): current_direction.data = "S"
#elif (current_heading.data < 4.31969): current_direction.data = "SW"
#elif (current_heading.data < 5.10509): current_direction.data = "W"
#elif (current_heading.data < 5.89048): current_direction.data = "NW"
#else: current_direction.data = "N"
self.heading_pub.publish(current_heading)
#self.direction_pub.publish(current_direction)
#self.time_ref_pub.publish(current_time_ref)
elif 'GGA' in parsed_sentence:
#rospy.loginfo("GGA!")
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']
current_fix.altitude = altitude
current_time_ref.time_ref = rospy.Time.from_sec(data['utc_time'])
self.fix_pub.publish(current_fix)
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
current_time_ref.time_ref = rospy.Time.from_sec(
data['utc_time'])
self.fix_pub.publish(current_fix)
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'])
self.vel_pub.publish(current_vel)
else:
return False
def posmv_nmea_listener():
position_pub = rospy.Publisher('/base/position', NavSatFix, queue_size=10)
timeref_pub = rospy.Publisher('/base/time_reference',
TimeReference,
queue_size=10)
orientation_pub = rospy.Publisher('/base/orientation',
NavEulerStamped,
queue_size=10)
rospy.init_node('posmv_nmea')
input_type = rospy.get_param('/posmv_nmea/input_type')
input_address = rospy.get_param('/posmv_nmea/input', '')
input_speed = rospy.get_param('/posmv_nmea/input_speed', 0)
input_port = int(rospy.get_param('/posmv_nmea/input_port', 0))
output_port = int(rospy.get_param('/posmv_nmea/output', 0))
output_address = rospy.get_param('/posmv_nmea/output_address',
'<broadcast>')
if input_type == 'serial':
serial_in = serial.Serial(input_address, int(input_speed))
else:
udp_in = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
udp_in.bind(('', input_port))
if output_port > 0:
udp_out = socket.socket(socket.AF_INET, socket.SOCK_DGRAM,
socket.IPPROTO_UDP)
udp_out.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
else:
udp_out = None
timestamp = datetime.datetime.utcfromtimestamp(
rospy.Time.now().to_time()).isoformat()
bag = rosbag.Bag(
'nodes/posmv_nmea_' + ('-'.join(timestamp.split(':'))) + '.bag', 'w',
rosbag.Compression.BZ2)
while not rospy.is_shutdown():
if input_type == 'serial':
nmea_in = serial_in.readline()
#print nmea_in
if udp_out is not None:
udp_out.sendto(nmea_in, (output_address, output_port))
else:
nmea_in, addr = udp_in.recvfrom(1024)
#print addr, nmea_in
now = rospy.get_rostime()
nmea_parts = nmea_in.strip().split(',')
if len(nmea_parts):
#print nmea_parts
try:
if nmea_parts[0] == '$GPZDA' and len(nmea_parts) >= 5:
tref = TimeReference()
tref.header.stamp = now
hour = int(nmea_parts[1][0:2])
minute = int(nmea_parts[1][2:4])
second = int(nmea_parts[1][4:6])
ms = int(float(nmea_parts[1][6:]) * 1000000)
day = int(nmea_parts[2])
month = int(nmea_parts[3])
year = int(nmea_parts[4])
zda = datetime.datetime(year, month, day, hour, minute,
second, ms)
tref.time_ref = rospy.Time(
calendar.timegm(zda.timetuple()),
zda.microsecond * 1000)
tref.source = 'posmv'
timeref_pub.publish(tref)
bag.write('/posmv_nmea/time_reference', tref)
if nmea_parts[0] == '$GPGGA' and len(nmea_parts) >= 10:
latitude = int(
nmea_parts[2][0:2]) + float(nmea_parts[2][2:]) / 60.0
if nmea_parts[3] == 'S':
latitude = -latitude
longitude = int(
nmea_parts[4][0:3]) + float(nmea_parts[4][3:]) / 60.0
if nmea_parts[5] == 'W':
longitude = -longitude
altitude = float(nmea_parts[9])
nsf = NavSatFix()
nsf.header.stamp = now
nsf.header.frame_id = 'posmv_operator'
nsf.latitude = latitude
nsf.longitude = longitude
nsf.altitude = altitude
position_pub.publish(nsf)
bag.write('/posmv_nmea/position', nsf)
if nmea_parts[0] == '$GPHDT' and len(nmea_parts) >= 2:
heading = float(nmea_parts[1])
nes = NavEulerStamped()
nes.header.stamp = now
nes.header.frame_id = 'posmv_operator'
nes.orientation.heading = heading
orientation_pub.publish(nes)
bag.write('/posmv_nmea/orientation', nes)
except ValueError:
pass
bag.close()
def ais_listener(logdir=None):
rospy.init_node('ais')
position_pub = rospy.Publisher('project11/ais/position',NavSatFix,queue_size=10)
timeref_pub = rospy.Publisher('project11/ais/time_reference',TimeReference,queue_size=10)
orientation_pub = rospy.Publisher('project11/ais/orientation', Imu, queue_size=10)
velocity_pub = rospy.Publisher('project11/ais/velocity', TwistWithCovarianceStamped, queue_size=10)
ais_pub = rospy.Publisher('project11/ais/contact',Contact,queue_size=10)
ais_raw_pub = rospy.Publisher('project11/ais/raw',Heartbeat,queue_size=10)
input_type = rospy.get_param('~input_type')
input_address = rospy.get_param('~input','')
input_speed = rospy.get_param('~input_speed',0)
input_port = int(rospy.get_param('~input_port',0))
output_port = int(rospy.get_param('~output',0))
output_address = rospy.get_param('~output_address','<broadcast>')
frame_id = rospy.get_param("~frame_id",'ais')
ais_decoder = ais.decoder.AISDecoder()
if logdir is not None:
logfile = open(logdir+'ais_'+'.'.join(datetime.datetime.utcnow().isoformat().split(':'))+'.log','w')
else:
logfile = None
if input_type == 'serial':
serial_in = serial.Serial(input_address, int(input_speed))
else:
udp_in = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
udp_in.settimeout(0.1)
udp_in.bind(('',input_port))
if output_port > 0:
udp_out = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP)
udp_out.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
else:
udp_out = None
while not rospy.is_shutdown():
if input_type == 'serial':
nmea_ins = (serial_in.readline(),)
#print( nmea_ins)
if udp_out is not None:
udp_out.sendto(nmea_in, (output_address,output_port))
else:
try:
nmea_in,addr = udp_in.recvfrom(2048)
#print addr, nmea_in
nmea_ins = nmea_in.decode('utf-8').split('\n')
except socket.timeout:
nmea_ins = None
now = rospy.get_rostime()
if nmea_ins is not None:
for nmea_in_b in nmea_ins:
try:
nmea_in = nmea_in_b.decode('utf-8')
except AttributeError:
nmea_in = nmea_in_b
#print(nmea_in)
if logfile is not None:
logfile.write(datetime.datetime.utcnow().isoformat()+','+nmea_in+'\n')
if nmea_in.startswith('!AIVDM'):
#print(nmea_in)
ais_decoder.addNMEA(nmea_in.strip())
msgs = ais_decoder.popMessages()
#print(msgs)
for m in msgs:
if m['type'] in (1,2,3,18,19): #position reports
c = Contact()
c.header.stamp = now
c.header.frame_id = frame_id
c.mmsi = m['mmsi']
if 'shipname' in ais_decoder.mmsi_db[m['mmsi']]:
c.name = ais_decoder.mmsi_db[m['mmsi']]['shipname']
if 'callsign' in ais_decoder.mmsi_db[m['mmsi']]:
c.callsign = ais_decoder.mmsi_db[m['mmsi']]['callsign']
c.position.latitude = m['lat']
c.position.longitude = m['lon']
if m['course'] is not None:
c.cog = math.radians(m['course'])
else:
c.cog = -1
if m['speed'] is not None:
c.sog = m['speed']*0.514444 #knots to m/s
else:
c.sog = -1
if m['heading'] is not None:
c.heading = math.radians(m['heading'])
else:
c.heading = -1
if 'to_bow' in ais_decoder.mmsi_db[m['mmsi']]:
c.dimension_to_bow = ais_decoder.mmsi_db[m['mmsi']]['to_bow']
if 'to_stern' in ais_decoder.mmsi_db[m['mmsi']]:
c.dimension_to_stern = ais_decoder.mmsi_db[m['mmsi']]['to_stern']
if 'to_port' in ais_decoder.mmsi_db[m['mmsi']]:
c.dimension_to_port = ais_decoder.mmsi_db[m['mmsi']]['to_port']
if 'to_starboard' in ais_decoder.mmsi_db[m['mmsi']]:
c.dimension_to_stbd = ais_decoder.mmsi_db[m['mmsi']]['to_starboard']
ais_pub.publish(c)
raw = Heartbeat()
for k,v in m.items():
raw.values.append(KeyValue(k,str(v)))
ais_raw_pub.publish(raw)
else:
nmea_parts = nmea_in.strip().split(',')
if len(nmea_parts):
#print nmea_parts
try:
if nmea_parts[0][3:] == 'ZDA' and len(nmea_parts) >= 5:
tref = TimeReference()
tref.header.stamp = now
tref.header.frame_id = frame_id
hour = int(nmea_parts[1][0:2])
minute = int(nmea_parts[1][2:4])
second = int(nmea_parts[1][4:6])
ms = int(float(nmea_parts[1][6:])*1000000)
day = int(nmea_parts[2])
month = int(nmea_parts[3])
year = int(nmea_parts[4])
zda = datetime.datetime(year,month,day,hour,minute,second,ms)
tref.time_ref = rospy.Time(calendar.timegm(zda.timetuple()),zda.microsecond*1000)
tref.source = 'ais'
timeref_pub.publish(tref)
if nmea_parts[0][3:] == 'GGA' and len(nmea_parts) >= 10:
latitude = int(nmea_parts[2][0:2])+float(nmea_parts[2][2:])/60.0
if nmea_parts[3] == 'S':
latitude = -latitude
longitude = int(nmea_parts[4][0:3])+float(nmea_parts[4][3:])/60.0
if nmea_parts[5] == 'W':
longitude = -longitude
altitude = float(nmea_parts[9])
nsf = NavSatFix()
nsf.header.stamp = now
nsf.header.frame_id = frame_id
nsf.latitude = latitude
nsf.longitude = longitude
nsf.altitude = altitude
position_pub.publish(nsf)
if nmea_parts[0][3:] == 'HDT' and len(nmea_parts) >= 2:
heading = float(nmea_parts[1])
imu = Imu()
imu.header.stamp = now
imu.header.frame_id = frame_id
q = tf.transformations.quaternion_from_euler(math.radians(90.0-heading), 0, 0, 'rzyx')
imu.orientation.x = q[0]
imu.orientation.y = q[1]
imu.orientation.z = q[2]
imu.orientation.w = q[3]
orientation_pub.publish(imu)
except ValueError:
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 = 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 = GPS()
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 'RMC' in parsed_sentence:
data = parsed_sentence['RMC']
if data['fix_valid']:
self.speed = data['speed']
if not math.isnan(data['true_course']):
self.ground_course = data['true_course']
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.fix = True
current_fix.NumSat = data['num_satellites']
# 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.covariance = 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']
current_fix.altitude = altitude
current_fix.speed = self.speed
current_fix.ground_course = self.ground_course
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:
current_fix.fix = True
current_fix.NumSat = data['num_satellites']
# 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
if data['fix_valid']:
current_fix.speed = data['speed']
if not math.isnan(data['true_course']):
current_fix.ground_course = data['true_course']
else:
current_fix.ground_course = self.ground_course
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)
else:
return False
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 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. Sentence 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_pose_utm = GpsLocal()
current_pose_utm.header.stamp = current_time
current_pose_utm.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:
self.seq = self.seq + 1
current_fix.position_covariance_type = \
NavSatFix.COVARIANCE_TYPE_APPROXIMATED
data = parsed_sentence['GGA']
gps_qual = data['fix_type']
try:
# Unpack the fix params for this quality value
current_fix.status.status, self.default_epe, self.fix_type = self.fix_mappings[
gps_qual]
except KeyError:
current_fix.status.status = NavSatStatus.STATUS_NO_FIX
self.fix_type = 'Unknown'
if gps_qual == 0:
current_fix.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
self.using_receiver_epe = False
self.invalid_cnt += 1
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
# use default epe std_dev unless we've received a GST sentence with epes
if not self.using_receiver_epe or math.isnan(self.lon_std_dev):
self.lon_std_dev = self.default_epe
if not self.using_receiver_epe or math.isnan(self.lat_std_dev):
self.lat_std_dev = self.default_epe
if not self.using_receiver_epe or math.isnan(self.alt_std_dev):
self.alt_std_dev = self.default_epe * 2
hdop = data['hdop']
current_fix.position_covariance[0] = (hdop * self.lon_std_dev)**2
current_fix.position_covariance[4] = (hdop * self.lat_std_dev)**2
current_fix.position_covariance[8] = (hdop *
self.alt_std_dev)**2 # FIXME
# Altitude is above ellipsoid, so adjust for mean-sea-level
altitude = data['altitude'] + data['mean_sea_level']
current_fix.altitude = altitude
self.fix_pub.publish(current_fix)
if not math.isnan(latitude) and not math.isnan(longitude) and (
gps_qual == 5 or gps_qual == 4):
UTMNorthing, UTMEasting = LLtoUTM(latitude, longitude)[0:2]
current_pose_utm.position.x = UTMEasting
current_pose_utm.position.y = UTMNorthing
current_pose_utm.position.z = altitude
# Pose x/y/z covariance is whatever we decided h & v covariance is.
# Here is it the same as for ECEF coordinates
current_pose_utm.covariance[0] = (hdop * self.lon_std_dev)**2
current_pose_utm.covariance[4] = (hdop * self.lat_std_dev)**2
current_pose_utm.covariance[8] = (hdop * self.alt_std_dev)**2
current_pose_utm.rtk_fix = True if gps_qual == 4 else False
self.local_pub.publish(current_pose_utm)
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)
if (self.diag_pub_time < rospy.get_time()):
self.diag_pub_time += 1
diag_arr = DiagnosticArray()
diag_arr.header.stamp = rospy.get_rostime()
diag_arr.header.frame_id = frame_id
diag_msg = DiagnosticStatus()
diag_msg.name = 'GPS_status'
diag_msg.hardware_id = 'GPS'
diag_msg.level = DiagnosticStatus.OK
diag_msg.message = 'Received GGA Fix'
diag_msg.values.append(
KeyValue('Sequence number', str(self.seq)))
diag_msg.values.append(
KeyValue('Invalid fix count', str(self.invalid_cnt)))
diag_msg.values.append(KeyValue('Latitude', str(latitude)))
diag_msg.values.append(KeyValue('Longitude', str(longitude)))
diag_msg.values.append(KeyValue('Altitude', str(altitude)))
diag_msg.values.append(KeyValue('GPS quality', str(gps_qual)))
diag_msg.values.append(KeyValue('Fix type', self.fix_type))
diag_msg.values.append(
KeyValue('Number of satellites',
str(data['num_satellites'])))
diag_msg.values.append(
KeyValue('Receiver providing accuracy',
str(self.using_receiver_epe)))
diag_msg.values.append(KeyValue('Hdop', str(hdop)))
diag_msg.values.append(
KeyValue('Latitude std dev', str(hdop * self.lat_std_dev)))
diag_msg.values.append(
KeyValue('Longitude std dev',
str(hdop * self.lon_std_dev)))
diag_msg.values.append(
KeyValue('Altitude std dev', str(hdop * self.alt_std_dev)))
diag_arr.status.append(diag_msg)
self.diag_pub.publish(diag_arr)
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'])
self.vel_pub.publish(current_vel)
elif 'GST' in parsed_sentence:
data = parsed_sentence['GST']
# Use receiver-provided error estimate if available
self.using_receiver_epe = True
self.lon_std_dev = data['lon_std_dev']
self.lat_std_dev = data['lat_std_dev']
self.alt_std_dev = data['alt_std_dev']
else:
return False
def add_sentence(self, nmea_string, frame_id, timestamp=None):
"""Public method to provide a new NMEA sentence to the driver.
Args:
nmea_string (str): NMEA sentence in string form.
frame_id (str): TF frame ID of the GPS receiver.
timestamp(rospy.Time, optional): Time the sentence was received.
If timestamp is not specified, the current time is used.
Returns:
bool: True if the NMEA string is successfully processed, False if there is an error.
"""
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. Sentence 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
if not self.use_GNSS_time:
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']
if self.use_GNSS_time:
if math.isnan(data['utc_time'][0]):
rospy.logwarn("Time in the NMEA sentence is NOT valid")
return False
current_fix.header.stamp = rospy.Time(data['utc_time'][0], data['utc_time'][1])
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
elif gps_qual == 9:
# Support specifically for NOVATEL OEM4 recievers which report WAAS fix as 9
# http://www.novatel.com/support/known-solutions/which-novatel-position-types-correspond-to-the-gga-quality-indicator/
current_fix.status.status = NavSatStatus.STATUS_SBAS_FIX
else:
current_fix.status.status = NavSatStatus.STATUS_NO_FIX
if gps_qual > 0:
self.valid_fix = True
else:
self.valid_fix = False
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']
current_fix.altitude = altitude
self.fix_pub.publish(current_fix)
if not (math.isnan(data['utc_time'][0]) or self.use_GNSS_time):
current_time_ref.time_ref = rospy.Time(
data['utc_time'][0], data['utc_time'][1])
self.last_valid_fix_time = current_time_ref
self.time_ref_pub.publish(current_time_ref)
elif not self.use_RMC and 'VTG' in parsed_sentence:
data = parsed_sentence['VTG']
# Only report VTG data when you've received a valid GGA fix as
# well.
if self.valid_fix:
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)
elif 'RMC' in parsed_sentence:
data = parsed_sentence['RMC']
if self.use_GNSS_time:
if math.isnan(data['utc_time'][0]):
#rospy.logwarn("Time in the NMEA sentence is NOT valid")
return False
current_fix.header.stamp = rospy.Time(data['utc_time'][0], data['utc_time'][1])
# 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'][0]) or self.use_GNSS_time):
current_time_ref.time_ref = rospy.Time(
data['utc_time'][0], data['utc_time'][1])
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'])
self.vel_pub.publish(current_vel)
else:
return False
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. Sentence was: %s" % nmea_string)
return False
if timestamp:
current_time = timestamp
else:
current_time = rospy.get_rostime()
current_fix = NavSatFix()
current_fix.header.seq = self.seq
self.seq += 1
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:
current_fix.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED
data = parsed_sentence['GGA']
fix_type = data['fix_type']
if not (fix_type in self.gps_qualities):
fix_type = -1
gps_qual = self.gps_qualities[fix_type]
default_epe = gps_qual[0];
current_fix.status.status = gps_qual[1]
current_fix.position_covariance_type = gps_qual[2];
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
# Altitude is above ellipsoid, so adjust for mean-sea-level
altitude = data['altitude'] + data['mean_sea_level']
current_fix.altitude = altitude
# use default epe std_dev unless we've received a GST sentence with epes
if not self.using_receiver_epe or math.isnan(self.lon_std_dev):
self.lon_std_dev = default_epe
if not self.using_receiver_epe or math.isnan(self.lat_std_dev):
self.lat_std_dev = default_epe
if not self.using_receiver_epe or math.isnan(self.alt_std_dev):
self.alt_std_dev = default_epe * 2
hdop = data['hdop']
current_fix.position_covariance[0] = (hdop * self.lon_std_dev) ** 2
current_fix.position_covariance[4] = (hdop * self.lat_std_dev) ** 2
current_fix.position_covariance[8] = (2 * hdop * self.alt_std_dev) ** 2 # FIXME
self.fix_pub.publish(current_fix)
# Publish GPGGA msgs
gpgga_msg = Gpgga()
gpgga_msg.header.stamp = current_time
gpgga_msg.header.frame_id = frame_id
gpgga_msg.message_id = data['sent_id']
gpgga_msg.utc_seconds = data['utc_time']
gpgga_msg.lat = latitude
gpgga_msg.lon = longitude
gpgga_msg.lat_dir = data['latitude_direction']
gpgga_msg.lon_dir = data['longitude_direction']
gpgga_msg.gps_qual = data['fix_type']
gpgga_msg.num_sats = data['num_satellites']
gpgga_msg.hdop = hdop
gpgga_msg.alt = data['altitude']
gpgga_msg.altitude_units = data['altitude_units']
gpgga_msg.undulation = data['mean_sea_level']
gpgga_msg.undulation_units = data['mean_sea_level_units']
gpgga_msg.diff_age = data['diff_age']*10
gpgga_msg.station_id = data['station_id']
self.gpgga_pub.publish(gpgga_msg)
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'])
self.vel_pub.publish(current_vel)
elif 'GST' in parsed_sentence:
data = parsed_sentence['GST']
# Use receiver-provided error estimate if available
self.using_receiver_epe = True
self.lon_std_dev = data['lon_std_dev']
self.lat_std_dev = data['lat_std_dev']
self.alt_std_dev = data['alt_std_dev']
else:
return False
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. Sentence 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:
current_fix.position_covariance_type = \
NavSatFix.COVARIANCE_TYPE_APPROXIMATED
data = parsed_sentence['GGA']
fix_type = data['fix_type']
if not (fix_type in self.gps_qualities):
fix_type = -1
gps_qual = self.gps_qualities[fix_type]
default_epe = gps_qual[0]
current_fix.status.status = gps_qual[1]
current_fix.position_covariance_type = gps_qual[2]
if gps_qual > 0:
self.valid_fix = True
else:
self.valid_fix = False
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
# Altitude is above ellipsoid, so adjust for mean-sea-level
altitude = data['altitude'] + data['mean_sea_level']
current_fix.altitude = altitude
# use default epe std_dev unless we've received a GST sentence with
# epes
if not self.using_receiver_epe or math.isnan(self.lon_std_dev):
self.lon_std_dev = default_epe
if not self.using_receiver_epe or math.isnan(self.lat_std_dev):
self.lat_std_dev = default_epe
if not self.using_receiver_epe or math.isnan(self.alt_std_dev):
self.alt_std_dev = default_epe * 2
hdop = data['hdop']
current_fix.position_covariance[0] = (hdop * self.lon_std_dev) ** 2
current_fix.position_covariance[4] = (hdop * self.lat_std_dev) ** 2
current_fix.position_covariance[8] = (
2 * hdop * self.alt_std_dev) ** 2 # FIXME
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.last_valid_fix_time = current_time_ref
self.time_ref_pub.publish(current_time_ref)
elif not self.use_RMC and 'VTG' in parsed_sentence:
data = parsed_sentence['VTG']
# Only report VTG data when you've received a valid GGA fix as
# well.
if self.valid_fix:
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)
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'])
self.vel_pub.publish(current_vel)
elif 'GST' in parsed_sentence:
data = parsed_sentence['GST']
# Use receiver-provided error estimate if available
self.using_receiver_epe = True
self.lon_std_dev = data['lon_std_dev']
self.lat_std_dev = data['lat_std_dev']
self.alt_std_dev = data['alt_std_dev']
elif 'HDT' in parsed_sentence:
data = parsed_sentence['HDT']
if data['heading']:
current_heading = QuaternionStamped()
current_heading.header.stamp = current_time
current_heading.header.frame_id = frame_id
q = quaternion_from_euler(0, 0, math.radians(data['heading']))
current_heading.quaternion.x = q[0]
current_heading.quaternion.y = q[1]
current_heading.quaternion.z = q[2]
current_heading.quaternion.w = q[3]
self.heading_pub.publish(current_heading)
else:
return False
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 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
elif gps_qual == 9:
# Support specifically for NOVATEL OEM4 recievers which report WAAS fix as 9
# http://www.novatel.com/support/known-solutions/which-novatel-position-types-correspond-to-the-gga-quality-indicator/
current_fix.status.status = NavSatStatus.STATUS_SBAS_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']
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_global = Vector3Stamped()
current_vel_global.header.stamp = current_time
current_vel_global.header.frame_id = frame_id
current_vel_global.vector.x = data['speed'] * \
math.sin(data['true_course'])
current_vel_global.vector.y = data['speed'] * \
math.cos(data['true_course'])
self.vel_pub.publish(current_vel_global)
current_vel_local = TwistStamped()
current_vel_local.header.stamp = current_time
current_vel_local.header.frame_id = frame_id
current_vel_local.twist.linear.x = data['speed']
self.vel_pub2.publish(current_vel_local)
elif 'HDT' in parsed_sentence:
data = parsed_sentence['HDT']
if data['heading_north']:
self.hdt_pub.publish(data['heading_north'])
elif 'ROT' in parsed_sentence:
data = parsed_sentence['ROT']
current_rot = TwistStamped()
current_rot.header.stamp = current_time
current_rot.header.frame_id = frame_id
current_rot.twist.angular.z = data['rate'] * 3.14 / 180 * 60
self.rot_pub.publish(current_rot)
elif 'VTG' in parsed_sentence:
data = parsed_sentence['VTG']
current_speed = TwistStamped()
current_speed.header.stamp = current_time
current_speed.header.frame_id = frame_id
current_speed.twist.linear.x = data['speed_kph'] / 360 * 1000
self.speed_pub.publish(current_speed)
elif 'AVR' in parsed_sentence:
data = parsed_sentence['AVR']
current_pyr = TwistStamped()
current_pyr.header.stamp = current_timecur
current_pyr.header.frame_id = frame_id
current_pyr.twist.angular.z = data['yaw']
current_pyr.twist.angular.y = data['pitch']
current_pyr.twist.angular.x = data['roll']
if data['fix_type'] > 0:
self.pyr_pub.publish(current_pyr)
else:
return False
def callback(self, data):
'''
Callback function, <data> is the depth image
'''
try:
time1 = time.time()
try:
frame = self.bridge.imgmsg_to_cv2(
data, desired_encoding="passthrough")
except CvBridgeError as err:
print err
return
mog2_res = self.mog2.run(False, frame.astype(np.float32))
if mog2_res is None:
return
mask1 = cv2.morphologyEx(mog2_res.copy(), cv2.MORPH_OPEN,
self.open_kernel)
check_sum = np.sum(mask1 > 0)
if not check_sum or check_sum == np.sum(frame > 0):
return
_, contours, _ = cv2.findContours(mask1, cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_NONE)
cont_ind = np.argmax(
[cv2.contourArea(contour) for contour in contours])
final_mask = np.zeros_like(mask1)
cv2.drawContours(final_mask, contours, cont_ind, 1, -1)
#cv2.imshow('test',mask1*255)
#cv2.waitKey(10)
frame = frame * final_mask
scores_exist, _ = self.used_classifier.run_testing(frame,
online=True)
#DEBUGGING
#cv2.imshow('test',(frame%256).astype(np.uint8))
#cv2.waitKey(10)
time2 = time.time()
self.time.append(np.mean(self.time[-9:] + [(time2 - time1)]))
if (self.used_classifier.recognized_classes is not None
and len(self.used_classifier.recognized_classes) > 0
and scores_exist):
self.image_pub.publish(
self.bridge.cv2_to_imgmsg(
self.used_classifier.frames_preproc.hand_img,
"passthrough"))
msg = TimeReference()
try:
msg.source = self.used_classifier.train_classes[
self.used_classifier.recognized_classes[-1]]
except TypeError:
msg.source = self.used_classifier.recognized_classes[
-1].name
msg.time_ref = Time()
msg.time_ref.secs = int(
1 / float(self.time[-1]) if self.time[-1] else 0)
self.class_pub.publish(msg)
self.skel_pub.publish(
self.bridge.cv2_to_imgmsg(
np.atleast_2d(
np.array(
self.used_classifier.frames_preproc.skeleton.
skeleton, np.int32))))
except Exception as e:
exc_type, exc_value, exc_traceback = sys.exc_info()
traceback.print_exception(exc_type,
exc_value,
exc_traceback,
limit=10,
file=sys.stdout)
quality = int(fields[5])
nr_sats = int(fields[6])
ratio = float(fields[14])
# Pose
enu.header.stamp = time_now
enu.pose.position.x = float(fields[2])
enu.pose.position.y = float(fields[3])
enu.pose.position.z = float(fields[4])
# Timeref
# Expects time as UTC
gpstime.header.stamp = time_now
t = time.strptime(fields[0] + ' ' + fields[1], "%Y/%m/%d %H:%M:%S.%f")
gpstime.time_ref = rospy.Time.from_sec(calendar.timegm(t))
rtk.header = enu.header
rtk.quality = quality
rtk.nsat = nr_sats
posepub.publish(enu)
timepub.publish(gpstime)
rtkpub.publish(rtk);
if not publish_tf: continue
# Transform
if quality in accept_quality and nr_sats >= accept_num_sats and ratio >= accept_ratio:
try:
tflisten.waitForTransform(odom_frame_id, base_frame_id, time_now, rospy.Duration(1.0))
#if not satellite lock message parse it separately
else:
if GPSLock == True:
if 'RMC' in fields[0]:
#print fields
gpsVel.header.stamp = timeNow
gpsVel.twist.linear.x = float(fields[7])*0.514444444444*math.sin(float(fields[8]))
gpsVel.twist.linear.y = float(fields[7])*0.514444444444*math.cos(float(fields[8]))
gpsVelPub.publish(gpsVel)
navData.status.status = NavSatStatus.STATUS_FIX
navData.header.stamp = gpsVel.header.stamp
navData.status.service = NavSatStatus.SERVICE_GPS
gpstime.header.stamp = gpsVel.header.stamp
gpstime.time_ref = convertNMEATimeToROS(fields[1])
longitude = float(fields[5][0:3]) + float(fields[5][3:])/60
if fields[6] == 'W':
longitude = -longitude
latitude = float(fields[3][0:2]) + float(fields[3][2:])/60
if fields[4] == 'S':
latitude = -latitude
#publish data
navData.latitude = latitude
navData.longitude = longitude
navData.altitude = float('NaN')
navData.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
gpspub.publish(navData)
