[OMPL] Nav2 OMPL planner

[shivaang12]

This ticket is for progress tracking and for the discussion on nav2 OMPL planner.

Initial implementation and it's feedback, refer to ros-planning/navigation2_tutorials#2

Link to my branch: https://github.com/shivaang12/navigation2/tree/feature/nav2_ompl_planner

[shivaang12]

@SteveMacenski I think cost aware algorithm has made difference (RRTstar instead of RRT). I need to check the exact time but I can say that it is << 1 second. I will trying testing with the suggested settings and get back to this issue.

@mamoll Thanks for the info! I was thinking if the inf cost thing is due to slow convergence or is it purely due to error in state validator. Thanks!

[mamoll]

@shivaang12 I don't know to what extent you have explored the OptimizationObjective infrastructure, but independent from the particular planning algorithm you can express the cost of states, motions, and how they should be aggregated into an overall path cost. The default objective is to minimize path length, but it might be worth playing around with this if the "optimal" path doesn't look all that optimal to you.

[shivaang12]

@mamoll I tried playing around with TRRT and BiTRRT planners but I am not satisfied with their performance. TRRT does converge but spits out jagged path much more similar to RRT, and BiTRRT couldn't able to find a feasible path in 1 second.

I tried using CostConvergenceTerminationCondition but couldn't able to make it work yet. It would be helpful if you can link some examples.

[mamoll]

This unit test has some basic examples. Once you have created a CostConvergenceTerminationCondition instance, you should be able to pass it as the only argument to Planner::solve(). Can you be more specific in what doesn't work?

[ardabbour]

So apparently the root of my memory issues was that I was asking the costmap to give me the cost of a point that did not exist. Now, I ran some tests and it seems that the plans are mostly valid and 1-2 Hz but very far from optimal. Here are some results:

• RRT
• EST
• KPIECE

I don't think we can get any faster than this without implementing a steering function for the controls or implementing a Balkcom-Mason curve and using it, as @mamoll said earlier.

The navigation server continuously asks for plans and that is not a good idea for this planner. Is there a parameter that switches this behaviour off?

Also I have moved my implementations to nav2_ompl_planner to make life easier for me. There is also a geometric branch where I am implementing the planner using the Reeds-Shepp curve.

[SteveMacenski]

https://github.com/ardabbour/nav2_ompl_planner/blob/master/CMakeLists.txt can you try adding optimization flags & build in release mode? Maybe there's some to be gained that way.

Those are hilariously bad plans, is that due to a mis-set maximum turning radius (looks like the turns are incredibly wide) maybe? Its not great if these take 0.5-1s when those paths are also very, very short (< 10 m).

I wonder how @shivaang12 is doing without the velocity space. That should in concept be much faster since its doing alot less. I would hope then it could be at least 300ms bounded...

[shivaang12]

As I said earlier, if we want 100ms - 300ms bounded performance it can be only possible using non-cost aware algorithms such as RRT etc. Cost aware planners takes nearly one second to get a single feasible path, some times beyond that. Going with non-cost ware planners, means settling for jagged path.

[SteveMacenski]

Maybe we should look into the curves then. This seems silly to me that its so slow.

[mamoll]

Cost aware planners takes nearly one second to get a single feasible path, some times beyond that.

Even with an appropriate CostConvergenceTerminationCondition set? What kind of state space are you planning in: SE(2), Dubins, Reeds-Shepp? What planner(s) are you using? What happens if you call OptimizationObjective::setCostThreshold with a very large cost?

[shivaang12]

I am using SE(2) state space (I got suggestion from Abdul to use Reeds-Shepp State Space). I am using RRTStar and planning to test with RRTSharp. Higher OptimizationObjective::setCostThreshold introduces more jaggedness into the path. I'll try with RRTSharp and will let you know the results.

[ardabbour]

@SteveMacenski I always ask colcon to compile with -o3 and release, but I added the optimizations to the cmake file anyway for others to test. Unless anyone has any other ideas, we should consider writing a steering function. Maybe @mamoll can guide us to a simple example or unit test to get started with.

Now moving on to the geometric methods, I combined @shivaang12's code with mine and used a Reeds-Shepp state space. On the example environment, it works at around 10-20 Hz but a different Nav2 replanning paradigm needs to be used for this planner and this video shows why. I don't know what the Nav2 condition for replanning is, but it is not suitable here because it seems to assume that generated plans will complement one another, which is not always the case for sampling-based planners, unless they are constrained to return highly optimal plans.

I understand that replanning is needed when a plan fails for whatever reason, but Nav2 seems to continuously request plans regardless? I understand this is achieved with the default behavior tree of Nav2, but I'm just trying to understand why creating global plans at 20 Hz is needed when we have a robust controller (dwb) attempting to follow the path...

@shivaang12 I think if you use the lazy bidirectional KPIECE, with path simplification and smoothing (as I have done here), you can get fairly fast results (for me this was around 0.05-0.1 seconds for the entire createPlan function, but any talk of bounding to a certain time doesn't make sense with sampling-based planners because of their nature.

[SteveMacenski]

Well that could also be a little bit on the fault of the trajectory planner, but yeah, that clearly is a problem. Current replanning is just at a constant rate (1hz), but soon we're discussing making it proportionate to speed. Plenty of other replanning behaviors could be implemented too using the behavior trees.

Those plans also seem really sub-optimal. If we can plan at 10-20hz now, could we back that down to 5-10hz if the quality is much closer to optimal? I'd also try on a realistic sized map to see what the planning rate is. That small distance isn't representative of an actual application.

Are you storing the different methods using OMPL that you're using in different branches? It would be good to make sure we have a history of attempts to work off of if needed. Is this one cost aware?

[mamoll]

For replanning, you'd typically seed a planner with the remainder of the trajectory the robot is executing. One trick to do this in OMPL is to create a special sampler that you feed the states along the remainder of the trajectory during construction. You configure planners to use this sampler. When generating "random" samples, the first samples will be the states that were passed in. After it has generated those, it falls back on the default random sampling behavior. There is no guarantee that every planner will exactly reconstruct the trajectory the robot was on, but you are more likely to stick with a similar solution path.

[ardabbour]

I will be trying it on a larger map soon, but from my experience we can expect good performance for each path but expecting all the paths to be consistent with one another is a bit of a stretch. I do like the idea of seeding the planner with previous states. All SLAM implementations of OMPL I have seen completely throw away all the information after each plan is made because the state space is recreated each time a plan is requested. If we can somehow keep the relevant information, it would be a great boost.

Also regarding the different methods, all the planners and their basic settings are exposed as ROS parameters. There are two different implementations right now: control-based on the master branch, and geometric-based on the geometry branch.

[soldierofhell]

The kinodynamic planners treat the vehicle dynamics by default as a black box, just probing the system with random controls to see what gets the system closer to the goal. Without spending a lot of time on a steering function you probably won't get the performance you want.

Hi @mamoll, are you aware of any example of such steering? I've searched a bit and haven't found any implementation of steer() function except some trivial ones

[SteveMacenski]

One of the ultimate aims of this work was to support other classes of robots that didn't fit well in the dubin / reeds-shepp motion models that are used in a hybrid-A* planner - which we've found to be substantially faster. Given it appears that the only want to make OMPL planners feasible is to use these motion models, I suggest perhaps we change our focus onto a state lattice node for the new smac planner that can take in the motion primitives for an arbitrary vehicle and use the optimized search framework to allow for arbitrary robots with arbitrary motion models.

That would involve simply:

• A new state lattice node template that reads in the projections from those motion models
• Scripts or documentation about how to generate these lattices

Any objections to closing this ticket and changing the focus on this?

[ardabbour]

None from me

[SteveMacenski]

Closing from comment in October after no other objections