This project is an obstacle detection and tracking framework designed for a hydrofoiling Unmaned Surface Vehicle (USV), using a LiDAR sensor and a ego-vehicle localization estimate given by an INS. This work serves as a perception layer for a future obstacle avoidance and path planning implementation. The project processes raw point cloud data, filters it and represents the obstacles in the environment either with an Occupancy Grid Map or a set of Bounding Boxes with a velocity estimate, or even a combination of both. The code is integrated with ROS2 Humble, it expects to receive the appropriate Tfs and an input raw point cloud. Outputs are the filtered point cloud, marker arrays and occupancy grid maps, all published with standard ROS2 topics. Througough testing has been made to prove that the algorithm runs in real-time.
A complete master thesis report of this project can be read here (link still missing). It includes a literature review of related scintific work, a complete explanation and justification of the chosen methods, and an analysis and discussion of results.
The project is purposely made for a surface maritime environment, where point clouds often detect water surface reflctions. The test platform was a hydrofoiling USV called Evolo, which often reaches high Roll and Pitch angles, that were also dealt with by this perception pipeline.
This package preprocesses raw 3D LiDAR data into a filtered 2d representation. It then creates a rolling local occupancy grid map with static obstacles and it performs multi-object tracking with dynamic obstacles.
The package outputs several representations of a filtered point cloud. You can choose if this filtered data is used to build a rolling local occupancy grid map, if it gets clustered and tracked with multi-object tracking. There is also an option to segment the clusters with dynamic and static labels, then tracking the dynamic clussters and building the local map with the static ones.
Preprocessing steps:
- Transformation of the point cloud from the LiDAR sensor's frame to the vehicle's base footprint frame.
- IMU-informed RANSAC to estimate the water surface plane, followed by removal of all points at or below the surface of the water.
- Dynamic Radius Outlier Filtering.
- Region-based pass-through filter for points too high up to be considered obstacles.
- Data reduction from a 3D point cloud to a 2D Laser Scan.
- Time decay: Temporal accumulation of 2D Laser Scans to construct a more complete 2D point cloud, gathered over a specified time horizon.
The implementation follows a modular design, where each high-level process is encapsulated within its own ROS 2 node. This approach enables:
- Parallel execution of nodes using multi-threading
- Clear input-output interfaces for each node
- Easier debugging through well-defined communication over ROS 2 topics
The full ROS 2 node graph for the proposed pipeline (including static-dynamic segmentation) is shown below:
Node graph illustrating the modular pipeline design with static-dynamic segmentation.
The system is composed of the following main nodes:
-
/base_footprint_publisher_node
Uses localization data from the SBG Systems INS to publish the vehicle’sbase_footprintcoordinate frame. -
/pointcloud_preprocessing_filtering
Transforms the raw LiDAR point cloud into thebase_footprintframe, filters the data, and publishes several representations of the filtered point cloud. -
/mapping_and_clustering_node
Performs clustering on time-accumulated laser scans received via/filtered/ls/laserscan_accumulated. It creates a segmentation map from the filtered points (from/filtered/ls/laserscan), segments clusters into static or dynamic classes using this map, and routes the data accordingly. It also publishes an occupancy grid map based on the static-classified points. Odometry from thebase_footprintframe is used to propagate the rolling occupancy maps. -
/object_tracking_node
Handles data association, computes 2D bounding boxes, and manages a Kalman filter for each tracked object.
git clone https://github.com/AlexReis1313/evolo-preprocessing-pointcloud.git
cd /evolo-preprocessing-pointcloud/
colcon build
source the workspace
Takes in raw LiDAR Point CLouds in sensor frame (or base_link), filters it and outputs several 2d representations in base_footprint. Also produces base_footprint
ros2 launch pointcloud_preprocessing pointcloud_preprocessing_launch_evolo.py
ros2 run clustering_segmentation clustering_segmentation
ros2 run bb_dataass_tracking bounding_box_node_main
ros2 launch pointcloud_preprocessing mapping_launch.py
ros2 launch lidar_cluster euclidean_spatial.launch.py topic:=/filtered/ls/pointcloud/accumulated
ros2 run occupancy_grid occupancy_grid_mqtt
ros2 launch str_json_mqtt_bridge waraps_bridge.launch
For alternative vehicle parameters, use one of the following options:
ros2 launch pointcloud_preprocessing pointcloud_preprocessing_launch_boat.py
ros2 launch pointcloud_preprocessing pointcloud_preprocessing_launch_sim.py
https://www.youtube.com/playlist?list=PLS_kAMaDCf634gqgQ6XRzQY9NJVkBzv5d
This reository uses the pointcloud_to_laserscan ROS 2 package that provides components to convert sensor_msgs/msg/PointCloud2 messages to sensor_msgs/msg/LaserScan messages and back.
Source code can be found here https://github.com/ros-perception/pointcloud_to_laserscan
This repository adapted and used the local ocupancy grid map implementation in https://github.com/hiwad-aziz/ros2_occupancy_grid