diff --git a/physics/discrete_trajectory_body.hpp b/physics/discrete_trajectory_body.hpp index 153786e66e..321ef98928 100644 --- a/physics/discrete_trajectory_body.hpp +++ b/physics/discrete_trajectory_body.hpp @@ -187,7 +187,7 @@ DiscreteTrajectory::NewSegment() { segment_by_left_endpoint_.end(), last_time, new_segment_sit); } - DCHECK_OK(ConsistencyStatus()); + CHECK_OK(ConsistencyStatus()); return new_self; } @@ -205,7 +205,7 @@ DiscreteTrajectory::DetachSegments(SegmentIterator const begin) { /*to=*/detached, /*to_segments_begin=*/detached.segments_->begin()); - DCHECK_OK(ConsistencyStatus()); + CHECK_OK(ConsistencyStatus()); return detached; } @@ -245,7 +245,7 @@ DiscreteTrajectory::AttachSegments(DiscreteTrajectory trajectory) { /*to=*/*this, /*to_segments_begin=*/end_before_splice); - DCHECK_OK(ConsistencyStatus()); + CHECK_OK(ConsistencyStatus()); return SegmentIterator(segments_.get(), end_before_splice); } @@ -270,7 +270,7 @@ void DiscreteTrajectory::DeleteSegments(SegmentIterator& begin) { // Make sure that the client doesn't try to use the invalid iterator. begin = segments().end(); - DCHECK_OK(ConsistencyStatus()); + CHECK_OK(ConsistencyStatus()); } template @@ -300,7 +300,7 @@ void DiscreteTrajectory::ForgetAfter(Instant const& t) { segment_by_left_endpoint_.end()); } - DCHECK_OK(ConsistencyStatus()); + CHECK_OK(ConsistencyStatus()); } template @@ -342,7 +342,7 @@ void DiscreteTrajectory::ForgetBefore(Instant const& t) { sit); } - DCHECK_OK(ConsistencyStatus()); + CHECK_OK(ConsistencyStatus()); } template @@ -396,12 +396,18 @@ void DiscreteTrajectory::Merge(DiscreteTrajectory trajectory) { // Merge corresponding segments. sit_t->Merge(std::move(*sit_s)); - // If the left endpoint has changed, remove its entry from the time-to- - // segment map. Insert a new entry if the segment is not empty. + // If the left endpoint of |sit_t| has changed, remove its entry from the + // time-to-segment map, if any. if (left_endpoint.has_value() && sit_t->front().time < left_endpoint.value()) { - segment_by_left_endpoint_.erase(left_endpoint.value()); + auto const it = segment_by_left_endpoint_.find(left_endpoint.value()); + if (it != segment_by_left_endpoint_.end() && it->second == sit_t) { + segment_by_left_endpoint_.erase(left_endpoint.value()); + } } + // Insert a new entry in the time-to-segment map if the segment is not + // empty. This entry will be overwritten by any future entry at the same + // time, thereby enforcing the invariants of the time-to-segment map. if (!sit_t->empty()) { segment_by_left_endpoint_.insert_or_assign(sit_t->front().time, sit_t); } @@ -431,7 +437,7 @@ void DiscreteTrajectory::Merge(DiscreteTrajectory trajectory) { } } - DCHECK_OK(ConsistencyStatus()); + CHECK_OK(ConsistencyStatus()); } template @@ -726,6 +732,12 @@ absl::Status DiscreteTrajectory::ConsistencyStatus() const { " and ", DebugString(sit2->front().time))); } } + if (sit1 != segments_->cend() && !sit1->empty()) { + return absl::InternalError( + absl::StrCat("Segment at time ", DebugString(sit1->front().time), + " missing in the time-to-segment map of size ", + segment_by_left_endpoint_.size())); + } } if (!segments_->empty()) { int i = 0; diff --git a/physics/discrete_trajectory_test.cpp b/physics/discrete_trajectory_test.cpp index 359d5d2260..92d794da74 100644 --- a/physics/discrete_trajectory_test.cpp +++ b/physics/discrete_trajectory_test.cpp @@ -628,6 +628,17 @@ TEST_F(DiscreteTrajectoryTest, Merge) { EXPECT_EQ(t0_ + 9 * Second, sit->front().time); EXPECT_EQ(t0_ + 14 * Second, sit->back().time); } + { + auto trajectory1 = MakeTrajectory(); + auto trajectory2 = MakeTrajectory(); + + trajectory1.ForgetAfter(t0_ + 9 * Second); + // This trajectory starts with a 1-point segment. Merge used to fail the + // consistency check because the time-to-segment map was losing an entry. + trajectory2.ForgetBefore(t0_ + 9 * Second); + + trajectory2.Merge(std::move(trajectory1)); + } } TEST_F(DiscreteTrajectoryTest, TMinTMaxEvaluate) {