Proposal for inclusion in Nav2 Stack #1
Comments
|
Hi! Continuing from the other thread. I should start being able to look into this more directly start the week of March 2nd after a bunch of my current deadlines are over. My focus is going to be on controllers (MPPI, MPC, DWB) so this will be one of the major areas of interest.
I don't think we need to penalize curvature, I think that actually how this controller works already kind of does that naturally, but it wouldn't be hard to test and see what we think. I think what is absolutely necessary though is being able to set a maximum curvature parameter (e.g. minimum turning radius) for which if a primitive exceeds, we throw it out entirely and not evaluate it. If we want to keep a consistent
Yeah, I noticed that. One of the benefits of changing to some variables for the Normally I'd say that Omni is a "nice to have" but actually I'm thinking now that this work might fully replace DWB in Nav2 if things work out well, so its a "must have" so that we continue to have Omni support. Ackermann continues to be a "nice to have" but its easier to add it now than later and from the discussion above, it's not exactly rocket since to have it supported either. |
Currently linear/angular velocity constraints are implemented. I'll think about additional constraints that you mentioned.
I had thoughts to keep data contiguous in the state/batch. Diffdrive/carlike state looks like [ VX, WZ, controlVX, controlWZ, dt (time intervals) ], and sometimes its efficient to have contiguous slice of current velocities from whole batch, e.g. [VX, WZ], so with Y it would be [ VX, VY, WZ ]. Consequently that requires index shifting. I've just implemented runtime version of such abstraction. https://github.com/artofnothingness/mppic/blob/develop/include/mppic/State.hpp Now adding Y sampling/propagation |
|
Why not always have the Y fields in place? Does it really add that much compute to solve the optimization problem to have 0-ed out tensor dimensions? That would make the code a bit easier to follow / less error prone with changes later w.r.t. dimensions. This is awesome, I see you've made a bunch of changes here already! |
|
If someone will use diffdrive/carlike and e.g. neural network as "Model" to predict velocities, and tensor wouldn't be contiguous (zeros Y, zeros controlY) it would require additional data manipulation to get right batch. Slices would require additional attention too. I think having holes in tensor would be more error prune. xtensor already has dynamic size of each dimension, all is needed to adjust size of last dimension to state vector size, and use idxes that match chosen vector. I've been thinking about this the last few days. Some implemented abstractions already works quite well |
|
OK, I can get behind that |
|
Y seems to work. Tested with https://github.com/neobotix/neo_simulation2. Also pluginized critics and fix some bugs |
|
Oh wow, incredible. Can you show a gif? How do you feel about the behavior? What's the runtime you can get this to go at on your CPU? I'm still working towards some deadlines that come up on March 1st (1 week). I have been tracking your changes briefly when I had time and had some comments, but I wanted to wait until it was all put together to share. Doing a deep dive over your changes is one of my first tasks (after I take a day or two off to relax). One of the easier things to pull back in is back into 1 package. I know DWB spreads things out into a bunch of individual packages. That's mostly done because they're picking and choosing packages for internal vs external use. For this work, there is no "internal" use so this can all be shipped together as 1 unit. Something you may consider doing is using the Costmap2D's collision checker object (https://github.com/ros-planning/navigation2/blob/main/nav2_costmap_2d/src/footprint_collision_checker.cpp) since you already have the dependency on Costmap2D. This will handle the footprint line iterator stuff for you for the full footprint. You can see an example of how I use it in Smac Planner to wrap it into an API for a more specific need (https://github.com/ros-planning/navigation2/blob/main/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp) but I think you could probably get away with the raw API. I did that because I needed to precompute some footprint orientations. I'd also recommend keeping the option to use single point-costs vs the full footprint in the situations that people actually are using a circular robot. Collision checking is definitely not cheap. A parameter would do, or you could implement another critic that only does point-costs (like DWB does) and that would be fine as well. |
Feels quite nice, but i've only tested it in gazebo and in a simple environment yet. Main observation - omni needs a larger batch_size Tested it on intel i5-4200H CPU
Yeah, that's easy change, and i feel the same, that it is redundant separation.
Thanks, I'll take a look at it.
Sure, that would make sense |
|
Added collision checker, consider_footprint parameter, and moved everything in one package |
I can't tell you how much I love that number. TEB is way slower than that and DWB its often hard to get that running at 20hz on a raspberry pi. If you're getting 45hz on a 4th gen i5, I'm really excited about that! |
|
Anyhow that number highly depends on iteration_count/batch_size/time_steps/reference path point count |
|
This is true. We should probably cull out the reference path some distance away from the current pose so that its only looking at a window of poses in the immediate term. Is this already happening implicitly? I see that its being sent the full global path but how do you handle the distance correlation when path points are far away? We do this in RPP and DWB, but I think for how your https://github.com/artofnothingness/mppic/blob/develop/include/mppic/utils/geometry.hpp is setup, we don't need to be nearly as conservative and only need to rough-out the right window. Maybe just cull points more than 1.5x the longest possible primitive length away? I only took a quick glance at the On the other points, we can definitely circle back to see if its a problem. But if you tuned your system and found generally good parameters for iteration_count/batch_size/time_steps that work well, and it gives you roughly that 22ms performance, I think that means its good. Certainly a user can badly tune those things. I think it would actually be useful if we found some really solid baseline numbers for iteration_count/batch_size/time_steps and made them default so that most users will have a great out of box experience for a typical mobile robot platform. |
It's clipped by PathHandler before it's passed to Optimizer by some fixed distance (lookahead_distance) considering current pose. Do you mean dynamically change this distance, depending on how the closest path point is far away ?
I had thoughts about it. It seemed to me that there was no need for this, since you can always manually set a static lookahead distance for global path clipping.
I think it would be nice to add dynamic parameters to test different parameters settings. Sure targeting nice defaults is the goal. |
|
Back from PTO. This week is going to be a bit of catch up and I still need to work on getting a hardware platform to test this work on, but this MPPI and DWB work is my main focus for the immediate future (beyond maintenance tasks within Nav2 like reviewing PRs and issues, which does take up my time daily). I just meant somehow clip the path reasonably so that it doesn't grow too high, but it sounds like what you have now would suffice. Later when we do more profiling maybe we find we can save some time here, but I wouldn't overoptimize for anything like that until we have evidence that it is a big contributor to CPU worth the time / effort. I did a quick look through of the codebase after your refactors and had some questions / comments. This is not a list of things I expect you to do all of, I can jump in here and help too, and will plan to once I can get started on touching the code directly (hopefully later this week). This is mostly to get everything in 1 concise place to get started with. Note that I did not critique deeply the implementation itself, this is more superficial stuff to hammer out first before the deep dive. But I did look over that stuff at length a couple of months ago so I don't think I'll have any major concerns as long as the changes are largely just refactors.
All in all, the refactor was done very well. I kind of liked the old version of MPPI because its simplicity of being largely all in 1 file, but we did need to split out the critics to make them plugins and some other of the changes lent themselves well for being spun out. I think you did a good job with that - I don't see anything you did as unreasonable. At most, I might suggest rolling the |
Hmm. I'm not sure if i get it right. As far as i know controller server has ownership of costmap.
As far as i know weak_ptr is needed for checking if object still alive. And here in controller plugin i assume that Node always alive while controller server plugins uses it. What you mean by "shutdown logistics in the server" ? Mb some reference to the code will explain that to me better.
Removed helper function getParam, but getParamGetter still seems useful to me. With that util i don't need to keep track of declare/get parameters in different places and also repeatedly concatenate node names with params names
Removed templates there.
Probably it's not
It may be redundant somewhere :) I'll take a look on this
c is for control. It was done in kinda haste. It's requires better documentation/naming.
It's from MPPI litrature. We have costs on each trajectory, and controls from each trajectory weighted respectively to those costs, this parameter defines how much we should consider those costs. Big value mean that we just take average controls from all trajectories and not considering costs at all. Honestly, i've experimented just a little with this. I think the main reason to this is to make result control sequence more smooth
i check cost from footprintCostAtPose (collision_checker) if bool variable set to true
I'll take a look on it Thanks for your comments! |
|
With respect to the smart pointers and WeakPtr topic - I can go into more detail, but its essentially related to the startup and shutdown sequences and making sure there aren't any circular dependencies when memory is being deallocated with smart pointers to have a clean shutdown. WeakPtrs are useful when you need access to a resource, but you don't want it to be incrementing the reference counter internally when its not being utilized. This is why we pass in Nav2 the node as a WeakPtr to the plugins so they can store and lock it when they need it, but it doesn't create a circular dependency / non-zero reference count to destroy it when we're trying to deterministically shut down the servers containing We encountered that issue long ago and we had some work to retool our system in Nav2 around that WeakPtr and only storing it in the plugin (never the shared pointer locked resource) so that we can get clean program exits. We used to use shared pointers in the plugins and then we found that Control-C's were not giving clean exits because a shared pointer reset was being blocked by the ownership of a reference within a plugin that was reset. With respect to the use of smart pointers vs Also, imagine a situation where we need to swap out pointers due to dynamic parameter reconfiguration the underlying pointer, then this plugin would be the only that would not get the updated info and its ptr would be deleted from the stack. Smart pointers have very little overhead and what they give you in behavior is well worth its use. If that's not a good enough reason, its just the code styling ROS 2 that Nav2 tries to comply with to be a "golden" example 😆. ROS 2 is heavily littered with smart, weak, and unique pointers and we rarely use raw pointers anymore. I think that's generally the direction of the C++ industry so I figure we're on the vanguard of hastening its use via example.
Sure, but it seems a little convoluted with the templates and such. We do a similar thing in the costmap layers using https://github.com/ros-planning/navigation2/blob/30b405c58e6d53ba8c96381416bc4679d35a1483/nav2_costmap_2d/src/layer.cpp#L78-L120 which is much more straight forward. There's not much reason to return this lambda for use, when we can just have the utility function itself just take in the node, namespace, and parameter type to grab.
Got it, yeah a little more exploration there may be warranted, if nothing else for my own education!
Sure, and that works there but in the e.g.
The affect of applying INFLATION cost as an obstacle when checking the SE2 footprint would be that trajectories in confined settings near, but not on, occupied cells would be marked as in collision rather than only having higher cost due to proximity to collision. Does that make sense? I know its a little confusing with words but a diagram helps alot Example for the smac planner: https://github.com/ros-planning/navigation2/blob/main/nav2_smac_planner/src/collision_checker.cpp#L93-L145 we have an if/else for if using the radius or SE2 footprint and we check if inflated or occupied as the threshold for collision, respectively. There are some other optimizations going on there that you don't need to do here. They are far more important for global planners that are searching hundreds of thousands of entries rather than low-thounsands. |
|
Starting to play around this afternoon. One thing I noticed is that this is licensed under MIT. This is perfectly acceptable, but I don't know if it would be possible to relicense under Apache 2.0. Apache 2.0 is what the other work in Nav2 is licensed under (with the notable exception of DWB created separately and AMCL from Player) so it would just help in trying to make things consistent. However, its really not a big deal either way, they're both very permissible. Apache just allows you, the author, to retain more patent rights should there be patents filed under the work (which would be odd, but certainly within your rights) and ask folks that fork and make modifications to retain a changelog. Pretty small stuff. My only interest is in trying to long term make all of Nav2 Apache 2.0 just for consistency's sake. But MIT is even more permissive so I wouldn't argue if you wanted to keep it that way. I didn't know if that license was selected carefully or more on a whim that it wouldn't be objectionable to change slightly. |
|
I opened a PR with today's work, let me know what you think. It's almost all just refactoring for simplicity (removing templates, moving files to src, etc) and I also updated the Controller API to meet the current ROS 2 Rolling API so I can build on my development system. I assume that's what you want from the |
|
Smart pointers are awesome without any doubt ) Approach where you lock weak ptr only when you use it definitely guarantees that while you use resource, it wouldn't be deleted. I think you better know what's more appropriate in nav2 in such case, i just tried to understand intention in case of plugins) However i think raw pointers still useful in some cases if it is guaranteed that object to which it refers remains alive. It's like you have Costmap2DROS that have getCostmap method which returns raw pointer, and this pointer is valid while LayeredCostmap inside Costmap2DROS is alive.
Got it.
It was chosen while my work at FastSense team. Now i just fork it and develop it independently. I dunno if it possible to change it without asking folks from FS. If so, i don't really care.
I took a look! That's awesome! Sure, dev branch is nice place to rolling stuffs |
Admittedly, I hate that we do that, but the API calls for I'll take care of reverting the smart pointer use today / tomorrow. Its kind of necessary anyway to deal with the |
|
During my refactor it wasn't immediately clear to me how to categorize or think about the It also seems to me like the StateModel and the MotionModel are in fact containing related content. Would it be possible to refactor these together? I'm thinking maybe of a MotionModel base class with implementations for Diff/Omni/Ackermann that contains metadata like if its holonomic and a function that can be called to project forward in time (e.g. the Would that be something you could work on? Otherwise, I finished adding smart pointers and a light refactor. I would like to have those 2 models combined and have that output file added to Tomorrow, I'll work on updating the namespaces of the files for the refactor, working on propagating loggers from the high level controller node, and adding proper headers to each of the files with copyright / ifdefs. That'll essentially be the end to the surface level refactoring and then its onto more detailed look at the API (which overall seems reasonable, only minor tweaks for copies I suspect) and adding more logging. |
In fact, I feel the same here ) Point was about memory management technique (costmap2d not passed as a smart pointer)
Sure. I'll have more time closer to the weekends to start helping with refactoring stuffs.
I had the same thoughts about it. Yeah, sure, i could try to rethink these stuffs |
|
Created PR for critics untemplating. Have problem with costmap2d passing it in Optimizer initialize method in optimizer_test.cpp. Dunno why but it fails in shared_ptr copy constructor. |
|
Yeah, I'm periodically running into build errors about things I long ago changed that are only showing up now. I haven't used conan before but perhaps its not very good at breaking the cache when required? Ran into that linking problem with the trajectory visualizer and a renamed header in the tests that only now picked up as problematic |
|
After we merge in the existing PRs to completely eliminate the templates, I'm going to typedef Can you tell me for each of those 3 what they are used to hold? That way, I can use that to name them appropriately and distribute to all files. |
Agreed, after the parameters were updated, I was seeing that this wasn't quite able to run at 20hz so if moving to another library could get us some real improvement, that would be significant. I think its worthwhile. You were reporting 50hz on a 4th gen intel CPU before tuning and after tuning I couldn't quite get 20hz reliably on a 8th gen i7 (8565U). |
|
I commented on #42 with a solution to that problem At this point, I'm a little at a loss of how I can help to continue to push this to completion. Let me know if there's something I can do to help or how we can get this over the last bit. by the way, there's a nav2 contributor that's playing with using flashlight for xtensor replacement here: https://github.com/mkolodziejczyk-piap/mppic/tree/libtorch. That wasn't on your comparison document for replacements, so maybe that's a good option. But I still agree from a purely technical perspective, I prefer Eigen. Its stable, available in binary form everywhere, and very performant. Just wanted to let you know in case something pans out of that work. He's doing this so he can use MPPI with a GPU |
|
That's nice, I didn't have time to watch the whole thing recently, i'll get to this soon.
Ok
I had thoughts to use flashlight as an tensor replacement to use gpu and trained model. Quick look on the repo, i notice branch with torch. As far as i know flashlight uses arrayfire as tensor lib. Since I'm just getting started of xtensor replacement work i could consider different alternatives (including arrayfire/torch) . But torch definitly wouldn't be much performant with CPU |
|
Hi, |
|
@mkolodziejczyk-piap |
|
I think my hope from the benchmark of tensor libraries is that eigen would be fast enough GPUs would largely not be required + Eigen is an easy to install thing and version control. If it makes most sense to use on of these other libraries, I can look into how we can manage that dependency for binary release, but im not sure its possible since I’m not sure how the build farm would build code for all the GPU vendors to be able to release the package to apt |
|
I also strongly lean towards Eigen for now. |
|
By the way, another user is going to beta test on their omni platform, so hopefully they can give us some tuned omni parameters based on the differential ones tested on hardware to take that off our agendas |
|
@mkolodziejczyk-piap i made optimizer part itself a plugin in art branch. probably you will be interested, and probably it will be helpful to not rewrite everything, but extend current implementation Still in alpha (need to rename some namespaces and classes), but works for me |
|
@SteveMacenski i don't remember if we discuss that. Can we use pragmas instead of header guards in mppi. It's really annoying without auto formatters to refactor them. I place all xtensor stuffs in optimizer/xtensor and now header guards must be two lines long |
|
i found a way to use catch2 with ament. Rewriting tests on catch2 + add catch benchmarks |
We're going to have to change them back before moving to Nav2, so I'd prefer not to remove them, but if you didn't keep updating them for now that would be fine and we can deal with the linting errors before moving into Nav2.
Make sure its not just using it, but also that the dependencies are properly managed so that it can be installed with |
https://index.ros.org/r/ament_cmake_catch2/ Catch2 itself not in 20.04 as well as xtensor, but it packaged in newer releases as far as i know |
|
What kind of rosdep issues? It should just work if you add |
rosdep can't find the package, but the package exists. I'll investigate this further |
|
By the way, I'm aiming for June 30th to get MPPI in Nav2 ideally. I have quite a bit of travel after July and other work that would need to take priority (conference talks, papers, etc) until early November. I'm just working on some scheduling today so that came as a conclusion just now |
|
Seems like major parts that left - performance optimization of reference critic and obstacle critic if possible, ackermann constraints, omni params. |
|
In the last PR we moved to 2 iterations from 1, did the optimizations for the path make that possible or just something we forgot from the parallelization work? I think other than that, the last bits are (1) smoother controls and (2) hardware testing / evaluation (maybe tuning defaults) |
|
for my machine it sometimes exceed 2 iterations so i would prefer to stay on 1 iteration till some parallelization work done |
|
Also more then one iteration make it possible to choose velocity multiple to max_v max_w. We limit velocities by sampling, so sampling twice make it possible to exceed those limits. I think if we need better trajectory at some point we could increase batch size and not iteration count |
|
OK makes sense to go back to 1 iteration for now. I think though it is crucial for this approach to use multiple iterations so we can converge to a refined solution. It very well might be that because we don't that is why the controls aren't smooth or could be wobbly when called at its 20hz (or whatever rate). If we evaluate only a single batch of any arbitrary size small or large, we basically are just finding the best among a random sampling. Repeatedly iterating over the previous best one adds the smoothing characteristics so that we converge to the best solution rather than just being in generally the right neighborhood of solutions. IMO from reading MPC and MPPI literature in the last couple of weeks, multiple iterations are pretty important to handle the random sampling and non-linearity of the system. To go from the 'general solution area' to 'the solution' needs iterations of sampling in that localized space to improve the result. The output would then probably be smoother and definitely more refined. Arguably for iterations after the first, it might even be wise to reduce the noise sampling
|
|
I ran the controller with valgrind to find the hotspots and came with the following result If you open it with kcachegrind (e.g. When Generating the noised controls and integrating the state velocities also took a higher chunk of time than I suspected (6% and 11%, respectively). When the obstacle critic was set to use the circular assumption, the entire critic trajectory scoring took about 31% of the time and When the obstacle critic is low from using the point-cost, now the path alignment critic is the single largest hog of CPU time at 21%. With circular footprint checking, MPPI can run at 20hz with 5 iterations, smaller model_dt, and a larger number of timesteps (to account for smaller dt's). We can tune with this to get the best behavior, but it would be good to find a way to reduce the cost of those collision checks. One example is https://github.com/ros-planning/navigation2/blob/main/nav2_smac_planner/src/collision_checker.cpp#L103-L108 where I check if the point cost at the center of the robot is greater than possibly inscribed and only do the full footprint checking if there's a possibility that some part of it is in collision. Else, just use the center cost since we can calculate the cost the center would be if any single part of the footprint was in collision. That should help greatly Unsurprisingly, when I ran with valgrind it was very slow, but that was illustrative actually. The MPPI controller was very wobbly / drunken which reminds me that maybe part of the wobbly issue we have is that perhaps we're running too slow? (another hypothesis to test along with critic smoother + iteration count increase) If you're curious how I did this with nav2, I wrote a tutorial about it: ros-navigation/docs.nav2.org#328 since I know someone will ask me about this someday tl;dr
|
|
Separately without I see if I push it with dt of 0.05 and 30 time steps (so still 1.5s ahead) with the same 300 batch size I could get 3 iterations within 20hz. If we dropped the batch size obviously we can play with the iteration count. So I think its worth doing a bit of optimization on the biggest offenders, but we're on the 'right side' of the minimum required performance. If we can improve the path align, full footprint critic, generating noised controls, and integrating state velocities, then I think that's the most optimization that we'd ever had to do. The footprint critic I've outlined a clear way to help that (will submit PR next week when I get a chance to implement it), with that much optimization I don't think any more will be required (but CPU will get higher when in closer proximity to obstacles since it'll need to do full footprint checks for more trajectories). The other three there aren't as clear algorithmic shortcuts we can make, but hoping you might have some ideas 😄 |
|
You did a great job, Thanks! What environment do you use for testing ? |
|
I tested on Rolling, the issues from Jammy exist there too, but I added the |
|
I think the existing open tickets cover the remaining items for a Nav2 release sufficiently now. Closing |
There are some suggested features/fixes/updates to be done to consider this controller as a part of Nav2 Stack.
Pluginize ModelNot required.PathAngleCriticandApproxReferenceTrajectoryCriticThe text was updated successfully, but these errors were encountered: