Wrong timeout for waitForTransform (mrpt_rbpf_slam)

[albertkasdorf]

Hi, I am using the mrpt_rbpf_slam for my bachelor thesis and I often get the following message:

"Failed to get transform target_frame (odom) to source_frame (base_link)"

I added the TransformException messages to the log and get the following information:

"Lookup would require extrapolation into the future. Requested time 1510749857.878693103 but the latest data is at time 1510749857.877105724, when looking up transform from frame [base_link] to frame [odom]"

After looking at the source code of the method "PfslamWrapper::waitForTransform" I found out, that the "timeout" parameter is never used in the method. Instead the parameter "polling_sleepduration" is used as the "timeout" parameter for the method "listenerTF.waitForTransform" with a duration of 0.01. That duration is a little bit short. So I fixed that and the message is gone!

Fixed method:

bool PFslamWrapper::waitForTransform(
  mrpt::poses::CPose3D& des,
  const std::string& target_frame,
  const std::string& source_frame,
  const ros::Time& time,
  const ros::Duration& timeout,
  const ros::Duration& polling_sleep_duration)
{
  tf::StampedTransform transform;
  try
  {
    listenerTF_.waitForTransform(target_frame, source_frame, time, timeout, polling_sleep_duration);
    listenerTF_.lookupTransform(target_frame, source_frame, time, transform);
  }
  catch (tf::TransformException ex)
  {
    ROS_ERROR(
      "Failed to subtract global_frame (%s) from odom_frame (%s). TransformException: %s",
      target_frame.c_str(), source_frame.c_str(), ex.what());
    return false;
  }
  mrpt_bridge::convert(transform, des);
  return true;
}

[jlblancoc]

👍 Great! Thanks for the feedback.

Please, propose a Pull Request with the change, so we can merge the patch and your GH user is credited with the change.

Cheers.