Simple Smoother Aborts Navigation with "No Segments Smoothed" Error When Approaching End of Path

[rayferric]

Bug report

Required Info:

• Operating System:
  • Ubuntu 24.04.1 LTS
• ROS2 Version:
  • Jazzy
• Version or commit hash:
  • Jazzy Binaries from 6 Oct 2024 with this PR
• DDS implementation:
  • Cyclone DDS

Steps to reproduce issue

1. Use the following nav2 parameters:

   smoother_server:
   ros__parameters:
     costmap_topic: global_costmap/costmap_raw
     footprint_topic: global_costmap/published_footprint
     robot_base_frame: base_footprint
     transform_timeout: 0.1
     smoother_plugins: ["simple_smoother"]
     simple_smoother:
       max_its: 1000
       do_refinement: True
       w_smooth: 0.3
       w_data: 0.2
       tolerance: 1e-10

2. Set up the behavior tree as follows:

   <Sequence name="ComputeAndSmoothPath">
     <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
     <SmoothPath unsmoothed_path="{path}" smoothed_path="{smooth_path}"/>
   </Sequence>

3. Execute navigation. The robot will traverse the path and then start approaching the end of it.

Expected behavior

The robot smoothly navigates the entire path without aborting, and SmoothPath completes as expected.

Actual behavior

When the robot approaches the end of the path, the simple_smoother aborts the navigation with the following error:

if (segments_smoothed == 0) {
  throw nav2_core::FailedToSmoothPath("No segments were smoothed");
}

Additional information

Logs:

[smoother_server]: Received a path to smooth.
[smoother_server]: No segments were smoothed
[smoother_server]: [smooth_path] [ActionServer] Aborting handle.
[global_costmap.global_costmap]: Received request to clear entirely the global_costmap
[local_costmap.local_costmap]: Received request to clear entirely the local_costmap

...repeating endlessly

The exception is thrown even though the smoother could simply return a jagged path. The above exception leads to the whole navigation process being aborted. On the internet, there doesn't seem to be available much information on how to properly setup the SmoothPath node or handle BehaviorTree errors.

[SteveMacenski]

I'm trying to figure out why it is that you had zero segments, it should work with a single point and I don't see any logging for why the smoothImpl returned false. The 2 places that can happen, only one wouldn't log at the INFO level or higher:

navigation2/nav2_smoother/src/simple_smoother.cpp

Lines 184 to 191 in 4e62a89

	if (cost > nav2_costmap_2d::MAX_NON_OBSTACLE && cost != nav2_costmap_2d::NO_INFORMATION) {
	RCLCPP_DEBUG(
	rclcpp::get_logger("SmacPlannerSmoother"),
	"Smoothing process resulted in an infeasible collision. "
	"Returning the last path before the infeasibility was introduced.");
	path = last_path;
	updateApproximatePathOrientations(path, reversing_segment);
	return false;

and that exit condition implies a collision with the path's poses. The other would log something we would see.

I would say we could change that to a return true;, but it could actually still be in collision if it failed on the first iteration implying that the path itself was in collision to begin with. We also return false in the case of iterations exceeded with the path-before-last, so for consistency we should keep the collision case the same. Unless, of course, we change both to true. Though in that case we're returning success in the cases which the path smoothing did not full converge which doesn't feel right to me.

This leads me to a few motivational questions

The exception is thrown even though the smoother could simply return a jagged path

Is that expected through for a "successful" return case? I would think not. I would think it would be better in the BT XML to decide what to do with a failed path smoothing iteration rather than having failures masked at the server level. Perhaps we could update the BT Node for the Smoother to have a parameter for returning the unaltered path if the path smoothing process fails and still have the BT node return success -- which would allow the server to return FAIL but allow your robot behavior use the unsmoothed path without issue.

Alternatively, it brings up the question: if we back out to the last valid path, isn't that OK to use if not converged?

And I don't have a clear answer to that off the top of my head. If we exceed the number of iterations, that's not converged fully, so that feels wrong to say "done successfully". Similarly, if we stop convergence due to collision, it also seems to me that this shouldn't be considered "succesful", even if partially smoothed. But I could also see where some users (like yourself) may want that.

I think this also could be a parameterization for user selection of behavior they'd want. If fail to converge due to constraints, but did some smoothing, whether to consider that valid or invalid.

So for a solution, we could make this a parameter either and/or in both of the BT Node for the smoother for handling at the BT XML level or within the plugin as an algorithm level paramaterization.

What do you think?