4. Robot state estimation

4.1 Program function Description

After the program is run, combined with the imu data read from the ROS expansion board and the speed vel data, output an odom data that integrates imu and odom data, which can be applied to the positioning function.

4.2. Program Code Reference path

After SSH connection car, the location of the function source code is located at,

xxxxxxxxxx
/userdata/yahboomcar_ws/src/yahboomcar_bringup/launch/yahboomcar_bringup_launch.py

ekf fusion program code reference path,

xxxxxxxxxx
/userdata/yahboomcar_ws/src/robot_localization/launch/ekf.launch.py

4.3. Program Startup

4.3.1. Launch command

After SSH connects to the car, terminal input,

xxxxxxxxxx
ros2 launch yahboomcar_bringup yahboomcar_bringup_launch.py

4.3.2. View the node communication graph

Open the VM terminal and enter.

xxxxxxxxxx
ros2 run rqt_graph rqt_graph

[Note] As RDK-X3 board does not provide a graphical desktop environment, ros rqt related operations need to be implemented on the PC side (virtual machine side). That is, as long as the communication between the PC and the car is normal, you can directly run the relevant instructions without SSH connection to the car after opening the terminal.

It can be seen from the node input and output in the red box in the above figure that /ekf_filter_node receives odom_raw data and imu_data data for fusion, and finally outputs and publishes an odom data, which can be viewed through ros2 node tool.

xxxxxxxxxx
ros2 node info /ekf_filter_node

4.4 launch file parsing

Let's look at the main relevant nodes of the launch file.

• /driver_node: start the car chassis, obtain the wheel speed vel data and publish it to the /base_node node, obtain the imu data and publish it to the /imu_filter_node node.
• /base_node: receives vel data, converts it into odom_raw data through calculation, and publishes it to the /ekf_node node.
• /imu_filter_node: receives the imu data released by the chassis, filters it by its own algorithm, and sends the filtered imu/data data to the /ekf_node.
• /ekf_node: receives odom data published by /base_node and imu/data published by /imu_filter_node, uses its own algorithm to fuse and publish odom data.