class GPSReader:
def __init__(self):
self.sb = SerialBuffer("/dev/ttyUSB0", 115200)
if not self.sb.is_open():
print "Cannot open port"
exit()
self.position_pub = rospy.Publisher('/rtk_gps/position',
Position,
queue_size=10)
self.orientation_pub = rospy.Publisher('/rtk_gps/orientation',
Orientation,
queue_size=10)
self.pose_pub = rospy.Publisher('/rtk_gps/pose', Pose, queue_size=10)
self.reset_srv = rospy.Service('rtk_gps/reset', Trigger,
self.resetCallback)
self.position_sub = message_filters.Subscriber('/rtk_gps/position',
Position)
self.orientation_sub = message_filters.Subscriber(
'/rtk_gps/orientation', Orientation)
self.sync = message_filters.ApproximateTimeSynchronizer(
[self.position_sub, self.orientation_sub], 10, 0.1)
self.sync.registerCallback(self.callback)
# self.sb.write('JASC,GPGGA,5,PORTA')
def spin_once(self):
serial_msg = self.sb.spin_once()
if serial_msg:
self.process(serial_msg)
def callback(self, position_msg, orientation_msg):
# yaw(z), pitch(x), roll(y)
msg = Pose()
msg.position.x = position_msg.northing
msg.position.y = -position_msg.easting
quaternion = tf.transformations.quaternion_from_euler(
orientation_msg.pitch, orientation_msg.roll, -orientation_msg.yaw)
msg.orientation.x = quaternion[0]
msg.orientation.y = quaternion[1]
msg.orientation.z = quaternion[2]
msg.orientation.w = quaternion[3]
self.pose_pub.publish(msg)
def resetCallback(self, req):
print 'Reset'
return TriggerResponse(True, 'Reset')
def process(self, msg):
if msg[0] == '$PTNL':
if len(msg) == 13:
mm = Position()
try:
mm.northing = float(msg[4])
mm.easting = float(msg[6])
mm.height = float(msg[11][3:])
mm.quality = int(msg[8])
mm.fixStatelliteNum = int(msg[9])
mm.fixDOP = float(msg[10])
self.position_pub.publish(mm)
except:
rospy.logerr('Failed to convert')
return
else:
rospy.logerr(
'Wrong number of $PTNL message: 13 expected but %d given',
len(msg))
return
elif msg[0] == '$PSAT':
if len(msg) != 7:
rospy.logerr(
'Wrong number of $PSAT message: 7 expected but %d given',
len(msg))
return
try:
mm = Orientation()
mm.yaw = float(msg[3]) / 180.0 * math.pi
mm.pitch = float(msg[4]) / 180.0 * math.pi
mm.roll = float(msg[5]) / 180.0 * math.pi
self.orientation_pub.publish(mm)
except:
rospy.logwarn('GPS orientation not ready')
return
class GPSReader:
def __init__(self):
self.sb = SerialBuffer("/dev/ttyUSB0", 115200)
if not self.sb.is_open():
print "Cannot open port"
exit()
self.reset_srv = rospy.Service('rtk_gps/reset', Trigger,
self.reset_callback)
self.pub = rospy.Publisher('/rtk_gps/gps', GPSFix)
self.msg = GPSFix()
def spin_once(self):
serial_msg = self.sb.spin_once()
if serial_msg:
self.process(serial_msg)
def reset_callback(self, req):
print 'Reset'
return TriggerResponse(True, 'Reset')
def process(self, msg):
if msg[0] == '$PTNL':
if len(msg) == 13:
# mm.northing = float(msg[4])
# mm.easting = float(msg[6])
self.msg.err_vert = to_float(msg[4])
self.msg.err_horz = to_float(msg[6])
self.msg.altitude = to_float(msg[11][3:])
quality = to_int(msg[8])
self.msg.status.status = {
0: GPSStatus.STATUS_NO_FIX,
1: GPSStatus.STATUS_FIX,
2: GPSStatus.STATUS_FIX,
3: GPSStatus.STATUS_DGPS_FIX
}[quality]
self.msg.status.satellites_used = to_int(msg[9])
self.msg.gdop = to_float(msg[10])
else:
rospy.logerr(
'Wrong number of $PTNL message: 13 expected but %d given',
len(msg))
return
elif msg[0] == '$PSAT':
if len(msg) != 7:
rospy.logerr(
'Wrong number of $PSAT message: 7 expected but %d given',
len(msg))
return
self.msg.track = to_float(msg[3]) / 180.0 * math.pi
self.msg.pitch = to_float(msg[4]) / 180.0 * math.pi
self.msg.roll = to_float(msg[5]) / 180.0 * math.pi
elif msg[0] == '$GPRMC':
if len(msg) != 14:
rospy.logerr(
'Wrong number of $GPRMC message: 14 expected but %d given',
len(msg))
return
if msg[2] == 'V':
self.msg.status.status = GPSStatus.STATUS_NO_FIX
else:
self.msg.latitude = (to_float(msg[3]) if msg[4] == 'N' else
-to_float(msg[3])) / 100
self.msg.longitude = (to_float(msg[5]) if msg[6] == 'E' else
-to_float(msg[5])) / 100
self.msg.speed = to_float(msg[7])
self.msg.track = to_float(msg[8])
if msg[12] == 'D':
self.msg.status.status = GPSStatus.STATUS_DGPS_FIX
self.publish()
else:
rospy.logwarn('Unknown msg:' + str(msg))
return
def publish(self):
self.msg.header.stamp = rospy.Time.now()
self.pub.publish(self.msg)