Hydro move_base/tf2 extrapolation error into the future errror

edit

Hi Niccer,

Because, I've run tests where I published transforms at 10Hz and 20Hz tf_monitor reports different delays. But here is an example of 20hz test.

Node: /state_filter 60.0905 Hz, Average Delay: 0.0086032 Max Delay: 0.0267848

I believe that the error happens in Time_Cache::find_closest in tf2/src/cache.cpp

To be verbose(sorry) this is the call chain

// move_base/src/move_base.cpp
        if(tc_->computeVelocityCommands(cmd_vel)){  

//dwa_local_planner/src/dwa_planner_ros.cpp
      bool DWAPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) {
    // dispatches to either dwa sampling control or stop and rotate control, depending on whether we have been close enough to goal
    if ( ! costmap_ros_->getRobotPose(current_pose_)) {
      ROS_ERROR("Could not get robot pose");
      return false;
    }
    std::vector<geometry_msgs::PoseStamped> transformed_plan;
    if ( ! planner_util_.getLocalPlan(current_pose_, transformed_plan)) {
      ROS_ERROR("Could not get local plan");
      return false;
    }   

//base_local_planner/src/local_planner_util.cpp
bool LocalPlannerUtil::getLocalPlan(tf::Stamped<tf::Pose>& global_pose, std::vector<geometry_msgs::PoseStamped>& transformed_plan) {
  //get the global plan in our frame
  if(!base_local_planner::transformGlobalPlan(
      *tf_,
      global_plan_,
      global_pose,
      *costmap_,
      global_frame_,
      transformed_plan)) {
    ROS_WARN("LocalPlannerUtil::getLocalPlan - Could not transform the global plan to the frame of the controller");
    return false;
  }

//base_local_planner/src/goal_functions.cpp
  bool transformGlobalPlan(
      const tf::TransformListener& tf,
      const std::vector<geometry_msgs::PoseStamped>& global_plan,
      const tf::Stamped<tf::Pose>& global_pose,
      const costmap_2d::Costmap2D& costmap,
      const std::string& global_frame,
      std::vector<geometry_msgs::PoseStamped>& transformed_plan){
    const geometry_msgs::PoseStamped& plan_pose = global_plan[0];

    transformed_plan.clear();

    try {
      if (!global_plan.size() > 0) {
        ROS_ERROR("transformGlobalPlan - Received plan with zero length");
        return false;
      }

      // get plan_to_global_transform from plan frame to global_frame
      tf::StampedTransform plan_to_global_transform;
      tf.lookupTransform(global_frame, ros::Time(), 
          plan_pose.header.frame_id, plan_pose.header.stamp, 
          plan_pose.header.frame_id, plan_to_global_transform);

//tf/src/tf.cpp
void Transformer::lookupTransform(const std::string& target_frame,const ros::Time& target_time, const std::string& source_frame,
                     const ros::Time& source_time, const std::string& fixed_frame, StampedTransform& transform) const
{
  geometry_msgs::TransformStamped output = 
    tf2_buffer_.lookupTransform(strip_leading_slash(target_frame), target_time,
                                strip_leading_slash(source_frame), source_time,
                                strip_leading_slash(fixed_frame));
  transformStampedMsgToTF(output, transform);
};

//tf2/src/buffer_core.cpp
geometry_msgs::TransformStamped BufferCore::lookupTransform(const std::string& target_frame, 
                                                        const ros::Time& target_time,
                                                        const std::string& source_frame,
                                                        const ros::Time& source_time,
                                                        const std::string& fixed_frame) const
{
  validateFrameId("lookupTransform argument target_frame", target_frame);
  validateFrameId("lookupTransform argument source_frame", source_frame);
  validateFrameId("lookupTransform argument fixed_frame", fixed_frame);

  geometry_msgs::TransformStamped output;
  geometry_msgs::TransformStamped temp1 =  lookupTransform(fixed_frame, source_frame, source_time);
  geometry_msgs::TransformStamped temp2 =  lookupTransform(target_frame, fixed_frame, target_time);


//tf2/src/buffer_core.cpp
geometry_msgs::TransformStamped BufferCore::lookupTransform(const std::string& target_frame,
                                                            const std::string& source_frame,
                                                            const ros::Time& time) const
{
  boost::mutex::scoped_lock lock(frame_mutex_);

  if (target_frame == source_frame) {
    geometry_msgs::TransformStamped identity;
    identity.header.frame_id = target_frame;
    identity.child_frame_id = source_frame;
    identity.transform.rotation.w = 1;

    if (time == ros::Time())
    {
      CompactFrameID target_id = lookupFrameNumber(target_frame);
      TimeCacheInterfacePtr cache = getFrame(target_id);
      if (cache)
        identity.header.stamp = cache->getLatestTimestamp();
      else
        identity.header.stamp = time;
    }
    else
      identity.header.stamp = time;

    return identity;
  }

  //Identify case does not need to be validated above
  CompactFrameID target_id = validateFrameId("lookupTransform argument target_frame", target_frame);
  CompactFrameID source_id = validateFrameId("lookupTransform argument source_frame", source_frame);

  std::string error_string;
  TransformAccum accum;
  int retval = walkToTopParent(accum, time, target_id, source_id, &error_string);
  if (retval != tf2_msgs::TF2Error::NO_ERROR)
  {
    switch (retval)
    {
    case tf2_msgs::TF2Error::CONNECTIVITY_ERROR:
      throw ConnectivityException(error_string);
    case tf2_msgs::TF2Error::EXTRAPOLATION_ERROR ...