Fix tf_listener to report right angles
Get wall thickening to work
Fix wall thickening and path finding (kind of)