diff --git a/cartographer/internal/mapping/2d/local_trajectory_builder_2d.cc b/cartographer/internal/mapping/2d/local_trajectory_builder_2d.cc index 6860e47..720dd65 100644 --- a/cartographer/internal/mapping/2d/local_trajectory_builder_2d.cc +++ b/cartographer/internal/mapping/2d/local_trajectory_builder_2d.cc @@ -106,8 +106,18 @@ LocalTrajectoryBuilder2D::AddRangeData( std::vector range_data_poses; range_data_poses.reserve(range_data.returns.size()); + bool warned = false; for (const Eigen::Vector4f& hit : range_data.returns) { - const common::Time time_point = time + common::FromSeconds(hit[3]); + common::Time time_point = time + common::FromSeconds(hit[3]); + if (time_point < extrapolator_->GetLastExtrapolatedTime()) { + if (!warned) { + LOG(ERROR) + << "Timestamp of individual range data point jumps backwards from " + << extrapolator_->GetLastExtrapolatedTime() << " to " << time_point; + warned = true; + } + time_point = extrapolator_->GetLastExtrapolatedTime(); + } range_data_poses.push_back( extrapolator_->ExtrapolatePose(time_point).cast()); } diff --git a/cartographer/internal/mapping/3d/local_trajectory_builder_3d.cc b/cartographer/internal/mapping/3d/local_trajectory_builder_3d.cc index b3e0bdb..f879a09 100644 --- a/cartographer/internal/mapping/3d/local_trajectory_builder_3d.cc +++ b/cartographer/internal/mapping/3d/local_trajectory_builder_3d.cc @@ -84,8 +84,18 @@ LocalTrajectoryBuilder3D::AddRangeData( std::vector hits_poses; hits_poses.reserve(hits.size()); + bool warned = false; for (const Eigen::Vector4f& hit : hits) { - const common::Time time_point = time + common::FromSeconds(hit[3]); + common::Time time_point = time + common::FromSeconds(hit[3]); + if (time_point < extrapolator_->GetLastExtrapolatedTime()) { + if (!warned) { + LOG(ERROR) + << "Timestamp of individual range data point jumps backwards from " + << extrapolator_->GetLastExtrapolatedTime() << " to " << time_point; + warned = true; + } + time_point = extrapolator_->GetLastExtrapolatedTime(); + } hits_poses.push_back( extrapolator_->ExtrapolatePose(time_point).cast()); }