Fix the time rounding bug in tf_bridge.cc

[clalancette]

There is currently a bug and a workaround in tf_bridge.cc having to do with time conversions. The bug is that if we try to request a TF transform at the time that the data comes in (as specified in the message header), TF always complains that it would have to extrapolate into the future to do that. The current workaround is to just request the latest data, but this isn't always correct. The problem seems to have to do with rounding of the times in cartographer_ros vs. the times stored in tf2. @dhood 's recent work to store the time as an integer instead of a double may help here; we should see if using an integer inside of cartographer_ros improves things.

[tfoote]

How far are you needing to extrapolate into the future? The symptom sounds more like an issue of the laser data simply getting there before the transforms are available, and the standard solution is to hold the data until the transforms are available. The standard way to do that is with the tf2_ros::MessageFilter http://wiki.ros.org/tf2/Tutorials/Using%20stamped%20datatypes%20with%20tf2%3A%3AMessageFilter

However that hasn't been ported because we don't have the underlying MessageFilter implementation for rcl(cpp).

[clalancette]

So when I briefly looked into a couple of days ago, the differences in what it was requesting and what TF had available was in the 100's of nanoseconds. That's why it seems to me to be a rounding error, rather than TF just not yet having the data available. Of course, I could be wrong :).

[mikaelarguedas]

@allenh1 to look at it soon (with @tfoote ?)

[BrannonKing]

The ExtrapolationException exception that is thrown shouldn't even exist. It is presently thrown when there is only a single transform recorded and you request a specific time. It's also thrown when the requested time exactly matches a transform frame time, which is definitely a bug in TFBuffer. Even if I pass it a future time, it's not an error to extrapolate into the future -- just extrapolate already. Anyhow, I've been using this code in my Cartographer branch, and it works very well:

std::unique_ptr<::cartographer::transform::Rigid3d> TfBridge::LookupToTracking(
    const ::cartographer::common::Time time, const string& frame_id) const {
  try {
    const auto requested_tf_time = ToRos(time);
    const auto requested_time = std::chrono::seconds(requested_tf_time.sec)
                                + std::chrono::nanoseconds(requested_tf_time.nanosec);

    // auto timeout = ::tf2::durationFromSec(lookup_transform_timeout_sec_); // won't compile, no good reason
    tf2::Duration timeout(tf2::Duration::rep(lookup_transform_timeout_sec_ * 1e9));
    auto timestamp = ::tf2::TimePoint(requested_time); // needs an overload to take a ros time obj

    while(true)
      try {
        auto transform = buffer_->lookupTransform(tracking_frame_, frame_id, timestamp, timeout);
        return ::cartographer::common::make_unique<cartographer::transform::Rigid3d>(ToRigid3d(transform));
      } catch (const tf2::ExtrapolationException& ex) {
        timestamp = tf2::TimePointZero; // just take latest if our timestamps match or there is only one
        timeout = tf2::Duration::zero();
      }
  } catch (const tf2::TransformException& ex) {
    LOG(WARNING) << ex.what();
  }
  return nullptr;
}

Looking at it now, I should probably just do a second lookup in the inner catch rather than using that while loop. That would be one line of code less.