Move from RCLPY to RCLCPP (ROS lib from Python to C++)

[edantec]

Hello,

Thanks to @MegMll, we now have benchmarks that show a performance increase up to 100x faster when using rclcpp (ros2 in c++) for sending eigen matrix over a topic, rather than using rclpy (ros2 for python).

(The benchmark in question https://github.com/MegMll/timings_ros2_conversion_msg)

Thus, we should move this part of the code to c++.

I think the interpolator node can be easily completely re-written in c++, but for the MPC node, if it is too annoying, we could just write a c++ lib that manage the ros messages and bind it to python...

Cheers