Lab 10

1. First you will implement A* path planning and validate your implementation in the simulated map environment. Be sure to pull the latest lab code from Github. For this lab you are provided the following files:

• planning.py – File in which you will implement A*.
• grid.py – Defines the CozGrid class, which represents an 8-connected discretized grid and holds information about start, goal, and obstacle locations as well as visited locations and the current path. Also specifies the size of a grid square, in mm.
• visualizer.py – A visualizer to help debug your algorithms. Displays start location (green), goals (blue), obstacles (dark gray), visited locations (light gray), and the current path (red arrows). The other files are set up to launch this automatically.
• map1.json – Map definition file containing test scenario for A*.
• autograder.py – This autograder is provided to help you verify your A* solution.
• testcase1.json – Autograder file corresponding to map1.json

You can test your A* implementation by running the autograder with:

$ python3 autograder.py testcase1.json 

2. Next you will integrate your path planner with Cozmo to make it safely navigate to a goal using its odometry. Specifically, your goal is to enable Cozmo to navigate to face Cube 1 from a predefined direction, while also avoiding Cubes 2 and 3 if they are present. You will implement the following functionality:

• Step 1: Update Map: Update the map to correctly represent all cubes detected by the robot.
• Step 2: Navigate to Goal: Successfully navigate the robot from a known start location to Cube 1 and stop within 10cm of Cube 1.
• Step 3: Cube Orientation: Extend your navigation code to consider which side of the cube the robot is approaching. Have the robot approach the side of the cube that looks like Figure 1. The orientation of the cube can be accessed with cube.pose.rotation.angle_z. The robot should stop within 10cm of the cube, facing toward the correct side of the cube but without touching it.
• Step 4: Obstacle Avoidance: Extend your code to have Cozmo successfully avoid coming into contact Cubes 2 and 3 if they are in the space.

The robot will be given its starting pose and the goal pose. However, it will not know the pose of any cubes. You will use the robot's vision system to find the cubes and add them to the map. Cube 1 will be in the arena from the beginning. If your robot does not see Cube 1 from its start position, have it navigate to the center of the grid and turn in place until the cube is seen. Cubes 2 and 3 may be added to the arena at any time. Your code must support continuous detection, map updates, and replanning. Grid coordinates are formatted as (x, y), with the origin in the lower left corner, x increasing to the right, and y increasing to the top. Cozmo will start facing the +x direction, as shown in Figure 2.

For this part of the lab you will work with the following files:

• planning.py – This file already has your A* implementation from Part 1. Here you will also implement the four functionalities described above. Executing this file starts your Cozmo code and a visualizer.
• emptygrid.json – Map file defining only start location.

To test your Cozmo code run:

$ python3 planning.py emptygrid.json

Some examples of the intended robot behavior are shown in Figure 3.