Ask Your Question
1

Hydro move_base/tf2 extrapolation error into the future errror

asked 2013-12-04 05:23:17 -0600

robotiqsguy gravatar image

updated 2013-12-04 05:51:28 -0600

Raptor gravatar image

Hi All,

We are using Hyrdo's move_base and we continue to get an extrapolation error into the future. I believe this is causing our robot to stop and start during a traverse.

In looking the ROS source code it appears that the lookupTransform in navigation/base_local_planner/src/goal_functions.cpp line 110 in transformGlobalPlan is returning the error. We are using the dwa_local_planner. We are publishing transforms at 20hz from a separate node. The parameters to tf.lookupTransform are global_frame = "local" and the plan_pose.header.frame_id = "global", and the plan_pose.header.stamp comes from the generating the gloabl plan. I think the bug is coming from tf2/src/buffer_core.cpp in WalkToTopParent(). I still need to do more debugging to find the exact problem. I've turned the debug information on for move_base so I'm including a snippet of it, and the console log below. Any help would be appreciated. Thanks.

[DEBUG] [1386166204.375994679]: The action server has received a new goal request
[DEBUG] [1386166204.376260355]: A new goal has been recieved by the single goal action server
[DEBUG] [1386166204.376656845]: Accepting a new goal
[DEBUG] [1386166204.376895493]: Accepting goal, id: /waypoint_management_node-1-1386166204.372833387, stamp: 1386166204.37
[DEBUG] [1386166204.377491239]: Publishing feedback for goal, id: /waypoint_management_node-1-1386166204.372833387, stamp: 1386166204.37
[DEBUG] [1386166204.377705512]: Publishing feedback for goal with id: /waypoint_management_node-1-1386166204.372833387 and stamp: 1386166204.37
[DEBUG] [1386166204.378025665]: Planning...
[DEBUG] [1386166204.378168840]: Waiting for plan, in the planning state.
[DEBUG] [1386166204.378332478]: Full control cycle time: 0.000979315

[DEBUG] [1386166204.455307564]: Got Plan with 49 points!
[DEBUG] [1386166204.457263957]: Generated a plan from the base_global_planner
[DEBUG] [1386166204.458307175]: Planner thread is suspending
[DEBUG] [1386166204.477544259]: Publishing feedback for goal, id: /waypoint_management_node-1-1386166204.372833387, stamp: 1386166204.37
[DEBUG] [1386166204.477708316]: Publishing feedback for goal with id: /waypoint_management_node-1-1386166204.372833387 and stamp: 1386166204.37
[DEBUG] [1386166204.477914069]: Got a new plan...swap pointers
[DEBUG] [1386166204.478065066]: pointers swapped!
[ INFO] [1386166204.478229751]: Got new plan
[DEBUG] [1386166204.478925371]: In controlling state.
[DEBUG] [1386166204.515009849]: Got a valid command from the local planner: 0.500, 0.000, 2.000
[DEBUG] [1386166204.516475469]: Full control cycle time: 0.037907056

[DEBUG] [1386166204.578542367]: Publishing feedback for goal, id: /waypoint_management_node-1-1386166204.372833387, stamp: 1386166204.37
[DEBUG] [1386166204.579298889]: Publishing feedback for goal with id: /waypoint_management_node-1-1386166204.372833387 and stamp: 1386166204.37
[DEBUG] [1386166204.580845315]: In controlling state.
[DEBUG] [1386166204.609181880]: Got a valid command from the local planner: 0.500, 0.000, 2.000
[DEBUG] [1386166204.610400540]: Full control cycle time: 0.031062960

[DEBUG] [1386166204.677557610]: Publishing feedback for goal, id: /waypoint_management_node-1-1386166204.372833387, stamp: 1386166204.37
[DEBUG] [1386166204.677725508]: Publishing feedback for goal with id: /waypoint_management_node-1-1386166204.372833387 and stamp: 1386166204.37
[DEBUG] [1386166204.677947673]: In controlling state.
[DEBUG] [1386166204.704567051]: Got a valid command from the local planner: 0.500, 0.000, 2.000
[DEBUG] [1386166204.705445516]: Full control cycle time: 0.027409982

[DEBUG] [1386166204.777927638]: Publishing feedback for goal, id: /waypoint_management_node-1-1386166204.372833387, stamp: 1386166204.37
[DEBUG] [1386166204.779469173]: Publishing feedback for goal with id: /waypoint_management_node-1-1386166204.372833387 and stamp: 1386166204.37 ...
(more)
edit retag flag offensive close merge delete

3 Answers

Sort by ยป oldest newest most voted
2

answered 2013-12-12 04:32:25 -0600

Niccer gravatar image

Well, the code requests transformPose with time t_b of the latest base pose. If you publish at different places of your node, time t_g of global would e.g. have a slightly earlier timestamp - and tf must fail. Same thing if one of the other tf-messages has not been received yet. Unless you enable extrapolation, this is how tf(2) should behave. In amcl - which would typically publish the map->odom or world->local transform - they timestamp the transform with 0.2s in the future. Maybe that's also a solution for you?

edit flag offensive delete link more

Comments

We timestamp our local and global transforms with the same time. They are being published in the same sendTransform since we use a vector of transforms. I assume upon receiving a vector that access to tf is locked, so if one transform is in tf the other one would be too. Transforms are sent at 20Hz.

robotiqsguy gravatar imagerobotiqsguy ( 2013-12-12 10:21:44 -0600 )edit

Maybe it helps to dump all the frames that are in your tf instance? There is the getFrames() method - not sure if it prints the timestamps though.

Niccer gravatar imageNiccer ( 2013-12-17 04:30:10 -0600 )edit

HI Niccer, I'm noobie to ROS, so please keep the suggestions coming. This is an intermittent problem. After the change I made to send a vector of Transforms the problem happens much less. But it still happens. I wish there were some good tools for online debugging of TF2 problems.

robotiqsguy gravatar imagerobotiqsguy ( 2013-12-17 13:16:42 -0600 )edit

To debug this problem I was thinking of pushing values on to the error_string. That way it would only get printed when an error happens.

robotiqsguy gravatar imagerobotiqsguy ( 2013-12-17 13:18:55 -0600 )edit

There are debugging tutorials here: http://wiki.ros.org/tf/Tutorials/Debugging%20tf%20problems and feel free to put debugging code in whereever you need.

tfoote gravatar imagetfoote ( 2013-12-17 13:42:46 -0600 )edit
1

answered 2013-12-05 04:41:35 -0600

Niccer gravatar image

updated 2013-12-05 04:46:16 -0600

Hi,

there are many questions here about that error in different scenarios. I am using slam_karto and groovy, but the code in goal_functions.cpp has not changed much.

I also found that the exception is thrown in navigation/base_local_planner/src/goal_functions.cpp:transformGlobalPlan() - yet for me, tf.lookupTransform (line 110) is ok, but tf.transformPose (line 116) fails. My tf-tree is /map -> /odom -> /base_footprint. /odom is published with an average delay of 90ms, while /base_footprint has a delay of 35ms. Debugging the code shows that tf.transformPose is requested to transform the pose to /map at now()-35ms. That fails, since the /odom transformation is older, and tf apparently does not extrapolate so far into the future.

My solution was to change the tf.transformPose call, such that it takes the most recent time: tf.transformPose(plan_pose.header.frame_id, ros::Time(0), global_pose, global_pose.frame_id_, robot_pose);

While this works, I wonder how things should behave in the original code. Are delays between the different frames not accepted? Or does the extrapolation usually work, for whatever reason? Maybe someone who runs move_base successfully can comment on that.

@robotiqsguy: Could you verify where exactly the exception is triggered in your case? Also, what delays do you see for your frames, if you run rosrun tf tf_monitor?

Best Regards!

edit flag offensive delete link more
0

answered 2013-12-05 06:52:29 -0600

robotiqsguy gravatar image
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 ...
(more)
edit flag offensive delete link more

Comments

Have you actually verified it fails really at tf.lookupTransform? Because in this line, you are not requesting a lookup 40ms before now(), as the error message says. What is your tf chain from local to global?

Niccer gravatar imageNiccer ( 2013-12-06 01:16:22 -0600 )edit

Pretty sure, I put some ROS_INFO statements around the lookupTransform. If you look at BufferCore::walkToTopParent if time == ros::Time() it calls getLatestCommonTime(...) to get a time. I believe this is where the time value seen in the error message is coming from. I'm trying to verify.

robotiqsguy gravatar imagerobotiqsguy ( 2013-12-06 04:12:09 -0600 )edit

I have now verified that it fails in the tf.transformPose() statement right after tf.lookupTransform. The first thing transformPose does is a lookupTransform, though. Different path through the code but still ends up calling BufferCore::WalktoTopParent(). I'll continue debugging.

robotiqsguy gravatar imagerobotiqsguy ( 2013-12-06 09:15:51 -0600 )edit

What's your transformation chain, and what node publishes the individual frames?

Niccer gravatar imageNiccer ( 2013-12-10 03:56:27 -0600 )edit

Our chain is world->global->local->base. One node publishes world, global and local transforms. They were being sent individually with SendTransform. I changed it to send a vactor of transforms which helped, but didn't fix the problem. I still think there is a bug in tf2.

robotiqsguy gravatar imagerobotiqsguy ( 2013-12-12 04:11:23 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2013-12-04 05:23:17 -0600

Seen: 1,819 times

Last updated: Dec 12 '13