[DRAFT] New geomteric planner package for viapoint based spline generators

[sbadamikar-research]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	(add tickets here #1)
Primary OS tested on	(Ubuntu, MacOS, Windows)
Robotic platform tested on	(Steve's Robot, gazebo simulation of Tally, hardware turtlebot)
Does this PR contain AI generated software?	(No; Yes and it is marked inline in the code)
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-*.

[SteveMacenski]

I think there are some test suites for exercising the planners, can you add your new planners to these?

[SteveMacenski]

Update the copyright please

[SteveMacenski]

OK - before I give this a look over, can you update the description to point to the ticket & add some consequential documentation about what this does, what algorithms you chose to implement, trade offs, and use-case intentions?

This is all kind of out of context for not just myself but future readers. This is alot of code to parse without context of understanding which you chose, why, why not others, performance, and use cases.

--- stderr: nav2_geometric_planners           
In file included from /opt/overlay_ws/src/navigation2/nav2_geometric_planner/CubicSplinePlanner/src/cubic_spline_planner.cpp:17:
/opt/overlay_ws/src/navigation2/nav2_geometric_planner/utils/geometric_planner_utils.hpp:22:10: fatal error: tf2/LinearMath/Quaternion.h: No such file or directory
   22 | #include "tf2/LinearMath/Quaternion.h"
      |          ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~

This just needs to be hpp for main. Also check out the linting job, though that's the least of our concerns right now :-)

[sbadamikar-research]

@SteveMacenski Absolutely. This is just V(-1) to keep you in the loop if you'd like to have a look. There is a lot of work to be done on my end. I'll update you once I have something objectively reviewable.

[SteveMacenski]

Thanks -- but in general I'm liking the starting point! My only high-level armchair suggestion is to see if there's alot of redundancy between these planner implementations. Maybe some utilities can be abstracted and/or making this into 1 planner that has run-time selection of which geometric algorithm to use. It looks from my last glance this morning you started on that path. If you create abstractions for createPlan for the geometric technique's common inputs, you can have non-ROS spline/clothoid/etc basic planner APIs that this plugin calls for the selected geometric curve generator model.

i.e. the only custom part of the bezier plugin is:

  const auto control_pts = buildControlPoints(start, viapoints, goal);
  const double resolution = costmap_->getResolution();

  nav_msgs::msg::Path path = sampleBezier(control_pts, resolution, start.header);

if you make a

class BaseGeometricPlanner
{
...

  virtual createPath(...) = 0;  // does the control points build / sample for the bezier derived implementation. One of the imputs is resolution.
}

Then the plugin can hold a std::unique_ptr<BaseGeometricPlanner> which calls that API in the high level ROS plugin's createPlan and does the general stuff like validation / collision checking. Then the selection of which base planner is simply a parameterization and we keep the geometric planners abstracted fully from ROS. New geometryic algorithms can be added very easily with little code more than the basic implementation of the algorithm + updating the parameter parser to load create it in the factory.

---

I'd also suggest adding in a straight-line and Dubins/Reed-Shepp/SE2 model for shits and giggles. OMPL gives you Dubins/Reed-Shepp/SE2 for free and we already have a dependency on it for Smac Planner (also with examples of how to work with those 3) so its an easy-add. Point Claude at the Analytic Expansion of Smac Planner and I'm sure it'll just do it for you.