Go2 Planner Suite

CUDA-Accelerated Autonomous Navigation for Unitree Go2 Quadruped Robot

Features • Architecture • Quick Start • CUDA Acceleration • FAQ

---

Overview

This repository contains a complete autonomous navigation stack for the Unitree Go2 quadruped robot, featuring:

• DLIO - Direct LiDAR-Inertial Odometry with CUDA-accelerated GICP
• Open3D SLAM - Dense mapping and localization
• Far Planner - GPU-accelerated visibility graph planning
• Terrain Analysis - Real-time traversability assessment
• Local Planner - Reactive obstacle avoidance

---

Architecture

Data Flow

LiDAR + IMU  ──►  DLIO  ──►  Open3D SLAM  ──►  Terrain Analysis  ──►  Far Planner  ──►  Local Planner  ──►  Go2 Robot
                   │              │                   │
              /odom_dlio    /state_estimation    /terrain_map
                                                      │
                                               Terrain Analysis Ext
                                                      │
                                               /terrain_map_ext

Key Topics

Topic	Source	Description
/odom_dlio	DLIO	LiDAR-inertial odometry
/state_estimation	Open3D SLAM	Fused localization
/registered_scan	Open3D SLAM	Registered point cloud in map frame
/terrain_map	Terrain Analysis	Local traversability (near-field)
/terrain_map_ext	Terrain Analysis Ext	Extended traversability (far-field)

TF Tree

---

Features

Component	Description	Acceleration
DLIO	LiDAR-Inertial Odometry	CUDA (GICP)
Far Planner	Global path planning	CUDA (Visibility Graph)
Boundary Handler	Obstacle boundary processing	CUDA
Terrain Analysis	Local traversability (4m radius)	CPU + OpenMP
Terrain Analysis Ext	Extended traversability (40m radius)	CPU + OpenMP
Local Planner	Reactive navigation	CPU

---

Terrain Analysis Tuning

The terrain analysis nodes require tuning based on your robot's physical dimensions.

Robot Parameters

Parameter	terrain_analysis	terrain_analysis_ext	Description
vehicleHeight	0.4	0.4	Robot height (m) - obstacles within this height are marked
minRelZ / lowerBoundZ	-0.5	-0.5	Min Z below base_link to consider
maxRelZ / upperBoundZ	1.0	1.0	Max Z above base_link to consider
terrainUnderVehicle	N/A	-0.1	Assumed ground level when no data (base_link on floor = small negative)
terrainConnThre	N/A	0.3	Max elevation change for connected terrain

Tuning for Different Robots

For a robot with base_link on the floor:

<!-- terrain_analysis.launch -->
<param name="vehicleHeight" value="YOUR_ROBOT_HEIGHT" />
<param name="minRelZ" value="-0.5" />  <!-- Captures slight slopes -->

<!-- terrain_analysis_ext.launch -->
<param name="terrainUnderVehicle" value="-0.1" />  <!-- Small margin below floor -->

For a robot with base_link elevated (e.g., at center of mass):

<!-- terrain_analysis.launch -->
<param name="vehicleHeight" value="YOUR_ROBOT_HEIGHT" />
<param name="minRelZ" value="-BASE_LINK_HEIGHT - 0.5" />  <!-- Goes below ground level -->

<!-- terrain_analysis_ext.launch -->
<param name="terrainUnderVehicle" value="-BASE_LINK_HEIGHT - 0.1" />

---

Visualization

Simulation Environment

Visibility Graph (CUDA-Accelerated)

The cyan lines show the visibility graph - navigation nodes that can "see" each other without obstacles. This computation is GPU-accelerated for real-time performance.

---

Quick Start

Prerequisites

• ROS2 Humble
• CUDA 11.0+ (for GPU acceleration)
• Unitree Go2 SDK (for real robot)

Installation

# Clone the repository
git clone https://github.com/Quadruped-dyn-insp/Go2_planner_suite.git
cd Go2_planner_suite

# Build all workspaces
./scripts/build.sh

Launch (Simulation)

./scripts/sim.sh

Launch (Real Robot)

./scripts/launch.sh

---

CUDA Acceleration

What's Accelerated?

1. DLIO - GICP Registration

Operation	CPU Time	GPU Time	Speedup
Point Transform	O(N) seq	O(N/1024) parallel	~100x
KNN Search	O(NxM) seq	O(NxM/1024) parallel	~50x
Hessian Computation	O(N) seq	O(N/1024) parallel	~100x

2. Far Planner - Visibility Graph

Scenario	Nodes	Edges	CPU Time	GPU Time
Small	100	500	2.5 sec	10 ms
Medium	500	2000	4 min	100 ms
Large	1000	5000	42 min	500 ms

Key CUDA Kernels

// DLIO - Point cloud registration
__global__ void transformPointsKernel(...);
__global__ void knnSearchKernel(...);
__global__ void computeHessianKernel(...);

// Far Planner - Visibility checking
__global__ void ComputeVisibilityConnections(...);
__device__ bool IsEdgeCollidePolygons_GPU(...);
__device__ bool doIntersect_GPU(...);

---

Project Structure

Go2_planner_suite/
├── scripts/
│   ├── build.sh              # Build all workspaces
│   ├── setup.sh              # Environment setup
│   ├── launch.sh             # Launch real robot
│   └── sim.sh                # Launch simulation
├── config/                   # Global configuration
├── docs/
│   ├── images/               # Documentation images
│   └── setup/                # Setup guides
├── tools/                    # Utility scripts and debugging tools
└── workspaces/
    ├── autonomous_exploration/       # Mid-layer navigation framework
    │   ├── local_planner/           # Reactive obstacle avoidance
    │   ├── terrain_analysis/        # Local traversability mapping (near-field)
    │   ├── terrain_analysis_ext/    # Extended traversability mapping (far-field)
    │   └── go2_simulator/           # Gazebo simulation for Go2
    ├── dlio/                        # CUDA-accelerated LiDAR-Inertial odometry
    │   └── src/nano_gicp/cuda/      # GICP CUDA kernels
    ├── far_planner/                 # CUDA-accelerated global planner
    │   ├── far_planner/             # Core visibility graph planner + CUDA
    │   └── boundary_handler/        # Obstacle boundary CUDA kernels
    ├── open3d_slam_ws/              # Open3D SLAM for dense mapping
    └── pipeline_launcher/           # System orchestration & launch management

---

FAQ

General

Q: What robot is this designed for?

A: Unitree Go2 quadruped robot with a Livox Mid-360 LiDAR and built-in IMU. It can be adapted for other robots by modifying the URDF and sensor configurations.

Q: Can I run this without a GPU?

A: Yes, but with reduced performance. The CUDA kernels have CPU fallbacks, but expect 10-100x slower planning and odometry in complex environments.

Q: What's the minimum GPU requirement?

A: Any CUDA-capable GPU with compute capability 6.0+ (Pascal or newer). Recommended: GTX 1060 or better for real-time performance.

Odometry and Localization

Q: Why is my RViz display blank?

A: Check these in order:

1. Verify /state_estimation is publishing: ros2 topic hz /state_estimation
2. Check TF tree is connected: ros2 run tf2_tools view_frames
3. Set RViz Fixed Frame to map
4. Ensure /registered_scan has data: ros2 topic echo /registered_scan --once

Q: DLIO is not receiving IMU data?

A: Check:

1. IMU topic name matches config: ros2 topic list | grep imu
2. IMU data rate is sufficient (>100Hz recommended)
3. Timestamps are synchronized with LiDAR

Q: Open3D SLAM shows "Failed to add odometry pose to buffer"?

A: This means DLIO odometry isn't reaching Open3D SLAM. Verify:

1. DLIO is running and publishing /odom_dlio
2. Topic remapping is correct in launch file
3. Timestamps are valid (not zero)

Planning

Q: The visibility graph has too many/few connections?

A: Adjust these parameters in Far Planner config:

• nav_clear_dist: Minimum clearance from obstacles (increase = fewer connections)
• project_dist: Maximum connection distance (decrease = fewer long connections)

Q: Path planning is slow even with GPU?

A: Check:

1. CUDA is actually being used: look for "CUDA available" in logs
2. Reduce number of navigation nodes if environment is too complex
3. Verify GPU isn't thermal throttling: nvidia-smi

Q: Robot doesn't follow the planned path?

A: The local planner may be overriding due to obstacles. Check:

1. /terrain_map shows correct obstacles
2. Local planner parameters aren't too aggressive
3. TF between map and base_link is accurate

Simulation

Q: Gazebo crashes on startup?

A: Common fixes:

1. Install missing dependencies: pip install lxml
2. Kill zombie processes: pkill -9 gzserver; pkill -9 gzclient
3. Check GPU drivers: nvidia-smi
4. Reduce world complexity

Q: Robot falls through the ground in simulation?

A: Check:

1. Spawn height in launch file (should be ~0.275m for Go2)
2. Gazebo physics step size isn't too large
3. Contact sensor plugin is loaded

Q: Controller manager service not available?

A: The robot model didn't spawn correctly. Check:

1. spawn_entity.py completed without errors
2. URDF/Xacro files are valid
3. Gazebo plugins are installed

Building and Dependencies

Q: CUDA compilation fails?

A: Ensure:

1. CUDA toolkit is installed: nvcc --version
2. Environment is set: source /usr/local/cuda/bin/setup.sh
3. CMake can find CUDA: check CMAKE_CUDA_COMPILER
4. GPU architecture matches: set CMAKE_CUDA_ARCHITECTURES

Q: Missing ROS2 packages?

A: Install common dependencies:

sudo apt install ros-humble-pcl-ros ros-humble-tf2-ros \
  ros-humble-nav-msgs ros-humble-geometry-msgs \
  ros-humble-gazebo-ros-pkgs

Q: Python module not found errors?

A: ROS2 Humble uses Python 3.10. If using conda:

conda deactivate  # Use system Python for ROS
# OR
pip install <package> --target=/opt/ros/humble/lib/python3.10/site-packages

---

Configuration

Key Parameters

Parameter	File	Description
sensor_frame	DLIO config	LiDAR frame name
nav_clear_dist	Far Planner	Obstacle clearance
terrain_resolution	Terrain Analysis	Grid cell size
local_planner_freq	Local Planner	Control loop rate

Topic Remapping

# Common remappings for real robot
/velodyne_points: /livox/lidar
/imu/data: /livox/imu
/odom: /odom_dlio

---

Performance

Benchmarks (RTX 3060, Intel i7-11800H)

Module	Input Size	CPU Time	GPU Time
DLIO GICP	10K points	45 ms	3 ms
Far Planner	500 nodes	240 sec	0.1 sec
Terrain Analysis	100K points	15 ms	15 ms*

*Terrain analysis uses CPU+OpenMP (CUDA version planned)

---

Contributing

1. Fork the repository
2. Create a feature branch: git checkout -b feature/amazing-feature
3. Commit changes: git commit -m 'Add amazing feature'
4. Push to branch: git push origin feature/amazing-feature
5. Open a Pull Request

---

License

This project is licensed under the MIT License - see the LICENSE file for details.

---

Acknowledgments

• DLIO - Base odometry implementation
• FAR Planner - Planning algorithms
• Unitree Robotics - Go2 robot platform

---

_{Built for autonomous quadruped navigation}