Transform data too old when converting from map to odom in nav2_straightline_planner

[root-robot]

Bug report

I was trying to use nav2_straightline_planner as mentioned here: https://navigation.ros.org/plugin_tutorials/docs/writing_new_nav2planner_plugin.html

Required Info:

Operating System: ubuntu20.04

ROS2 Version: foxy

Version or commit hash: a1028c57c9168509186769d1a1cff58fb7957339

Error Log:

[controller_server-4] [INFO] [1617794494.106441780] [controller_server]: Passing new path to controller.
[controller_server-4] [ERROR] [1617794494.107058570] [tf_help]: Transform data too old when converting from map to odom
[controller_server-4] [ERROR] [1617794494.107080741] [tf_help]: Data time: 1617794473s 481323259ns, Transform time: 826s 621000000ns
[controller_server-4] [INFO] [1617794495.106504024] [controller_server]: Passing new path to controller.
[controller_server-4] [ERROR] [1617794495.106999333] [tf_help]: Transform data too old when converting from map to odom
[controller_server-4] [ERROR] [1617794495.107018282] [tf_help]: Data time: 1617794473s 481323259ns, Transform time: 827s 521000000ns
[controller_server-4] [INFO] [1617794496.106503821] [controller_server]: Passing new path to controller.
[controller_server-4] [ERROR] [1617794496.106976139] [tf_help]: Transform data too old when converting from map to odom
[controller_server-4] [ERROR] [1617794496.106996291] [tf_help]: Data time: 1617794473s 481323259ns, Transform time: 828s 621000000ns
[controller_server-4] [INFO] [1617794497.306577073] [controller_server]: Passing new path to controller.
[controller_server-4] [ERROR] [1617794497.307159186] [tf_help]: Transform data too old when converting from map to odom
[controller_server-4] [ERROR] [1617794497.307185101] [tf_help]: Data time: 1617794473s 481323259ns, Transform time: 829s 721000000ns
[controller_server-4] [INFO] [1617794498.306491576] [controller_server]: Passing new path to controller.
[controller_server-4] [ERROR] [1617794498.306999256] [tf_help]: Transform data too old when converting from map to odom
[controller_server-4] [ERROR] [1617794498.307018059] [tf_help]: Data time: 1617794473s 481323259ns, Transform time: 830s 721000000ns
[controller_server-4] [INFO] [1617794499.306503259] [controller_server]: Passing new path to controller.
[controller_server-4] [ERROR] [1617794499.307099119] [tf_help]: Transform data too old when converting from map to odom
[controller_server-4] [ERROR] [1617794499.307125979] [tf_help]: Data time: 1617794473s 481323259ns, Transform time: 831s 721000000ns
[controller_server-4] [INFO] [1617794500.306443555] [controller_server]: Passing new path to controller.
[controller_server-4] [ERROR] [1617794500.307062602] [tf_help]: Transform data too old when converting from map to odom
[controller_server-4] [ERROR] [1617794500.307088974] [tf_help]: Data time: 1617794473s 481323259ns, Transform time: 832s 721000000ns
[controller_server-4] [INFO] [1617794501.306475765] [controller_server]: Passing new path to controller.
[controller_server-4] [ERROR] [1617794501.307010382] [tf_help]: Transform data too old when converting from map to odom

Current Behaviour:

When visualising on rviz2 the planner plans perfectly and also the controller follows the path correctly, but when the bot reaches the end of path neither the controller nor the bt_navigator shows the message "Navigation Succeded " and hence the bot never successfully completes its navigation and keeps on trying to reach the goal.

Ideal Behaviour:

Bot successfully completes its navigation task and [tf_help] error should not come.

Here is my navigation2.yaml file:

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: 10.0
    laser_min_range: 0.22
    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: 10.0
    min_x_velocity_threshold: 0.001
    min_y_velocity_threshold: 0.5
    min_theta_velocity_threshold: 0.001
    controller_plugins: ["FollowPath"]
   #DWB parameters
    FollowPath:
      plugin: "dwb_core::DWBLocalPlanner"
      debug_trajectory_details: True
      min_vel_x: 0.0
      min_vel_y: 0.0
      max_vel_x: 0.1
      max_vel_y: 0.0
      max_vel_theta: 1.5
      min_speed_xy: 0.0
      max_speed_xy: .1
      min_speed_theta: 0.0
      # Add high threshold velocity for turtlebot 3 issue.
      # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
      acc_lim_x: 1.5
      acc_lim_y: 0.0
      acc_lim_theta: 1.2
      decel_lim_x: -1.5
      decel_lim_y: 0.0
      decel_lim_theta: -1.2
      vx_samples: 20
      vy_samples: 5
      vtheta_samples: 20
      sim_time: 1.7
      linear_granularity: 0.02
      angular_granularity: 0.017
      transform_tolerance: 0.2
      xy_goal_tolerance: 0.05                
      yaw_goal_tolerance: 0.034
      stateful: true
      trans_stopped_velocity: 0.25
      short_circuit_trajectory_evaluation: True
      stateful: True
      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist","ObstacleFootprint"]
      BaseObstacle.scale: 0.08
      PathAlign.scale: 32.0
      PathAlign.forward_point_distance: 0.2
      GoalAlign.scale: 24.0
      GoalAlign.forward_point_distance: 0.2
      PathDist.scale: 32.0
      GoalDist.scale: 20.0
      RotateToGoal.scale: 32.0
      RotateToGoal.slowing_factor: 5.0
      RotateToGoal.lookahead_time: -1.0
      ObstacleFootprint.scale: 0.08


controller_server_rclcpp_node:
  ros__parameters:
    use_sim_time: True
local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 5.0
      publish_frequency: 2.0
      global_frame: odom
      robot_base_frame: base_link
      use_sim_time: False
      rolling_window: true
      width: 3
      height: 3
      resolution: 0.05
      robot_radius: 0.105
      plugins: ["obstacle_layer", "voxel_layer", "inflation_layer"]
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
      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.05
        z_voxels: 16
        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: False
  local_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: False

global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_link
      use_sim_time: False
      robot_radius: 0.105
      resolution: 0.05
      plugins: ["static_layer", "obstacle_layer", "voxel_layer", "inflation_layer"]
      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.05
        z_voxels: 16
        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:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
      always_send_full_costmap: True
  global_costmap_client:
    ros__parameters:
      use_sim_time: False
  global_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: False

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

map_saver:
  ros__parameters:
    use_sim_time: False
    save_map_timeout: 5000
    free_thresh_default: 0.25
    occupied_thresh_default: 0.65

planner_server:
  ros__parameters:
    expected_planner_frequency: 5.0
    use_sim_time: False
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_straightline_planner/StraightLine" 
      interpolation_resolution: 0.1


planner_server_rclcpp_node:
  ros__parameters:
    use_sim_time: False

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: False
    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: False

[SteveMacenski]

Use sim time is inconsistently set.

[root-robot]

Hi @SteveMacenski
Although I was over-writing the use_sim_time parameter value from the terminal, still I changed all the use_sim_time values to true and still the error remains the same.

[root-robot]

Also when running the nav2_navfn_planner and the smac_planner there was no error and navigation executed perfectly. Hence a conclusion can be made that the error is not due to the controller but due to the planner.
Currently I am using the nav2_dwb_controller.

Also thanks for the quick response to the issue.

[dhruv-rightbot]

Hey @SteveMacenski , @shivaang12 , Any update or solution would be very helpful.

Thanks

[shivaang12]

Did you wrote the planner from the article or cloned from here https://github.com/ros-planning/navigation2_tutorials ?

[dhruv-rightbot]

Hey, @shivaang12 thanks for the reply.

I cloned it from here https://github.com/ros-planning/navigation2_tutorials/commit/a1028c57c9168509186769d1a1cff58fb7957339. https://github.com/ros-planning/navigation2_tutorials and https://github.com/ros-planning/navigation2_tutorials/commit/a1028c57c9168509186769d1a1cff58fb7957339 are basically the same the only difference was that the master branch used weak pointers and the latter used shared pointers.

Any thoughts on why is this happening ??

bool transformPose(
  const std::shared_ptr<tf2_ros::Buffer> tf,
  const std::string frame,
  const geometry_msgs::msg::PoseStamped & in_pose,
  geometry_msgs::msg::PoseStamped & out_pose,
  rclcpp::Duration & transform_tolerance
)
{
  if (in_pose.header.frame_id == frame) {
    out_pose = in_pose;
    return true;
  }

  try {
    tf->transform(in_pose, out_pose, frame);
    return true;
  } catch (tf2::ExtrapolationException & ex) {
    auto transform = tf->lookupTransform(
      frame,
      in_pose.header.frame_id,
      tf2::TimePointZero
    );
    if (
      (rclcpp::Time(in_pose.header.stamp) - rclcpp::Time(transform.header.stamp)) >
      transform_tolerance)
    {
      RCLCPP_ERROR(
        rclcpp::get_logger("tf_help"),
        "Transform data too old when converting from %s to %s",
        in_pose.header.frame_id.c_str(),
        frame.c_str()
      );
      RCLCPP_ERROR(
        rclcpp::get_logger("tf_help"),
        "Data time: %ds %uns, Transform time: %ds %uns",
        in_pose.header.stamp.sec,
        in_pose.header.stamp.nanosec,
        transform.header.stamp.sec,
        transform.header.stamp.nanosec
      );
      return false;
    } else {
      tf2::doTransform(in_pose, out_pose, transform);
      return true;
    }
  } catch (tf2::TransformException & ex) {
    RCLCPP_ERROR(
      rclcpp::get_logger("tf_help"),
      "Exception in transformPose: %s",
      ex.what()
    );
    return false;
  }
  return false;

Because when I print the values in this loop inside the controller server, the values seem to be inside the tolerance range. And the loop returns true. So the error is also not arising from here. Currently, I am unable to figure out from where the error is arising.

Thanks

[BlueBirdHouse]

I problem occurs because the goal pose is added to the global_path at the end of the example code. The pose, which is given by the user, contains the wrong time stamp.
To fix it, just comment on the line.

// global_path.poses.push_back(goal);

I also comment on the lines

// pose.header.stamp = clock_->now();
// pose.header.frame_id = global_frame_;

because I don't think the time stamp is needed in this exercise.

The NAV2 seems fragile at this point. I understand that the robot needs to move to a certain place at a certain time in some mission. So, it is completely necessary to add a timestamp for a plain. However, if the timing information is not right, or it contains no timing information, the NAV2 should just give a warning and adapt it.