Robotics StackExchange | Archived questions

Nav2 ComputePathToPose throws tf error because goal stamp is out of tf buffer

So this is an issue I've been wragling with for a few weeks now and I can't tell if its an intentional design decision or not.

My bot in ign gazebo is using waypointfollower and dwb on Galactic. When I send it a goal pose from rviz, it successfully begins to navigate. After 10s however, it will fail to transform the goal pose with `[transformPoseInTargetFrame]: Extrapolation Error looking up target frame: Lookup would require extrapolation into the past. Requested time 5.093000 but the earliest data is at time 5.600000, when looking up transform from frame [basefootprint] to frame [map]` .

After much tinkering with the nav2 source, I finally found these lines in PlannerServer::computePlan() to be the culprate:

 RCLCPP_INFO(this->get_logger(), "start goal has time of: %d", start.header.stamp.sec); 
    // Transform them into the global frame
    geometry_msgs::msg::PoseStamped goal_pose = goal->goal;
    RCLCPP_INFO(this->get_logger(), "goal time has time of: %d", goal_pose.header.stamp.sec); 
    if (!transformPosesToGlobalFrame(action_server_pose_, start, goal_pose)) {
      return;
    }

The info logs added by me. What seems to happen is that the goal pose I set in rviz has a constant timestamp throughout the life of the navigation, which will cause the above tf error in transformPosesToGlobalFrame when that timestamp is no longer in the tf_buffer (10s in the costmap).

now to the actual question: is this intentional? It seems like this would be a huge issue (and indeed it is for me) as it doesnt look to have any adjustable timeout. I could make the tf buffer larger, but that just kicks the issue down the line. I've tried to just update the goals timestamp, but that causes the bot to endlessly loop around the goal for some reason. I'm really at a loss of what to do and it's halting any other work.

Some more logs demenstrating the issue using the RCLPP_INFO statements above: (start goal here is the current pose, and thus sim time)

[controllerserver-13] [INFO] [1646097745.474858392] [controllerserver]: Passing new path to controller.

[plannerserver-14] [INFO] [1646097746.473745591] [plannerserver]: start goal has time of: 13

[plannerserver-14] [INFO] [1646097746.474311370] [plannerserver]: goal time has time of: 5

[controllerserver-13] [INFO] [1646097746.524860391] [controllerserver]: Passing new path to controller.

[plannerserver-14] [INFO] [1646097747.503713158] [plannerserver]: start goal has time of: 14

[plannerserver-14] [INFO] [1646097747.503756010] [plannerserver]: goal time has time of: 5

[controllerserver-13] [INFO] [1646097747.524859344] [controllerserver]: Passing new path to controller.

[plannerserver-14] [INFO] [1646097748.533716358] [plannerserver]: start goal has time of: 14

[plannerserver-14] [INFO] [1646097748.533757847] [plannerserver]: goal time has time of: 5

[controllerserver-13] [INFO] [1646097748.574861110] [controllerserver]: Passing new path to controller.

[plannerserver-14] [INFO] [1646097749.563905395] [plannerserver]: start goal has time of: 15

[plannerserver-14] [INFO] [1646097749.564018180] [plannerserver]: goal time has time of: 5

[plannerserver-14] [ERROR] [1646097750.133748814] [transformPoseInTargetFrame]: Extrapolation Error looking up target frame: Lookup would require extrapolation into the past. Requested time 5.093000 but the earliest data is at time 5.600000, when looking up transform from frame [basefootprint] to frame [map]

Asked by Andyblarblar on 2022-02-28 22:11:00 UTC

Comments

Answers

I don't have an answer to whether this is intentional behavior or not, but I have opened an issue on the Nav2 repo: https://github.com/ros-planning/navigation2/issues/3075

In the meantime, setting the goal's timestamp to 0 resolves the problem. A timestamp of 0 will allow TF2 to use the most recent available transform, instead of forcing a transform at a specific time. To allow RViz input of a goal, I remapped RViz's /goal_pose to /rviz_goal, and added a node to subscribe to this new topic and republish a goal to /goal_pose with a 0 timestamp.

Asked by Ronoman on 2022-07-13 18:30:34 UTC

Comments