ExoMy_Cuno/ExoMy_Software-master/src/gps_node.py

#!/usr/bin/env python
import errno
import os
import select
import termios
from datetime import datetime

import rospy
from sensor_msgs.msg import NavSatFix, NavSatStatus
from std_msgs.msg import String


PORT = '/dev/ttyUSB0'
BAUD = 4800
FRAME_ID = 'gps'
READ_SIZE = 512

BAUD_MAP = {
    4800: termios.B4800,
    9600: termios.B9600,
    19200: termios.B19200,
    38400: termios.B38400,
    57600: termios.B57600,
    115200: termios.B115200,
}


def nmea_to_decimal(raw_value, hemisphere):
    if not raw_value or not hemisphere:
        return None

    try:
        value = float(raw_value)
    except ValueError:
        return None

    degrees = int(value / 100)
    minutes = value - degrees * 100
    decimal = degrees + minutes / 60.0

    if hemisphere in ('S', 'W'):
        decimal *= -1.0

    return decimal


def verify_checksum(sentence):
    if not sentence.startswith('$'):
        return False

    if '*' not in sentence:
        return True

    payload, checksum_text = sentence[1:].split('*', 1)
    checksum_text = checksum_text.strip()
    checksum = 0
    for char in payload:
        checksum ^= ord(char)

    try:
        expected = int(checksum_text[:2], 16)
    except ValueError:
        return False

    return checksum == expected


class GpsState(object):
    def __init__(self):
        self.connected = False
        self.latitude = None
        self.longitude = None
        self.altitude = None
        self.hdop = None
        self.speed_mps = None
        self.track_deg = None
        self.fix_quality = 0
        self.satellites = 0
        self.valid = False
        self.last_sentence_time = None
        self.last_fix_time = None

    def summary_text(self):
        if not self.connected:
            return 'Offline'
        if self.valid and self.latitude is not None and self.longitude is not None:
            speed_kmh = (self.speed_mps or 0.0) * 3.6
            return 'Fix | {0} Sat | {1:.1f} km/h'.format(max(0, int(self.satellites or 0)), speed_kmh)
        if self.last_sentence_time is not None:
            return 'Suche Satelliten'
        return 'Initialisiere'


def configure_serial(fd, baud):
    attrs = termios.tcgetattr(fd)
    attrs[0] = 0
    attrs[1] = 0
    attrs[2] = termios.CS8 | termios.CREAD | termios.CLOCAL
    attrs[3] = 0
    attrs[4] = baud
    attrs[5] = baud
    attrs[6][termios.VMIN] = 0
    attrs[6][termios.VTIME] = 0
    termios.tcsetattr(fd, termios.TCSANOW, attrs)
    termios.tcflush(fd, termios.TCIFLUSH)


def open_device(port, baud):
    while not rospy.is_shutdown():
        try:
            fd = os.open(port, os.O_RDONLY | os.O_NOCTTY | os.O_NONBLOCK)
            configure_serial(fd, baud)
            rospy.loginfo('GPS verbunden: %s @ %s Baud', port, baud_rate)
            return fd
        except OSError as exc:
            if exc.errno != errno.ENOENT:
                rospy.logwarn('GPS kann nicht geoeffnet werden: %s', exc)
            rospy.sleep(1.0)
        except termios.error as exc:
            rospy.logwarn('GPS-Port kann nicht konfiguriert werden: %s', exc)
            rospy.sleep(1.0)
    return None


def close_device(fd):
    if fd is None:
        return
    try:
        os.close(fd)
    except OSError:
        pass


def parse_gga(fields, state):
    if len(fields) < 10:
        return False

    latitude = nmea_to_decimal(fields[2], fields[3])
    longitude = nmea_to_decimal(fields[4], fields[5])

    try:
        fix_quality = int(fields[6] or 0)
    except ValueError:
        fix_quality = 0

    try:
        satellites = int(fields[7] or 0)
    except ValueError:
        satellites = 0

    try:
        hdop = float(fields[8]) if fields[8] else None
    except ValueError:
        hdop = None

    try:
        altitude = float(fields[9]) if fields[9] else None
    except ValueError:
        altitude = None

    if latitude is not None:
        state.latitude = latitude
    if longitude is not None:
        state.longitude = longitude
    state.fix_quality = fix_quality
    state.satellites = satellites
    state.hdop = hdop
    state.altitude = altitude
    state.valid = fix_quality > 0 and latitude is not None and longitude is not None
    if state.valid:
        state.last_fix_time = rospy.Time.now()
    return True


def parse_rmc(fields, state):
    if len(fields) < 10:
        return False

    status = fields[2] if len(fields) > 2 else ''
    latitude = nmea_to_decimal(fields[3], fields[4])
    longitude = nmea_to_decimal(fields[5], fields[6])

    try:
        speed_knots = float(fields[7]) if fields[7] else 0.0
    except ValueError:
        speed_knots = 0.0

    try:
        track_deg = float(fields[8]) if fields[8] else 0.0
    except ValueError:
        track_deg = 0.0

    if latitude is not None:
        state.latitude = latitude
    if longitude is not None:
        state.longitude = longitude
    state.speed_mps = speed_knots * 0.514444
    state.track_deg = track_deg
    if status == 'A' and state.latitude is not None and state.longitude is not None:
        state.valid = True
        state.last_fix_time = rospy.Time.now()

    # Optional date/time parse for debugging consistency.
    if fields[1] and fields[9]:
        try:
            datetime.strptime(fields[9] + fields[1].split('.')[0], '%d%m%y%H%M%S')
        except ValueError:
            pass
    return True


def parse_sentence(sentence, state):
    if not verify_checksum(sentence):
        return False

    fields = sentence.split('*', 1)[0].split(',')
    if not fields:
        return False

    sentence_type = fields[0]
    state.last_sentence_time = rospy.Time.now()

    if sentence_type.endswith('GGA'):
        return parse_gga(fields, state)
    if sentence_type.endswith('RMC'):
        return parse_rmc(fields, state)
    return False


def publish_fix(fix_publisher, overlay_publisher, state, frame_id):
    message = NavSatFix()
    message.header.stamp = rospy.Time.now()
    message.header.frame_id = frame_id
    message.status.service = NavSatStatus.SERVICE_GPS

    if state.valid and state.latitude is not None and state.longitude is not None:
        message.status.status = NavSatStatus.STATUS_FIX
        message.latitude = state.latitude
        message.longitude = state.longitude
        message.altitude = state.altitude if state.altitude is not None else 0.0
        if state.hdop is not None:
            horizontal_variance = max(0.25, state.hdop * state.hdop)
            vertical_variance = max(1.0, (state.hdop * 2.0) * (state.hdop * 2.0))
            message.position_covariance = [
                horizontal_variance, 0.0, 0.0,
                0.0, horizontal_variance, 0.0,
                0.0, 0.0, vertical_variance
            ]
            message.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED
        else:
            message.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
    else:
        message.status.status = NavSatStatus.STATUS_NO_FIX
        message.latitude = 0.0
        message.longitude = 0.0
        message.altitude = 0.0
        message.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN

    fix_publisher.publish(message)
    overlay_publisher.publish(String(data=state.summary_text()))


if __name__ == '__main__':
    rospy.init_node('gps_node')

    port = rospy.get_param('~port', PORT)
    baud_rate = int(rospy.get_param('~baud', BAUD))
    frame_id = rospy.get_param('~frame_id', FRAME_ID)
    baud = BAUD_MAP.get(baud_rate, termios.B4800)

    fix_publisher = rospy.Publisher('/fix', NavSatFix, queue_size=5)
    overlay_publisher = rospy.Publisher('/gps/status_text', String, queue_size=5)

    state = GpsState()
    serial_fd = None
    buffer = bytearray()

    while not rospy.is_shutdown():
        if serial_fd is None:
            serial_fd = open_device(port, baud)
            buffer = bytearray()
            state.connected = serial_fd is not None
            publish_fix(fix_publisher, overlay_publisher, state, frame_id)
            if serial_fd is None:
                break

        try:
            readable, _, _ = select.select([serial_fd], [], [], 0.5)
            if serial_fd not in readable:
                publish_fix(fix_publisher, overlay_publisher, state, frame_id)
                continue

            chunk = os.read(serial_fd, READ_SIZE)
            if not chunk:
                raise OSError(errno.ENODEV, 'GPS getrennt')

            buffer.extend(chunk)

            while b'\n' in buffer:
                raw_line, _, buffer = buffer.partition(b'\n')
                line = raw_line.decode('ascii', 'ignore').strip()
                if not line or not line.startswith('$'):
                    continue
                if parse_sentence(line, state):
                    publish_fix(fix_publisher, overlay_publisher, state, frame_id)

        except OSError as exc:
            if exc.errno not in (errno.EAGAIN, errno.EWOULDBLOCK):
                rospy.logwarn('GPS-Lesefehler: %s', exc)
                close_device(serial_fd)
                serial_fd = None
                state.connected = False
                state.valid = False
                state.last_sentence_time = None
                state.satellites = 0
                rospy.sleep(1.0)
        except termios.error as exc:
            rospy.logwarn('GPS-Fehler: %s', exc)
            close_device(serial_fd)
            serial_fd = None
            state.connected = False
            state.valid = False
            state.last_sentence_time = None
            state.satellites = 0
            rospy.sleep(1.0)

    close_device(serial_fd)