For Robotics Teachers: ROS2 EKF Sensor Fusion with Wheel & Laser Odometry | Nav2 Tutorial

[00:00:01][music] [music] [music] [music] >> Hello everyone. Welcome to today's open class. I'm Desire and in today's open class what we're going to be doing is we are going to be improving on the stuff that we have been working on. So, we have been mostly trying to deal with the fact that automated especially when you uh when you are working in the real environment, right? So, maybe your wheels are slipping or you have some other variables that are bringing error into your odometry, right? So, in the previous open class what we did, if you've been following them in order, is we developed a system whereby we we can test for odometry drift or we can test for any inaccuracies in our odometry that we have in the first bots, right? So, first bot one, two, and three that we find in the bot box warehouse. So, now that we have that groundwork laid, right? We can basically go and check to see is our odometry working correctly?

[00:02:18]Um how is it affected by wheel slip, for example, right? Having known those things, we want to go a step further and find a way of addressing that issue, right? So, we want to see how can we improve odometry? And what we're going to do in this open class is we are going to uh use a particular form of odometry that's slightly different from the default one that we've been using. So, i.e. we've been using wheel encoders as the source of odometry for uh the first bots. So, first bot one, two, and three, uh we've been relying on wheel encoders, right? So, in this open class, what we want to do is to introduce another type of odometry that is um that uses what is called a lidar scan matcher, right? So, what it does is that it looks for features in your environment. So, as your robot moves, right? Uh the changes in the lidar that's coming uh through the lidar scan topic, right? Can allow our robot to predict how much it has moved from its initial position, right?

[00:03:24]So, what we want to do is to find a way of combining these two odometries. So, the odometry from the lidar scan matcher to the odometry that's coming from the wheel encoders, right? So, the only uh issue with relying on on odometry that is based on uh matching lidar or changes in lidar as the robot moves is that when you have got a an environment that is uh either symmetrical or that is very little changes in uh obstacles or features, right? Lidar scan matcher can start to struggle to predict the location of the robot because it use it uses uh what we can call uniqueness of features, right? So, when that is lacking, a lidar scan matcher cannot uh do a good job on giving you the odometry of your robot. So, what you're going to see is that combining these two uh will give us a a better odometry, right?

[00:04:23]And it will also protect us from the problem of wheel slipping that we've been noticing in the previous open classes, right? So, what I'm going to do is I'm going to switch to my uh screen here, and then we can start step by step to uh do the ROS jack that we uh that has been created for this class, right? So, I'm going to go to the to our website, and we can then start doing it step by step. So, as always, you can find open classes here.

[00:04:53]Right on open classes. So, once this open class becomes available for you, you will find it under open classes, right? And there are some past open classes that uh would be in this listing that have been recorded previously, right? And this open class will also appear on the top left here, right? So, if you want, you can also have a look at other open classes that we've done in case some things are not clear to you, or you want to uh refresh your memory on some basics of navigation and odometry, for example, you can find other open classes, right?

[00:05:29]And maybe if sensor fusion is a bit missing, or you're struggling to understand that, you can also refer to this open class that we've got here. So, sensor fusion in ROS 2, right? And you understand why we will, at the end of the day, have to use what is called an EKF filter to combine the odometry that is coming from the wheel encoders with that that is coming from the lighter scan measure.

[00:05:54]All right, so what I'm going to do is because the raw stick for this open class already have a copy in my account. I'm going to go on develop here. And then what I'll do is I'll go on a section under my raw stick and then I'll open this open class it says fusing wheel wheel and lighter scan odometry, right?

[00:06:11]I'll click open.

[00:06:12]And then we'll start our open class is is always, right? So let's allow the open class to to load and let me check if we're still together here quickly.

[00:06:23]All right, yeah, we are together. So I'll allow it to load and then we'll continue.

[00:06:29]All right, now that our project has loaded successfully. So what we're going to do is let's just go through the steps that we're going to follow. So what we're going to do is we're going to do two exercises, right? So the first exercise that we will do is one that involves downloading the package, right? That's called Let me check it for you. So that's called RF2O lighter odometry, right? So it's um What it means is it means range to odometry, right? So or range flow to odometry. So what it basically means in basic terms is that when your robot moves, right? And it realizes that there is a change in lighter, right? There this package is responsible for doing a a calculation that is that allows the robot to determine how much it has moved, right? Relative to the change in the the lighter that it that it is receiving, right? So if you think about it in in terms of features, right? If the first incoming sensor data or lighter data that points to a particular corner, right? Will obviously change. So, that range will change as the robot moves towards the corner, right? So, that is a basically the prediction that is used to determine how much the robot has moved, and by using different reference features, right? That's why it's also vulnerable when there is just a wall without any unique features to say that the robot has moved so much distance from maybe its starting position to the next position, right? Because maybe it's just a plain corridor, for example, right? So, in that case, then the filter that we're going to develop, or the uh Ekman common filter that we're going to develop, right? Extended common filter, I mean. This one here will combine the two forms of odometry, right? So, wheel odometry and uh lighter odometry, so that when one is weak, the other one gets carried more weight, right? For example, in a featureless environment like a corridor, uh the wheel odometry must carry more weight, or the encoder odometry that's coming from the wheels must carry more weight relative to the um range flow um odometry, or lighter based odometry, right? So, so, what we're going to do is let's go through these steps, right? So, this exercise will create a package that we just call robust odometry, right? And in there, we'll do the normal stuff, which means we'll create a launch file, a config file, and you'll see why we require a config file in that, right?

[00:09:16]And what we're going to do is we're going to clone this um uh RF2O odometry package, right? The reason we chose this one relative to other um range to odometry packages is that the most common one, for example, the one that's used with open RMF, for example, uses a package that's called CSM, right? And that requires us to clone more than a one driver, for example, right? So, the nice thing about RF2O laser odometry is that it is a self-contained package. So, in the event that you want to deploy it on your fast bots, right? Your robots themselves, you only have to clone this one package.

[00:09:54]And that's all you have to do, right?

[00:09:56]They It's the idea is to minimize dependencies. So, what we're going to do is after that, we're going to create normal launch file that we call laser odometry, right? The reason why we want to do this first is that we want to compare, right? So, we're going to put a odometry from encoders and odometry from this package that we call RF2O, right?

[00:10:19]And we want to compare the variation, right? Of the the predicted a orientation or predicted change in the robot's orientation and position, right? And then see which one is more accurate or which one is complementary under what circumstances, for example, if the robot is rotating and now that we know that there's a vulnerability of wheel slip and a it bringing or causing a lot of odometry drift, right? We want to see when one odometry is more vulnerable than the other and when is it very, very important to have a secondary form of odometry, especially in the case that our robot is operating in the real environment where we know that wheel slip is actually a big issue, right? So, we're going to do that and this is the first exercise, right? So, we'll build and then I'll show do a brief RViz demo so that you can be able to see these changes, right? Using odometry arrows, right? Now, the next thing that we're going to do is on exercise two, right?

[00:11:24]We are going to combine these two odometries, right? So, we're going to combine wheel odometry and I E odometry from wheel encoders and odometry from um the range to uh let's just say RF2O, right? We are going to combine the two and publish a topic that's called filtered odometry. And what we'll do is we'll now combine the three, right? So, we'll combine wheel encoder odometry, we'll combine uh um lidar scan based odometry, and we'll combine that to the combined version of both of them, right? So, and will you you also get to notice how we do this uh process of configuring an extended Kalman filter so that we can combine uh these two topics, right?

[00:12:09]Okay, so let's go. I'm just going to go through the steps of the second exercise so that we have a general idea of the things that we're going to do so that we can be um following step-by-step and we can make our classes brief but also as clear as possible, all right? So, let's go to that step.

[00:12:29]And I'll explain again just a little bit. So, in this one we're going to create an EKF um filter or we're going to configure an EKF um filter using a a configuration file we call EKF.yaml, right? So, this is where we're going to put all the configurations for the extended Kalman filter, right? And the nice thing about the extended Kalman filter is that you don't have to tune the weights yourself, right? The Kalman filter can be able to uh deduce the cases where the range odometry is not working correctly and then it puts more weight on the wheel encoder odometry. And in cases where wheel encoder odometry is failing, for example, when the robot is trying to rotate in place, right? That's when we know that there's a lot of wheel slip, right? Then the extended Kalman filter will put more weight on the on the range uh odometry, right? So, the odometry of the lidar that is using lidar scan data is the one that's going to carry more weight, right? So, you're going to see this even in the demo, right? So, what I'm going to do is let me go back to the first exercise, then we'll do it together slowly, and I'll be explaining some of the key concepts, especially um on configuration and the things that you're going to be doing. So, as always, I will go to our terminal.

[00:13:46]And then in this terminal, I will remove the packages that I've created so that we create it from from scratch together, and then we try to get it to work, right? That's the whole point. So, let's go and do that really quickly. So, I will navigate to the ROS 2 workspace here, and what I'll do is uh let's check.

[00:14:08]Okay.

[00:14:10]And in here, we have the build, install, log, and SRC directory. So, what I'm going to do is remove everything and recreate the SRC directory empty, and then we start uh from scratch as as as always, right?

[00:14:23]So, let's do that quickly, and then we can click on the next step.

[00:14:32]So, we remove the build, install, log.

[00:14:35]In fact, we could have just uh given it a wild card, but no worries. And then now, we have an empty package completely, so let's clear. And then what we're going to do is let's remake a mk dir. Okay.

[00:14:50]Let's see if it allows us to do that.

[00:14:53]So, okay.

[00:14:58]So, we've made a directory that we call SRC. So, this is the directory that will contain a the our packages as always, right? So, this is typically this is how you get your workspace, right? You get an empty SRC folder, and in that SRC folder, that's where you're supposed to put um your your packages, right? So, let me continue, right? So, what I'll do is I'll minimize this so that we have a bit more space here to work with and then we'll be following our notebook on that side. So, we just have this uh graphical visualization tool here open because we would want to configure our views when we when everything is set correctly, right? So, let's uh let's do that quickly here.

[00:15:42]All right. So, now that we have uh created the SRC directory, let's check our steps one more time. So, we have to navigate to that and then create a Python package, right? So, you see here it specifies our main Python and then we specify that our dependencies are we want the RCL uh.py, so the client library for Python, and geometry messages, nav messages, and sensor messages, right?

[00:16:07]Um So, let's copy this command here and paste it here, right? And then eventually you should see that we'd have our a robust odometry package created, right? So, let me just hit enter time and and let's give it time. All right.

[00:16:29]And okay, let me check with another terminal here quickly.

[00:16:33]All right.

[00:16:37]All right. So, we see that our package has been created, so let's uh verify in this other terminal. Okay, that's cool.

[00:16:43]Now, uh let's clear this and then we got the next step. So, the next step is what we want to do is we want to create within the uh the package itself, right? Uh that we call robust odometry, we want to create a launch directory and the config directory, right? So, that's where we'll put our launch files and our config files. So, I'm going to navigate into that package and let's now paste this command here to create the launch In fact, let's just do this, right? So, we just want to copy this part.

[00:17:19]And then we create those directories that we need, right? So, you see now we've got our config and then we've got our launch launch directory, right? Okay, so now let's get the next step here. Okay, so the next step is we have to clone this RF2O light laser odometry package, right? And then after cloning it, we have to build, right? We have to first create a launch file for ourselves, right? So, that we don't use the default one.

[00:17:52]The reason why we create one for ourselves is because we actually know that we where is the We're trying to reconfigure the topics on which the the light has gone the laser odometry package should receive, right? And on which topics should the new odometry be published, right? That's why we have to create our own. In this case here, you see that we published TF is set to false because the reason is what we're doing at the moment is we're simply trying to configure this and odometry frame is already being published by um the robot's drives, right? Or in this case odometry is already coming from the simulation, right? The other reason why we don't set this to true is because we don't want to have conflicts, right? Because at this moment that responsibility of publishing the curated odometry or the most accurate odometry will be done by the EKF filter, right? So, in this case we're saying we're just configuring this light as can um odometry or this package to publish to us a different topic which we call odometry RF2O, right? Or fast foot one underscore I mean first word underscore one Odom a RF2O, right?

[00:19:18]Why? Because we want to visualize this.

[00:19:21]Otherwise, in some cases the most important thing why you'd have this published onto this topic is because you want to combine it eventually with odometry that's coming from a wheel encoders, right? And then only then will you now allow or configure your EKF to publish odometry because that's the most accurate odometry you're getting and then the node that's publishing or the node that is responsible for deciding what is the most accurate odometry is the one that you give the power to publish the Odom to base link frame, right? Because it doesn't make sense for you to say, "Okay, I want my RF2O laser scan odometry package to publish the absolute odometry i.e. setting this to true." Because when you do that, you are saying that all the other forms of odometry that are coming are not relevant. You're not giving them power to correct, right, the discrepancies of other forms of odometry in this case wheel encoder odometry for example, right? So, I will I will speak about this point a bit more. Uh but I was just trying to preempt to that idea, okay?

[00:20:26]So, let's see if we're still together here. Okay, good. We're still together, right? So, let's go to the next step, right? So, the next step was having to clone this, right? So, we have to go into our SRC directory big, right?

[00:20:42]So, I will navigate there and I'm just going to copy this command as a whole, right? Because I'm already there, the first execution will not do anything, right? And then we hit enter, right? So, now let's allow our ROS yet to clone this repo.

[00:20:57]And then we can we can check if we still have it, right?

[00:21:02]So, here you see now we have created RF2O, right?

[00:21:07]If you want to have a look at this, you can just click on that a Git repo and then you can come here and read a bit more about it and you can also read the paper about a that a this RF2O automatic, right? This will enlighten and deepen your understanding about a what a range to odometry means, right?

[00:21:29]So, I highly encourage you to go have a look at this and then you will be able to give a bit more context to your students, especially with respect to how this is working, right? And how to convey it to the kind of odometry that you get from a wheel and gutter, for example. So, let I will close this really quickly and then we'll go back to our a a packages here. So, now that we've done that, so within the robust odometry, right? So, this package here, we're going to create a light laser odome launch file, right? So, remember the whole aspect of this is that we're using a different sensor, right? Or we're using a different system to get odometry. So, in this case, laser odome is very um relevant. It's a very descriptive name about what form of odometry we are getting from this. So, let's a create that in the launch directory.

[00:22:23]A note, we are using an absolute path, right? So, I can paste this anywhere and it will be created in the correct file, right? I e. the launch file, right? So, if we navigate to robust odometry launch and we check, we should be able to see that we have got a laser odometry, right? So, that's good.

[00:22:41]Now, let's go to the next step here.

[00:22:43]So, the next step is a we have to populate the this file, right? So, we we're going to a put the code for that file in there, right? And then I'll just maybe ever explain it a little bit. I I know I did it when we were looking at what does it mean to pub to say publish a the odometry base link frame true or false, right?

[00:23:08]Um But we just have a look at it in case we need uh other things to be explained there, all right? So in this uh launch folder here we've got laser li- laser odometry and then we paste our code here, right? So yeah, basically this is the most important part. So there's a node that's called uh RF2 laser odometry, right? Or laser odometry. And then there will node is called uh RF2 or laser odometry node, right?

[00:23:38]That's the one node that you are configuring or you're launching with this, right? And then that is the one that receives the laser scan uh data [clears throat] via this fast boat underscore one forward slash scan. If you want to do this for fast boat two for example, then you'd have to launch a node that does this for fast boat two, right? And then this topic here would also be slightly different, right? So depending on how many um odometries you want to a how many robots you're doing this for, right? Okay, so now that we've done that, so in by the way, this is also very important, right? You have to understand the structure of your TF. And if you remember from the previous classes, the way you visualize or you you you look at the TF of your robot is you type here so in any way, in whichever direction, right? I normally want to do it in the um in this direction here in the ROS 2 workspace, right? So you can type ROS 2 uh TF2 tools. Uh so it has to be ROS 2 run.

[00:24:44]Right? Because you're launching an executable directly, you're not uh you're running an executable directly, you're not launching anything, right?

[00:24:51]And view frames, right? So what this will do is it will look a into the structure of of the simulation in this case. If you're connected to the real robot, it will do this for the real robot as well, right? Um so so that you get the names of what the frame IDs are, right? And then so let's allow this to finish and then we'll open that really quickly. Okay, so now it's done. A So it's generated two files.

[00:25:17]One of them is a PDF file. That's the one we rely on most of the time because it's easier to understand. The other one is got um connections, right? So it's not very useful uh for humans at least.

[00:25:28]Unless you want to pass it yourself by eye, it's going to take you some time, but it will help, right? So if you look here, uh currently we are dealing with first word one, right? So these are the frame names, right? So in the event that you don't know what frame names um or the frame names of your robots, right? Um let's say maybe you're working on a system that's pre-developed for you, you can check using the TF2 tools uh to visualize the frames and then get the frame names, which you will need to know to configure this node, right? You can see here we've got first word underscore one base link. That's the base link frame ID, right? Or the base frame ID, so here.

[00:26:08]And then you have got uh base foot footprint, so it's just a projection to the floor of this frame, right? But normally for most tasks we use this, right? And then you have your first word underscore one odum, very important, right?

[00:26:20]Anyways, so now that we're speaking about that launch file, let's go to the next step.

[00:26:25]So the next step is we want to a now allow this to launch, right? So we want to make available the launch files and the configuration files that we've just uh created, right? And in this case we're just mostly a installing the launch files and the config files, right? So to do that we have to modify the setup to pi, so here.

[00:26:47]And what we're going to do is I will just copy these lines here so that we ensure that our config as well as the launch files are installed, right? So, they're made available for execution, right? In this what we've done here, we haven't added the anything to the config directory. However, we will do that.

[00:27:09]That's why we want to ensure that whatever configurations we have in there will also be installed and made available for us or for package, right?

[00:27:18]So, that's why we giving we're just combining all that data file installation directive, right? So, I'll copy this here.

[00:27:27]And then we'll put it here where from line 13 to line from line number 10 to line 13, right?

[00:27:35]In fact, let's do yes, line number nine and then we use control V here. So, what I'll do is let's correct indentation here quickly.

[00:27:46]All right.

[00:27:48]Okay, so you can hit save. And then you still have to import um operating system and globe, right?

[00:27:58]All right. And let's do from globe import globe.

[00:28:10]All right. So, yes, that's done. So, now the other thing is um in this case, right? We want we are not going to um have a direct executable in this case, right? We're just going to launch it.

[00:28:26]So, what you're going to do is we're not going to uh specify any um entry points here in the counter scripts, right? So, let's do let's build our package quickly, right?

[00:28:39]So, navigate to the um ROS 2 workspace, right?

[00:28:44]And we're already there, so I'm just going to hit this, uh uh hit enter here.

[00:28:50]And then then we allow it to build.

[00:28:56]All right, so let's give it some time to build and then we'll launch our laser odometry, right? Our laser odometry, right? And when we do, uh what we want to do is we want to launch RViz, right? Into our visualization.

[00:29:09]And when when we have done that, we will run a teleop to see how uh the laser odometry, um is tracking the movement of our robot, right? So, let me just check really quickly if it's still together. Okay, good.

[00:29:27]All right, so let's let's allow it to finish building and then we can uh move on to the next step.

[00:29:34]Right?

[00:29:38]Okay, uh let's check our simulation so and let's zoom out a bit.

[00:29:45]All right.

[00:29:46]So, we will go full screen on the simulation soon. Okay.

[00:29:52]So that we can see everything clearly, right? And then let's put our terminal here. So, finally, uh our package is finished building, so we want to launch that uh node.

[00:30:04]So, to do that, let's go and check our the command here. So, we'll copy this quickly.

[00:30:13]And let's launch it, right? So, let's check the last command that was executed. So, source, right? We have sourced already.

[00:30:21]So, let's launch that uh laser odometry, right? And then now we see that it's running. Okay, good. Let's do this quickly. So, we're going to launch RViz, right? And the way we do that in this case is we just want So, let's label this terminal RViz so that we always have it for reference and we'll will which one is which. All right, so I'll just call this RViz 2.

[00:30:46]And let's clear this here.

[00:30:49]So, and navigate to the root.

[00:30:53]Uh clear one more time.

[00:30:55]And then let's launch so we're just going to launch RViz with nothing cuz we want it empty we're not going to use any configuration file. We'll configure it as we what we see fit as we what we need to show, right? So, there's not going to be much. However, there's going to be some very very useful important stuff, okay? So, I'll do this and if you allow me, I will zoom out a bit so that we can see both uh all the three windows without uh struggling, right?

[00:31:23]Okay.

[00:31:25]All right, so this is good, right? And our RViz window is loaded. Let's uh give it time, right? So, first thing that we need to do is we need to check if the odometry is being published on the uh fastboot_1 and score 1 and forward slash odom_rf2o, right? That's the topic that we want to see. So, to do that, we have to go on the easiest way is you go on add, right?

[00:31:50]And then instead of adding odometry display by display type, you click by topic, right? Because this will give you the topic already configured with the right QA settings, right? So, you see here, this is very nice, right? We have got odom_rf2o, so that that means it's good. It means the rf2o odometry um laser scan odometry node is running correctly. Right? So, let's double click there.

[00:32:13]And then let's do one thing here. So, let's check let's change the map here to So, the fixed frame we're going to change it to uh fastboot_1 odom, right?

[00:32:26]Cuz we don't have a map frame being published here, right? Um we're not navigating. So, let's double click there, and now you can see that this frame here this arrow here this is a representative of this odometry here. So, it's representative of uh odometry that's coming from the um RF2O node, right? If I Let's do this, right? So, let's label this uh for good measure. So, we're going to click rename here, and then let's call it uh RF2O.

[00:32:59]Uh odom, right? So, that we know which odometry is which. And then we're going to change the arrow color, right? So, um let's set keep here. So, keep is um how many arrows do you want to be displayed in Arviz, right? How many uh previous instances of odom do you want to be um kept, right? So, in this case, 100 is a lot, right? So, you realize that they'll fill the whole window, right?

[00:33:26]Because it's trying to display the previous 100 places that the robot has been, right? So, we're just going to hit one here because we want to see only that. Um it is easier also for us to distinguish in cases uh where we're going to try to uh check the differences between a RF2O odom and the default odom that we've got, which is the one using wheel encoders, right?

[00:33:46]And then, if we go on shape, on shape, we're going to change the color here. So, I want us to use Hmm.

[00:33:54]Let's use yellow for this one.

[00:33:57]Uh let's do this. Uh Right. So, our RF2O odometry arrow is yellow. Now, let's add another odometry, right? So, the odometry that we're going to add here, let's add by topic, is the odom that's coming from a that's using the wheel encoder basis, right? So, this is the one, right? So, this is the one that you get from um uh by default, right? So, now that we have those things done, and we can We're just going to keep this odometry, right? And let's Let's Let's rename it to maybe wheel encoder, right?

[00:34:35]Or wheel and then {underscore} odom, right? Just so that we know. And we know the color is red, so right now the arrows are one on top of the other, which means they are saying the same thing. So, that's good.

[00:34:47]But, notice what happens when you start moving the robot. So, what I'm going to do is I'm going to open a third terminal here. I want to use teleop uh not the arrows in this case.

[00:34:57]Just to make sure that we keep our environment a bit clean and we don't have uh a windows changing uh frequently. So, let's open our notebook and get this uh teleop command. So, I'll just copy it.

[00:35:10]And we'll paste it in this third terminal. Okay?

[00:35:14]And now that we have that done, I'm just going to close the the notebook here so that we can All right. So, I will reduce both angular and linear velocity by pressing Z.

[00:35:28]So, you have to have the focus in that terminal, so uh okay, let's do this.

[00:35:33]Hm.

[00:35:37]All right. Uh I think let's refresh our simulation quickly here.

[00:35:46]Okay. Now that our simulation has loaded correctly, so what I'm going to do is I'm going to reduce uh both linear and angular velocity here. So, by pressing um control you just press Z, right? So, this will reduce both. If you want to reduce just linear, you can just use small X. If you want to reduce just uh angular velocity, you just uh press C, right? So, a small C. Right? So, now that uh I have done that, so what you're going to do is uh I'm going to start by rotating the robot and notice what is happening to the to the um odometry arrows, right? So, at the moment the red one is the encoder odometry. So, I'm going to maximize this and reduce the cube again so that it's one, right? So, if we go on wheel encoder odometry here um let's keep it one here, right? So, you can now see the arrows clearly, right? So, at the moment in simulation you can see that one uh both of them, right? Uh laser scan latest scan odometry latest scan odometry and wheel encoder odometry are doing very well, right? So, if I rotate you can see that the arrows are moving together, right? So, that's what we want, right? Uh that means that they are predicting the same thing, right?

[00:37:02]However, this is simulation, right? And once you get to the real environment, you see that um there will be some deviation, right? Already you can see what odometry is saying here in this case um odometry from wheel encoders is perfect, right?

[00:37:18]Cuz it's simulation, right? Um but also there are a lot of other things that we are protected from in the simulated environment, right? So, you cannot see the variation uh clearly. That's why the arrows are seemingly sticking together, but you can see that there's some subtle differences in yaw, right? If you look, right? They're not pointing exactly the same direction, maybe differences of 2° or something, right? But that's those are the things we want to see, right? We want to understand when is one working well and when is one not doing as well, right? Anyways, so let's go to the next step, right? So, that we can um get to a a fuse this uh or join these two, right?

[00:37:58]So, what we're going to do in exercise two this time around is we're going to configure an EKF filter, right? So, and to do that you need a package a ROS 2 Humble uh robot localization, right?

[00:38:10]This will bring all the EKF um plumbing or the uh required packages to do that.

[00:38:15]So, what we'll do is uh let's uh install that first, right?

[00:38:20]Uh so, what I'll do is let me stop this, right? In the terminal one. So, that's where our RF was. Right? We'll reconfigure that and let's stop this.

[00:38:31]Right? And we'll leave Tello up running.

[00:38:33]So, I'm just going to label this Tello, right? Cuz we'll use it there as well.

[00:38:36]So, uh Right. Let's keep it Tello. So, now step-by-step, which is the installation first, okay?

[00:38:47]I'm just going to copy that command and let's paste it here, right? This is where we're going to be doing everything, right? In terminal two.

[00:38:56]Right? So, and we'll allow it to install quickly.

[00:39:03]Right? So, zero upgraded is zero in installed. That basically means that the package is already there on system default um on our systems. If you're doing it on another system, you need to make sure that you uh check that uh a robot localization is installed, right? So, now let's create an EKF config file.

[00:39:23]So, I will do that here.

[00:39:26]Right? So, remember because we're using absolute path to be created in exactly where we want it to be, right? So, get the config directory for our robot odometry. And then in there we have our configuration, right? So, we'll look at this configuration file.

[00:39:40]So, let's uh open our IDE here quickly and then find config here EKF uh filter.

[00:39:48]And then here. So, let's uh have a look at what's going on, right? So, if you look here we have got two odometries, right? And you can see the way they're labeled is this odom zero, right? So, the first source of odometry, right? Or IE your primary, right? So, in this case our wheel encoder odometry is the one that's being used as uh primary, right? And then here you have a odom that's coming from the range uh RF 2 O, so a range 2 Odom, right? Or the one that's using lighter to predict how much the robot has moved, right? And then the other thing that you also notice is maybe something very important here is you have automatic differential set to false, right? So, the only thing The only time you get to set this to true is in cases where you all your odometries, right? So, the primary one or odometry zero and odometry one are trying to publish the absolute value, right? Or they're trying to give absolute position. So, one of them needs to be relative to the other, otherwise they will end up um conflicting, right? So, in that case, um you will get like predictions that will be um basically one There's one that tells its positioning and then the other one uh contradict it immediately, right? So, when you set it to true, what it basically means is that one of the odometries being used as a um as a difference calculator, right? So, it will rely more on how much is um the position changing.

[00:41:21]So, it will calculate velocities instead of calculating absolute position, right?

[00:41:24]So, one odometry becomes more responsible for tracking position, the other one is becomes more responsible for tracking the change in position, right? So, now you have two parameters that are being used to update your odometry, right? So, only under those conditions, right? Will you need to set this um odometry differential to uh true, right? Um but the default configuration, if as long as you don't have conflicts between your the odometries that you're trying to fuse, right? For as long as they don't contradict by both of them or all three or all four, however many, as long as they don't con- contradict by giving absolute positioning um at the same time, right? Then you you have no need to um set odom odom differential uh to true, right? So, in the event that you have both of them or more than two or three trying to do that, then they you have to select one primary, which is responsible for publishing absolute a positioning, and then the rest will be able will be dealing with um relative or differential odometry, right? They'll be more responsible for updating a using a change in a position rather than the position itself, right? Okay. So, now let's uh maybe looking at some of the key important things is also again, frames are very important. You have to know a the frames for which uh to set your uh your EKF filter, right? And then the other thing is uh let me check if I can find it quickly here.

[00:42:59]Uh Right. This one here, publish TF, right?

[00:43:06]So, remember what I said that when you are fusing odometry, the the node that will be so responsible for publishing the the transformation frame from the odome to the base link will become the EKF itself, right? Because this is the one that will hold the uh let's say the greatest truth, right?

[00:43:29]Relative to the other odometries that you really have, right? So, instead of having odometry that's coming from wheel encoders or that node, right? Being the absolute publisher of the TF between the odome and the base link, in this case now you are now saying the EKF is supposed to take over that publication because now the EKF is the one that has got more accurate odometry tracking, right? This is the one variable that we had set to false in the in this setup, right? You can see here it it was set to false because it was going to be done by the extended common filter, right? Okay. So, if you're going to look at it as a hierarchy, you have uh uh like the laser scanner dome sitting um on the bottom as well as the wheel encoder odometry sitting on the bottom, and then you have the EKF filter combining information from these two nodes to have um the most accurate publication transformation frame between the odometry uh frame and the base link, right? So, that row will be done by the EKF filter.

[00:44:35]In the event that you now have a map, right? The map TF will refer to the odometry frame that is being published by the extended common filter instead of using one from the either laser odometry or from the wheel encoder odometry, right?

[00:44:51]So, very important to understand the chain and what what is changing when you now have a more accurate odometry topic like odometry filtered, which will be uh what the EKF node will publish, right? If you look at uh let's see here. If you look at the launch file for that, I will show you quickly. It will show you on what topic will the finalized odometry be publishing, okay? So, let's go to the next step here. So, the next step is to create a a launch file for this fused odometry um uh that will basically launch an odometry that will publish a filtered odometry, right? So, I'm going to copy the the the command into the terminal here as usual, and I'm going to hit enter. And then, let's first take the contents of this launch file and paste them into that launch file, and then we'll talk about them quickly, right? So, it will be fused odometry and then in here. So, in here, this is where we're launching our uh robot locally, our EKF node, right? That will be the one for responsible for publishing the transformation frame between the new odometry frame, let's say, right? Or the more accurate to don't frame and the robot base link, right? And look what we are doing here, if you look uh closely, is we have got a remapping that's going on here.

[00:46:13]So, uh let me look that up for you really quickly.

[00:46:21]Right. So, first of all, you have this one. Again, you know where this is coming from, so I won't dwell too much on that. But, here is uh one that's more right. So, by default, right? Odometry filtered is what it will publish to. So, in this case, we're not going to do any remapping, right? However, if you wanted your odometry to come on a topic that's slightly different from odometry filtered, then you'd have to do a something similar to this where you remap the topic, right? But, by default, the design of the EKF filter of extended common filter node is that whatever how many however many odometries that you combine to get a a more accurate odometry, uh the result will be published on this odom frame, right? On this odom forward slash filter, that's why we're not doing any remapping here, right? Okay. So, let's go back and uh continue to the next step. So, the next step is we're going to uh just make sure we install the required files.

[00:47:18]So, we install our config folder and we install our launch folder, right? And and build, right? So, in this case, the things that we wanted to do, which is installation of config directory because that's the one that is changed, right?

[00:47:33]And the launch file, we already have those uh set up in the launch file here.

[00:47:37]So, if see, we have that setting already, so that means everything that is in the config directory will be installed to the shared folder as well as everything that is in the launch file, right? So, our setup.py is fine.

[00:47:48]So, let's build this and launch a our node quickly, right? So, I'm going to paste this here and let's wait for it to finish, right?

[00:47:59]So, now that it is finished, so let's launch our fused odometry. So, launching the fused odometry will allow us to visualize three different forms of odometry this time around. Now, we have odometry from wheel encoders, we have odometry from uh the extended common filter, right?

[00:48:18]And then we have odometry from the range um lidar like the odometry that's coming from the lidar scan odometry package, right? The package that creates odometry from lidar scan data, right?

[00:48:31]So, what I'm going to do is let me launch this and we can now see this in RViz, right?

[00:48:38]In the logs right now it's very difficult, there's not much you can see, but once you try to visualize using our odometry arrows, this this will become very clear, right? So, I'm going to do this quickly and let's open one more terminal. Okay, we've got it running. So, this terminal Okay, we've got an RViz terminal reserved already, so let's just relaunch this one.

[00:49:01]Right, so would redo the configuration, but I'm sure by now you will know exactly what we'll do. So, we just add three different odometry displays and then we just set them to the different topics, right? So, let's do that quickly here once we have connected.

[00:49:18]All right.

[00:49:19]Right, so the first thing, let's maximize this. So, as I've said, it's easier to add by topic because then you can you get the Q settings already done for you.

[00:49:29]Right? However, the first thing is maybe Okay.

[00:49:33]Let's do Let's start with the wheel encoder odometry, i.e. the default one, this one.

[00:49:39]So, I'll double click that and then we will set this to a first one odon, right? So, that it shows.

[00:49:49]And now what I will do is I won't relabel. So, red is the wheel encoder odometry. Okay, let's just do that so that we don't get confused, right? So, um Okay, I think I've removed it by accident, so let's add one more time.

[00:50:07]And we want odometry, right? So, this one here.

[00:50:16]And let's rename.

[00:50:20]So, we'll call this wheel encoder, right?

[00:50:32]Okay.

[00:50:36]Right.

[00:50:37]Okay. Now, let's just change the queue here to one, right?

[00:50:45]And then let's add another one. So, we're going to add by topic again.

[00:50:50]And this time around we're going to add the RF2O odometry, right? So, we'll click that.

[00:50:56]And let's change the color, right? So, this one we're going to set it to yellow.

[00:51:00]Again, queue is going to be one.

[00:51:03]And let's set it to yellow. I think that'll be the easiest way to differentiate the two.

[00:51:13]And All right. So, Okay, let me try one more time.

[00:51:25]Okay, cool.

[00:51:33]All right.

[00:51:34]If I escape, yes, it's saved like that.

[00:51:36]Okay, that's good. So, we're going to label this uh RF2O. So, I will click rename here.

[00:51:44]And I'll hit enter, right? And then now let's add the uh final one, right? Which is uh filtered, right? The reason why they're already different right now is because the simulation did not start from zero, right? We didn't reset the simulation before we launched this. That's why they're already different, so it will be slightly difficult to track the changes.

[00:52:09]Um however, let's uh let's just add the last one and then we can relaunch the simulation and we can see them and how they differ from each other once we start moving the robot, okay?

[00:52:23]So, this time around we want a odometry filtered, so it should be on uh just odometry filtered, right? It's not namespaced, so but that's not an issue.

[00:52:38]Um as long as you know that it's only fastboot one that's that has got odometry filtered being populated, right? However, in some if you want to do this for fastboot two and three, right? Then you have to remap that topic. Remember in that launch file where I said you have to remap from filtered uh forward slash from odometry forward slash filtered, you'd have to say fastboot one forward slash odometry filtered or odometry underscore filtered. That way you won't confuse which uh filtered odometry you're looking at, right? So, let's just add this quickly.

[00:53:08]And then now this time around we going to uh set this one to purple.

[00:53:17]So, let's find it.

[00:53:22]All right, so let's rename and we will call it uh filtered, right?

[00:53:30]Okay, so we've called it filtered, so what I'll do is let's uh configure the arrow color because we want to know we want to differentiate them, right? And then we'll relaunch the simulation and then set the cube to one again.

[00:53:46]And arrow Then we're going to set the color to purple. So Let's see.

[00:54:03]Right. Okay. So Now what you're going to do is we're going to relaunch our simulation so that they start stick together, right? And then we won't close this RV's window. Everything will just show up exactly the way we want. All right.

[00:54:22]Okay, now that our simulation is loaded, you can see that the arrows right now, they are exactly right one on top of the other, right? So what we're going to do is Now let's move our robot, right? And see how they start to differ or if they stick together, let's see that, right? So if I So let me restart the launch command for Tello, okay?

[00:55:02]All right, so let me do this quickly. So >> All right. So, what you're going to do is um let's see if we can use teleop.

[00:56:07]All right. So, if teleop is not working, so what I'm going to do is I'm going to uh let's minimize the simulation quickly, right?

[00:56:14]And then what we're going to do is I will click the robot and uh move it using the arrows, right? And you can see that there is the paper one is not updating, right? Automatic filtered. And what we want to see is let's check if the node is running correctly here.

[00:56:31]Um and Okay, so now let's move our robot forward quickly to just check.

[00:56:53]Right. So, the first thing to check in this event is what you want to do is you want to check if echoing that topic will produce any results, right? So, we go ros2 topic echo.

[00:57:08]And we say uh okay. Let's first list, right? That's the easiest way. So, if we list uh our topics, so ros2 topic list.

[00:57:23]We see that we've got automatic filtered, right? So, now let's run ros2 topic uh echo.

[00:57:45]and hit enter.

[00:57:47]Right. So, at the moment, you can see that a there's nothing that's coming in from odometer filter, right? So, there's something to do with configurations, right? So, let's check one more time to see if our configuration file is set correctly.

[00:58:02]So, if you go into EKF filter, right, and see this odem one, odem two.

[00:58:18]All right. So, let's do this one more time. Let's restart the node.

[00:58:32]Right. Okay. So, and let's move our robot.

[00:58:46]Now, it's updating, right? You can see that it's updating, so it wanted to relaunch. Right, so you can see that a the filtered odometry and the lighter scan measure, right, i.e. the yellow arrow, right, they're sticking one on top of the other. Right, however, the wheel encoder odometry in this case, right, started a little bit off, but that's because you would need to restart the simulation one more time, right, to get them to to be one on top of the other. Right. Um but, what we'll do is let's connect to the real robot because there is where you'd actually see um what the major differences are, right? So, in order to do, >> [clears throat] >> connect to the robot, we'd need to have a booking. So, first of all, you need to go on uh my labs, right? So, as an educator, you'd have your robot registered on my labs, right? And then you click book a slot. And then here, what I'm going to do is I'm going to book a slot and I will book another one so that we have enough time to do our demo.

[00:59:45]All right?

[00:59:46]And then once you book a slot, if you go on bookings, here you can see there's a button that says connect. You click connect there.

[00:59:55]And then you should be allowed you should see your connection going through, right? Even if it's a robot that you have got within your office and it's registered on our platform, the same thing applies, right? It will appear under my labs and then you can connect your robot from there. So, I'll allow the camera stream to load really quickly and then what we'll do is let's create all the terminals that we need, right? So that we can really see. So, what you're going to do is we're not going to do lidar scan matcher on its own. What we're going to do is we're going to do launch the whole filtered odometry and then compare the three like we did lastly in the simulation so that we can notice what are the nuances, what are the differences, right?

[01:00:36]So, to do that, let's check our notebook one more time. So, what we're going to do is we're going to skip to the real robot part. Again, the only variable that changes here is you have to set sim to false, right?

[01:00:51]Um So, I'm going to do that and I'll copy this. So, this will allow us to launch our odometry filtered, right? And and I'm going to hit enter here, right?

[01:01:02]So, we have it running. So, what I will do is let's have RViz, right? So, and we're going to open RViz one more time and set the configurations exactly the way we did earlier, right? So, I will say RViz two here.

[01:01:17]And let's maximize this quickly so we can see a the details that you want to see, right? So, now that our RViz window is running, let's see it and configure it really quickly. So, I won't rename them this time around. I'll just click a topic and add and change the colors, right? So, notice what I'll do. So, what I'll do is I'll add wheel encoder odometry first. Again, it will be the red arrow, right? So, I'll leave it red arrow. I'll just change I'll just change the color here and the keep. So, we want one. We don't want to check the previous uh 100 occurrences of odometry, right? And then what I'll do is um I'll disable covariance because it will uh be very wild. Um I can keep it there just to show you how it's going to uh look like, but that's not we shouldn't be concerned about that right now.

[01:02:10]Um So, I'll disable disable it once we're done. So, the next thing that we're going to do is let's add the second one, which is a RF2O odometry, right? This one.

[01:02:23]And this is the one that we're going to set to yellow. Again, we set keep to one.

[01:02:28]And hit enter, and then we'll set uh the arrow color to yellow, right?

[01:02:35]So, to do that, let's uh double click here.

[01:02:42]And set it to yellow, right?

[01:02:44]Now, um the next thing is you have to set our frame before we add the last filter to the odometry. Let's set the frame to uh something valid. So, first with one odometry is uh the ideal one, right? And you see that you've got uh this yellow uh ellipse, right? If you zoom out, you would see that it's an ellipse. So, it's the covariance. It's trying to show you how how much noise uh there is, right? So, the bigger this is, the more error it it thinks there is, but that's again an RV scaling. So, let's just uncheck covariance for odometry.

[01:03:22]This is for wheel encoder odometry, by the way. So, if you uncheck that, you won't have that. Um and then let's get a bit closer.

[01:03:31]So, at the moment, uh they are separate, right? But, that's because we're not moving the robot yet, right?

[01:03:38]However, they must be together eventually. And also, that's also because I've been moving the robot. So, sometimes what one is saying is not exactly what the other is saying, but we'll see soon uh what exactly will happen when you start moving the robot, right? Um So, let's add the other odometry, right?

[01:03:57]So, that's the filtered odometry. So, we'll go to filtered odometry here, which is this one.

[01:04:03]And this one, we're going to set it to purple.

[01:04:06]All right. So, if we go on filtered odometry, which is the last one that we added, we're going to Again, we set keep to one.

[01:04:17]And let's set the color. So, arrow color to purple, right?

[01:04:27]So, if we hit 255 here, so that's purple. Right. So, let's move our robot and see what will happen, right? So, first of all, let's try run let's run teleop so that we can move the robot around and then we see the changes, right?

[01:04:45]Right now, if you look at wheel encoder odometry, it is it is no reference, right? Because there is no map, so it is no way of correcting itself at this point.

[01:04:54]Um but, what's important is if we could let's reset this. So, if we go in RVs and reset, this must be able to reset the uh okay.

[01:05:07]Hm. All right. No worries. Um let's relaunch, right? I think the best way is to relaunch. So, we leave RViz like that because it's not dependent on this node. And let's relaunch this, right?

[01:05:24]Uh-huh. Okay.

[01:05:30]Okay, let's clear.

[01:05:33]Okay, so the other thing that's causing this is also because of the fact that if you the robot odometry, right? Is started the first time you turn on the robot, right? So, at this point, because we don't have a map, the most accurate odometry that we've got right now is the wheel encoder odometry.

[01:05:52]Because that's the one that has been there first before the odometry filtered and um the RF2O, right? However, the moment we start moving, right? The difference, right? It's not the absolute values that is important at this point, but the difference is what we want to notice, right? So, what I'm going to do is I will ignore that for now. And let's run teleop, all right? Running teleop and being able to see those differences and allowing the absolute to correct will enable us to visualize what we want to visualize a better. So, I will minimize this.

[01:06:27]And let me make the terminal smaller here.

[01:06:33]Right, so.

[01:06:36]Now, you can see that our wheel encoder odometry is updating. However, our node is not updating, right? The node that's supposed to be publishing a the other types of odometry that I've got, but that's because it's not running, right? So, let's run it one more time. So, let me check here.

[01:06:57]And then this is a So, yes, let's run it one more time here.

[01:07:01]And then this time around it must a Hmm.

[01:07:08]All right, let's see.

[01:07:13]Let's try to move the robot quickly.

[01:07:16]Just a little bit.

[01:07:18]Okay, now you can see. So, now it's updating, right?

[01:07:21]Um but the absolute positioning is not updated, right? So, it's it's difficult to give reference right now, but let's do a bit more movement.

[01:07:35]And then we should be able to see that one of them or at least they converge to some value.

[01:07:51]All right. All right.

[01:07:56]Let's do a little bit more movement.

[01:07:58]Okay.

[01:08:02]So, what I'm trying to do at the moment is I'm trying to get get the absolute uh positioning, right? So, not the relative positioning, but the what they predict to be the X and Y values to to converge or to be as close to each other as possible, right? So that um there is no this difference that you see, but the key point here is that the difference is as a result of the fact that when the robot was turned on, right? It sets its own initial odometry frame uh that that has got almost nothing to do with the node that we're right?

[01:08:36]And as a result of that, when we don't switch it off and switch it on, like we were doing refreshing the simulation, it's very difficult to get them to be one on top of the other. However, what we'd want to do, right, in future is to set, right? Is to set a wheel encoder odometry as the beginning absolute truth. So, let's do this.

[01:09:01]All right. So, basically, besides the fact that the the positioning that they are referring to is different, uh this is basically what I wanted to show you, right? So, you can see that the way the angle variation goes, for example, if you look at how much the angle of the odometry is uh that is being published via the wheel encoder system uh changes the angle its own um angular uh distance covered is very much different from the way that the odometry filter division does, right? So, this is the main difference. However, to be able to see this in uh the context that will enlighten us, what we need to do is eventually we would need to uh start the same, right? So, that you can see the divergence of the other or to be able to go on a map, right? And use it to do some form of navigation.

[01:09:54]That way, you can definitely notice that one that is using wheel encoder odometry will find itself going astray more often than the one that uses the filtered or the fused odometry, right? So, for the purposes of what we wanted to demonstrate, I think it's more clear in simulation where you can easily reset what the wheel encoder is is saying.

[01:10:12]However, its practicality comes in the greatest sense when you um use it beyond just testing, right? For example, when you're now in the real robot environment and you want your robot to navigate, it will be able to navigate and track all the differences in your and angle changes more uh more robust when you use the filtered odometry version, right? So, for the purposes of our demonstration here, it didn't go as we planned. However, um the concept is very much the same. If you go through the notebook and you fully understand that principle, you can leverage this to have uh your navigation stack working a little better because this is where uh we are building towards, right? So, what I will do is I'll switch to my uh other screen here and then we wrap our class here for the time being and then we'll continue trying to see how we can make good use of this filter to geometry system and how we can make use of this more advanced version of a the robot's ability to track its uh its position within the given map, all right? So, until next time, wish you all the best and keep pushing your ROS learning. Bye-bye.

[01:11:34]>> [music] [music] [music] [music] [music] [music] [music]