Following server

[ajtudela]

I've opened this draft so you can take a look at the code and open a discussion to add changes, improvemnet, etc. Later, I'll create a new PR with all the changes.

The following server is currently working, as seen in this demo video I made: https://youtu.be/YaQQm7aWb-Y

I'm working on the following new features:

• Add the local costmap so the robot can avoid obstacles (this would be an optional parameter). In some clutter spaces, like our lab, sometimes the controller generates a trajectory directly to the tables 😅. I'm thinking that part of this behavior to avoid collisions and the trajectory visualizer can be shared from the graceful controller: by adding it to the smooth control law or share the already implemented class to avoid collisions from the controller. That way we can share code and reduce errors.
• Implement some kind of recovery mecanism for when the robot loses sight of the object to follow.

As the following and docking server share a large part of the code. I'll try to extract the code into an ApproachServer class (I'm really bad with the names :/) so both server can inherit from it.

[SteveMacenski]

Something with the smoothness would be great too! I think the thread in ros-navigation/navigation2#4115 covers that intention a bit on the controller side. Otherwise, I think your other descriptions make sense. I actually filed a ticket when migrating this over to Nav2 (ros-navigation/navigation2#4404) on the collision checking bits, I agree that it would also be applicable here too!

This generally looks good! I think there's thing we should do to reduce code duplication across the different servers, but that's something once we have a working final prototype so we know what we need to abstract out or see if its sensible to have some common base class or something similar

[ajtudela]

Great. I'll keep going then.

Because the docking is ported to nav2, should I make the PR here, merge here and then port it to nav2 or make it directly to nav2?

[SteveMacenski]

Probably just leave it here for the moment. The Nav2 version has some refactoring that we can wait to address until we're done with a POC