From 71f04ae70fcb74baf2fbe6509e915c1291b80329 Mon Sep 17 00:00:00 2001 From: Pascal Leroy Date: Thu, 23 Dec 2021 22:59:34 +0100 Subject: [PATCH 1/4] Proper management of segments_by_left_endpoints_ in Merge when a segment has a single point. --- physics/discrete_trajectory_body.hpp | 32 +++++++++++++++++++--------- physics/discrete_trajectory_test.cpp | 12 +++++++++++ 2 files changed, 34 insertions(+), 10 deletions(-) diff --git a/physics/discrete_trajectory_body.hpp b/physics/discrete_trajectory_body.hpp index 153786e66e..2fc746ac43 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()); + } } + // sInsert a new entry in the time-to-segment map if the segment is not + // empty. This entry will be overridden 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()) { + 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..6980a4c369 100644 --- a/physics/discrete_trajectory_test.cpp +++ b/physics/discrete_trajectory_test.cpp @@ -52,6 +52,7 @@ using testing_utilities::EqualsProto; using testing_utilities::IsNear; using testing_utilities::NewCircularTrajectoryTimeline; using testing_utilities::NewLinearTrajectoryTimeline; +using testing_utilities::NewMotionlessTrajectoryTimeline; using testing_utilities::ReadFromBinaryFile; using testing_utilities::StringLogSink; using ::testing::AllOf; @@ -628,6 +629,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) { From 7dfde9c8b34196d078acc971c87efa7a7f43a8ff Mon Sep 17 00:00:00 2001 From: Pascal Leroy Date: Thu, 23 Dec 2021 23:34:19 +0100 Subject: [PATCH 2/4] Handle empty segments in consistency checks. --- physics/discrete_trajectory_body.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/physics/discrete_trajectory_body.hpp b/physics/discrete_trajectory_body.hpp index 2fc746ac43..91c48269b4 100644 --- a/physics/discrete_trajectory_body.hpp +++ b/physics/discrete_trajectory_body.hpp @@ -732,7 +732,7 @@ absl::Status DiscreteTrajectory::ConsistencyStatus() const { " and ", DebugString(sit2->front().time))); } } - if (sit1 != segments_->cend()) { + 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 ", From 7a0cb0d38211113ce8994285e223efe9ecc401ce Mon Sep 17 00:00:00 2001 From: Pascal Leroy Date: Fri, 24 Dec 2021 00:02:33 +0100 Subject: [PATCH 3/4] Typos. --- physics/discrete_trajectory_body.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/physics/discrete_trajectory_body.hpp b/physics/discrete_trajectory_body.hpp index 91c48269b4..321ef98928 100644 --- a/physics/discrete_trajectory_body.hpp +++ b/physics/discrete_trajectory_body.hpp @@ -397,7 +397,7 @@ void DiscreteTrajectory::Merge(DiscreteTrajectory trajectory) { sit_t->Merge(std::move(*sit_s)); // If the left endpoint of |sit_t| has changed, remove its entry from the - // time-to- segment map, if any. + // time-to-segment map, if any. if (left_endpoint.has_value() && sit_t->front().time < left_endpoint.value()) { auto const it = segment_by_left_endpoint_.find(left_endpoint.value()); @@ -405,8 +405,8 @@ void DiscreteTrajectory::Merge(DiscreteTrajectory trajectory) { segment_by_left_endpoint_.erase(left_endpoint.value()); } } - // sInsert a new entry in the time-to-segment map if the segment is not - // empty. This entry will be overridden by any future entry at the same + // 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); From fc5bca034f3462c6fe2c0e064a595abc8e8f89de Mon Sep 17 00:00:00 2001 From: Pascal Leroy Date: Fri, 24 Dec 2021 00:06:11 +0100 Subject: [PATCH 4/4] Cleanup. --- physics/discrete_trajectory_test.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/physics/discrete_trajectory_test.cpp b/physics/discrete_trajectory_test.cpp index 6980a4c369..92d794da74 100644 --- a/physics/discrete_trajectory_test.cpp +++ b/physics/discrete_trajectory_test.cpp @@ -52,7 +52,6 @@ using testing_utilities::EqualsProto; using testing_utilities::IsNear; using testing_utilities::NewCircularTrajectoryTimeline; using testing_utilities::NewLinearTrajectoryTimeline; -using testing_utilities::NewMotionlessTrajectoryTimeline; using testing_utilities::ReadFromBinaryFile; using testing_utilities::StringLogSink; using ::testing::AllOf;