pointcloud2_costmap_plugin

A ROS 2 Nav2 costmap layer plugin that ingests live PointCloud2 data and marks obstacle and free cells directly into the Nav2 layered costmap — developed for an Autonomous Surface Vehicle (ASV) at the University of Michigan.

Companion to Hybrid-A-Star: this plugin provides the obstacle map; Hybrid-A-Star plans the kinematically feasible path on top of it.

---

Overview

Nav2's built-in obstacle layers are designed primarily for 2D LaserScan data. On platforms that carry a 3D sensor (LiDAR, depth camera, sonar) but operate in a planar environment — such as an ASV on open water — a custom layer is needed to project the 3D pointcloud into the 2D costmap frame.

pointcloud2_costmap_plugin implements a nav2_costmap_2d::CostmapLayer that subscribes to any sensor_msgs/PointCloud2 topic, projects each point into the costmap grid, and marks cells as LETHAL_OBSTACLE or FREE_SPACE. The layer integrates cleanly into the Nav2 layered costmap stack with no changes required to other stack components.

---

Key Features

• Drop-in Nav2 costmap layer — registered as a plugin; loaded via the standard Nav2 params YAML
• Live PointCloud2 ingestion — subscribes to any sensor_msgs/PointCloud2 topic (LiDAR, depth camera, sonar)
• Obstacle and free-cell tracking — separately tracks lethal obstacle cells and free-space cells per update cycle
• Incremental bound updates — updateBounds reports only the changed region, minimising unnecessary costmap recomputation
• Clearable layer — implements reset() and isClearable() for full Nav2 lifecycle compliance
• ASV-validated — designed and tested on a surface vehicle platform at the University of Michigan

---

How It Works

sensor_msgs/PointCloud2  (3D points from LiDAR / depth camera / sonar)
        |
  pointCloudCallback()
        |
  Project each point into 2D costmap grid (x, y -> cell i, j)
        |
  +-----+------+
  |            |
obstacle    free space
cells[]     cells[]
  |            |
  +-----+------+
        |
  updateBounds()  ->  report dirty bounding box to layered costmap
        |
  updateCosts()   ->  write LETHAL_OBSTACLE / FREE_SPACE into master_grid
        |
  Nav2 LayeredCostmap  ->  Hybrid-A-Star global planner

---

Requirements

Dependency	Version
ROS 2	Jazzy or Humble
Nav2	matching ROS 2 version
sensor_msgs	standard ROS 2 package
C++	17
CMake	3.16+

---

Installation

# 1. Clone into your ROS 2 workspace
cd ~/ros2_ws/src
git clone https://github.com/yslin0524/pointcloud2_costmap_plugin.git

# 2. Install dependencies
cd ~/ros2_ws
rosdep install --from-paths src --ignore-src -r -y

# 3. Build
colcon build --symlink-install --packages-select pointcloud2_costmap_plugin
source install/setup.bash

---

Usage

Loading the plugin in Nav2

Add the layer to your Nav2 costmap params YAML:

local_costmap:
  local_costmap:
    ros__parameters:
      plugins: ["pointcloud_layer", "inflation_layer"]
      pointcloud_layer:
        plugin: "my_pointcloud_costmap_plugin::MyPointCloudLayer"
        enabled: true
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        inflation_radius: 0.5

The plugin subscribes to the PointCloud2 topic configured in onInitialize. Adjust the topic name to match your sensor driver.

Pairing with Hybrid-A-Star

This plugin is designed to work alongside the Hybrid-A-Star global planner. The plugin populates the costmap; Hybrid-A-Star queries it to find kinematically feasible, collision-free paths:

planner_server:
  ros__parameters:
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "hybrid_a_star_planner::HybridAStarPlanner"
      obstacle_threshold: 65

---

Application

Developed during an independent research study at the University of Michigan, applied to an Autonomous Surface Vehicle (ASV). The ASV carries a sensor that produces PointCloud2 data; this plugin projects those points into the Nav2 costmap so the Hybrid-A-Star planner can avoid obstacles in real time.

---

License

MIT