It works except for the tf (and vizualisation in rviz)