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?

[rayferric]

I completely agree that smoother plugins should throw errors when they can't fully converge. It's no problem for me to add custom error handling in the BT. Like you suggested, we could also introduce a parameter in the smoother plugin (e.g. convergence_failure) to disable this error. That would be useful for users who want to set up a smoother without digging into behavior trees.

I assume that the reason I did not see the "infeasible collision" message is that my logging level was set to the default, so DEBUG logs weren't shown.

As to why segments_smoothed was zero, I believe this is due to the check here:

navigation2/nav2_smoother/src/simple_smoother.cpp

Line 78 in 6311978

if (path_segments[i].end - path_segments[i].start > 9) {

This would prevent smoothing if the last (and only) path segment has fewer than 9 vertices which might be the case during the final approach.

I'd be happy to help with the development of those features.

[SteveMacenski]

OK, great! What do you think is best? I'm thinking handling within the Smoother BT Node would be best. Have a param for use_unsmoothed_path_on_failure (or... something less verbose) which if it fails, just uses the unsmoothed path instead (or the partially smoothed path?).

[rayferric]

Okay, so I can see that SmoothPath BT node already implements an error_code_id parameter, which I assume can be used to handle the failure? Though I'm not nearly enough experienced with BTs to know if that's the case so further modification may be needed.

And yeah, I think such a parameter in SimpleSmoother plugin would be quite helpful. The parameter could be named something like allow_partial_smoothing or fallback_to_unsmoothed for clarity.

As for whether to use the unsmoothed or partially smoothed path, I think it would be useful to have an option for both to give a bit more freedom to the User.

Actually, we could create a single parameter that allows users to choose between using the unsmoothed path, the partially smoothed path, or treating the smoothing as failed. This could be something like convergence_failure_result with 3 possible values such as UNSMOOTHED, PARTIAL, or FAIL.

[SteveMacenski]

Sorry for the delay. Its a difficult time of year with conferences and travel.

SmoothPath BT node already implements an error_code_id parameter, which I assume can be used to handle the failure?

Yeah, if the only times in the algorithms those fail are places we want to skip, then we could check the error codes and make a decision then about if we either (a) use the unsmoothed, input path or (b) use the semi-smoothed, output path. Total failures we should use (a), failures that are partially valuable we can use (b).

If we're using the error codes, then we could do this in the BT node itself. I think its best to keep the Task Servers as application-independent as possible and the BT node should contain all the info we require.

This could be something like convergence_failure_result with 3 possible values such as UNSMOOTHED, PARTIAL, or FAIL.

Indeed!

So, it seems that we landed one:

• a parameter to return SUCCESS on the BT node if the action return a failure, such that we use some path rather than failing
• Use the error code IDs for the failure to indicate if we use the original, unsmoothed path or the semi-smoothed path
• OR have the parameter be a 3-way that indicates if we should fail/succeed unsmoothed/succeed semismoothed.

I'd argue that the context of the error code IDs should be enough for us to tell if we should use the unsmoothed or semismoothed if the user indicates a "yes". But, that's worth looking at the IDs and verifying. If we can't, then your suggestion is best :-)

[rayferric]

Thanks for the follow up!

Based on the agreed development approach, I'll be working on implementing this feature in the coming days.

Since I'm not very experienced with behavior trees, it might take me a bit longer than usual. I'll keep you posted and mention you here once I make some progress.

[SteveMacenski]

Great! I look forward to the contribution!