Easy SLAM with ROS using slam_toolbox

in this video we're going to see how we can do slam on our robot so we're going to use the lighter on the robot along with the Ross slam toolbox to generate a map of the environment around the robot and then figure out the robot's position within that map then in the next video we're going to be able to reuse that map with the nav 2 stack so it can use the map to figure out where any obstacles are and autonomously navigate a safe path towards a goal if you're familiar with slim you'll be aware that it comes in many different shapes and forms that use many different sensors but today we're going to be focusing on grid slam with a 2d lidar so I'm going to start with a quick overview of what slam is and how it works then we'll look at how slam integrates into Ross then we'll learn about the slam toolbox and we'll get that up and going using gazebo and then finally we'll run slam on our actual robot slim is an acronym and it stands for simultaneous localization and mapping so water localization and mapping well let's start with mapping imagine you want to make a map of your street you've got a phone on you so that you can track your GPS location and as you walk along the street you take note of what you see orange House on the Left greenhouse on the right turn the corner Red Roof on the right white fence on the left and so on once you're finished you'll have a nice little map of your street now that you've got your map you can use it for localization let's say your phone battery runs out so you've got no GPS and you need to figure out where you are you can see a white fence on your right and a Red Roof further up on the left using the map you'll be able to pretty accurately pinpoint your location wherever you are you've found your location in the global coordinate system you've localized it's worth pointing out here that localization isn't navigation navigation also uses our map but that's where we're calculating a path to reach an objective like how do I get to the orange house via the house with a red roof and planning a safe path to follow through the environment Advanced navigation may involve dynamically updating the path as new or moving obstacles are detected we'll be covering navigation in the next tutorial when we look at the nav2 stack the problem with the approach we just saw is that we needed that GPS on our phone in the first place to make an accurate map but sometimes we might only have a GPS coordinate for our starting position or no GPS at all in that case we need to do slam we need to simultaneously localize and map from our starting position we might be able to see the orange house and the greenhouse and as we walk we'll keep an eye on where they are and consequently where we are compared to them we could also use our stride length to help us with this position estimate then whenever we see a new object we know where it is because we know where we are and it all just goes on continuously congratulations we just slammed and the result may not be as accurate as with the GPS but it's much better than having no map at all slam comes in many different forms with many different algorithms but often we can categorize most slam methods into one of two categories there's feature or Landmark slam and grid slam so what we saw just then that was feature slam so our features are our landmarks for things like a Red Roof or a white fence grid slam on the other hand is where we divide the world that we're seeing up into a series of cells and each cell can either be occupied or unoccupied or somewhere in between slam toolbox that we're going to be using today is a grid map based approach before we start using slam and rosto we need to take a moment to understand some of the coordinate frame conventions this is some stuff I probably should have covered earlier in the series but you know better late than never so to start off with we know that the frame attached to our robot is called base link up until this point when we've driven the robot around we've set our fixed frame in Arvest to something called Odom in the absence of any other reference Odom represents our world origin and the transform from Odom to base link is calculated by the differential Drve controller using the wheel odometry when our robot first starts up it's at zero zero the two frames are on top of each other as the robot drives around base link will move compared to Odom now here's the important bit the motion of bass link relative to Odom may not be exactly correct but it will be smooth what I mean is it's possible that you drive the robot forward in real life by two meters but when you check RVs bass link is only at 1. 9 meters ahead of Odon this hundred mil error did not suddenly appear it built up over time as the odometry drifted you could take any small section of that trajectory and it would be nice and smooth and basically correct but it just slowly got away from the truth this is because odometry is effectively a measurement of the robot's velocity even if it's technically measuring wheel angular position this velocity is then integrated smoothly over time to produce the position transform estimate let's say we now introduce a system that can correct for this drift like GPS or slam because these systems directly measure the robot position if we were to use them to update the Odom to base link transform this could cause the robot to jump around now this might not be a problem but some algorithms won't like it instead we want to introduce a new frame the map frame and we can now Express the location of Base link compared to that so we can have the base link compared to the Odom frame or we can have the base link compared to the map frame this gives us a new problem though which is that a frame in Ross can only have one parent the way we get around this is that the code will need to take the position estimate from slam or GPS or whatever along with the current Odeon to base link transform and calculate the appropriate map to Odom transform this is a bit weird since the map to Odom transform is kind of meaningless out of context but it means that we can get the base link pose relative to either reference point with no trebles and it should move smoothly but drift compared to Odom and jump around but stay generally correct over time compared to map as well as the Odom and map frames you'll also find Odom and map topics which contain the data about the odometry and the map the Odom topic contains basically the same position information as the Odom to base link transform but it also contains the current velocity and the associated covariances the map topic contains the actual occupancy data for the grid map so that it can be shared to other nodes and we'll be using this one later in this tutorial one other thing to be aware of is that in addition to the base link frame some slam systems also like you to have a base footprint frame and that's because some robots can move up and down in 3D the base link will move up around in Zed but we want to treat the slam problem in this case as a 2d slam problem so the base footprint frame is kind of like the shadow of the base link frame on the ground so it's stuck to the X Y plane Z equals zero it's stuck on the ground and it'll move around underneath the base link and be used for slam one last note on phrasing I'll sometimes use the word pose which refers to the robot's position its location and its orientation what angle it's at and the pose is pretty much equivalent to the robot's transform frame all of this coordinate frame stuff can be super confusing and hopefully it'll start to make a little bit more sense once we put it into practice on our robot if you do want to find out more about it you can find it in the Ross standards particularly rep 105 and also a little bit in rep 120. for this tutorial we're going to be using the Fantastic slam toolbox package by Steve masenski now Steve has contributed a number of great packages to the ross2 ecosystem particularly slam toolbox and also nav 2 that we'll be looking at in the next video before we install it though I'm going to go ahead and add an extra link to our robot urdf and that's that base footprint oh this one that's uh the base footprint that I talked about just before so if we go up to here to our base link I'm just going to add it here just under the base link I've already got it copied and we can see it's the base footprint link the name is just base footprint and it's attached to the base link rigidly with no offsets or anything so it's going to be just sitting exactly on top of the base link because our robot is just sitting on the ground anyway so I'm going to save that and then I'm going to go ahead and install slam toolbox so sudo apt install Ros foxy slam toolbox slam toolbox has a few different modes that it can run in and a lot of different options now the mode that we're going to be using today is called online asynchronous so online means we're running it live rather than running it through some previously logged data and asynchronous means that we don't care about processing absolutely every scan instead say that our processing rate is a bit slower than the scan rate we just always want to be processing the latest scan even if that means we miss one occasionally now to make things a bit easier for us slam toolbox comes with some launch files and params files that help us set the options so I'm going to take one of those params files and actually copy it into our local directory so I'm going to copy slash opt Ross foxy slash share slash slam toolbox slash config and now you can see there's a few different options here I want mapper params online async and I'm going to copy them into my workspace into my config directory and now if we swap back to vs code and look at our config directory we can see the file here so this should be all the parameters that we might want to change for slam toolbox now it looks pretty scary we're actually only going to change a couple of these in this video but the rest are there for you to play around with and see how it works and to start off with we're going to be running in mapping mode like we see here and we can see that it's going to use the Odon frame as Odom the map frame as map and the base frame as base footprint and the scanner topic as scan so everything as we've already got it set up so I'm going to hop into our workspace I'm just going to build it with colcon just in case anything's changed and Source the workspace and I'm going to launch our simulation and I'll also launch Avis we should see our robot come up like normal and if I just drive it around for a little bit and what I'm going to do is here in gazebo I'm going to try and come straight back onto the origin and what we'll probably find in Avis is that I'm actually shifted a bit from where I started now to be honest that's that's pretty good I've actually ended up quite close maybe our gazebo odometry is a bit better than it would be in real life but usually you'd find that the robot at this point has drifted away a little bit from where you started so what I'm going to do now is I'm going to restart the Gazebo simulation to put our robot back at the origin I'm restarting RVs as well you really shouldn't need to but I find that sometimes it just plays up and gets stuck in a bit of a state so we'll rerun it all all right there's our robot back at the origin and now what I'm going to do in a new tab I'm going to type Ross to Launch slam toolbox and we can see what launch files we've got here and we want online async dot launch dot pi we'll specify the params file so that's a parameter for this launch file and that'll be Source slash a tickybot one config mapper params online async and then we can also set use sim time to true and that should pass it down to everything underneath so that should be running slam now and what we can do back in our viz is we're going to add a map I'm going to set the topic to slash map and hopefully what we'll see is our map appears so this is the area around the robot and the Rays that it's casting now I'm going to go ahead and change the fixed frame to the map frame and what that's going to mean is that the robot can jump around and the map is going to stay steady if I left it at Odom the robot would be moving smoothly but sometimes the map could jump around it's the same thing it's just a matter of perspective so now with the map frame fixed I can start driving the robot around and we'll see that as we drive it around it starts to generate a map of what it's seeing and you can go ahead and tweak the parameters in the config file to change this as I move out of bounds it should just grow the map to contain the new area that I'm in and sometimes it might look like things are a little bit off and this is where it helps to swap your RV's view to top down orthographic so this is a 2d view I can't rotate in 3D anymore but we should find that everything stays in roughly the right spot with no perspective errors and so if I keep driving and get back to the origin now what we'll see is that if I get back to the map origin which is here I'd expect to be very close to my origin and gazebo but if we open up the transform tree and show the names you can see that the Odom frame has drifted a little bit so that distance there which is I don't know maybe uh 200 mil that way 400 mil that way that's how much the Odom has drifted compared to the ground truth and we've used the map to keep us locked in to where the robot really is once we've made that map we might want to save it to use in the future now if I open a new tab and write ros2 service list we should see that slam toolbox offers us a whole bunch of services for things like saving map and serializing map but to make that a little bit easier it also comes with an rvis plugin so we can go add new panel slam toolbox plug-in and we see all these options here and in particular the save map and serialize map buttons now this is really confusing but as I understand it save map is to kind of save in the old format that Maps use so you want to do that if you want to pass it into some kind of external system that's using the map so serializing the map is if you want to save it to reuse with slam toolbox either to localize with it or say you want to modify the map in the future so I'm going to actually save in both formats and I'm going to call it my map save and my map serial there's nothing wrong with giving them the same name because they'll have different file extensions I'm just doing it to make it clear for the tutorial so I'll hit save map and then serialize map and there's no kind of prompt or anything but if I open up the file browser and go into our workspace we can see here we've got my map save.

pgm and look at that we even get a little preview of the map there my mapsave. yaml so that's the old format and then the serialized ones is dot data and Dot pose graph now that we've saved our map we can try to reuse it for localization but first I'm actually just going to clear the map so that we can see that it is actually loading it and not just keeping it open from before so I've just rerun slam toolbox and if we give it a moment this should clear all right so I've just rerun it and cleared the map so that you can see that it's all been reset so we're going to go back into our parameters now we're going to change mapping to be localization for the map file name I'm going to write the path to our file but without the extension so it's this my map serial one so it'll be slash home it'll be slash home slash Dev slash devws slash my map serial and then I'm going to tell it to start at Doc so I think that's telling it to start at What's called the first node of the pose graph what that should mean is that it's starting at the same place that you started when you made the map the first time you can also use the start pose to specify a specific pose in space to do it but we're going to go start at Doc localization all right all that looks good so now I'm going to rerun slim toolbox and you can see it's pre-loaded back up our old map and if I drive around we should be localizing just like we were before so you can see the Odom frame there that's jumping around a little bit our robot would also be subtly jumping but it's hard to tell um and I can start driving around if I start driving a bit faster we should see the jumps a little bit more obviously now the odd thing you will notice is that we do seem to sometimes be creating new maps at first I thought this was just kind of part of how the pose graph based system works but the more I looked at it the more it seemed to actually be doing mapping so I'm not 100 sure what's going on there but the point is that we can reload an old map and localize against it once we've generated our map with slam toolbox we can actually use that map with other localization systems which might be better for different situations one of those systems that's particularly popular is amcl which stands for adaptive Monte Carlo localization now the standard ros2 implementation of amcl is actually part of the nav2 stack so even though we're going to really be doing a deep dive on nav2 in the next video I'm going to go ahead and install it now so here's the command for installing nav2 I've just copied and pasted this straight from the getting started guide note that if you're on humble the command is slightly different and you probably don't need the turtlebot 3 stuff to start with but if you're going to go ahead and do the nav2 tutorials it's going to be helpful so I'm going to go ahead and install that actually running the nav2 amcl is a little bit clunky and so it comes with a launch file which is very helpful and we're going to use that in the next video but this time we're going to do things a long way it's a little bit more difficult but it'll help us to understand things a bit better so the first step of this is that we're going to run a map server so that's a node that's going to take that map file that we've saved and make that available on the map topic so to run that I'm going to type ros2 run nav to map server and we want to run the map server node now we've got some arguments for this so the first argument is going to be the yaml file name and that's going to be the path to the map yaml file now it's actually in the current directory we're in so I think it was my map save.