CARVER-GEN3 is an autonomous mobile robot platform with rear-wheel drive, Ackermann steering, and LiDAR-based navigation. Built on ROS2 Humble for research in autonomous navigation, SLAM, and path tracking control.
Version: 3.0
ROS2 Distribution: Humble
Operating System: Ubuntu 22.04 LTS
License: MIT
Localization via MOLA-LO |
SLAM via MOLA-LO |
Controller Path Tracking Showcase |
- Key Features
- System Architecture
- Hardware Requirements
- Installation
- Package Descriptions
- Additional Hardware Connections
- Configuration
- Quick Start Guide
- Path Tracking Controllers
- Localization and Mapping
- Troubleshooting
- References
- Drive System: Rear-wheel drive (Using ODrive controllers)
- Steering: Front Ackermann steering with absolute encoders
- Sensing: Livox MID360 LiDAR, BNO055 IMU, AMT212EV encoders
- Control: 3× STM32G474RE microcontrollers running Micro-ROS
- SLAM: MOLA framework for mapping (Complete MOLA guide here)
- Localization: GICP/ICP Cov matching
- Controllers: Stanley, Pure Pursuit and Combined path tracking
- Real-time Control: 10 Hz control loop, 10 Hz localization
- Wheelbase: 0.8m
- Max Speed: 3.5 m/s
- Steering Range: ±0.6 rad (±34°)
- Localization Accuracy: <0.02m
View System Architecture (PDF)
The Carver system consists of three main subsystems: Perception & Localization, Decision & Control, and Hardware Interface (Micro-ROS). The system uses pre-planned paths with MOLA-SLAM for localization, enabling autonomous path following.
The micro_ros_agent bridges three STM32 microcontrollers to ROS2:
-
bno055_publisher- Reads BNO055 IMU data from STM32- Publishes:
/bno055_data(Float64MultiArray)
- Publishes:
-
carver_steering- Controls steering servo, reads AMT212 encoder feedback- Subscribes:
/steering_angle(Float32) - Publishes:
/amt_publisher(amt_read)
- Subscribes:
-
carver_interface- Handles accelerator input, emergency button, and lighting control- Subscribes:
/light_signal_command(Int8) - Publishes:
/accl_publisher(UInt16),/carver_emergency(Bool),/accel_direction(Int8),/carver_mode(Int8)
- Subscribes:
Sensors:
-
carver_livox_lidar.launch.py- Launches Livox 3D LiDAR- Publishes:
/livox/lidar(PointCloud2),/livox/imu(Imu)
- Publishes:
-
bno055_imu.py- Processes raw IMU data from STM32- Subscribes:
/bno055_data - Publishes:
/imu(sensor_msgs/Imu)
- Subscribes:
Localization:
MOLA_lidar_localization- 3D LiDAR-based localization using pre-built maps- Subscribes:
/livox/lidar,/imu - Publishes:
/state_estimator/pose(nav_msgs/Odometry) - Requires:
map.simplemap+map.mm(pre-built map files)
- Subscribes:
Mode Management:
carver_mode.py- Switches modes- Subscribes:
/controller_speed,/manual_speed - Publishes:
/target_speed,/carver_mode
- Subscribes:
Path Tracking Controllers:
carver_controller.py- Implements Stanley / Pure Pursuit / Combined controller- Subscribes:
/state_estimator/pose - Publishes:
/controller_speed,/steering_angle - Requires:
path.yaml(pre-planned waypoints)
- Subscribes:
Manual Control:
carver_manual_steering.py- Manual joystick/handle control- Subscribes:
/accel_direction,/accl_publisher - Publishes:
/manual_speed
- Subscribes:
Motor Control:
carver_odrive.py- Interfaces with ODrive motor controller- Subscribes:
/target_speed - Outputs: Velocity setpoint to ODrive (with velocity feedback)
- Subscribes:
Phase 1: Mapping (Offline)
- Drive the vehicle manually while recording LiDAR/IMU data
- Build
map.simplemapandmap.mmusing MOLA offline tools - Define waypoints and save as
path.yaml
Phase 2: Localization (Runtime)
- Livox LiDAR publishes point cloud to
/livox/lidar - BNO055 IMU data flows: STM32 → micro_ros →
bno055_imu.py→/imu - MOLA localizes against pre-built map, publishes pose to
/state_estimator/pose
Phase 3: Path Following (Runtime)
- Controller loads waypoints from
path.yaml - Controller receives current pose from
/state_estimator/pose - Stanley/Pure Pursuit algorithm computes:
- Steering angle →
/steering_angle→ STM32 → Servo - Target speed →
/controller_speed→carver_mode→/target_speed→ ODrive
- Steering angle →
Phase 4: Actuation
carver_steering(STM32) receives steering command, controls servo via PWM- AMT212 encoder provides steering angle feedback
carver_odrivesends velocity setpoint to ODrive S1 Pro- ODrive returns velocity feedback for closed-loop control
Minimum Requirements:
- CPU: Quad-core x86-64 @ 2.0 GHz
- RAM: 16 GB DDR5
- Storage: 256 GB SSD
- OS: Ubuntu 22.04 LTS
- USB: Multiple USB 3.0 ports
Recommended (Tested Configuration):
- System: ASUS ROG G17 (2025)
- CPU: AMD Ryzen 9 7945HX (16 cores, 32 threads)
- GPU: NVIDIA RTX 4060 (optional, for future vision features)
- RAM: 24 GB DDR5
- Storage: NVMe SSD 2TB
3× STM32G474RE Units:
- ARM Cortex-M4F @ 170MHz
- 512KB Flash, 128KB RAM
- Hardware FPU, USB 2.0
- Running Micro-ROS firmware
Unit 1: Encoder interface (AMT212EV)
Unit 2: Steering control
Unit 3: BNO055 IMU interface
Livox MID360 LiDAR:
- 360° horizontal FOV
- Up to 70m range
- 200K points/sec
- Connection: Ethernet or USB
- Power: 12V DC
BNO055 9-DOF IMU:
- Built-in sensor fusion
- Connection: I2C to STM32
- Power: 3.3V or 5V
AMT212EV Absolute Encoders:
- 12-bit resolution (4096 positions/rev)
- Interface: SPI
- Power: 5V DC
ODrive v3.6 Motor Controllers:
- Connection: USB 2.0
- Power: 24V DC
- Hall sensor feedback
Motors:
- Brushless DC motors (rear wheels)
- Built-in Hall sensors
- Direct drive
Front Steering:
- DC Motor actuated
- Ackermann geometry linkage
- AMT212EV encoders for feedback
- Ubuntu 22.04 LTS
- ROS2 Humble (Installation Guide)
- rosdep initialized
sudo rosdep init && rosdep updategit clone https://github.com/Whan000/CARVER-GEN3.git
cd CARVER-GEN3/carver_ws# Point Cloud Library
sudo apt install libpcl-dev ros-humble-pcl-ros ros-humble-pcl-conversions pcl-tools
# Python dependencies
pip3 install numpy scipy matplotlib transforms3d pyserial pyyaml odrive
# Clone Livox driver into src/
git clone https://github.com/Livox-SDK/livox_ros_driver2.git src/livox_ros_driver2
# Install ROS dependencies via rosdep
rosdep install --from-paths src --ignore-src -r -y
# Build
colcon build
source install/setup.bash
echo "source ~/CARVER-GEN3/carver_ws/install/setup.bash" >> ~/.bashrcmkdir -p ~/microros_ws/src
cd ~/microros_ws
git clone -b $ROS_DISTRO https://github.com/micro-ROS/micro_ros_setup.git src/micro_ros_setup
sudo apt update && rosdep update
rosdep install --from-paths src --ignore-src -y
colcon build
source install/local_setup.bash
# Build firmware and agent
ros2 run micro_ros_setup create_firmware_ws.sh host
ros2 run micro_ros_setup build_firmware.sh
source install/local_setup.bash
ros2 run micro_ros_setup create_agent_ws.sh
ros2 run micro_ros_setup build_agent.sh
source install/local_setup.bashSTM32/Nucleo Setup: See Micro-ROS with Nucleo Board for firmware flashing and configuration.
# Add user to dialout group
sudo usermod -a -G dialout $USER
# ODrive udev rules
echo 'SUBSYSTEM=="usb", ATTR{idVendor}=="1209", ATTR{idProduct}=="0d32", MODE="0666"' | \
sudo tee /etc/udev/rules.d/91-odrive.rules
sudo udevadm control --reload-rules && sudo udevadm trigger
# Logout/login requiredros2 pkg listExpected packages:
- amt212ev_interfaces
- bno055_imu
- carver_bringup
- carver_controller
- carver_description
- carver_manager
- carver_odrive
- carver_simulation
- lidar_localization_ros2
- ndt_omp_ros2
URDF robot model and visualization.
Contents:
urdf/- Robot structure definitionmeshes/- STL fileslaunch/- Visualization launch filesconfig/- Controller parameters
Usage:
ros2 launch carver_description simple_display.launch.pySystem startup launch files.
Launch Files:
bringup.launch.py- Full system startupmanual_steering.launch.py- Manual control onlyslam.launch.py- SLAM / mapping mode (see MOLA-SLAM for details)uros.launch.py- Micro-ROS agents only
Usage:
ros2 launch carver_bringup bringup.launch.pyPath tracking controllers.
Controllers:
carver_stanley.py- Stanley Controllercarver_purepursuit.py- Pure Pursuit Controllercarver_combined.py- Adaptive Combined Controller
Trajectory Files:
path/trajectory200.yaml- 200 path sampling pointspath/trajectory1000.yaml- 1000 path sampling points
ODrive motor controller interface.
Scripts:
carver_odrive.py- Main interface nodedummy_steering.py- Testing/simulation
Mode management and manual control.
Scripts:
carver_mode.py- Mode switchingcarver_manual_steering.py- Manual control
Gazebo simulation environment.
Usage:
ros2 launch carver_simulation simulation-full.launch.pyBNO055 IMU data converter (Micro-ROS → standard ROS2).
NDT-OMP based localization.
Features:
- Real-time point cloud registration
- Map loading and management
- Pose estimation
OpenMP-accelerated NDT library.
Ethernet Connection:
# Configure static IP
sudo ifconfig eth0 192.168.1.50 netmask 255.255.255.0
# Test connection
ping 192.168.1.10Create udev rules:
sudo nano /etc/udev/rules.d/99-carver-usb.rulesAdd rules (adjust serial numbers to match your devices):
# Encoder Interface
SUBSYSTEM=="tty", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740", \
ATTRS{serial}=="ENCODER_UNIT_001", SYMLINK+="carver_interface", \
MODE="0666", GROUP="dialout"
# Steering Control
SUBSYSTEM=="tty", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740", \
ATTRS{serial}=="STEERING_UNIT_001", SYMLINK+="carver_steering", \
MODE="0666", GROUP="dialout"
# IMU
SUBSYSTEM=="tty", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740", \
ATTRS{serial}=="IMU_UNIT_001", SYMLINK+="carver_bno055", \
MODE="0666", GROUP="dialout"Reload rules:
sudo udevadm control --reload-rules
sudo udevadm triggerVerify:
ls -l /dev/carver_*Always use symbolic names in code:
# ✓ Good
serial_port = '/dev/carver_interface'
# ✗ Bad - will break!
serial_port = '/dev/ttyACM1'File: carver_description/urdf/carver_params.xacro
<!-- Physical dimensions -->
<xacro:property name="wheelbase_length" value="0.8"/> <!-- meters -->
<xacro:property name="track_width" value="0.4"/>
<xacro:property name="wheel_radius" value="0.075"/>
<!-- Steering limits -->
<xacro:property name="max_steering_angle" value="0.6"/> <!-- ~34° -->
<!-- Mass properties -->
<xacro:property name="base_mass" value="25.0"/> <!-- kg -->File: odrive_config/OdriveConfig.json
Key Parameters:
{
"axis0.config.motor.pole_pairs": 4,
"axis0.config.motor.phase_resistance": 0.0354,
"axis0.config.motor.current_soft_max": 50.0,
"axis0.config.motor.current_hard_max": 100.0,
"axis0.controller.config.control_mode": 2, // Velocity
"axis0.controller.config.vel_limit": 60.0, // Rev/s
"axis0.controller.config.vel_gain": 3.0,
"hall_encoder0.config.enabled": true,
"hall_encoder0.config.edges_calibrated": true
}Calibrate each ODrive:
# In odrivetool
odrv0.axis0.requested_state = AXIS_STATE_FULL_CALIBRATION_SEQUENCE
# Wait for completion
odrv0.save_configuration()
odrv0.reboot()File: carver_description/config/carver_controller_param.yaml
controller_manager:
ros__parameters:
update_rate: 500 # Hz
use_sim_time: true
steering_controller:
ros__parameters:
joints:
- base_front_left_steering_joint
- base_front_right_steering_joint
wheel_velocity_controller:
ros__parameters:
joints:
- base_rear_left_wheel_joint
- base_rear_right_wheel_jointMOLA SLAM is used to create maps for autonomous navigation.
Complete MOLA configuration and usage guide:
https://github.com/Whan000/MOLA-SLAM
1. Start System:
# Terminal 1
ros2 launch carver_bringup bringup.launch.py
# Terminal 2 - Localization
ros2 launch mola_bringup mola_slam.py2. Set Initial Pose:
- mola_localize_launch.py will show a GUI
- Key your initial pose
3. Start Controller:
Stanley: default:
ros2 run carver_controller carver_stanley.py Pure Pursuit: default:
ros2 run carver_controller carver_purepursuit.py Combined: default:
ros2 run carver_controller carver_combined.py4. Emergency Stop:
Press hardware E-stop
The Stanley Controller uses a non-linear control law to keep the vehicle's front axle on the path. It combines heading error (
Topics:
Subscribed:
/state_estimator/pose(nav_msgs/Odometry)/imu(sensor_msgs/Imu)
Published:
/steering_angle(std_msgs/Float32)/controller_speed(std_msgs/Float32)/path_visualization(nav_msgs/Path)
Parameters:
k_heading: 0.25 # Heading error gain (Implicit in control law)
k_crosstrack: 0.1 # Cross-track error gain (k)
ks_gain: 0.1 # Velocity softening factor (k_s, for denominator)
target_speed: 1.25 # m/s
max_steer: 0.6 # rad (~34°)
lookahead_distance: 5.0 # m (Note: Stanley uses *closest point* for CTE, not lookahead distance)Steering Law:
The control law calculates the required steering angle (
Where:
-
$\delta$ : steering angle -
$\theta_e$ : heading error (orientation difference between vehicle and path segment) -
$e$ : cross-track error (perpendicular distance from front axle to path) -
$v$ : current longitudinal velocity -
$k_{\text{crosstrack}}$ : proportional gain for correcting lateral error -
$k_s$ : speed softening constant to prevent singularity at$v \approx 0$
Tuning:
-
$\uparrow$ k_heading= more aggressive heading correction -
$\uparrow$ k_crosstrack= faster lateral correction -
$\uparrow$ lookahead_distance= smoother but may cut corners
Best for:
- Preplanned trajectories
- Racing or time trials
The Pure Pursuit Controller uses geometric control based on driving the vehicle towards a single target point located some distance (
Topics:
Subscribed:
/state_estimator/pose(nav_msgs/Odometry)/imu(sensor_msgs/Imu)
Published:
/steering_angle(std_msgs/Float32)/controller_speed(std_msgs/Float32)/path_visualization(nav_msgs/Path)
Parameters:
wheelbase: 0.8 # m (L in the formula)
max_steering_angle: 0.6 # rad
min_lookahead: 3.0 # m (Ld minimum)
max_lookahead: 5.0 # m (Ld maximum)
lookahead_gain: 1.0 # Speed-dependent gain (k_l in dynamic Ld formula)
target_speed: 1.0 # m/sSteering Law:
The required steering angle (
Where:
-
$L$ : wheelbase -
$\alpha$ : angle between the vehicle's current heading and the vector to the lookahead point -
$l_d$ : lookahead distance
Dynamic Lookahead: The lookahead distance is often made proportional to the vehicle's speed for smooth performance:
Features:
- Automatic speed reduction in turns
- Monotonic waypoint progression
- Completion detection
Best for:
- Smooth curved paths
- Initial testing
The Combined Controller dynamically blends the outputs of the Pure Pursuit (
Algorithm Logic:
-
Calculate Path Curvature (
$\gamma$ ): Determine the path curvature at the current segment using the angle change between adjacent path vectors. -
Normalize Curvature (
$\gamma_{\text{norm}}$ ):$\gamma$ is normalized relative to a minimum turning radius constraint ($\gamma_{\text{norm}}$ ).
-
Calculate Weight Factors (
$\mathbf{k_{PP}}, \mathbf{k_{ST}}$ ): The weighting factors depend on the normalized curvature, varying between$K_{\min}$ (for straight sections) and$K_{\max}$ (for sharp turns).
-
Combine Outputs: The final steering command (
$\delta$ ) is a weighted average.
Control Law:
Tuning Parameters (Embedded in Code):
-
Weights:
$K_{\min} (0.2)$ and$K_{\max} (0.8)$ define the range of Pure Pursuit influence. -
Curvature Norm (
$\gamma_{\text{norm}}$ ): Derived from the minimum turning radius ($R_{\min} = 7.0\text{ m}$ ). -
Gains: Inherits tuning properties from both Stanley and Pure Pursuit (e.g.,
k_crosstrack,lookahead_gain_k1).
YAML Format:
waypoints:
- x: 0.0
y: 0.0
yaw: 0.0
- x: 5.0
y: 0.0
yaw: 0.0
- x: 10.0
y: 2.0
yaw: 0.3During development, we evaluated four LiDAR-based SLAM frameworks to determine the best fit for the Carver platform. Each method was tested with the Livox MID360 LiDAR under real-world conditions.
Methods Evaluated:
-
LIO-SAM - Tightly-coupled LiDAR-Inertial odometry via smoothing and mapping using factor graphs. Provides loop closure and GPS integration capabilities.
- Repository: https://github.com/TixiaoShan/LIO-SAM/tree/ros2
-
FAST-LIO / FAST-LIO2 - Fast direct LiDAR-Inertial odometry using iterated Kalman filter with incremental kd-tree (ikd-Tree). Known for computational efficiency and robustness.
- Repository: https://github.com/hku-mars/FAST_LIO/tree/ROS2
- ROS2 Fork: https://github.com/Ericsii/FAST_LIO_ROS2
-
lidarslam_ros2 (rsasaki) - Modular LiDAR SLAM with NDT/GICP scan matching and graph-based optimization. Includes separate localization package for navigation.
-
MOLA - Modular Optimization framework for Localization And mapping. Flexible pipeline architecture supporting multiple front-ends and back-ends with real-time performance.
- Repository: https://github.com/MOLAorg/mola
Why MOLA?
After extensive testing, MOLA was selected for the following reasons:
-
Superior Localization Accuracy - Achieved consistent sub-2cm accuracy in our test environment, outperforming other methods in repeated trials.
-
Robust Map Quality - Produced clean, dense point cloud maps with minimal drift over long trajectories.
-
Flexible Architecture - Modular pipeline design allows easy switching between ICP/GICP/NDT matching algorithms and different optimization backends.
-
Native Livox Support - Works seamlessly with non-repetitive scanning patterns of Livox MID360 without requiring point cloud preprocessing.
-
Separate Mapping and Localization - Clean separation between offline mapping (SLAM) and online localization modes, ideal for autonomous navigation workflows.
-
Active Development - Well-maintained with comprehensive documentation and responsive community support.
Two-Stage Approach:
- SLAM (Mapping): Use MOLA to create maps
- Localization (Navigation): Use ICP/GICP for real-time pose estimation → Configured in MOLA packages
Complete Documentation:
https://github.com/Whan000/MOLA-SLAM
For details on:
- MOLA installation and setup
- Configuration parameters
- Mapping best practices
- Troubleshooting
- Advanced features
See the dedicated MOLA-SLAM repository above.
Check devices:
ls -l /dev/ttyACM*
lsusb | grep ODrive
#Or just go to your browser and type for odriveGUITest individual components:
ros2 run micro_ros_agent micro_ros_agent serial --dev /dev/ttyACM0
ros2 run carver_odrive carver_odrive.pyFix permissions:
sudo chmod 666 /dev/ttyACM*Verify map loaded:
#Check on RViZ2 or MOLA-VizCheck LiDAR data:
ros2 topic echo /livox/lidar -n 1Check ODrive:
# In odrivetool
odrv0.axis0.error
odrv0.clear_errors()Run calibration:
odrv0.axis0.requested_state = AXIS_STATE_FULL_CALIBRATION_SEQUENCECheck control mode:
odrv0.axis0.controller.config.control_mode = 2 # Velocity
odrv0.save_configuration()Check pose:
ros2 topic echo /state_estimator/poseAdjust gains:
ros2 param set /stanley_controller k_heading 0.5Network (Ethernet):
ping 192.168.1.10
sudo ifconfig eth0 192.168.1.50 netmask 255.255.255.0Start driver:
ros2 launch livox_ros_driver2 rviz_MID360_launch.pyReduce load:
#Config in MOLA pipelines aim for bigger voxels sizeDisable visualization:
# Comment out RViz in launch fileUpdate dependencies:
rosdep update
rosdep install --from-paths src --ignore-src -r -yClean build:
cd ~/carver_ws
rm -rf build/ install/ log/
colcon buildCARVER-GEN3 Main Repository:
MOLA SLAM Configuration (Complete Guide):
- https://github.com/Whan000/MOLA-SLAM
- Installation, setup, parameters, mapping procedures
- Tested configuration for Livox MID360
- Troubleshooting and best practices
Livox MID360 Guidelines:
- https://github.com/Whan000/Livox-MID360
- Installation, setup, parameters
ROS2:
MOLA (Upstream):
Hardware:
- ODrive: https://docs.odriverobotics.com/
- Livox SDK: https://github.com/Livox-SDK/
- STM32: https://www.st.com/
This project exists thanks to all the people who contributed.
- MOLA: "A Modular Optimization Framework for Localization and Mapping" (RSS 2019)
- Stanley Controller: "Stanley: The Robot that Won the DARPA Grand Challenge"
- Pure Pursuit Controller: "Implementation of the Pure Pursuit Path Tracking Algorithm"
- Combined Controller: "Combined Path Following Controller for Autonomous Vehicles"