Working list of algorithms to include

[SteveMacenski]

Hi all,

I have a working list of planners / controllers that I've been compiling and I wanted to share them here for potential ideas if anyone's interested in developing / adapting one in particular. Feel free to ping me about this and I'll help however I can

Planners

• A* - holonomic robots or robots small relative to environment
• Dij. - holonomic robots or robots small relative to environment
• OMPL - non-holonomic and non-circular footprint robots, potential formulation to include dynamic obstacles [IN PROGRESS] state lattice and hybrid-A* supersedes
• GPS / cartesian route following - taking in a list of semantic GPS waypoints and issuing a path based on it to follow exactly. GPS waypoint navigation demo #1631 (comment). Or more generally a teach and repeat for non-GPS too. Teach via recording while moving or manual annotation.
• Hybrid-A* - non-holonomic and non-circular robots with smoothed paths
• Vornoi / potential field planner - for a robot to stay far, far away from things as its primary goal NavFn, Theta*, and other planners can be tuned to highly penalize costed regions. While these are easy to implement, the quality is low so its not a good addition for a modern stack. Very 1990s.
• Theta* - doing straight line planning based on LoS
• State Lattice - non-holonomic and non-circular robots, useful in analog situations as Hybrid-A* but for arbitrary motion models (omni, diff, non-cars, forklifts) Hybrid-A* meets this need, might be adding state lattice template node to smac
• non-zero via-point planning (e.g. calling a continuous, kinematically feasible planner from A->1->2->3->…->B and then adding them all to a single path) -- route server Route Server anologous to Planner Server #2229 ? Or new action interface in planner server to have multiple via-points to plan (!)

Controllers

• DWA - compute trajectories based on weighted heuristics
• TEB - compute trajectories based on optimal bands
• MPPI / MPC /Nonlinear MPC- compute trajectories by model simulation supporting high speeds. Exact path follower / dynamic obstacles in formulation in progress
• LQR/iLQR/CiLQR - "MPC-lite" is the easiest way to describe it supporting high speeds MPPI supersedes
• Pure Pursuit - following a carrot

Recoveries

• Spin - spin in place
• Backup - back up from pose
• Alerts - sound, lights, etc to alert obstacle to move too usecase specific
• Wait - wait out some duration or until event occurs
• Push - if you know its safe, push a thing out of the way too usecase specific
• Call for help - Slack, msgs, email, operations center request for assistance
• Clear - clear state, plans, and costmaps
• "Get out of here" - find some permissible path using a dynamically feasible trajectory to just get out of this area some fixed distance, if any way of motion is possible too usecase specific

feel free to comment to add more and how they're useful

[maxlein]

Is there some issue/repo to see the progress of these tasks here?
Like Graph / lane or Pure Pursuit.

[SteveMacenski]

Pure Pursuit PR should land this week. Graph/lane is not likely to be ready anytime particularly soon. If someone else is interested in working on that, very much so welcome to do so.

[soldierofhell]

Hi @SteveMacenski, as for Hybrid-A*, is there any draft on this planner? I'll need it soon, so I'm gonna do this, but it would be good to have some starting point (if there's any)

[SteveMacenski]

Its in progress, if you're interested in collaborating with the 3 of us to finish it, I'm sure we can find a place for you to help out!

I had intended to finish it months ago but had to drop it to do more community management tasks and unblocking other people. Starting next week I should be working on it again in a limited capacity that will grow as other existing commitments complete.

I'd love to hear about your requirements, needs, and uses to make sure it meets your needs.

[soldierofhell]

I did my first approch, but seems like A* is too slow on large grid. Does anyone know any efficient parallization of A* on CPU?

[ghost]

Its in progress, if you're interested in collaborating with the 3 of us to finish it, I'm sure we can find a place for you to help out!

I had intended to finish it months ago but had to drop it to do more community management tasks and unblocking other people. Starting next week I should be working on it again in a limited capacity that will grow as other existing commitments complete.

I'd love to hear about your requirements, needs, and uses to make sure it meets your needs.

Hi @SteveMacenski
A colleague and I have some spare cycles to help with Hybrid-A*. What is the best way to get involved?

[SteveMacenski]

Hi @Robomoco please join the slack with this link https://join.slack.com/t/navigation2/shared_invite/zt-gnsw70li-tUsWwB1C9h3FLIabNVvB~A and introduce yourself on the on-boarding channel. You'll probably want to join the hybrid A* channel and also give me a PM. I'm in the middle of working on that this week too so its a really good time to jump in since my head is in it.

We also have a bi-weekly working group meeting on Thursdays. If you go to the TSC ROS2 page, at the bottom there's a calendar. You can add yourself to the navigation2 meetings and there's a link there for the Google Meet meeting.

[SteveMacenski]

@soldierofhell the hybrid A* planner has been merged and ready for use!

[SteveMacenski]

Pure Pursuit now also merged and complete!

[dkuenster]

Porting mpc_local_planner to ROS2/Navigation 2 here:
rst-tu-dortmund/mpc_local_planner#35

[SteveMacenski]

Sounds good! I thought it was still experimental, so I didn't add that to my queue to work on, thanks for the great work!

[mkolodziejczyk-piap]

Thanks @dkuenster for putting it all together. I actually did it my port few months ago and just couldn't find time to clean to public and in the meantime it grew with some additional features. I'll try to review your work. We've been using it already with LGSVL, Carla and custom Gazebo simulator and now starting more testing on real vehicles. In general I'm happy with results and the similarity to teb interface.
BTW I'm using acceleration control and thinking about implementing AckermannDrive message replacement in nav2, because now I have to put acceleration, jerk and steering angle into Twist fields

[dkuenster]

@mkolodziejczyk-piap A review would be much appreciated. The new AckermannDrive msgs also sound interesting.

[PauloFavero]

Hi @SteveMacenski,

I am researching comparing planners applied to a holonomic robot. Asides from A* and Dijkstra, I would like to also use and compare algorithms such as RRT, RRT*, or PRM, for example.

I found this issue that is a work to incorporate OMPL as a planner plugin. I didn't see this implementation integrated on nav2 or anything related to this on the roadmap. is the implementation of such planner algorithms planned soon? As I read on the issue, the focus of the project changed due to other priorities.

If there is a need, I will be interested in implementing an RRT planner plugin.

[SteveMacenski]

Hi,

There are no plans on creating an RRT or similar planner in Nav2 currently. We had that work going on and did a great deal of prototyping but were unable to get a planner of sufficient performance worth using in Nav2. At best, we were able to get an RRT* planner using Dubin motion models to work relatively well, but then Hybrid-A* is a better choice for mobile robot navigation and uses the same models (and faster).

If there was a legitimate need for an RRT (or similar) planner, I'd like to hear it and if it fills a niche that another algorithm we already have or is on the roadmap doesn't solve, then we can add it and I'd definitely appreciate your help in implementing it

[PauloFavero]

Thanks for your feedback @SteveMacenski.

What performance requirement the RRT* prototype didn't fill? Convergence time, time to cover a region... Talking about performance issues, do you have a benchmark comparing these methods when implemented on Nav2? Like RRT* vs Hybrid-A*?

[soldierofhell]

I'd compare probabilistic methods (like RRT*) vs search based to Monte Carlo integration vs regular quadratures. Probabilistic methods are for higher dimension spaces, to overcome curse of dimensionality. This is their strength, not in SE2 space.

[Parv-Maheshwari]

@SteveMacenski continuing our discussion from #2042 , me and my team have a sampling based parallelized local planner in Frenet frame. Our code is available here.

We have applied parallelisation on the entire trajectory generation step - sampling trajectories in Frenet frame, trajectories converted from Frenet frame to global frame and collision checking. This has resulted in a 5 fold increase in frequency. We also have used openmp in our costmap_callback function which accesses the costmap and updates the obstacle coordinates.

You can find these in two files - ./frenet_planner_agv/src/frenetROS_obst.cpp and frenet_planner_agv/src/frenet_optimal_trajectory.cpp

So we would like to develop this as a formal Nav2 controller plugin like DWA, TEB etc. This can be a step towards parallelized controller plugins.

P.S. - We have already started working on porting our code to ROS2 and we are also cleaning the code so apologies for that.

[SteveMacenski]

I've taken a glance at the codebase, but clearly any analysis I do without really diving into it isn't going to be complete. So it might be helpful if you started from the beginning and provide us context of why you made this project in the first place and a common understanding of what you've done to start off a discussion with. For example answering the questions

• What was your rationale for this particular algorithm? What environments / robot types / drivetypes is this meant for and tested on?
• What is the benefit or detractors of this approach when compared to DWB, TEB/MPC, or a Pure Pursuit algorithm?
• Has this been significantly tested / validated on hardware and is this something you actually are using on some product / advanced research on a regular basis?
• What group would benefit from this being in Nav2 (e.g. what problem does this solve that is sufficiently reusable that it is worth us maintaining in Nav2 itself)?

At least one concern I have from looking over it is that I see a parameter for "road width" which makes me question this approach's usefulness to a broad class of mobile robotics problems, since that is not typically going to be usable for free space planning and for some specific mobile robotics applications where you do have lanes, they will probably vary at different locations. I'm fine with having some specialized (e.g. non freespace) planners if there are sufficient reusability, but I'm not sure what that scope is here.

[Parv-Maheshwari]

* What was your rationale for this particular algorithm? What environments / robot types / drivetypes is this meant for and tested on?

The planner in its current state, is deployed & tested on a non-holonomic electric vehicle. The planner samples in both spatial & temporal with special focus being given to minimization of jerk for passenger comfort. Previous works focused solely on finding the shortest path only which given the use case of autonomous driving isn't the most suited factor for consideration.

* What is the benefit or detractors of this approach when compared to DWB, TEB/MPC, or a Pure Pursuit algorithm?

TEB works very well for holonomic vehicles. But in cases of non-holonomic vehicles, there is a lot of parameter tuning that needs to be done to make it functional on an actual vehicle. Even then the algorithm has a large overhead being a heuristic based one. Comparisons on an actual test vehicle (Mahindra e2O) showed that while TEB ran at <1Hz planning frequency, we were able to attain 3-5Hz on frenet without any kind of optimizations.

Apart from that the algorithm is a local planning algorithm & still requires running MPC and/or pure pursuit for controller and tracking purpose for actual vehicle (as can be observed in our package)

* Has this been significantly tested / validated on hardware and is this something you actually are using on some product / advanced research on a regular basis?

Before the onset of COVID, the above package has been successfully tested on an actual test vehicle Mahindra e2O along with successful Gazebo simulations.

* What group would benefit from this being in Nav2 (e.g. what problem does this solve that is sufficiently reusable that it is worth us maintaining in Nav2 itself)?

The planner is specially useful for non-holonomic scenarios. But sampling on both spatial & temporal values is the key component of the algorithm and thus makes it applicable in various other fields specially but not restricted to where lane like scenarios are applicable.

At least one concern I have from looking over it is that I see a parameter for "road width" which makes me question this approach's usefulness to a broad class of mobile robotics problems, since that is not typically going to be usable for free space planning and for some specific mobile robotics applications where you do have lanes, they will probably vary at different locations. I'm fine with having some specialized (e.g. non freespace) planners if there are sufficient reusability, but I'm not sure what that scope is here.

The road_width value can be considered as a parameter which limits the range of sampling of lateral offset at the final point of the trajectory.

This can be changed according to the lane width that is supposed to be input from the laserscan/semantic_segmentation module.

[SteveMacenski]

Hi,

I understand better now. I don't think this plugin would make sense for us to add to Nav2 directly, but I'd be happy to include it in the list of Nav2 plugins available for users to use on our documentation https://navigation.ros.org/plugins/index.html if/when it is made available in ROS2. The reason is that this controller is really more suited to AV situations and this is an autonomous mobile robot framework. While you could use this on an AV, it would NOT be recommended and none of our design requirements are based on that intention. I would be open to reconsidering this if you could provide examples of other independent users of this algorithm in conjunction with Nav2 (to prove a userbase) and significant hardware validation testing / deployment (to prove functionality).

Does that make sense?

[Parv-Maheshwari]

Sure. We will work on ROS2 port as of now and would also try to implement it in non AV scenarios to expand the userbase and functionality. Thank you. Would update here when significant progress has been made.

[salmagro]

* MPC /Nonlinear MPC- compute trajectories by model simulation supporting high speeds. Exact path follower / dynamic obstacles in formulation

Is there any source for this implementation (repository/paper)? There are several options for this, but I would like to know which specific approach are you analysing.

[vinnnyr]

Any feedback on thinking of e_star style nav2 planner for online coverage? https://linkslab.uconn.edu/wp-content/uploads/sites/246/2018/11/e_star_Preprint.pdf

[SteveMacenski]

There are time / space optimal coverage algorithms that depend on what your application is for which makes most sense (e.g. randomly bouncing around is a bad idea for a lawn mower; and row by row coverage is probably bad for a security robot) so I don't think there's going to be 1 coverage algorithm to rule them all.

I do wonder thusly if a coverage algorithm makes sense to add to Nav2 itself, but we certainly can use more of them in the ecosystem and link to it to Nav2 documentation so that others can be aware of them and use them.

Some others that exist for ROS 1

• https://github.com/Greenzie/boustrophedon_planner
• https://github.com/ethz-asl/polygon_coverage_planning
• https://github.com/nobleo/full_coverage_path_planner

Which I have explored in the past and for various reasons I don't recall for each I decided not to continue to explore for Nav2 inclusion. I believe at least one wasn't actually being used and more by the company and at least one more it wasn't actually used ever for more than a paper. That's not to say they don't work or that there is anything wrong with them, just not to a known quality level for production that would make me comfortable ingesting it into Nav2 without either (a) a company's support or (b) known quality for a product's use

[LotfiZ]

Hi,

I dont know if its relevant here but i'm wondering if the controllers listed above takes into account the localization uncertainties ? We experienced with TEB for example that even if the trajectory is optimal, the execution is jerky. First i thought that the very simple control law implemented there caused this, but i'm not sure that it's the only reason ! Do you think that with MPC ( and its variants) the trajectory tracking will be smooth even using an accurate models ?

Thanks

[SteveMacenski]

All trajectory planners work in the odometry frame which is continuous and not subject to localization uncertainty / jumping. That seems more like a potential problem on your robot hardware controllers or not having TEB tuned well. By the way, TEB is MPC.

[LotfiZ]

Thanks Steve for your answer. Correct me if i'm wrong but if for example the localization stack (e.g AMCL ) jumps for X reason while performing local planning, the first pose of the TEB will change as its the pose of the robot on the map so the TEB will try continuously "compensate" for this kind of drift. In another word : if i take same robot in simulation with same scenario, in first place i run TEB with a perfect localization system and after that i run the same configuration with putting some uncertainties in localization. The output commands will be more jerky than in the case of perfect localization. Even in reality, a part of hardware limitation, the worse my localization, the more jerk i have.

Concerning the control law, dont you think that an improvement in the actual one for TEB ( takes two first poses of the optimized trajectory within the time difference between them and compute the velocity commands ) could be interesting to revaluate ? Because at each frequency cycle both of optimizer and computation of the velocity commands are executed. Is it a good approach for example to separate this two execution with different frequencies ? I read something pointing that here : https://github.com/BadgerTechnologies/teb_local_planner/commit/460d73b21be3a1f8f20b0535190ac319d073005a

I'm very interested about contributing to limit the jerk in the TEB without using an external module like a velocity smoother. we have the opportunity to work with real industrial AGVs/Forklifts so we are able to tests/log a lot of things in real life scenarios !

Thank you very much !

[SteveMacenski]

I can't answer you how TEB works internally too much, that's not my codebase. What I'm saying though is that the trajectory planner operates in the odometry frame which is smooth and continuous, not subject to localization updates. The global plan is probably transformed into the odometry frame for use, as we do in Nav2's internal trajectory planners, which could then be subject to some of those discontinuities in the position of the path, but the trajectory outputs should still not jerk if the smoothness terms are evaluated. But I'm can't get into the specific details of TEB, I really haven't looked that closely.

TEB is not in this project, if you have concerns, you should file tickets with TEB directly.

[SteveMacenski]

GPS covered by another ticket and MPPI/MPC is imminent to be added in January. I think that closes this out for the cross-section of capabilities I wanted to have added! It only took me 3 years 😆

[chenwu-cc]

hello，I can't find the MPPI，

[salmagro]

hello，I can't find the MPPI，

@chenwu-cc Please give a look at https://github.com/artofnothingness/mppic
The MR is still open: #3350

[chenwu-cc]

ok,thank you

[chenwu-cc]

hello，I can't find the MPPI，

@chenwu-cc Please give a look at https://github.com/artofnothingness/mppic The MR is still open: #3350

I also want to ask, do you know if there is a ROS2 implementation of mpc

[SteveMacenski]

MPPI merged here: #3350

MPPI is an MPC controller. We selected this over another variant of MPC because MPPI lets us set up less restrictive penalty functions which are necessary to do "realistic" mobile robotics things beyond just chase a carrot and minimize cross-track error.

[chenwu-cc]

MPPI merged here: #3350

MPPI is an MPC controller. We selected this over another variant of MPC because MPPI lets us set up less restrictive penalty functions which are necessary to do "realistic" mobile robotics things beyond just chase a carrot and minimize cross-track error.

I used the Humble version to compile the mppi, but an error occurred. What is your working environment?

[SteveMacenski]

Its in main, so Rolling and newer. It may be backported to Humble, but not for several weeks at the earliest.

[SilvioMueller]

I need a controller that follows a given path with an exact speed. Can I realize this with MPPI? How could I do that?

I have a working implementation in a custom MPC controller, with a cost function for the velocity, but I would like to move to the NAV2 stack.