# 2.6 Sensor fusion and SLAM

### get the start package

```
git clone -b gazebosim2 https://github.com/roboticslabs/mec_mobile.git
```

## Sensor fusion with ekf

Sensor fusion is the process of combining data from multiple sensors (possibly of different types) to obtain a more accurate system or environment. A Kalman Filter (KF) is a mathematical algorithm that estimates the internal state of a system (e.g., position, velocity). The standard Kalman Filter assumes the system is linear.

Real-world systems involving orientation, rotations, or non-linear sensor models (e.g., fusing IMU acceleration, odometry, GPS position, magnetometer) often do not follow purely linear equations. That’s where the Extended Kalman Filter (EKF) comes in for non-linear system.&#x20;

Luckily, we don't have to bother too much about its implementation, because we'll use the widely used [robot localization package](https://docs.ros.org/en/melodic/api/robot_localization/html/index.html) and the default node name is **ekf\_node.** You can install it with:

```
sudo apt install ros-jazzy-robot-localization
```

### 1. Create a new package

```
cd ~/ros2_ws/src/mec_mobile/
ros2 pkg create --build-type ament_cmake --license BSD-3-Clause mec_mobile_navigation
mkdir -p config && cd config
touch ekf.yaml
```

To configure `robot_localization` package can be tricky in the beginning (official doc at [this website](https://docs.ros.org/en/noetic/api/robot_localization/html/configuring_robot_localization.html)), but I already created an [`ekf.yaml`](https://github.com/roboticslabs/mec_mobile/blob/Navigation/mec_mobile_navigation/config/ekf.yaml) file in the `config` folder based on the official guidelines that will do the job in this lesson. &#x20;

The **ekf\_node** we configure here (`ekf.yaml`) will subscribe to the following topics (ROS message types are in parentheses):

* **/odom** ([nav\_msgs/Odometry](http://docs.ros.org/en/api/nav_msgs/html/msg/Odometry.html))
* **/imu** ([sensor\_msgs/Imu.msg](http://docs.ros.org/en/api/sensor_msgs/html/msg/Imu.html))

This **ekf\_node** will publish data to the following topics:

* **/odometry/filtered** : The smoothed odometry information ([nav\_msgs/Odometry](http://docs.ros.org/en/api/nav_msgs/html/msg/Odometry.html)) generated by fusing the IMU and wheel odometry data.
* **/tf** : Coordinate transform from the **odom** frame (<mark style="color:red;">parent</mark>) to the **base\_footprint** frame (<mark style="color:red;">child</mark>).

<mark style="color:red;">The</mark> <mark style="color:red;"></mark><mark style="color:red;">`robot_localization`</mark> <mark style="color:red;"></mark><mark style="color:red;">will publishes a filtered</mark> <mark style="color:red;"></mark><mark style="color:red;">`odometry`</mark> <mark style="color:red;"></mark><mark style="color:red;">topic and a</mark> <mark style="color:red;"></mark><mark style="color:red;">`tf`</mark> (*odom* -> *base\_footprint*) <mark style="color:red;">to improved odometry coordinate system.</mark>

Create a [***spawn\_robot.launch.py***](https://github.com/roboticslabs/mec_mobile/blob/Navigation/mec_mobile_navigation/launch/spawn_robot.launch.py) file. <mark style="color:red;">Since a robot cannot have 2 odomnetry tf, we should stop Gazebo doing it. The easiest way is not bridging it in the</mark> <mark style="color:red;"></mark><mark style="color:red;">`parameter_bridge`</mark>.&#x20;

<details>

<summary>Add <code>robot_localization</code> (<strong>ekf_node</strong>) to the launch file and comment the /tf</summary>

```python
    # Node to bridge /cmd_vel and /odom
    gz_bridge_node = Node(
        package="ros_gz_bridge",
        executable="parameter_bridge",
        arguments=[
            "/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock",
            "/cmd_vel@geometry_msgs/msg/Twist@gz.msgs.Twist",
            "/odom@nav_msgs/msg/Odometry@gz.msgs.Odometry",
            "/joint_states@sensor_msgs/msg/JointState@gz.msgs.Model",
            #"/tf@tf2_msgs/msg/TFMessage@gz.msgs.Pose_V",
            #"/camera/image@sensor_msgs/msg/Image@gz.msgs.Image",
            "/camera/camera_info@sensor_msgs/msg/CameraInfo@gz.msgs.CameraInfo",
            "imu@sensor_msgs/msg/Imu@gz.msgs.IMU",
        ],
        output="screen",
        parameters=[
            {'use_sim_time': LaunchConfiguration('use_sim_time')},
        ]
    )
    
    ekf_node = Node(
        package='robot_localization',
        executable='ekf_node',
        name='ekf_filter_node',
        output='screen',
        parameters=[
            os.path.join(pkg_mec_mobile_navigation, 'config', 'ekf.yaml'),
            {'use_sim_time': LaunchConfiguration('use_sim_time')},
             ]
    )
    
    launchDescriptionObject.add_action(ekf_node)
```

</details>

```
ros2 run rqt_tf_tree rqt_tf_tree
```

From the `rqt_tf_tree` tool we cannot tell which node is broadcasting the TF, but we can check the `/tf` topic which nodes are publishing it by: `ros2 topic info /tf --verbose` And we will see 2 publisher nodes, the `ros_gz_bridge` and the `robot_state_publisher` as we expected.&#x20;

### 2. Tidy the launch file

For the launch file in `mec_mobile_navigation` package, moved all the `parameter_bridge` topics into a yaml config file `gz_bridge.yaml`

```
cd ~/ros2_ws/src/mec_mobile/mec_mobile_navigation/
mkdir -p launch/ && cd launch
touch ekf_gazebo.launch.py
ros2 launch mec_mobile_navigation spawn_robot.launch.py
```

Rebuild the workspace and try it!&#x20;

As we expected the `tf_tree` looks the same, but if we check the publishers of the `/tf` we'll see the following nodes: `ekf_filter_node` and `robot_state_publisher`. We can also see that there is a `/odometry/filtered` topic published by the `ekf_filter_node`.

## SLAM

Let's learn **how to create the map of the robot's surrounding**. In practice we are using SLAM algorithms, SLAM stands for Simultaneous Localization and Mapping. It is a fundamental technique in robotics (and other fields) that allows a robot to:

1. Build a map of an unknown environment (mapping).
2. Track its own pose (position and orientation) within that map at the same time (localization).

### 1. SLAM Toolbox

In this lesson we will use the `slam_toolbox` package that has to be installed first:

```
sudo apt install ros-jazzy-slam-toolbox
# following are optional
sudo apt install ros-<ros2-distro>-navigation2
sudo apt install ros-<ros2-distro>-nav2-bringup
```

After installing it we will create a new launch file for mapping, the `slam_toolbox` parameters are already in the `config` folder ([`slam_toolbox_mapping.yaml`](https://github.com/roboticslabs/mec_mobile/blob/Navigation/mec_mobile_navigation/config/slam_toolbox_mapping.yaml)) we'll use these parameters.&#x20;

Let's create `mapping.launch.py` in `mec_mobile_navigation` package:

<details>

<summary>mapping.launch.py</summary>

```python
import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, Command
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():

    pkg_mec_mobile_navigation = get_package_share_directory('mec_mobile_navigation')

    rviz_launch_arg = DeclareLaunchArgument(
        'rviz', default_value='true',
        description='Open RViz'
    )

    rviz_config_arg = DeclareLaunchArgument(
        'rviz_config', default_value='mapping.rviz',
        description='RViz config file'
    )

    sim_time_arg = DeclareLaunchArgument(
        'use_sim_time', default_value='True',
        description='Flag to enable use_sim_time'
    )

    # Path to the Slam Toolbox launch file
    slam_toolbox_launch_path = os.path.join(
        get_package_share_directory('slam_toolbox'),
        'launch',
        'online_async_launch.py'
    )

    slam_toolbox_params_path = os.path.join(
        get_package_share_directory('mec_mobile_navigation'),
        'config',
        'slam_toolbox_mapping.yaml'
    )

    # Launch rviz
    rviz_node = Node(
        package='rviz2',
        executable='rviz2',
        arguments=['-d', PathJoinSubstitution([pkg_mec_mobile_navigation, 'rviz', LaunchConfiguration('rviz_config')])],
        condition=IfCondition(LaunchConfiguration('rviz')),
        parameters=[
            {'use_sim_time': LaunchConfiguration('use_sim_time')},
        ]
    )

    slam_toolbox_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(slam_toolbox_launch_path),
        launch_arguments={
                'use_sim_time': LaunchConfiguration('use_sim_time'),
                'slam_params_file': slam_toolbox_params_path,
        }.items()
    )

    launchDescriptionObject = LaunchDescription()

    launchDescriptionObject.add_action(rviz_launch_arg)
    launchDescriptionObject.add_action(rviz_config_arg)
    launchDescriptionObject.add_action(sim_time_arg)
    launchDescriptionObject.add_action(rviz_node)
    launchDescriptionObject.add_action(slam_toolbox_launch)

    return launchDescriptionObject
```

</details>

Let's also move the RViz functions to the new launch file from `spawn_robot.launch.py`.

```
    #launchDescriptionObject.add_action(rviz_launch_arg)
    #launchDescriptionObject.add_action(rviz_config_arg)
    #launchDescriptionObject.add_action(rviz_node)
    launchDescriptionObject.add_action(ekf_node)
```

Build the workspace and we'll need 2 terminals to test it. In the first terminal we launch the simulation like before (but this time it won't open RViz):

```
ros2 launch mec_mobile_navigation spawn_robot.launch.py
```

And in another terminal we launch the new `mapping.launch.py`:

```
ros2 launch mec_mobile_navigation mapping.launch.py
```

Let's take a look first on `rqt_tf_tree`:

```
ros2 run rqt_tf_tree rqt_tf_tree
```

We can see an additional frame `map` over the `odom` odometry frame. We can also visualize this transformation in RViz:

The difference between the `odom` and `map` frames show the accumulated drift of our odometry over time. don't forget that this is already improved a lot of thanks to the sensor fusion.

### 2. Build the map

With SLAM Toolbox we can also save the maps, we have two options:

1. `Save Map`: The map is saved as a `.pgm` file and a `.yaml` file. This is a black and white image file that can be used with other ROS nodes for localization. Since it's only an image file it's <mark style="color:red;">impossible to continue</mark> the mapping with such a file.
2. `Serialize Map`: With this feature we can serialize and later deserialize SLAM Toolbox's graph, so it can be loaded, and <mark style="color:red;">the mapping can be continued</mark>. Although other ROS nodes won't be able to read or use it for localization.

After saving a serialized map next time we can load (deserialize) it. \
![](https://2579747216-files.gitbook.io/~/files/v0/b/gitbook-x-prod.appspot.com/o/spaces%2FkSq9ZTh3ba8gAVAntL52%2Fuploads%2FnXNdRTVPQoylEKhuODTS%2Fimage.png?alt=media\&token=734c7c91-3bc5-4868-802f-3e4cbd0ba5de)

And we can also load the map that is in the starter package of this lesson. We can use a custom node to deserialize the already saved map, similar as the `Deserialize Map` button in RViz:

```
ros2 run mec_mobile_navigation_py slam_toolbox_load_map
```

Another way to save the `.pgm` and `.yaml` map for later use is the `map_saver_cli` tool of the navigation stack.

```
ros2 run nav2_map_server map_saver_cli -f my_map
```

To use it, `map_server` has to be installed:

```
sudo apt install ros-jazzy-nav2-map-server
```
