def motion_planning(self, world_state):
"""
Calculates a trajectory for driving from the current position to the goal contained in world_state['goal']. The
result is placed into world_state['trajectory']
:param world_state: Outputs of perception and behavior planning
"""
if world_state['flail']:
world_state['trajectory'] = None
world_state['grid'] = self.obstacle_grid
return
# Call A* to generate a path to goal
trajectory = None
if world_state['goal'] is not None:
start = world_state['pose'][0]
goal = world_state['goal']
trajectory = geom.a_star(self.obstacle_grid, self.obstacle_probability_threshold, start, goal)
if trajectory:
trajectory = geom.smooth_trajectory(trajectory, self.obstacle_grid, self.obstacle_probability_threshold)
world_state['trajectory'] = trajectory
world_state['grid'] = self.obstacle_grid
def test_a_star_succeeds_when_all_cells_unoccupied(self):
result = geom.a_star(self.occupancy_grid, 1.0, self.start, self.goal)
expected = 4
actual = len(result)
self.assertEqual(expected, actual)
def test_a_star_fails_when_goal_unreachable(self):
self.occupancy_grid.occupancy[2, 3] = 1
self.occupancy_grid.occupancy[2, 2] = 1
self.occupancy_grid.occupancy[3, 2] = 1
result = geom.a_star(self.occupancy_grid, 1.0, self.start, self.goal)
self.assertIsNone(result)
def test_a_star_fails_when_goal_occluded(self):
goal_node = self.occupancy_grid.get_cell(self.goal)
self.occupancy_grid.occupancy[goal_node.indices] = 1
result = geom.a_star(self.occupancy_grid, 1.0, self.start, self.goal)
self.assertIsNone(result)