# 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.