Implement a "go to goal" controller

[rjcausarano]

The docking/undocking algorithm requires a go-to-goal controller in order to work.
The controller should Ideally be a ros action that receives a coordinate in space and from that input calculates and sends velocity commands to move the robot to the desired Pose.

Take a look at: https://github.com/clebercoutof/turtlesim_cleaner/blob/master/src/gotogoal.py for an example of how to write this code.

[rjcausarano]

@lchico @eborghi10 @apojomovsky feel free to add anything I might be missing. here

[eborghi10]

We would like to have the action topic hidden because such a task does not exist in the real robot.
Besides that, the action should be cancellable just in case we want to do something with it.
Feedback should also be provided so the docking/undocking task can receive intermediate information in the case is needed.

Add a reference to the base code to avoid possible licenses issues.