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: