Commit 8c304ac2 by Brian Hou

### 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!