Open rayferric opened 2 weeks ago
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: https://github.com/ros-navigation/navigation2/blob/4e62a89d01aeb3cc88562a559cd142688305272b/nav2_smoother/src/simple_smoother.cpp#L184-L191 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?
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:
https://github.com/ros-navigation/navigation2/blob/6311978e0902b807a05883b3764e72f731d446f8/nav2_smoother/src/simple_smoother.cpp#L78
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.
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?).
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
.
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:
SUCCESS
on the BT node if the action return a failure, such that we use some path rather than failing 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 :-)
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.
Great! I look forward to the contribution!
Bug report
Required Info:
Steps to reproduce issue
Use the following
nav2
parameters:Set up the behavior tree as follows:
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:Additional information
Logs:
...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.