def main():
print('Running!')
# Read in arguments from the command line
args = parser.parse_args()
# File to be plotted
input_path = args.file_path
path, file = os.path.split(input_path)
# Folder to which the graph should be saved
if args.output_directory == ' ':
output_directory = path
else:
output_directory = args.output_directory
# Assign variable with inputs from the command line
name = args.name
plot_type = args.plot_type
plot_th = input_boolean(args.plot_temp_and_hum)
plot_fit = input_boolean(args.plot_fits)
average = args.choose_average
remove = args.remove_anomalous
# Extract the data
data = Data()
data.name = name
data.type = plot_type
data.extract_data(input_path, average)
if remove is not None:
for i in range(len(remove)):
data.remove_anomalies(i)
# Plot the graph
plot = Plot()
plot.plot_graph(output_directory, plot_th, plot_fit, data)
def mission_control(host, filename):
from Map import Config
from Plotting import Plot
from control import CompositeControl2, ScanHLine, FenceBumps, RingControls, FenceShrink, PathControls
config = Config("mission.yaml")
if config.aoi:
aoi = config.fence.intersection(config.aoi)
else:
aoi = config.fence
speed = config.mission.speed or 0.05
omega = config.mission.omega or 0.2
cut = config.mission.cut or 20
# controls = FenceBumps(fence, speed, omega)
# controls = RingControls(fence.exterior.coords, speed, omega)
aoi = aoi.buffer(-1,5, join_style=JOIN_STYLE.mitre)
controls = FenceShrink(config.fence, aoi, speed, omega)
# controls = ScanHLine(-10, -4.5, 13, -1.5, speed, omega) # midten - lang
# controls = ScanHLine(-10.5, -1.6, 17, -0.9, speed, omega) # roser
# controls = ScanHLine(-10, -7.5, -1, -4, speed, omega) # slackline - gml gran
# controls = ScanHLine(-10, -9, -6, -4, speed, omega) # slackline - plomme
# controls = ScanHLine(-11, -7, -4, -1.2, speed, omega) # midten med mest gras
# controls = ScanHLine(-8, -3, -4, -1.2, speed, omega) # midten med mest gras
# controls = ScanHLine(-4, -4, 3, -1.2, speed, omega) # midten med mest gras
# controls = ScanHLine(-11, -6, -5, -0.4, speed, omega) # mot epler
# controls = ScanHLine(-2, -4, 2, -1, speed, omega) # test
# from control import LineControlTest
# controls = LineControlTest()
# controls = PathControls(aoi.exterior.coords, speed, omega)
control = CompositeControl2(controls)
logger.info("Starting mission control for %s", controls)
plot = Plot(frames_per_plot=host is None and 5 or 1)
plot.add_shape(config.fence, facecolor="khaki")
plot.add_shape(aoi, facecolor="darkkhaki")
plot.pause()
# input("Wait for key press")
if host is None:
if filename:
# replay(open(filename), plot)
asyncio.run(replay2(play.parse(filename), plot))
else:
simulated_control(control, plot)
else:
asyncio.run(realtime_control(host, control, plot, cut), debug=True)
asyncio.run(shutdown(asyncio.get_event_loop()))
plot.pause()
plot.show()
async def simulate_point():
from RobotModel import RobotModel
model = RobotModel(State(1, 1, 0, 0))
x = -5
y = 2
control = PointControl(x, y, 0.2, 0.2, distance(x, y, 0.4))
plot = Plot()
dt = 0.1
t = 0.0
state = model.get_state()
plot.update(state)
while not control.end(t, state):
t += dt
model.update(dt)
state = model.get_state()
plot.update(state)
speed, omega = control.update(t, state)
if speed is not None:
model.set_speed_omega(speed, omega)
# Get the training and test data
train_data, test_data = OnehotDatasetSplit(
config['paths']['dataset_path'], config['hyper_param']['test_split_pr'],
config['hyper_param']['batch_size'], config['hyper_param']['max_data'])
# Create a model.
model = getattr(Models, model_type)(test_data.dataset.data.shape[2:])
# Define a optimizer
optimizer = getattr(Optimizer, optimizer_type)(config, model)
# Get a loss function
loss_fn = getattr(LossFunction, loss_type)
# Class creation
Plot = Plot(config, model, optimizer)
Run = Run(config, Plot)
# Loads information from pre trained model
if config['pre_trained']['use_saved_model']:
# Load the stored information
stored_model_info = torch.load(config['pre_trained']['path_saved_model'] +
'/model.pth')
model.load_state_dict(stored_model_info['model_state_dict'])
optimizer.load_state_dict(stored_model_info['optimizer_state_dict'])
if config['utils']['train_model']:
Run.train_model(model, optimizer, loss_fn, train_data, test_data)
#Run.test_model(model, optimizer, loss_fn, train_data, test_data)
import math
from math import sin, cos, pi
import cmath
from Plotting import Plot
from PID import PID
from state import State
from dataclasses import replace
import numpy as np
from statistics import leastsq
plot = Plot()
# e.g. "slipping" if diameter of left wheel is smaller
slip_left = 0.0
# slip_left = 0.05
# FIXME: does not handle any error here, e.g. 1.01
omega_scale = 1.01
"""
TODO:
- Estimate heading - from odometry - with noise
- Test error in position estimate
- Fuse GPS and odometry, with noise in GPS and odometry - and systematic error in odometry
"""
class Estimator:
def update(self, dt: float, state: State):
pass
def state(self) -> State:
def main():
print('Running!')
# Read in arguments from the command line
args = parser.parse_args()
# Folder containing the data
input_directory = args.file_directory
# Folder to which the graphs should be saved
if args.output_directory == ' ':
output_directory = args.file_directory
else:
output_directory = args.output_directory
# Assign variable with inputs from the command line
plot_iv = input_boolean(args.plot_iv)
plot_cv = input_boolean(args.plot_cv)
plot_it = input_boolean(args.plot_it)
plot_ivth = input_boolean(args.plot_ivth)
plot_cvth = input_boolean(args.plot_cvth)
plot_itth = input_boolean(args.plot_itth)
plot_ivf = input_boolean(args.plot_ivf)
plot_cvf = input_boolean(args.plot_cvf)
plot_itf = input_boolean(args.plot_itf)
#plot_ivg = input_boolean(args.plot_ivg)
#plot_cvg = input_boolean(args.plot_cvg)
#plot_itg = input_boolean(args.plot_itg)
average_type = args.choose_average
# Print inputs
print('Input Directory: ' + input_directory)
print('Output Directory: ' + output_directory)
print(f'Plot CV Graphs: {plot_cv}')
print(f'Plot IV Graphs: {plot_iv}')
print(f'Plot It Graphs: {plot_it}')
print(f'Plot CV Graphs with temperature and humidity: {plot_cvth}')
print(f'Plot IV Graphs with temperature and humidity: {plot_ivth}')
print(f'Plot It Graphs with temperature and humidity: {plot_itth}')
print(f'Plot IV Graphs with fits: {plot_ivf}')
print(f'Plot It Graphs with fits: {plot_itf}')
print('Calculate averages using ' + average_type)
# List of data files
files = []
#print(os.listdir(input_directory))
folder = os.listdir(input_directory)
# Struggled to get the code below to work
"""
for Input in args.file_directory:
print("\tInput, ", Input)
OSInputs = sorted(glob.glob(Input+'/*.txt'))
print(OSInputs)
for OSInput in OSInputs:
if os.path.isfile(OSInput) and OSInput.endswith('.txt') and 'short' not in OSInput:
files.append(OSInput)
print("\t\t" + OSInput)
"""
# Fill files list
for filename in folder:
if os.path.isfile(os.path.join(input_directory, filename)) and filename.endswith('.txt') and 'short' not in filename:
files.append(os.path.join(input_directory, filename))
else:
continue
file_data = {}
#print(files)
# Create a dictionary with all the data files and the corresponding data object
for i in range(len(files)):
data = Data()
data.extract_data(files[i], average_type)
file_data[files[i]] = data
"""
for f in file_data.values():
print(f.v_mean)
continue
"""
# Separate the data into IV, CV and It
iv_data = {}
cv_data = {}
it_data = {}
for f1, f2 in file_data.items():
if f2.type == 'iv':
iv_data[f1] = f2
elif f2.type == 'cv':
cv_data[f1] = f2
elif f2.type == 'it':
it_data[f1] = f2
# Plot all IV files
if plot_iv:
print('Plotting IV Graphs')
for f1, f2 in iv_data.items():
graph = Plot()
graph.plot_graph(output_directory, plot_ivth, plot_ivf, f2)
# Plot all CV files
if plot_cv:
print('Plotting CV Graphs')
for f1, f2 in cv_data.items():
graph = Plot()
graph.plot_graph(output_directory, plot_cvth, plot_cvf, f2)
# Plot all It files
if plot_it:
print('Plotting It Graphs')
for f1, f2 in it_data.items():
graph = Plot()
graph.plot_graph(output_directory, plot_itth, plot_itf, f2)