Behavior
Since the Chiara has only one camera, and two shifted images are needed to do stereo depth mapping, we needed to implement a behavior to take these images and do the depth mapping. This behavior is implemented as a fairly standard Tekkotsu state machine. The robot is assumed to be in the left position when the behavior is started. It takes an image, walks to the right (while facing the same direction), and takes a second image.
When taking images, the Chiara lies down on its belly in order to stabilize the image and ensure that the images are taken at the same height.
Once the Chiara has taken the two images, it creates a mask based on selected colors in the lefthand image, then passes this mask and the two images to the depth-mapping algorithm. Currently, the behavior is complete once the depth mapping finishes and the results are displayed in the sketch GUI, but in the future, behaviors could make use of this depth map for many different purpose
A video of this behavior working and the source code are available in the files section.