Increase transform_tolerance for controller and transform_timeout for…

… SLAM

in order to avoid TF lag-related issues

nav2_bringup/bringup/launch/bringup_launch.py

@@ -30,6 +30,10 @@ def generate_launch_description():
     bringup_dir = get_package_share_directory('nav2_bringup')
     launch_dir = os.path.join(bringup_dir, 'launch')
 
+    # Transform tolerances
+    transform_tolerance_navigation = '0.2'
+    transform_tolerance_slam = '1.0'
+
     # Create the launch configuration variables
     namespace = LaunchConfiguration('namespace')
     use_namespace = LaunchConfiguration('use_namespace')
@@ -39,6 +43,9 @@ def generate_launch_description():
     params_file = LaunchConfiguration('params_file')
     bt_xml_file = LaunchConfiguration('bt_xml_file')
     autostart = LaunchConfiguration('autostart')
+    transform_tolerance = LaunchConfiguration(
+        'transform_tolerance',
+        default=transform_tolerance_navigation)
 
     stdout_linebuf_envvar = SetEnvironmentVariable(
         'RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1')
@@ -83,6 +90,20 @@ def generate_launch_description():
         'autostart', default_value='true',
         description='Automatically startup the nav2 stack')
 
+    # Sometimes transforms are getting slower exceeding default transform_tolerance in SLAM mode.
+    # As a workaround, increasing transform_tolerance for controller in order to avoid TF
+    # lag-related issues.
+    declare_transform_tolerance_slam_cmd = DeclareLaunchArgument(
+        'transform_tolerance',
+        condition=IfCondition(slam),
+        default_value=transform_tolerance_slam,
+        description='Tolerance time for transforms reading by controller')
+    declare_transform_tolerance_navigation_cmd = DeclareLaunchArgument(
+        'transform_tolerance',
+        condition=IfCondition(PythonExpression(['not ', slam])),
+        default_value=transform_tolerance_navigation,
+        description='Tolerance time for transforms reading by controller')
+
     # Specify the actions
     bringup_cmd_group = GroupAction([
         PushRosNamespace(
@@ -116,7 +137,8 @@ def generate_launch_description():
                               'params_file': params_file,
                               'bt_xml_file': bt_xml_file,
                               'use_lifecycle_mgr': 'false',
-                              'map_subscribe_transient_local': 'true'}.items()),
+                              'map_subscribe_transient_local': 'true',
+                              'transform_tolerance': transform_tolerance}.items()),
     ])
 
     # Create the launch description and populate
@@ -134,6 +156,8 @@ def generate_launch_description():
     ld.add_action(declare_params_file_cmd)
     ld.add_action(declare_autostart_cmd)
     ld.add_action(declare_bt_xml_cmd)
+    ld.add_action(declare_transform_tolerance_slam_cmd)
+    ld.add_action(declare_transform_tolerance_navigation_cmd)
 
     # Add the actions to launch all of the navigation nodes
     ld.add_action(bringup_cmd_group)

nav2_bringup/bringup/launch/navigation_launch.py

@@ -27,12 +27,16 @@ def generate_launch_description():
     # Get the launch directory
     bringup_dir = get_package_share_directory('nav2_bringup')
 
+    # Transform tolerances
+    transform_tolerance_navigation = '0.2'
+
     namespace = LaunchConfiguration('namespace')
     use_sim_time = LaunchConfiguration('use_sim_time')
     autostart = LaunchConfiguration('autostart')
     params_file = LaunchConfiguration('params_file')
     bt_xml_file = LaunchConfiguration('bt_xml_file')
     map_subscribe_transient_local = LaunchConfiguration('map_subscribe_transient_local')
+    transform_tolerance = LaunchConfiguration('transform_tolerance')
 
     lifecycle_nodes = ['controller_server',
                        'planner_server',
@@ -54,7 +58,8 @@ def generate_launch_description():
         'use_sim_time': use_sim_time,
         'bt_xml_filename': bt_xml_file,
         'autostart': autostart,
-        'map_subscribe_transient_local': map_subscribe_transient_local}
+        'map_subscribe_transient_local': map_subscribe_transient_local,
+        'FollowPath.transform_tolerance': transform_tolerance}
 
     configured_params = RewrittenYaml(
             source_file=params_file,
@@ -94,6 +99,10 @@ def generate_launch_description():
             'map_subscribe_transient_local', default_value='false',
             description='Whether to set the map subscriber QoS to transient local'),
 
+        DeclareLaunchArgument(
+            'transform_tolerance', default_value=transform_tolerance_navigation,
+            description='Tolerance time for transforms reading by controller'),
+
         Node(
             package='nav2_controller',
             node_executable='controller_server',

nav2_bringup/bringup/launch/slam_launch.py

@@ -31,9 +31,11 @@ def generate_launch_description():
     params_file = LaunchConfiguration('params_file')
     use_sim_time = LaunchConfiguration('use_sim_time')
     autostart = LaunchConfiguration('autostart')
+    transform_timeout = LaunchConfiguration('transform_timeout')
 
     # Variables
     lifecycle_nodes = ['map_saver']
+    transform_timeout_slam = '1.0'
 
     # Getting directories and launch-files
     bringup_dir = get_package_share_directory('nav2_bringup')
@@ -68,14 +70,23 @@ def generate_launch_description():
         default_value='true',
         description='Use simulation (Gazebo) clock if true')
 
+    # Sometimes transforms are getting slower exceeding default transform_tolerance in SLAM mode.
+    # As a workaround, increasing transform_timeout for SLAM in order to avoid TF lag-related
+    # issues.
+    declare_use_sim_time_cmd = DeclareLaunchArgument(
+        'transform_timeout',
+        default_value=transform_timeout_slam,
+        description='TF ahead time emitting by SLAM')
+
     declare_autostart_cmd = DeclareLaunchArgument(
         'autostart', default_value='true',
         description='Automatically startup the nav2 stack')
 
     # Nodes launching commands
     start_slam_toolbox_cmd = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(slam_launch_file),
-        launch_arguments={'use_sim_time': use_sim_time}.items())
+        launch_arguments={'use_sim_time': use_sim_time,
+                          'transform_timeout': transform_timeout}.items())
 
     start_map_saver_server_cmd = Node(
             package='nav2_map_server',