Remove pviz dependency
Remove normalize_angle, change debug lines to adapt to 6DoF