Smoother error codes

[jwallace42]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#3043
Primary OS tested on	Ubuntu
Robotic platform tested on	None

---

Description of contribution in a few bullet points

• added error codes to smoother server
• added error codes to smoother bt action node
• added tests smoother error codes

Description of documentation updates required from your changes

---

Future work that may be required in bullet points

For Maintainers:

• Check that any new parameters added are updated in navigation.ros.org
• Check that any significant change is added to the migration guide
• Check that any new features OR changes to existing behaviors are reflected in the tuning guide
• Check that any new functions have Doxygen added
• Check that any new features have test coverage
• Check that any new plugins is added to the plugins page
• If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

[mergify]

@jwallace42, please properly fill in PR template in the future. @SteveMacenski, use this instead.

• Check that any new parameters added are updated in navigation.ros.org
• Check that any significant change is added to the migration guide
• Check that any new features OR changes to existing behaviors are reflected in the tuning guide
• Check that any new functions have Doxygen added
• Check that any new features have test coverage
• Check that any new plugins is added to the plugins page
• If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

[jwallace42]

Hey Steve!

From reading the server this weekend it seems like we still would like the server to succeed if it times out. (I.e partial smoothing has occurred) and just set a blackboard var to let anything else know that the smoothing was not completed. If this is the intention, I don't think timeout is a necessary error code since we handle it in the server.

My current implementation is the rough structure assuming that timeout is indeed a error. If not it can be easily changed.

[SteveMacenski]

On PTO this week, but I took a quick glance on the plane and this looks good to me assuming CI turns over and the details align.

Only potential question I have right now is the error code setting for BT navigator / documentation updates. Also missing TF and invalid inputs (path invalid or empty) error code

[SteveMacenski]

CI shows some linting issues + comments on missing stuff above stands. Otherwise LGTM now in front of a computer

[jwallace42]

This is looking like a Wednesday completion :). I got class tomorrow.

[SteveMacenski]

error code setting for BT navigator / documentation updates.

missing TF and invalid inputs (path invalid or empty) error code

Still valid :-)

[jwallace42]

missing TF and invalid inputs (path invalid or empty) error code

It appears that the current set of smoothers and the smoother server do not transform anything. So I think we can ignore the TF error. I will add the path invalid error in :).

[SteveMacenski]

It appears that the current set of smoothers and the smoother server do not transform anything. So I think we can ignore the TF error. I will add the path invalid error in :).

OK! You do have some linting errors now

[SteveMacenski]

Last little bits and can be merged in this week!

Make sure to update the docs PR and if there's anything I asked for there to merge in

[SteveMacenski]

You have some linting issues in CI

[SteveMacenski]

Rerunning CI since something odd happened but I don't think its due to this work. I think this is good to go now, anything else to add before merging? The docs are the only thing that come to mind, though I see in my notifications tray you updated something there I'll look at shortly.

WRT #3043 the last bit are some documentation to make it clear and loud to users that there are now error codes that can be used + update the behavior trees to potentially use them (or at least 1-2 trees as demos). I could see a few easy ways to add support in the BTs for the planner/controller so that we only attempt to execute contextual recoveries if they'd actually help (e.g. if robot is out of bounds, clearing a costmap won't help).

W.r.t. documentation, that PR will have things like adding general documentation support for it, but I think there's a few other places it might make sense to add it (e.g. in the Behavior Tree's section itself) and potentially a new page (?) since this is a big and new feature that is not the kind of thing that you'd run into in the code easily. I want to make it clear and available on a first glance that this exists. Maybe even edits to the Nav2 landing page or navigation concepts page to expose it early.

Then we (or you 😉 ) can announce on discourse!

[jwallace42]

I could see a few easy ways to add support in the BTs for the planner/controller so that we only attempt to execute contextual recoveries if they'd actually help (e.g. if robot is out of bounds, clearing a costmap won't help).

Yeah, I think making sure we only execute the costmap clearing in the correct context for the navigate to pose and navigate through poses would be a good first update for the behavior trees.

As for docs.

• navigation concepts

All these depend on the updates to the behavior trees. Maybe once I get update these pages.

• detailed behavior tree walk though
• navigate to pose
• navigate though pose

Maybe there is others?

I will open up a separate pr for the these doc updates at some point next week once the pipe error codes doc pr gets merged.

[SteveMacenski]

Sounds good! I think the only thing blocking this being merged is the issues with the documentation PR.

Next steps seem like: updating BTs to use this feature + more in documentation

• https://navigation.ros.org/ list in bullets of features on main page + description in paragraph 2 when we talk about action servers
• https://navigation.ros.org/concepts/index.html when we talk about task servers
• https://navigation.ros.org/tuning/index.html perhaps an entry in the tuning guide around using these to communicate contextual failures for people looking to design behavior trees
• https://navigation.ros.org/behavior_trees/index.html Something in the BT section, naturally

Other than the BT section, the other 3 bullets look like: 1 bullet, 2 sentences, and a paragraph, so not too bad at all. At that point, I think this is ready for you to announce with your BT demos -- this is a big feature to bridge the last gap between the BT based Nav2 and the FSM based move_base_flex.

[rayferric]

With those changes, simple smoother aborts navigation when the robot approaches the end of path.

This is the cause:

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

I can't seem to find a way to handle this error in any online resources.

nav2 params:

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:
    # The maximum number of iterations the smoother has to smooth the path, to bound potential computation.
    max_its: 1000
    # Whether to smooth the smoothed path recursively to refine the quality further
    do_refinement: True
    # Weight for smoother to apply to smooth out the data points
    w_smooth: 0.3
    # Weight for smoother to apply to retain original data information
    w_data: 0.2
    # Parameter tolerance change amount to terminate smoothing session
    tolerance: 1e-10

bt:

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