$ show jizhang-cmu/autonomy_stack_go2
This repository implements a full autonomy stack for the Unitree Go2 quadruped robot, enabling autonomous navigation to a goal point in unknown environments. The system integrates SLAM (point_lio_unilidar), global route planning (far_planner), local path following with obstacle avoidance (local_planner), terrain analysis, sensor transformation, and teleoperation. It supports both real-robot operation and simulation via a Unity bridge. The codebase is built on ROS2 (Foxy/Humble) and is designed for research and experimentation in autonomous exploration. The architecture is modular, consisting of ROS2 packages organized into base_autonomy, route_planner, slam, and utilities groups. Key entry points are shell scripts (system_simulation.sh, system_real_robot.sh) that launch the full stack. The SLAM module uses LiDAR-inertial odometry with an Iterated Kalman Filter on Manifolds (IKFoM) and an incremental k-d tree (ikd-Tree) for point cloud management. The route planner builds visibility graphs from LiDAR scans and plans paths using contour detection and graph search. The local planner follows waypoints while avoiding obstacles using a precomputed path library and real-time terrain analysis. Data flows begin with LiDAR and IMU sensor data, which are processed by SLAM to produce odometry and registered point clouds. The route planner consumes these to generate a global path, which is then refined by the local planner into velocity commands. The system also includes RViz plugins for interactive waypoint/goalpoint setting and teleoperation. External dependencies include ROS2, PCL, OpenCV, nlohmann/json, and the Unitree SDK. The simulation environment uses Unity and a custom TCP endpoint for ROS2-Unity communication. While the codebase is functional and implements advanced algorithms, it exhibits characteristics of a research prototype: duplicated vendored libraries (nlohmann/json in three packages), hardcoded parameters, large monolithic source files, and mixed ROS1/ROS2 idioms. Some components, such as the IKFoM toolkit and ikd-Tree, are self-contained and demonstrate high craftsmanship, making them candidates for reuse in other robotics projects.
The LLM identified 5 files worth highlighting for reuse:
Implements an Iterated Kalman Filter on Manifolds (IKFoM) for robust state estimation in SLAM. Provides manifold types (SO3, S2, SEN) and a generic ESKF framework. Self-contained and can be extracted for any robotics state estimation task requiring Lie group or manifold representations.
An incremental k-d tree data structure for dynamic point cloud management. Supports efficient nearest neighbor, box, and radius searches with lazy deletion and automatic rebalancing. Designed for real-time LiDAR processing and reusable in any 3D spatial indexing context.
Computes the field-of-view (FOV) of a LiDAR sensor given a 3D bounding box and sensor pose. Determines which parts of the environment are visible, useful for occlusion-aware planning and sensor placement. Self-contained and adaptable to different sensor geometries.
A templated 3D grid data structure with ray tracing capabilities. Used by the terrain planner and scan handler for efficient spatial queries. Can be extracted for any application requiring voxel grid operations with line-of-sight calculations.
A generic TCP server that bridges ROS2 and Unity, dynamically creating ROS publishers, subscribers, and services based on Unity commands. Supports message type resolution via importlib. Can be extracted for any ROS2-Unity integration project.