From 3d9cf330c9eaa04c1e2c3ee8c95c4c3c510b8787 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjo@openrobotics.org>
Date: Tue, 29 Aug 2023 23:23:31 +0800
Subject: [PATCH] Adds a validity check for Sessions created using the
 `TimeVaryingVolumetricGrid` (#551)

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<Session>`, 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 <arjoc@intrinsic.ai>
---
 include/gz/math/TimeVaryingVolumetricGrid.hh  | 22 ++++++++++++++++++-
 .../TimeVaryingVolumetricGridLookupField.hh   |  5 +++++
 ...meVaryingVolumetricGridLookupField_TEST.cc |  8 +++++++
 src/TimeVaryingVolumetricGrid_TEST.cc         |  7 ++++++
 4 files changed, 41 insertions(+), 1 deletion(-)

diff --git a/include/gz/math/TimeVaryingVolumetricGrid.hh b/include/gz/math/TimeVaryingVolumetricGrid.hh
index 4b98a234a..27c737f3c 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<V> LookUp(const S &_session, const Vector3<P> &_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<V>, Vector3<V>> Bounds(const S &_session) const;
 };
@@ -71,6 +79,18 @@ class TimeVaryingVolumetricGrid<T, V, InMemorySession<T, P>, P>
     return indices.CreateSession();
   }
 
+  /// \brief Documentation Inherited
+  public: InMemorySession<T, V> CreateSession(const T &_time) const
+  {
+    return indices.CreateSession(_time);
+  }
+
+  /// \brief Documentation Inherited
+  public: bool IsValid(const InMemorySession<T, P> &_session) const
+  {
+    return indices.IsValid(_session);
+  }
+
   /// \brief Documentation Inherited
   public: std::optional<InMemorySession<T, P>>
     StepTo(const InMemorySession<T, double> &_session, const T &_time) const
diff --git a/include/gz/math/TimeVaryingVolumetricGridLookupField.hh b/include/gz/math/TimeVaryingVolumetricGridLookupField.hh
index b505b6acd..9acb75e25 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<T, V> &_session) const {
+        return this->gridFields.end() != _session.iter;
+      }
+
       /// \brief Documentation inherited
       public: std::optional<InMemorySession<T, V>> StepTo(
         const InMemorySession<T, V> &_session, const T &_time) const {
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..f99f156aa 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(invalid_session));
 }