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))
