add tf lookup for set_point listener

[incebellipipo]

No description provided.

[incebellipipo]

Currently, this creates problem for global trajectory tracking tracking. This issue blocks localization issue on ALPHA AUV.

[incebellipipo]

the issue is this

• [problem definition] path_tracking behavior transforms the points to controllers' frame_id. But it does it only once. When drift happens it doesn't recognizes the drift.
• [solution] path_tracking behavior should transform the process_values to its own frame to keep track of the heading.
  • If it publishing waypoints on map, it should transform the vehicle position to map: cg_link to map.
  • It should compute the heading and distance according to this new points, and it should transform it back to cg_link.
  • It should do so on every iteration.
    at the same time,
• MVP Control should be able to accept control inputs on different frames.