def init_logger(self): self.logger = DataLogger('drivetrain.csv') self.logger.add("time", lambda: self.timer.get()) #self.logger.add("heading", lambda: self.navx.getAngle()) self.logger.add("enc_pos_l", lambda: self.getLeftEncoder()) self.logger.add("enc_pos_r", lambda: self.getRightEncoder()) self.logger.add("enc_vel_l", lambda: self.motor_lb.getSelectedSensorVelocity(0)) self.logger.add("enc_vel_r", lambda: self.motor_rb.getSelectedSensorVelocity(0)) #self.logger.add("error_l", lambda: self.motor_lb.getClosedLoopError(0)) #self.logger.add("error_r", lambda: self.motor_rb.getClosedLoopError(0)) #self.logger.add("target_l", lambda: self.motor_lb.getClosedLoopTarget(0)) #self.logger.add("target_r", lambda: self.motor_rb.getClosedLoopTarget(0)) #self.logger.add("computed_velocity", lambda: self.computed_velocity) #self.logger.add("mode", lambda: self.running_command_name()) self.logger.add("voltage", lambda: self.motor_lb.getBusVoltage()) #self.logger.add("voltagep_l", lambda: self.motor_lb.getMotorOutputPercent()) #self.logger.add("voltagep_r", lambda: self.motor_rb.getMotorOutputPercent()) #self.logger.add("current_rf", lambda: self.pdp.getCurrent(0)) #self.logger.add("current_rb", lambda: self.pdp.getCurrent(1)) #self.logger.add("current_lf", lambda: self.pdp.getCurrent(15)) #self.logger.add("current_lb", lambda: self.pdp.getCurrent(13)) #self.logger.add("enc_offset_l", lambda: self.left_offset) #self.logger.add("enc_offset_r", lambda: self.right_offset) self.logger.add("VPR", lambda: self.motor_rb.getMotorOutputPercent()) self.logger.add("VRL", lambda: self.motor_lb.getMotorOutputPercent())
def train(problem, # Problem object, describing the task. initial_policy, # Initial policy. goal_state, # Goal state (s^g). full_start_dist, # Full start state distribution (rho_0). N_new=200, N_old=100, # Num new and old start states to sample. R_min=0.5, R_max=1.0, # Reward bounds defining what a "good" start state is. num_iters=100, # Number of iterations of training to run. num_ppo_iters=20, # Number of iterations of PPO training to run per train step. curriculum_strategy=random, train_algo=ppo, start_distribution=uniform, debug=False): data_logger = DataLogger(col_names=['overall_iter', 'ppo_iter', 'overall_perf', 'overall_area', 'overall_reward', 'ppo_perf', 'ppo_lens', 'ppo_rews'], filepath=os.path.join(DATA_DIR, 'data_%s.csv' % args.type), data_dir=DATA_DIR) print(locals(), flush=True); if curriculum_strategy in [random]: return train_pointwise(**locals()); elif curriculum_strategy in [backward_reachable]: return train_gridwise(**locals()); elif curriculum_strategy is None: return train_ppo_only(**locals()); else: raise ValueError("You passed in an unknown curriculum strategy!");
def test_profiler1(): """ forward velocity, trapezoid, no overshoot """ DT = 0.02 profiler = TrapezoidalProfile(cruise_v=10, a=20, target_pos=50, tolerance=.5, current_target_v=0) t = 0 pos = 0 if log_trajectory: logger = DataLogger("test_profiler1.csv") logger.log_while_disabled = True logger.do_print = True logger.add('t', lambda: t) logger.add('pos', lambda: pos) logger.add('v', lambda: profiler.current_target_v) logger.add('a', lambda: profiler.current_a) while not profiler.isFinished(pos): if log_trajectory: logger.log() profiler.calculate_new_velocity(pos, DT) pos += profiler.current_target_v * DT t += DT if t > 10: if log_trajectory: logger.close() assert False, "sim loop timed out" if t < 0.501: assert profiler.current_a == pytest.approx(20, 0.01), "t = %f" % (t, ) if 0.501 < t < 5: assert profiler.current_target_v == pytest.approx( 10., 0.01), "t = %f" % (t, ) assert profiler.current_a == 0, "t = %f" % (t, ) if 5 < t < 5.5 - 0.001: assert profiler.current_a == pytest.approx(-20., 0.01), "t = %f" % (t, ) if t == pytest.approx(5.50, 0.001): assert profiler.current_a == pytest.approx(0., 0.01), "t = %f" % (t, ) assert profiler.current_target_v == pytest.approx( 0., 0.01), "t = %f" % (t, ) if log_trajectory: logger.log() logger.close() assert t == pytest.approx(5.52, 0.01) assert profiler.current_a == 0
def create_data_logger_instance(self): """Create the DataLogger class instance for logging the data.""" self.__data_log_path = os.path.join( PATH_LOG, time.strftime("%Y-%m-%d_%H%M%S") + "_log") if not os.path.exists(self.__data_log_path): os.makedirs(self.__data_log_path) self.data_logger = DataLogger('data_log.csv', self.__data_log_path, 'plot.png')
def test_StateSpaceDriveCommand(Notifier): global log_trajectory left_drive = DriveSide( Ks=1.293985, Kv=0.014172 * 63. / 0.3048, Ka=0.005938 * 63. / 0.3048) right_drive = DriveSide( Ks=1.320812, Kv=0.013736 * 63. / 0.3048, Ka=0.005938 * 63. / 0.3048) robot = Rockslide() robot.robotInit() robot.drivetrain.getLeftEncoder = getLeftEncoder = MagicMock() robot.drivetrain.getRightEncoder = getRightEncoder = MagicMock() robot.drivetrain.getVoltage = MagicMock(return_value=10) command = StateSpaceDriveCommand("straight3m.tra") command.initialize() dt = robot.getPeriod() t = 0 if log_trajectory: logger = DataLogger("test_StateSpaceDriveCommand.csv") logger.log_while_disabled = True logger.do_print = False logger.add('t', lambda: t) logger.add('pos_l_m', lambda: left_drive.pos_m) logger.add('pos_r_m', lambda: right_drive.pos_m) logger.add('m_pos_l_m', lambda: command.y[0,0]) logger.add('m_pos_r_m', lambda: command.y[1,0]) logger.add('vel_l_mps', lambda: left_drive.v_mps) logger.add('vel_r_mps', lambda: right_drive.v_mps) logger.add('target_pos_l_m', lambda: command.r[0,0]) logger.add('target_pos_r_m', lambda: command.r[2,0]) logger.add('target_vel_l_mps', lambda: command.r[1,0]) logger.add('target_vel_r_mps', lambda: command.r[3,0]) logger.add('voltage', lambda: command.drivetrain.getVoltage()) logger.add('vpl', lambda: command.drivetrain.motor_lb.get()) logger.add('vpr', lambda: command.drivetrain.motor_rb.get()) while not command.isFinished(): logger.log() getLeftEncoder.return_value = left_drive.pos_m * 630 / 0.3048 getRightEncoder.return_value = right_drive.pos_m * 630 / 0.3048 command.execute() V = command.drivetrain.getVoltage() vpl = command.drivetrain.motor_lb.get() vpr = command.drivetrain.motor_rb.get() left_drive.update(V * vpl, dt) right_drive.update(V * vpr, dt) t += dt assert t < 10 command.end()
def __init__(self): self.board = ArduinoBoard("/dev/ttyACM0", 230400, timeout=5) self.data_analyser = DataLogger(self.board.sample_no, self.board.sample_freq) self.f, self.x = self.data_analyser.set_sample_freq( self.board.sample_freq) self.mode = None self.xscale = 1 self.yscale = 1 # pyqtgraph stuff pg.setConfigOptions(antialias=True) self.traces = dict() self.app = QtGui.QApplication(sys.argv) self.win = KeyPressWindow(title='Spectrum Analyzer') self.win.setWindowTitle('Spectrum Analyzer') self.win.sigKeyPress.connect(self.keyPressed) self.waveform = self.win.addPlot(title='WAVEFORM', row=1, col=1, labels={'bottom': "Time (s)"}) self.spectrum = self.win.addPlot(title='SPECTRUM', row=1, col=2, labels={'bottom': "Frequency (Hz)"}) self.specgram = self.win.addPlot(title='SPECTROGRAM', row=2, col=1, colspan=2, labels={'bottom': "Frequency (Hz)"}) self.img = pg.ImageItem() self.specgram.addItem(self.img) # bipolar colormap pos = np.array([0., 1., 0.5, 0.25, 0.75]) color = np.array( [[0, 255, 255, 255], [255, 255, 0, 255], [0, 0, 0, 255], (0, 0, 255, 255), (255, 0, 0, 255)], dtype=np.ubyte) cmap = pg.ColorMap(pos, color) lut = cmap.getLookupTable(0.0, 1.0, 256) self.img.setLookupTable(lut) self.img.setLevels([20 * np.log10(10), 20 * np.log10(10000)]) # waveform and spectrum x points self.scale_plots() # tell Arduino to start sending data self.board.send_command("Send Data")
def test_profiler4(): """ reverse velocity, triangle, no overshoot """ DT = 0.02 profiler = TrapezoidalProfile(cruise_v=10, a=20, target_pos=-4, tolerance=.5, current_target_v=0) t = 0 pos = 0 if log_trajectory: logger = DataLogger("test_profiler4.csv") logger.log_while_disabled = True logger.add('t', lambda: t) logger.add('pos', lambda: pos) logger.add('v', lambda: profiler.current_target_v) logger.add('a', lambda: profiler.current_a) while not profiler.isFinished(pos): if log_trajectory: logger.log() profiler.calculate_new_velocity(pos, DT) pos += profiler.current_target_v * DT t += DT if t > 10: if log_trajectory: logger.close() assert False, "sim loop timed out" if t < 0.4599: assert profiler.current_a == pytest.approx(-20, 0.01), "t = %f" % (t, ) if t == pytest.approx(0.46, 0.01): assert profiler.current_target_v == pytest.approx(-9.2, 0.01) assert profiler.current_a == pytest.approx(-20, 0.01), "t = %f" % (t, ) if 0.4601 < t < 0.92: assert profiler.current_a == pytest.approx(20, 0.01), "t = %f" % (t, ) if log_trajectory: logger.log() logger.close() assert t == pytest.approx(0.94, 0.01) assert profiler.current_a == 0
def init_logger(self): self.logger = DataLogger('elevator.csv') self.logger.add("time", lambda: self.timer.get()) self.logger.add("enc_pos", lambda: self.motor.getSelectedSensorPosition(0)) #self.logger.add("voltagep_motor", lambda: self.motor.getMotorOutputPercent()) #self.logger.add("voltagep_othermotor", lambda: self.other_motor.getMotorOutputPercent()) self.logger.add("voltage", lambda: self.motor.getBusVoltage()) self.logger.add("current_motor", lambda: self.motor.getOutputCurrent()) self.logger.add("current_othermotor", lambda: self.other_motor.getOutputCurrent()) self.logger.add("zeroed", lambda: 1 if self.zeroed else 0)
def init_logger(self): self.logger = DataLogger('intake.csv') self.logger.add("time", lambda: self.timer.get()) self.logger.add("voltagep_r", lambda: self.intake_motor_rightWheel.get()) self.logger.add("voltagep_m", lambda: self.intake_motor_closeOpen.get()) self.logger.add("voltagep_l", lambda: self.intake_motor_leftWheel.get()) self.logger.add("voltage", lambda: self.pdp.getVoltage()) self.logger.add("current_r", lambda: self.pdp.getCurrent(0)) self.logger.add("current_m", lambda: self.pdp.getCurrent(1)) self.logger.add("current_l", lambda: self.pdp.getCurrent(15))
def test_ProfiledForward2(Notifier, sim_hooks): global log_trajectory robot = Rockslide() robot.robotInit() DT = robot.getPeriod() robot.drivetrain.getLeftEncoder = getLeftEncoder = MagicMock() robot.drivetrain.getRightEncoder = getRightEncoder = MagicMock() getLeftEncoder.return_value = 0 getRightEncoder.return_value = 0 command = ProfiledForward(10) command.initialize() t = 0 pos_ft = 0 if log_trajectory: logger = DataLogger("test_profiled_forward2.csv") logger.log_while_disabled = True logger.do_print = True logger.add('t', lambda: t) logger.add('pos', lambda: pos_ft) logger.add('target_pos', lambda: command.dist_ft) logger.add('v', lambda: command.profiler_l.current_target_v) logger.add('max_v', lambda: command.max_v_encps) logger.add('a', lambda: command.profiler_l.current_a) logger.add('max_a', lambda: command.max_acceleration) logger.add('voltage', lambda: command.drivetrain.getVoltage()) logger.add('vpl', lambda: command.drivetrain.motor_lb.get()) logger.add('adist', lambda: command.profiler_l.adist) logger.add('err', lambda: command.profiler_l.err) while t < 10: if log_trajectory: logger.log() getLeftEncoder.return_value = pos_ft * command.drivetrain.ratio getRightEncoder.return_value = -pos_ft * command.drivetrain.ratio command.execute() v = command.profiler_l.current_target_v pos_ft += v * DT t += DT sim_hooks.time = t if command.isFinished(): break command.end() if log_trajectory: logger.log() logger.close()
def initialize(self): self.drivetrain.zeroEncoders() self.drivetrain.zeroNavx() self.ctrl_l.enable() self.ctrl_r.enable() self.ctrl_heading.enable() self.logger = DataLogger("csv_trajectory1.csv") self.drivetrain.init_logger(self.logger) self.logger.add("profile_vel_r", lambda: self.target_v_r) self.logger.add("profile_vel_l", lambda: self.target_v_l) self.logger.add("pos_ft_l", lambda: self.pos_ft_l) self.logger.add("i", lambda: self.i) self.timer.start() self.i = 0
def __init__(self): super().__init__('Drivetrain') #The set motor controllers for this years robot and how motors are coded self.motor_rb = ctre.WPI_TalonSRX(1) self.motor_rf = ctre.WPI_VictorSPX(17) self.motor_lb = ctre.WPI_TalonSRX(13) self.motor_lf = ctre.WPI_VictorSPX(12) self.motor_rf.follow(self.motor_rb) self.motor_lf.follow(self.motor_lb) self.motors = [ self.motor_rb, self.motor_lb, self.motor_rf, self.motor_lf ] self.drive = wpilib.drive.DifferentialDrive(self.motor_rb, self.motor_lb) self.navx = navx.AHRS.create_spi() self.pdp = wpilib.PowerDistributionPanel(16) self.motor_lb.setNeutralMode(ctre.NeutralMode.Brake) self.motor_rb.setNeutralMode(ctre.NeutralMode.Brake) self.motor_rf.setNeutralMode(ctre.NeutralMode.Brake) self.motor_lf.setNeutralMode(ctre.NeutralMode.Brake) self.motor_lb.configSelectedFeedbackSensor( ctre.FeedbackDevice.QuadEncoder, 0, 0) self.motor_rb.configSelectedFeedbackSensor( ctre.FeedbackDevice.QuadEncoder, 0, 0) self.motor_rb.selectProfileSlot(0, 0) self.motor_lb.selectProfileSlot(0, 0) self.table = networktables.NetworkTables.getTable("/Drivetrain") self.sd_table = networktables.NetworkTables.getTable("/SmartDashboard") self.motor_lb.setSensorPhase(False) self.motor_rb.setSensorPhase(False) self.motor_lb.setInverted(False) self.motor_rb.setInverted(False) self.motor_rf.setInverted(False) self.motor_lf.setInverted(False) self.left_offset = 0 self.right_offset = 0 self.timer = wpilib.Timer() self.timer.start() self.computed_velocity = 0 self.logger_enabled = False #self.logger = None self.logger = DataLogger('drivetrain.csv') self.init_logger(self.logger)
def __init__(self, config): self._config = config self._time = 0 if config['gui'] == 'pygame': if config['display'] == 'fullscreen': self._display = pygame.display.set_mode( config['display_size'], pygame.FULLSCREEN) else: self._display = pygame.display.set_mode(config['display_size']) # Mutation rates that probably ought to be zero. self._mutation_of_mean_mutation_rate = 0.000 self._mutation_of_sigma_mutation_rate = 0.000 self._reproduction_mutation_rate = 0.000 self._init_agents() self._init_landscape_locks() # render_data stores RGB, Energy and first 3 Keys for rendering. if config['world_d'] == 1: self._render_data = np.zeros( (config['map_size'][0], config['num_1d_history'], 7)) self._bitmap = np.zeros( (config['map_size'][0], config['num_1d_history'], 3), dtype=np.uint8) else: self._render_data = np.zeros((config['map_size'] + (7, ))) self._bitmap = np.zeros((config['map_size'] + (3, )), dtype=np.uint8) self._data = {} # For plotting, etc. self._data_history_len = self._render_data.shape[1] self._repo_energy_stats = [] # Track mean min, max; sigma min, max self._locks_metrics = np.zeros((4, config['num_locks'])) self._locks_metrics[0] = np.inf self._locks_metrics[1] = -np.inf self._locks_metrics[2] = np.inf self._locks_metrics[3] = -np.inf self._data_logger = None if config['log_data'] is not None: self._data_logger = DataLogger(['sigma'], config['log_data'], 1000)
def initialize(self): self.drivetrain.zeroEncoders() self.drivetrain.zeroNavx() self.ctrl_l.enable() self.ctrl_r.enable() self.ctrl_heading.enable() self.logger = DataLogger("profiled_forward.csv") self.drivetrain.init_logger(self.logger) self.logger.add("profile_vel_r", lambda: self.target_v_r) self.logger.add("profile_vel_l", lambda: self.target_v_l) self.logger.add("pos_ft_l", lambda: self.pos_ft_l) self.logger.add("target_pos_l", lambda: self.profiler_l.target_pos) self.logger.add("adist_l", lambda: self.profiler_l.adist) self.logger.add("err_l", lambda: self.profiler_l.err) self.logger.add("choice_l", lambda: self.profiler_l.choice) self.logger.add("adist_r", lambda: self.profiler_l.adist) self.logger.add("err_r", lambda: self.profiler_l.err) self.logger.add("choice_r", lambda: self.profiler_l.choice) self.timer.start()
def init_logger(self): self.logger = DataLogger('drivetrain.csv') self.logger.add("time", lambda: self.timer.get()) self.logger.add("heading", lambda: self.navx.getAngle()) self.logger.add("enc_pos_l", lambda: self.motor_lb.getSelectedSensorPosition(0)) self.logger.add("enc_pos_r", lambda: self.motor_rb.getSelectedSensorPosition(0)) self.logger.add("enc_vel_l", lambda: self.motor_lb.getSelectedSensorVelocity(0)) self.logger.add("enc_vel_r", lambda: self.motor_rb.getSelectedSensorVelocity(0)) self.logger.add("error_l", lambda: self.motor_lb.getClosedLoopError(0)) self.logger.add("error_r", lambda: self.motor_rb.getClosedLoopError(0)) self.logger.add("target_l", lambda: self.motor_lb.getClosedLoopTarget(0)) self.logger.add("target_r", lambda: self.motor_rb.getClosedLoopTarget(0)) self.logger.add("computed_velocity", lambda: self.computed_velocity) self.logger.add("mode", lambda: self.mode) self.logger.add("voltage", lambda: self.motor_lb.getBusVoltage()) self.logger.add("voltagep_l", lambda: self.motor_lb.getMotorOutputPercent()) self.logger.add("voltagep_r", lambda: self.motor_rb.getMotorOutputPercent()) self.logger.add("current_rf", lambda: self.pdp.getCurrent(0)) self.logger.add("current_rb", lambda: self.pdp.getCurrent(1)) self.logger.add("current_lf", lambda: self.pdp.getCurrent(15)) self.logger.add("current_lb", lambda: self.pdp.getCurrent(13))
def initialize(self): self.logger = DataLogger('elevator-majig.csv') self.logger.add("time", lambda: self.timer.get()) self.logger.add( "enc_pos1", lambda: self.elevator.motor.getSelectedSensorVelocity(0)) self.logger.add("current_motor1", lambda: self.elevator.motor.getOutputCurrent()) self.logger.add("current_follow1", lambda: self.elevator.other_motor.getOutputCurrent()) self.logger.add("voltage_motor1", lambda: self.elevator.motor.getMotorOutputVoltage()) self.logger.add( "voltage_follow1", lambda: self.elevator.other_motor.getMotorOutputVoltage()) self.logger.add("bvoltage_motor1", lambda: self.elevator.motor.getBusVoltage()) self.logger.add("bvoltage_follow1", lambda: self.elevator.other_motor.getBusVoltage()) self.logger.add( "enc_pos2", lambda: self.elevator.other_right_motor. getSelectedSensorVelocity(0)) self.logger.add("current_motor2", lambda: self.elevator.right_motor.getOutputCurrent()) self.logger.add( "current_follow2", lambda: self.elevator.other_right_motor.getOutputCurrent()) self.logger.add( "voltage_motor2", lambda: self.elevator.right_motor.getMotorOutputVoltage()) self.logger.add( "voltage_follow2", lambda: self.elevator.other_right_motor.getMotorOutputVoltage()) self.logger.add("bvoltage_motor2", lambda: self.elevator.right_motor.getBusVoltage()) self.logger.add( "bvoltage_follow2", lambda: self.elevator.other_right_motor.getBusVoltage()) self.timer.start()
if len(sys.argv) > 1: N_max = int(sys.argv[1]) if len(sys.argv) > 2: name = sys.argv[2] else: name = 'prime_numbers_1.csv' else: print("Using default value N_max = 100") print("Optional command line argument to change this") N_max = 100 name = 'prime_numbers_1.csv' # setup other default parameters var_names = ['i', 'pn'] path = os.getcwd() + '/' logger = DataLogger(path, name, var_names, log_interval=99) def is_prime(n): for ni in range(2, n): if (n % ni) == 0: return False return True index = 0 for i in range(N_max): if is_prime(i): print(index, i) logger.log(i=index, pn=i) index += 1
from data_logger import DataLogger from data_reader import * from genetic_algorithm import * pop_size = 100 gen = 100 Px = 0.7 Pm = 0.01 Tour = 5 selection = 'tournament' for i in range(12, 21, 2): reader = DataReader() n, flow_matrix, distance_matrix = reader.read_data("data/had%s.dat" % str(i)) logger = DataLogger('had%s' % str(i)) logger.write_header(pop_size, gen, Px, Pm, Tour, selection) for j in range(0, 10): genetic_algorithm = GeneticAlgorithm(n, pop_size, flow_matrix, distance_matrix, Pm, Px, Tour, logger, selection) print('Had%s file best result: %s' % (i, genetic_algorithm.run(gen))) logger.close()
def initialize(self): self.drivetrain.zeroEncoders() self.drivetrain.zeroNavx() self.i = 0 self.logger = DataLogger("ss_trajectory.csv") self.drivetrain.init_logger(self.logger)
def runSim(): print('sim started') ## SET-UP SIM ## # Spawn a trike batch = [] blueprint = random.choice(blueprints) if blueprint.has_attribute('color'): color = random.choice( blueprint.get_attribute('color').recommended_values) blueprint.set_attribute('color', color) blueprint.set_attribute('role_name', 'autopilot') random.shuffle(spawn_points) batch.append(SpawnActor(blueprint, spawn_points[0])) for response in client.apply_batch_sync(batch): if response.error: logging.error(response.error) else: vehicle = response.actor_id # Attach nmeaSensor to trike (speed directly taken from trike actor) actor = world.get_actors().find(vehicle) for x in world.get_actors(): if vehicle == x.id: actor = x #print(vehicle) #print(world.get_actors()) #next((x for x in world.get_actors() if x.id == vehicle), None) logger = DataLogger(actor) blueprint = world.get_blueprint_library().find('sensor.other.gnss') transform = carla.Transform(carla.Location(x=0.8, z=1.7)) nmeaSensor = world.spawn_actor(blueprint, transform, actor) sensors.append(nmeaSensor) #speedSensor = world.spawn_actor(blueprint,transform,actor) ### SET SIM VIEWPOINT TO VEHICLE #### world.get_spectator().set_transform(actor.get_transform()) ################# ## RUN SIM ## try: ser.write('f'.encode('utf-8')) #Notify Due that system is starting ##### FOR GPS SENSOR USAGE #### logger.setListen() logger.setNmea('Test@') # Fill private member to avoid error nmeaSensor.listen(lambda data: nmeaGPS.consoleout(data, logger)) ########################## while True: # Wait for ready queue from due if ser.in_waiting: ### FOR GPS SENSOR USAGE (Set the stop variable to short the listener func) ### logger.setStopListen() #nmeaSensor.stop() ########### ''' ADD SERIAL READS FOR ACTUATION HERE Currently just clears an arbitrary char in buffer sent from router Due ''' ## Receive in order: throttle, steer, brake t = float(ser.readline().decode('ASCII')) s = float(ser.readline().decode('ASCII')) b = float(ser.readline().decode('ASCII')) actor.apply_control( carla.VehicleControl(throttle=t, steer=s, brake=b)) ''' Finish processing actuation commands here Here's how data is sent from Due: - Throttle : float (-1 to 1) - Steering : float (-1 to 1) - Brakes : float (0 for off 0.3 for on) <- because current implementation of brake is siimply on/off. Feel free to change on value of 0.3. ''' ### ACCESS/SEND LAT/LONG FROM LAST TICK ### ### Can use for location if disabling GPS Sensor #geo = world.get_map().transform_to_geolocation(logger.actorId.get_location()) #msg = geo.__str__() + '@' #logger.setNmea(msg) ########### ### ACCESS/SEND X/Y/Z FROM LAST TICK ##### ### Can use for location if disabling GPS Sensor #msg = logger.actorId.get_location().__str__() + '@' #logger.setNmea(msg) ######## # Get the speed of Trike getSpeed(logger.actorId, logger) # Send most current data # ORDER MATTERS FOR ARDUINO CODE logger.sendCyclometer(ser) logger.sendNmea(ser) ### FOR SENSOR USAGE ### logger.setListen() #nmeaSensor.listen(lambda data: nmeaGPS.consoleout(data,logger)) ############# except KeyboardInterrupt: print('\ndestroying %d sensors' % len(sensors)) for x in sensors: carla.command.DestroyActor(x) print('\ndestroying vehicle') actor.destroy() return
def generate_data(arg_step=10): # argparser parser = argparse.ArgumentParser() parser.add_argument('--step', type=int, default=arg_step) parser.add_argument('--nogui', action='store_true') parser.add_argument('--path', type=str, default='./log') parser.add_argument('--file_name', type=str, default=None) args = parser.parse_args() # logger if args.file_name is not None: mkdir(args.path) logger = DataLogger() # set robot & camera cfg = get_sim_config() view_matrix = tuple(invRt(cfg.Camera_pose).T.reshape(16)) imsize_w, imsize_h = 640, 360 f = 462 fov_w = math.atan(imsize_w / 2 / f) * 2 / math.pi * 180 fov_h = math.atan(imsize_h / 2 / f) * 2 / math.pi * 180 proj_matrix = p.computeProjectionMatrixFOV(fov=fov_h, aspect=imsize_w / imsize_h, nearVal=0.01, farVal=5.0) cam = simCam(imsize=[640, 360], view_matrix=view_matrix, proj_matrix=proj_matrix, z_near=0.01, z_far=5.0) robot = SimUR5(center=cfg.Table_center, prepare_height=cfg.Gripper_prepare_height, real_time_sim=False, GUI=not args.nogui) obj_num = cfg.Object_num # set object scene = Sim_Scene(view_matrix=view_matrix, fov_w=fov_w, imsize=[imsize_w, imsize_h], cube_num=cfg.Object_num) image_RGB_ini, image_depth_ini, _ = cam.get_data() # set property friction_discrete, mass_discrete = set_friction_and_mass( obj_num, scene.object_ids, cfg) set_position_and_color(obj_num, scene.object_ids) # always go home first robot.open_gripper(blocking=True) robot.close_gripper(blocking=True) logger.save_attribute('friction', friction_discrete) logger.save_attribute('mass', mass_discrete) cnt_dict = {'collide': 0, 'push': 0} all_proposal = {'collide': [], 'push': []} # start action for step in range(args.step): robot.close_gripper(blocking=True) image_RGB, image_depth, _ = cam.get_data() mask = get_mask_real(image_RGB=image_RGB, image_RGB_ini=image_RGB_ini, image_depth=image_depth, image_depth_ini=image_depth_ini, Object_num=cfg.Object_num) # depth normalization tmp_depth = image_depth.copy() tmp_depth -= np.min(tmp_depth) tmp_depth /= np.max(tmp_depth) obj_pos, points_list = [], [] for index in range(cfg.Object_num): cur_pos, points = get_pos((mask == (index + 1)).astype(np.int), image_RGB, image_depth, cfg.Camera_pose, cfg.Camera_intrinsic) obj_pos.append(cur_pos) points_list.append(points) scene.reset_coord_prev() # generate proposal # collide for i in range(cfg.Object_num): for dir in range(2): if can_collide(obj_pos, i, cfg.Collide_region, dir): all_proposal['collide'].append({ 'action_type': 'collide', 'obj_id': i, 'dir': dir }) # push for i in range(cfg.Object_num): for dir in range(cfg.direction_num): if can_push(obj_pos, points_list, i, dir / cfg.direction_num * 2 * np.pi, cfg.Table_center): all_proposal['push'].append({ 'action_type': 'push', 'obj_id': i, 'dir': dir, }) np.random.shuffle(all_proposal['push']) np.random.shuffle(all_proposal['collide']) proposal_list = all_proposal['push'] + all_proposal['collide'] if cnt_dict['push'] > cnt_dict['collide']: proposal_list = all_proposal['collide'] + all_proposal['push'] if get_random(0, 1) < 0.2: np.random.shuffle(proposal_list) # choose action randomly if len(proposal_list) == 0: print('No Proposal') exit() action = proposal_list[0] cnt_dict[action['action_type']] += 1 # prepare cur_pos = obj_pos[action['obj_id']] points = points_list[action['obj_id']] logger.save_data(step, 'depth', image_depth) logger.save_data(step, 'mask', mask) logger.save_data(step, 'action_type', action['action_type']) logger.save_data(step, 'obj_id', action['obj_id']) if action['action_type'] == 'collide': dir = action['dir'] aux_init = cfg.Auxiliary_initialization[dir] # set the auxiliary cylinder p.resetBasePositionAndOrientation( scene.object_ids[cfg.Object_num], posObj=aux_init + [cfg.Auxiliary_initialization_height], ornObj=p.getQuaternionFromEuler([0, 0, np.pi / 2])) # start action robot.primitive_topdown_grasp( aux_init + [cfg.Gripper_grasp_height], np.pi / 2) robot.primitive_place([ cur_pos[0], cfg.Gripper_place_y[dir], cfg.Gripper_place_height ], np.pi / 2, cfg.Slow_speed) robot.close_gripper(True) # finish action robot.primitive_gohome(speed=cfg.Fast_speed) p.resetBasePositionAndOrientation( scene.object_ids[cfg.Object_num], posObj=cfg.Auxiliary_initialization[0] + [cfg.Auxiliary_initialization_height], ornObj=cfg.Orientation_initialization) # save info logger.save_data(step, 'action', [1] + [0, 0, 0] + [0] * 30 + [0] + [cur_pos[0], action['dir']]) logger.save_data(step, 'direction', action['dir']) elif action['action_type'] == 'push': angle = action['dir'] / cfg.direction_num * 2 * np.pi delta = -1 * np.asarray([ np.cos(angle), np.sin(angle) ]) * (get_dist(points, cur_pos, [np.cos(angle), np.sin(angle)]) + 0.075) speed_discrete = np.random.choice(len(cfg.Push_speed)) push_speed = cfg.Push_speed[speed_discrete] start = [ cur_pos[0] + delta[0], cur_pos[1] + delta[1], cfg.Push_height ] robot.primitive_push_tilt(start, angle, push_speed) robot.primitive_gohome(speed=cfg.Fast_speed) # save info logger.save_data(step, 'action', [0] + start + get_onehot(action['dir'], 30) + [push_speed] + [0, 0]) logger.save_data(step, 'direction', action['dir']) logger.save_data(step, 'speed', speed_discrete) # calc the optical flow flowim, depth_im_prev, depth_im_curr = scene.comput_2dflow( get_mask=False) _, flow = depth_smooth(depth_im_prev, flowim) flow = flow * (mask == (action['obj_id'] + 1)).astype(np.float32) logger.save_data(step, 'flow', flow) if args.file_name is not None: joblib.dump((logger.attribute_dict, logger.data_dict), os.path.join(args.path, args.file_name), compress=True)
def receive_and_execute_commands(self): """Read all incoming commands until START SIMULATION Command.""" if self.debug_mode: print("Waiting Commands") road_segments_to_add = [] add_road_network_to_world = False v_u_t_to_add = [] add_v_u_t_to_world = [] dummy_vhc_to_add = [] add_dummy_vhc_to_world = [] continue_simulation = False remote_command = None while continue_simulation is False or self.sim_data is None: rcv_msg = self.comm_server.receive_blocking(self.client_socket) remote_command = self.message_interface.interpret_message(rcv_msg) remote_response = [] if self.debug_mode: print("Received command : {}".format(remote_command.command)) if remote_command.command == self.message_interface.START_SIM: self.sim_data = remote_command.object continue_simulation = True remote_response = self.message_interface.generate_ack_message() elif remote_command.command == self.message_interface.RELOAD_WORLD: continue_simulation = True remote_response = self.message_interface.generate_ack_message() elif remote_command.command == self.message_interface.CONTINUE_SIM: continue_simulation = True remote_response = self.message_interface.generate_ack_message() elif remote_command.command == self.message_interface.SET_DATA_LOG_PERIOD_MS: if self.data_logger is None: self.data_logger = DataLogger() self.data_logger.set_environment_manager( self.environment_manager) self.data_logger.set_vehicles_manager( self.vehicles_manager) self.data_logger.set_log_period(remote_command.object) remote_response = self.message_interface.generate_ack_message() elif remote_command.command == self.message_interface.SET_CONTROLLER_PARAMETER: if self.vehicles_manager is not None: emitter = self.vehicles_manager.get_emitter() if emitter is not None: self.controller_comm_interface.transmit_set_controller_parameters_message( emitter, remote_command.object.vehicle_id, remote_command.object.parameter_name, remote_command.object.parameter_data) remote_response = self.message_interface.generate_ack_message() elif remote_command.command in ( self.message_interface.SURROUNDINGS_DEF, self.message_interface.SURROUNDINGS_ADD): road_segments_to_add.append(remote_command.object) add_road_network_to_world = ( remote_command.command == self.message_interface.SURROUNDINGS_ADD) remote_response = self.message_interface.generate_ack_message() elif remote_command.command in ( self.message_interface.DUMMY_ACTORS_DEF, self.message_interface.DUMMY_ACTORS_ADD): dummy_vhc_to_add.append(remote_command.object) if remote_command.command == self.message_interface.DUMMY_ACTORS_ADD: add_dummy_vhc_to_world.append(True) else: add_dummy_vhc_to_world.append(False) remote_response = self.message_interface.generate_ack_message() elif remote_command.command in (self.message_interface.VUT_DEF, self.message_interface.VUT_ADD): v_u_t_to_add.append(remote_command.object) if remote_command.command == self.message_interface.VUT_ADD: add_v_u_t_to_world.append(True) else: add_v_u_t_to_world.append(False) remote_response = self.message_interface.generate_ack_message() elif remote_command.command == self.message_interface.SET_HEART_BEAT_CONFIG: self.heart_beat_config = remote_command.object remote_response = self.message_interface.generate_ack_message() elif remote_command.command == self.message_interface.SET_ROBUSTNESS_TYPE: robustness_type = remote_command.object self.robustness_function = RobustnessComputation( robustness_type, self.supervisor_control, self.vehicles_manager, self.environment_manager) remote_response = self.message_interface.generate_ack_message() elif remote_command.command == self.message_interface.SET_VIEW_FOLLOW_ITEM: self.view_follow_item = remote_command.object remote_response = self.message_interface.generate_ack_message() elif remote_command.command == self.message_interface.ADD_DATA_LOG_DESCRIPTION: if self.data_logger is None: self.data_logger = DataLogger() self.data_logger.set_environment_manager( self.environment_manager) self.data_logger.set_vehicles_manager( self.vehicles_manager) self.data_logger.add_data_log_description( remote_command.object) if self.sim_data is not None: self.data_logger.set_expected_simulation_time( self.sim_data.simulation_duration_ms) self.data_logger.set_simulation_step_size( self.sim_data.simulation_step_size_ms) remote_response = self.message_interface.generate_ack_message() elif remote_command.command == self.message_interface.GET_ROBUSTNESS: if self.robustness_function is not None: rob = self.robustness_function.get_robustness() else: rob = 0.0 remote_response = self.message_interface.generate_robustness_msg( rob) elif remote_command.command == self.message_interface.GET_DATA_LOG_INFO: if self.data_logger is not None: log_info = self.data_logger.get_log_info() else: log_info = (0, 0) remote_response = self.message_interface.generate_log_info_message( log_info) elif remote_command.command == self.message_interface.GET_DATA_LOG: requested_log_start_index = remote_command.object[0] requested_log_end_index = remote_command.object[1] if self.data_logger is not None: data_log = self.data_logger.get_log( requested_log_start_index, requested_log_end_index) else: data_log = np.empty(0) remote_response = self.message_interface.generate_data_log_message( data_log) if len(remote_response) > 0: self.comm_server.send_blocking(self.client_socket, remote_response) # Generate simulation environment (add VUT, Dummy Actors and Surroundings) if road_segments_to_add and self.debug_mode: print("Number of road segments: {}".format( len(road_segments_to_add))) if add_road_network_to_world: self.generate_road_network( road_segments_to_add, self.sim_obj_generator, self.environment_manager.get_num_of_road_networks() + 1) if road_segments_to_add: self.environment_manager.record_road_network(road_segments_to_add) if v_u_t_to_add and self.debug_mode: print("Number of VUT: {}".format(len(v_u_t_to_add))) for i in range(len(v_u_t_to_add)): vhc = v_u_t_to_add[i] if add_v_u_t_to_world[i]: self.generate_vehicle(vhc, self.sim_obj_generator) self.vehicles_manager.record_vehicle(vhc, self.vehicles_manager.VHC_VUT) if dummy_vhc_to_add and self.debug_mode: print("Number of Dummy vehicles: {}".format(len(dummy_vhc_to_add))) for i in range(len(dummy_vhc_to_add)): vhc = dummy_vhc_to_add[i] if add_dummy_vhc_to_world[i]: self.generate_vehicle(vhc, self.sim_obj_generator) self.vehicles_manager.record_vehicle( vhc, self.vehicles_manager.VHC_DUMMY) if remote_command is not None and remote_command.command == self.message_interface.RELOAD_WORLD: time.sleep(1.0) try: print('Closing connection!') self.comm_server.close_connection() except: print('Could not close connection!') pass time.sleep(0.5) self.comm_server = None print('Reverting Simulation!') self.supervisor_control.revert_simulation() time.sleep(1)
import time import datetime import json import argparse from data_logger import DataLogger, DataType # Store the log file to "./logs/" try: os.makedirs("logs") except os.error as err: # It's OK if the log directory exists. This is to be compatible with Python 2.7 if err.errno != errno.EEXIST: raise err data_logger = DataLogger("logs", max_file_size=4000, max_file_count=10) def _parse_configs(config_file_path): """ Parse the configuration file. :param config_file_path: The path to the configuration file (including the filename) :type: str :return: The configuration data :rtype: dict """ with open(config_file_path, 'r') as config_file: configs = json.load(config_file) return configs
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. Please contact the author(s) of this library if you have any questions. Authors: David Fridovich-Keil ( [email protected] ) """ ################################################################################ # # Node to wrap the DataLogger class. # ################################################################################ from data_logger import DataLogger import rospy import sys import numpy as np if __name__ == "__main__": rospy.init_node("data_logger") logger = DataLogger() if not logger.Initialize(): print("Failed to initialize the data_logger node.") sys.exit(1) rospy.on_shutdown(logger.Save) rospy.spin()
LogManager.setupLogging("climate-station") log = logging.getLogger('climate-station') log.info('Hello world.') sensor = Sensor(i2c(1, Sensor.BASE_ADDRESS)) poller = DevicePoller(1) poller.add_device("sensor", sensor) display = LedDisplayController(i2c(1, LedDisplayController.BASE_ADDRESS)) display.turnOnOscillator() display.turnOnDisplay() display.setDimming(0) presenter = Presenter(2, poller, display) dataLogger = DataLogger(60, poller) try: with IdleLoop() as idle: idle.setTicksPerSecond(1) idle.register(dataLogger) idle.register(presenter) idle.run() except: e = sys.exc_info()[0] log.exception(e) print('Caught exception: {0}'.format(e))
LOGGER = logging.getLogger() def excepthook(exc_type, value, traceback): LOGGER.critical("Uncaught exception", exc_info=(exc_type, value, traceback)) if __name__ == '__main__': try: sys.excepthook = excepthook parser = argparse.ArgumentParser( description='Muon system voltage controller.') parser.add_argument( 'devices', metavar='DEVICES', help='Path to a CSV file containing device addresses') parser.add_argument( '--profile', metavar='PATH', help='Path to a CSV file containing parameters of voltage cells') args = parser.parse_args() data_logger = DataLogger(program_settings.data_log_file) gui_main(args, data_logger) except KeyboardInterrupt: LOGGER.warning('Process interrupted')
import logging from threading import Thread from data_logger import DataLogger from poller import ItemExistsError from flask import Flask, request, abort, jsonify, make_response, url_for # Configure the logging before creating the Flask app otherwise it will logging.basicConfig(format="%(asctime)s %(levelname)s %(message)s", level=logging.INFO) app = Flask(__name__) data_logger = DataLogger() def item_as_dict(poll_item): """return a dict version of the given PollItem""" # simply return all the fields of the PollItem, plus a url field item_dict = poll_item.__dict__ item_dict["url"] = url_for("get_item", name=poll_item.name, _external=True) return item_dict def item_valid(item): return "name" in item and "key" in item and "interval" in item def make_error_response(error_message, error_code): return make_response(jsonify({"error": error_message}), error_code)
from data_logger import DataLogger import time if __name__ == "__main__": log_variables = [ "air_temperature", "humidity", "co2", "o2", "water_temperature", "ph", "ec", "heater_is_on", "humidifier_is_on" ] data_logger = DataLogger(log_variables) while True: data_logger.run() time.sleep(1)
def hw_monitor(device, args): collect_sensor_data = init_sensors() sensors_spec = dict(CPU='it8686-isa-0a40.temp3', GPU='amdgpu-pci-4200.edge', Chipset='it8686-isa-0a40.temp2', System='it8792-isa-0a60.temp3') fan_spec = dict(CPU='it8686-isa-0a40.fan1', GPU='amdgpu-pci-4200.fan1', Rear='it8792-isa-0a60.fan3', SYS1='it8686-isa-0a40.fan2') sensors_data_logger = DataLogger(collect_sensor_data, max_entries=(device.width / 2) - 10).start() loadavg_data_logger = DataLogger(psutil.getloadavg, max_entries=device.width - 2).start() network_data_logger = DataLogger( lambda: psutil.net_io_counters(pernic=True)[args.network], max_entries=device.width / 2 - 15).start() def shutdownHandler(signal, frame): loadavg_data_logger.stop() sensors_data_logger.stop() network_data_logger.stop() exit(0) signal.signal(signal.SIGINT, shutdownHandler) signal.signal(signal.SIGTERM, shutdownHandler) virtual = viewport(device, width=device.width, height=1024, mode='RGBA', dither=True) with canvas(virtual) as draw: y_offset = render_logo(draw, 0, device.width, args.title) hotspots = [ snapshot(device.width, 9, cpu_percent.render, interval=0.5), snapshot(device.width, cpu_barchart.height + 4, cpu_barchart.render, interval=0.5), snapshot(device.width, cpu_stats.height + 11, cpu_stats.render, interval=2), snapshot(device.width, uptime.height, uptime.render, interval=10), snapshot(device.width, 10, system_load.render, interval=1.0), snapshot(device.width, loadavg_chart.height, loadavg_chart.using(loadavg_data_logger), interval=1.0), snapshot(device.width, 10, memory.render, interval=5.0), snapshot(device.width, 28, disk.directory('/'), interval=5.0), snapshot(device.width, network.height, network.using(args.network, network_data_logger), interval=2.0), snapshot(device.width, 64, sensors_chart.using(sensors_data_logger, sensors_spec, fan_spec), interval=1.0), snapshot(device.width, 64 * 6, device_list.init('http://192.168.1.254'), interval=30) ] for hotspot in hotspots: virtual.add_hotspot(hotspot, (0, y_offset)) y_offset += hotspot.height render_logo(draw, y_offset, device.width, args.title) # time.sleep(5.0) # for y in pause_every(64, 96, infinite(y_offset)): for y in stepper(64, y_offset, 512): with framerate_regulator(): virtual.set_position((0, y))