Another approach, would be to handle the problem at the level of the state machine, assuming you had one, which would allow you to use more simplistic planners. For instance, something like…
If dynamic obstacle is >10ft away, use planner A, with settings X; // We’ll call this state1
If the dynamic obstacle is <10ft away, use planner B (or just pause), with settings Y; // Which we’ll call state2
Obviously this is quite dependent on the particulars of your application. But I would say that is a strength of handling problems like this at the State Machine level as opposed to the planners.
This problem largely does not relate to state machines and I’m not sure why you bring this up. Nav2 is also already built on behavior trees and it is already being done to use multiple planners in unique contexts, but that really isn’t what the asker is asking about.
TEB is a decent option but you need to be more precise with your language. Do you want a local planner / controller or global planner that handles dynamic obstacles? Your list contains both which serve different reasons.
I’ve gone into detail in other venues, so I don’t want to repeat myself too much, but there are 3 prevailing strategies I see for handling these situations:
A costmap layer that uses detections and tracks to insert obstacles into the costmap and “smear” or create a guassian distribution around the obstacle favoring the direction of travel to make non-kinodynamic planners able to artificially plan around where obstacles are or will be in the immediate future.
A kinodynamic global planner to take in detections and tracks and plan through space and time in order to avoid dynamic obstacles globally
A local planner to take in detections and tracks and has logic internally for avoiding dynamic obstacles, there are any strategies that can be employed here, including new critics in DWB, and MPC/TEB, or more.
As you see, the main issue here is not in the planning algorithms, most of these already exist or would not be too difficult for someone to make, its in a generalized detection and tracking. These can be application specific, or hardware specific, or even compute specific.
There’s some work we would like to do in Nav2 that was started about a year ago but we haven’t made progress due to being low on resources. This is to make a dynamic detection and tracking pipeline that generalizes so that people can bring their own detectors to the party to use in Nav2, and then the outputs of that pipeline are ingested by a new costmap layer for the first strategy, or used in the planning/control algorithms for the other two. We have the basic Alpha of the tracker done but haven’t pushed the RGBD / 2D / 3D lidar detectors to conclusion so that we could meaningfully test, evaluate, and fix any non-foreseen issues. It might be another year before we pick that work up again unfortunately.
But if this is something you would be interested in contributing to, I’d be happy to help steer you in the right direction if you’d be willing to keep this open source and integrate in with Nav2!
This problem largely does not relate to state machines and I’m not sure why you bring this up. Nav2 is also already built on behavior trees and it is already being done to use multiple planners in unique contexts, but that really isn’t what the asker is asking about.
State machines might offer a different way of looking at the problem, which is why I brought it up.
And the OP didn’t specify ROS2, and attached links to the ROS1 wiki.
And even if he did, (although I doubt I would have chimed in), not everyone agrees with the BT approach.
But I’m not here to hijack the original post. So, please carry on.
Hi, I appreciate the suggestion and I already have that working. What I mainly need is what would happen in the state where there are moving obstacles.
Thank you so much for the explanation, it’s quite helpful.
I have a few follow up questions:
I would be working in a largely open space with few constrained areas that have restricted entry and exit points, my guess is that a global planner handling dynamic obstacles should be more robust, is that right?
It would be great if you could point me to some of those locations so that I can continue researching on those.
I do have a detection and tracking pipeline, so based on what you have mentioned my understanding/questions for each are:
I use any planner that can update trajectory in real time and I update the costmap according to the motion of the obstacle and use it directly. This seems like a decent approach since we have a slow moving UGV, we don’t really need to worry too much about the dynamic constraints but it may lead to sub-optimal trajectories if I not wrong?
Is there a kinodynamic global planner that can plan through space and time integrated into the ROS navigation stack already? I believe that sbpl dynamic planner would have fallen under this category.
Our current approach is to use MPC local planner since it creates the smoothest trajectory in uneven terrain but I could not find a way to get it to handle moving obstacles. Am I missing something?
Yes @smac I would be glad to contribute to this. My questions regarding whether something of the sort exists directly integrated into ROS or not was mainly focused towards understanding if I can contribute this to the ROS community.
I can’t answer that with any degree of certainty, it depends on more than what would be appropriate for you to send me. This is up to a system designer. In general from that description, I’d think “no”, local dynamic handling would be good enough. But depends alot on the number of agents in the environment and how “open” open means.
Correct, but also, does 30 seconds matter to your application? Some it might, and in those cases true kinodynamic planning might be worth looking at. If adding 30 seconds to your navigation time doesn’t really matter, then I wouldn’t worry about the suboptimality. By nature of dealing with dynamic obstacles with unknown velocity vectors that are subject to change (e.g. people don’t walk in a straight line), theoretical optimality is unlikely to be actually optimal in any sense.
I’m pretty practically minded. Truly optimal matters in less cases than you might think, but when it matters, it matters. However once you start talking about dynamic agents that are changing motion frequently, that throws some wrenches around.
Not at the moment, but its on my list of algorithms to have added at some point Working list of algorithms to include · Issue #1710 · ros-planning/navigation2 · GitHub. The 2 major schools of thoughts there are lattice based and sampling based. I’m still working on getting the kinematically feasible lattice planner in the stack, so the kinodynamic one is taking a back seat to that (and the need from users for it). A sampling based one would be markedly simpler, but higher run-time. All that’s to wait until we have dynamic tracking pipelines merged in as well so its even relevant to have a planner to ingest it.
Its algorithm specific, something like a TEB will handle dynamic obstacles. It wouldn’t be hard to add dynamic obstacle critics to DWB framework. There’s definitely options of methods to implement or modifications to existing methods – but generally a DWA and an MPC are the big branches to look at. An MPC can handle it as well, but MPC is a big branch to look at, there are many ways of formulation and data to ingest.
I’d love to have your contribution! Maybe we should chat here or in a PM thread about how you might want to get involved and some milestones to hit! Certainly helping with dynamic obstacle detection/tracking that you already have would be a great starting point and then build it into the planners to see what kinds of methods work best. The costmap guassian method would be pretty simple to make / integrate into Nav2 once we had a reliable / tested dynamic tracking pipeline available. I’m a fan of “one problem at a time”, since having the costmap layers but no way of using them isn’t that impactful (and wouldn’t be able to confidently know they work as well as I’d want them to in order to merge in). Once that first problem is done though, that opens the door to alot of collaborations and parallel work that alot of folks might be open to joining in on.
Hi @smac, could you point out at trackers work in progress developments? I’d like to understand whether my application could use them and whether my dev forces could be helpful in current development.
Hi all, I’m very interesting on the matter described on this thread. What a pity the abrupt end!
@Ridhwanluthra, you mention that you have a tracking system? Is it available as open source?
@smac, I saw a PR open and a fork with trackers for Nav2, but all seems to be frozen. Nothing going on on Nav2 side (or anywhere else, to your knowledge)?
Mostly a lack of resources to continue. I’d be happy to continue it, but it would need outside contribution and/or funding. We may eventually get back to it, but its not on my imminent agenda for the next 9-12 months unless some outside force steps in to make it happen.
The biggest two things that made this get shelved was (1) a lack of a current end-user to do realistic testing of the multi-object tracker for me to gain confidence that we could move on and (2) a lack of non-student non-hobbyist level expertise in deep-learning to refine and ship the first round of detectors to be able to then use the pipeline with costmap layers, controller algorithms, etc.
This is the kind of project that requires a partnership with someone that needs it and has the ability to test it. This is unfortunately not the kind of thing I can bench test, test for a few hours in my lab, or simulate effectively with a high degree of confidence. It needs longer duration evaluation in the settings it’ll be deployed in, which unfortunately I haven’t had (A) access to nor (B) a partner organization that needs it to provide that testing.
You can be that outside force though I just have high quality standards of things we stamp the “Nav2” name on and I didn’t see a path forward for it at the time, so it got shelved for another time, with another partner.
Also note that MPPI does quite well without explicit modeling. That’s obviously fresh off the presses in the last few months so you may not have tried it yet
Try MPPI? nope, but it’s gonna happen soon, he he. By now I just saw your talk on ROSCON (congrats!).
We have already done some interesting work on dynamic obstacles avoidance (indeed there was a lightning talk by Leonie Gadner about one of the approaches we developed, namely timed costmaps).
Main problem is that the 2 approaches we have developed require feeding the velocity and direction of the dynamic obstacles, and we don’t have at the moment a perception expert available to develop the tracking part.
I just have high quality standards of things we stamp the “Nav2” name on and I didn’t see a path forward for it at the time, so it got shelved for another time, with another partner.