RotationShimController does not rotate when goal is closer than forward_sampling_distance

[gennartan]

Bug report

Required Info:

• Operating System: Ubuntu 20.04
• ROS2 Version: Iron
• Version or commit hash: ros-iron-nav2-rotation-shim-controller 1.2.6-1jammy.20240213.190815
• DDS implementation: /

Steps to reproduce issue

1- Send a goal to the controller closer than forward_sampling_distance
2- Done, the robot does not rotate towards the goal.

Expected behavior

If the path is smaller than forward_sampling_distance, the robot should rotate towards the last point of the path.

Actual behavior

If the new goal is not in front of the robot, the robot motion will be as described in nav2 documentation. The result is an awkward, stuttering, or whipping around behavior often resulting in a goal failure.

Additional information

• I also tried to reduce the forward_sampling_distance to the xy_goal_tolerance to ensure that the goal is validated when it is too close and avoid this akward behavior. This also failed because the requested orientation when starting navigation toward a new goal was too random.
• I am ready to solve this problem by creating a PR. But I first wanted to be sure that this could be a wanted behavior of this controller.

[SteveMacenski]

Thanks for the note! Can you highlight what you think we should do in this situation instead? If the goal is that close, it seems reasonable to me for the primarily controller to be immediately called instead of attempting to rotate. You could also decrease the forward sampling distance to be small, but you wouldn't want it to be super small (< 0.2m) else that angle may not represent the true intent of the path future (or if very very small, the atan2 function blowing up).

[gennartan]

I was thinking of something like this: [1]. We are already using this in my current project. Without it, there are some paths that systematically fails.

As I see it, using my commit [1], there are three cases:

• ´goal_dist < xy_goal_tolerance´: the goal will be validated by the goal checker if in the correct orientation. If not in the correct orientation, the robot will first rotate towards the goal, then rotate to the goal heading. This could be a bit akward since it would perform 2 different rotations. (Maybe later, a new feature could rotate the robot directly toward the goal heading when the goal is closer than the goal checker tolerance).
• ´xy_goal_tolerance < goal_dist < forward_sampling_distance´: this is the case that I am solving: the rotationShimController will make the robot rotate towards the goal. This should solve the akward behavior of the controller as described in the initial issue report.
• ´forward_sampling_distance < goal_dist´: normal use case.

The ´xy_goal_tolerance´ is another node's parameter, and would depend on the goal checker used, so it can not directly be used here. I used it to describe the expected behavior of a robot after my commit.

[1] gennartan@bf5c162

[SteveMacenski]

The commit you link to only changes an exception with 2 line additions and 5 deletions 🥲

´goal_dist < xy_goal_tolerance´: the goal will be validated by the goal checker if in the correct orientation. If not in the correct orientation, the robot will first rotate towards the goal, then rotate to the goal heading. This could be a bit akward since it would perform 2 different rotations. (Maybe later, a new feature could rotate the robot directly toward the goal heading when the goal is closer than the goal checker tolerance).

Why not have it just rotate to the goal in this case? If we're within tolerance, then simply rotating to the final heading would actually complete the goal!

´xy_goal_tolerance < goal_dist < forward_sampling_distance´: this is the case that I am solving: the rotationShimController will make the robot rotate towards the goal. This should solve the akward behavior of the controller as described in the initial issue report.

Keep me posted! I think this breakdown of the problem makes sense and I look forward to your contribution! Let me know if you have any more questions I can help answer 😄

[gennartan]

The commit you link to only changes an exception with 2 line additions and 5 deletions 🥲

Yes there was a small mistake in my commit. I forgot to add the ´- 1´ to access the last element of the vector. Here is the new commit [1].
The previous behavior was to throw an exception when the there was not point at a distance greater than ´forward_sampling_distance´. I just removed this exception and return the last point (the goal) of the path in this case.

´goal_dist < xy_goal_tolerance´: the goal will be validated by the goal checker if in the correct orientation. If not in the correct orientation, the robot will first rotate towards the goal, then rotate to the goal heading. This could be a bit akward since it would perform 2 different rotations. (Maybe later, a new feature could rotate the robot directly toward the goal heading when the goal is closer than the goal checker tolerance).

Why not have it just rotate to the goal in this case? If we're within tolerance, then simply rotating to the final heading would actually complete the goal!

Checking the condition if we're within tolerance might be more complex that it appear since the tolerance is defined within the goal checker node. I will take a look to this, but I don't think the solution will be so simple.

´xy_goal_tolerance < goal_dist < forward_sampling_distance´: this is the case that I am solving: the rotationShimController will make the robot rotate towards the goal. This should solve the akward behavior of the controller as described in the initial issue report.

Keep me posted! I think this breakdown of the problem makes sense and I look forward to your contribution! Let me know if you have any more questions I can help answer 😄

Thanks, I think I will create a PR for this modification. I will also take a look at the rotation issue when in the goal tolerances to rotate to the goal heading, but this will be for another PR / feature request.

[1] https://github.com/open-navigation/navigation2/commit/bb4d65e621bdfe576cf5ed9c399ec634f43fde78

[SteveMacenski]

Checking the condition if we're within tolerance might be more complex that it appear since the tolerance is defined within the goal checker node. I will take a look to this, but I don't think the solution will be so simple.

It should have access to the goal checker node, there is a method to obtain that tolerance information! We use that in the controllers commonly :-)

[gennartan]

I did not know about it ^^
I just opened a new feature request for it to avoid mixing the comments: https://github.com/open-navigation/navigation2/issues/4289

[SteveMacenski]

Will merge once CI turns over