El threshold no funcionava. Provo de fer una funcio que calculi el minim intentant evitar els valors <= 0.005

separat missatge lasers en 2 i afegit debug stream per veure possibles espuris[1] dels lasers. [1] Pregunteu a l'angel si no enteneu el significat

- optimitzada funcio de calcul maxim vel
- manicura de codi (usar _ enlloc de this-> ...)
- Testejat amb simulacio teo_sim

 - added parameters for variables
 - if a scan is not received, outputs an error (but keeps working)