MPPI feedback for progress on global path

[sebatztian]

Feature request

Have option for MPPI to publish the idx of the current reference position along the global path.

Feature description

For cases, where the MPPI is employed as a local planner that follows a static global path, the information about the progress can be vital for other nodes and behavior, yet is not provided by the mppi controller.

Providing the information within the CriticData to be used within cost critic plugins, by adding e.g. current_idx and goal_idx (or path_length) would allow for creation of flexible new critics that can consider the global frame, like the one discussed in #5484 within the plugin structure at low additional cost.

Implementation considerations

I assume the function that provides the pruned path needs to keep track of the position within the complete path.

[tonynajjar]

For cases, where the MPPI is employed as a local planner that follows a static global path, the information about the progress can be vital for other nodes and behavior, yet is not provided by the mppi controller.

out of curiosity, can you cite an example how you would use that info for external nodes?

[sebatztian]

• on the assumption that the given path is unchanged throughout execution, we want to provide feedback on the global progress. Currently this is done by an external node estimating the reference position in a similar fashion but duplicates functionality that is done by the mppi anyways.
• Second we want to adjust behavior of non-navigation related components without cutting the path into pieces, so we don't lose obstacle avoidance capabilities in the transition points.

[SteveMacenski]

Is the feedback provided by the navigators insufficient to estimate progress?

https://github.com/ros-navigation/navigation2/blob/main/nav2_msgs/action/NavigateToPose.action#L19-L23

[sebatztian]

Thank you for the input.

As I understand it, the NavigateToPose feedback is given by the BT Navigator, and also estimated based on the current closest point. This makes sense in a point to point navigation, that most likely also updates its global plans repeatedly.

In our case, we have a static precomputed coverage path, that may overlap itself. We are giving this directly to the controller while a custom BT handles the overall task.

So, we are not using the BT Navigator itself, and even then they would in my opinion potentially provide wrong information, like in the illustrated case:

The only other option I see is that an external process keeps track of the progress along the path in parallel to the controller itself, where similarly a new position along the path can only be a certain distance away from the previous position.
However, I feel like this duplicates work that is internally already done by the controller and could potentially lead to differing assumptions.
As stated, I think the most clean approach would be if the controller publishes its internal state - given your input with the NavigateToPose action, potentially even as feedback of the FollowPath action, if we think this globally.

I would love to get your feedback on it.

[SteveMacenski]

There's a ticket open to address that issue #5037 since I'd like to solve that issue globally. Some places like in MPPI do so rather intelligently, but others do not. The BT Navigator is one place that it does not, but really only affects a marginally small number of applications using NavigateThroughPoses (where looping overitself is involved) or as you have shown with CoverageNavigation whereas the robot is not tuned to accurately follow the path.

I think the right answer is to get all that together in the ticket. That would provide the shared tooling that can be used across the controllers, BT Navigators, and Behavior Tree Nodes to compute this value. There's been some starting work there, but I'm working on some quality control issues with that contributor (that perhaps could be accelerated by a more experienced engineer taking on the project). Then, you could use the BT Node or the tools outright to compute the value in the manner you find best.

We could have the feedback updated to include the current path IDX, but would mean that we'd have to (a) move the computation bit into the Controller Server or (b) make a new controller API for it to return what it thinks is the current IDX. (a) makes the most sense to me, and actually there's some work on that direction in #5446. This moves the Path Handler into the Controller Server as a shared resource that can do all the fancy pruning and estimation in one place.

Funny enough, your request stumbles into a couple of areas of redundant current issues/work. If you had interest in accelerating that, I'm sure both contributors would be interested in some assistance!

P.S. For coverage navigation, wouldn't you prefer the controller to be better tuned so this isn't an issue?

[sebatztian]

Thanks for the answer! I will look into the issues you linked.

What I wanted to show in the example is that cases can appear where the robot deliberately sways away from the path to avoid an obstacle on the route.
I assume that the MPPI gives the robot a significant avoidance capability, so that the coverage plan does not have to be adjusted for minor obstacles. Hence we change from a strict path following behavior to a more free approach (which can be done quite nicely in my opinion via the max_path_occupancy_ratio of the path align critic).

Would you suggest a fundamentally different approach?

I too believe that this issue goes hand in hand with a more sophisticated goal checker, since the available plugins do not cater for cases, where the robot may drive over its final goal position.

[SteveMacenski]

Usually full coverage applications (that I’ve seen) want more strict adherence to have promises of coverage completion. If you don’t need that, then this seems good! Let me know what you think after reviewing the linked items.

[sebatztian]

I looked into your suggested topics. Here are my 2 cents on the two matters:

Regarding Issue #5037, I can grasp what a potential util function would look like, and how it can be used within the controller server to give the feedback I want. I outlined my train of thought for the first step here, although there are some open design decisions unclear to me.

I'm also not sure about the current state of the PR, since I couldn't find anything from the original editors, and the latter seems to try to implement a lot at once. Could this also just be implemented without the BT node?

Assumptions of MPPI handling:

• MPPI path_handler prunes its internal global_plan by removing everything before the new closest idx >> Previous idx in the next step is always at 0
• Path consists of dense points >> it suffices to find the closest point, not closest line element between points

What needs to be done for the desired functionality of #5037 and the additional progress feedback:

• Utility function that finds the index of the closest point to the robots current position.
  • Arguments: Path, (optional) prev_idx, closest_pose_upper_bound (with default value)
  • If prev_idx is given, the search space of the path should be pruned according to closest_pose_upper_bound
    • Design decision: enable moving backwards on global path by considering points before prev_idx?
    • idx of path needs to be found that reaches an accumulated distance of closest_pose_upper_bound
    • Design decision: additionally return distance_to_path (sign indicating left / right) I.e. tracking error?
    • Design decision: if interpolating between points, should function return position of closest interpolated point on path and idx of point on path?
  • If no prev_idx given, the function should find the nearest point on the path (global path)
    • First local minimum does not make any sense to me(?)
    • --> Maybe this function would be a bit overloaded and should / could be split into two.
  • Add current_idx_ to Controller Server in order to track the last known reference point on path
    • Reset the index, when a new path is received.
  • Have Controller Server publish the progress along the global path as feedback
    • Design decision: what variables are actually returned? (current_idx, total_path_points, remaining_path_length, tracking_error)
  • (Optionally) Write a critic that uses the new utility function to penalize divergence of the track above a certain threshold
    • Design decision: hard border constraint when above the threshold (similar to a fixed width lane) or increasing with larger distance ?
    • As suggested this could also be solved by a new costmap layer.. Probably only one of the two needed

Regarding PR #5446, I think it makes a lot of sense to move the path_handler into the controller server itself. I assume that it would be best to wait for this to be merged, to then build up feedback functionality from there or how would you proceed in this?

I am new to contributing to the project, so it is a) unclear to me how far along #5446 is, and b) how I could provide meaningful aid to either of the topics.

If you have some guidance how I could accelerate the relevant topics, I would love to hear it.

[SteveMacenski]

I'm also not sure about the current state of the PR

Personally, same 😆 its on my list for later today to reengage. I've been going back and forth with that author to ensure quality and some process, but if you wanted to jump in, I'm sure they wouldn't mind the hand (and I'd appreciate another set of eyes / hands). Its definitely still a work in progress as you can see from my review comments.

Could this also just be implemented without the BT node?

To start, yes. I think it is definitely useful as a BT node though and if we have the util, then the BT node is a trivial implementation.

Regarding PR #5446, I think it makes a lot of sense to move the path_handler into the controller server itself. I assume that it would be best to wait for this to be merged, to then build up feedback functionality from there or how would you proceed in this?

Perhaps, yes. It could be done simultaneously though if you needed them imminently so that it is not blocked. These are inherently related parts of the codebase, but also different features. That PR could be updated if this is completed first. If you could wait, then I'd maybe recommend forking that branch and working off of that instead to be merged in afterwards.

I am new to contributing to the project, so it is a) unclear to me how far along #5446 is, and b) how I could provide meaningful aid to either of the topics.

See #5446 (comment). I think its in a holding pattern for another PR (which I will also be reengaging with today).

If you have some guidance how I could accelerate the relevant topics, I would love to hear it.

Reviewing PRs and/or picking up where authors' left off if they're not responsive (I'm not sure that applies here though?) are all great ways to accelerate this. I am one person and can only get to as many PRs/issues as I can review in 3-4 hours daily - which means I don't always get to everything every day with the recent pace of the project. Having more reviews or 'just doing it' are great ways.

---

These are all excellent, thoughtful questions.

Design decision: enable moving backwards on global path by considering points before prev_idx?

Not something we do at the moment. We do allow reverse movements, but that's reversing in the context of the global plan having as part of it a reversing segment. So that would still be moving 'forward' along the path, even if via reverse motions. That is an interesting point to consider however, I suppose we could have the sliding window also include reversing if the robot actually moves backward or localization is updated. That should be an easy feature to add either a lower bound and/or just apply the negative of the upper bound (bounded at 0).

Design decision: additionally return distance_to_path (sign indicating left / right) I.e. tracking error?

Good thought. Also a good separate utility to have access to for other tracking error computations. I'd implement that as a separate function once the IDX on the path is found.

Design decision: if interpolating between points, should function return position of closest interpolated point on path and idx of point on path?

For the distance from the path to the robot pose for tracking error, the interpolated point's distance. For the IDX, the actual IDX. We have a nice controller_utils feature that will interpolate path points along a distance that might be useful.

Add current_idx_ to Controller Server in order to track the last known reference point on path

I think the path handler, but agreed.

Have Controller Server publish the progress along the global path as feedback ... current_idx, total_path_points, remaining_path_length, tracking_error

All are good candidates. I don't think total_path_points thought since I think current_idx would cover that. The remaining path length could be directly used by the Navigator's feedback as well if the piping made sense. remaining_path_length is essentially a replacement to the current distance_to_goal or could be reused.