IMU Fusion Radar Mapping and Navigation

1. Source Code Description

We provide two sets of IMU function source code: one is the source code for the IMU driver only, and the other is the source code for the IMU fusion radar mapping and navigation example.

imu_ros2_device: This is the source code for the IMU driver only.

yahboomcar_ws: This is the source code for the IMU fusion radar mapping and navigation example.

Generally, the imu_ros2_device package is sufficient for installation on your own motherboard. The other packages shown in the examples require some dependencies, and errors may occur during installation and compilation. Users need to resolve environment dependency issues themselves. The source code here is for reference only. It will not run if the corresponding vehicle and radar equipment are not provided. The source code is implemented using our company's Rosmaster A1 vehicle product. Please be aware of this.

2. Radar Mapping

2.1 Pre-Use Configuration

This vehicle model is equipped with a USB camera, a depth camera, and two different models of LiDAR. However, since the products cannot be automatically recognized, the machine type and radar model need to be manually set.

Based on the vehicle model, the radar type and camera type should be modified as follows:

xxxxxxxxxx
root@ubuntu:/# cd
root@ubuntu:~# vim .bashrc

Find this location, press the 'i' key on the keyboard, and change it to the corresponding camera and radar model. Here, the default is tmini and nuwa.

After making the changes, save and exit Vim, then execute:

xxxxxxxxxx
root@raspberrypi:~# source .bashrc
--------------------------------------------------------
ROS_DOMAIN_ID: 61 | ROS: humble
my_robot_type: A1 | my_lidar: tmini | my_camera: nuwa
--------------------------------------------------------
root@raspberrypi:~# 

2.2 Program Startup

Start Map Building

The following map building algorithm can only be selected from two options; they cannot be started simultaneously.

Gamapping

xxxxxxxxxx
ros2 launch yahboomcar_nav map_gmapping_launch.py

cartographer

xxxxxxxxxx
ros2 launch yahboomcar_nav map_cartographer_launch.py

Input command to start rviz visual mapping

xxxxxxxxxx
ros2 launch yahboomcar_nav display_map_launch.py

The program has gamepad control enabled by default. If you are using a gamepad, you can directly connect to the receiver for control. If you want to use keyboard control, enter the following in the terminal:

xxxxxxxxxx
ros2 run yahboomcar_ctrl yahboom_keyboard

Then, control the car to slowly traverse the area requiring mapping. After mapping is complete, enter the following command to save the map:

xxxxxxxxxx
ros2 launch yahboomcar_nav save_map_launch.py

This will save a map named yahboomcar in:

~/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_nav/maps/yahboomcar.yaml

Two files will be generated: yahboomcar.pgm and yahboomcar.yaml.

Let's look at the contents of yahboom_map.yaml:

xxxxxxxxxx
image: yahboomcar.pgm
mode: trinary
resolution: 0.05
origin: [-9.02, -15.5, 0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.25

• image: Represents the map image, i.e., yahboomcar.pgm
• mode: This attribute can be one of trinary, scale, or raw, depending on the selected mode. Trinary mode is the default mode.
• resolution: Map resolution, meters per pixel
• origin: 2D pose (x, y, yaw) of the bottom left corner of the map. Here, yaw is a counter-clockwise rotation (yaw=0 means no rotation). Currently, many parts of the system ignore the yaw value.
• negate: Whether to reverse the meaning of white/black, free/occupied (the interpretation of the threshold is not affected)
• occupied_thresh: Pixels with an occupation probability greater than this threshold are considered fully occupied.
• free_thresh: Pixels with an occupation probability less than this threshold are considered completely free.

3. Navigation and Obstacle Avoidance

Robot vehicle terminal launches underlying sensor commands:

xxxxxxxxxx
ros2 launch yahboomcar_nav laser_bringup_launch.py

Input command to launch RViz visualization mapping (RViz visualization can be launched on either the vehicle terminal or the virtual machine terminal; choose one, but do not launch it repeatedly on both the virtual machine and the vehicle terminal :), the image below shows the input on the vehicle terminal:

xxxxxxxxxx
ros2 launch yahboomcar_nav display_nav_launch.py

At this point, the map is not yet loaded because the navigation program has not been started. Next, run the navigation node by entering the following in the terminal:

xxxxxxxxxx
ros2 launch yahboomcar_nav navigation_teb_launch.py 

Now you can see the map has loaded. Next, click on "2D Pose Estimate" to set the initial pose for the car. Based on the car's position in the actual environment, use the mouse to click and drag in RViz to move the car model to the position we set. As shown in the image below, if the area scanned by the radar roughly overlaps with the actual obstacles, the pose is accurate. After pose initialization, the robot model and the expanded region will appear in the RViz interface.

For single-point navigation, click the "2D Goal Pose" tool, then use the mouse in RViz to select a target point and target orientation, and then release.

The robot, considering its surroundings, plans a path and moves along it to the target point.

Once the robot successfully reaches the target point, the vehicle terminal will display "Goal succeeded," indicating successful navigation.