Release Project 4: Planning.

planning/test/astar.py

+#!/usr/bin/env python
+
+import numpy as np
+import os
+import rosunit
+import unittest
+
+from planning import search
+from planning.samplers import HaltonSampler
+from planning.problems import R2Problem, SE2Problem
+from planning.roadmap import Roadmap
+
+
+class TestAStarR2(unittest.TestCase):
+    def setUp(self):
+        fname = os.path.join(os.path.dirname(__file__), "share", "map1.txt")
+        permissible_region = np.loadtxt(fname, dtype=bool)
+        self.problem = R2Problem(permissible_region, check_resolution=0.5)
+        self.sampler = HaltonSampler(self.problem.extents)
+
+    def test_astar_r2problem(self):
+        num_vertices = 10
+        connection_radius = 5.0
+        lazy = False
+        rm = Roadmap(self.problem, self.sampler, num_vertices, connection_radius, lazy)
+
+        start_id = rm.add_node(np.array([1, 1]), is_start=True)
+        goal_id = rm.add_node(np.array([8, 8]), is_start=False)
+        correct_path = [start_id, 9, 2, 7, goal_id]
+
+        path, parents = search.astar(rm, start_id, goal_id)
+        self.assertEqual(
+            path[0],
+            start_id,
+            msg="Path must begin at start",
+        )
+        self.assertEqual(
+            path[-1],
+            goal_id,
+            msg="Path must end at goal",
+        )
+        np.testing.assert_almost_equal(
+            rm.compute_path_length(path),
+            rm.compute_path_length(correct_path),
+            err_msg="A* implementation is incorrect",
+        )
+        self.assertEqual(
+            path,
+            correct_path,
+            msg="A* implementation is incorrect",
+        )
+
+        self.assertEqual(
+            parents[start_id],
+            search.NULL,
+            msg="Parent of start is -1 (sentinel value for null)",
+        )
+        for start_child in (5, 0, 9):
+            self.assertEqual(
+                parents[start_child],
+                start_id,
+                msg="Child of start should have start as parent",
+            )
+        for parent, child in zip(correct_path[:-1], correct_path[1:]):
+            self.assertEqual(
+                parents[child],
+                parent,
+                msg="Previous node in shortest path should be parent",
+            )
+
+
+class TestAStarSE2(unittest.TestCase):
+    def setUp(self):
+        fname = os.path.join(os.path.dirname(__file__), "share", "map1.txt")
+        permissible_region = np.loadtxt(fname, dtype=bool)
+        self.problem = SE2Problem(permissible_region, curvature=3.0)
+        self.sampler = HaltonSampler(self.problem.extents)
+
+    def test_astar_se2problem(self):
+        num_vertices = 8
+        connection_radius = 5.0
+        lazy = False
+        rm = Roadmap(self.problem, self.sampler, num_vertices, connection_radius, lazy)
+
+        start_id = rm.add_node(np.array([1, 1, 0]), is_start=True)
+        goal_id = rm.add_node(np.array([7, 1, 0]), is_start=False)
+        correct_path = [start_id, 3, goal_id]
+
+        path, parents = search.astar(rm, start_id, goal_id)
+        self.assertEqual(
+            path[0],
+            start_id,
+            msg="Path must begin at start",
+        )
+        self.assertEqual(
+            path[-1],
+            goal_id,
+            msg="Path must end at goal",
+        )
+        self.assertEqual(
+            path,
+            correct_path,
+            msg="A* implementation is incorrect",
+        )
+
+        self.assertEqual(
+            parents[start_id],
+            search.NULL,
+            msg="Parent of start is -1 (sentinel value for null)",
+        )
+        for parent, child in zip(correct_path[:-1], correct_path[1:]):
+            self.assertEqual(
+                parents[child],
+                parent,
+                msg="Previous node in shortest path should be parent",
+            )
+
+
+class TestLazyAStar(unittest.TestCase):
+    def setUp(self):
+        fname = os.path.join(os.path.dirname(__file__), "share", "map1.txt")
+        permissible_region = np.loadtxt(fname, dtype=bool)
+        self.problem = R2Problem(permissible_region, check_resolution=0.5)
+        self.sampler = HaltonSampler(self.problem.extents)
+
+    def test_lazy_astar(self):
+        num_vertices = 10
+        connection_radius = 5.0
+        lazy = True
+        rm = Roadmap(self.problem, self.sampler, num_vertices, connection_radius, lazy)
+
+        start_id = rm.add_node(np.array([1, 1]), is_start=True)
+        goal_id = rm.add_node(np.array([8, 8]), is_start=False)
+        correct_path = [start_id, 9, 2, 7, goal_id]
+
+        path, parents = search.astar(rm, start_id, goal_id)
+        self.assertEqual(
+            path[0],
+            start_id,
+            msg="Path must begin at start",
+        )
+        self.assertEqual(
+            path[-1],
+            goal_id,
+            msg="Path must end at goal",
+        )
+        np.testing.assert_almost_equal(
+            rm.compute_path_length(path),
+            rm.compute_path_length(correct_path),
+            err_msg="Lazy A* implementation is incorrect",
+        )
+        self.assertEqual(
+            path,
+            correct_path,
+            msg="Lazy A* implementation is incorrect",
+        )
+
+        self.assertEqual(
+            parents[start_id],
+            search.NULL,
+            msg="Parent of start is -1 (sentinel value for null)",
+        )
+        for start_child in (5, 0, 9):
+            self.assertEqual(
+                parents[start_child],
+                start_id,
+                msg="Child of start should have start as parent",
+            )
+        for parent, child in zip(correct_path[:-1], correct_path[1:]):
+            self.assertEqual(
+                parents[child],
+                parent,
+                msg="Previous node in shortest path should be parent",
+            )
+
+        self.assertEqual(
+            rm.edges_evaluated, 10, msg="Incorrect number of edges evaluated"
+        )
+
+
+if __name__ == "__main__":
+    rosunit.unitrun("planning", "test_astar_r2", TestAStarR2)
+    rosunit.unitrun("planning", "test_astar_se2", TestAStarSE2)
+    rosunit.unitrun("planning", "test_lazy_astar", TestLazyAStar)

planning/test/problems.py

+#!/usr/bin/env python
+
+import numpy as np
+import rosunit
+import unittest
+
+from planning.problems import PlanarProblem, R2Problem
+from planning.samplers import LatticeSampler
+
+
+class TestPlanarProblem(unittest.TestCase):
+    def setUp(self):
+        permissible_region = np.ones((5, 10), dtype=bool)
+        permissible_region[3:7, 3:7] = 0
+        self.problem = PlanarProblem(permissible_region)
+
+    def test_planar_state_validity_extents(self):
+        states = np.array(
+            [
+                [1, 1],
+                [1, -1],
+                [-1, 1],
+                [-1, -1],
+            ]
+        )
+        np.testing.assert_equal(
+            self.problem.check_state_validity(states),
+            np.array([1, 0, 0, 0], dtype=bool),
+            err_msg="States below the extents are not valid",
+        )
+
+        states = np.array(
+            [
+                [9, 4],
+                [9.9, 4.9],
+                [9, 1],
+                [10, 1],
+                [11, 1],
+                [11, 11],
+                [1, 9],
+                [1, 10],
+                [1, 11],
+            ]
+        )
+        np.testing.assert_equal(
+            self.problem.check_state_validity(states),
+            np.array([1, 1, 1, 0, 0, 0, 0, 0, 0], dtype=bool),
+            err_msg="States above the extents are not valid",
+        )
+
+    def test_planar_state_validity_collision(self):
+        permissible_region = np.ones((10, 10), dtype=bool)
+        permissible_region[4:6, 3:7] = 0
+        self.problem = PlanarProblem(permissible_region)
+
+        states = np.array(
+            [
+                [3, 4],
+                [4, 3],
+                [3.1, 4.1],
+                [4.1, 3.1],
+            ]
+        )
+        np.testing.assert_equal(
+            self.problem.check_state_validity(states),
+            np.array([0, 1, 0, 1], dtype=bool),
+            err_msg="States in collision are not valid",
+        )
+
+
+class TestR2Problem(unittest.TestCase):
+    def setUp(self):
+        permissible_region = np.ones((10, 10), dtype=bool)
+        permissible_region[3:7, 3:7] = 0
+        self.problem = R2Problem(permissible_region)
+        self.sampler = LatticeSampler(self.problem.extents)
+
+    def test_r2_heuristic(self):
+        # This sampler creates vertices for x and y = (1.67, 5.0, 8.33).
+        # Only the state (5.0, 5.0) should be in collision.
+
+        num_samples = 9
+        samples = self.sampler.sample(num_samples)
+        valid = self.problem.check_state_validity(samples)
+
+        self.assertEqual(
+            valid.sum(),
+            8,
+            msg="Only one sample should be in collision",
+        )
+        np.testing.assert_equal(
+            samples[~valid, :],
+            np.array([[5, 5]]),
+            err_msg="The sample in collision should be (5, 5)",
+        )
+
+        samples = samples[valid, :]
+        heuristics = self.problem.compute_heuristic(samples[0, :], samples[1:, :])
+
+        distances = np.linalg.norm(samples[1:, :] - samples[0, :], axis=1)
+        np.testing.assert_allclose(
+            heuristics,
+            distances,
+            err_msg="The R2 heuristic should be the Euclidean norm",
+        )
+
+
+if __name__ == "__main__":
+    rosunit.unitrun("planning", "test_planar_problem", TestPlanarProblem)
+    rosunit.unitrun("planning", "test_r2_problem", TestR2Problem)

planning/test/roadmap.py

+#!/usr/bin/env python
+
+import collections
+import numpy as np
+import rosunit
+import unittest
+
+from planning.problems import R2Problem
+from planning.samplers import LatticeSampler, HaltonSampler
+from planning.roadmap import Roadmap
+
+
+class TestRoadmap(unittest.TestCase):
+    def setUp(self):
+        permissible_region = np.ones((10, 10), dtype=bool)
+        permissible_region[3:7, 3:7] = 0
+        self.problem = R2Problem(permissible_region)
+        self.sampler = LatticeSampler(self.problem.extents)
+
+    def _count_outdegree(self, edges, directed):
+        counter = collections.Counter()
+        for (u, v) in edges:
+            counter[u] += 1
+            if not directed:
+                counter[v] += 1
+        return counter
+
+    def test_r2problem_roadmap_edge_collisions(self):
+        # This lattice roadmap has vertices for x and y = (1.67, 5.0, 8.33).
+        # Setting the connection radius to 15 creates a fully-connected graph.
+        num_vertices = 9
+        connection_radius = 15
+        lazy = False
+        rm = Roadmap(
+            self.problem,
+            self.sampler,
+            num_vertices,
+            connection_radius,
+            lazy,
+        )
+        outdegree = self._count_outdegree(rm.graph.edges(), directed=False)
+        for node in range(rm.graph.number_of_nodes()):
+            deg = outdegree.get(node, 0)
+            self.assertGreater(deg, 0, msg="All nodes should be connected")
+
+        # Uncomment this line to visualize the roadmap
+        # rm.visualize(show_edges=True)
+
+        test_node = 1
+        test_state = rm.vertices[test_node, :]
+        self.assertEqual(
+            outdegree[test_node],
+            2,
+            msg="State {} should be connected to two neighbors".format(test_state),
+        )
+
+    def test_r2problem_roadmap_four(self):
+        # This lattice roadmap has vertices for x and y = (0.5, 1.5, ..., 9.5).
+        # Setting the radius to 1.0 creates a four-connected lattice graph,
+        # excluding vertices that are in collision.
+
+        num_vertices = 100
+        connection_radius = 1.0
+        lazy = False
+        rm = Roadmap(
+            self.problem,
+            self.sampler,
+            num_vertices,
+            connection_radius,
+            lazy,
+        )
+        outdegree = self._count_outdegree(rm.graph.edges(), directed=False)
+        for node in range(rm.graph.number_of_nodes()):
+            deg = outdegree.get(node, 0)
+            self.assertGreater(deg, 0, msg="All nodes should be connected")
+            self.assertLessEqual(
+                deg, 4, msg="All nodes should be connected to at most four neighbors"
+            )
+
+        # These start and goal states are in the center of a lattice cell, so
+        # they should be connected to four neighbors (the corners of the cell).
+        start_state = np.ones(2)
+        start_node = rm.add_node(start_state, is_start=True)
+        goal_state = 9 * np.ones(2)
+        goal_node = rm.add_node(goal_state, is_start=False)
+        outdegree = self._count_outdegree(rm.graph.edges(), directed=False)
+        self.assertEqual(
+            outdegree[start_node], 4, msg="Start should be connected to four neighbors"
+        )
+        self.assertEqual(
+            outdegree[goal_node], 4, msg="Goal should be connected to four neighbors"
+        )
+
+        # Uncomment this line to visualize the roadmap
+        # rm.visualize(show_edges=True)
+
+    def test_r2problem_roadmap_eight(self):
+        # This lattice roadmap has vertices for x and y = (0.5, 1.5, ..., 9.5).
+        # Setting the radius to sqrt(2) creates an eight-connected lattice graph,
+        # excluding vertices that are in collision.
+
+        num_vertices = 100
+        connection_radius = np.sqrt(2)
+        lazy = False
+        rm = Roadmap(
+            self.problem,
+            self.sampler,
+            num_vertices,
+            connection_radius,
+            lazy,
+        )
+        outdegree = self._count_outdegree(rm.graph.edges(), directed=False)
+        for node in range(rm.graph.number_of_nodes()):
+            deg = outdegree.get(node, 0)
+            self.assertGreater(deg, 0, msg="All nodes should be connected")
+            self.assertLessEqual(
+                deg, 8, msg="All nodes should be connected to at most four neighbors"
+            )
+
+        # This start state is on an edge of a lattice cell, so it should be
+        # connected to six neighbors (0.5, 0.5), (1.5, 0.5), (2.5, 0.5),
+        # (0.5, 1.5), (1.5, 1.5), (2.5, 1.5).
+        start_state = np.array([1.5, 1.0])
+        start_node = rm.add_node(start_state, is_start=True)
+        outdegree = self._count_outdegree(rm.graph.edges(), directed=False)
+        self.assertEqual(
+            outdegree[start_node], 6, msg="Start should be connected to four neighbors"
+        )
+
+        # Uncomment this line to visualize the roadmap
+        # rm.visualize(show_edges=True)
+
+    def test_r2problem_roadmap_halton(self):
+        self.sampler = HaltonSampler(self.problem.extents)
+
+        num_vertices = 36
+        connection_radius = 2.5
+        lazy = False
+        rm = Roadmap(
+            self.problem,
+            self.sampler,
+            num_vertices,
+            connection_radius,
+            lazy,
+        )
+
+        outdegree = self._count_outdegree(rm.graph.edges(), directed=False)
+        for node in range(rm.graph.number_of_nodes()):
+            deg = outdegree.get(node, 0)
+            self.assertGreater(deg, 0, msg="All nodes should be connected")
+
+        try:
+            state = 5 * np.ones(2)
+            node = rm.add_node(state, is_start=True)
+            self.fail("state in collision should error")
+        except ValueError:
+            pass
+
+        state = 0.1 * np.ones(2)
+        node = rm.add_node(state, is_start=True)
+        outdegree = self._count_outdegree(rm.graph.edges(), directed=False)
+        self.assertEqual(
+            outdegree[node], 2, msg="Node should be connected to two neighbors"
+        )
+
+        state = np.ones(2)
+        node = rm.add_node(state, is_start=True)
+        outdegree = self._count_outdegree(rm.graph.edges(), directed=False)
+        self.assertEqual(
+            outdegree[node], 5, msg="Node should be connected to five neighbors"
+        )
+
+        state = 9 * np.ones(2)
+        node = rm.add_node(state, is_start=True)
+        outdegree = self._count_outdegree(rm.graph.edges(), directed=False)
+        self.assertEqual(
+            outdegree[node], 4, msg="Node should be connected to four neighbors"
+        )
+
+        # Uncomment this line to visualize the roadmap
+        # rm.visualize(show_edges=True)
+
+
+if __name__ == "__main__":
+    rosunit.unitrun("planning", "test_roadmap", TestRoadmap)

planning/test/samplers.py

+#!/usr/bin/env python
+
+from __future__ import division
+
+import numpy as np
+import rosunit
+import unittest
+
+from planning.samplers import HaltonSampler, LatticeSampler
+
+
+class TestHaltonSampler(unittest.TestCase):
+    def setUp(self):
+        extents = np.zeros((2, 2))
+        extents[:, 1] = 1
+        self.sampler = HaltonSampler(extents)
+        self.batch_size = 1000
+
+    def test_compute_sample_base_two(self):
+        # The Halton sequence for base two begins with
+        # 0.5, 0.25, 0.75, 0.125, 0.625, 0.375, 0.875
+
+        np.testing.assert_allclose(
+            [self.sampler.compute_sample(i, 2) for i in range(0, 1)],
+            np.array([1]) / 2,
+            err_msg="The Halton sequence does not match.",
+        )
+        np.testing.assert_allclose(
+            [self.sampler.compute_sample(i, 2) for i in range(1, 3)],
+            np.array([1, 3]) / 4,
+            err_msg="The Halton sequence does not match.",
+        )
+        np.testing.assert_allclose(
+            [self.sampler.compute_sample(i, 2) for i in range(3, 7)],
+            np.array([1, 5, 3, 7]) / 8,
+            err_msg="The Halton sequence does not match.",
+        )
+        np.testing.assert_allclose(
+            [self.sampler.compute_sample(i, 2) for i in range(7, 15)],
+            np.array([1, 9, 5, 13, 3, 11, 7, 15]) / 16,
+            err_msg="The Halton sequence does not match.",
+        )
+
+    def test_compute_sample_base_three(self):
+        # The Halton sequence for base three begins with
+        # 1/3, 2/3, 1/9, 4/9, 7/9, 2/9, 5/9, 8/9
+
+        np.testing.assert_allclose(
+            [self.sampler.compute_sample(i, 3) for i in range(0, 2)],
+            np.array([1, 2]) / 3,
+            err_msg="The Halton sequence does not match.",
+        )
+        np.testing.assert_allclose(
+            [self.sampler.compute_sample(i, 3) for i in range(2, 8)],
+            np.array([1, 4, 7, 2, 5, 8]) / 9,
+            err_msg="The Halton sequence does not match.",
+        )
+
+    def test_sample_2d(self):
+        batch1 = self.sampler.sample(self.batch_size)
+        self.assertEqual(
+            batch1.shape,
+            (self.batch_size, 2),
+            msg="sample should return the number of samples requested",
+        )
+        np.testing.assert_allclose(
+            (batch1 > 0.5).all(axis=1).sum() / self.batch_size,
+            0.25,
+            atol=0.001,
+            err_msg="samples are not distributed uniformly across the extents",
+        )
+
+        batch2 = self.sampler.sample(self.batch_size)
+        self.assertEqual(
+            batch2.shape,
+            (self.batch_size, 2),
+            msg="sample should return the number of samples requested",
+        )
+        np.testing.assert_allclose(
+            (batch2 > 0.5).all(axis=1).sum() / self.batch_size,
+            0.25,
+            atol=0.001,
+            err_msg="samples are not distributed uniformly across the extents",
+        )
+
+        self.assertFalse(
+            np.array_equal(batch1, batch2),
+            msg="Each call to sample should return different samples",
+        )
+
+    def test_sample_2d_scaled_upper_extent(self):
+        extents = np.zeros((2, 2))
+        extents[:, 1] = 10
+        self.sampler = HaltonSampler(extents)
+
+        batch1 = self.sampler.sample(self.batch_size)
+        self.assertEqual(
+            batch1.shape,
+            (self.batch_size, 2),
+            msg="sample should return the number of samples requested",
+        )
+        np.testing.assert_allclose(
+            (batch1 > 5).all(axis=1).sum() / self.batch_size,
+            0.25,
+            atol=0.001,
+            err_msg="samples are not distributed uniformly across the extents",
+        )
+
+        batch2 = self.sampler.sample(self.batch_size)
+        self.assertEqual(
+            batch2.shape,
+            (self.batch_size, 2),
+            msg="sample should return the number of samples requested",
+        )
+        np.testing.assert_allclose(
+            (batch2 > 5).all(axis=1).sum() / self.batch_size,
+            0.25,
+            atol=0.001,
+            err_msg="samples are not distributed uniformly across the extents",
+        )
+
+        self.assertFalse(
+            np.array_equal(batch1, batch2),
+            msg="Each call to sample should return different samples",
+        )
+
+    def test_sample_2d_scaled_lower_extent(self):
+        extents = 10 * np.ones((2, 2))
+        extents[:, 0] = -10
+        self.sampler = HaltonSampler(extents)
+
+        batch1 = self.sampler.sample(self.batch_size)
+        self.assertEqual(
+            batch1.shape,
+            (self.batch_size, 2),
+            msg="sample should return the number of samples requested",
+        )
+        np.testing.assert_allclose(
+            (batch1 > 0).all(axis=1).sum() / self.batch_size,
+            0.25,
+            atol=0.001,
+            err_msg="samples are not distributed uniformly across the extents",
+        )
+
+        batch2 = self.sampler.sample(self.batch_size)
+        self.assertEqual(
+            batch2.shape,
+            (self.batch_size, 2),
+            msg="sample should return the number of samples requested",
+        )
+        np.testing.assert_allclose(
+            (batch2 > 0).all(axis=1).sum() / self.batch_size,
+            0.25,
+            atol=0.001,
+            err_msg="samples are not distributed uniformly across the extents",
+        )
+
+        self.assertFalse(
+            np.array_equal(batch1, batch2),
+            msg="Each call to sample should return different samples",
+        )
+
+    def test_sample_3d(self):
+        extents = np.zeros((3, 2))
+        extents[:, 1] = 1
+        self.sampler = HaltonSampler(extents)
+
+        batch1 = self.sampler.sample(self.batch_size)
+        self.assertEqual(
+            batch1.shape,
+            (self.batch_size, 3),
+            msg="sample should return the number of samples requested",
+        )
+        np.testing.assert_allclose(
+            (batch1 > 0.5)[:, :2].all(axis=1).sum() / self.batch_size,
+            0.25,
+            atol=0.005,
+            err_msg="samples are not distributed uniformly across the extents",
+        )
+        np.testing.assert_allclose(
+            (batch1 > 0.5).all(axis=1).sum() / self.batch_size,
+            0.125,
+            atol=0.005,
+            err_msg="samples are not distributed uniformly across the extents",
+        )
+
+        batch2 = self.sampler.sample(self.batch_size)
+        self.assertEqual(
+            batch2.shape,
+            (self.batch_size, 3),
+            msg="sample should return the number of samples requested",
+        )
+        np.testing.assert_allclose(
+            (batch2 > 0.5)[:, :2].all(axis=1).sum() / self.batch_size,
+            0.25,
+            atol=0.005,
+            err_msg="samples are not distributed uniformly across the extents",
+        )
+        np.testing.assert_allclose(
+            (batch2 > 0.5).all(axis=1).sum() / self.batch_size,
+            0.125,
+            atol=0.005,
+            err_msg="samples are not distributed uniformly across the extents",
+        )
+
+        self.assertFalse(
+            np.array_equal(batch1, batch2),
+            msg="Each call to sample should return different samples",
+        )
+
+    def test_sample_3d_scaled(self):
+        extents = np.zeros((3, 2))
+        extents[:, 1] = 10
+        self.sampler = HaltonSampler(extents)
+
+        batch1 = self.sampler.sample(self.batch_size)
+        self.assertEqual(
+            batch1.shape,
+            (self.batch_size, 3),
+            msg="sample should return the number of samples requested",
+        )
+        np.testing.assert_allclose(
+            (batch1 > 5)[:, :2].all(axis=1).sum() / self.batch_size,
+            0.25,
+            atol=0.005,
+            err_msg="samples are not distributed uniformly across the extents",
+        )
+        np.testing.assert_allclose(
+            (batch1 > 5).all(axis=1).sum() / self.batch_size,
+            0.125,
+            atol=0.005,
+            err_msg="samples are not distributed uniformly across the extents",
+        )
+
+        batch2 = self.sampler.sample(self.batch_size)
+        self.assertEqual(
+            batch2.shape,
+            (self.batch_size, 3),
+            msg="sample should return the number of samples requested",
+        )
+        np.testing.assert_allclose(
+            (batch2 > 5)[:, :2].all(axis=1).sum() / self.batch_size,
+            0.25,
+            atol=0.005,
+            err_msg="samples are not distributed uniformly across the extents",
+        )
+        np.testing.assert_allclose(
+            (batch2 > 5).all(axis=1).sum() / self.batch_size,
+            0.125,
+            atol=0.005,
+            err_msg="samples are not distributed uniformly across the extents",
+        )
+
+        self.assertFalse(
+            np.array_equal(batch1, batch2),
+            msg="Each call to sample should return different samples",
+        )
+
+
+class TestLatticeSampler(unittest.TestCase):
+    def setUp(self):
+        extents = np.zeros((2, 2))
+        extents[:, 1] = 1
+        self.sampler = LatticeSampler(extents)
+        self.batch_size = 9
+
+    def test_sample_2d(self):
+        samples = self.sampler.sample(self.batch_size)
+        self.assertEqual(
+            samples.shape,
+            (self.batch_size, 2),
+            msg="sample should return the number of samples requested",
+        )
+
+
+if __name__ == "__main__":
+    rosunit.unitrun("planning", "test_halton_sampler", TestHaltonSampler)
+    rosunit.unitrun("planning", "test_lattice_sampler", TestLatticeSampler)

planning/test/share/map1.txt

+1 1 1 1 1 1 1 1 1 1 1
+1 1 1 1 1 1 1 1 1 1 1
+1 1 0 0 1 0 0 1 1 1 1
+1 1 0 0 1 0 0 1 1 1 1
+1 1 0 1 1 1 0 1 1 1 1
+1 1 0 1 1 1 0 1 1 1 1
+1 1 0 0 0 0 0 1 1 1 1
+1 1 1 1 1 1 1 1 1 1 1
+1 1 1 1 1 1 1 1 1 1 1

planning/test/shortcut.py

+#!/usr/bin/env python
+
+import numpy as np
+import os
+import rosunit
+import unittest
+
+from planning import search
+from planning.samplers import HaltonSampler
+from planning.problems import R2Problem
+from planning.roadmap import Roadmap
+
+
+class TestShortcut(unittest.TestCase):
+    def setUp(self):
+        fname = os.path.join(os.path.dirname(__file__), "share", "map1.txt")
+        permissible_region = np.loadtxt(fname, dtype=bool)
+        self.problem = R2Problem(permissible_region, check_resolution=0.5)
+        self.sampler = HaltonSampler(self.problem.extents)
+
+    def test_shortcut(self):
+        num_vertices = 10
+        connection_radius = 5.0
+        lazy = False
+        rm = Roadmap(self.problem, self.sampler, num_vertices, connection_radius, lazy)
+
+        start_id = rm.add_node(np.array([1, 1]), is_start=True)
+        goal_id = rm.add_node(np.array([8, 8]), is_start=False)
+        correct_path = [start_id, 9, 2, 7, goal_id]
+
+        path, _ = search.astar(rm, start_id, goal_id)
+        np.testing.assert_almost_equal(
+            rm.compute_path_length(path),
+            rm.compute_path_length(correct_path),
+            err_msg="A* implementation is incorrect",
+        )
+        self.assertEqual(
+            path,
+            correct_path,
+            msg="A* implementation is incorrect",
+        )
+
+        shortcut_path = search.shortcut(rm, path)
+        self.assertLess(
+            rm.compute_path_length(shortcut_path),
+            rm.compute_path_length(path),
+            msg="Shortcut should return a shorter path",
+        )
+
+        correct_shortcut_path = [start_id, 2, goal_id]
+        np.testing.assert_almost_equal(
+            rm.compute_path_length(shortcut_path),
+            rm.compute_path_length(correct_shortcut_path),
+            err_msg="Shortcut implementation is incorrect",
+        )
+        self.assertEqual(
+            shortcut_path,
+            correct_shortcut_path,
+            msg="Shortcut implementation is incorrect",
+        )
+
+
+if __name__ == "__main__":
+    rosunit.unitrun("planning", "test_shortcut", TestShortcut)

planning/writeup/README.md

+# Project 4: Planning [![tests](../../../badges/submit-proj4/pipeline.svg)](../../../pipelines/submit-proj4/latest)
+
+Replace this with your own writeup! Please place all figures in this directory.
+