clean the code and add dynamic reconfigure
add use of dynamic reconfigure add waitTransform

    WaitTranform is used to ensure that th trasfomation is possible
    -TODO put the function parameter in a launch file