In this video, we're going to see how we can do SLAM on our robot. So we're going to use the lidar on the robot along with the ROS `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 Nav2 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 SLAM, 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 ROS.

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.

SLAM is an acronym, and it stands for Simultaneous Localization and Mapping. So what are 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 in ROS, 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 RViz 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 drive controller using the wheel odometry.

When our robot first starts up, it's at 0, 0.

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 `base_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 RViz, `base_link` is only at 1.9 meters ahead of `odom`.

This 100 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 ROS 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 Odom 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 troubles, 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 Z, 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 XY 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 ROS 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 Macenski. Now Steve has contributed a number of great packages to the ROS 2 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 `/opt/ros/foxy/share/slam_toolbox/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 Odom 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 RViz. 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 RViz 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 RViz 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 `ros2 launch slam_toolbox` and we can see what launch files we've got here.

And we want `online_async.launch.py`. We'll specify the `params` file, so that's a parameter for this launch file, and that'll be `source/articubot_one/config/mapper_params_online_async.

yaml`. 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 RViz is we're going to add a map.

I'm going to set the topic to `/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 in 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 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 RViz plugin. So we can go add new panel, `slam_toolbox` plugin, 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.

Easy SLAM with ROS using slam_toolbox

Instructional and entertaining videos on the subject of robotics. Including theory, practice, and using ROS.

Easy SLAM with ROS using slam_toolbox

Easy SLAM with ROS using slam_toolbox

Scripts