Navigation Function Package

Overview#

This function package adopts a hybrid architecture of 3D positioning and 2D navigation. The system integrates the FAST-LIO2 algorithm, utilizing error state Kalman filtering and dynamic ikd-Tree structure to achieve absolute positioning with extremely low drift in feature-degraded scenarios such as long corridors and open halls. By introducing a Nav2 adapter, it uses 3D poses for path planning and obstacle avoidance on a 2D plane.

This functional package provides a complete set of engineering implementation toolkits. Based on normal vector estimation and slope analysis, the toolkit converts 3D point clouds into 2D grid maps and generates high-quality Nav2 Costmaps. Meanwhile, the system supports hot switching between the "continuous patrol without stopping" and "precise point operation" modes, providing a full-process solution from mapping to navigation for the construction of industrial-grade autonomous mobile robots.

Open source address: https://github.com/Roboparty/roboparty_navigation

Usage Guide (see README.md in the repository for details)#

Prerequisites#

System Requirements#

Operating System: Ubuntu 22.04
ROS Version: ROS 2 Humble

Hardware Requirements#

LiDAR: Livox MID-360 or compatible models
IMU: Integrated or external IMU (gravitational unit should be 'g')
UWB Module: NLink LinkTrack (optional)

dependency#

Please refer to the README of each functional package to install detailed dependencies.

Universal Dependency

Bash

sudo apt update
sudo apt install -y \
    ros-humble-navigation2 \
    ros-humble-nav2-* \
    ros-humble-pcl-ros \
    ros-humble-pcl-conversions \
    ros-humble-tf2-ros \
    ros-humble-tf2-geometry-msgs \
    libceres-dev \
    ccache

Python Dependencies

Bash

pip install open3d numpy pyyaml Pillow scipy

Third-party library compilation

**Sophus ** (Recommended 1.22.10)

Bash

git clone https://github.com/strasdat/Sophus.git
cd Sophus/
git checkout 1.22.10
mkdir build && cd build
cmake ..
make -j$(nproc)
sudo make install

fmt (need to enable -fPIC)

Bash

git clone https://github.com/fmtlib/fmt
cd fmt
mkdir build && cd build
cmake .. -DCMAKE_POSITION_INDEPENDENT_CODE=ON
make -j$(nproc)
sudo make install

Note: You must add add_compile_options(-fPIC) to CMakeLists.txt!!

Livox SDK2 & Driver

It is recommended to install the Livox driver outside the workspace. Reference links:Livox-SDK2 and livox_ros_driver2

Bash

# Livox SDK2
git clone https://github.com/Livox-SDK/Livox-SDK2.git
cd Livox-SDK2
mkdir build && cd build
cmake ..
make -j$(nproc)
sudo make install

# Livox ROS Driver2 (建议放在 src 目录外)
git clone https://github.com/Livox-SDK/livox_ros_driver2.git
cd livox_ros_driver2
./build.sh humble

Install#

1. Clone the repository#

Use the --recursive parameter to automatically clone submodules:

Bash

cd ~/
mkdir -p atom01_ws/src
cd atom01_ws/src
git clone --recursive https://github.com/Roboparty/atom01_navigation.git

2. Compile the workspace#

Bash

cd ~/atom01_ws
colcon build --symlink-install

Usage Instructions#

Before using any command, please ensure that the workspace environment variables have been loaded:

Bash

cd ~/atom01_ws
source install/setup.bash

1. Mapping and Localization#

For detailed operations, please refer to robots_localization_ros2/README.md.

**Brief Steps: **

Start the radar driver.
Modify the mapping_en parameter in config/robots_localization.yaml:
- true: Mapping Mode
- false: Positioning Mode

Start Node:

# 建图模式
# 注：建图和定位使用同一个启动文件，区别在于配置文件中的 mapping_en 参数
ros2 launch robots_localization_ros2 localization.launch.py

# 或 定位模式
ros2 launch robots_localization_ros2 localization.launch.py

2. Navigation and Map Processing#

Refence: nav2_localization_adapter/README.md。

**Brief Steps: **

PCD to 2D Map: Use the pcd2pgm.py tool to convert the map.

**Start Navigation **:

ros2 launch nav2_localization_adapter navigation.launch.py map:=<地图路径>

Configuration Instructions#

robots_localization_ros2#

Main configuration file:config/robots_localization.yaml

Key Parameters:

common.lid_topic: LiDAR point cloud topic
common.imu_topic: IMU data topic
preprocess.lidar_type: LiDAR type (Livox is 1)
mapping_en: Mapping enable (true: mapping, false: pure localization)
mapping.extrinsic_T: Translation from LiDAR to IMU
mapping.extrinsic_R: Rotation from LiDAR to IMU

nav2_localization_adapter#

For detailed configuration, please refer to the documentation within the package:nav2_localization_adapter

Known Issues and Solutions#

1. Radar connection issue#

Check IP configuration (the robot should not use a proxy)
Verify that the radar IP address matches the configuration
Ensure that the network interface settings are correct

Important Note#

Livox IMU: The unit of gravity is 'g', ensure that the normalization in the code matches this
External parameters of MID-360 IMU: Position in the LiDAR coordinate system is [0.011, 0.0234, -0.044]
Point Cloud Timestamp: Header time is the time of the earliest point; the time of an individual point is the offset relative to the header time (unit: milliseconds)
PCD Map: Place the PCD file in the specified map directory before starting the positioning mode