ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

Rviz2 and Igntion Gazebo: Message filter dropping message because queue is full

asked 2022-09-20 03:14:10 -0500

Kingslayer gravatar image

Greetings. I am new to ROS2 and I am having a difficulty in using the ROS_IGN_GAZEBO_DEMOS package to test different sensors models.

The ROS_IGN_GAZEBO_DEMOS package works fine when I directly run it from the workspace I built the package in. I do not get any error while running like this and the visualization of the LaserScan and Lidar points work fine in Rviz

But when I use the same code and build it in another package, Rviz fails to load the LaserScan topic and Lidar points gives me the following error:

[rviz2-3] [INFO] [1663661037.983189664] [rviz]: Message Filter dropping message: frame 'model_with_lidar/link/gpu_lidar' at time 13.600 for reason 'discarding message because the queue is full'

I am using the following launch file :

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.actions import IncludeLaunchDescription
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration

from launch_ros.actions import Node


def generate_launch_description():

    pkg_ros_ign_gazebo_demos = get_package_share_directory('ros_ign_gazebo_demos')
    pkg_ros_ign_gazebo = get_package_share_directory('ros_ign_gazebo')

    ign_gazebo = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(pkg_ros_ign_gazebo, 'launch', 'ign_gazebo.launch.py')),
        launch_arguments={
            'ign_args': '-r gpu_lidar_sensor.sdf'
        }.items(),
    )

    # RViz
    rviz = Node(
       package='rviz2',
       executable='rviz2',
       arguments=['-d', os.path.join(pkg_ros_ign_gazebo_demos, 'rviz', 'gpu_lidar.rviz')],
       condition=IfCondition(LaunchConfiguration('rviz'))
    )

    # Bridge
    bridge = Node(
        package='ros_ign_bridge',
        executable='parameter_bridge',
        arguments=['lidar@sensor_msgs/msg/LaserScan@ignition.msgs.LaserScan',
                   '/lidar/points@sensor_msgs/msg/PointCloud2@ignition.msgs.PointCloudPacked'],
        output='screen'
    )

    return LaunchDescription([
        ign_gazebo,
        DeclareLaunchArgument('rviz', default_value='true',
                              description='Open RViz.'),
        bridge,
        rviz,
    ])

Any assistance in this regard would be greatly helpful.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2022-12-25 16:15:49 -0500

Luczia gravatar image

updated 2022-12-25 16:21:03 -0500

I believe Rviz misses a tf (transform information) between the map frame and your model_with_lidar/link/gpu_lidar frame.

To correct this, you could publish a static tf, by launching in another terminal :

ros2 run tf2_ros static_transform_publisher 0 0 0 0 0 0 map model_with_lidar/link/gpu_lidar

More infos about tfs here.

edit flag offensive delete link more

Comments

why would you impose a static transform on 2 frame that are not static?

guidout gravatar image guidout  ( 2023-06-24 19:07:01 -0500 )edit

Question Tools

Stats

Asked: 2022-09-20 03:14:10 -0500

Seen: 849 times

Last updated: Dec 25 '22