enu_to_pose.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 import roslib
00003 roslib.load_manifest('rtklib')
00004 import rospy
00005 import sys
00006 import socket
00007 import re
00008 import time
00009 import calendar
00010 
00011 from sensor_msgs.msg import TimeReference
00012 from geometry_msgs.msg import PoseStamped,TransformStamped
00013 from tf.msg import tfMessage
00014 from tf import transformations
00015 import tf
00016 
00017 
00018 #Add the tf_prefix to the given frame id
00019 def addTFPrefix(frame_id):
00020     prefix = ""
00021     prefix_param = rospy.search_param("tf_prefix")
00022     if prefix_param:
00023         prefix = rospy.get_param(prefix_param)
00024         if prefix[0] != "/":
00025             prefix = "/%s" % prefix
00026 
00027     return "%s/%s" % (prefix, frame_id)
00028 
00029 
00030 if __name__ == "__main__":
00031     # Init publisher
00032     rospy.init_node('enu_to_pose')
00033     posepub = rospy.Publisher('enu', PoseStamped)
00034     timepub = rospy.Publisher('time_reference', TimeReference)
00035     tfpub = tf.TransformBroadcaster()
00036     tflisten = tf.TransformListener()
00037 
00038     # Init rtklib port
00039     host = rospy.get_param('~host', 'localhost')
00040     port = rospy.get_param('~port', 3333)
00041     global_frame_id = rospy.get_param('~global_frame_id', '/map')
00042     odom_frame_id   = rospy.get_param('~odom_frame_id',   '/odom')
00043     base_frame_id   = rospy.get_param('~base_frame_id',   '/base_link')
00044     publish_tf      = rospy.get_param('~publish_tf',      True)
00045 
00046     # Quality parameters
00047     accept_quality  = rospy.get_param('~quality','1,2,4,5,6')
00048     accept_quality  = [int(x) for x in accept_quality.split(',')]
00049     accept_num_sats = int(rospy.get_param('~min_sat', 5))
00050     accept_ratio    = float(rospy.get_param('~min_ratio', 1.0))
00051 
00052     enu = PoseStamped()
00053     enu.header.frame_id = '/map'
00054     time_ref_source = rospy.get_param('~time_ref_source', global_frame_id)
00055     gpstime = TimeReference()
00056     gpstime.source = time_ref_source
00057     trans_corr = (0.0, 0.0, 0.0)
00058 
00059     try:
00060         sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
00061         sock.connect((host, port))
00062         file = sock.makefile()
00063 
00064         while not rospy.is_shutdown():
00065             data = file.readline()
00066             time_now = rospy.get_rostime()
00067             fields = re.split('\s+', data)
00068             if fields[0] == '%': continue
00069             assert len(fields) is 16
00070 
00071             quality = int(fields[5])
00072             nr_sats = int(fields[6])
00073             ratio   = float(fields[14])
00074 
00075             # Pose
00076             enu.header.stamp = time_now
00077             enu.pose.position.x = float(fields[2])
00078             enu.pose.position.y = float(fields[3])
00079             enu.pose.position.z = float(fields[4])
00080 
00081             # Timeref
00082             # Expects time as UTC
00083             gpstime.header.stamp = time_now
00084             t = time.strptime(fields[0] + ' ' + fields[1], "%Y/%m/%d %H:%M:%S.%f")
00085             gpstime.time_ref = rospy.Time.from_sec(calendar.timegm(t))
00086 
00087             posepub.publish(enu)
00088             timepub.publish(gpstime)
00089 
00090             if not publish_tf: continue
00091 
00092             # Transform
00093             if quality in accept_quality and nr_sats >= accept_num_sats and ratio >= accept_ratio:
00094                 try:
00095                     tflisten.waitForTransform(odom_frame_id, base_frame_id, time_now, rospy.Duration(1.0))
00096                     (trans,rot) = tflisten.lookupTransform(odom_frame_id, base_frame_id, time_now)
00097                 except tf.Exception:
00098                     print("Catched tf.Exception, i.e. no transform received in time, using zero-values, (just for testing)")
00099                     (trans, rot) = ((0,0,0), (0,0,0,0.1))
00100 
00101                 trans_gps = (float(fields[2]), float(fields[3]), float(fields[4]))
00102                 trans_corr = (trans_gps[0] - trans[0], trans_gps[1] - trans[1], trans_gps[2] - trans[2])
00103                 rot = (0.0, 0.0, 0.0, 1.0)
00104                 tfpub.sendTransform(trans_corr, rot, rospy.get_rostime(), odom_frame_id, global_frame_id)
00105 
00106     except rospy.ROSInterruptException:
00107         sock.close()
00108 


rtklib
Author(s): Manuel Stahl
autogenerated on Mon Oct 6 2014 00:14:19