diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index 9b73139..74aa1b6 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -136,10 +136,13 @@ void Node::PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event) { geometry_msgs::TransformStamped stamped_transform; // If we do not publish a new point cloud, we still allow time of the - // published poses to advance. - stamped_transform.header.stamp = ros::Time::now(); - const Rigid3d tracking_to_local = - extrapolator.ExtrapolatePose(FromRos(stamped_transform.header.stamp)); + // published poses to advance. If we already know a newer pose, we use its + // time instead. Since tf knows how to interpolate, providing newer + // information is better. + const ::cartographer::common::Time now = + std::max(FromRos(ros::Time::now()), extrapolator.GetLastPoseTime()); + stamped_transform.header.stamp = ToRos(now); + const Rigid3d tracking_to_local = extrapolator.ExtrapolatePose(now); const Rigid3d tracking_to_map = trajectory_state.local_to_map * tracking_to_local;