RViz fails to display messages with timestamps slightly in the future

[rayferric]

RViz fails to display messages with timestamps slightly in the future

RViz skips displaying messages with timestamps a few milliseconds into the future, showing a vague error message:

rviz/rviz_common/src/rviz_common/display.cpp

Lines 211 to 218 in 325f272

	void Display::setMissingTransformToFixedFrame(
	const std::string & frame, const std::string & additional_info)
	{
	std::string error_string =
	"Could not transform " + (additional_info.empty() ? "from [" : additional_info + " from [") +
	frame + "] to [" + fixed_frame_.toStdString() + "]";
	setStatusStd(properties::StatusProperty::Error, "Transform", error_string);
	}

Upon investigation, I traced the issue to the TFWrapper class, where TF2 is queried without any fallback in case of an ExtrapolationError:

rviz/rviz_default_plugins/src/rviz_default_plugins/transformation/tf_wrapper.cpp

Lines 47 to 53 in 325f272

	void TFWrapper::transform(
	const geometry_msgs::msg::PoseStamped & pose_in,
	geometry_msgs::msg::PoseStamped & pose_out,
	const std::string & frame)
	{
	buffer_->transform(pose_in, pose_out, frame);
	}

RViz handles messages slightly in the past correctly, so it’s unclear why messages with timestamps slightly in the future are not displayed. While TF2 may seem like the cause, it is reasonable to expect users to handle extrapolation themselves.

Possible Solutions

1. Message History: RViz could maintain a history of messages and display the most recent message that can be transformed without extrapolation.
2. Extrapolation by RViz: RViz could extrapolate the transform itself.
3. Skip Extrapolation: If TF2 fails with tf2::ExtrapolationException, simply use the latest available transform in the TF buffer.

Proposed Solution (Option 3)

Here’s a potential fix using the third approach:

void TFWrapper::transform(
  const geometry_msgs::msg::PoseStamped & pose_in,
  geometry_msgs::msg::PoseStamped & pose_out,
  const std::string & frame)
{
  try {
    buffer_->transform(pose_in, pose_out, frame);
  } catch (const tf2::ExtrapolationException & exception) {
    (void) exception;
    geometry_msgs::msg::PoseStamped zero_pose = pose_in;
    zero_pose.header.stamp = rclcpp::Time(0);
    buffer_->transform(zero_pose, pose_out, frame);
  }
}

Similar changes would need to be applied to other TFWrapper methods.