ROS2 client for navigation2

[eliasdc]

Implemented feature

ROS 2 navigation stack client #84

Implementation description

The implementation replicates the behavior of the ROS1 free fleet client targeting the main branch, as the develop branch is still under active development.

We could not wait for the refactoring results of develop, but I'll be glad to refactor this code once it's done.

[eliasdc]

Thanks for the review. For the request_path it would be better to change it to navigate_through_poses instead of navigate_to_pose. I can update and test this code with it if you want. I think it would make more sense as now the robot stops on each waypoint.

[mxgrey]

I'm not familiar with Nav2 so I apologize if this comment is noise, but one thing to watch out for if you use a navigate_through_poses command instead of a navigate_to_pose command is that the time field of the command from RMF is important for avoiding traffic conflicts with other robots. If a navigate_through_poses command is issued and that command doesn't prevent the robot from passing a waypoint before the prescribed time, then the robot may drive itself into conflicts because it was supposed to wait for another robot to pass.

We have some improvements in development that will make the commander smart enough to know information like "I can safely have the robot move through some [ x, y, z ] sequence of waypoints with any timing and no risk of conflicts happening", but the current system doesn't have that intelligence yet, so it's sensitive to the timing.

[eliasdc]

I'm not familiar with Nav2 so I apologize if this comment is noise, but one thing to watch out for if you use a navigate_through_poses command instead of a navigate_to_pose command is that the time field of the command from RMF is important for avoiding traffic conflicts with other robots. If a navigate_through_poses command is issued and that command doesn't prevent the robot from passing a waypoint before the prescribed time, then the robot may drive itself into conflicts because it was supposed to wait for another robot to pass.

That's a good point. With navigate_through_poses I can't indeed handle timings of each waypoint. It would immediately see the end goal as a target. Maybe canceling an active path could work if the robot is reaching it's target to soon but that is less clean.

We have some improvements in development that will make the commander smart enough to know information like "I can safely have the robot move through some [ x, y, z ] sequence of waypoints with any timing and no risk of conflicts happening", but the current system doesn't have that intelligence yet, so it's sensitive to the timing.

Conclusion: I'll wait until these new features are included before changing the behavior.

[aaronchongth]

Thanks for getting back so quickly, @eliasdc! That certainly sounds like it will make navigation through a path much smoother! However as @mxgrey pointed out, the time at which the robot is expected to arrive at the pose or wait on the pose is critical to work with RMF, hence it might not be a good idea.

I see that NavigateThroughPoses have feedback for number of waypoints remaining, you are right and we could have a workaround by pausing and resuming the robot. But that would make path planning a little more expensive as it needs to plan to the end goal every time it arrives at the waypoint.

Again thanks for the help so far, I will merge this, and I'll get some documentation and simulation examples in.

[mxgrey]

I think it'll be great to switch to navigate_through_poses once the next iteration of the planner is finished. Then we can issue all the poses at once, up to the next mandatory waiting event.