Dedupe shared code between AMCL node and NDT_AMCL node

[serraramiro1]

Feature description

While there're some details that cannot be shared, a lot of the node logic is shared and should be reused.