Proposal for inclusion in Nav2 Stack

[artofnothingness]

[[ Deprecated ]] See https://github.com/artofnothingness/mppic/issues/1

There are some suggested features/fixes/updates to be done to consider this controller as a part of Nav2 Stack.

• [ ] Support for at least omni robots, ideally also ackermann, but not strictly required to start off
• [x] Footprint collision checking
• [ ] A readme with the run-time rate / other information / parameters / link to relevant paper
• [ ] Test coverage for ~90% of the software, which can included system-level tests, not just unit tests
• [x] Parameters handle lifecycle transition multiple times
• [x] Publishers use std::move
• [ ] Use of ament / colcon /package.xml for building so its easy for users

[SteveMacenski]

By the way, I've put this on my schedule for 2022 to get involved in to help move along and make improvements where I can. You can leave the ament/colcon/package.xml for me if you like. I'd like to get this in Nav2 as part of my push next year into getting the controller behavior down solid (tuning DWB for even remotely reasonable out of box behavior; adding new DWB critics; probably pluginizing this repo to use other critic types for expansion; getting this optimization approach in Nav2; adding rotate in place shim controllers; working to add dynamic obstacle information into the stack + integrated with all the controllers).

If I were to ask one thing from you on this ticket that could use your experience, I'd ask for adding support for omni and/or ackermann vehicles in the trajectory candidates. I could probably handle the rest if you could answer a few questions for me along the way.

Did you have a chance to test this in a couple more practical environments?

Also, I see that you're modifying this to work with grid maps, super cool! How are you (or are you) integrating that in with Nav2? A controller server replacement using grid maps + traversibility estimation or elevation maping? How's that working out for you? We are planning on going that route in Nav2 longer term (something more custom but based on grid_maps using lessons learned from elevation and traversibility). Frankly, I just haven't had the cycles to look into it yet as I've been focusing on getting the algorithms (planners, controllers, BT behaviors) in place first. I'd be looking at a late-2022 at the earliest to start looking into that, but any feedback / info you can give earlier about some of your requirements I can make sure to add to our list for Nav2 as well to remain compatible and perhaps use Nav2's.

As always, this is really great, well written, work and I'm really excited to have the opportunity to work with you on this and getting it integrated more closely with the core ROS ecosystem!

[SteveMacenski]

Hi! I just wanted to touch base on this! Last we spoke we were discussing around testing with other dynamic obstacles in the scene. How has that gone? Does the performance work well with people / other robots around?

I'm working on some papers right now due in early March, so I'm about 4-5 weeks from circling back to this and working on integrating it into Nav2, so its getting there and I definitely have not forgotten about this. Over the holidays I had some time to read over the papers and other implementations related to MPPI so I could hit the ground running. The only major thing is supporting Y velocity directions which is more of an engineering challenge than a robotics algorithm challenge.

A few of my big focuses this year are on out of the box behavior & local trajectory controllers. I'll be able to spend some real time on this.

[artofnothingness]

@SteveMacenski I'm sorry for such a late reply.
I'm no longer mantaining this repo, and i do not participate in 2.5D project anymore. I could continue to work on MPPIC here https://github.com/artofnothingness/mppic, and i would appreciate any help (especially in ament/colcon/package.xml) =)

adding new DWB critics; probably pluginizing this repo to use other critic types for expansion;

That would be nice

Did you have a chance to test this in a couple more practical environments?

It is unlikely that in the recent future I could make tests on a real robot. But i could make some experiments in Gazebo.

Also, I see that you're modifying this to work with grid maps, super cool! How are you (or are you) integrating that in with Nav2? A controller server replacement using grid maps + traversibility estimation or elevation maping? How's that working out for you?

In current state grid_map hardcoded to the specific algo (MPPI), and tested with some critic functions that use grid_map representation filled with test data. Based on my observations grid map is fine as controller server parametrized environment representation or costmap replacement, but it's hard to make it runtime generic with costmap. Traversability estimation from elevation maps may vary for the specific algorithms. Elevation mapping more like cosmap layer for grid maps.

As always, this is really great, well written, work and I'm really excited to have the opportunity to work with you on this and getting it integrated more closely with the core ROS ecosystem!

Thanks! As i said, i could continue work on it, but in another repo.

Can't say much about part of the work related to dynamic obstacles now.

The only major thing is supporting Y velocity directions which is more of an engineering challenge than a robotics algorithm challenge.

Ackerman probably would work without any change, but i have some doubts that MPPI with omni would. Need to do some tests, And implementation need some arch rework with dimensions for Y. I'll back to this later.

[SteveMacenski]

I'm no longer mantaining this repo, and i do not participate in 2.5D project anymore.

Mostly for my clarity, what's going on with this repo then and why aren't you doing it over here anymore?

Good to know on the grid_maps stuff, its not super important right now for Nav2's purpose, but always good to know what folks are doing with non-flat environments. We're eventually going to get there in Nav2.

For ackermann, I think we just need the motion generators to follow curvature constraints. I think right now those aren't really applied which is fine for a differential drive robot, but obviously an ackermann has limits. Is there an easy way to add those minimum turning radius constraints? applyControlConstraints seems like a good place or including in the model_.

I think the biggest thing is that rework for including Y directions for omni support. I'm not sure how deep the rearchitecture would be. Practically speaking, I think it would mostly just involve adding a Y field to everything (that for now is just 0) and then make the trajectory generator provide Y components as well later on.

I've been chatting with folks behind some of the DWA-based controllers in ROS and I think that MPPI might be a good default replacement. We just need to make sure we have at least as much support for robot types as that does (e.g. omni & diff) before I can deprecate DWB.

[artofnothingness]

Mostly for my clarity, what's going on with this repo then and why aren't you doing it over here anymore?

As far as i know, the academic part of the team still work on 2.5d project, but focuses of the other's has changed, mine as well. So after some discussions we decided that it would be better to me to left FastSense team.

For ackermann, I think we just need the motion generators to follow curvature constraints. I think right now those aren't really applied which is fine for a differential drive robot, but obviously an ackermann has limits. Is there an easy way to add those minimum turning radius constraints? applyControlConstraints seems like a good place or including in the model_

Hmm. All trajectories come from integrating random controls by n time steps. One way is to add critiс to already integrated trajectories to penalize based on the curvature constraints, other is to add some constraints on control actions, and may be change averaging policy. Don't know how to implement second approach, the first one seems doable.

I think the biggest thing is that rework for including Y directions for omni support. I'm not sure how deep the rearchitecture would be. Practically speaking, I think it would mostly just involve adding a Y field to everything (that for now is just 0) and then make the trajectory generator provide Y components as well later on.

It requires a little more work coz the main data structures are tensors, and not structs/classes. I'm on the way abstracting away from these nasty tensors in some parts, to make OMNI integration more smooth.

I've recreated topic here https://github.com/artofnothingness/mppic/issues/1 So we could continue discussion there.

[artofnothingness]

[[ Deprecated ]] See https://github.com/artofnothingness/mppic/issues/1