BT XML Idea: Stop replanning when near the goal if path is valid

[SteveMacenski]

When using feasible planners, it might not be sensible to replan when things are clear within X meters of the goal. It can actually be detrimental to do so if the finite change in path tracking error makes exact goals not possible once within < 1m to the goal result in some "looping" behaviors. This is totally unnecessary to achieve the goal successfully given the positional and rotational tolerances that the exact planners don't know about. Or due to correctable finite error in state estimation.

So, it seems very reasonable to use the goal distance BT node to check if we're with x meters of the goal and if so, select not to replan if the path continues to be totally valid (e.g. no occopancy). This could be added as either a new BT or modify existing BTs to take advantage of this knowledge

[VArt3m]

That's a very sensible thing to do. Was thinking about doing something like that to prevent loops when near goal myself - it may be possible to use GoalReached BT node with high (1-3m) threshold for that purpose. Although I suspect that having a "path length" condition would be more correct - but there's no node for that yet.

P.S. I find it a bit strange that GoalReached BT node doesn't leverage goal checker plugins from Controller

[SteveMacenski]

@shannonwarren Lets discuss here:

Making it based on a timer seems excessive, we could simply add a control flow node under the current rate timer which uses a condition node isGoalReached -- or more accurately would be to make a new one using the nav2_util for getting the integrated path distance.

If we're within the distance to the goal spatially or by integrated path length, then we can use isPathValid to check if the path is collision free. If its in collision, then replan. If not, pass on replanning.

We should be able to do this with no changes to existing BT nodes and possibly just creating 1 new BT node which just wraps an existing nav2_util function. Then its just organizing those stages using the basic control flow nodes

[shannonwarren]

Sounds good. I'll work on it.

[padhupradheep]

Did something similar for another project..

however, when the robot is in the goal radius with an obstacle, I just wait and not go for replanning..

project_fox.mp4

[mini-1235]

Sharing a little bit about our use case:

We are using RPP as our controller now, and we are hoping that once the XY tolerance reached, it can rotate the yaw only, which should be this part of code in RPP

navigation2/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp

Line 239 in f10aa77

if (shouldRotateToGoalHeading(carrot_pose)) {

However, the controller might receive a new plan when it is purely rotating the yaw, which resets the xy goal tolerance

navigation2/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp

Lines 480 to 484 in f10aa77

	void RegulatedPurePursuitController::setPlan(const nav_msgs::msg::Path & path)
	{
	has_reached_xy_tolerance_ = false;
	path_handler_->setPlan(path);
	}

this is causing the rpp controller to switch to the third condition

navigation2/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp

Line 246 in f10aa77

} else {

causing unstable behavior for my robot

I implement a goalnearby node by utilizing this part of code

navigation2/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp

Lines 149 to 171 in f10aa77

	// Get current path points
	nav_msgs::msg::Path current_path;
	auto res = blackboard->get(path_blackboard_id_, current_path);
	if (res && current_path.poses.size() > 0u) {
	// Find the closest pose to current pose on global path
	auto find_closest_pose_idx =
	[&current_pose, &current_path]() {
	size_t closest_pose_idx = 0;
	double curr_min_dist = std::numeric_limits<double>::max();
	for (size_t curr_idx = 0; curr_idx < current_path.poses.size(); ++curr_idx) {
	double curr_dist = nav2_util::geometry_utils::euclidean_distance(
	current_pose, current_path.poses[curr_idx]);
	if (curr_dist < curr_min_dist) {
	curr_min_dist = curr_dist;
	closest_pose_idx = curr_idx;
	}
	}
	return closest_pose_idx;
	};
	// Calculate distance on the path
	double distance_remaining =
	nav2_util::geometry_utils::calculate_path_length(current_path, find_closest_pose_idx());

and check if the path is valid or not. Finally, it replans or does not replan based on these conditions

[SteveMacenski]

Might be a nice standalone BT node if you wanted to contribute that back into the main Nav2 BT library. The only thing to be aware of is that if the path looks over itself, it might pick the wrong closest point from the start or end of the loop depending on which is minutely closer to the robot's pose. This can be a pain in some cases but broadly speaking still useful.

[mini-1235]

The only thing to be aware of is that if the path looks over itself, it might pick the wrong closest point from the start or end of the loop depending on which is minutely closer to the robot's pose.

Are you referring to NavigateThroughPoses? I haven't really played with that, I will check it and try to come up with a solution that can function correctly in all cases if possible

[SteveMacenski]

We have some designs for that in Nav2 in various places (though not in all... which is something we should fix). For cases in which we call the function regularly (i.e. controller server at a fixed frequency, not planner server since that could be called infrequently or irregularly), we can:

• Use the first path point to start
• Have a parameter for forward search distance to check, which can be set as maximum speed * update rate + some healthy margin (or 1/2 costmap size as a good default). This is set as the maximum distance the robot can possibly move in an iteration.
• Check from the start -> that distance for the nearest point, only
• Return the value
• Next iteration start with this point to seed the problem. This could be done by storing and setting the last-closest IDX on the path, or by pruning the path behind that pose at the end of the method so that we initialize each at the first point of the path.

This seems to work well. Open to other ideas, but this is what is 'proven' at least, for the case in which we update regularly to know what a maximum forward distance should be to check. If we don't know that, we may still need to check the full path, but maybe have a prune-passed-points feature to remove former loops from the equation. Deciding on future loops is a little more involved, also considering paths that may be near each other and not necessarily looping (like U turns)

[mini-1235]

I think this is also related to the distance_remaining feedback in NavToPose and NavThroughPose

navigation2/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp

Lines 153 to 174 in 332bcab

	// Get current path points
	nav_msgs::msg::Path current_path;
	res = blackboard->get(path_blackboard_id_, current_path);
	if (res && current_path.poses.size() > 0u) {
	// Find the closest pose to current pose on global path
	auto find_closest_pose_idx =
	[&current_pose, &current_path]() {
	size_t closest_pose_idx = 0;
	double curr_min_dist = std::numeric_limits<double>::max();
	for (size_t curr_idx = 0; curr_idx < current_path.poses.size(); ++curr_idx) {
	double curr_dist = nav2_util::geometry_utils::euclidean_distance(
	current_pose, current_path.poses[curr_idx]);
	if (curr_dist < curr_min_dist) {
	curr_min_dist = curr_dist;
	closest_pose_idx = curr_idx;
	}
	}
	return closest_pose_idx;
	};
	// Calculate distance on the path
	double distance_remaining =

Do you think they both need to be updated?

[SteveMacenski]

Ideally speaking, yes. If we can actually abstract this process into a library in nav2_utils that would be ideal so we can just use it across the board more easily rather than redoing it in multiple places. The API might differ a little bit in a few places, I'm not sure the full list of locations where we try to do something like this. I'm guessing at least in the controller's path handlers / handling processes and in the BT Navigator's feedback. The Navigators are another good place for the procedure I outlined since that also runs at a regular frequency.

I don't think Nav To Pose needs it as much, but I figure it doesn't hurt if we apply it to both Navigators if they share an implementation in a utils file.