marv_robotics.trajectory — MARV Robotics 20.06.0 documentation

# Source code for marv_robotics.trajectory

    # Copyright 2016 - 2018  Ternaris.
    # SPDX-License-Identifier: AGPL-3.0-only

    import numpy as np

    import marv_api as marv
    from marv.types import GeoJson
    from .bag import get_message_type, messages

    @marv.node()
    @marv.input('stream', foreach=marv.select(messages, '*:sensor_msgs/NavSatFix'))
    def navsatfix(stream):
        yield marv.set_header(title=stream.topic)
        pytype = get_message_type(stream)
        rosmsg = pytype()
        erroneous = 0
        while True:
            msg = yield marv.pull(stream)
            if msg is None:
                break
            rosmsg.deserialize(msg.data)
            if not hasattr(rosmsg, 'status') or \
               np.isnan(rosmsg.longitude) or \
               np.isnan(rosmsg.latitude) or \
               np.isnan(rosmsg.altitude):
                erroneous += 1
                continue
            # TODO: namedtuple?
            out = {'status': rosmsg.status.status,
                   'lon': rosmsg.longitude,
                   'lat': rosmsg.latitude,
                   'timestamp': rosmsg.header.stamp.to_time()}
            yield marv.push(out)
        if erroneous:
            log = yield marv.get_logger()
            log.warning('skipped %d erroneous messages', erroneous)

    def _create_feature(coords, quality, timestamps):
        if len(coords) == 1:
            coords = coords[0]
            geotype = 'point'
        else:
            geotype = 'line_string'

        color = ((1., 0.00, 0., 1.),  # rgba
                 (1., 0.65, 0., 1.),
                 (0., 0.00, 1., 1.),
                 (0., 1.00, 0., 1.))[quality]
        return {
            'properties': {
                'color': color,
                'width': 4.,
                'timestamps': timestamps,
                'markervertices': [c * 30 for c in (0., 0., -1., .3, -1., -.3)]},
            'geometry': {geotype: {'coordinates': coords}},
        }

    @marv.node(GeoJson)
    @marv.input('navsatfixes', default=navsatfix)
    def trajectory(navsatfixes):
        # Only one topic for now
        navsatfix = yield marv.pull(navsatfixes)  # pylint: disable=redefined-outer-name
        if not navsatfix:
            raise marv.Abort()
        yield marv.set_header(title=navsatfix.title)
        features = []
        quality = None
        coords = []
        timestamps = []
        while True:
            msg = yield marv.pull(navsatfix)
            if msg is None:
                break

            dt = msg['timestamp']  # pylint: disable=invalid-name
            timestamps.append(int(dt * 1e9))

            # Whether to output an augmented fix is determined by both the fix
            # type and the last time differential corrections were received.  A
            # fix is valid when status >= STATUS_FIX.
            # STATUS_NO_FIX =  -1 -> unable to fix position       -> color id 0 = red
            # STATUS_FIX =      0 -> unaugmented fix              -> color id 1 = orange
            # STATUS_SBAS_FIX = 1 -> satellite-based augmentation -> color id 2 = blue
            # STATUS_GBAS_FIX = 2 -> ground-based augmentation    -> color id 3 = green
            #                     -> unknown status id            -> color id 4 = black
            if -1 <= msg['status'] <= 2:
                new_quality = msg['status'] + 1
            else:
                new_quality = 4

            # start new feature if quality changed
            if quality != new_quality:
                if coords:
                    features.append(_create_feature(coords, quality, timestamps))
                quality = new_quality
                coords = []
                timestamps = []

            coords.append((msg['lon'], msg['lat']))

        if coords:
            features.append(_create_feature(coords, quality, timestamps))

        if features:
            out = {'feature_collection': {'features': features}}
            yield marv.push(out)