Changed the code to be working for one encoder for the qp.
Ros bags with the acutal QP running on two cars. Small changes to the acutal code