Can't produce map with namespaced slam-toolbox

Question

I am trying to simulate a robot with a namespace using slam-toolbox but I can't get it to publish a map and I keep getting the message "[async_slam_toolbox_node-6] [INFO] [1710771843.519004315] [rover1.slam_toolbox]: Message Filter dropping message: frame 'rover1/base_link/lidar' at time 62.640 for reason 'discarding message because the queue is full'" .

Could someone help me with what am I doing wrong/missing in the setup?

I am using ROS2 Humble and Gazebo Fortress.

This is my tf-tree: The output from ros2 topic list:

/client_count
/clock
/connected_clients
/parameter_events
/rosout
/rover1/camera
/rover1/clock
/rover1/cmd_vel
/rover1/imu/data
/rover1/joint_states
/rover1/map
/rover1/map_metadata
/rover1/odom
/rover1/parameter_events
/rover1/pose
/rover1/robot_description
/rover1/rosout
/rover1/scan
/rover1/scan/points
/rover1/slam_toolbox/feedback
/rover1/slam_toolbox/graph_visualization
/rover1/slam_toolbox/scan_visualization
/rover1/slam_toolbox/update
/rover1/tf
/rover1/tf_static

The output from ros2 node list:

/rosapi
/rosapi_params
/rosbridge_websocket
/rover1/bridge_node
/rover1/rover1_camera_stf
/rover1/rover1_imu_stf
/rover1/rover1_lidar_stf
/rover1/rover1_robot_state_publisher
/rover1/rviz2
/rover1/slam_toolbox
/rover1/transform_listener_impl_5aa38d46bf40
/rover1/transform_listener_impl_607d9fbb99f0

How the slam-toolbox is launched (I tried to remap every topic I could think of, thinking that maybe it would solve the problem but no luck.):

    slam_toolbox_node = launch_ros.actions.Node(
launch.substitutions.LaunchConfiguration('use_sim_time')}],
        parameters=[{'use_sim_time': launch.substitutions.LaunchConfiguration('use_sim_time')}],
        remappings=[('/odom', 'odom'), 
                    ('/map','map'), 
                    ('/map_metadata', 'map_metadata'), 
                    ('/scan','scan'),
                    ('/slam_toolbox/scan_visualization', 'slam_toolbox/scan_visualization'),
                    ('/joint_states','joint_states'), 
                    ('/imu/data','imu/data'),
                    ('/slam_toolbox/graph_visualization', 'slam_toolbox/graph_visualization'), 
                    ('/robot_description','robot_description'),
                    ('/tf', 'tf'), 
                    ('/tf_static', 'tf_static'), 
                    #('/clock','clock'), 
                    #('/parameter_events','parameter_events'),
                    ('/robot_description','robot_description'),
                    #('/rosout','rosout'),
                    ('/camera','camera'),
                    ('/cmd_vel','cmd_vel'),
                    ('/pose','pose'),
                    ('/scan/points','scan/points'),
                    ('/slam_toolbox/feedback','slam_toolbox/feedback'),
                    ('/slam_toolbox/update','slam_toolbox/update'),],
        package='slam_toolbox',
        executable='async_slam_toolbox_node',
        name='slam_toolbox', 
        output='screen')

And finally the output of ros2 node info /rover1/slam-toolbox:

/rover1/slam_toolbox
  Subscribers:
    /clock: rosgraph_msgs/msg/Clock
    /parameter_events: rcl_interfaces/msg/ParameterEvent
    /rover1/map: nav_msgs/msg/OccupancyGrid
    /rover1/scan: sensor_msgs/msg/LaserScan
    /rover1/slam_toolbox/feedback: visualization_msgs/msg/InteractiveMarkerFeedback
  Publishers:
    /parameter_events: rcl_interfaces/msg/ParameterEvent
    /rosout: rcl_interfaces/msg/Log
    /rover1/map: nav_msgs/msg/OccupancyGrid
    /rover1/map_metadata: nav_msgs/msg/MapMetaData
    /rover1/pose: geometry_msgs/msg/PoseWithCovarianceStamped
    /rover1/slam_toolbox/graph_visualization: visualization_msgs/msg/MarkerArray
    /rover1/slam_toolbox/scan_visualization: sensor_msgs/msg/LaserScan
    /rover1/slam_toolbox/update: visualization_msgs/msg/InteractiveMarkerUpdate
    /rover1/tf: tf2_msgs/msg/TFMessage
  Service Servers:
    /rover1/slam_toolbox/clear_changes: slam_toolbox/srv/Clear
    /rover1/slam_toolbox/describe_parameters: rcl_interfaces/srv/DescribeParameters
    /rover1/slam_toolbox/deserialize_map: slam_toolbox/srv/DeserializePoseGraph
    /rover1/slam_toolbox/dynamic_map: nav_msgs/srv/GetMap
    /rover1/slam_toolbox/get_interactive_markers: visualization_msgs/srv/GetInteractiveMarkers
    /rover1/slam_toolbox/get_parameter_types: rcl_interfaces/srv/GetParameterTypes
    /rover1/slam_toolbox/get_parameters: rcl_interfaces/srv/GetParameters
    /rover1/slam_toolbox/list_parameters: rcl_interfaces/srv/ListParameters
    /rover1/slam_toolbox/manual_loop_closure: slam_toolbox/srv/LoopClosure
    /rover1/slam_toolbox/pause_new_measurements: slam_toolbox/srv/Pause
    /rover1/slam_toolbox/save_map: slam_toolbox/srv/SaveMap
    /rover1/slam_toolbox/serialize_map: slam_toolbox/srv/SerializePoseGraph
    /rover1/slam_toolbox/set_parameters: rcl_interfaces/srv/SetParameters
    /rover1/slam_toolbox/set_parameters_atomically: rcl_interfaces/srv/SetParametersAtomically
    /rover1/slam_toolbox/toggle_interactive_mode: slam_toolbox/srv/ToggleInteractive
  Service Clients:

  Action Servers:

  Action Clients:

Lidar data does get published to the /rover1/scan topic.

Answer 1

I suppose the warning you are receiving is being generated because the lidar data is being stored but not processed, and that's why it is prompting that the queue is full.

It's been a while since I coded in ROS2 (I must return to it early on). I have seen a remapping from /odom to odom but in your tf_tree the odom frame is being called /rover1/odom. Are you sure you have remapped the typical frames such as base_link, odom, etc. to match your namespace? Also, why is your rqt publishing "default_authority" for the broadcaster of the frames?

Answer 2

I had similar errors

queue full

Because TF from various sources had different timestamps (I used real robot). So TF, odometry (which came from robot) had different (older) time stamps than lidar (from PC). Since slamtoolbox needs to match data from various sources (TF, odom, lidar) it needs to have accurate time stamps.

That's why you have queue full, it thinks the data is older (or newer) and waits for corresponding data...and queue fills up.

On my real robot I solved it by setting up chrony and syncing time.

On your simulated robot and Gazebo, search for use_sim_time variable, maybe you need to set it up for every node: using a simulation? Set use_sim_time to true for all nodes you start

Anyways, check if you have time set up properly, you can also check time with:

ros2 topic echo /scan --field header