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
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()
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
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()
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 = {}
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
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
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)
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
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 = {}
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()
