def spin(self):
#############################################################################
r = rospy.Rate(self.rate)
while not rospy.is_shutdown():
current_time = rospy.get_rostime()
frame_id = 'gps'
latitude = 40.444539
longitude = -79.940777
current_fix = NavSatFix()
current_fix.header.stamp = current_time
current_fix.header.frame_id = frame_id
current_time_ref = TimeReference()
current_time_ref.header.stamp = current_time
current_time_ref.header.frame_id = frame_id
current_time_ref.source = frame_id
current_fix.status.status = NavSatStatus.STATUS_FIX
current_fix.status.service = NavSatStatus.SERVICE_GPS
current_fix.latitude = latitude
current_fix.longitude = longitude
current_fix.position_covariance[0] = 0.1
current_fix.position_covariance[4] = 0.1
current_fix.position_covariance[8] = 0.25
current_fix.position_covariance_type = \
NavSatFix.COVARIANCE_TYPE_APPROXIMATED
current_fix.altitude = 0.0
self.fix_pub.publish(current_fix)
self.update()
r.sleep()
def run(self):
r = rospy.Rate(self.freq)
while not rospy.is_shutdown():
t = TimeReference()
t.source = str(self.freq)
t.header.stamp = rospy.Time.now()
self.pub.publish(t)
r.sleep()
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 run(self):
r = rospy.Rate(self.freq)
while not rospy.is_shutdown():
t = TimeReference()
t.source = str(self.freq)
t.header.stamp = rospy.Time.now()
if rospy.get_param("RL_agent_active", True):
self.pub.publish(t)
r.sleep()
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 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 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 ImageCallbackResize(Imagedata):
ResizedImage = TimeReference()
img = np.array(list(map(ord, list(Imagedata.data)))).astype("uint8")
img = np.reshape(img[::-1], (Imagedata.height, Imagedata.width))
img = resize(img, (128, 128),
preserve_range=True,
mode='constant',
anti_aliasing=True)
img = np.reshape(img, (1, 16384)).astype("uint8")
img_byte = img.tobytes()
ResizedImage.source = base64.b64encode(img_byte)
ResizedImage.header = Imagedata.header
pub.publish(ResizedImage)
def __init__(self, **kwargs):
self.system_name = socket.gethostname()
rospy.init_node('timesync_node', anonymous=True)
# Parameters
self.timeref_topic = rospy.get_param("~topic",
"walltime/time_reference")
self.mode = rospy.get_param("~mode", "server")
self.tolerance = rospy.get_param(
"~tolerance",
2) # Tolerance in seconds for considering as "synchronized"
# Publishers
self.timeref_pub = rospy.Publisher(self.timeref_topic,
TimeReference,
queue_size=10)
self.status_pub = rospy.Publisher('/diagnostics',
DiagnosticArray,
queue_size=10)
# Subscribers
rospy.Subscriber(self.timeref_topic,
TimeReference,
callback=self.timeref_callback)
# Messages
self.timeref_msg = TimeReference()
self.diag_status = DiagnosticStatus()
self.last_update = 0
# Rate
self.rate = rospy.Rate(0.5) # Once every 2 secs
def __init__(self):
# Our publishers
self.fix_pub = rospy.Publisher('tcpfix', NavSatFix, queue_size=1)
self.vel_pub = rospy.Publisher('tcpvel', TwistStamped, queue_size=1)
self.timeref_pub = rospy.Publisher('tcptime', TimeReference, queue_size=1)
# Frame of references we should publish in
self.frame_timeref = rospy.get_param('~frame_timeref', 'gps')
self.frame_gps = rospy.get_param('~frame_gps', 'gps')
self.use_rostime = rospy.get_param('~use_rostime', True)
self.use_rmc = rospy.get_param('~use_rmc', False)
# Publish NMEA messages, debug mode
self.publish_nmea = rospy.get_param('~publish_nmea', False)
if self.publish_nmea:
self.nmea_pub = rospy.Publisher('~nmea', String, queue_size=1)
#TODO: to be completed
self.use_mag_ori = rospy.get_param('~use_magnetic_orientation', False)
# Flags for what information we have
self.has_fix = False
self.has_std = False
self.has_vel = False
self.has_timeref = False
# Blank messages we create
self.msg_fix = NavSatFix()
self.msg_vel = TwistStamped()
self.msg_timeref = TimeReference()
def __init__(self):
self.no_decbbox = 0
print("[INFO] Initializing ROS...")
rospy.init_node('Detection')
print("[INFO] Loading modules...")
self.yolo = Load_Yolo_model()
self.bridge = CvBridge()
print("[INFO] Loading config...")
# Create local variables
self.timer = TimeReference()
print("[INFO] Initialize ROS publishers...")
# Create Topics to publish
self.boxes_pub = rospy.Publisher("/Tracker/Detection/Boxes",
Detection2DArray,
queue_size=1)
self.timer_pub = rospy.Publisher("/Tracker/Timer",
TimeReference,
queue_size=1)
print("[INFO] Initialize ROS Subscribers...")
rospy.Subscriber("/zed2/zed_node/left/image_rect_color/compressed",
CompressedImage,
self.callback,
queue_size=400)
# Create subscriptions
print("[INFO] Loading complete")
def __init__(self):
rospy.init_node('nmea_to_navsat_converter')
# Get parameters
self.nmea_sub = rospy.get_param("~nmea_sub", '/fmLib/nmea_from_gps')
self.fix_pub = rospy.get_param("~fix_pub", 'fix')
self.vel_pub = rospy.get_param("~vel_pub", 'vel')
self.time_pub = rospy.get_param("~time_pub", 'time_reference')
# Set up topics
self.gpspub = rospy.Publisher(self.fix_pub, NavSatFix)
self.gpsVelPub = rospy.Publisher(self.vel_pub, TwistStamped)
self.gpstimePub = rospy.Publisher(self.time_pub, TimeReference)
self.status_sub = rospy.Subscriber(self.nmea_sub, nmea, self.onNmea)
# Set up frame
self.frame_id = rospy.get_param('~frame_id', 'gps')
if self.frame_id[0] != "/":
self.frame_id = self.addTFPrefix(self.frame_id)
self.time_ref_source = rospy.get_param('~time_ref_source',
self.frame_id)
self.useRMC = rospy.get_param('~useRMC', False)
#useRMC == True -> generate info from RMC+GSA
#useRMC == False -> generate info from GGA
self.navData = NavSatFix()
self.gpsVel = TwistStamped()
self.gpstime = TimeReference()
self.gpstime.source = self.time_ref_source
self.navData.header.frame_id = self.frame_id
self.gpsVel.header.frame_id = self.frame_id
self.GPSLock = False
def __init__(self):
print("[INFO] Initializing ROS...")
rospy.init_node('Detection')
print("[INFO] Loading modules...")
self.yolo = Load_Yolo_model()
self.bridge = CvBridge()
print("[INFO] Loading config...")
self.timer = TimeReference()
print("[INFO] Initialize ROS publishers...")
self.boxes_pub = rospy.Publisher("/Tracker/Detection/Boxes",
Detection2DArray,
queue_size=1)
self.timer_pub = rospy.Publisher("/Tracker/Timer",
TimeReference,
queue_size=1)
print("[INFO] Initialize ROS Subscribers...")
rospy.Subscriber("/zed2/zed_node/left/image_rect_color/compressed",
CompressedImage,
self.callback_sub,
queue_size=1)
print("[INFO] Loading complete")
self.init_time_delay = 0
self.callback()
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 __init__(self):
# ------------------------------------------------------------------------------
# init values
self.text_buffer = ' '
self.parser = anavs_parser.ANAVSParserUBX()
self.odometry_msg = Odometry()
self.rtk_groundtruth = odom()
self.nav_msg = NavSatFix()
self.gnss_time_msg = TimeReference()
self.odom_local = Odometry()
self.tcp_ip = rospy.get_param('/anavs_rtk_static_node/rtk_module_ip')
#self.tcp_ip = rospy.get_param('/anavs_rtk_static_node/rtk_module_ip', "localhost") # dummy_receiver (PAD_solution)
#self.tcp_ip = rospy.get_param('/anavs_rtk_static_node/rtk_module_ip', "192.168.42.1") # tum-nav
#self.tcp_ip = rospy.get_param('/anavs_rtk_static_node/rtk_module_ip', "192.168.20.13") # dlr-kn: Columbus (pw: #LocoExplo#)
#self.tcp_ip = rospy.get_param('/anavs_rtk_static_node/rtk_module_ip', "192.168.20.63") # dlr-kn: Vespucci (pw: #LocoExplo#)
# ------------------------------------------------------------------------------
# create publisher, subscriber and node handle
self.pub_odometry = rospy.Publisher('rtk_odometry_static',
odom,
queue_size=10)
self.pub_nav = rospy.Publisher('gnss_nav_static',
NavSatFix,
queue_size=10)
self.pub_time = rospy.Publisher('gnss_time_static',
TimeReference,
queue_size=10)
rospy.init_node('anavs_rtk_static_node', anonymous=True)
# ------------------------------------------------------------------------------
# create connection
self.tcp_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.tcp_socket.connect((self.tcp_ip, TCP_PORT))
self.tcp_socket.setblocking(0)
print "Connected to RTK processing module %s:%d" % (self.tcp_ip,
TCP_PORT)
# ------------------------------------------------------------------------------
# main loop
rate = rospy.Rate(10)
while not rospy.is_shutdown():
readable, writable, exceptional = select.select([self.tcp_socket],
[],
[self.tcp_socket],
1)
if self.tcp_socket in readable:
self.text_buffer += self.tcp_socket.recv(4096)
if self.parse_data_ubx():
# TODO: Remove 1 from the statement
if self.parser.code == 4 or self.parser.code == 5 or self.parser.code == 1: ## 1 added just for test
current_time = rospy.Time.now()
self.build_odometry_msg(current_time, self.parser.code)
self.build_nav_msg(current_time)
self.build_time(current_time)
rate.sleep()
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 main(args):
model = YOLO(**vars(args))
node.create_subscription(Image, 'usb_cam/image_raw', callback)
pub = node.create_publisher(TimeReference, 'people_info')
msg = TimeReference()
while rclpy.ok():
# for _ in range(5):
rclpy.spin_once(node)
try:
result = detect_image(model, args.classes_path)
print(result)
except:
result = {'objects': []}
print('no people')
msg.header = header
msg.source = json.dumps(result)
pub.publish(msg)
model.close_session()
node.destroy_node()
rclpy.shutdown()
def process_line(self, nmea_string):
# Check if valid message
if not check_nmea_checksum(nmea_string):
rospy.logwarn(
"Received a sentence with an invalid checksum. Sentence was: %s"
% repr(nmea_string))
return
# Else we are good, lets try to process this message
parsed_sentence = reach_ros_node.parser.parse_nmea_sentence(
nmea_string)
if not parsed_sentence:
rospy.logwarn("Failed to parse NMEA sentence. Sentence was: %s" %
nmea_string)
return
# We have a good message!!
# Lets send it to the correct method to get processed
self.parse_GGA(parsed_sentence)
self.parse_GST(parsed_sentence)
self.parse_VTG(parsed_sentence)
self.parse_RMC(parsed_sentence)
# Special care to parse the time reference
# This can come from either the GGA or RMC
self.parse_time(parsed_sentence)
# Now that we are done with processing messages
# Lets publish what we have!
if self.has_fix and self.has_std:
self.fix_pub.publish(self.msg_fix)
self.msg_fix = NavSatFix()
self.has_fix = False
self.has_std = False
if self.has_vel:
self.vel_pub.publish(self.msg_vel)
self.msg_vel = TwistStamped()
self.has_vel = False
if self.has_timeref:
self.timeref_pub.publish(self.msg_timeref)
self.msg_timeref = TimeReference()
self.has_timeref = False
def __init__(self):
# Our publishers
self.fix_pub = rospy.Publisher('tcpfix', NavSatFix, queue_size=1)
self.vel_pub = rospy.Publisher('tcpvel', TwistStamped, queue_size=1)
self.timeref_pub = rospy.Publisher('tcptime',
TimeReference,
queue_size=1)
# Frame of references we should publish in
self.frame_timeref = rospy.get_param('~frame_timeref', 'gps')
self.frame_gps = rospy.get_param('~frame_gps', 'gps')
self.use_rostime = rospy.get_param('~use_rostime', True)
self.use_rmc = rospy.get_param('~use_rmc', False)
# Flags for what information we have
self.has_fix = False
self.has_std = False
self.has_vel = False
self.has_timeref = False
# Blank messages we create
self.msg_fix = NavSatFix()
self.msg_vel = TwistStamped()
self.msg_timeref = TimeReference()
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
if __name__ == "__main__":
#init publisher
rospy.init_node('http_gps_driver')
gpspub = rospy.Publisher('fix', NavSatFix)
gpsVelPub = rospy.Publisher('vel',TwistStamped)
gpstimePub = rospy.Publisher('time_reference', TimeReference)
#Init GPS port
frame_id = rospy.get_param('~frame_id','gps')
if frame_id[0] != "/":
frame_id = addTFPrefix(frame_id)
time_ref_source = rospy.get_param('~time_ref_source', frame_id)
navData = NavSatFix()
gpsVel = TwistStamped()
gpstime = TimeReference()
gpstime.source = time_ref_source
navData.header.frame_id = frame_id
gpsVel.header.frame_id = frame_id
GPSLock = False
try:
#Read in GPS
while not rospy.is_shutdown():
#read GPS line
req = urllib2.Request('http://192.168.3.16/prog/show?position')
response = urllib2.urlopen(req)
data = response.read()
rospy.sleep(0.5)
timeNow = rospy.get_rostime()
rospy.init_node('novatel_gps_driver')
gpsPub = rospy.Publisher('gps_fix', NavSatFix, queue_size=1)
gpsStatPub = rospy.Publisher('gps_status', GPSStatus, queue_size=1)
gpsVelPub = rospy.Publisher('gps_vel',TwistStamped, queue_size=1)
#gpsTimePub = rospy.Publisher('time_reference', TimeReference)
#Init gpsSerial port
GPSport = rospy.get_param('~port','/dev/ttyUSB0')
GPSrate = rospy.get_param('~baud',57600)
frame_id = rospy.get_param('~frame_id','gps')
if frame_id[0] != "/":
frame_id = addTFPrefix(frame_id)
time_ref_source = rospy.get_param('~time_ref_source', frame_id)
navData = NavSatFix()
gpsVel = TwistStamped()
gpstime = TimeReference()
gpsStatus = GPSStatus()
gpstime.source = time_ref_source
navData.header.frame_id = frame_id
gpsVel.header.frame_id = frame_id
GPSLock = False
parser = NovatelParser()
try:
gpsSerial = serial.Serial(port=GPSport,baudrate=GPSrate,timeout=.01)
#Read in gpsSerial data
sync0 = '\x00'; sync1 = '\x00'; sync2 = '\x00';
while not rospy.is_shutdown():
# READ UNTIL SYNC
data = gpsSerial.read(1)
def __init__(self):
# Movement spesifications
self.overlap = rospy.get_param('/cam_shutter/overlap')
rospy.logwarn('The Overlap parm selected was %.2f' % self.overlap)
# Camera spesifications
self.cam_ref = rospy.get_param('/cam_shutter/Cam_ref')
self.Focal_len = float(rospy.get_param('/cam_shutter/Focal_len'))
self.Sensor_dim_h = float(rospy.get_param('/cam_shutter/Sensor_dim_h'))
self.Sensor_dim_w = float(rospy.get_param('/cam_shutter/Sensor_dim_w'))
self.Pixel_size = float(rospy.get_param('/cam_shutter/Pixel_size'))
self.Depth_filed = float(rospy.get_param('/cam_shutter/Depth_filed'))
rospy.Subscriber('/ekf', Odometry, self.EKF_calback)
rospy.Subscriber('/echoes', MultiEchoLaserScan, self.Lidar_calback)
rospy.Subscriber("/ctrl_flag", KeyValue, self.mode_callbaback)
rospy.Subscriber('/WL', Float32, self.Wl_callbaback)
rospy.Subscriber('/WR', Float32, self.Wr_callbaback)
self.photo_cont = 0
self.t_photo = 0.0
self.t_source = ""
self.dk = 0.0
self.r = 3.0
self.mode = 0
self.flag_k = True
self.x1 = 0.0
self.y1 = 0.0
self.wl = 0.0
self.wr = 0.0
rate = rospy.Rate(100) # 100hz
pub = rospy.Publisher('/cam_time', TimeReference, queue_size=10)
pub_flag = rospy.Publisher('/cam_shut', Bool, queue_size=10)
d = self.Cam_calculation(self.r)
d_ = d * (1 - self.overlap)
flag = False
while not rospy.is_shutdown():
print(flag)
if (self.dk >= d_) and (self.mode == 2):
flag = True
if (self.wl == 0.0) and (self.wr == 0.0):
print('photo')
os.system('echo 1 > /sys/class/gpio/gpio38/value')
# take picture
t_photo = TimeReference()
t_photo.header.seq = self.photo_cont
t_photo.header.stamp = rospy.Time.now()
t_photo.header.frame_id = self.cam_ref
t_photo.time_ref = self.t_photo
t_photo.source = self.t_source
pub.publish(t_photo)
self.photo_cont = self.photo_cont + 1
self.flag_k = True
self.dk = 0
d = self.Cam_calculation(self.r)
d_ = d * (1 - self.overlap)
flag = False
os.system('echo 0 > /sys/class/gpio/gpio38/value')
print(self.dk, d_)
pub_flag.publish(flag)
rate.sleep()
port = rospy.get_param('~port', 3333)
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
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 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. Sentence was: %s" %
nmea_string)
return False
rospy.logdebug(parsed_sentence)
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
self.extend_fix.header.stamp = current_time
self.extend_fix.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)
# set extend fix
self.extend_fix.status.header.stamp = current_time
self.extend_fix.status.header.frame_id = frame_id
self.extend_fix.status.status = gps_qual[1]
self.extend_fix.status.satellites_used = data['num_satellites']
self.extend_fix.status.motion_source = GPSStatus.SOURCE_GPS
self.extend_fix.status.orientation_source = GPSStatus.SOURCE_GPS
self.extend_fix.status.position_source = GPSStatus.SOURCE_GPS
self.extend_fix.latitude = current_fix.latitude
self.extend_fix.longitude = current_fix.longitude
self.extend_fix.altitude = current_fix.altitude
self.extend_fix.position_covariance = current_fix.position_covariance
self.position_covariance_type = current_fix.position_covariance_type
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)
self.extend_fix.time = current_time_ref.time_ref.to_sec()
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)
self.extend_fix.track = data['true_course']
self.extend_fix.speed = data['speed']
self.extend_fix_pub.publish(self.extend_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)
self.extend_fix.track = data['true_course']
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 'GSA' in parsed_sentence:
data = parsed_sentence['GSA']
self.star_use_gps = [
data['sate_id1'], data['sate_id2'], data['sate_id3'],
data['sate_id4'], data['sate_id5'], data['sate_id6'],
data['sate_id7'], data['sate_id8'], data['sate_id9'],
data['sate_id10'], data['sate_id11'], data['sate_id12']
]
self.star_use_gps = filter(lambda star: star != 0,
self.star_use_gps)
self.extend_fix.pdop = data['pdop']
self.extend_fix.hdop = data['hdop']
self.extend_fix.vdop = data['vdop']
elif 'BDGSA' in parsed_sentence:
data = parsed_sentence['BDGSA']
self.star_use_bd = [
data['sate_id1'], data['sate_id2'], data['sate_id3'],
data['sate_id4'], data['sate_id5'], data['sate_id6'],
data['sate_id7'], data['sate_id8'], data['sate_id9'],
data['sate_id10'], data['sate_id11'], data['sate_id12']
]
self.star_use_bd = filter(lambda star: star != 0, self.star_use_bd)
self.extend_fix.pdop = data['pdop']
self.extend_fix.hdop = data['hdop']
self.extend_fix.vdop = data['vdop']
self.extend_fix.status.satellite_used_prn = self.star_use_gps + self.star_use_bd
elif 'GSV' in parsed_sentence:
data = parsed_sentence['GSV']
if data['index'] == 1:
self.star_map_gps = []
self.star_map_gps.append({
'id': data['id_satellites1'],
'elevation': data['elevation_satellites1'],
'azimuth': data['azimuth_satellites1'],
'snr': data['snr1']
})
self.star_map_gps.append({
'id': data['id_satellites2'],
'elevation': data['elevation_satellites2'],
'azimuth': data['azimuth_satellites2'],
'snr': data['snr2']
})
self.star_map_gps.append({
'id': data['id_satellites3'],
'elevation': data['elevation_satellites3'],
'azimuth': data['azimuth_satellites3'],
'snr': data['snr3']
})
self.star_map_gps.append({
'id': data['id_satellites4'],
'elevation': data['elevation_satellites4'],
'azimuth': data['azimuth_satellites4'],
'snr': data['snr4']
})
self.star_map_gps = filter(lambda star: star['id'] != 0,
self.star_map_gps)
elif 'BDGSV' in parsed_sentence:
data = parsed_sentence['BDGSV']
if data['index'] == 1:
self.star_map_bd = []
self.star_map_bd.append({
'id': data['id_satellites1'],
'elevation': data['elevation_satellites1'],
'azimuth': data['azimuth_satellites1'],
'snr': data['snr1']
})
self.star_map_bd.append({
'id': data['id_satellites2'],
'elevation': data['elevation_satellites2'],
'azimuth': data['azimuth_satellites2'],
'snr': data['snr2']
})
self.star_map_bd.append({
'id': data['id_satellites3'],
'elevation': data['elevation_satellites3'],
'azimuth': data['azimuth_satellites3'],
'snr': data['snr3']
})
self.star_map_bd.append({
'id': data['id_satellites4'],
'elevation': data['elevation_satellites4'],
'azimuth': data['azimuth_satellites4'],
'snr': data['snr4']
})
self.star_map_bd = filter(lambda star: star['id'] != 0,
self.star_map_bd)
self.star_map = self.star_map_gps + self.star_map_bd
if data['length'] == data['index']:
self.extend_fix.status.satellites_visible = len(self.star_map)
self.extend_fix.status.satellite_visible_prn = [
star['id'] for star in self.star_map
]
self.extend_fix.status.satellite_visible_snr = [
star['snr'] for star in self.star_map
]
self.extend_fix.status.satellite_visible_azimuth = [
star['azimuth'] for star in self.star_map
]
self.extend_fix.status.satellite_visible_z = [
star['elevation'] for star in self.star_map
]
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
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. 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_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
gpsVelPub = rospy.Publisher('vel',TwistStamped)
gpstimePub = rospy.Publisher('time_reference', TimeReference)
#Init GPS port
GPSport = rospy.get_param('~port','/dev/ttyUSB0')
GPSrate = rospy.get_param('~baud',4800)
frame_id = rospy.get_param('~frame_id','gps')
if frame_id[0] != "/":
frame_id = addTFPrefix(frame_id)
time_ref_source = rospy.get_param('~time_ref_source', frame_id)
useRMC = rospy.get_param('~useRMC', False)
#useRMC == True -> generate info from RMC+GSA
#useRMC == False -> generate info from GGA
navData = NavSatFix()
gpsVel = TwistStamped()
gpstime = TimeReference()
gpstime.source = time_ref_source
navData.header.frame_id = frame_id
gpsVel.header.frame_id = frame_id
GPSLock = False
try:
GPS = serial.Serial(port=GPSport, baudrate=GPSrate, timeout=2)
#Read in GPS
while not rospy.is_shutdown():
#read GPS line
data = GPS.readline()
if not check_checksum(data):
rospy.logerr("Received a sentence with an invalid checksum. Sentence was: %s" % data)
continue
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 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.covarianceMatrix and isinstance(
self.covarianceMatrix, list) and len(
self.covarianceMatrix) == 9:
for i in range(9):
current_fix.position_covariance[i] = self.covarianceMatrix[
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):
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, 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:
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)
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)
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 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 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):
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
