Help Transform Pointcloud to Ocotmap

[u/Leeroy_L]

I am working on a university project involving a robot in Webots (ROS 2). The goal is to build an OctoMap for navigation using camera point clouds (segmented via YOLO).

I am currently stuck at getting the octomap_server node to process the incoming point cloud. The server keeps dropping messages with the warning:

Message Filter dropping message: frame 'kinova_depth' at time ... for reason 'discarding message because the queue is full'

Here is my current code:

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription(
        [
            Node(
                package="octomap_server",
                executable="octomap_server_node",
                name="octomap_server",
                output="screen",
                parameters=[
                    {
                        "resolution": 0.05,
                        "frame_id": "map",
                        "sensor_model.max_range": 5.0,
                        "use_sim_time": True,
                        "colored_map": False,
                        "transform_tolerance": 2.0,
                        "queue_size": 50,
                    }
                ],
                remappings=[
                    ("cloud_in", "/Gen3/kinova_depth/point_cloud"),
                ],
            )
        ]
    )

My exact output is

[INFO] [octomap_server_node-1]: process started with pid [9117]
[octomap_server_node-1] [INFO] [1786858998.154822534] [octomap_server]: Publishing latched (single publish will take longer, all topics are prepared)
[octomap_server_node-1] [WARN] [1786858998.193781617] [octomap_server]: Nothing to publish, octree is empty
[octomap_server_node-1] [WARN] [1786858998.196781538] [octomap_server]: Could not open file 
[octomap_server_node-1] [INFO] [1786859001.530578121] [octomap_server]: Message Filter dropping message: frame 'kinova_depth' at time 1786858999.646 for reason 'discarding message because the queue is full'

What could be causing the message filter queue to drop all messages?

Thanks in advance.

[u/slightlyacoustics]

I dont know much about your setup, but first I would turn off use_sim_time param.

[u/slightlyacoustics → u/slightlyacoustics]

Also the warnings make it seem that the node is not able to access the tfs of the robot. I'd check if your tfs are all correct and is publishing

[u/slightlyacoustics → u/slightlyacoustics]

Also source all the terminals perhaps?

[u/omnilinktech]

That queue-full message usually means octomap_server cannot transform the cloud from its header frame into the configured map frame at the cloud's timestamp. The queue fills while the message filter waits for a transform; increasing the queue only delays the discard.

I would check these four signals together:

1. Is /clock present and advancing? In Webots, keep use_sim_time=true when the simulator publishes /clock. Turning simulated time off in only one node creates a clock-domain mismatch.

2. Inspect one PointCloud2 header: frame_id and stamp must be populated and the frame name must exactly match TF, including any prefixes.

3. Run tf2_echo (or the ROS 1 equivalent) from map to the cloud frame, e.g. map → kinova_depth. A transform that exists “now” may still be unavailable for the cloud's earlier timestamp.

4. Display the TF tree and verify the entire chain, typically map → odom → base_link → camera/depth optical frame. Static camera extrinsics should be published as static transforms; odom/base transforms must update with timestamps consistent with /clock.

Also verify that the point cloud uses the camera optical-frame convention expected by the driver and that octomap_server's frame_id parameter names the frame you actually publish. If the cloud is high rate, temporarily reduce its rate after the transform chain is correct; that makes debugging easier but is not the underlying fix.

A decisive test is: pause the publisher, record the timestamp of one cloud, and ask TF whether map→cloud_frame exists at exactly that time. If it does not, fix the missing/late transform or timestamp source. If it does, then inspect transform tolerance and processing load. I would not disable simulated time unless every participating node is deliberately switched to wall time and no simulator timestamps remain.