From e28e82b8445902a5999f3b3964104f0a5c69c8b7 Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov Date: Tue, 26 May 2020 16:26:57 +0300 Subject: [PATCH] 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 | 26 ++++++++++++++++++- .../bringup/launch/navigation_launch.py | 11 +++++++- nav2_bringup/bringup/launch/slam_launch.py | 13 +++++++++- 3 files changed, 47 insertions(+), 3 deletions(-) diff --git a/nav2_bringup/bringup/launch/bringup_launch.py b/nav2_bringup/bringup/launch/bringup_launch.py index 9904b5bc7f1..b2f79883ac1 100644 --- a/nav2_bringup/bringup/launch/bringup_launch.py +++ b/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) diff --git a/nav2_bringup/bringup/launch/navigation_launch.py b/nav2_bringup/bringup/launch/navigation_launch.py index 59e3e331e99..050ff270e44 100644 --- a/nav2_bringup/bringup/launch/navigation_launch.py +++ b/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', diff --git a/nav2_bringup/bringup/launch/slam_launch.py b/nav2_bringup/bringup/launch/slam_launch.py index ef5589a9874..3b5d0bc9469 100644 --- a/nav2_bringup/bringup/launch/slam_launch.py +++ b/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,6 +70,14 @@ 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') @@ -75,7 +85,8 @@ def generate_launch_description(): # 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',