def stripLog(inFile, outFile):
channels = {
'CAMERA': True,
'SCAN': True,
'PRE_SPINDLE_TO_POST_SPINDLE': True
}
inLog = lcm.EventLog(inFile, 'r')
outLog = lcm.EventLog(outFile, 'w', overwrite=True)
for event in inLog:
if event.channel in channels:
outLog.write_event(event.timestamp, event.channel, event.data)
print 'done'
def process(self):
if self.filename is not None:
# we have a file to plot!
# process all the observations immediately...
#self.lcm = lcm.LCM("file://{}?speed=0".format(self.filename))
lcm_log = lcm.EventLog(self.filename)
# TODO: thread this off to handle import in other thread
total_size = lcm_log.size()
last = 0
step = 5
for event in lcm_log:
self.handle_message(event.channel, event.data)
# decode progress
percent = 100 * lcm_log.tell() / total_size
if percent >= last + step:
print "{:3}".format(percent)
last += step
self.name = self.filename # perhaps change this to basename of the file?
else:
# TODO: clean up LCM handling here
# assume using live plots otherwise
self.lcm = lcm.LCM()
self.name = "Live"
self.lcm.subscribe(".*", self.handle_message)
# we still need to handle the LCM object to get the messages
self.lcm_thread = LCMThread(self.lcm)
self.lcm_thread.start()
def main():
my_lcm = lcm.LCM()
path = "data.log"
msg_logger = lcm.EventLog(path, 'w', overwrite=True)
def quit_handler(signum, _):
print "received SIGQUIT, doing graceful shutting down.."
msg_logger.close()
sys.exit(0)
signal.signal(getattr(signal, 'SIGQUIT', signal.SIGTERM), quit_handler)
def int_handler(signum, _):
print "received SIGINT, doing force shutting down.."
sys.exit(1)
signal.signal(signal.SIGINT, int_handler)
def msg_handler(channel, msg):
timestamp = time.time()
print 'Receive:', timestamp, channel, msg
msg_logger.write_event(timestamp, channel, msg)
# subscribe all channels.
my_lcm.subscribe(".*", msg_handler)
try:
while (not my_lcm.handle()):
pass
except CError:
pass
def write_isam_factors(self):
if not self.sol:
print '{Surveyor} writing isam factors requires a prior... call solve first'
return
else:
print '{Surveyor} writing isam factors to lcmlog'
log = lcm.EventLog('lcmlog-surveyor', 'w', overwrite=True)
for ii in range(self.nbeacons):
write_isam_prior3d(log, ii, self.sol[ii])
for ii in range(len(self.owtt_utime)):
b_ii = np.where(self.good_ranges[ii, :])[0]
if not len(b_ii): continue
ut = self.owtt_utime[ii]
# write surveyor pose
x_g = np.interp(ut, self.xyz_utime, self.xyz[:, 0])
y_g = np.interp(ut, self.xyz_utime, self.xyz[:, 1])
z_g = np.interp(ut, self.xyz_utime, self.xyz[:, 2])
write_isam_prior3d(log, ut, [x_g, y_g, z_g])
for b in b_ii:
write_isam_range3d(log, ut, b,
speed_of_sound * self.owtt[ii, b])
def trimLog(inLog, outLog, numMsgs=15000):
log = lcm.EventLog(inLog, "r")
new_log = lcm.EventLog(outLog, 'w', overwrite=True)
msg_types = ["POSE", "PALLET_LIST", "GPS_TO_LOCAL", "OBJECT_LIST"]
for i, e in enumerate(log):
if e.channel in msg_types:
new_log.write_event(e.timestamp, e.channel, e.data)
else:
continue
if i > numMsgs:
new_log.close()
return
def main():
import lcm
import sys
logfile = sys.argv[1]
log = lcm.EventLog(logfile, "r")
print_log_summary(logfile, log)
def getStartEndTime(logfile):
log = lcm.EventLog(logfile, 'r')
size = log.size()
# seek to (end minus 5 MB )
position = max(0, size - 5e6)
# get start time
event = log.read_next_event()
startTime = event.timestamp
# go to the end-ish of file to look for end time
log.seek(position)
event = log.read_next_event()
while event:
event = log.read_next_event()
if event:
endTime = event.timestamp
# clean up
log.close()
return (startTime, endTime)
def parseLog(self):
log = lcm.EventLog(self.logFile, 'r')
print 'Log size: ' + sizeof_fmt(log.size())
log.seek(0)
while True:
event = log.read_next_event()
if not event:
break
timestamp = event.timestamp
if event.channel == 'EST_ROBOT_STATE':
msg = spy.decodeMessage(event.data)
self.jointVelocityTimes.append(timestamp)
self.jointVelocityNorms.append(
np.linalg.norm(msg.joint_velocity))
elif event.channel == 'ATLAS_BATTERY_DATA':
msg = spy.decodeMessage(event.data)
self.batteryTimes.append(timestamp)
self.batteryPercentage.append(msg.remaining_charge_percentage)
elif event.channel == 'ATLAS_STATUS':
msg = spy.decodeMessage(event.data)
self.pressureTimes.append(timestamp)
self.pressureReadings.append(msg.pump_supply_pressure)
print 'parsed ' + str(len(self.jointVelocityNorms)) + ' robot states'
print 'parsed ' + str(len(self.batteryPercentage)) + ' battery states'
print 'parsed ' + str(len(self.pressureReadings)) + ' pump readings'
def convert(in_filename, out_filename):
log = lcm.EventLog(in_filename, "r")
seq = 0
prev_time = None
with rosbag.Bag(out_filename, 'w') as bag:
for event in log:
ros_msg = None
channel = None
if event.channel == "ODOMETRY":
lcm_msg = odometry_t.decode(event.data)
ros_msg = convert_pose(lcm_msg)
channel = "odometry"
elif event.channel == "SLAM_POSE":
lcm_msg = pose_xyt_t.decode(event.data)
ros_msg = convert_pose(lcm_msg)
channel = "slam_pose"
elif event.channel == "SLAM_MAP":
lcm_msg = occupancy_grid_t.decode(event.data)
ros_msg = convert_map(lcm_msg, prev_time)
channel = "map"
elif event.channel == "MBOT_IMU":
lcm_msg = mbot_imu_t.decode(event.data)
ros_msg = convert_imu(lcm_msg)
channel = "imu"
else:
continue
ros_msg.header.seq = seq
seq += 1
prev_time = ros_msg.header.stamp
bag.write(channel, ros_msg, t=ros_msg.header.stamp)
def convert_log(src, dest):
srclog = lcm.EventLog(src)
destlog = lcm.EventLog(dest, "w", overwrite=True)
for event in srclog:
if event.channel == "KINECT_POINTS_REDUCED":
msg = kinect.pointcloud_t.decode(event.data)
core_msg = bot_core.pointcloud_t()
core_msg.utime = msg.timestamp
core_msg.n_points = msg.num // 2
core_msg.points = [[msg.x[i], msg.y[i], msg.z[i]]
for i in range(0, msg.num, 2)]
core_msg.n_channels = 3
core_msg.channel_names = ["r", "g", "b"]
core_msg.channels = [[v[i] for i in range(1, msg.num, 2)]
for v in [msg.x, msg.y, msg.z]]
data = core_msg.encode()
else:
data = event.data
destlog.write_event(event.timestamp, event.channel, data)
srclog.close()
destlog.close()
def printLogFileDescription(filename):
log = lcm.EventLog(filename, 'r')
print 'reading %s' % filename
print 'log file size: %.2f MB' % (log.size()/(1024.0**2))
for event in log:
onLCMMessage(event.channel, event.data)
log.close()
printLCMCatalog()
def replace_timestamps_with_log_times(manager,
input_log,
output_log,
overwrite=False):
warned_channels = set()
if not isinstance(input_log, lcm.EventLog):
input_log = lcm.EventLog(input_log, "r")
if not isinstance(output_log, lcm.EventLog):
output_log = lcm.EventLog(output_log, "w", overwrite=overwrite)
for event in input_log:
try:
msg = manager.decode_event(event)
for field in ["utime", "timestamp"]:
if hasattr(msg, field):
setattr(msg, field, event.timestamp)
break
output_log.write_event(event.timestamp, event.channel,
msg.encode())
except FingerprintNotFoundException:
if event.channel not in warned_channels:
print "Warning: fingerprint not found for message on channel: {:s}".format(
event.channel)
warned_channels.add(event.channel)
def updateLogInfo(self, filename):
fieldData = self.catalog.get(filename)
if not fieldData:
print 'discovered new file:', filename
fieldData = FieldData(filename=filename, fileSize=0, lastFilePos=0, channelTypes={})
self.catalog[filename] = fieldData
self.cropUtimeMap(self.utimeMap, self.cropTimeWindow)
log = lcm.EventLog(filename, 'r')
fileSize = log.size()
# if the log file is the same size as the last time it was inspected
# then there is no more work to do, return.
if fileSize == fieldData.fileSize:
return
fieldData.fileSize = fileSize
# seek to the last processed event, if one exists, then read past it
if fieldData.lastFilePos > 0:
log.seek(fieldData.lastFilePos)
event = log.read_next_event()
while True:
filepos = log.tell()
event = log.read_next_event()
if not event:
break
fieldData.lastFilePos = filepos
timestamp = event.timestamp
channel = event.channel
if channel == self.videoChannel:
self.utimeMap[timestamp] = (filename, filepos)
# maintain a catalog of channelName->messageType
#if channel not in fieldData.channelTypes:
# messageClass = spy.getMessageClass(event.data)
# fieldData.channelTypes[channel] = messageClass
log.close()
def getImage(self, utime):
filename, filepos = self.utimeMap[utime]
log = self.logs.get(filename)
if log is None:
log = lcm.EventLog(filename, 'r')
self.logs[filename] = log
log.seek(filepos)
event = log.read_next_event()
msg = spy.decodeMessage(event.data)
if hasattr(msg, 'images'):
msg = msg.images[0]
return msg, filename
def readLog(self, filename, eventTimeFunction=None, progressFunction=None):
log = lcm.EventLog(filename, 'r')
self.log = log
timestamps = []
filePositions = []
offsetIsDefined = False
timestampOffset = 0
lastEventTimestamp = 0
nextProgressTime = 0.0
while True:
filepos = log.tell()
event = log.read_next_event()
if not event:
break
if eventTimeFunction:
eventTimestamp = eventTimeFunction(event)
else:
eventTimestamp = event.timestamp
if eventTimestamp is None:
eventTimestamp = lastEventTimestamp
elif not offsetIsDefined:
timestampOffset = eventTimestamp
offsetIsDefined = True
lastEventTimestamp = eventTimestamp
timestamp = eventTimestamp - timestampOffset
if progressFunction:
progressTime = timestamp * 1e-6
if progressTime >= nextProgressTime:
nextProgressTime += 1.0
if not progressFunction(timestamp * 1e-6):
break
filePositions.append(filepos)
timestamps.append(timestamp)
self.filePositions = filePositions
self.timestamps = np.array(timestamps)
self.timestampOffset = timestampOffset
def read_log(path, jlist, channel):
# store boolean mask of target joint names
joint_mask = []
joint_names = []
# matrix containing joint position values of target joints and time stamps
data = []
time = []
log = lcm.EventLog(path)
for event in log:
if event.channel == channel:
# decode message based on channel
if channel == "CORE_ROBOT_STATE":
msg = joint_state_t.decode(event.data)
if channel == "EST_ROBOT_STATE":
msg = robot_state_t.decode(event.data)
sample = []
# find target joints indices once
if not joint_mask:
for n in msg.joint_name:
joint_mask.append(n in jlist)
#print "mask", joint_mask
#print "joints", np.asarray(msg.joint_name)[np.where(joint_mask)[0]]
joint_names = np.asarray(
msg.joint_name)[np.where(joint_mask)[0]]
# store values from current message
time.append(msg.utime / 1000000.0)
sample.append(
np.asarray(
msg.joint_position)[np.where(joint_mask)[0]].tolist())
sample = [item for sublist in sample for item in sublist]
# add sample
data.append(sample)
return np.asarray(data), np.asarray(time), joint_names
def process_one_log(args):
"""
Walk through one lcm log converting desired channels to a rosbag.
Args:
log_path (str): Path to raw log file to convert to ROS
rosbag_path (str): Path to rosbag to be created
settings (dict): Container for the channels to write and their channel-specific settings
"""
log_path, rosbag_path, csv_path, settings_static = args
settings = settings_static.copy()
lcm_channels = settings["lcm_channels"]
# open the lcm log
try:
log = lcm.EventLog(log_path, "r")
except IOError:
sys.stderr.write("Unable to read " + log_path)
sys.exit(1)
# Set image timestamps
poseList = np.loadtxt(open(csv_path, "rb"), delimiter=",")
settings["image_timestamps"] = poseList[:,0]
# open the rosbag writer object
with RosbagWriter(rosbag_path, settings) as bag_writer:
# walk through and process the messages in the lcm log
#t = tqdm(total=log.size(), bar_format="{postfix[1][value]:6.0f}% {postfix[0]}",
# postfix=[os.path.basename(log_path), dict(value=0)])
# walk through the log events and save them to bag.
for event in log:
#t.postfix[1]["value"] = float(log.tell())/log.size()*100
#t.update()
if event.channel in lcm_channels:
bag_writer.write_message(event.channel, event.data)
def __init__(self, logfile):
log = lcm.EventLog(logfile)
print '{Iver} parsing acomms range events...'
self.acomms = parse_from_lcm(log, 'ACOMMS_RANGE', Acomms_range_series)
print '{Iver} parsing dstar events...'
self.dstar = parse_from_lcm(log, 'DESERT_STAR', Dstar_series)
print '{Iver} parsing gpsd events...'
#self.gpsd = parse_from_lcm (log, 'GPSD3', Gpsd_series)
self.gpsd = parse_from_lcm(log, 'TOPSIDE_GPSD3', Gpsd_series)
print '{Iver} parsing osm vis events...'
self.osm = parse_from_lcm(log, 'OSM_VIS', Osm_vis_series)
print '{Iver} parsing rdi events...'
self.rdi = parse_from_lcm(log, 'RDI', Rdi_series)
print '{Iver} parsing iver_state events...'
self.state = parse_from_lcm(log, 'STATE', Iver_state_series)
print '{Iver} parsing microstrain events...'
self.ustrain = parse_from_lcm(log, 'MICROSTRAIN_ATTITUDE_BIAS',
Rph_series)
import lcm
from imitationlearning.lcmparser.gokart_status_event import GokartStatusEvent
from imitationlearning.lcmparser.seye_aedvs_event import SeyeAeApsEvent
if __name__ == '__main__':
log_path = "/media/ale/dubi_usb1/20181005T135151_1cb189b4.lcm.00"
log = lcm.EventLog(log_path, 'r')
for event in log:
if "gokart.status.get" in event.channel and False:
# msg = BinaryBlob.decode(event.data)
gokart_status_event = GokartStatusEvent(event.data)
if "seye.overview.aeaps" in event.channel:
print("reading events")
events_event = SeyeAeApsEvent(event.data)
#%%
import os
import lcm
import numpy as np
import matplotlib.pyplot as plt
from drake import lcmt_iiwa_status, lcmt_iiwa_command
#%%
# log_path = os.path.join("/Users", "pangtao", "PycharmProjects",
# "lcmlog-2021-04-26.01")
log_path = os.path.join("/Users", "pangtao", "PycharmProjects",
"lcmlog-2021-05-15.02")
lcm_log = lcm.EventLog(log_path)
#%%
iiwa_status_list = []
iiwa_cmd_list = []
# extract time stamps and utimes of status and command messages from log file.
for event in lcm_log:
if event.channel == "IIWA_STATUS":
msg = lcmt_iiwa_status.decode(event.data)
iiwa_status_list.append((event.timestamp, msg.utime))
elif event.channel == "IIWA_COMMAND":
msg = lcmt_iiwa_command.decode(event.data)
iiwa_cmd_list.append((event.timestamp, msg.utime))
#%% match time stamps
# make sure iiwa_cmd_list[0].utime >= iiwa_status_list[0].utime
def __init__(self, logfile, channel):
print '{Iver} parsing estimator events from %s...' % channel
log = lcm.EventLog(logfile)
self.pose = parse_from_lcm(log, channel, Position_series)
#!/usr/bin/env python
import sys
import lcm
from lab3lcm import imu_t
if len(sys.argv) < 2:
sys.stderr.write("usage: log_file_reader.py <logfile>\n")
sys.exit(1)
log = lcm.EventLog(sys.argv[1], "r") # set log from log file specified
c = 1
for event in log: # look at all events logged
if event.channel == "IMU": # check for events logged on channel name "GPS"
msg = imu_t.decode(event.data) # read event data from GPS events
c += 1
print("IMU Data:")
print(" yaw = %s" % str(msg.yaw))
print(" pitch = %s" % str(msg.pitch))
print(" roll = %s" % str(msg.roll))
print(" mag x = %s" % str(msg.mag_x))
print(" mag y = %s" % str(msg.mag_y))
print(" mag z = %s" % str(msg.mag_z))
print("count: %d" % c)
print("")
# flatten the telemetry into a list of messages
telemetry = []
for tc in conf.telemetry:
for t in tc['messages']:
if not (t['has_key']('logger') and t['logger'] == "ignore"):
telemetry.append(t)
# open the log
full_logfile = genconfig.logfile_directory + "/" + logfile + '_' + str(
args.airframe) + genconfig.logfile_extension
if not args.quiet:
print 'opening logfile \"' + full_logfile + '\"'
try:
log = lcm.EventLog(full_logfile, mode="w", overwrite=args.overwrite)
except ValueError as ve:
print ve
exit(1)
# archive the conf folder
tar = tarfile.open(
genconfig.logfile_directory + "/" + logfile + "_conf.tar.gz", "w:gz")
tar.add(genconfig.config_folder, arcname=logfile + "_conf")
tar.close()
# subscribe to all messages
def handler(channel, data):
log.write_event(long(time.time() * 1e6 + 0.5), channel, data)
import pickle
sys.path.append("lcmtypes")
from lcmtypes import mbot_encoder_t
from lcmtypes import mbot_imu_t
from lcmtypes import mbot_motor_command_t
from lcmtypes import odometry_t
from lcmtypes import pose_xyt_t
from lcmtypes import robot_path_t
from lcmtypes import timestamp_t
if len(sys.argv) < 2:
sys.stderr.write("usage: read-log <logfile>\n")
sys.exit(1)
log = lcm.EventLog(sys.argv[1], "r")
data = {
'slam_pose': {
'x': [],
'y': [],
'th': [],
'time': []
},
'odometry': {
'x': [],
'y': [],
'th': [],
'time': []
}
}
from perls.lcmtypes.perllcm import isam2_f_pose_pose_partial_t
from perls.lcmtypes.perllcm import isam2_f_glc_t
if __name__ == '__main__':
if len(sys.argv) < 4:
print 'usage: %s <est_ceif.pkl> <est_deif.pkl> <lcmlog-client-gm>' % sys.argv[
0]
sys.exit(0)
with open(sys.argv[1], 'rb') as f:
estc = pickle.load(f)
with open(sys.argv[2], 'rb') as f:
estd = pickle.load(f)
logc = lcm.EventLog(sys.argv[3])
f_owtt = [
isam2_f_pose_pose_partial_t.decode(e.data) for e in logc
if e.channel == 'F_RANGE2D'
]
t_ds = np.asarray([f.utime for f in f_owtt])
#f_ds = [isam2_f_glc_t.decode (e.data) for e in logc if e.channel == 'F_DS2D']
#t_ds = np.asarray ([f.utime for f in f_ds])
# plot paths
fig_path = plt.figure()
ax_path = fig_path.add_subplot(111)
estc.plot(ax_path, 'b.-', label='CEIF')
estd.plot(ax_path, 'r.-', label='DEIF')
def main():
parser = argparse.ArgumentParser(
description="Convert an LCM log to a ROS bag (mono/stereo images only)."
)
parser.add_argument('--input', help='Input LCM log.')
parser.add_argument('--namespace', help='camera namespace')
parser.add_argument('--cam_yml',
help='Image calibration YAML file from ROS calibrator')
parser.add_argument('--visualise', help='Visualise image', default=False)
args = parser.parse_args()
print "Converting images in %s to ROS bag file..." % (args.input)
log = lcm.EventLog(args.input, 'r')
bag = rosbag.Bag(args.input + '.converted.bag',
mode='w',
compression='lz4')
bridge = CvBridge()
# # Read in YAML files.
with open(args.cam_yml) as f:
config = yaml.load(f)
lcm_channels = ['MICROSTRAIN_RAW', 'XTION']
try:
count = 0
with click.progressbar(length=log.size()) as bar:
for event in log:
event_stamp = rospy.Time().from_sec(
float(event.timestamp) / 1e6)
if event.channel == 'XTION':
lcm_msg = rgbd_t.decode(event.data)
rgb_img = cv2.imdecode(
np.fromstring(lcm_msg.rgb, np.uint8), cv2.IMREAD_COLOR)
rgb_img = cv2.cvtColor(rgb_img, cv2.COLOR_RGB2BGR)
if args.visualise:
cv2.imshow('RGB', rgb_img)
cv2.waitKey(10)
depth_img = np.fromstring(zlib.decompress(lcm_msg.depth),
np.uint16).reshape(
lcm_msg.height,
lcm_msg.width)
rgb_ros_msg = bridge.cv2_to_imgmsg(rgb_img, 'bgr8')
rgb_ros_msg.header.seq = event.eventnum
secs_float = float(lcm_msg.utime) / 1e6
nsecs_float = (secs_float - np.floor(secs_float)) * 1e9
rgb_ros_msg.header.stamp.secs = int(secs_float)
rgb_ros_msg.header.stamp.nsecs = int(nsecs_float)
rgb_ros_msg.header.frame_id = args.namespace
depth_ros_msg = bridge.cv2_to_imgmsg(depth_img)
depth_ros_msg.header.seq = event.eventnum
depth_ros_msg.header = rgb_ros_msg.header
camera_info = CameraInfo()
camera_info.header = rgb_ros_msg.header
camera_info.height = rgb_img.shape[0]
camera_info.width = rgb_img.shape[1]
camera_info.distortion_model = 'plumb_bob'
camera_info.K = config["K"]
bag.write(args.namespace + '/rgb/image_raw', rgb_ros_msg,
event_stamp)
bag.write(args.namespace + '/depth/image_rect',
depth_ros_msg, event_stamp)
bag.write(args.namespace + '/camera_info', camera_info,
event_stamp)
if event.channel == 'MICROSTRAIN_RAW':
lcm_msg = raw_t.decode(event.data)
imu_ros_msg = Imu()
secs_float = float(lcm_msg.utime) / 1e9
nsecs_float = (secs_float - np.floor(secs_float)) * 1e9
imu_ros_msg.header.seq = event.eventnum
imu_ros_msg.header.stamp.secs = int(secs_float)
imu_ros_msg.header.stamp.nsecs = int(nsecs_float)
imu_ros_msg.header.frame_id = "imu"
imu_ros_msg.linear_acceleration.x = lcm_msg.accel[0]
imu_ros_msg.linear_acceleration.y = lcm_msg.accel[1]
imu_ros_msg.linear_acceleration.z = lcm_msg.accel[2]
imu_ros_msg.angular_velocity.x = lcm_msg.gyro[0]
imu_ros_msg.angular_velocity.y = lcm_msg.gyro[1]
imu_ros_msg.angular_velocity.z = lcm_msg.gyro[2]
# Store magnetometer in magnetometer msg
mag_msg = MagneticField()
mag_msg.header = imu_ros_msg.header
mag_msg.magnetic_field.x = lcm_msg.mag[0]
mag_msg.magnetic_field.y = lcm_msg.mag[1]
mag_msg.magnetic_field.z = lcm_msg.mag[2]
# Ignore pressure for now
# lcm_msg.pressure
bag.write('imu_raw', imu_ros_msg, event_stamp)
bag.write('imu_mag', mag_msg, event_stamp)
bar.update(event.__sizeof__() + event.data.__sizeof__())
finally:
log.close()
bag.close()
print("Done.")
def load_log(self, filename):
return lcm.EventLog(self.filename, 'r')
def main():
config_file = \
'bindings/pydairlib/analysis/plot_configs/cassie_default_plot.yaml'
plot_config = CassiePlotConfig(config_file)
use_floating_base = plot_config.use_floating_base
use_springs = plot_config.use_springs
channel_x = plot_config.channel_x
channel_u = plot_config.channel_u
channel_osc = plot_config.channel_osc
''' Get the plant '''
plant, context = cassie_plots.make_plant_and_context(
floating_base=use_floating_base, springs=use_springs)
pos_map, vel_map, act_map = mbp_plots.make_name_to_mbp_maps(plant)
pos_names, vel_names, act_names = mbp_plots.make_mbp_name_vectors(plant)
''' Read the log '''
filename = sys.argv[1]
log = lcm.EventLog(filename, "r")
robot_output, robot_input, osc_debug = \
get_log_data(log, # log
cassie_plots.cassie_default_channels, # lcm channels
plot_config.end_time,
mbp_plots.load_default_channels, # processing callback
plant, channel_x, channel_u, channel_osc) # processing callback arguments
print('Finished processing log - making plots')
# Define x time slice
t_x_slice = slice(robot_output['t_x'].size)
t_osc_slice = slice(osc_debug['t_osc'].size)
''' Plot Positions '''
# Plot floating base positions if applicable
if use_floating_base and plot_config.plot_floating_base_positions:
mbp_plots.plot_floating_base_positions(robot_output, pos_names, 7,
t_x_slice)
# Plot joint positions
if plot_config.plot_joint_positions:
mbp_plots.plot_joint_positions(robot_output, pos_names,
7 if use_floating_base else 0,
t_x_slice)
# Plot specific positions
if plot_config.pos_names:
mbp_plots.plot_positions_by_name(robot_output, plot_config.pos_names,
t_x_slice, pos_map)
''' Plot Velocities '''
# Plot floating base velocities if applicable
if use_floating_base and plot_config.plot_floating_base_velocities:
mbp_plots.plot_floating_base_velocities(robot_output, vel_names, 6,
t_x_slice)
# Plot all joint velocities
if plot_config.plot_joint_positions:
mbp_plots.plot_joint_velocities(robot_output, vel_names,
6 if use_floating_base else 0,
t_x_slice)
# Plot specific velocities
if plot_config.vel_names:
mbp_plots.plot_velocities_by_name(robot_output, plot_config.vel_names,
t_x_slice, vel_map)
''' Plot Efforts '''
if plot_config.plot_measured_efforts:
mbp_plots.plot_measured_efforts(robot_output, act_names, t_x_slice)
if plot_config.act_names:
mbp_plots.plot_measured_efforts_by_name(robot_output,
plot_config.act_names,
t_x_slice, act_map)
''' Plot OSC '''
if plot_config.plot_qp_costs:
mbp_plots.plot_qp_costs(osc_debug, t_osc_slice)
if plot_config.plot_tracking_costs:
mbp_plots.plot_tracking_costs(osc_debug, t_osc_slice)
if plot_config.tracking_datas_to_plot:
for traj_name, config in plot_config.tracking_datas_to_plot.items():
for deriv in config['derivs']:
for dim in config['dims']:
mbp_plots.plot_osc_tracking_data(osc_debug, traj_name, dim,
deriv, t_osc_slice)
''' Plot Foot Positions '''
if plot_config.foot_positions_to_plot:
_, pts_map = cassie_plots.get_toe_frames_and_points(plant)
foot_frames = []
dims = {}
pts = {}
for pos in plot_config.foot_positions_to_plot:
foot_frames.append('toe_' + pos)
dims['toe_' + pos] = plot_config.foot_xyz_to_plot[pos]
pts['toe_' + pos] = pts_map[plot_config.pt_on_foot_to_plot]
mbp_plots.plot_points_positions(robot_output, t_x_slice, plant,
context, foot_frames, pts, dims)
if plot_config.plot_qp_solve_time:
mbp_plots.plot_qp_solve_time(osc_debug, t_osc_slice)
plt.show()
win = (1. / sz) * np.ones(sz)
throughput = np.convolve(bits, win, mode='same')
return t, throughput
if __name__ == '__main__':
if len(sys.argv) < 3:
print 'usage: %s <lcmlog-server> <lcmlog-client>' % sys.argv[0]
sys.exit(0)
if 'pkl' in sys.argv[1]:
with open(sys.argv[1], 'rb') as f:
server = pickle.load(f)
else:
logs = lcm.EventLog(sys.argv[1])
server = Acoustic_node(logs, 'SERVER_OSP', 'SERVER_RECOVERY')
with open('stats_server.pkl', 'wb') as f:
pickle.dump(server, f)
if 'pkl' in sys.argv[2]:
with open(sys.argv[2], 'rb') as f:
client = pickle.load(f)
else:
logc = lcm.EventLog(sys.argv[2])
client = Acoustic_node(logc, 'CLIENT_OSP', 'CLIENT_RECOVERY')
with open('stats_client.pkl', 'wb') as f:
pickle.dump(client, f)
reception_rate(server.utime, client.utime)
"""
cook server lcmlog-graph-manager:
shift tol_no. used for when the estimator was started before the logger.
"""
import sys
import lcm
from perls.lcmtypes.perllcm import isam2_f_prior_t
from perls.lcmtypes.perllcm import isam2_f_pose_pose_t
if __name__ == '__main__':
if len(sys.argv) < 2:
print 'usage: %s <lcmlog-graph-manager>' % sys.argv[0]
sys.exit(0)
ilog = lcm.EventLog(sys.argv[1])
olog = lcm.EventLog('lcmlog-graph-manager-cooked', 'w', overwrite=True)
DTOL = 2
for e in ilog:
if e.channel == 'F_PRIOR2D':
f = isam2_f_prior_t.decode(e.data)
if f.node_id < 1e5:
f.node_id += DTOL
olog.write_event(e.timestamp, e.channel, f.encode())
continue
elif e.channel == 'F_ODO2D':
f = isam2_f_pose_pose_t.decode(e.data)
if f.node_id1 < 1e5:
f.node_id1 += DTOL
if f.node_id2 < 1e5:
