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 publish_time_ref(secs, nsecs, source):
"""Publish a time reference."""
# Doesn't follow the standard publishing pattern since several time
# refs could be published simultaneously
if self.time_ref_pub is None:
self.time_ref_pub = rospy.Publisher(
'time_reference', TimeReference, queue_size=10)
time_ref_msg = TimeReference()
time_ref_msg.header = self.h
time_ref_msg.time_ref.secs = secs
time_ref_msg.time_ref.nsecs = nsecs
time_ref_msg.source = source
self.time_ref_pub.publish(time_ref_msg)
def add_sentence(self, nmea_string, frame_id, timestamp=None):
if not check_nmea_checksum(nmea_string):
rospy.logwarn("Received a sentence with an invalid checksum. " +
"Sentence was: %s" % repr(nmea_string))
return False
parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence(nmea_string)
if not parsed_sentence:
rospy.logdebug("Failed to parse NMEA sentence. Sentece was: %s" % nmea_string)
return False
if timestamp:
current_time = timestamp
else:
current_time = rospy.get_rostime()
current_fix = NavSatFix()
current_fix.header.stamp = current_time
current_fix.header.frame_id = frame_id
current_time_ref = TimeReference()
current_time_ref.header.stamp = current_time
current_time_ref.header.frame_id = frame_id
if self.time_ref_source:
current_time_ref.source = self.time_ref_source
else:
current_time_ref.source = frame_id
if 'B' in parsed_sentence:
data = parsed_sentence['B']
# Only publish a fix from RMC if the use_RMC flag is set.
if True:
current_fix.status.status = NavSatStatus.STATUS_FIX
current_fix.status.service = NavSatStatus.SERVICE_GPS
current_fix.latitude = data['latitude']
current_fix.longitude = data['longitude']
current_fix.altitude = float('NaN')
current_fix.position_covariance_type = \
NavSatFix.COVARIANCE_TYPE_UNKNOWN
self.fix_pub.publish(current_fix)
if not math.isnan(data['utc_time']):
current_time_ref.time_ref = rospy.Time.from_sec(data['utc_time'])
self.time_ref_pub.publish(current_time_ref)
# Publish velocity from RMC regardless, since GGA doesn't provide it.
if True:
current_vel = TwistStamped()
current_vel.header.stamp = current_time
current_vel.header.frame_id = frame_id
current_vel.twist.linear.x = data['speed'] * \
math.sin(data['true_course'])
current_vel.twist.linear.y = data['speed'] * \
math.cos(data['true_course'])
self.vel_pub.publish(current_vel)
else:
return False
def 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 gpsCallback(data):
gps_reading = marshal.loads(data.data)
current_time = rospy.Time.now()
frame_id = "gps_link"
time_ref_msg = TimeReference()
time_ref_msg.header.stamp = current_time
time_ref_msg.header.frame_id = frame_id
# if 'timestamp' in gps_reading:
# timestamp = gps_reading['timestamp']
# timestamp_s = datetime.time(
# hour=int(timestamp[0:2]),
# minute=int(timestamp[3:5]),
# second=int(timestamp[6:8]),
# microsecond=int(timestamp[9:]))
# time_ref_msg.time_ref = rospy.Time.from_sec(timestamp_s.second)
# time_ref_msg.source = "gps_time"
# else:
# time_ref_msg.source = frame_id
time_ref_msg.source = frame_id
time_ref_pub.publish(time_ref_msg)
nav_msg = NavSatFix()
nav_msg.header.stamp = current_time
nav_msg.header.frame_id = frame_id
gps_qual = gps_reading['qual']
if gps_qual == 1:
nav_msg.status.status = NavSatStatus.STATUS_FIX
elif gps_qual == 2:
nav_msg.status.status = NavSatStatus.STATUS_SBAS_FIX
elif gps_qual in (4, 5):
nav_msg.status.status = NavSatStatus.STATUS_GBAS_FIX
elif gps_qual == 9:
nav_msg.status.status = NavSatStatus.STATUS_SBAS_FIX
else:
nav_msg.status.status = NavSatStatus.STATUS_NO_FIX
nav_msg.status.service = NavSatStatus.SERVICE_GPS
nav_msg.latitude = gps_reading['latitude']
nav_msg.longitude = gps_reading['longitude']
# nav_msg.altitude = float('NaN')
nav_msg.altitude = 0 # EKF Not outputing when using NaN?
nav_msg.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
navsatfix_pub.publish(nav_msg)
vel_msg = TwistStamped()
vel_msg.header.stamp = current_time
vel_msg.header.frame_id = frame_id
vel_msg.twist.linear.x = gps_reading['speed_over_ground'] * math.sin(gps_reading['course_over_ground'])
vel_msg.twist.linear.y = gps_reading['speed_over_ground'] * math.cos(gps_reading['course_over_ground'])
vel_pub.publish(vel_msg)
marker_msg = Marker()
marker_msg.header = nav_msg.header
marker_msg.action = 0 # ADD
marker_msg.type = 0 # ARROW
marker_msg.scale.x = 1
marker_msg.scale.y = 0.1
marker_msg.scale.z = 0.1
marker_msg.color.a = 1.0
marker_msg.color.r = 0.0;
marker_msg.color.g = 0.0;
marker_msg.color.b = 1.0;
marker_msg.pose.position.x = 0
marker_msg.pose.position.y = 0
quat = tf.transformations.quaternion_from_euler(0, 0, 0)
marker_msg.pose.orientation.x = quat[0]
marker_msg.pose.orientation.y = quat[1]
marker_msg.pose.orientation.z = quat[2]
marker_msg.pose.orientation.w = quat[3]
marker_pub.publish(marker_msg)
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 = 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
global_frame_id = rospy.get_param('~global_frame_id', '/map')
odom_frame_id = rospy.get_param('~odom_frame_id', '/odom')
base_frame_id = rospy.get_param('~base_frame_id', '/base_link')
publish_tf = rospy.get_param('~publish_tf', True)
# Quality parameters
accept_quality = rospy.get_param('~quality', '1,2,4,5,6')
accept_quality = [int(x) for x in accept_quality.split(',')]
accept_num_sats = int(rospy.get_param('~min_sat', 5))
accept_ratio = float(rospy.get_param('~min_ratio', 1.0))
enu = PoseStamped()
enu.header.frame_id = '/map'
time_ref_source = rospy.get_param('~time_ref_source', global_frame_id)
gpstime = TimeReference()
gpstime.source = time_ref_source
trans_corr = (0.0, 0.0, 0.0)
try:
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
sock.connect((host, port))
file = sock.makefile()
while not rospy.is_shutdown():
data = file.readline()
time_now = rospy.get_rostime()
fields = re.split('\s+', data)
if fields[0] == '%': continue
assert len(fields) is 16
quality = int(fields[5])
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 = 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 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 got_image(self, rgb_msg, depth_msg):
color_image = self._bridge.imgmsg_to_cv2(rgb_msg, '8UC3')
depth_image = self._bridge.imgmsg_to_cv2(depth_msg, "8UC1")
## Enable to reporject depth map th 3D
"""
points_3D = cv2.reprojectImageTo3D(depth_image, self.Q2)
mask_map = depth_image > 0
output_points = points_3D[mask_map]
output_colors = color_image[mask_map]
cv2.imwrite("color.jpg", color_image)
cv2.imwrite("depth.jpg", depth_image)
if self.savepointcloud:
# output_file = "reconstructed.ply"
print ("\n Creating the output file... \n")
self.write_pointcloud(output_points, output_colors, output_file)
self.savepointcloud = False
"""
final_bbox = None
if self._is_first_frame and self._inital_bbox is not None:
rospy.loginfo("Initializing tracker")
current_bbox = self._inital_bbox
bbox_center = self.calculate_bbox_center(current_bbox)
self._tracker.init(color_image, current_bbox)
self._is_first_frame = False
final_bbox = current_bbox
elif not self._is_first_frame:
ok, self.tracker_suggested_bbox = self._tracker.update(color_image)
if ok:
final_bbox = self.tracker_suggested_bbox
else:
self._current_status = 0
T = TimeReference()
T.header.stamp = depth_msg.header.stamp
T.source = str(self._current_status)
self._pub_status.publish(T)
if final_bbox is not None:
self._last_bbox = final_bbox
width_ratio = float(final_bbox[2]) / float(color_image.shape[1])
height_ratio = float(final_bbox[3]) / float(color_image.shape[0])
if (width_ratio > self.max_bbox_ratio
or height_ratio > self.max_bbox_ratio
) and self._scale != self._fallback_scale:
rospy.loginfo("Scaling down...")
self._scale = self._fallback_scale
self._has_scale_changed = True
elif (width_ratio < self.max_bbox_ratio and height_ratio <
self.max_bbox_ratio) and self._scale == self._fallback_scale:
rospy.loginfo("Scaling back up...")
self._scale = 1.0
self._has_scale_changed = True
center = self.calculate_bbox_center(final_bbox)
if self.check_point_oob(center, color_image, self.oob_threshold):
self._current_status = 0
bbox_message = Detection2D()
# Initialize header info with that of depthmap's
bbox_message.header.stamp = depth_msg.header.stamp
bbox_message.header.seq = "bbox_INFO"
# bbox info
bbox_message.bbox.size_x = final_bbox[2]
bbox_message.bbox.size_y = final_bbox[3]
bbox_message.bbox.center.theta = 0
bbox_message.bbox.center.x = final_bbox[0] + final_bbox[2] / 2
bbox_message.bbox.center.y = final_bbox[1] + final_bbox[3] / 2
self._pub_bbox.publish(bbox_message)
T = TimeReference()
T.header.stamp = depth_msg.header.stamp
T.source = str(self._current_status)
self._pub_status.publish(T)
# cv2.imshow('depth',depth_image)
# cv2.waitKey()
if self.publish_result_img:
final_bbox = tuple([int(i) for i in final_bbox])
if self._current_status == 1:
cv2.rectangle(color_image, (final_bbox[0], final_bbox[1]),
(final_bbox[0] + final_bbox[2],
final_bbox[1] + final_bbox[3]), (0, 0, 255),
2)
else:
cv2.rectangle(color_image, (final_bbox[0], final_bbox[1]),
(final_bbox[0] + final_bbox[2],
final_bbox[1] + final_bbox[3]), (255, 0, 0),
2)
cv2.circle(color_image, center, 3, (255, 0, 0), 2)
imgmsg = self._bridge.cv2_to_imgmsg(color_image, 'rgb8')
self._pub_result_img.publish(imgmsg)
def handle_gps(self, data, timestamp):
"""
Date gps_Date MM/DD/YYYY or DD/MM/YYYY
Time gps_Time HH:MM:SS.SSS
Lat & Lon gps_Lat,gps_Long Degrees-7
Altitude gps_Alt mm
Altitude MSL gps_AltMSL mm
SIV gps_SIV Count
Fix Type gps_FixType 0-5
Carrier Soln. gps_CarrierSolution 0-2
Ground Speed gps_GroundSpeed mm/s
Heading gps_Heading Degrees-5
PDOP gps_pDOP 10-2 (dimensionless)
Time Of Week gps_iTOW ms
Lat = Latitude
Lon = Longitude
MSL = Metres above Sea Level
SIV = Satellites In View
PDOP = Positional Dilution Of Precision
Fix Type:
0: No
1: Dead Reckoning Only
2: 2D
3: 3D
4: GNSS + Dead Reckoning
5: Time Only
Carrier Solution:
0: No
1: Float Solution
2: Fixed Solution
"""
gps_msg = NavSatFix()
gps_msg.header.frame_id = self.tf_prefix+"world"
gps_msg.header.stamp = timestamp
gps_msg.status = NavSatStatus(status=0, service=1)
gps_msg.latitude = float(data[self.data_headers["GPS"]["Lat"]])
gps_msg.longitude = float(data[self.data_headers["GPS"]["Long"]])
gps_msg.altitude = float(data[self.data_headers["GPS"]["Alt"]])
# COVARIANCE_TYPE_UNKNOWN = 0, COVARIANCE_TYPE_APPROXIMATED = 1
# COVARIANCE_TYPE_DIAGONAL_KNOWN = 2, COVARIANCE_TYPE_KNOWN = 3
gps_msg.position_covariance = [0.0] * 9
gps_msg.position_covariance_type = 0
self.gps_pub.publish(gps_msg)
# Time Reference
time_msg = TimeReference()
time_msg.header.stamp = timestamp
gps_time = datetime.strptime("{} {}".format(data[self.data_headers["GPS"]["Date"]], data[self.data_headers["GPS"]["Time"]]),
"%d/%m/%Y %H:%M:%S.%f")
total_secs = (gps_time - epoch).total_seconds()
time_msg.time_ref.secs = int(total_secs)
time_msg.time_ref.nsecs = total_secs-int(total_secs)
time_msg.source = "GPS"
self.time_ref.publish(time_msg)
return True
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 None
parsed_sentence = enway_reach_rs_driver.parser.parse_nmea_sentence(
nmea_string)
if not parsed_sentence:
rospy.logdebug("Failed to parse NMEA sentence. Sentence was: %s" %
nmea_string)
return None
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
if self.covariance_matrix and isinstance(
self.covariance_matrix, list) and len(
self.covariance_matrix) == 9:
for i in range(9):
current_fix.position_covariance[
i] = self.covariance_matrix[i]
current_fix.position_covariance_type = \
NavSatFix.COVARIANCE_TYPE_KNOWN
else:
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)
return current_fix
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)
return current_fix
else:
return None
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
# else:
# rospy.logwarn("Valid sentence was: %s" % repr(nmea_string))
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:
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]
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'][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)
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)
elif 'SHR' in parsed_sentence:
data = parsed_sentence['SHR']
if data['heading']:
current_attitude = PoseWithCovarianceStamped()
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_attitude.header.stamp = rospy.Time(
data['utc_time'][0], data['utc_time'][1])
else:
current_attitude.header.stamp = current_time
current_attitude.header.frame_id = frame_id
current_attitude.pose.pose.position.x = math.radians(
data['roll'])
current_attitude.pose.pose.position.y = math.radians(
data['pitch'])
current_attitude.pose.pose.position.z = math.radians(
data['heading'])
q = quaternion_from_euler(math.radians(data['roll']),
math.radians(data['pitch']),
math.radians(data['heading']))
current_attitude.pose.pose.orientation.x = q[0]
current_attitude.pose.pose.orientation.y = q[1]
current_attitude.pose.pose.orientation.z = q[2]
current_attitude.pose.pose.orientation.w = q[3]
current_attitude.pose.covariance[0] = data['gps_quality']
current_attitude.pose.covariance[1] = data['imu_state']
current_attitude.pose.covariance[21] = math.radians(
data['roll_accuracy'])
current_attitude.pose.covariance[28] = math.radians(
data['pitch_accuracy'])
current_attitude.pose.covariance[35] = math.radians(
data['heading_accuracy'])
self.attitude_pub.publish(current_attitude)
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 = 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. 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. 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
global_frame_id = rospy.get_param('~global_frame_id', '/map')
odom_frame_id = rospy.get_param('~odom_frame_id', '/odom')
base_frame_id = rospy.get_param('~base_frame_id', '/base_link')
publish_tf = rospy.get_param('~publish_tf', True)
# Quality parameters
accept_quality = rospy.get_param('~quality','1,2,4,5,6')
accept_quality = [int(x) for x in accept_quality.split(',')]
accept_num_sats = int(rospy.get_param('~min_sat', 5))
accept_ratio = float(rospy.get_param('~min_ratio', 1.0))
enu = PoseStamped()
enu.header.frame_id = '/map'
time_ref_source = rospy.get_param('~time_ref_source', global_frame_id)
gpstime = TimeReference()
gpstime.source = time_ref_source
trans_corr = (0.0, 0.0, 0.0)
rtk = rtk()
try:
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
sock.connect((host, port))
file = sock.makefile()
while not rospy.is_shutdown():
data = file.readline()
time_now = rospy.get_rostime()
fields = re.split('\s+', data)
if fields[0] == '%': continue
assert len(fields) is 16
import message_filters
from sensor_msgs.msg import Image, CameraInfo, TimeReference
import numpy as np
import rospy
from cv_bridge import CvBridge
bridge = CvBridge()
frame = bridge.imgmsg_to_cv2()
image_time_ref_msg = TimeReference()
image_time_ref_msg.source = "image"
image_time_ref_msg.header.stamp =
image_sub = message_filters.Subscriber('image', Image)
ts = message_filters.ApproximateTimeSynchronizer()
pub = rospy.Publisher('ceva', CameraInfo,queue_size=10)
pub.publish()
