Allow reversing feature in RPP fails due to negative value of a variable

[Naraharirahul]

Hi, With respect to my previous issue #2580 I have been trying to test the reversing feature in my simulation and I found a issue that it fails to reverse when a goal pose was given (behind the robot with same heading angle) but can move forward without any error.

While debugging the code of regulated_pure_pursuit_controller.cpp on the galactic branch, I saw that the projection_time variable at the time of calculation is getting divided by the linear_velocity which would be negative if the goal location was behind and allow_reversing is set as true.

https://github.com/ros-planning/navigation2/blob/ba9851280f747434f143762c6211d5eb8a34e9b3/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp#L398

This made the variable projection_time a negative value and not only that it caused the while loop not to break at all.
The if condition should break the while loop but as the variable i is being incremented, the max_time_to_collision is a positive value (lets say 1.0) and the projection_time is a negative value, it is not entering the if condition.

https://github.com/ros-planning/navigation2/blob/ba9851280f747434f143762c6211d5eb8a34e9b3/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp#L406-L430

I solved the issue by adding std::abs() to the linear_vel in the following line and after that it has started to reverse.
Let me know if this is the right way of resolving this issue and if not, what should be the best way of dealing this?

https://github.com/ros-planning/navigation2/blob/ba9851280f747434f143762c6211d5eb8a34e9b3/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp#L398

[SteveMacenski]

I have been trying to test the reversing feature in my simulation and I found a issue that it fails to reverse when a goal pose was given (behind the robot with same heading angle) but can move forward without any error.

The changing in direction is caused by cusps in the path requiring a change in direction. Simply because your goal is behind you does not mean the robot will reverse to achieve it. The path planned has to indicate to the controller that a change in direction is necessary. That is done via a cusp which is the most clear and fundamental description of a hard change in continuity.

There's an argument to be made for the initial conditions on the path being reverse instead of forward though. Whereas a path isn't starting forward, but starting reverse, so there isn't a cusp for changing the direction, we're simply starting in the reverse direction. We assume all paths are starting forward. I'm not sure off the top of my head how to accomplish that though, but that's a valid point. I'd be open to a proposal!

I solved the issue by adding std::abs() to the linear_vel in the following line and after that it has started to reverse.

This is a good point - a PR to add that would be appreciated! Please also include 2 gifs or videos in the PR: one before showing that the collision detection published vector is going forward when it should be going backward, and another where this is fixed.

[padhupradheep]

Somehow we both agreed to remove fabs, but I do not remember the exact details why that was the case.

Somehow I missed this part in the final test before the PR I guess!

It's actually a good find @Naraharirahul

I did the check and it works, anyways, I'll do the PR, if @Naraharirahul is busy with something else.

[Naraharirahul]

Hi @padhupradheep , I'll try to get this done by tonight, if not I will let you know.