Better behavior when the base to sensor transform is not available

[pavel-kirienko]

Hi everyone,

Current odometer implementation may break the estimated odometry if the transfom from base frame to camera frame becomes unavailable for a short period of time.

The problem is here. I suggest to fix that by using the last available transform in case when the transform for requested timestamp is not available.

My suggestion is based on the assumption that most of the robotic applications use cameras with static, non-identity base-to-camera transforms, therefore in case of TF failure it is better to assume that the transform was not changed.

Suggested patch:

@@ -123,12 +123,14 @@ protected:
     }
     else
     {
-      ROS_WARN_THROTTLE(10.0, "The tf from '%s' to '%s' does not seem to be available, "
-                              "will assume it as identity!", 
+      ROS_WARN_THROTTLE(10.0, "The tf from '%s' to '%s' does not seem to be available", 
                               base_link_frame_id_.c_str(),
                               sensor_frame_id_.c_str());
       ROS_DEBUG("Transform error: %s", error_msg.c_str());
-      base_to_sensor.setIdentity();
+      tf_listener_.lookupTransform(
+          base_link_frame_id_,
+          sensor_frame_id_,
+          ros::Time(0), base_to_sensor);
     }

[pavel-kirienko]

I'd like to bring the same suggestion for your FOVIS wrapper.

[plnegre]

Hi pavel-kirienko!

Thank you very much for the comments! The best way to implement your suggestions is that you do a fork of our repositories, make the appropriate changes and apply for a pull request. We will check and test the whole code and publish it if everything is fine.

If you have no time to make the changes and do the pull request, we will take into account this issue in the following commits.

Thank you!

[stwirth]

I would rather suggest to cache the tf in a member variable and use this when the tf lookup fails, than using a lookup with ros::Time(0), as this could fail as well if the cache of the tf listener is not big enough (or tf has some other problem).

[pavel-kirienko]

Nice catch, I'll take it into account.