search

ros-navigation / navigation2

ROS 2 Navigation Framework and System
https://nav2.org/
Other
2.47k stars 1.25k forks source link

Working list of algorithms to include #1710

Closed SteveMacenski closed 1 year ago

SteveMacenski commented 4 years ago

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

Controllers

Recoveries

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

maxlein commented 4 years ago

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

SteveMacenski commented 4 years ago

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 commented 4 years ago

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 commented 4 years ago

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 commented 4 years ago

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 commented 4 years ago

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 commented 4 years ago

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 commented 3 years ago

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

SteveMacenski commented 3 years ago

Pure Pursuit now also merged and complete!

dkuenster commented 3 years ago

Porting mpc_local_planner to ROS2/Navigation 2 here: https://github.com/rst-tu-dortmund/mpc_local_planner/pull/35

SteveMacenski commented 3 years ago

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 commented 3 years ago

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 commented 3 years ago

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

PauloFavero commented 3 years ago

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 commented 3 years ago

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 commented 3 years ago

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 commented 3 years ago

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 commented 3 years ago

@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 commented 3 years ago

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

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 commented 3 years ago 
* 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 commented 3 years ago

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 commented 3 years ago

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 commented 2 years ago 
* 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 commented 2 years ago

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 commented 2 years ago

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

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 commented 2 years ago

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 commented 2 years ago

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 commented 2 years ago

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 commented 2 years ago

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 commented 1 year ago

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 :laughing:

chenwu-cc commented 1 year ago

hello，I can't find the MPPI，

salmagro commented 1 year ago

hello，I can't find the MPPI，

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

chenwu-cc commented 1 year ago

ok,thank you

chenwu-cc commented 1 year ago

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 commented 1 year ago

MPPI merged here: https://github.com/ros-planning/navigation2/pull/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 commented 1 year ago

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 commented 1 year ago

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

SilvioMueller commented 6 months ago

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.