From 9a595cde956d42d019b7b89bfa9f43d1faf111b5 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 28 Aug 2023 10:38:12 +0000 Subject: [PATCH] Adds a validity check for Sessions created using the `TimeVaryingVolumetricGrid` This is needed to resolve a bug in https://github.com/gazebosim/gz-sim/pull/1842#pullrequestreview-1596324924. In particular part of the issue is poor API design. Ideally `CreateSession` would return an `Option`, however it returns a `Session` even if it is not possible to create the session (for instance if it is too far ahead in time or an empty grid). Hence downstream users need a way to verify session validity. Signed-off-by: Arjo Chakravarty --- include/gz/math/TimeVaryingVolumetricGrid.hh | 22 ++++++++++++++++++- .../TimeVaryingVolumetricGridLookupField.hh | 10 +++++++++ ...meVaryingVolumetricGridLookupField_TEST.cc | 8 +++++++ src/TimeVaryingVolumetricGrid_TEST.cc | 7 ++++++ 4 files changed, 46 insertions(+), 1 deletion(-) diff --git a/include/gz/math/TimeVaryingVolumetricGrid.hh b/include/gz/math/TimeVaryingVolumetricGrid.hh index 4b98a234a..bce5b9405 100644 --- a/include/gz/math/TimeVaryingVolumetricGrid.hh +++ b/include/gz/math/TimeVaryingVolumetricGrid.hh @@ -41,6 +41,14 @@ class TimeVaryingVolumetricGrid /// \brief Creates a session. Call this before querying the interface. public: S CreateSession() const; + /// \brief Creates a session at a given time. + /// Call this before querying the interface. + /// \param[in] _time - The time at which to create the session. + public: S CreateSession(const T &_time) const; + + /// \brief Returns true if a session is valid. False if invalid, + public: bool IsValid(const S &_session) const; + /// \brief Steps the session to a fixed time step /// \param[in] _session - The session to be stepped /// \param[in] _time - The time at which the session should be set. @@ -54,7 +62,7 @@ class TimeVaryingVolumetricGrid public: std::optional LookUp(const S &_session, const Vector3

&_pos) const; - /// \brief Get the bounds of this grid field at given time. + /// \brief Get the spatial bounds of this grid field at given time. /// \return A pair of vectors. All zeros if session is invalid. public: std::pair, Vector3> Bounds(const S &_session) const; }; @@ -71,6 +79,18 @@ class TimeVaryingVolumetricGrid, P> return indices.CreateSession(); } + /// \brief Documentation Inherited + public: InMemorySession CreateSession(const T &_time) const + { + return indices.CreateSession(_time); + } + + /// \brief Documentation Inherited + public: bool IsValid(const InMemorySession &_session) const + { + return indices.IsValid(_session); + } + /// \brief Documentation Inherited public: std::optional> StepTo(const InMemorySession &_session, const T &_time) const diff --git a/include/gz/math/TimeVaryingVolumetricGridLookupField.hh b/include/gz/math/TimeVaryingVolumetricGridLookupField.hh index b505b6acd..c937a1999 100644 --- a/include/gz/math/TimeVaryingVolumetricGridLookupField.hh +++ b/include/gz/math/TimeVaryingVolumetricGridLookupField.hh @@ -142,6 +142,11 @@ namespace gz return sess; } + /// \brief In memory session + public: bool IsValid(const InMemorySession &_session) const { + return this->gridFields.end() != _session.iter; + } + /// \brief Documentation inherited public: std::optional> StepTo( const InMemorySession &_session, const T &_time) const { @@ -165,6 +170,11 @@ namespace gz nextTime = std::next(nextTime); } newSess.time = _time; + + /*if (nextTime == this->gridFields.end()) + { + return std::nullopt; + }*/ return newSess; } diff --git a/src/TimeVaryingVolumetricGridLookupField_TEST.cc b/src/TimeVaryingVolumetricGridLookupField_TEST.cc index 3fbd7c18a..c1c1b7f1d 100644 --- a/src/TimeVaryingVolumetricGridLookupField_TEST.cc +++ b/src/TimeVaryingVolumetricGridLookupField_TEST.cc @@ -44,6 +44,10 @@ TEST(TimeVaryingVolumetricLookupFieldTest, TestConstruction) { // Get data at T=0 auto session = timeVaryingField.CreateSession(); + + // Check session validity + ASSERT_TRUE(timeVaryingField.IsValid(session)); + auto points = timeVaryingField.LookUp(session, Vector3d{0.5, 0.5, 0.5}); ASSERT_EQ(points.size(), 2); ASSERT_EQ(points[0].time, 0); @@ -100,6 +104,10 @@ TEST(TimeVaryingVolumetricLookupFieldTest, TestConstruction) // Create invalid session auto invalid_session = timeVaryingField.CreateSession(500); + + // Check session validity + ASSERT_FALSE(timeVaryingField.IsValid(invalid_session)); + points = timeVaryingField.LookUp( invalid_session, Vector3d{0.5, 0.5, 0.5}); ASSERT_EQ(points.size(), 0); diff --git a/src/TimeVaryingVolumetricGrid_TEST.cc b/src/TimeVaryingVolumetricGrid_TEST.cc index 685c75253..6173bea95 100644 --- a/src/TimeVaryingVolumetricGrid_TEST.cc +++ b/src/TimeVaryingVolumetricGrid_TEST.cc @@ -40,6 +40,9 @@ TEST(TimeVaryingVolumetricGridTest, TestConstruction) auto grid = gridFactory.Build(); auto session = grid.CreateSession(); + // Check validity + ASSERT_TRUE(grid.IsValid(session)); + // Check stepping auto val = grid.LookUp(session, Vector3d{0.5, 0.5, 0.5}); ASSERT_TRUE(val.has_value()); @@ -114,4 +117,8 @@ TEST(TimeVaryingVolumetricGridTest, TestEmptyGrid) ASSERT_NEAR(max.X(), 0, 1e-6); ASSERT_NEAR(max.Y(), 0, 1e-6); ASSERT_NEAR(max.Z(), 0, 1e-6); + + auto invalid_session = grid.CreateSession(500); + // Check validity + ASSERT_FALSE(grid.IsValid(session)); }