# robot_localization package tutorial
###### tags: `2022 SDC course`
This is a guide to use ROS `robot_localization` package fusing continuous measurements (ex. IMU, wheel odometry) and global measurements (ex. GPS, LiDAR scan matching).
:::info
:bulb: We are using **two ekf nodes**, for local and global filtering.
:::
:::info
:bulb: The output of the ekf node can be used as a good initial guess for ICP.
:::
## Download modified data
To use the robot_localization package the frame_ids need to be modified (remove `/`), download the modified bags here: https://drive.google.com/drive/folders/1S_7DyCgpGXFAG9G6q-j3FbCMS9JRuvmH?usp=sharing
## Setup
The original tf recorded in the bag would have some conflict with our tf, use `--topic` to specify the desired topics:
```shell
rosbag play sdc_localization_2_lite.bag \
--clock \
-r 0.01 \
--pause \
--topic /wheel_odometry /lidar_points /gps /imu/data
```
Then we publish the static transform in launch file:
```launch
<node pkg="tf2_ros" type="static_transform_publisher" name="car_lidar_frame_publisher" args="0.986 0 1.84 -0.015 0.017 -0.707 0.707 car nuscenes_lidar" />
```
:::warning
:zap:**Warning**
The `header/frame_id` and `child_frame_id` should not start with `/`.
:::
:::warning
:zap: Remove the tf publish part in the sample code: `br.sendTransform(...);`
:::
## Configure `ekf_odom`
Set the **world_frame** same as **odom_frame**.
This is used to generating the tf from `origin` to `car`.
Remap this node's output `odometry/filtered` to `odometry/filtered_wheel`
:::info
:bulb:IMU can also be fused in this scope!
:::
```yaml=
# other parameters are ignored
frequency: 100
two_d_mode: true
publish_tf: true
map_frame: world
odom_frame: origin
base_link_frame: car
world_frame: origin
odom0: wheel_odometry
odom0_config: [false, false, false,
false, false, false,
true, true, false,
false, false, true,
false, false, false]
odom0_queue_size: 100
```
## Configure `ekf`
Set the **world_frame** as **map_frame**.
This node will generate the tf from `map` to `origin`.
:::info
:bulb: Make sure the frame id of `pose0` is correct. This should be the pose of what you set as `base_link_frame` in the `world_frame` (i.e. publish the **car** pose in `world frame`).
:::
:::info
:bulb: Publish `car_pose` in the localization node.
:::
```yaml=
# other parameters are ignored
frequency: 100
two_d_mode: false
publish_tf: true
map_frame: world
odom_frame: origin
base_link_frame: car
world_frame: world
odom0: odometry/filtered_wheel
odom0_config: [false, false, false,
false, false, false,
true, true, false,
false, false, true,
false, false, false]
odom0_queue_size: 100
pose0: car_pose
pose0_config: [true, true, true,
true, true, true,
false, false, false,
false, false, false,
false, false, false]
pose0_queue_size: 100
pose0_nodelay: true
```
:::info
:bulb: use `rostopic echo /diagnostics` to check the state of your ekf nodes.
:::
Then add them into launch file:
```launch=
<node pkg="robot_localization" type="ekf_localization_node" name="ekf" clear_params="true">
<rosparam file="$(find localization)/params/ekf.yaml" command="load" />
</node>
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_odom" clear_params="true">
<rosparam file="$(find localization)/params/ekf_odom.yaml" command="load" />
<remap from="odometry/filtered" to="odometry/filtered_wheel" />
</node>
```
:::warning
:zap: Do set other not mentioned parameters properly.
Example: https://github.com/cra-ros-pkg/robot_localization/blob/kinetic-devel/params/ekf_template.yaml
:::
## Set pose
Initialize your ekf node is important, since in our cases, the car doesn't start at (0, 0, 0).
### Approach 1.
Set your initial state into the config of ekf node:
```yaml=
# in ekf.yaml
initial_state: [..., ..., ...,
0, 0, ...,
0, 0, 0,
0, 0, 0,
0, 0, 0]
```
### Approach 2.
Publish type of `geometry_msgs/PoseWithCovarianceStamped` msg (same as `/car_pose`) to `/set_pose` to set the state of car for the first localization result.
:::warning
:zap: This system assumes running in real time. Please play the bag in very slow rate according to the calculation time of ICP. Playing the bag too fast may cause timestamp inconsistency.
:::
## TF tree
The tf tree should look like this:
![](https://i.imgur.com/la6dP4m.png)
---
## References
1. Official documents: http://docs.ros.org/en/noetic/api/robot_localization/html/state_estimation_nodes.html
2. Official tutorial: https://www.youtube.com/watch?v=nfvvpYBAMww
3. Turtle bot example: https://kapernikov.com/the-ros-robot_localization-package/
4. Other examples: http://docs.ros.org/en/noetic/api/robot_localization/html/user_contributed_tutorials.html
5. Translated document: https://blog.csdn.net/weixin_42317511/article/details/103052649
6. Example of two nodes: https://github.com/cra-ros-pkg/robot_localization/blob/humble-devel/params/dual_ekf_navsat_example.yaml
7. Chinese tutorial: https://blog.csdn.net/gwplovekimi/article/details/109098641
8. Architecture of fusing local and global measurements: https://answers.ros.org/question/309176/how-to-integate-the-package-with-sensors-such-as-lidar-or-cameras/
---
## Debugging
### Check ekf_odom node
* Run ekf_odom node alone then check `/odometry/filtered_wheel` and `/lidar_points` are functioning properly in rviz.
* Check ekf_odom generates the tf `origin` -> `car`.
### Check ekf node
* Run two ekf nodes together. Check `/odometry/filtered` is in the right position in the map.
* Check if you initialized the state properly.
* Check the tf tree.
### Tips
* Make sure the two ekf nodes function properly before using `odometry/filtered` as initial guess.
* `rostopic echo /diagnostics` to check the state of ekf nodes.
* Make sure the frame_id and subject of each information you are using and apply proper transformations. The initial guess for ICP should be the lidar's pose. `/car_pose` is the car's pose.