Fixed angles work

- Used ros angles library to compute angles
- square recognition improved a lot by this, still some errors...