Fix discrepancy between tf and odometry/filtered when world_frame is set to map_frame

doc/state_estimation_nodes.rst

@@ -67,7 +67,13 @@ Some packages require that your transforms are future-dated by a small time offs
 
 ~transform_timeout
 ^^^^^^^^^^^^^^^^^^
-The ``robot_localization`` package uses ``tf2``'s ``lookupTransform`` method to request transformations. This parameter specifies how long we would like to wait if a transformation is not available yet. Defaults to 0 if not set. The value 0 means we just get us the latest available (see ``tf2`` implementation) transform so we are not blocking the filter. Specifying a non-zero `transform_timeout` affects the filter's timing since it waits for a maximum of the `transform_timeout` for a transform to become available. This directly implies that mostly the specified desired output rate is not met since the filter has to wait for transforms when updating.
+The ``robot_localization`` package uses ``tf2``'s ``lookupTransform`` method to obtain the transformations needed to convert measurements into a target frame (likely being base_link). This parameter specifies the maximum amount of time, in seconds, to wait for the desired transformation to become available. The parameter defaults to a value of 0 if not set. A value of 0 means that only currently available information is used. This may not result in the ideal transformation, but ensures that the filter execution is not blocked. A non-zero value can yield a more accurate transformation but can temporarely block the execution of the filter. The filter's timing can be affected if excessive blocking occurs causing it to be unable to maintain the specified output rate.
+
+~transform_timeout_odom_bl
+^^^^^^^^^^^^^^^^^^^^^^^^^^
+When ``world_frame`` is set to the same value as ``map_frame``, the filter computes the transformation from ``map_frame`` to ``base_link_frame``. If ``publish_tf`` is set to *true*, this transformation should be published to ``tf2``. However, publishing it directly is not permitted due to the frame conventions established by `REP-105 <https://www.ros.org/reps/rep-0105.html>`_. Instead, the filter must look up the ``odom_frame`` to ``base_link_frame`` transformation and use it to compute and broadcast the ``map_frame`` to ``odom_frame`` transformation.
+
+The ``robot_localization`` package uses ``tf2``'s ``lookupTransform`` method to obtain the required ``odom_frame`` to ``base_link_frame`` transformation. This parameter specifies the maximum amount of time, in seconds, to wait for the desired transformation to become available. Setting it to a suitable value can improve performance in applications that rely on accurate transformations related to the ``map_frame``. The parameter defaults to a value of 0 if not set. A value of 0 means that only currently available information is used. This may not result in the ideal transformation, but ensures that the filter execution is not blocked. A non-zero value can yield a more accurate transformation but can temporarely block the execution of the filter. The filter's timing can be affected if excessive blocking occurs causing it to be unable to maintain the specified output rate.
 
 ~[sensor]
 ^^^^^^^^^

include/robot_localization/ros_filter.hpp

@@ -762,11 +762,17 @@ class RosFilter : public rclcpp::Node
   //!
   rclcpp::Time latest_control_time_;
 
-  //! @brief Parameter that specifies the how long we wait for a transform to
+  //! @brief Parameter that specifies how long we wait for a transform to
   //! become available.
   //!
   rclcpp::Duration tf_timeout_;
 
+  //! @brief Parameter that specifies how long we wait for the odom->base_link
+  //! transform to become available. Only relevant when the world_frame is the same
+  //! as the map_frame and publish_tf is set to true.
+  //!
+  rclcpp::Duration tf_timeout_odom_bl_;
+
   //! @brief For future (or past) dating the world_frame->base_link_frame
   //! transform
   //!

src/ros_filter.cpp

@@ -111,6 +111,7 @@ RosFilter<T>::RosFilter(const rclcpp::NodeOptions & options)
   last_set_pose_time_(0, 0, RCL_ROS_TIME),
   latest_control_time_(0, 0, RCL_ROS_TIME),
   tf_timeout_(0ns),
+  tf_timeout_odom_bl_(0ns),
   tf_time_offset_(0ns)
 {
   tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
@@ -960,6 +961,10 @@ void RosFilter<T>::loadParams()
   double timeout_tmp = this->declare_parameter("transform_timeout", 0.0);
   tf_timeout_ = rclcpp::Duration::from_seconds(timeout_tmp);
 
+  // Transform timeout odom->base_link
+  double timeout_odom_bl_tmp = this->declare_parameter("transform_timeout_odom_bl", 0.0);
+  tf_timeout_odom_bl_ = rclcpp::Duration::from_seconds(timeout_odom_bl_tmp);
+
   // Update frequency and sensor timeout
   frequency_ = this->declare_parameter("frequency", 30.0);
 
@@ -1139,6 +1144,7 @@ void RosFilter<T>::loadParams()
       "\nworld_frame is " << world_frame_id_ <<
       "\ntransform_time_offset is " << filter_utilities::toSec(tf_time_offset_) <<
       "\ntransform_timeout is " << filter_utilities::toSec(tf_timeout_) <<
+      "\ntransform_timeout_odom_bl is " << filter_utilities::toSec(tf_timeout_odom_bl_) <<
       "\nfrequency is " << frequency_ <<
       "\nsensor_timeout is " << filter_utilities::toSec(filter_.getSensorTimeout()) <<
       "\ntwo_d_mode is " << (two_d_mode_ ? "true" : "false") <<
@@ -2219,30 +2225,29 @@ void RosFilter<T>::periodicUpdate()
       if (filtered_position->header.frame_id == odom_frame_id_) {
         world_transform_broadcaster_->sendTransform(world_base_link_trans_msg_);
       } else if (filtered_position->header.frame_id == map_frame_id_) {
-        try {
-          tf2::Transform world_base_link_trans;
-          tf2::fromMsg(
-            world_base_link_trans_msg_.transform,
-            world_base_link_trans);
-
-          tf2::Transform base_link_odom_trans;
-          tf2::fromMsg(
-            tf_buffer_
-            ->lookupTransform(
-              base_link_frame_id_,
-              odom_frame_id_,
-              tf2::TimePointZero)
-            .transform,
-            base_link_odom_trans);
-
+        tf2::Transform world_base_link_trans;
+        tf2::fromMsg(
+          world_base_link_trans_msg_.transform,
+          world_base_link_trans);
+
+        tf2::Transform base_link_odom_trans;
+        bool can_transform_odom = ros_filter_utilities::lookupTransformSafe(
+          tf_buffer_.get(),
+          base_link_frame_id_,
+          odom_frame_id_,
+          filtered_position->header.stamp,
+          tf_timeout_odom_bl_,
+          base_link_odom_trans);
+
+        if(can_transform_odom) {
           /*
            * First, see these two references:
            * http://wiki.ros.org/tf/Overview/Using%20Published%20Transforms#lookupTransform
            * http://wiki.ros.org/geometry/CoordinateFrameConventions#Transform_Direction
            * We have a transform from map_frame_id_->base_link_frame_id_, but
            * it would actually transform a given pose from
-           * base_link_frame_id_->map_frame_id_. We then used lookupTransform,
-           * whose first two arguments are target frame and source frame, to
+           * base_link_frame_id_->map_frame_id_. We then used lookupTransformSafe,
+           * whose second and third arguments are target frame and source frame, to
            * get a transform from base_link_frame_id_->odom_frame_id_.
            * However, this transform would actually transform data from
            * odom_frame_id_->base_link_frame_id_. Now imagine that we have a
@@ -2268,7 +2273,7 @@ void RosFilter<T>::periodicUpdate()
           map_odom_trans_msg.child_frame_id = odom_frame_id_;
 
           world_transform_broadcaster_->sendTransform(map_odom_trans_msg);
-        } catch (...) {
+        } else {
           RCLCPP_ERROR_STREAM_SKIPFIRST_THROTTLE(
             get_logger(),
             *get_clock(),