tablebot: first pass at phase 3

[mikeferguson]

• update packet header to indicate type rather than relying on size
• add new packet type for segmented points to visualize goal
• add visualization of block and goal pose
• define state machine for phase 3
• goal detection works
• phase 3 works if the cube is relatively centered on the table - still need to adjust the "block pose" to make sure it works when cube is not centered