There is sample code in ContainerTest.fsm that shows how to extract custom markers from the world map. Project components:

1. Determine the container size and pose. Once two markers on adjacent sides have been detected, we can calculate the size and pose of the container. Each marker's orientation (marker.theta) defines a plane, and the intersection of that plane with the floor is a line. Finding the intersection points of the two lines gives one corner of the container. Finding the distances of the markers from that corner gives the half-length of each side.

2. Develop a representation for containers. Define a new class ContainerObj in worldmap.py to represent the container. Write an update_container function to update the container's representation in the world map; you can model this after the other update functions in worldmap.py.

3. Flesh out a container's representation. If we only see one custom marker we can get an idea of where the container is located, but we don't know its width and depth. Develop a strategy to find the dimensions of the container by systematically moving the robot to locate a second custom marker.

4. Navigate to the container. In order to drop a cube into the container we need to bring the robot to the container's edge, at an orientation perpendicular to the edge. This is similar to the problem of docking with a cube, which is already solved in the Pilot. However, for long containers there is not a single docking point on each side; there is a range of feasible docking points. So if an obstacle blocks access to one portion of the container it may be possible to find an alternate docking point on the same side.

5. Drop the cube in the container. Two approaches should be considered. One is to use the lip of the container to lever the cube off of the lift hooks as the lift is lowered. Another would be to use a rapid lift movement to toss the cube into the container. Experiment with both methods to see which works best.

6. Multiple containers. We can have up to four distinct containers in the environment by using a different set of custom markers for each one. (The SDK contains support for 16 custom markers.)

• Sample code in projects/container

• Custom markers

• pickup.fsm in cozmo-tools/cozmo_fsm contains the GoToCube class that can be used as a starting point for this project.