The idea was that the controller must be ease to program, for that I used Arduino (full program available in the files section), so here are an explanation of the parts of the code that I considered more useful to understand and, of course, a general review of all the code.

The first part are de libraries used:

By default ros in the ros serial examples the TX/RX in the IC handle the nodes connections, but is possible to change that defining a new class and using it in the ros::NodeHandle declaration:

The function that fill the variables with data is:

The last part is:

So that's all, if you have any question just ask in the comments.