Ejemplo n.º 1
0
def job(fileset, messages, max_frames, image_width):
    import cv2
    import cv_bridge
    bridge = cv_bridge.CvBridge()

    topic_counts = dict((x.topic.name, x.msg_count) for x in fileset.bag.topics
                        if x.topic.name in TOPICS)
    intervals = dict((name, int(math.ceil(float(count) / max_frames)))
                     for name, count in topic_counts.items())
    msg_indexes = dict.fromkeys(topic_counts.keys(), 0)
    frame_indexes = dict.fromkeys(topic_counts.keys(), 0)
    logger.debug('Message counts %r', topic_counts)
    logger.debug('Intervals %r', intervals)
    for topic, msg, timestamp in messages:
        msg_idx = msg_indexes[topic]
        msg_indexes[topic] += 1
        if msg_idx % intervals[topic]:
            continue
        frame_idx = frame_indexes[topic]
        frame_indexes[topic] += 1
        image_name = bb.make_job_file(
            'topic{name}_{idx:03d}.jpg'.format(name=topic.replace('/', '_'),
                                               idx=frame_idx))

        cv_image = bridge.imgmsg_to_cv2(msg, "rgb8")
        ratio = float(image_width) / float(cv_image.shape[1])
        scaled_img = cv2.resize(cv_image, (image_width, int(cv_image.shape[0]*ratio)),
                                interpolation=cv2.INTER_AREA)
        cv2.imwrite(image_name, scaled_img, (cv2.IMWRITE_JPEG_QUALITY, 60))
        logger.debug('saved %s', image_name)

    return []
Ejemplo n.º 2
0
def job(fileset, messages):
    import datetime
    from collections import defaultdict
    tempmap = defaultdict(list)

    for topic, msg, timestamp in messages:
        now = datetime.datetime.fromtimestamp(msg.header.stamp.to_sec())
        sensorid = topic.split('/')[-1],
        tempmap[sensorid].append((now, msg.temperature))

    import matplotlib
    # don't attempt to use X11
    matplotlib.use('Agg')
    import matplotlib.pyplot as plt
    from matplotlib import dates

    fig = plt.figure()
    ax1 = fig.gca()
    ax1.xaxis.set_major_formatter(dates.DateFormatter('%H:%M:%S'))
    ax1.xaxis_date()
    fig.autofmt_xdate()

    for sensorid, templist in tempmap.iteritems():
        timestamps, temps = zip(*templist)
        ax1.plot(timestamps, temps, 'o-', label=sensorid[0])

    # add some margin
    xlim = ax1.get_xlim()
    ax1.set_xlim(dates.num2date(xlim[0])-datetime.timedelta(0, 2),
                 dates.num2date(xlim[1])+datetime.timedelta(0, 2))
    ylim = ax1.get_ylim()
    ax1.set_ylim(ylim[0]-2, ylim[1]+2)

    ax1.set_ylabel(u'Temperature [°C]')
    ax1.legend(bbox_to_anchor=(0.75, 1.13))
    path = bb.make_job_file('onewire_temperature.png')
    try:
        fig.savefig(path)
    except:
        logger.warn('could not create temperature plot for %s' % fileset)
        raise
    return []
Ejemplo n.º 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 []