Mppi goal to critic

[redvinaa]

Reopen #4812 against main

[mergify]

@redvinaa, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

[SteveMacenski]

@redvinaa before you ask, no this CI failure is also not from you. Geometry2 just changed a bunch of headers from .h to .hpp and the Rolling release with those changes came out last night. #4732 resolves, but need to merge that in and you'll need to pull in those changes or rebase. Then any CI failures are real 😆

[SteveMacenski]

Rebase with #4823 for CI

[SteveMacenski]

This breaks a MPPI unit test - please make sure that this works properly. You may need to broadcast a transformation in that test so that the getTransformedGoal method can function. Likewise in your other PR.

Otherwise, LGTM

[mergify]

@redvinaa, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

[SteveMacenski]

Waiting on CI to turn over - sorry you've hit landmine after landmine in our usually very stable CI. I'm not sure why circle's not cloning but its not your fault. I'm asking our CI guru to investigate :-)

[mergify]

@redvinaa, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

[ruffsl]

Cloning git repository - [email protected]:ros-navigation/navigation2.git
Cloning into '.'...
[email protected]: Permission denied (publickey).
fatal: Could not read from remote repository.

Please make sure you have the correct access rights
and the repository exists.
error running git clone "[email protected]:ros-navigation/navigation2.git": exit status 128

Hmm, it failed to even clone the repo. That is strange...

Looks like the CircleCI Deploy key got deleted from GitHub for this GitHub repo.
Did anyone purge this repo's Deploy keys

• https://github.com/ros-navigation/navigation2/settings/keys

Seems to be cloning the repo just fine now after re-adding a new deploy key!

• https://app.circleci.com/pipelines/github/ros-navigation/navigation2/13343/workflows/4d64d956-d331-4d19-9704-675b22292a11/jobs/39973

[SteveMacenski]

@redvinaa you have a test failing:

/opt/overlay_ws/src/navigation2/nav2_mppi_controller/test/controller_state_transition_test.cpp:67
Expected: controller->computeVelocityCommands(pose, velocity, {}) doesn't throw an exception.
  Actual: it throws nav2_core::ControllerTFError with description "Unable to transform goal pose into costmap frame".

/opt/overlay_ws/src/navigation2/nav2_mppi_controller/test/controller_state_transition_test.cpp:67
Expected: controller->computeVelocityCommands(pose, velocity, {}) doesn't throw an exception.
  Actual: it throws nav2_core::ControllerTFError with description "Unable to transform goal pose into costmap frame".

[redvinaa]

I'm not sure what's the best way to go here, because the dummy path is generated with "odom" frame, while the costmap is in "map" frame. @SteveMacenski if you have a better solution than mine, do tell me.

The test now fails because of docking. :D Could you maybe rerun it? It passed on my machine on humble.

[SteveMacenski]

I'm not sure what's the best way to go here, because the dummy path is generated with "odom" frame, while the costmap is in "map" frame. @SteveMacenski if you have a better solution than mine, do tell me.

Fine by me - that test is a really basic exercising of the lifecycle and functions to make sure it can come up, do work, and shut down cleanly. Its not really doing any "work" in that test and each method have their own extensive test cases below.

The test now fails because of docking. :D Could you maybe rerun it? It passed on my machine on humble.

Something's going on with that test right now, not your fault. Merging! Thanks for the great contribution, as always 😄

[BriceRenaudeau]

This MR seems to break the enforce_path_inversion behavior.
The robot doesn't care for local goals anymore and is more attracted by the final goal.

Mppi_goal_to_critic_inversion_fail.webm

[SteveMacenski]

@BriceRenaudeau how so, specifically? I'm looking over the PR and I think you're referring to the change in the GoalCritic, but that shouldn't have been triggering before approaching the final goal anyway. Doesn't the PathFollow critic handle this situation OK?

Is this happening when the inversion is with in the threshold to consider for the other critics to pass off for on-goal-approach behavior?

The only 2 changes I see in this PR are:

• For all critics, use the global goal to evaluate bypass conditions like withinPositionGoalTolerance
• The GoalCritic uses the global goal to score (and also its bypass condition) rather than the end of the pruned path

Which of those (or both) are an issue?

[alex-roba]

@SteveMacenski

Which of those (or both) are an issue?

I am facing the same issue.
I believe the problem occurs because GoalCritic scores based on the global goal (and its bypass condition) rather than the end of the pruned path. In some cases, inversion can be close to the goal where GoalCritic is active, causing the controller to ignore the inversion and focus only on the final goal pose.

Additionally, with the default parameters, when the robot is outside the GoalCritic threshold, the parameters may not be sufficient to guide the robot toward inversion. However, when GoalCritic considers the pruned path, it helps the robot converge within the inversion tolerances.

[BriceRenaudeau]

In our case, the GoalCritic is always active to perform TEB like behavior hence cutting the inversion point.

[BriceRenaudeau]

Here is another example of the problem. We want the robot in the opposite direction in a narrow aisle, it should go out of the aisle to make the half turn and then come into the aisle again.

MPPI_inversion_point_ok.webm

But with this MR, the robot totally bypasses the path and just moves to the endpoint in the wrong direction.

MPPI_inversion_point_failed.webm

[SteveMacenski]

@BriceRenaudeau this keeps falling off of my radar by accident. Can you file a ticket for this with your context and we can take it from there? There are some Nav2 contributors around that are looking for projects and if we provide a detailed explanation of the issue and possible solutions, I'm sure we can find a contributor that would be willing to work on this to fix the issue.

I think possibly we should use the 'real' goal unless there's a path inversion and we are caring about the path inversions. If so, use the intermediary goal. We can add in the parameter similar to that we have for the path follower if we should consider inversions / path pose orientations.

Does that seem reasonable?

My apologies for the delay