Vector Robotics Ground Autonomy Stack

This repository provides a generalizable autonomy stack for ground robots. The stack is designed to be embodiment-agnostic (wheeled, legged, etc.) by selecting a robot configuration via ROBOT_CONFIG_PATH.

It includes:

• SLAM (mapping + localization)
• Base autonomy (terrain analysis, collision avoidance, waypoint following)
• Route planning:
  • FAR Planner (default, visibility graph based planner)
  • PCT Planner (optional; efficient global navigation in multi-layer 3D structures)
• Exploration planning (TARE Planner)

For the ROS API (topics/services), see AUTONOMY_STACK_README.md.

Supported Environment

• Ubuntu 22.04 + ROS 2 Humble
• Ubuntu 24.04 + ROS 2 Jazzy

Quick Start

1) Install ROS + system dependencies

After installing ROS 2 Humble or Jazzy, source it in your shell: Replace $ROS_DISTRO with humble or jazzy in the commands below.

echo "source /opt/ros/$ROS_DISTRO/setup.bash" >> ~/.bashrc
source ~/.bashrc

Install core dependencies:

sudo apt update
sudo apt install -y \
  ros-$ROS_DISTRO-desktop-full \
  ros-$ROS_DISTRO-pcl-ros \
  libpcl-dev \
  git

Additional native dependencies (MID360 + SLAM builds)

If you are building the MID360 driver and SLAM from source (real robot setup), install the additional C++ toolchain + math/logging deps:

sudo apt update
sudo apt install -y \
  cmake \
  libgoogle-glog-dev \
  libgflags-dev \
  libatlas-base-dev \
  libeigen3-dev \
  libsuitesparse-dev

Build Livox-SDK2 (required for livox_ros_driver2)

Livox-SDK2 is vendored under this repo at src/utilities/livox_ros_driver2/Livox-SDK2:

cd src/utilities/livox_ros_driver2/Livox-SDK2
mkdir -p build && cd build
cmake ..
make -j"$(nproc)"
sudo make install

Then build the ROS 2 driver package:

cd ~/autonomy_stack_mecanum_wheel_platform
colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-select livox_ros_driver2

Build SLAM dependencies (Sophus / Ceres / GTSAM)

These dependencies are vendored under src/slam/dependency/. If you don’t already have them installed system-wide, you can install them from source (or use your distro packages, e.g. libceres-dev).

# Sophus
cd src/slam/dependency/Sophus
mkdir -p build && cd build
cmake .. -DBUILD_TESTS=OFF
make -j"$(nproc)"
sudo make install

# Ceres Solver
cd src/slam/dependency/ceres-solver
mkdir -p build && cd build
cmake ..
make -j"$(nproc)"
sudo make install

# GTSAM
cd src/slam/dependency/gtsam
mkdir -p build && cd build
cmake .. -DGTSAM_USE_SYSTEM_EIGEN=ON -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF
make -j"$(nproc)"
sudo make install
sudo /sbin/ldconfig

Then build the SLAM packages:

cd ~/autonomy_stack_mecanum_wheel_platform
colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-select arise_slam_mid360 arise_slam_mid360_msgs

2) Build

From the repository root:

colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release

Simulation-only build (skips SLAM + MID360 driver):

colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release \
  --packages-skip arise_slam_mid360 arise_slam_mid360_msgs livox_ros_driver2

3) Configure environment variables (robot + optional localization map)

Robot configuration

ROBOT_CONFIG_PATH selects the robot-specific YAML under src/base_autonomy/local_planner/config/.

Examples:

# Wheeled example
export ROBOT_CONFIG_PATH="mechanum_drive"

# Legged examples (see src/base_autonomy/local_planner/config/unitree/)
export ROBOT_CONFIG_PATH="unitree/unitree_g1"
# export ROBOT_CONFIG_PATH="unitree/unitree_go2"
# export ROBOT_CONFIG_PATH="unitree/unitree_go2_fast"
# export ROBOT_CONFIG_PATH="unitree/unitree_go2_stairs"
# export ROBOT_CONFIG_PATH="unitree/unitree_b1"

Map configuration (localization mode) 🚧 Under development

The stack uses MAP_PATH to switch from SLAM/mapping mode to localization mode.

• MAP_PATH is a file prefix (no extension)
• SLAM loads: "$MAP_PATH.pcd" (or .txt legacy)
• PCT planner loads: "$MAP_PATH_tomogram.pickle"

Recommended map layout:

/home/user/maps/warehouse/
├── map.pcd
└── map_tomogram.pickle

Set MAP_PATH to the prefix (.../map):

export MAP_PATH="/home/user/maps/warehouse/map"

To generate the tomogram from a PCD (requires PCT planner dependencies; see src/route_planner/PCT_planner/README.md):

source install/setup.bash
ros2 run pct_planner pcd_to_tomogram.py /home/user/maps/warehouse/map.pcd -o /home/user/maps/warehouse/map_tomogram.pickle

Run

Simulation

Unity environment model

The stack supports Unity-based simulation environments. Download a Unity environment model and unzip to:

Download a Unity environment model for the Mecanum wheel platform and unzip the files to the 'src/base_autonomy/vehicle_simulator/mesh/unity' folder. The environment model files should look like below. For computers without a powerful GPU, please try the 'without_360_camera' version for a higher rendering rate.

mesh/
    unity/
        environment/
            Model_Data/ (multiple files in the folder)
            Model.x86_64
            UnityPlayer.so
            AssetList.csv (generated at runtime)
            Dimensions.csv
            Categories.csv
        map.ply
        object_list.txt
        traversable_area.ply
        map.jpg
        render.jpg

Base autonomy (simulation)

./system_simulation.sh

With route planner (simulation)

./system_simulation_with_route_planner.sh

To use PCT planner instead of FAR planner, run directly via launch and set use_pct_planner:=true:

source install/setup.bash
ros2 launch vehicle_simulator system_simulation_with_route_planner.launch.py use_pct_planner:=true

With exploration planner (simulation)

./system_simulation_with_exploration_planner.sh

Real robot

1) Add user to dialout

sudo adduser "$USER" dialout
sudo reboot now

2) MID360 Ethernet configuration (recommended)

Set the MID360 Ethernet interface IPv4 to:

• Address: 192.168.1.5
• Netmask: 255.255.255.0
• Gateway: 192.168.1.1

Then configure the MID360 IP inside:

src/utilities/livox_ros_driver2/config/MID360_config.json

"ip": "192.168.1.1xx" where xx are the last two digits of the serial number.

Verify:

ping 192.168.1.1xx

More detailed setup notes: DIMOS_SETUP.md.

3) Launch

Base autonomy + SLAM:

./system_real_robot.sh

With route planner (Choose this by default):

./system_real_robot_with_route_planner.sh

With exploration planner:

./system_real_robot_with_exploration_planner.sh

To use PCT planner instead of FAR planner:

source install/setup.bash
ros2 launch vehicle_simulator system_real_robot_with_route_planner.launch.py use_pct_planner:=true

Operating modes (RViz + joystick)

Base autonomy (smart joystick, waypoint, and manual modes)

• Smart joystick mode (default): robot follows joystick intent but still avoids obstacles.
  • Use the RViz teleop panel or the controller sticks to command motion.
  • If the system is in another mode, commanding motion will switch back to smart joystick mode.
• Waypoint mode: robot follows waypoints and still avoids obstacles.
  • Use RViz “Waypoint” to set a nearby waypoint around the robot.
  • Route/exploration planners also drive the base autonomy in waypoint mode.
  • If using the route planner / exploration planner UI, click Resume Navigation to Goal to resume waypoint-mode navigation.
• Manual mode (no obstacle checking): robot follows joystick commands without collision avoidance.
  • This is intended for careful operator control only.

Example waypoint sender:

source install/setup.bash
ros2 launch waypoint_example waypoint_example.launch

Controller mapping (Xbox / PS controllers)

This stack consumes /joy directly. The current code expects these indices:

• Manual drive:
  • Forward/back: axes[4]
  • Left/right strafe: axes[3]
  • Yaw: axes[0] (Mode 2 convention)
• Hold-to-enable autonomy / obstacle checking:
  • Autonomy enable: axes[2] (pressed -> value < -0.1)
  • Obstacle checking enable: axes[5] (released -> value > -0.1; pressed disables obstacle checking)
• Clear terrain map button: buttons[5]

For most Xbox controllers on Linux (xpad), buttons[5] is RB. For PS controllers it may map differently.

If the mapping is off on your machine, verify indices by watching /joy while pressing controls:

source install/setup.bash
ros2 topic echo /joy

Bagfile workflow

Record (on robot, while running):

source install/setup.bash
ros2 bag record /imu/data /lidar/scan -o 'bagfolder_path'

Run the stack on a bag:

./system_bagfile.sh
./system_bagfile_with_route_planner.sh
./system_bagfile_with_exploration_planner.sh

Play the bag (in a separate terminal):

source install/setup.bash
ros2 bag play 'bagfolder_path/bagfile_name.mcap'

Example bags: https://drive.google.com/drive/folders/1G1JYkccvoSlxyySuTlPfvmrWoJUO8oSs?usp=sharing

Notes / Troubleshooting

• RViz on Wayland can be problematic on Jazzy. If needed, switch to X11.
• If Unity bridge fails at startup, retry the launch (ROS-TCP-Endpoint can be flaky).
• If joystick isn’t recognized, replug the USB dongle.
• Wheel type changes: update config:=omniDir vs config:=standard in local planner launch.
• If SLAM drifts or the robot starts behaving strangely, clear the terrain map and retry:
  • Press the controller button mapped to joy.buttons[5] (often RB on Xbox).
  • Or publish to /map_clearing (clears around the robot by distance):

source install/setup.bash
ros2 topic pub /map_clearing std_msgs/msg/Float32 "data: 8.0" --once

Credits / References

Development of the autonomy stack is led by Ji Zhang's group at Carnegie Mellon University.

Relevant projects:

• SLAM: upgraded implementation of LOAM
• Base autonomy: inspired by Autonomous Exploration Development Environment
• FAR Planner: https://github.com/MichaelFYang/far_planner
• PCT Planner paper & demos: https://byangw.github.io/projects/tmech2024/