Esempio n. 1
0
def parse_lcm_log(file: str, lcm_types: Dict) -> Dict:
    log.log(f"Parsing lcm log file {file}")

    # Open log as an LCM EventLog object
    lcm_log = EventLog(file, "r")

    first_timestamp = None
    events: Dict[str, Dict[str, List]] = {
    }  # {CHANNEL: {FIELD1: [values], FIELD2: [values]}}

    missing_channels = []
    for event in lcm_log:
        assert isinstance(event, Event)
        # Record the time of the first event as start time
        if not first_timestamp:
            first_timestamp = event.timestamp

        # todo - ignored channels

        log.if_verbose(f"Event on channel: {event.channel}", end="\t")

        # Match the message to an lcm type
        fingerprint = event.data[:8]
        lcm_type = lcm_types.get(fingerprint, None)
        if not lcm_type:
            if event.channel not in missing_channels:
                missing_channels.append(event.channel)
                log.error(
                    f"Unable to find lcm type for events on channel {event.channel}"
                )
            continue
        log.if_verbose(f"-> {lcm_type.__name__}")

        # Decode the message into a python object
        try:
            message = lcm_type.decode(event.data)
        except:
            log.error(f"Error decoding event on channel {event.channel}")
            continue

        # Convert the message into loggable form & store
        message_dict = convert_to_primitive(message,
                                            event.timestamp - first_timestamp)

        # Convert to list of values for each field
        for field in message_dict.keys():
            events.setdefault(event.channel,
                              {}).setdefault(field,
                                             []).append(message_dict[field])

    return events
Esempio n. 2
0
def getInitialStateFromLog(log_file, app):
    app.reset()

    log = EventLog(log_file, "r")
    for i, e in enumerate(log):
        if e.channel == "POSE":
            app.on_pose_msg(e.channel, e.data)
        elif e.channel == "PALLET_LIST":
            app.on_pallet_msg(e.channel, e.data)
        elif e.channel == "GPS_TO_LOCAL":
            app.on_transform_msg(e.channel, e.data)
        elif e.channel == "OBJECT_LIST":
            app.on_objects_msg(e.channel, e.data)

        if i > 5000:
            break

    return app.get_current_state()
Esempio n. 3
0
        checkIgnore = True
    elif o in ("-l", "--lcm_packages="):
        lcm_packages = a.split(",")
    else:
        assert False, "unhandled option"

fullPathName = os.path.abspath(outFname)
dirname = os.path.dirname(fullPathName)
outBaseName = ".".join(os.path.basename(outFname).split(".")[0:-1])
fullBaseName = dirname + "/" + outBaseName

type_db = make_lcmtype_dictionary()

channelsToProcess = re.compile(channelsToProcess)
channelsToIgnore = re.compile(channelsToIgnore)
log = EventLog(fname, "r")

if printOutput:
    sys.stderr.write("opened % s, printing output to %s \n" %
                     (fname, printFname))
    if printFname == "stdout":
        printFile = sys.stdout
    else:
        printFile = open(printFname, "w")
else:
    sys.stderr.write("opened % s, outputing to % s\n" % (fname, outFname))

ignored_channels = []
msgCount = 0
statusMsg = ""
startTime = 0
Esempio n. 4
0
def parse_and_save(args, opts={}):

    if isinstance(args, (list, tuple)):
        fname = args[0]
    elif isinstance(args, str):
        fname = args
    else:
        usage()

    #default options
    lcm_packages = ["botlcm"]

    printFname = "stdout"
    printFile = sys.stdout
    verbose = False
    printOutput = False
    savePickle = False
    printFormat = False
    channelsToIgnore = ""
    checkIgnore = False
    channelsToProcess = ".*"
    separator = ' '
    for o, a in opts:
        if o == "-v":
            verbose = True
        elif o in ("-h", "--help"):
            usage()
        elif o in ("-p", "--print"):
            printOutput = True
        elif o in ("-k", "--pickle"):
            savePickle = True
        elif o in ("-f", "--format"):
            printFormat = True
        elif o in ("-s", "--separator="):
            separator = a
        elif o in ("-o", "--outfile="):
            outFname = a
            printFname = a
        elif o in ("-c", "--channelsToProcess="):
            channelsToProcess = a
        elif o in ("-i", "--ignore="):
            channelsToIgnore = a
            checkIgnore = True
        elif o in ("-l", "--lcm_packages="):
            lcm_packages = a.split(",")
        else:
            assert False, "unhandled option"

    try:
        outFname
    except NameError:
        outDir, outFname = os.path.split(os.path.abspath(fname))

        if savePickle:
            outFname = outDir + "/" + outFname + ".pkl"
        else:
            outFname = outFname.replace(".", "_")
            outFname = outFname.replace("-", "_")
            outFname = outDir + "/" + outFname + ".mat"

    fullPathName = os.path.abspath(outFname)
    dirname = os.path.dirname(fullPathName)
    outBaseName = ".".join(os.path.basename(outFname).split(".")[0:-1])
    fullBaseName = dirname + "/" + outBaseName

    data = {}

    print("Searching for LCM types...")
    type_db = make_lcmtype_dictionary()

    channelsToProcess = re.compile(channelsToProcess)
    channelsToIgnore = re.compile(channelsToIgnore)
    log = EventLog(fname, "r")

    if printOutput:
        sys.stderr.write("opened % s, printing output to %s \n" %
                         (fname, printFname))
        if printFname == "stdout":
            printFile = sys.stdout
        else:
            printFile = open(printFname, "w")
    else:
        sys.stderr.write("opened % s, outputing to % s\n" % (fname, outFname))

    ignored_channels = []
    msgCount = 0
    statusMsg = ""
    startTime = 0

    # Iterate LCM log file
    for e in log:
        if msgCount == 0:
            startTime = e.timestamp

        if e.channel in ignored_channels:
            continue
        if ((checkIgnore and channelsToIgnore.match(e.channel) and len(channelsToIgnore.match(e.channel).group())==len(e.channel)) \
             or (not channelsToProcess.match(e.channel))):
            if verbose:
                statusMsg = deleteStatusMsg(statusMsg)
                sys.stderr.write("ignoring channel %s\n" % e.channel)
            ignored_channels.append(e.channel)
            continue

        packed_fingerprint = e.data[:8]
        lcmtype = type_db.get(packed_fingerprint, None)
        if not lcmtype:
            if verbose:
                statusMsg = deleteStatusMsg(statusMsg)
                sys.stderr.write(
                    "ignoring channel %s -not a known LCM type\n" % e.channel)
            ignored_channels.append(e.channel)
            continue
        try:
            msg = lcmtype.decode(e.data)
        except:
            statusMsg = deleteStatusMsg(statusMsg)
            sys.stderr.write("error: couldn't decode msg on channel %s\n" %
                             e.channel)
            continue

        msgCount = msgCount + 1
        if (msgCount % 5000) == 0:
            statusMsg = deleteStatusMsg(statusMsg)
            statusMsg = "read % d messages, % d %% done" % (
                msgCount, log.tell() / float(log.size()) * 100)
            sys.stderr.write(statusMsg)
            sys.stderr.flush()

        msg_to_dict(data, e.channel, msg, statusMsg, verbose,
                    (e.timestamp - startTime) / 1e6)

    deleteStatusMsg(statusMsg)
    if not printOutput:

        sys.stderr.write("loaded all %d messages, saving to % s\n" %
                         (msgCount, outFname))

        if savePickle:

            # Pickle the list/dictonary using the highest protocol available.
            output = open(outFname, 'wb')
            pickle.dump(data, output, -1)
            output.close()
        else:
            # Matlab format using scipy
            if sys.version_info < (2, 6):
                scipy.io.mio.savemat(outFname, data)
            else:
                scipy.io.matlab.mio.savemat(outFname, data, oned_as='row')

            mfile = open(dirname + "/" + outBaseName + ".m",
                         "w",
                         encoding='utf-8')
            loadFunc = """function [d imFnames]={_outBaseName}()
full_fname = '{_outFname}';
fname = '{_fullPathName}';
if (exist(full_fname,'file'))
    filename = full_fname;
else
    filename = fname;
end
d = load(filename);
""".format(_outBaseName=outBaseName,
            _outFname=outFname,
            _fullPathName=fullPathName)

            print(loadFunc)
            mfile.write(unicode(loadFunc))
            mfile.close()
Esempio n. 5
0
        dist_thresh = float(a)
        globalCheck = True
    else:
        print "unhandled option"
        usage()
        

fullPathName = os.path.abspath(outFname)
dirname = os.path.dirname(fullPathName)
outBaseName = os.path.basename(outFname).split(".")[0] + "_images"
fullBaseName = dirname + "/" + outBaseName

imageDir = fullBaseName

channels = re.compile(channels)
log = EventLog(fname, "r")

sys.stderr.write("Enforcing a distance of %f between images\n" % dist_thresh)
sys.stderr.write("opened % s, outputing to % s\n" % (fname, imageDir))

lc = lcm.LCM()

msgCount = 0
statusMsg = ""
startTime = 0
imageChannels = []
lastPose =pose_t()
lastPose.utime =-1;

imageFnames = {}
imagePoses = {}
Esempio n. 6
0
        checkIgnore = True
    elif o in ("-l", "--lcm_packages="):
        lcm_packages = a.split(",")
    else:
        assert False, "unhandled option"

fullPathName = os.path.abspath(outFname)
dirname = os.path.dirname(fullPathName)
outBaseName = os.path.basename(outFname).split(".")[0]
fullBaseName = dirname + "/" + outBaseName

type_db = LCMTypeDatabase(lcm_packages, verbose)

channelsToProcess = re.compile(channelsToProcess)
channelsToIgnore = re.compile(channelsToIgnore)
log = EventLog(fname, "r")

if printOutput:
    sys.stderr.write("opened % s, printing output to %s \n" % (fname, printFname))
    if printFname == "stdout":
        printFile = sys.stdout
    else:
        printFile = open(printFname, "w")
else:
    sys.stderr.write("opened % s, outputing to % s\n" % (fname, outFname))

ignored_channels = []
msgCount = 0
statusMsg = ""
startTime = 0
Esempio n. 7
0
def parse_log(rndf_fname, log_fname, wait_until_teleport=False):
    app = App(rndf_fname)
    try:
        log = EventLog(log_fname, "r")
    except:
        print "can't read", log_fname
        raise

    pallets = dict()
    for e in log:
        if e.channel == "PALLET_LIST":
            msg = pallet_list_t.decode(e.data)
            for p in msg.pallets:
                if not p.id in pallets:
                    pallets[p.id] = p
                if p.relative_to_id == 0:
                    p.pos = ru.robot_pose_to_rndf(p.pos, app.trans_xyz,
                                                  app.trans_theta,
                                                  app.trans_latlon, app.rndf)
        if e.channel == "GPS_TO_LOCAL":
            app.on_transform_msg(e.channel, e.data)

    #should have all pallets in the first position they were seen
    app.curr_pallets = pallets

    #now that pallets are initialized, restart
    log = EventLog(log_fname, "r")
    agent_path = []
    object_paths = collections.defaultdict(lambda: list())
    obj_id_to_pobj = {}
    agent_prism = None
    sample_frequency_hz = 1
    last_sample_micros = None
    timestamps = []
    teleport_ts = None
    for e in log:
        if e.channel == "SIM_TELEPORT":
            teleport_ts = e.timestamp
        if (teleport_ts == None or e.timestamp - teleport_ts <
            (1 * 1000 * 1000)):
            if wait_until_teleport:
                continue

        if e.channel == "POSE":
            app.on_pose_msg(e.channel, e.data)

        elif e.channel == "PALLET_LIST":
            app.on_pallet_msg(e.channel, e.data)
        elif e.channel == "GPS_TO_LOCAL":
            app.on_transform_msg(e.channel, e.data)
        elif e.channel == "OBJECT_LIST":
            app.on_objects_msg(e.channel, e.data)

        if last_sample_micros == None:
            last_sample_micros = e.timestamp

        if (e.timestamp - last_sample_micros >=
            (1.0 / sample_frequency_hz) * 1000 * 1000):
            state, new_am = app.get_current_state()
            x, y, z = app.curr_location
            if agent_prism == None:
                agent_prism = app.curr_prism

            agent_path.append((x, y, z, app.curr_orientation))
            if state != None:
                last_sample_micros = e.timestamp
                timestamps.append(e.timestamp)
                for pobj_id in state.getObjectsSet():
                    if pobj_id in app.curr_objects:
                        lcm_obj = app.curr_objects[pobj_id]
                    elif pobj_id in app.curr_pallets:
                        lcm_obj = app.curr_pallets[pobj_id]
                    else:
                        raise ValueError()
                    o_vec = app.bot_quat_rotate(lcm_obj.orientation, (1, 0, 0))
                    orientation = math.atan2(o_vec[1], o_vec[0])

                    pobj = state.getGroundableById(pobj_id)
                    object_paths[pobj.lcmId].append(pobj.centroid3d +
                                                    (orientation, ))
                    if not pobj.lcmId in obj_id_to_pobj:
                        obj_id_to_pobj[pobj.lcmId] = pobj

    lcm_parse = LcmParse()
    lcm_parse.agent_obj = PhysicalObject(agent_prism,
                                         tags=["forklift"],
                                         path=Path(
                                             timestamps,
                                             points_xyztheta=tp(agent_path)),
                                         lcmId=FORKLIFT_ID)
    for obj_id, path in object_paths.iteritems():
        if obj_id == FORKLIFT_ID:
            continue  # forklift weirdly appears here, but we deal with it separately.
        pobj = obj_id_to_pobj[obj_id]

        path = Path(timestamps, points_xyztheta=tp(object_paths[pobj.lcmId]))
        pobj.path = path
        pobj.updateRep()
        lcm_parse.pobjs.append(pobj)
        lcm_parse.object_id_to_path[pobj.lcmId] = path

    lcm_parse.places = []
    for place_id in app.curr_state.getPlacesSet():
        lcm_parse.places.append(app.curr_state.getGroundableById(place_id))

    lcm_parse.object_id_to_path[FORKLIFT_ID] = lcm_parse.agent_obj.path

    return lcm_parse
Esempio n. 8
0
from time import time
from lcm import EventLog
from bot_core.planar_lidar_t import planar_lidar_t
from bot_core.image_t import image_t
from xsens.ins_t import ins_t
import sys

fname_in = sys.argv[1]
log = EventLog(fname_in, "r")

print >> sys.stderr, "opened %s" % fname_in

imu_utime = -1

for e in log:

    if e.channel == "IMU":
        imu = ins_t.decode(e.data)

        imu_utime = imu.utime

    if (e.channel == "SKIRT_FRONT" or e.channel == "SKIRT_REAR"):
        lidar = planar_lidar_t.decode(e.data)

        delta = (lidar.utime - imu_utime) * 1E-6
        if (abs(delta) > 0.2):
            print "%s: Difference from IMU is %.4f" % (e.channel, delta)

    if e.channel == "DRAGONFLY_IMAGE":
        image = image_t.decode(e.data)
Esempio n. 9
0
        checkIgnore = True
    elif o in ("-l", "--lcm_packages="):
        lcm_packages = a.split(",")
    else:
        assert False, "unhandled option"

fullPathName = os.path.abspath(outFname)
dirname = os.path.dirname(fullPathName)
outBaseName = ".".join(os.path.basename(outFname).split(".")[0:-1])
fullBaseName = dirname + "/" + outBaseName

type_db = make_lcmtype_dictionary()

channelsToProcess = re.compile(channelsToProcess)
channelsToIgnore = re.compile(channelsToIgnore)
log = EventLog(fname, "r")

if printOutput:
    sys.stderr.write("opened % s, printing output to %s \n" % (fname, printFname))
    if printFname == "stdout":
        printFile = sys.stdout
    else:
        printFile = open(printFname, "w")
else:
    sys.stderr.write("opened % s, outputing to % s\n" % (fname, outFname))

ignored_channels = []
msgCount = 0
statusMsg = ""
startTime = 0
Esempio n. 10
0
    elif o in ("-D", "--global_dist="):
        dist_thresh = float(a)
        globalCheck = True
    else:
        print "unhandled option"
        usage()

fullPathName = os.path.abspath(outFname)
dirname = os.path.dirname(fullPathName)
outBaseName = os.path.basename(outFname).split(".")[0] + "_images"
fullBaseName = dirname + "/" + outBaseName

imageDir = fullBaseName

channels = re.compile(channels)
log = EventLog(fname, "r")

sys.stderr.write("Enforcing a distance of %f between images\n" % dist_thresh)
sys.stderr.write("opened % s, outputing to % s\n" % (fname, imageDir))

lc = lcm.LCM()

msgCount = 0
statusMsg = ""
startTime = 0
imageChannels = []
lastPose = pose_t()
lastPose.utime = -1

imageFnames = {}
imagePoses = {}
Esempio n. 11
0
def plot_control(logfile):
    print "Loading %s..." % logfile
    log = EventLog(logfile, "r")

    start_time = -1

    # controller data
    aux_time = []
    ct_err = []
    ra_err = []
    ct_err_corr = []
    ra_err_corr = []
    steer_command = []
    tv_command = []
    rv_command = []

    # low-level velocity data
    odometry_time = []
    odometry_tv = []
    odometry_rv = []
    velocity_cmd_time = []
    velocity_cmd_tv = []
    velocity_cmd_rv = []

    for event in log:
        if start_time < 0:
            if hasattr(event.data, 'read'):
                buf = event.data
            else:
                buf = StringIO.StringIO(event.data)
            buf.read(8)
            temp = struct.unpack(">q", buf.read(8))
            start_time = temp[0]
            local_time = time.localtime(start_time * 1e-6)
            if local_time.tm_year < 2000:
                start_time = -1
                print "%s has bad utime or does not have utime by definition" % event.channel
            else:
                print "Logging started at %02d:%02d:%02d, %d-%02d-%02d" % (
                    local_time.tm_hour, local_time.tm_min, local_time.tm_sec,
                    local_time.tm_year, local_time.tm_mon, local_time.tm_mday)
        if event.channel == "TRAJECTORY_CONTROLLER_AUX":
            a = trajectory_controller_aux_t.decode(event.data)
            if a.utime > start_time:
                aux_time += [(a.utime - start_time) * 1e-6]
                ct_err += [a.cross_track_error]
                ra_err += [a.relative_angle]
                ct_err_corr += [a.cross_track_error_correction]
                ra_err_corr += [math.degrees(a.relative_angle_correction)]
                steer_command += [math.degrees(a.steer)]
                tv_command += [a.translational_velocity]
                rv_command += [math.degrees(a.rotational_velocity)]

        if event.channel == "ODOMETRY":
            o = raw_odometry_msg_t.decode(event.data)
            if o.utime > start_time:
                odometry_time += [(o.utime - start_time) * 1e-6]
                odometry_tv += [o.tv]
                odometry_rv += [math.degrees(o.rv)]

        if event.channel == "ROBOT_VELOCITY_CMD":
            v = velocity_msg_t.decode(event.data)
            # this part is a hack because aux message was not set correctly before
            if v.utime > start_time:
                velocity_cmd_time += [(v.utime - start_time) * 1e-6]
                velocity_cmd_tv += [v.tv]
                velocity_cmd_rv += [math.degrees(v.rv)]

    t_max_list = []
    t_max_list.append(max(aux_time))
    t_max_list.append(max(odometry_time))
    t_max_list.append(max(velocity_cmd_time))
    t_max = max(t_max_list)

    pyplot.figure()
    pyplot.plot(aux_time, ct_err)
    pyplot.xlabel('Time (sec)')
    pyplot.ylabel('Cross-track Error (m)')
    pyplot.title('Cross-track Error')
    pyplot.axis([0, t_max, -0.1, max(ct_err) + 0.1])

    pyplot.figure()
    pyplot.plot(aux_time, ra_err)
    pyplot.xlabel('Time (sec)')
    pyplot.ylabel('Relative Angle (Error) (degrees)')
    pyplot.title('Relative Angle (Error)')
    pyplot.axis([0, t_max, -0.1, max(ra_err) + 0.1])

    pyplot.figure()
    pyplot.plot(odometry_time, odometry_tv)
    pyplot.plot(velocity_cmd_time, velocity_cmd_tv, color='r')
    pyplot.xlabel('Time (sec)')
    pyplot.ylabel('Translational Velocity (m/s)')
    pyplot.title('Low-level Translational Velocity Control')
    pyplot.axis([0, t_max, -0.1, max(odometry_tv) + 0.1])
    pyplot.legend(('Actual', 'Requested'), 'upper left')

    pyplot.figure()
    pyplot.plot(odometry_time, odometry_rv)
    pyplot.plot(velocity_cmd_time, velocity_cmd_rv, color='r')
    pyplot.xlabel('Time (sec)')
    pyplot.ylabel('Rotational Velocity (deg/s)')
    pyplot.title('Low-level Rotational Velocity Control')
    pyplot.axis([0, t_max, min(odometry_rv) - 5, max(odometry_rv) + 5])
    pyplot.legend(('Actual', 'Requested'), 'upper left')

    pyplot.show()