# Use LaTex to write all text
"text.usetex": True,
"font.family": "serif",
# Use 11pt (10 is standard but due to file conversions, I use 11) font in plots, to $
"axes.labelsize": 11,
"font.size": 11,
# Make the legend/label fonts a little smaller
"legend.fontsize": 9,
"xtick.labelsize": 9,
"ytick.labelsize": 9,
}
matplotlib.rcParams.update(nice_fonts) # LaTeX fonts and sizes for graphs!
imu = sensors.IMU()
lidar = sensors.Lidar()
lss = models.LidarStateSpace()
iss = models.IMUStateSpace()
obstacles_lidar = []
for i in range(1):
heading = (-1 * np.radians(imu.euler[0] - iss.theta_bias)) % (2 * np.pi)
scan = lidar.get_scan(100, 0.04)
obstacles_lidar = lss.get_obstacles(scan, heading)
print(obstacles_lidar)
obstacles = []
for point in obstacles_lidar:
#obstacles.append((int(point[0]/20), (int(point[1]/20 + extents[1]/2))))
def main():
# Sensor init
imu = sensors.IMU()
enc = sensors.Encoders()
lidar = sensors.Lidar()
# State Space Models
rss = models.RobotStateSpace()
iss = models.IMUStateSpace()
oss = models.OdometryStateSpace()
lss = models.LidarStateSpace()
# Control init
ctrl = control.Control()
# Path Planning init
#extents = input("Enter world extents: e.g. 3000 2200")
#start = input("Enter start pose: e.g. 500 200 0").split()
#goal = input("Enter goal pose: e.g. 2800 0 0")
extents = Config['path_planning']['test_params']['extents']
start = Config['path_planning']['test_params']['start']
goal = Config['path_planning']['test_params']['goal']
# Obstacle Detection
obstacles_lidar = []
heading = (-1 * np.radians(imu.euler[0] - iss.theta_bias)) % (2 * np.pi)
scan = lidar.get_scan(100,
0.04) # add more updates to improve initial scan
obstacles_lidar = lss.get_obstacles(scan, heading)
print(obstacles_lidar)
# Scaling down
extents = (int(extents[0] / d), int(extents[1] / d))
start = (start[0] / d, (start[1] / d) + (extents[1] / 2),
np.radians(start[2]))
goal = (goal[0] / d, (goal[1] / d) + (extents[1] / 2), np.radians(goal[2]))
obstacles = []
for point in obstacles_lidar:
obstacles.append(
(int(point[0] / 20 + start[0]), (int(point[1] / 20 + start[1]))))
pp = kastar.KinematicAStar(extents)
path_scaled_down, moves, visited, obstacles_array = pp.plan_path(
start, goal, obstacles)
# scaling path coords back to real scale
path = []
for a, b in path_scaled_down:
log_data(path_file, [a * d, b * d])
path.append((a * d, b * d))
print(path)
print(moves)
input("Ready? ")
print(3)
time.sleep(1)
print(2)
time.sleep(1)
print(1)
time.sleep(1)
print("GO!")
# Threading init
print_lock = threading.Lock()
state_vector_lock = threading.Lock()
fast_thread = threading.Thread(target=fast_loop,
args=(print_lock, state_vector_lock, imu,
enc, iss, oss, rss, path, ctrl),
name="fast_loop")
fast_thread.start()
#%%
import numpy as np
import matplotlib.pyplot as plt
import time
from skimage.draw import line
from tqdm import tqdm_notebook
import cv2
import sensors
from occupancy_map import Map
from transform import Transform
from particle_filter import ParticleFilter
# %% import sensor
lidar = sensors.Lidar('data/sensor_data/lidar.csv', downsample_rate=1)
gyro = sensors.Gyroscope('data/sensor_data/fog.csv', downsample_rate=1)
encoder = sensors.Encoder('data/sensor_data/encoder.csv', downsample_rate=1)
# %% initialization
myMap = Map(res=1, x_range=[-1300, 1300], y_range=[-1200, 1200])
tf = Transform()
# %% Main Loop
pf = ParticleFilter(n_particles=1, add_noise=False)
gyro_range = int(gyro.get_length() * 1)
now = time.time()
car_trajactory = np.zeros([2, gyro_range])
# initialize index
encoder_idx = 0
update_count = 0
max_idx = min(encoder.get_length() - 1, lidar.get_length() - 1)
update_now = False