Constrain distance from robot to path: helper function, Controller feedback, BT condition node, DWB/MPPI Controller Critics, Possible Costmap Layer Constraint for other controllers

[adivardi]

Feature request

Feature description

Implement a nav2_util helper function that conputes the distance from the robot's pose to a given path to compute tracking error.
The method should have an optional argument with the closest path point's index, if already known. If not given, the method should find that closest point itself. Once found, the method should interpolate the path's poses to find an accurate point to increase its resolution to increase the computed distance's accuracy (Steve edit: which can be obtained from the RPP controller's interpolation logic).

This method could be used in controller_server to provide additional feedback.
It could also be used in a BT condition node to check the path tracking accuracy.
It can also be used in plugins in DWB/MPPI to bound the divergence from the path if required given VDA5050 requirements or other path tracking requirements. It could be made as part of the FollowPath request to server's algorithms.

• Create utility with testing
• Expose in Controller Server for feedback on the task status Path distance feature #5387
• Create BT node to compute it and make decisions based off of it potentially Path trackin conditon node #5602
• Create plugins for MPPI/DWB to heavily penalize divergences beyond the requirement (possibly even a penalty proportional to the divergence?)
• Create costmap layer to use the max deviation to carve out a tunnel where the plans/controls may go through to stay in bounds Tracking error layer #5605
• Optional: Expose to the controller request API the path tracking error allowable rather than making it a static parameterization, so that different requests or areas can have different requirements. Not all plugins will use / respond to this (RPP just follows the path; graceful computes its own curves based on the path) but others may. The ones that wouldn't consider this would also largely be direct path following plugins anyway. We could create costmap layers that take in the requested bounds and put cost boundaries in the local costmap around the path at the extents to hard-prevent these algorithms from exceeding it (less fail).

Implementation considerations

Searching for the nearest point could have different startegies with different pros and cons:

• Choose the overall nearest point on the path (global minimum)
  • May jump forward/backward if path is looping
• Choose the first nearest point, starting from the start of the path (first local minimum)
  • May jump backward if path is looping
• Choose the first nearest point, starting from the point found in the last call, or a point given by the user (iterative local minimum)
  • More consistent
  • Require tracking of the last point found and resetting it when appropriate. Does not allow moving backwards on the path.

Related to

See discussion in #4931 (comment)

[SteveMacenski]

I did a couple of minor edits for clarity, but otherwise this sounds like an apt and useful feature description!

[dheerubhai-101]

Hi Adi and Steve, I'd like to try this. I'll get back to you after going through the PR #4931 (comment) with questions.

[SteveMacenski]

Great, thanks @dheerubhai-101! When do you expect you'd have time to complete it? 😄

[dheerubhai-101]

hey, so I jotted down few points while I was going through Adi's PR #4931 and nav2_util.

The simplest way would be finding the closest point index by iterating over path (as in controller_server.cpp) to find the error. Then as the closest pose is known from first call, we can interpolate the path using vector projection of the robot pose for closest point and the next point. There on, the error will be calcualted using interpolated point and we can switch points once we move past next_path_pose.

variable _iterative_closest_idx  
// first call
compute_distance(path, robot_pose):
	closest_pose_idx = find_closest_pose_idx(path, robot_pose)
	iterative_closest_idx -> closest_pose_idx
	tracking_error = euclidean_distance(robot_pose, path[closest_pose_dx])
	return tracking_error

// subsequent calls
compute_error(path, robot_pose, closest_pose_idx = _iterative_closest_idx):  
	cur_path_pose -> path.poses[closest_pose_idx]
        next_path_pose -> path.poses[closest_pose_idx + 1]

	// vector projection
	t = inner_product(robot_pose - cur_path_pose, next_path_pose - cur-path_pose)/ ||next_path_pose - cur-path_pose||^2
	if t > 1 or t < 0:
		cur_path_pose = next_path_pose
		next_path_pose = cur_path_pose.next()
	projected_position = calculate_vector_projection(robot_pose, cur_path_pose, next_path_pose)	
	tracking_error = euclidean_distance(robot_pose, projected_position)

        return tracking_error

But I am skeptical if I should assume that the _iterative_closest_idx will be the closest index as logically we are only traversing forward in the poses array after finding first closest point. (I just realized this was probably the same thing you mentioned in third point of implementations)

I could maybe

• find global minima efficiently. Could use Ball Tree + Nearest Neighbour Search
• find local minima in a fixed radius. Can use RPP interpolation logic this way
  (also realized you were talking of the same things in the first two points)

[SteveMacenski]

I think there are two situations

1. When there are no loops in the path and/or globally searching the path
2. When there can be loops in the path so we need to consider which path point to use

In some of the applications in Nav2, you'll see a 'forward search distance' parameter when we're working in incremental systems (i.e. path tracking) where we run often and can compare our last pose to our current pose. This lets us deal with (2) by searching only within a future looking window of the last pose based on what is kinematically possible within motion limitations. We can't move 100m in 100ms to be on the other side of a loop, but we could move 1m, for example.

When we're not in an incremental sub-system where we don't run often, we don't have that luxury since we don't have a baseline of where we previously were within a knowable time constraint. So, we have to search the path globally (I think) since we don't have anything more than the current snapshot in time. The only way to deal with the loops would be to have the path pruned from previous calls so that all old information is forgotten and we find the nearest path point. We can improve this by making the criteria for some much later index (i.e. idx=1000) override a more recent index (idx=12) being the closest point when the distance is significantly smaller, not just 1cm.

That's some context I think is good to know. So when building this feature, we want to be able to handle both cases. One where we are given a path and we know the last nearest pose and we want to use a search window and one where we do not and just do a snapshot based on the current path we have (assume pruned, might be good to have that in the doxygen entry).

I don't think closest_pose_idx and closest_pose_idx+1 are appropriate, we should be finding the nearest path point moving forward from the last (if the last is given) but doesn't mean that it can't be still the path point or that its limited only to its nearest single neighbor. You should search in a localized window configured by the user based on the robot's speed and what it can do in its loop.

If no pose is given, do a iteration on the path to find the nearest path point instead (maybe considering that idx=1000 feature I mentioned, but maybe that's good to add later after we get the core idea down).

From that nearest point, then do the interpolation for the nearest point on the path to do a point-to-point distance calculation or use the line segment from the path points to do a line-to-point distance calculation

[dheerubhai-101]

true that, I overlooked the fact that the next nearest neighour won't necessarily be closest_pose_idx + 1. I'll test a few things to understand about the path retrieved from global planner and get started with the implementation. I'll write to you if I get any doubts

[RamanRobotics]

I am looking forward to involve more into this issue.

[SteveMacenski]

After something like this is made, we can use it to also create critics in MPPI/DWB for bounding tracking error of the path for applications that have hard constraints for that

[dheerubhai-101]

Working on the first checkpoint, the utility. @RamanRobotics connected with me who wanted to hop in on this PR. This further segregation can allow him to contribute as well. I'll keep you posted.

[SteveMacenski]

Great! I look forward to it! :-)