[TEB Local Planner] Goal is aborting for ROS2 Foxy

[sachinkum0009]

TEB local planner is able to find the trajectory but the robot is not moving. It rotates a little and then abort the goal

##################################################################
				Log File for Teb Local Planner
##################################################################

[controller_server-4] [ERROR] [1612853914.973906030] [controller_server]: Extrapolation Error: Lookup would require extrapolation into the future.  Requested time 1612853914.462717 but the latest data is at time 69.486000, when looking up transform from frame [map] to frame [odom]
[controller_server-4] 
[controller_server-4] [ERROR] [1612853914.974122751] [controller_server]: Global Frame: odom Plan Frame size 69: map
[controller_server-4] 
[controller_server-4] [ERROR] [1612853914.974198322] [controller_server]: Could not transform the global plan to the frame of the controller
[controller_server-4] [WARN] [1612853914.974529978] [controller_server_rclcpp_node]: [follow_path] [ActionServer] Aborting handle.
[controller_server-4] [INFO] [1612853914.974612436] [controller_server]: Received a goal, begin computing control effort.
[controller_server-4] [ERROR] [1612853915.485176247] [controller_server]: Extrapolation Error: Lookup would require extrapolation into the future.  Requested time 1612853914.462717 but the latest data is at time 69.682000, when looking up transform from frame [map] to frame [odom]
[controller_server-4] 
[controller_server-4] [ERROR] [1612853915.485440914] [controller_server]: Global Frame: odom Plan Frame size 69: map
[controller_server-4] 
[controller_server-4] [ERROR] [1612853915.485606161] [controller_server]: Could not transform the global plan to the frame of the controller
[controller_server-4] [WARN] [1612853915.486009813] [controller_server_rclcpp_node]: [follow_path] [ActionServer] Aborting handle.
[controller_server-4] [INFO] [1612853915.511635978] [controller_server]: Received a goal, begin computing control effort.
[controller_server-4] [INFO] [1612853915.519049492] [local_costmap.local_costmap]: Received request to clear entirely the local_costmap
[planner_server-5] [INFO] [1612853915.523121295] [global_costmap.global_costmap]: Received request to clear entirely the global_costmap
[recoveries_server-6] [INFO] [1612853915.534351158] [recoveries_server]: Attempting spin
[recoveries_server-6] [INFO] [1612853915.534435933] [recoveries_server]: Turning 1.57 for spin recovery.
[controller_server-4] [ERROR] [1612853916.017842535] [controller_server]: Extrapolation Error: Lookup would require extrapolation into the future.  Requested time 1612853915.471823 but the latest data is at time 69.884000, when looking up transform from frame [map] to frame [odom]
[controller_server-4] 
[controller_server-4] [ERROR] [1612853916.017915436] [controller_server]: Global Frame: odom Plan Frame size 69: map
[controller_server-4] 
[controller_server-4] [ERROR] [1612853916.017954136] [controller_server]: Could not transform the global plan to the frame of the controller
[controller_server-4] [WARN] [1612853916.018051706] [controller_server_rclcpp_node]: [follow_path] [ActionServer] Aborting handle.
[recoveries_server-6] [INFO] [1612853916.535182490] [recoveries_server]: spin running...
[recoveries_server-6] [INFO] [1612853917.534517168] [recoveries_server]: spin running...
[recoveries_server-6] [INFO] [1612853918.534640689] [recoveries_server]: spin running...
[recoveries_server-6] [INFO] [1612853919.534443093] [recoveries_server]: spin running...
[recoveries_server-6] [INFO] [1612853919.839228088] [recoveries_server]: spin completed successfully
[recoveries_server-6] [INFO] [1612853919.866411043] [recoveries_server]: Attempting wait
[recoveries_server-6] [INFO] [1612853920.868645751] [recoveries_server]: wait running...
[recoveries_server-6] [INFO] [1612853921.868524167] [recoveries_server]: wait running...
[recoveries_server-6] [INFO] [1612853922.868519721] [recoveries_server]: wait running...
[recoveries_server-6] [INFO] [1612853923.868571869] [recoveries_server]: wait running...
[recoveries_server-6] [INFO] [1612853924.868453029] [recoveries_server]: wait running...
[recoveries_server-6] [INFO] [1612853924.870746763] [recoveries_server]: wait completed successfully
[controller_server-4] [INFO] [1612853924.925362466] [controller_server]: Received a goal, begin computing control effort.
[controller_server-4] [ERROR] [1612853925.430621036] [controller_server]: Extrapolation Error: Lookup would require extrapolation into the future.  Requested time 1612853924.901750 but the latest data is at time 73.688000, when looking up transform from frame [map] to frame [odom]
[controller_server-4] 
[controller_server-4] [ERROR] [1612853925.430922663] [controller_server]: Global Frame: odom Plan Frame size 68: map
[controller_server-4] 
[controller_server-4] [ERROR] [1612853925.431111835] [controller_server]: Could not transform the global plan to the frame of the controller
[controller_server-4] [WARN] [1612853925.431575689] [controller_server_rclcpp_node]: [follow_path] [ActionServer] Aborting handle.
[controller_server-4] [INFO] [1612853925.432280776] [controller_server]: Received a goal, begin computing control effort.
[controller_server-4] [ERROR] [1612853925.939998539] [controller_server]: Extrapolation Error: Lookup would require extrapolation into the future.  Requested time 1612853924.901750 but the latest data is at time 73.895000, when looking up transform from frame [map] to frame [odom]
[controller_server-4] 
[controller_server-4] [ERROR] [1612853925.940128851] [controller_server]: Global Frame: odom Plan Frame size 68: map
[controller_server-4] 
[controller_server-4] [ERROR] [1612853925.940192236] [controller_server]: Could not transform the global plan to the frame of the controller
[controller_server-4] [WARN] [1612853925.940350284] [controller_server_rclcpp_node]: [follow_path] [ActionServer] Aborting handle.
[planner_server-5] [INFO] [1612853925.957458893] [planner_server]: Goal was canceled. Canceling planning action.
[planner_server-5] [WARN] [1612853925.958105452] [planner_server_rclcpp_node]: [compute_path_to_pose] [ActionServer] Client requested to cancel the goal. Cancelling.
[controller_server-4] [INFO] [1612853925.966592728] [local_costmap.local_costmap]: Received request to clear entirely the local_costmap
[bt_navigator-7] [WARN] [1612853925.970601835] [bt_navigator_rclcpp_node]: Node timed out while executing service call to local_costmap/clear_entirely_local_costmap.
[bt_navigator-7] [ERROR] [1612853925.971202867] [bt_navigator]: Navigation failed
[bt_navigator-7] [WARN] [1612853925.971368665] [bt_navigator]: [navigate_to_pose] [ActionServer] Aborting handle.
^C[WARNING] [launch]: user interrupted with ctrl-c (SIGINT)
[amcl-2] [INFO] [1612854068.198611099] [rclcpp]: signal_handler(signal_value=2)
[amcl-2] [INFO] [1612854068.204154070] [amcl]: Destroying
[INFO] [recoveries_server-6]: process has finished cleanly [pid 5234]
[INFO] [planner_server-5]: process has finished cleanly [pid 5232]
[INFO] [map_server-1]: process has finished cleanly [pid 5203]
[lifecycle_manager-9] [INFO] [1612854068.198793116] [rclcpp]: signal_handler(signal_value=2)
[lifecycle_manager-9] [INFO] [1612854068.217026622] [lifecycle_manager_navigation]: Destroying
[INFO] [lifecycle_manager-3]: process has finished cleanly [pid 5222]
[INFO] [amcl-2]: process has finished cleanly [pid 5214]
[INFO] [waypoint_follower-8]: process has finished cleanly [pid 5238]
[recoveries_server-6] [INFO] [1612854068.198836218] [rclcpp]: signal_handler(signal_value=2)
[INFO] [lifecycle_manager-9]: process has finished cleanly [pid 5240]
[waypoint_follower-8] [INFO] [1612854068.198989724] [rclcpp]: signal_handler(signal_value=2)
[waypoint_follower-8] [INFO] [1612854068.225603057] [waypoint_follower]: Destroying
[waypoint_follower-8] [ERROR] [1612854068.238145727] [rclcpp_action]: Error in destruction of rcl action client handle: the Node Handle was destructed too early. You will leak memory
[planner_server-5] [INFO] [1612854068.199199088] [rclcpp]: signal_handler(signal_value=2)
[lifecycle_manager-3] [INFO] [1612854068.199373852] [rclcpp]: signal_handler(signal_value=2)
[lifecycle_manager-3] [INFO] [1612854068.201657997] [lifecycle_manager_localization]: Destroying
[map_server-1] [INFO] [1612854068.199477767] [rclcpp]: signal_handler(signal_value=2)
[map_server-1] [INFO] [1612854068.201829490] [map_server]: Destroying
[controller_server-4] [INFO] [1612854068.204493083] [rclcpp]: signal_handler(signal_value=2)
[bt_navigator-7] [INFO] [1612854068.204643206] [rclcpp]: signal_handler(signal_value=2)
[bt_navigator-7] [INFO] [1612854068.212282193] [bt_navigator]: Destroying
[controller_server-4] terminate called after throwing an instance of 'rclcpp::exceptions::RCLError'
[controller_server-4]   what():  Couldn't initialize guard condition: the given context is not valid, either rcl_init() was not called or rcl_shutdown() was called., at /tmp/binarydeb/ros-foxy-rcl-1.1.10/src/rcl/guard_condition.c:67
[INFO] [bt_navigator-7]: process has finished cleanly [pid 5236]
[ERROR] [controller_server-4]: process has died [pid 5226, exit code -6, cmd '/opt/ros/foxy/lib/nav2_controller/controller_server --ros-args --params-file /tmp/tmpk2i0opw2 -r /tf:=tf -r /tf_static:=tf_static'].

Param Used

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: "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: 2000
    min_particles: 500
    odom_frame_id: "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

amcl_map_client:
  ros__parameters:
    use_sim_time: True

amcl_rclcpp_node:
  ros__parameters:
    use_sim_time: True

bt_navigator:
  ros__parameters:
    use_sim_time: true
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /odom
    default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
    plugin_lib_names:
    - nav2_compute_path_to_pose_action_bt_node
    - nav2_follow_path_action_bt_node
    - nav2_back_up_action_bt_node
    - nav2_spin_action_bt_node
    - nav2_wait_action_bt_node
    - nav2_clear_costmap_service_bt_node
    - nav2_is_stuck_condition_bt_node
    - nav2_goal_reached_condition_bt_node
    - nav2_goal_updated_condition_bt_node
    - nav2_initial_pose_received_condition_bt_node
    - nav2_reinitialize_global_localization_service_bt_node
    - nav2_rate_controller_bt_node
    - nav2_distance_controller_bt_node
    - nav2_speed_controller_bt_node
    - nav2_truncate_path_action_bt_node
    - nav2_goal_updater_node_bt_node
    - nav2_recovery_node_bt_node
    - nav2_pipeline_sequence_bt_node
    - nav2_round_robin_node_bt_node
    - nav2_transform_available_condition_bt_node
    - nav2_time_expired_condition_bt_node
    - nav2_distance_traveled_condition_bt_node

bt_navigator_rclcpp_node:
  ros__parameters:
    use_sim_time: True

controller_server:
  ros__parameters:
    use_sim_time: True
    controller_frequency: 20.0
    min_x_velocity_threshold: 0.001
    min_y_velocity_threshold: 0.5
    min_theta_velocity_threshold: 0.001
    progress_checker_plugin: "progress_checker"
    goal_checker_plugin: "goal_checker"
    controller_plugins: ["FollowPath"]
    # Progress checker parameters
    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 0.5
      movement_time_allowance: 10.0
    # Goal checker parameters
    goal_checker:
      plugin: "nav2_controller::SimpleGoalChecker"
      xy_goal_tolerance: 0.25
      yaw_goal_tolerance: 0.25
      stateful: True
    TEB parameters
    FollowPath:
      plugin: "teb_local_planner::TebLocalPlannerROS"
      footprint_model.type: circular
      footprint_model.radius: 0.17
      min_obstacle_dist: 0.1
      inflation_dist: 0.1
      costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSMCCH"
      costmap_converter_spin_thread: True
      costmap_converter_rate: 15
      enable_homotopy_class_planning: True
      enable_multithreading: True
      optimization_verbose: False
      teb_autoresize: True
      min_samples: 3
      max_samples: 20
      max_global_plan_lookahead_dist: 1.0
      visualize_hc_graph: True
      max_vel_x: 0.26
      max_vel_theta: 1.0
      acc_lim_x: 2.5
      acc_lim_theta: 3.2

    
costmap_converter:
  ros__parameters:
    use_sim_time: True

controller_server_rclcpp_node:
  ros__parameters:
    use_sim_time: True

local_costmap:
  local_costmap:
    ros__parameters:
      use_sim_time: True
      update_frequency: 5.0
      publish_frequency: 2.0
      global_frame: odom
      robot_base_frame: base_link
      rolling_window: true
      width: 3
      height: 3
      resolution: 0.05
      robot_radius: 0.22
      plugins: ["obstacle_layer", "voxel_layer", "inflation_layer"]
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.01
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
      voxel_layer:
        plugin: "nav2_costmap_2d::VoxelLayer"
        enabled: True
        publish_voxel_map: True
        origin_z: 0.0
        z_resolution: 0.2
        z_voxels: 10
        max_obstacle_height: 2.0
        mark_threshold: 0
        observation_sources: pointcloud
        pointcloud:
          topic: /intel_realsense_r200_depth/points
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "PointCloud2"
      static_layer:
        map_subscribe_transient_local: True
      always_send_full_costmap: True

local_costmap_client:
  ros__parameters:
    use_sim_time: True

local_costmap_rclcpp_node:
  ros__parameters:
    use_sim_time: True

global_costmap:
  global_costmap:
    ros__parameters:
      use_sim_time: True
      footprint_padding: 0.03
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_link
      robot_radius: 0.22 # radius set and used, so no footprint points
      resolution: 0.05
      plugins: ["static_layer", "obstacle_layer", "voxel_layer", "inflation_layer"]
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan
        footprint_clearing_enabled: true
        max_obstacle_height: 2.0
        combination_method: 1
        observation_sources: scan
        scan:
          topic: /scan
          obstacle_range: 2.5
          raytrace_range: 3.0
          max_obstacle_height: 2.0
          min_obstacle_height: 0.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          inf_is_valid: false
      voxel_layer:
        plugin: "nav2_costmap_2d::VoxelLayer"
        enabled: True
        footprint_clearing_enabled: true
        max_obstacle_height: 2.0
        publish_voxel_map: True
        origin_z: 0.0
        z_resolution: 0.05
        z_voxels: 16
        max_obstacle_height: 2.0
        unknown_threshold: 15
        mark_threshold: 0
        observation_sources: pointcloud
        combination_method: 1
        pointcloud:  # no frame set, uses frame from message
          topic: /intel_realsense_r200_depth/points
          max_obstacle_height: 2.0
          min_obstacle_height: 0.0
          obstacle_range: 2.5
          raytrace_range: 3.0
          clearing: True
          marking: True
          data_type: "PointCloud2"  
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
        nabled: true
        subscribe_to_updates: true
        transform_tolerance: 0.1
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        enabled: true
        inflation_radius: 0.01
        cost_scaling_factor: 1.0
        inflate_unknown: false
        inflate_around_unknown: true  
      always_send_full_costmap: True

global_costmap_client:
  ros__parameters:
    use_sim_time: True
  
global_costmap_rclcpp_node:
  ros__parameters:
    use_sim_time: True

map_server:
  ros__parameters:
    use_sim_time: True
    yaml_filename: "turtlebot3_world.yaml"

map_saver:
  ros__parameters:
    use_sim_time: True
    save_map_timeout: 5.0
    free_thresh_default: 0.25
    occupied_thresh_default: 0.65
    map_subscribe_transient_local: True

planner_server:
  ros__parameters:
    expected_planner_frequency: 20.0
    use_sim_time: True
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_navfn_planner/NavfnPlanner"
      tolerance: 0.5
      use_astar: false
      allow_unknown: true

planner_server_rclcpp_node:
  ros__parameters:
    use_sim_time: True

recoveries_server:
  ros__parameters:
    costmap_topic: local_costmap/costmap_raw
    footprint_topic: local_costmap/published_footprint
    cycle_frequency: 10.0
    recovery_plugins: ["spin", "backup", "wait"]
    spin:
      plugin: "nav2_recoveries/Spin"
    backup:
      plugin: "nav2_recoveries/BackUp"
    wait:
      plugin: "nav2_recoveries/Wait"
    global_frame: odom
    robot_base_frame: base_link
    transform_timeout: 0.1
    use_sim_time: true
    simulate_ahead_time: 2.0
    max_rotational_vel: 1.0
    min_rotational_vel: 0.4
    rotational_acc_lim: 3.2

robot_state_publisher:
  ros__parameters:
    use_sim_time: True

waypoint_follower:
  ros__parameters:
    loop_rate: 20
    stop_on_failure: false
    waypoint_task_executor_plugin: "wait_at_waypoint"   
    wait_at_waypoint:
      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
      enabled: True
      waypoint_pause_duration: 200

Addtional information

[SteveMacenski]

Did you try now with the new params on ros2-master config folder?

[sachinkum0009]

No, I will try it now.
Thank you for response

[HazemAbidi]

Hello, I'm facing similar errors :
controller_server-1] [INFO] [1619712384.151567022] [controller_server]: Received a goal, begin computing control effort. [controller_server-1] [ERROR] [1619712384.655240957] [controller_server]: Extrapolation Error: Lookup would require extrapolation into the future. Requested time 1619712383.626969 but the latest data is at time 976.434000, when looking up transform from frame [map] to frame [odom] [controller_server-1] [controller_server-1] [ERROR] [1619712384.655287167] [controller_server]: Global Frame: odom Plan Frame size 587: map [controller_server-1] [controller_server-1] [ERROR] [1619712384.655358394] [controller_server]: Could not transform the global plan to the frame of the controller [controller_server-1] [WARN] [1619712384.655508720] [controller_server_rclcpp_node]: [follow_path] [ActionServer] Aborting handle. [bt_navigator-4] [ERROR] [1619712384.666088517] [bt_navigator]: Navigation failed [bt_navigator-4] [WARN] [1619712384.666143659] [bt_navigator]: [navigate_to_pose] [ActionServer] Aborting handle.
I'm also using the params in the ros2-master branch.
I'm using ros2 foxy.

[sachinkum0009]

The issue is related to the transform rate from frame [map] to frame [odom]. Hence the controller server could not transform the global plan to the frame of the controller. @SteveMacenski should we reopen the issue.

[HazemAbidi]

@sachinkum0009 Have you been able to make it work? If yes How?

[sachinkum0009]

@HazemAbidi I am also facing same issue. The controller is not able to plan the trajectory to move the robot.

[HazemAbidi]

@sachinkum0009 I solved this by changing the global frame from Odom to map and it does the work. However, I wonder how to fix this properly @SteveMacenski?

[Paul-Ajay]

I think there is a missing TF timeout here https://github.com/rst-tu-dortmund/teb_local_planner/blob/foxy-devel/teb_local_planner/src/teb_local_planner_ros.cpp#L718 adding tf2::durationFromSec(0.5) as third argument here solved this issue for me with global_frame as odom for local costmap