Integrating a sensor fusion stack into an existing ROS 2 system

Integrating a new perception stack into an existing production ROS 2 system is almost never a clean drop-in. The failure modes aren't in the algorithms — they're in the transport layer. Topic timing, QoS policy mismatches, executor thread contention, and callback scheduling gaps are where most integrations break down in practice, causing engineers to spend two weeks debugging behavior that looks like an algorithm problem but is actually a middleware problem.

This post covers the specific transport-layer issues we've observed when integrating PathVynt's ROS 2 nodes into existing robotics stacks, and how to diagnose and fix each one.

Understanding PathVynt's ROS 2 topic graph

PathVynt publishes four primary topic namespaces when running the full pipeline:

• /pathvynt/fused — sensor_msgs/PointCloud2 at fusion rate (default 20Hz). The fused LiDAR-camera point cloud with per-point uncertainty annotation.
• /pathvynt/obstacles — custom pathvynt_msgs/ObstacleArray at prediction rate (20Hz). Tracked obstacle states with trajectory forecasts.
• /pathvynt/occupancy — nav_msgs/OccupancyGrid at grid update rate (default 30Hz). Compatible with nav2's costmap_2d layer system.
• /pathvynt/pose — geometry_msgs/PoseWithCovarianceStamped at localization rate (10Hz). Map-relative pose with full 6×6 covariance matrix.

PathVynt subscribes to your sensor topics using the names you configure in your launch file. The default subscriptions follow the standard ROS 2 sensor naming convention: /sensor/velodyne/points for LiDAR, /sensor/camera/image_raw and /sensor/camera/camera_info for camera. Remap these in your launch description if your sensor drivers use different names.

QoS policy mismatches

The most common and most confusing integration problem. ROS 2's DDS-based transport requires that publisher and subscriber QoS policies be compatible. PathVynt's sensor input subscribers use SENSOR_DATA QoS by default — best-effort reliability, volatile durability, and a small queue depth (depth=5). This matches the default profile of most hardware sensor drivers.

The problem appears when your sensor driver was configured with SYSTEM_DEFAULT QoS (which is reliable delivery) and PathVynt's subscriber expects best-effort. ROS 2 will silently drop all messages — no error, no warning, just a subscriber that receives nothing. You can diagnose this with ros2 topic info /sensor/velodyne/points --verbose which will show the QoS of both the publisher and subscriber and whether they match.

To resolve: either configure your sensor driver to publish with best-effort QoS, or override PathVynt's subscriber QoS in your launch file:

from rclpy.qos import QoSProfile, ReliabilityPolicy
from pathvynt_ros2 import FusionNode

fusion = FusionNode(
    lidar_qos=QoSProfile(
        reliability=ReliabilityPolicy.RELIABLE,
        depth=10
    )
)

Note that using RELIABLE QoS for high-rate sensor topics (20Hz LiDAR) on a congested network or with slow disk storage can cause significant latency buildup in the reliability retransmission buffer. Profile your system's CPU and memory under load before switching to RELIABLE for production deployments.

Executor thread contention

PathVynt's fusion node is compute-intensive. Running it in a SingleThreadedExecutor alongside your nav2 nodes and your robot's hardware drivers is a reliable recipe for callback starvation. When the fusion engine's scan_callback takes 18ms (one full scan cycle), other callbacks queued behind it — including time-critical nav2 control loops — will be delayed.

The correct approach: run PathVynt nodes in a dedicated MultiThreadedExecutor with at least 4 threads, in a separate process from nav2. The ROS 2 inter-process communication via DDS is efficient enough for the message volumes PathVynt produces, and process isolation prevents a spike in PathVynt's compute load from affecting nav2's control rate.

In your launch description:

from launch_ros.actions import Node
import launch

def generate_launch_description():
    pathvynt_node = Node(
        package='pathvynt_ros2',
        executable='fusion_pipeline',
        name='pathvynt',
        parameters=['config/pathvynt_warehouse.yaml'],
        # Run in separate component container for process isolation
        output='screen',
    )
    return launch.LaunchDescription([pathvynt_node])

Replacing nav2's default costmap with PathVynt's occupancy grid

If you're using nav2, replacing its laser scan-based costmap layer with PathVynt's dynamic occupancy grid gives you immediate access to trajectory-prediction-informed costmap inflation. PathVynt's occupancy grid knows about predicted obstacle positions at t+0.5s — nav2's default laser scan layer does not.

PathVynt provides a nav2 costmap plugin: pathvynt_nav2_plugin::PathVyntCostmapLayer. Add it to your nav2 costmap configuration:

local_costmap:
  local_costmap:
    ros__parameters:
      plugins: ["pathvynt_layer", "inflation_layer"]
      pathvynt_layer:
        plugin: "pathvynt_nav2_plugin::PathVyntCostmapLayer"
        topic: /pathvynt/occupancy
        enabled: true
        prediction_horizon: 0.5

The prediction_horizon parameter controls which time horizon from PathVynt's obstacle predictions is used for costmap inflation. Setting this to 0.5s means obstacles predicted to be at a position in 0.5 seconds will be reflected in the costmap now — the planner sees future obstacle positions when computing trajectories. This is the feature that most meaningfully improves AMR throughput compared to reactive-only costmap approaches: the robot starts its avoidance maneuver earlier because it can see where the forklift will be.

Topic timing and message synchronization

PathVynt's fusion node uses an internal message synchronizer to match LiDAR scans with camera frames. If your LiDAR and camera clocks are not synchronized — using PTP or a shared hardware trigger — the synchronizer will frequently fail to find matching pairs within the default 15ms temporal tolerance window, causing high rates of dropped fusion cycles.

Check your synchronization rate with /pathvynt/diagnostics — the field fusion_sync_rate shows the percentage of scan cycles that produced a successful LiDAR-camera pair. Anything below 90% indicates a synchronization problem. Above 95% is the target for production operation.

If hardware synchronization isn't available on your platform, increase the temporal tolerance in FusionConfig.temporal_gap_tolerance_ms to 30–40ms. You will accept more latency in exchange for higher sync rates. The fusion output will be timestamped with the LiDAR scan time, so downstream nodes see consistent timestamp ordering regardless of the extended tolerance window.

For a complete integration walkthrough, see the PathVynt ROS 2 quickstart guide at /docs/quickstart.html and the full API reference at /docs/api-reference.html.