def init():
init = Config['test']['initial_state']
x = np.array([[
init['x_0'], init['vx_0'], init['y_0'], init['vy_0'], init['theta_0']
]]).T
P = np.array([[0, 0, 0, 0, 0], [0, 0, 0, 0, 0], [0, 0, 0, 0, 0],
[0, 0, 0, 0, 0], [0, 0, 0, 0, 0]])
oss = models.OdometryStateSpace()
iss = models.IMUStateSpace()
rss = models.RobotStateSpace()
return x, P, oss, iss, rss
"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))))
obstacles.append(
(int(point[0] / 20 + start[0]), (int(point[1] / 20 + start[1]))))