Occupancy Grid Mapping with Webots and ROS2 | by Debby Nirwan | Towards Data Science

One of the many tasks that an autonomous robot has to perform is mapping the environment. Having a map of the environment is important because many other tasks may depend on the map as a prerequisite.
For some robots, the map is given as input but for many others that is not possible. The robot has to map the environment autonomously by exploring it.
Mapping is also a chicken-and-egg problem often referred to as SLAM (Simultaneous Localization and Mapping), where we have two interrelated problems to be solved. Localization needs a map to estimate robot pose and Mapping needs an accurate pose to map the environment.
Because SLAM is a harder problem to solve, in this article we assume that the pose is accurate so that we can focus on the mapping task.
Also, readers must have basic knowledge of robotics and Robot Operating System version 2 (ROS2) to follow.
The good thing when we want to learn to write software for robots is that we don’t have to have the “real” robot hardware which can be quite expensive. Also, you may need a little hardware knowledge to set it up and troubleshoot when there are issues.
For learning or testing purposes, we can use robot simulators. There are many free and open-source robot simulators that we can use. Check this Wikipedia page to see them:
en.wikipedia.org
Gazebo simulator is one of the most popular ones used by roboticists:
gazebosim.org
But I find that Webots is easier for beginners because it offers many readily available commercial robots and supports multiple programming languages, especially Python which is a great language for prototyping.
cyberbotics.com
Webots is an open-source three-dimensional mobile robot simulator that is used in this article because it has many available robots that we can use for our simulation and most importantly like Gazebo it supports ROS2.
Robot Operating System version 2 or ROS2 in short, is an open-source robotics software framework.
It is not an operating system like its name suggests, but it is a collection of frameworks that are supported by the community. It makes it easy to write software for robots, and more importantly, there are many open-source ROS2 packages that we can use in our robot software application.
With ROS2, we can use the packages to focus on the areas that we want to focus on for our purposes such as learning, research, etc.
docs.ros.org
Webots ROS2 Package is a ROS2 package that provides the ROS2 interface to simulate our robot and environment on Webots.
Before we start writing our code, we should have all the required software above installed on our system. We need the following installed:
Webots
ROS2 (currently using foxy distro)
Webots ROS2 package
How to set up the software is out of the scope of this article and there is good documentation on their websites for that purpose.
The occupancy grid mapping is about creating a 2D map of the environment from sensor measurement data assuming that the pose is known. The map is represented as a grid of evenly spaced binary (random) variables. True or 1 means that location is occupied by some objects, False or 0 represents a free space.
The image above is a 5x5 occupancy grid map. Our occupancy grid map has the following properties:
Width
Height
Resolution
Data
Width and Height are helping us to read Data which is usually a 1D array and Resolution is the size of the cells, for example, if the resolution of cells is 1x1 cm, the Height and Width of our map are 5 cm.
The Occupancy Probability Map represents the probability that a location in the map is occupied. So instead of 1 or 0, each cell represents the probability of it being occupied.
This algorithm is from the Probabilistic Robotics book by Sebastian Thrun, Wolfram Burgard, and Dieter Fox. The algorithm is to calculate the posterior over maps given the data:
However, the calculation of this is intractable because a map can contain a large number of cells. So instead of the probability over the maps, we calculate the posterior probability over each cell.
The posterior over the maps are the product of its cells:
The mapping algorithm is:
Updata(map, range, pose): for cell in map:  if cell in perceptual field:   cell = cell + prob - prior return map
The algorithm takes in the current map, sensor measurement, and pose of the robot. We are only interested in updating the cells that are in the perceptual field, for this we can use Bresenham’s line algorithm. The prior probability of all cells can be set to a value like 0.5 or others. The cell at the end of the line is occupied the other cells are free space (probability = 0).
The probability of the occupied cell can be calculated from the sensor model which may be provided by the sensor manufacturer or from our own measurement.
To implement the autonomous mapping mission, we need three components (nodes):
Mapper
Mission Controller
Random Bouncer
The following videos show the autonomous mapping mission in action:
I have implemented the three nodes and made them available on Github (see Summary).
The Mission Controller controls the whole mission by starting and stopping the Random Bouncer as well as converting the map to PNG files and save them to the given path. It controls how long the mission should run. The PNG files for the missions in the videos above are below.
The Random Bouncer moves the robot by setting the linear and angular speeds and detecting obstacles from the Laser Scan.
The Mapper takes in Sensor Measurement Data and Transformation to calculate the pose of the robot and create and update the Occupancy Probability Map and publish it.
To use the code, first as described above, you will need to install Webots, ROS2, and Webots ROS2 package. Assuming that you have installed all of them, you can then:
Clone the git repository
Build
Install
cd /home/$USER/ros2_projectscolcon build --packages-select mapping_controller. install/setup.bash
To run it you can execute the launch file with different parameters:
ros2 launch mapping_controller mapping_controller_launch.py probability:=true rviz:=true mapping_time:=15 world:="test.wbt" path:="/home/$USER/Documents"
probability: run Probability Occupancy mapper
rviz: run rviz
mapping_time: how long the mission should run (in minutes), default is 5 minutes
world: Webots world file
path: where to save the PNG files
We have seen above that we can use open-source software to learn to develop software for robots and test our software on the simulator.
One simple example that I’ve shown here is the autonomous mapping mission for the robot to map its environment to be used for different purposes such as localization.
As you can see from the videos the random bounce is not efficient because it may take a long time to map even normal-sized rooms. A more intelligent motion pattern will be much better.
Also, you may notice that the map isn’t very accurate, that is mostly because we only use odometry for the robot pose which is prone to accumulating error over time. We need SLAM to give us a more accurate pose. Other than that, the errors are also caused by the characteristics of the sensors themselves.
You can try the code by cloning my repository at:
github.com
Try to improve it and raise a PR if you’re interested. Thanks for reading.