Commit 8c304ac2 authored by Brian Hou's avatar Brian Hou
Browse files

Update Project 4: Planning.

Add new autograder tests to catch sampler and collision-checking errors
earlier. In particular, the sampler tests should help identify scaling errors
and clarify expected behavior. Also, the collision-checking tests using
MapMetaData should help catch errors before running planner_sim.launch.
parent ceaf63a2
Pipeline #425984 failed with stage
in 6 minutes and 10 seconds
......@@ -283,7 +283,8 @@ def map_to_world(poses, map_info, out=None):
# Rotation
out[:, [0, 1]] = np.matmul(poses[:, [0, 1]], rotation_matrix(angle))
out[:, 2] = poses[:, 2] + angle
if poses.shape[1] == 3:
out[:, 2] = poses[:, 2] + angle
# Scale
out[:, :2] *= float(scale)
......@@ -318,7 +319,8 @@ def world_to_map(poses, map_info, out=None):
# Rotate poses
out[:, [0, 1]] = np.matmul(out[:, [0, 1]], rotation_matrix(angle))
out[:, 2] = poses[:, 2] + angle
if poses.shape[1] == 3:
out[:, 2] = poses[:, 2] + angle
return out
......
......@@ -363,5 +363,5 @@ class Roadmap(object):
if saveto is not None:
plt.savefig(saveto, bbox_inches="tight")
print("Saved graph image to", self.saveto)
print("Saved graph image to", saveto)
plt.show()
......@@ -4,6 +4,8 @@ import numpy as np
import rosunit
import unittest
from nav_msgs.msg import MapMetaData
from planning.problems import PlanarProblem, R2Problem
from planning.samplers import LatticeSampler
......@@ -21,7 +23,8 @@ class TestPlanarProblem(unittest.TestCase):
[1, -1],
[-1, 1],
[-1, -1],
]
],
dtype=float,
)
np.testing.assert_equal(
self.problem.check_state_validity(states),
......@@ -40,7 +43,8 @@ class TestPlanarProblem(unittest.TestCase):
[1, 9],
[1, 10],
[1, 11],
]
],
dtype=float,
)
np.testing.assert_equal(
self.problem.check_state_validity(states),
......@@ -59,7 +63,8 @@ class TestPlanarProblem(unittest.TestCase):
[4, 3],
[3.1, 4.1],
[4.1, 3.1],
]
],
dtype=float,
)
np.testing.assert_equal(
self.problem.check_state_validity(states),
......@@ -67,6 +72,113 @@ class TestPlanarProblem(unittest.TestCase):
err_msg="States in collision are not valid",
)
def test_planar_state_validity_extents_with_map_resolution(self):
permissible_region = np.ones((5, 10), dtype=bool)
resolution = 0.1
map_info = MapMetaData(resolution=resolution)
self.problem = PlanarProblem(permissible_region, map_info)
# The permissible region is 10 pixels by 5 pixels. A resolution of 0.1
# meters/pixels means that map width/height is 1 meter x 0.5
# meters. This results in extents of (0, 1) x (0, 0.5).
np.testing.assert_equal(
self.problem.extents,
np.array([[0, 1], [0, 0.5]]),
err_msg="extents do not match expected extents",
)
states = np.array(
[
[0.4, 0.3],
[0.5, 0.3],
[0.7, 0.2],
],
dtype=float,
)
np.testing.assert_equal(
self.problem.check_state_validity(states),
np.array([1, 1, 1], dtype=bool),
err_msg="States within the extents are valid",
)
states = np.array(
[
[4, 3],
[5, 3],
[7, 2],
],
dtype=float,
)
np.testing.assert_equal(
self.problem.check_state_validity(states),
np.array([0, 0, 0], dtype=bool),
err_msg="States outside the extents are not valid",
)
def test_planar_state_validity_extents_with_map_translation(self):
permissible_region = np.ones((20, 20), dtype=bool)
resolution = 0.1
map_info = MapMetaData(resolution=resolution)
shift = 1
map_info.origin.position.x -= shift
map_info.origin.position.y -= shift
self.problem = PlanarProblem(permissible_region, map_info)
# The permissible region is 20 pixels by 20 pixels. A resolution of 0.1
# meters/pixels means that map width/height is 2 meters. Shifting the x
# and y position of the origin by -1 meter results in extents of (-1, 1)
# x (-1, 1).
np.testing.assert_equal(
self.problem.extents,
np.array([[-1, 1], [-1, 1]]),
err_msg="extents do not match expected extents",
)
states = np.array(
[
[0.4, 0.3],
[0.5, 0.3],
[0.7, 0.2],
[-1.0, -1.0],
],
dtype=float,
)
np.testing.assert_equal(
self.problem.check_state_validity(states),
np.array([1, 1, 1, 1], dtype=bool),
err_msg="States within the extents are valid",
)
states = np.array(
[
[1.4, 1.3],
[1.5, 1.3],
[1.7, 1.2],
[0, 0],
],
dtype=float,
)
np.testing.assert_equal(
self.problem.check_state_validity(states),
np.array([0, 0, 0, 1], dtype=bool),
err_msg="States outside the extents are not valid",
)
states = np.array(
[
[14, 13],
[15, 13],
[17, 12],
[0, 0],
],
dtype=float,
)
np.testing.assert_equal(
self.problem.check_state_validity(states),
np.array([0, 0, 0, 1], dtype=bool),
err_msg="States outside the extents are not valid",
)
class TestR2Problem(unittest.TestCase):
def setUp(self):
......
......@@ -160,6 +160,73 @@ class TestHaltonSampler(unittest.TestCase):
msg="Each call to sample should return different samples",
)
def test_sample_2d_scaled_asymmetric_extent(self):
batch_size = 20
extents = np.array([[-5, 10], [4, 8]])
self.sampler = HaltonSampler(extents)
batch = self.sampler.sample(batch_size)
self.assertEqual(
batch.shape,
(batch_size, 2),
msg="sample should return the number of samples requested",
)
xmin, xmax = extents[0, :]
ymin, ymax = extents[1, :]
self.assertTrue(
(batch[:, 0] >= xmin).all(),
msg="sampled x values should be at least the minimum x value",
)
self.assertTrue(
(batch[:, 0] < xmax).all(),
msg="sampled x values should be less than the maximum x value",
)
self.assertTrue(
(batch[:, 1] >= ymin).all(),
msg="sampled y values should be at least the minimum y value",
)
self.assertTrue(
(batch[:, 1] < ymax).all(),
msg="sampled y values should be less than the maximum y value",
)
def test_sample_2d_scaled_domain(self):
# Mock out the Halton sample generator with two samples: (0.4, 0.6) and (0.3, 0.7)
mocked_halton_gen = np.array([[0.4, 0.6], [0.3, 0.7]])
sample_size = mocked_halton_gen.shape[0]
scale = 10
extents = np.zeros((2, 2))
extents[:, 1] = scale
# The Halton samples should be scaled from the range (0, 1) x (0, 1) to
# match the extents (0, 10) x (0, 10). Therefore, the expected batch of
# samples is (4, 6) and (3, 7).
self.sampler = HaltonSampler(extents)
self.sampler.gen = mocked_halton_gen
batch = self.sampler.sample(sample_size)
np.testing.assert_equal(
batch,
scale * mocked_halton_gen,
err_msg="Halton samples were scaled incorrectly",
)
shift = 5
extents -= shift
# The Halton samples should be scaled from the range (0, 1) x (0, 1) to
# match the extents (-5, 5) x (-5, 5). Therefore, the expected batch of
# samples is (-1, 1) and (-2, 2).
self.sampler = HaltonSampler(extents)
self.sampler.gen = mocked_halton_gen
batch = self.sampler.sample(sample_size)
np.testing.assert_equal(
batch,
scale * mocked_halton_gen - shift,
err_msg="Halton samples were scaled incorrectly",
)
def test_sample_3d(self):
extents = np.zeros((3, 2))
extents[:, 1] = 1
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment