Robot Navigation with Nav2 and ROS: A Complete Guide

[00:00:00]in the last video we looked at slam as the acronym suggests the two main outputs of this algorithm are a localization estimate and a map and with this information we can start to build some autonomous capabilities into our robot the first of those is going to be navigation so that's where we specify our Target location and the robot autonomously figures out how to safely get there we're going to be using the nav2 stack for this and just like the other videos I'm going to start with a quick overview of navigation and how it works then we're going to get it up and going in gazebo then on the real robot and finally some extra tips and tricks and improvements so what is navigation we talked about this briefly in the last video but essentially navigation is where we want to plan and execute a safe trajectory from an initial pose to a Target pose and the first piece of information we're going to need to be able to do that is an accurate position estimate because if we don't actually know where we are at any point in time how can we figure out where we're trying to go slam is going to provide this position estimate for us if we didn't have slam or some other kind of localization system we could try to use odometry but that would fall apart pretty quickly it's also worth noting that this is a different kind of autonomous operation to tracking a sensed Target we'll be doing that in the next video and that doesn't necessarily require localization the second thing we need is an awareness of the obstacles that are around our robot and this information can usually come from one of two sources or sometimes both combined the first of those is a map so say we've got a system like slam that has already produced a map for us we can use that map as a basis for navigation so the same obstacles that we were using for localization before now become something to be avoided this is most straightforward with the grid slam type of map where we take our occupied cells add a bit of a buffer around them and that's our obstacles and then everything left is free space but we can also do it with other types of map alternatively if a pre-recorded map is not available or our obstacles are moving then we can generate a map on the fly using sensor data typically a lidar this will prevent our robot from bumping into things which is good but it means it can't plan its whole trajectory from the start it kind of needs to figure things out on the fly so the ideal world is where we combine both of these we have an initial map that helps us plan our trajectory and then we can use the live lidar data to update this with any new obstacles or things that change along the way all of this information will be stored in something called a cost map now it's worth being clear that the cost map is different to the slam map it might have been based on the slam map initially but its values mean different things and it's getting updated independently so the cost map assigns a cost to different areas of the environment where a high cost is something to be avoided while a low cost is a safe space to go for this simple example where we've got an obstacle that's going to be a high cost where we've got free space that's going to be a low cost and the area kind of just near the obstacles will be a medium cost we want to avoid that if we can but we can go there if we have to and navigation system will typically require other bits of information as well things like the size of the robot some of its Dynamics and so on in the last video we saw just how easy it is to get started with Slam in Ross using the slam toolbox and its creator Steve musinski is back again with another amazing package and this time it's nav2 so just like with slam toolbox we're only going to be seeing the very tip of the nav2 iceberg today it's a really powerful tool that's useful for many different Navigation tasks to start off with we've got to install nav2 now I actually did that in the last video but I'll put the commands back up on screen in case you haven't done that we have one other tool we're going to install this time though and that's twist marks so pseudo app install Ross foxy twist box before we dive too deep into nav2 we're going to do just a little bit of prep work one problem that we're going to run into is that our controller is expecting the command velocities on slash diff con command Val unstamped but nav2 is going to want to be publishing them to just commandvel now you might think that we can get in and just remap the topic and that is possible but we're going to do things a little bit of a different way this time I'll talk through a few other Alternatives at the end of the video we're going to introduce a new node called twist marks which takes a bunch of twist topics that's what command velocities are and it's going to Multiplex them into a single topic and this is useful because it allows us to do things like prioritizing certain topics and locking and blocking certain topics at different times that sort of thing we're going to set up twist marks to take the joystick velocity on command Val joy and the navigation velocity on just commandvel that way we don't need to worry about changing it right now it'll take in both combine them together and publish it to diffcon command Bell on stand so we're going to start by adding a new file called twistmarks.yaml and the parameters that I've got in here so the node is Twist marks we've got an array called topics in that we've got navigation is on command Val and we've got timeouts and priorities you don't need to worry too much about these and then joystick on command Val Joy so we'll save that so we're going to hop into our workspace and then in a new tab we can type rust to run twist marks twist marks we've got some arguments so we've got the params file which is in source tickybot one config twistmarks.yaml and then we're also going to need to remap the output topic so by default twist mux publishes to command Vel out and we want to remap that to diff font command vowel and stand we'll add this into our launch files later the other thing we need to do is modify our joystick launch file so before our teleop node was remapping command Val directly to diff count command Val unstamped now instead we're going to remap this to command Bell Joy so that's going to come unveil Joy then twist marks will be taking that and Publishing that out to Defcon command Vel unstamped if you are using stamped twists none of this is going to work and we're going to talk a bit more about that at the end so what we should be able to do now is launch up gazebo just like normal so I've also got RVs running here and I'm just going to run slam toolbox like we did in the last video and it's just loaded up the map that I had last time but otherwise we could go around drive around creating a little map for ourselves so then in a new tab all we've got to do is type ros2 launch nav2 bring up navigation.launch.pi and we'll set use same time to True like normal it'll go through do a whole bunch of things and then we should be able to add a new map set the topic to Global cost map and we can see there oh we'll set the color scheme to cost map and we can see when we hide our regular map this is the cost map that's being used for navigation so we can see the obstacles the exclusion areas and also the kind of fall off zones around them just enable the camera view as well so we can see it there now what we can do is we can see this 2D gold pose button here so I'm going to click that and I'm going to request that the robot goes to here pointing that way and you can see nav2 just does it but let's change our fixed frame to map you can see it's commanding the robot nav2 is generating the appropriate command velocities sending them to the Gazebo simulation and the robot is moving to that location and because of twist marks if I want to take over and start driving with the controller I can do that if I let go wait a sec nav 2 is going to take over again so twist marks is prioritizing the GamePad but when the gamepad's not doing anything then it'll let nav2 run so let's try going over here we'll see if it can get through this little Corridor sometimes it has trouble this last Gap is only just big enough but we'll send a pose like that and we'll see the robot should navigate through we can see it here on the camera squeezing through the cones and out the other side now it's time to test it on the real robot so we've moved back inside now just like in the last video I've connected to the pi over SSH and there I'm running the normal ros2 control stuff I'm also running twist mucks on there and the lidar driver then on the dev machine I'm running the joystick driver so that's to get our joystick commands I'm also running slim toolbox and Arvis and I've already driven around and created a little bit of a map of the room so all we should need to do to run navigation now is to open up a new tab in the dev machine and we're going to run that same navigation command that we had before only this time we're going to set use sim time to false now that that's running back in RVs all we should need to do actually let's just add our cost map just so we can check it out very good we can see there's a few stray laser points causing us some trouble but let's try and set a goal so we'll set a goal pose over here and we can see the robot autonomously navigates to that location something else we can do that's cool is we can add a new panel so we're going to add the navigation to panel down here we can also click this button up here to add a new tool type and we're going to add the nav2 goal tool and what this lets us do we can swap to Waypoint mode and using the nav2 goal button we can specify some waypoints so I might put one here and then here and then tell it to come back roughly to where it was before but facing the other way and then we can click start navigation and it should go to Waypoint one then once it gets there turn around and go to the next Waypoint and so on this is all pretty rough because we haven't tuned it very well but there's so much that can be done with nav2 now that we've seen nav2 in action in both gazebo and on the real robot we're going to look at some Alternatives and some improvements we can make to our setup we saw in the last video that once we've made a map using slam we can keep reusing that same map over and over again for localization so what if we want to use that kind of a setup with nav2 it's not too hard there's just one little trick so I've got things set up again here with just the simulation running with the joystick controls arviz and twist marks and so what we're going to do now instead of running slam toolbox I'm going to type Ross to Launch and now we're going to be running amcl we saw how to run amcl in the last video but this is going to do it a bit of a quicker way so we're going to go nerve to bring up localization launch.pi now we need to specify the map for this one so I've got the map saved in my workspace I think it's my map save Dot yaml and then we also need to set use CM time to true so that'll start up it'll load up our old map now it's asking us to set the initial pose just like last time so we've got to hop in the map frame and I'm still sitting at the origin so I can just specify that as my initial pose it should receive it and then you might remember that we need to set the durability of this topic to transient local and there we go our map should appear and we should be running amcl so this is running the map server and amcl just like we did in the last video but in one launch file so now to run navigation we'll open our other tab we'll run the same command as before but this time it's got one extra parameter and this is a flag that's going to tell nav 2 that it needs to subscribe to the map with transient local as well so it's just map subscribe transient local set that to true so that tells nav2 to start up and to listen to the map so now nav2 should be running just like before and then in our views we can now add the cost map in again so we'll add that and this is going to let us see something that we hadn't been able to clearly see before because the as we drive around the base map is no longer going to be updated because we're only running localization not slam but we should see that the cost map gets updated as we move so to start off with we can see it's built the cost map based on the slam map that's great now what we can do let's put in an extra obstacle so let's go for a construction barrel and we'll drop that just in here behind the robot we can see that it's added it there now if we hide this map we'll set the alpha to zero we can see it hasn't added anything into the localization map but it is there in the navigation cost map and so just like before we should be able to Now set the goal and move to it sorry I'm not quite sure what happened there I've just reset everything so we're going to set that goal pose again so set 2D goal pose to here and it should work again now something that we will find if I if I just take over because amcl's localization is a bit more rough we'll see that nav2 starts to put obstacles where they don't really exist and what we'll find is that pretty soon the whole map becomes a bit of a mess sometimes nav2 will clear things or just reset the whole map this isn't nav 2's fault it's just because we haven't tuned amcl properly and so the localization just isn't quite accurate if we want to start making modifications and tuning these parameters for navigation and amcl the easiest way is going to be to copy the launch and config files into our own directory similar to what we did with slam toolbox so I'm just going to type copy slash opt Ros slash foxy slash share slash nav2 bring up then we've got the params directory and we want to just copy there should be a file called nav2 params so we're going to copy all of that into source I typically got one into our config directory then we want to get the launch files so we had there was one that was just navigation.launch.pi and we're going to copy that into our launch directory and then we also had localization.launch.pi so that should add these two launch files in here now in both of them they're going to have a reference to their own package so here the bring up deal we're going to change this to the name of your package so for me that's a gigabyte one and then if we find where bring up deer is actually used you can see here it's creating a path bring up Geo params and have two params but you remember that ours is actually in a directory called config rather than params and we're going to have the same thing here in localization so we're going to change this to you bought one now this one also has a path to a Maps directory we don't have a Maps directory at the moment so that's just for setting a default map if you're using the same map often if you are doing that you can go ahead create a Maps directory I'm just going to leave that and once again change this one to config and so that means now when we run the commands we should be able to instead of having launch nav2 bring up we should be able to put a tick you bought one navigation.launch.pi and so on and so forth and now that we've done that we can treat those parameters really easily so you'll see the nav2 params this is a huge file with lots of parameters so just go ahead start taking a look at them playing with things so maybe tweaking robot radius you can start playing around with some of the inflation radius that kind of a thing just see what difference it makes while I'm here I'm also going to take a second to copy the launch the same launch file that we were using in the last one for slam toolbox so I'm just going to go in here into the slam toolbox launch directory and then we were using online async.launch.pi just so that we can modify that one in the future if we need to and it's going to have that same thing to change so we'll go to this and we'll change this get package directory to tickybot one whoop and this one's params directory is already called config so we don't need to change that one this is also a good moment to note why I couldn't get localization working properly in the last video with slam toolbox and that's because if you have a look in here I needed to actually instead of running the online async launch file I need to be running this localization launch.pi I'm not going to copy that over right now because it'll conflict with the one from nav2 but it's just been worth being aware of that it launches a different kind of Slam toolbox node to the one that online async did and that's what's needed to do localization this is also a good time to add twist marks to our launch files because we're always going to be using it every time we run so I'm going to go and find our launch Sim and I'm just going to put it here below the joystick so I've just copied it from somewhere else but you can see it is getting the params file I should substitute this for package name so params config twistmarks.yaml so it's passing that params in same time to true and then remapping command vote out to command Bell unstamped and then we just have to remember down at the end here to actually launch twist mux and then we'll do the same thing in our launch robot except this time we're not going to have some time set to true speaking of twist marks there are a couple of other Alternatives that we have rather than using twist marks to forward on that command velocity so one thing that we could do is to remap the nav2 output to be directly on diff count command Val unstamped and then it and the joystick will both just be publishing on it and it will listen to whichever one it hears now it's a bit messy to do that it's here in our navigation launch file we could add an extra remapping here for command Val or we could do it here on the actual nav 2 controller node so we could add an extra remapping in here but that gets a little bit messy especially with humble where you need to change it in multiple locations so just be a bit careful of that the other thing you can do is if you're using twist Stamper to stamp your twists we can kind of let it double as a multiplexer so here was our twist Stamper where we were remapping uh from command Val unstamped in to command Val out on divcon instead we can remap command Val in to just be command Vel and then change our joystick to publish to command valve and now the joystick and nav2 were both published to command valve twist Stamper will take it in on command Val and send it out to Defcon command Val the other alternative is we could add a relay node that just reads the nav21 and republishes it to a new topic again it's just a little bit messy but yeah a few different ways that we can do that for the first time our robot is doing something autonomously that is we can just give it a task we could walk away and let it go about its business so this is a good chance to start creating some more complex environments see what the kind of limitations are have a go at tuning it to overcome them thanks so much to Steve and the whole nav2 team for creating such an amazing package for us to use in the next video we're going to be using a different sensor for autonomy and that's the camera so we're going to use the camera to detect a particular kind of object and then steer the robot to keep tracking that object and follow it around the room make sure you don't miss out on that as always there's a link down in the description to the discussion thread over at the articulated robotics discourse and that's the best place to go to let us know any questions or comments or troubles you're having with this tutorial thanks as always to the patrons over at patreon it's only through your support that this channel is possible thanks everyone and I'll see you next time [Music]