ROS2 Navigation - Multirobot Localization - Ex 6.2 base_scan reports error consistently

Hi,

I am currently on the last chapter of ROS2 Navigation (Galactic) Course - Unit 6: Multi-robot Navigation on the Multi-robot Localization Section.

I did as instructed in the Exercise 6.2. But multi_localization launch reports the following error consistently. I do not know what to do. [I checked this forum for same issue but I could not find.]
Message Filter dropping message: frame 'tb3_0/base_scan' at time 1644.601 for reason 'the timestamp on the message is earlier than all the data in the transform cache'

Here is my output of ros2 launch localization_server multi_localization.launch.py:

user:~/ros2_ws$ ros2 launch localization_server multi_localization.launch.py
[INFO] [launch]: All log files can be found below /home/user/.ros/log/2023-01-12-14-02-18-699080-1_xterm-3989
[INFO] [launch]: Default logging verbosity is set to INFO
[WARNING] [amcl-3]: there are now at least 2 nodes with the name /tb3_0/amcl created within this launch context
[INFO] [map_server-1]: process started with pid [3991]
[INFO] [amcl-2]: process started with pid [3993]
[INFO] [amcl-3]: process started with pid [3995]
[INFO] [lifecycle_manager-4]: process started with pid [3997]
[lifecycle_manager-4] [INFO] [1673532139.219664965] [lifecycle_manager_localization]: Creating
[lifecycle_manager-4] [INFO] [1673532139.241897382] [lifecycle_manager_localization]: Creating and initializing lifecycle service clients
[amcl-2] [INFO] [1673532139.249025078] [tb3_0.amcl]:
[amcl-2]        amcl lifecycle node launched.
[amcl-2]        Waiting on external lifecycle transitions to activate
[amcl-2]        See https://design.ros2.org/articles/node_lifecycle.html for more information.
[amcl-2] [INFO] [1673532139.249546683] [tb3_0.amcl]: Creating
[amcl-3] [INFO] [1673532139.252373025] [tb3_0.amcl]:
[amcl-3]        amcl lifecycle node launched.
[amcl-3]        Waiting on external lifecycle transitions to activate
[amcl-3]        See https://design.ros2.org/articles/node_lifecycle.html for more information.
[amcl-3] [INFO] [1673532139.252591501] [tb3_0.amcl]: Creating
[lifecycle_manager-4] [INFO] [1673532139.264220837] [lifecycle_manager_localization]: Starting managed nodes bringup...
[lifecycle_manager-4] [INFO] [1673532139.264282242] [lifecycle_manager_localization]: Configuring map_server
[map_server-1] [INFO] [1673532139.287763581] [map_server]:
[map_server-1]  map_server lifecycle node launched.
[map_server-1]  Waiting on external lifecycle transitions to activate
[map_server-1]  See https://design.ros2.org/articles/node_lifecycle.html for more information.
[map_server-1] [INFO] [1673532139.287897750] [map_server]: Creating
[map_server-1] [INFO] [1673532139.302619243] [map_server]: Configuring
[map_server-1] [INFO] [map_io]: Loading yaml file: /home/user/ros2_ws/install/map_server/share/map_server/config/turtlebot_area.yaml
[map_server-1] [DEBUG] [map_io]: resolution: 0.05
[map_server-1] [DEBUG] [map_io]: origin[0]: -2.06
[map_server-1] [DEBUG] [map_io]: origin[1]: -3.7
[map_server-1] [DEBUG] [map_io]: origin[2]: 0
[map_server-1] [DEBUG] [map_io]: free_thresh: 0.25
[map_server-1] [DEBUG] [map_io]: occupied_thresh: 0.65
[map_server-1] [DEBUG] [map_io]: mode: trinary
[map_server-1] [DEBUG] [map_io]: negate: 0
[map_server-1] [INFO] [map_io]: Loading image_file: /home/user/ros2_ws/install/map_server/share/map_server/config/turtlebot_area.pgm
[map_server-1] [DEBUG] [map_io]: Read map /home/user/ros2_ws/install/map_server/share/map_server/config/turtlebot_area.pgm: 151 X 143 map @ 0.05 m/cell
[lifecycle_manager-4] [INFO] [1673532139.339884730] [lifecycle_manager_localization]: Configuring tb3_0/amcl
[amcl-2] [INFO] [1673532139.340208804] [tb3_0.amcl]: Configuring
[amcl-2] [INFO] [1673532139.340369045] [tb3_0.amcl]: initTransforms
[amcl-3] [INFO] [1673532139.340391747] [tb3_0.amcl]: Configuring
[amcl-3] [INFO] [1673532139.340539177] [tb3_0.amcl]: initTransforms
[amcl-3] [INFO] [1673532139.348505891] [tb3_0.amcl]: initPubSub
[amcl-3] [INFO] [1673532139.350881351] [tb3_0.amcl]: Subscribed to map topic.
[lifecycle_manager-4] [INFO] [1673532139.361313075] [lifecycle_manager_localization]: Configuring tb3_1/amcl
[amcl-2] [INFO] [1673532139.361889842] [tb3_0.amcl]: initPubSub
[amcl-2] [INFO] [1673532139.368353362] [tb3_0.amcl]: Subscribed to map topic.
[amcl-3] [INFO] [1673532140.564882838] [tb3_0.amcl_rclcpp_node]: Message Filter dropping message: frame 'tb3_0/base_scan' at time 1644.401 for reason 'the timestamp on the message is earlier than all the data in the transform cache'
[amcl-3] [INFO] [1673532140.763781315] [tb3_0.amcl_rclcpp_node]: Message Filter dropping message: frame 'tb3_0/base_scan' at time 1644.601 for reason 'the timestamp on the message is earlier than all the data in the transform cache'
[amcl-3] [INFO] [1673532140.968774872] [tb3_0.amcl_rclcpp_node]: Message Filter dropping message: frame 'tb3_0/base_scan' at time 1644.801 for reason 'the timestamp on the message is earlier than all the data in the transform cache'
[amcl-3] [INFO] [1673532141.178373830] [tb3_0.amcl_rclcpp_node]: Message Filter dropping message: frame 'tb3_0/base_scan' at time 1645.000 for reason 'the timestamp on the message is earlier than all the data in the transform cache'
[amcl-3] [INFO] [1673532141.377200280] [tb3_0.amcl_rclcpp_node]: Message Filter dropping message: frame 'tb3_0/base_scan' at time 1645.201 for reason 'the timestamp on the message is earlier than all the data in the transform cache'
[amcl-3] [INFO] [1673532141.588359910] [tb3_0.amcl_rclcpp_node]: Message Filter dropping message: frame 'tb3_0/base_scan' at time 1645.401 for reason 'the timestamp on the message is earlier than all the data in the transform cache'
^C[WARNING] [launch]: user interrupted with ctrl-c (SIGINT)
[lifecycle_manager-4] [INFO] [1673532169.493158337] [rclcpp]: signal_handler(signal_value=2)
[amcl-3] [INFO] [1673532169.493171280] [rclcpp]: signal_handler(signal_value=2)
[amcl-2] [INFO] [1673532169.493173080] [rclcpp]: signal_handler(signal_value=2)
[map_server-1] [INFO] [1673532169.493192529] [rclcpp]: signal_handler(signal_value=2)
[lifecycle_manager-4] terminate called after throwing an instance of 'std::runtime_error'
[lifecycle_manager-4]   what():  tb3_1/amcl/change_state service client: interrupted while waiting for service
[map_server-1] [INFO] [1673532169.496480707] [map_server]: Destroying
[amcl-3] [INFO] [1673532169.508158851] [tb3_0.amcl]: Destroying
[amcl-2] [INFO] [1673532169.509591589] [tb3_0.amcl]: Destroying
[ERROR] [lifecycle_manager-4]: process has died [pid 3997, exit code -6, cmd '/opt/ros/galactic/lib/nav2_lifecycle_manager/lifecycle_manager --ros-args -r __node:=lifecycle_manager_localization --params-file /tmp/launch_params_5wua1uw9 --params-file /tmp/launch_params_16jzx00z --params-file /tmp/launch_params_by0ogr8w --params-file /tmp/launch_params_1xsvte8b'].
[INFO] [amcl-2]: process has finished cleanly [pid 3993]
[INFO] [map_server-1]: process has finished cleanly [pid 3991]
[INFO] [amcl-3]: process has finished cleanly [pid 3995]

Here is my multi_localization.launch.py:

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node


def generate_launch_description():

    map_file = os.path.join(get_package_share_directory(
        "map_server"), "config", "turtlebot_area.yaml")
    tb3_0_amcl_yaml = os.path.join(get_package_share_directory(
        "localization_server"), "config", "tb3_0_amcl.yaml")
    tb3_1_amcl_yaml = os.path.join(get_package_share_directory(
        "localization_server"), "config", "tb3_1_amcl.yaml")

    return LaunchDescription([

        Node(package="nav2_map_server",
             executable="map_server",
             name="map_server",
             output="screen",
             parameters=[{"use_sim_time": True},
                         {"topic_name": "map"},
                         {"frame_id": "map"},
                         {"yaml_filename": map_file}],
             emulate_tty=True),

        Node(namespace="tb3_0",
             package="nav2_amcl",
             executable="amcl",
             name="amcl",
             output="screen",
             parameters=[tb3_0_amcl_yaml],
             emulate_tty=True),

        Node(namespace="tb3_0",
             package="nav2_amcl",
             executable="amcl",
             name="amcl",
             output="screen",
             parameters=[tb3_1_amcl_yaml],
             emulate_tty=True),

        Node(package="nav2_lifecycle_manager",
             executable="lifecycle_manager",
             name="lifecycle_manager_localization",
             output="screen",
             parameters=[{"use_sim_time": True},
                         {"autostart": True},
                         {"bond_timeout": 0.0},
                         {"node_names": ["map_server",
                                         "tb3_0/amcl",
                                         "tb3_1/amcl"]}],
             emulate_tty=True),

    ])

# End of Code

Here is my tb3_0_amcl.yaml:

tb3_0/amcl:
  ros__parameters:
    use_sim_time: True
    alpha1: 0.2
    alpha2: 0.2
    alpha3: 0.2
    alpha4: 0.2
    alpha5: 0.2
    base_frame_id: "tb3_0/base_footprint"
    beam_skip_distance: 0.5
    beam_skip_error_threshold: 0.9
    beam_skip_threshold: 0.3
    do_beamskip: false
    global_frame_id: "map"
    lambda_short: 0.1
    laser_likelihood_max_dist: 2.0
    laser_max_range: 100.0
    laser_min_range: -1.0
    laser_model_type: "likelihood_field"
    max_beams: 60
    max_particles: 8000
    min_particles: 200
    odom_frame_id: "tb3_0/odom"
    pf_err: 0.05
    pf_z: 0.99
    recovery_alpha_fast: 0.0
    recovery_alpha_slow: 0.0
    resample_interval: 1
    robot_model_type: "differential"
    save_pose_rate: 0.5
    sigma_hit: 0.2
    tf_broadcast: true
    transform_tolerance: 1.0
    update_min_a: 0.2
    update_min_d: 0.25
    z_hit: 0.5
    z_max: 0.05
    z_rand: 0.5
    z_short: 0.05
    scan_topic: "scan"
    map_topic: "/map" # forced setting
    # ACTIVATE THE set_initial_pose WHEN YOU HAVE A PROPER initial_pose, by uncommenting the code below
    #set_initial_pose: true
    #initial_pose:
    # x: 7.778
    # y: -9.589
    # z: 0.0
    # a: -0.211

tb3_0/amcl_map_client:
  ros__parameters:
    use_sim_time: True

tb3_0/amcl_rclcpp_node:
  ros__parameters:
    use_sim_time: True

# End of File

For tb3_1_amcl.yaml I have all copy-pasted the same contents of tb3_0_amcl.yaml - and changed all tb3_0 to tb3_1.

I need some help.

Thanks,
Girish

Fixed this problem.

I had made a mistake in the launch file. It was a copy-paste mistake.

        Node(namespace="tb3_0",
             package="nav2_amcl",
             executable="amcl",
             name="amcl",
             output="screen",
             parameters=[tb3_0_amcl_yaml],
             emulate_tty=True),

        Node(namespace="tb3_0",   <--- This should have been "tb3_1"
             package="nav2_amcl",
             executable="amcl",
             name="amcl",
             output="screen",
             parameters=[tb3_1_amcl_yaml],
             emulate_tty=True),

Now everything works as it should.

– Girish

1 Like

This topic was automatically closed 3 days after the last reply. New replies are no longer allowed.