def test_sbpd_map(visualize=False):
np.random.seed(seed=1)
# Define a set of positions and evaluate objective
pos_nk2 = tf.constant([[[8., 16.], [8., 12.5], [18., 16.5]]],
dtype=tf.float32)
trajectory = Trajectory(dt=0.1, n=1, k=3, position_nk2=pos_nk2)
p = create_params()
# Create an SBPD Map
obstacle_map = SBPDMap(p.obstacle_map_params)
obs_dists_nk = obstacle_map.dist_to_nearest_obs(trajectory.position_nk2())
assert (np.allclose(obs_dists_nk, [0.59727454, 1.3223624, 0.47055122]))
if visualize:
occupancy_grid_nn = obstacle_map.create_occupancy_grid()
fig = plt.figure()
ax = fig.add_subplot(121)
obstacle_map.render(ax)
ax = fig.add_subplot(122)
ax.imshow(occupancy_grid_nn, cmap='gray', origin='lower')
ax.set_axis_off()
plt.show()
def test_sbpd_map(visualize=False):
np.random.seed(seed=1)
p = create_params()
r, dx_cm, traversible = load_building(p)
trajectory = Trajectory(dt=0.1, n=1, k=3, position_nk2=p.pos_nk2)
obstacle_map = SBPDMap(p.obstacle_map_params,
renderer=0,
res=dx_cm,
map_trav=traversible)
obs_dists_nk = obstacle_map.dist_to_nearest_obs(trajectory.position_nk2())
assert (np.allclose(obs_dists_nk[0], p.test_obst_map_ans))
if visualize:
#occupancy_grid_nn = obstacle_map.create_occupancy_grid(trajectory.position_nk2())
fig = plt.figure()
ax = fig.add_subplot(121)
obstacle_map.render(ax)
ax = fig.add_subplot(122)
# ax.imshow(occupancy_grid_nn, cmap='gray', origin='lower')
ax.set_axis_off()
# plt.show()
fig.savefig('./tests/obstacles/test_obstacle_map.png',
bbox_inches='tight',
pad_inches=0)