Beispiel #1
0
def job(fileset, messages, foo):
    """Different forms of annotations"""
    logger.debug(__name__)
    for topic, msg, timestamp in messages:
        if topic == 'annotations':
            yield Annotation(data=str(msg.data), timestamp=0)
        else:
            yield Annotation(data=str(msg.data), timestamp=timestamp.to_sec())
    logger.error('err %s', foo)
    logger.warn(' warn %s', foo)
    logger.info('info %s', foo)
    logger.debug('debug %s', foo)
Beispiel #2
0
def job(fileset, messages):
    if not fileset.bag:
        return

    name_topic = '/robot_name/name'
    messages = messages \
        if any(True for x in fileset.bag.topics if x.topic.name == name_topic) \
        else ()
    for topic, msg, timestamp in messages:
        if topic == name_topic:
            try:
                robot_name = msg.data
            except AttributeError:
                robot_name = msg.robot_name
            logger.debug('found robot_name via topic: %s' % msg)
            use_case = ''
            break
    else:
        path = fileset.dirpath.split(os.sep)
        robot_name = path[3] if len(path) > 3 else 'unknown'
        use_case = path[6] if len(path) > 6 else 'unknown'

    logger.info('robot_name=%s, use_case=%s', robot_name, use_case)
    yield Metadata(robot_name=robot_name, use_case=use_case)
Beispiel #3
0
def job(fileset, messages):
    position_topics = [x.topic.name for x in fileset.bag.topics
                       if x.topic.name in POSITION_TOPICS]
    orientation_topics = [x.topic.name for x in fileset.bag.topics
                          if x.topic.name in ORIENTATION_TOPICS]
    if not position_topics:
        logger.debug('no gps topic found')
        return []
    logger.info('starting with {} and {}'.format(position_topics,
                                                 orientation_topics))

    import datetime
    import pyproj
    import matplotlib.dates as md
    import matplotlib.pyplot as plt
    from matplotlib import cm
    import numpy as np

    proj = pyproj.Proj(proj='utm', zone=32, ellps='WGS84')

    class Position(object):
        def __init__(self):
            self.e_offset = 0
            self.n_offset = 0
            self.u_offset = 0
            self.gps = []

        def update(self, msg):
            e, n = proj(msg.longitude, msg.latitude)

            if self.e_offset == 0:
                self.e_offset, self.n_offset, self.u_offset = e, n, msg.altitude

            e, n, u = (e - self.e_offset,
                       n - self.n_offset,
                       msg.altitude - self.u_offset)

            self.gps.append([
                msg.header.stamp.to_sec(),
                msg.latitude,
                msg.longitude,
                msg.altitude,
                e, n, u,
                msg.status.status,
                np.sqrt(msg.position_covariance[0])
            ])

    class Orientation(object):
        def __init__(self):
            self.orientation = []

        def update(self, msg):
            if hasattr(msg, 'yaw') and not np.isnan(msg.yaw):
                self.orientation.append([
                    msg.header.stamp.to_sec(),
                    msg.yaw
                    ])
            elif hasattr(msg, 'orientation') and not np.isnan(msg.orientation.x):
                self.orientation.append([
                    msg.header.stamp.to_sec(),
                    self.yaw_angle(msg.orientation)
                ])

        # calculate imu orientation
        @staticmethod
        def yaw_angle(frame):
            rot = np.zeros((3, 3))

            # consists of time, x, y, z, w
            q1 = frame.x
            q2 = frame.y
            q3 = frame.z
            q4 = frame.w

            rot[0, 0] = 1 - 2 * q2 * q2 - 2 * q3 * q3
            rot[0, 1] = 2 * (q1 * q2 - q3 * q4)
            rot[0, 2] = 2 * (q1 * q3 + q2 * q4)
            rot[1, 0] = 2 * (q1 * q2 + q3 * q4)
            rot[1, 1] = 1 - 2 * q1 * q1 - 2 * q3 * q3
            rot[1, 2] = 2 * (q2 * q3 - q1 * q4)
            rot[2, 0] = 2 * (q1 * q3 - q2 * q4)
            rot[2, 1] = 2 * (q1 * q4 + q2 * q3)
            rot[2, 2] = 1 - 2 * q1 * q1 - 2 * q2 * q2

            vec = np.dot(rot, [1, 0, 0])

            # calculate the angle
            return np.arctan2(vec[1], vec[0])

    positionMap = {position_topic: Position() for position_topic in position_topics}
    orientationMap = {orientation_topic: Orientation()
                      for orientation_topic in orientation_topics}
    erroneous_msg_count = defaultdict(int)

    for topic, msg, timestamp in messages:
        if topic in position_topics:
            # skip erroneous messages
            if np.isnan(msg.longitude) or \
               np.isnan(msg.latitude) or \
               np.isnan(msg.altitude):
                erroneous_msg_count[topic] += 1
                continue

            if hasattr(msg, 'status'):
                positionMap[topic].update(msg)
            else:
                raise Exception('Invalid position topic')
        elif topic in orientation_topics:
            orientationMap[topic].update(msg)

    if erroneous_msg_count:
        logger.warn('Skipped erroneous GNSS messages %r', erroneous_msg_count.items())

    for position_topic, orientation_topic in product(
            position_topics, orientation_topics
    ):
        gps = np.array(positionMap[position_topic].gps)

        if not len(gps):
            logger.error('Aborting due to missing gps messages on topic %s',
                         position_topic)
            continue

        if orientationMap[orientation_topic].orientation:
            orientation = np.array(orientationMap[orientation_topic].orientation)
        else:
            logger.warn('No orientation messages on topic %s', orientation_topic)

        # plotting
        fig = plt.figure()
        fig.subplots_adjust(wspace=0.3)

        ax1 = fig.add_subplot(1, 3, 1)  # e-n plot
        ax2 = fig.add_subplot(2, 3, 2)  # orientation plot
        ax3 = fig.add_subplot(2, 3, 3)  # e-time plot
        ax4 = fig.add_subplot(2, 3, 5)  # up plot
        ax5 = fig.add_subplot(2, 3, 6)  # n-time plot

        # masking for finite values
        gps = gps[np.isfinite(gps[:, 1])]

        # precompute plot vars
        c = cm.prism(gps[:, 7]/2)

        ax1.scatter(gps[:, 4], gps[:, 5], c=c, edgecolor='none', s=3,
                    label="green: RTK\nyellow: DGPS\nred: Single")

        xfmt = md.DateFormatter('%H:%M:%S')
        ax2.xaxis.set_major_formatter(xfmt)
        ax3.xaxis.set_major_formatter(xfmt)
        ax4.xaxis.set_major_formatter(xfmt)
        ax5.xaxis.set_major_formatter(xfmt)

        if orientationMap[orientation_topic].orientation:
            ax2.plot([datetime.datetime.fromtimestamp(timestamp)
                      for timestamp in orientation[:, 0]], orientation[:, 1])

        ax3.plot([datetime.datetime.fromtimestamp(timestamp)
                  for timestamp in gps[:, 0]], gps[:, 4])
        ax4.plot([datetime.datetime.fromtimestamp(timestamp)
                  for timestamp in gps[:, 0]], gps[:, 6])
        ax5.plot([datetime.datetime.fromtimestamp(timestamp)
                  for timestamp in gps[:, 0]], gps[:, 5])

        fig.autofmt_xdate()

        # add the legends
        ax1.legend(loc="best")

        ax1.set_ylabel('GNSS northing [m]')
        ax1.set_xlabel('GNSS easting [m]')
        ax2.set_ylabel('Heading over time [rad]')
        ax3.set_ylabel('GNSS easting over time [m]')
        ax4.set_ylabel('GNSS height over time [m]')
        ax5.set_ylabel('GNSS northing over time [m]')

        fig.set_size_inches(16, 9)
        path = bb.make_job_file(position_topic.replace("/", "_")+'.jpg')

        try:
            fig.savefig(path)
        except:
            logger.warn(gps[:, 4])
            logger.warn(gps[:, 5])
            logger.warn(gps[:, 6])
            raise
        finally:
            plt.close()
    return []