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.

cse478/src/cse478/utils.py

@@ -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
 

planning/src/planning/roadmap.py

@@ -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()

planning/test/problems.py

@@ -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):

planning/test/samplers.py

@@ -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