Add support for switching between SMAC planners

[mini-1235]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	Fixes #5118
Primary OS tested on	Ubuntu
Robotic platform tested on	Tb4
Does this PR contain AI generated software?	No
Was this PR description generated by AI software?	Out of respect for maintainers, AI for human-to-human communications are banned

---

Description of contribution in a few bullet points

Description of documentation updates required from your changes

Description of how this change was tested

---

Future work that may be required in bullet points

For Maintainers:

• Check that any new parameters added are updated in docs.nav2.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
• Should this be backported to current distributions? If so, tag with backport-*.

[mini-1235]

Opening as draft for visibility, @SteveMacenski can you do a high level review here? I will fix the doxygen, formatting issue later on

For reference:

Before:

Read data
---------  -----------------------  -------------------  ------------------  --------
Planner    Average path length (m)  Average Time (s)     Average cost        Max cost
GridBased  48.675026690642426       0.06390911739999999  1.3501362760185982  65.7
---------  -----------------------  -------------------  ------------------  --------

After:

Read data
---------  -----------------------  ----------------  ------------------  --------
Planner    Average path length (m)  Average Time (s)  Average cost        Max cost
GridBased  49.20803095324081        0.11183508235     1.2601202876943096  64.24
---------  -----------------------  ----------------  ------------------  --------

Switching between SMAC hybrid (dubins) and SMAC hybrid (reeds shepp):

Jan.6.2026.Video.mp4

[codecov]

Codecov Report

❌ Patch coverage is 91.74603% with 26 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
nav2_smac_planner/src/distance_heuristic.cpp	86.66%	12 Missing ⚠️
nav2_smac_planner/src/node_lattice.cpp	89.74%	4 Missing ⚠️
nav2_smac_planner/src/a_star.cpp	86.95%	3 Missing ⚠️
nav2_smac_planner/src/analytic_expansion.cpp	88.23%	2 Missing ⚠️
nav2_smac_planner/src/node_hybrid.cpp	90.90%	2 Missing ⚠️
nav2_smac_planner/src/obstacle_heuristic.cpp	97.75%	2 Missing ⚠️
...2_smac_planner/include/nav2_smac_planner/types.hpp	80.00%	1 Missing ⚠️

Files with missing lines	Coverage Δ
..._smac_planner/include/nav2_smac_planner/a_star.hpp	75.00% <ø> (ø)
...r/include/nav2_smac_planner/analytic_expansion.hpp	81.81% <ø> (ø)
...r/include/nav2_smac_planner/distance_heuristic.hpp	100.00% <100.00%> (ø)
...planner/include/nav2_smac_planner/goal_manager.hpp	100.00% <100.00%> (ø)
...smac_planner/include/nav2_smac_planner/node_2d.hpp	100.00% <100.00%> (ø)
..._planner/include/nav2_smac_planner/node_hybrid.hpp	100.00% <100.00%> (+4.34%)	⬆️
...planner/include/nav2_smac_planner/node_lattice.hpp	100.00% <100.00%> (ø)
...r/include/nav2_smac_planner/obstacle_heuristic.hpp	100.00% <100.00%> (ø)
nav2_smac_planner/src/node_2d.cpp	98.21% <100.00%> (+0.03%)	⬆️
nav2_smac_planner/src/smac_planner_hybrid.cpp	90.08% <ø> (ø)
... and 9 more

... and 40 files with indirect coverage changes

🚀 New features to boost your workflow:

• ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

[mini-1235]

Hmmm, I reran the planner benchmarks today and don't see any performance difference. I suspect the earlier result was simply because I forgot to rebuild after removing cout 😮‍💨

Before:

SMAC 2D

100by100_15.yaml

---------  -----------------------  -------------------  ------------------  --------
Planner    Average path length (m)  Average Time (s)     Average cost        Max cost
GridBased  52.17770760819163        0.07869108617000001  3.6076562008930044  58.09
---------  -----------------------  -------------------  ------------------  --------

100by100_20.yaml

---------  -----------------------  -------------------  -----------------  --------
Planner    Average path length (m)  Average Time (s)     Average cost       Max cost
GridBased  48.98086498596548        0.06538113713999999  4.591232153301037  70.21
---------  -----------------------  -------------------  -----------------  --------

SMAC Hybrid

100by100_15.yaml

---------  -----------------------  -------------------  ------------------  --------
Planner    Average path length (m)  Average Time (s)     Average cost        Max cost
GridBased  51.91414253615302        0.05742170818000001  1.3208241326823664  58.19
---------  -----------------------  -------------------  ------------------  --------

100by100_20.yaml

---------  -----------------------  ----------------  ------------------  --------
Planner    Average path length (m)  Average Time (s)  Average cost        Max cost
GridBased  49.20803095324081        0.05250086263     1.2601202876943096  64.24
---------  -----------------------  ----------------  ------------------  --------

SMAC Lattice

100by100_15.yaml

---------  -----------------------  ----------------  ------------------  --------
Planner    Average path length (m)  Average Time (s)  Average cost        Max cost
GridBased  52.910467731112014       0.07542792343     1.5345268730643076  70.38
---------  -----------------------  ----------------  ------------------  --------

100by100_20.yaml

---------  -----------------------  ----------------  ------------------  --------
Planner    Average path length (m)  Average Time (s)  Average cost        Max cost
GridBased  49.68671584359208        0.07771957899     1.7684700761580858  75.39
---------  -----------------------  ----------------  ------------------  --------

After this PR:

SMAC 2D

100by100_15.yaml

---------  -----------------------  -------------------  ------------------  --------
Planner    Average path length (m)  Average Time (s)     Average cost        Max cost
GridBased  52.17770760819163        0.08154277580999998  3.6076562008930044  58.09
---------  -----------------------  -------------------  ------------------  --------

100by100_20.yaml

---------  -----------------------  ----------------  -----------------  --------
Planner    Average path length (m)  Average Time (s)  Average cost       Max cost
GridBased  48.98086498596548        0.06843810643     4.591232153301037  70.21
---------  -----------------------  ----------------  -----------------  --------

SMAC Hybrid

100by100_15.yaml

---------  -----------------------  --------------------  ------------------  --------
Planner    Average path length (m)  Average Time (s)      Average cost        Max cost
GridBased  51.91414253615302        0.056133314749999996  1.3208241326823664  58.19
---------  -----------------------  --------------------  ------------------  --------

100by100_20.yaml

---------  -----------------------  -------------------  ------------------  --------
Planner    Average path length (m)  Average Time (s)     Average cost        Max cost
GridBased  49.20803095324081        0.05457305396000001  1.2601202876943096  64.24
---------  -----------------------  -------------------  ------------------  --------

SMAC Lattice

100by100_15.yaml

---------  -----------------------  -------------------  ------------------  --------
Planner    Average path length (m)  Average Time (s)     Average cost        Max cost
GridBased  52.910467731112014       0.07674453998000001  1.5345268730643076  70.38
---------  -----------------------  -------------------  ------------------  --------

100by100_20.yaml

---------  -----------------------  -------------------  ------------------  --------
Planner    Average path length (m)  Average Time (s)     Average cost        Max cost
GridBased  49.68671584359208        0.07932330702000001  1.7684700761580858  75.39
---------  -----------------------  -------------------  ------------------  --------

I also loop this line over 1000 times and the execution time is 0 ms, so I think removing static is fine here

navigation2/nav2_smac_planner/src/smoother.cpp

Line 256 in f2dde95

static ompl::base::ScopedState<> from(state_space_), to(state_space_), s(state_space_);

[SteveMacenski]

This PR is one where I'd love to give you more specific feedback, but unfortunately it is quite a bit involved that I know any specifics I would give you, you'd find a reason why it wasn't possible. This is one of those things you just roll your sleeves up and work it out.

What I would like to see is that the encapsulation I fought so much more in the Smac Planner remains in place. My original thinking (which may or may not be possible) was that we created a grouping of the NodeT static members "Context" within the Node templates. Then we initialize it in the Planner plugins so that the planner plugins host the context. When we replan, we check if the context exists in NodeT and only create it if it does not exist or change it to the hosted-context if the current context is different (maybe have a randomly generated ID in it?). Or I suppose if the static member in NodeT is a pointer, then we can just always assign it since its not involving a copy.

That's not perfect, I know. But, I think it would reduce the complexity of what's going on here and keep the internal details of each node outside of the A* algorithm.

---

Another direction is to try using -fvisibility=hidden. That may solve the issue without any changes by making the symbols not exported to the dynamic symbols table. If the pluginlib architects were forward thinking, they may have made it so that the main class having public visbility regardless. If not, we may be able to set the public visibility of a class (?) https://stackoverflow.com/questions/3570355/c-fvisibility-hidden-fvisibility-inlines-hidden though I'm not familiar with it.

[mini-1235]

Another direction is to try using -fvisibility=hidden. That may solve the issue without any changes by making the symbols not exported to the dynamic symbols table. If the pluginlib architects were forward thinking, they may have made it so that the main class having public visbility regardless. If not, we may be able to set the public visibility of a class (?) https://stackoverflow.com/questions/3570355/c-fvisibility-hidden-fvisibility-inlines-hidden though I'm not familiar with it.

I spent some time trying this today, but I ended up encountering a lot of linking errors. Unless I am missing something, this will likely more changes

What I would like to see is that the encapsulation I fought so much more in the Smac Planner remains in place

I was moving the two functions to astar for two main reasons:

• I want to keep the other classes read only
• NodeHybrid and NodeLattice share the same implementation

but I understand your point. We could instead allow write access to the other classes and introduce a common base class, then have the NodeContext in both classes inherit from it, is this something you would accept?

My original thinking (which may or may not be possible) was that we created a grouping of the NodeT static members "Context" within the Node templates. Then we initialize it in the Planner plugins so that the planner plugins host the context. When we replan, we check if the context exists in NodeT and only create it if it does not exist or change it to the hosted-context if the current context is different (maybe have a randomly generated ID in it?)

When doing this, I am trying to get rid of every static member and function, to avoid running into any pluginlib issues that are hard to debug. I think your solution will work as well, if you don't really like my solution above, I can give it a try. Do you have a strong preference between the two?

[SteveMacenski]

I spent some time trying this today, but I ended up encountering a lot of linking errors. Unless I am missing something, this will likely more changes

Unsurprising, I think that would be something that would take some trial and error to figure out (or finally figure out its simply not possible).

We could instead allow write access to the other classes and introduce a common base class, then have the NodeContext in both classes inherit from it, is this something you would accept?

Mhm, interesting. Sure, that would work. What methods would that implement though? Just holding the context or also some of the common A*-based calls? This sounds like a similar solution to having a static context object in each NodeT. Understanding more of your design intent would be good before jumping into that in earnest if you wanted to go this way over the others.

I think your solution will work as well, if you don't really like my solution above, I can give it a try

Of the current method (not the 2 above) and my solution, I would prefer my solution. It has less moving parts / easier to review / doesn't bleed concepts from the NodeT's into the A*.

[mini-1235]

Mhm, interesting. Sure, that would work. What methods would that implement though? Just holding the context or also some of the common A*-based calls? This sounds like a similar solution to having a static context object in each NodeT. Understanding more of your design intent would be good before jumping into that in earnest if you wanted to go this way over the others.

I think we can have a new class like NodeSE2:

NodeSE2 {

    LookupTable obstacle_heuristic_lookup_table;
    ObstacleHeuristicQueue obstacle_heuristic_queue;

    std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros;
    // Dubin / Reeds-Shepp lookup and size for dereferencing
    LookupTable dist_heuristic_lookup_table;
    float size_lookup;
    
    float getObstacleHeuristic(
    const Coordinates & node_coords,
    const Coordinates & goal_coords,
    const double & cost_penalty,
    const bool use_quadratic_cost_penalty,
    const bool downsample_obstacle_heuristic);
    
    void resetObstacleHeuristic(
    std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros,
    const unsigned int & start_x, const unsigned int & start_y,
    const unsigned int & goal_x, const unsigned int & goal_y,
    const bool downsample_obstacle_heuristic)
    
    inline uint64_t getIndex(
    const unsigned int & x, const unsigned int & y, const unsigned int & angle);
}

then in node_hybrid.hpp and node_lattice.hpp, we inherit from this class:

struct NodeContext: public NodeSE2

Perhaps we can also move NodeHybrid::Coordinates to NodeSE2. What do you think?

[SteveMacenski]

My head's not in this problem in the same way as yours working on it -- how is NodeContext then used by NodeHybrid and NodeLattice? Also, wouldn't that involve a copy for each node instance unless NodeContext or NodeSE2 was static composed as a member?

I don't think this handles the static motion_table either.

Minor bit: I'd rename this if not dealing with SE2-nature things. This is just the obstacle / distance heuristic bits.

Perhaps we can also move NodeHybrid::Coordinates to NodeSE2. What do you think?

I'd keep the context completely generic, so I'd avoid that. There are some other NodeT's I have on my mind which I would want to use this with that would have different Coordinates / GetIndex implementations.

[SteveMacenski]

Otherwise, I have no more notes - doing perf testing is the last thing

[mini-1235]

For correctness and performance:

Before:

SMAC 2D

100by100_15.yaml

---------  -----------------------  -------------------  ------------------  --------
Planner    Average path length (m)  Average Time (s)     Average cost        Max cost
GridBased  52.17770760819163        0.07869108617000001  3.6076562008930044  58.09
---------  -----------------------  -------------------  ------------------  --------

100by100_20.yaml

---------  -----------------------  -------------------  -----------------  --------
Planner    Average path length (m)  Average Time (s)     Average cost       Max cost
GridBased  48.98086498596548        0.06538113713999999  4.591232153301037  70.21
---------  -----------------------  -------------------  -----------------  --------

SMAC Hybrid

100by100_15.yaml

---------  -----------------------  -------------------  ------------------  --------
Planner    Average path length (m)  Average Time (s)     Average cost        Max cost
GridBased  51.91414253615302        0.05742170818000001  1.3208241326823664  58.19
---------  -----------------------  -------------------  ------------------  --------

100by100_20.yaml

---------  -----------------------  ----------------  ------------------  --------
Planner    Average path length (m)  Average Time (s)  Average cost        Max cost
GridBased  49.20803095324081        0.05250086263     1.2601202876943096  64.24
---------  -----------------------  ----------------  ------------------  --------

SMAC Lattice

100by100_15.yaml

---------  -----------------------  ----------------  ------------------  --------
Planner    Average path length (m)  Average Time (s)  Average cost        Max cost
GridBased  52.910467731112014       0.07542792343     1.5345268730643076  70.38
---------  -----------------------  ----------------  ------------------  --------

100by100_20.yaml

---------  -----------------------  ----------------  ------------------  --------
Planner    Average path length (m)  Average Time (s)  Average cost        Max cost
GridBased  49.68671584359208        0.07771957899     1.7684700761580858  75.39
---------  -----------------------  ----------------  ------------------  --------

After this PR:

SMAC 2D

100by100_15.yaml

---------  -----------------------  -------------------  ------------------  --------
Planner    Average path length (m)  Average Time (s)     Average cost        Max cost
GridBased  52.17770760819163        0.08154277580999998  3.6076562008930044  58.09
---------  -----------------------  -------------------  ------------------  --------

100by100_20.yaml

---------  -----------------------  ----------------  -----------------  --------
Planner    Average path length (m)  Average Time (s)  Average cost       Max cost
GridBased  48.98086498596548        0.06843810643     4.591232153301037  70.21
---------  -----------------------  ----------------  -----------------  --------

SMAC Hybrid

100by100_15.yaml

---------  -----------------------  --------------------  ------------------  --------
Planner    Average path length (m)  Average Time (s)      Average cost        Max cost
GridBased  51.91414253615302        0.056133314749999996  1.3208241326823664  58.19
---------  -----------------------  --------------------  ------------------  --------

100by100_20.yaml

---------  -----------------------  -------------------  ------------------  --------
Planner    Average path length (m)  Average Time (s)     Average cost        Max cost
GridBased  49.20803095324081        0.05457305396000001  1.2601202876943096  64.24
---------  -----------------------  -------------------  ------------------  --------

SMAC Lattice

100by100_15.yaml

---------  -----------------------  -------------------  ------------------  --------
Planner    Average path length (m)  Average Time (s)     Average cost        Max cost
GridBased  52.910467731112014       0.07674453998000001  1.5345268730643076  70.38
---------  -----------------------  -------------------  ------------------  --------

100by100_20.yaml

---------  -----------------------  -------------------  ------------------  --------
Planner    Average path length (m)  Average Time (s)     Average cost        Max cost
GridBased  49.68671584359208        0.07932330702000001  1.7684700761580858  75.39
---------  -----------------------  -------------------  ------------------  --------

I also added more tests to get > 90% in code coverage today

One last thing from my side: would you like to rename "context" in this PR? Do you think something like NodeResources would be clearer?

[mini-1235]

Also moving to final review, I think this is close to merge

[SteveMacenski]

LGTM

On the metrics, if you run them again for the "after" do they get more in line with the "before"? I see its a little bit slower, but I'm really thinking its probably just variance of running and wanted to just sanity check that you don't see a meaningful change if run a few times (and maybe averaging them out). The ~2-5% difference here isn't nothing, but could also just be from a single run.

[SteveMacenski]

For all: initialize to nullptr. Could save us some hassle for later changes that create bugs with garbage data.

[mini-1235]

Yes, I reran both "before" and "after" a few more times, and the results look quite similar overall. Sometimes the "before" run is faster, sometimes the "after" run is faster

[SteveMacenski]

Anything else you think before merging?

[mini-1235]

Nothing from my side. But let me know if you would like to rename "NodeContext"

[SteveMacenski]

We can leave it. Can you open a migration guide PR mentioning this fix? Its not a ‘migration’ task but its a good FYI this is fixed

[mini-1235]

We can leave it. Can you open a migration guide PR mentioning this fix? Its not a ‘migration’ task but its a good FYI this is fixed

Ok will do