Adds a validity check for Sessions created using the

`TimeVaryingVolumetricGrid`

This is needed to resolve a bug in gazebosim/gz-sim#1842 (review). 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 <[email protected]>

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

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 {
@@ -165,6 +170,11 @@ namespace gz
           nextTime = std::next(nextTime);
         }
         newSess.time = _time;
+
+        /*if (nextTime == this->gridFields.end())
+        {
+          return std::nullopt;
+        }*/
         return newSess;
       }
 

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);

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));
 }