-
Notifications
You must be signed in to change notification settings - Fork 8
Sensor Processing Node
Execution model: Event-driven; spins continuously
Triggering: Runs only when a new PointCloud2 message arrives
Callback frequency: Processes every 20th incoming cloud (frame skip)
Primary workload: CPU-heavy point cloud extraction + filtering
Expected output rate: ~1 Hz
Latency characteristics:
Most of the compute cost occurs in extract_all_points()
Filtering and repacking into PointCloud2 is lightweight
/src/autonomous_nav/autonomous_nav/sensor_processing_node.py
/zed/zed_node/pointcloud/cloud_registered
Message Type: sensor_msgs/msg/PointCloud2
Format: 1D array
Data Type: Tuple (x, y, z, rgb)
The zed_node publishes the data from the stereo depth camera through this topic. It's frame is zed_camera_frame.
/filtered_point_cloud
Message Type: sensor_msgs/msg/PointCloud2
Format: 1D array
Data Type: Tuple (x, y, z)
This topic contains cleaned, range-limited, finite 3D points extracted from the ZED cloud_registered data. It is designed to be used by mapping systems such as octomap_server, voxel grids, or obstacle detection nodes.
This node does not have any services or actions.
Possible services or actions to add:
- Service to toggle filtering parameters
- Service to force a new pointcloud snapshot
min_range
Used to determine the minimum distance a point can be otherwise it is filtered.
max_range
Used to determine the maximum distance a point can be otherwise it is filtered.
camera_frame_id
The reference frame for the output cloud.
The sensor_processing_node processes raw ZED point clouds and outputs a clean, filtered subset suitable for mapping and occupancy-grid construction. When a point cloud arrives on /zed/zed_node/point_cloud/cloud_registered, the node first throttles the rate by only processing every 20th frame. It then extracts all XYZ points from the flat binary PointCloud2 array by manually unpacking each point based on its point_step and field offsets.
After extraction, the node removes invalid data such as NaN/Inf values and applies a configurable depth filter (min_range < z < max_range). This eliminates noise near the camera and distant low-confidence points. The remaining valid points are repacked into a new, compact PointCloud2 message containing only XYZ fields (no color) and published to /filtered_point_cloud. The resulting cloud is significantly smaller, unorganized (height = 1), and optimized for downstream mapping tools such as octomap_server.
This filtering pipeline reduces compute load, ensures input cleanliness, and provides a consistent perception layer for the autonomous navigation stack.
Python Modules:
math, struct, sys, numpy,
Ros2 packages:
rclpy, sensor_msgs.msg.PointCloud2, sensor_msgs.msg.PointField, std_msgs.msg.Header,
External Dependencies:
zed_wrapper, TF2