Provide distanced add/substract to trajectory iterator

Signed-off-by: Michael X. Grey <[email protected]>

rmf_traffic/include/rmf_traffic/Trajectory.hpp

@@ -367,6 +367,13 @@ class Trajectory::base_iterator
   /// \return a copy of the iterator before it was decremented
   base_iterator operator--(int);
 
+  /// Get the iterator that would be offset from this one by the specified
+  /// amount
+  base_iterator operator+(int offset) const;
+
+  /// Get the iterator that would be offset from this one by the specified
+  /// amount in the opposite direction.
+  base_iterator operator-(int offset) const;
 
   // TODO(MXG): Consider the spaceship operator when we can use C++20
 

rmf_traffic/src/rmf_traffic/Trajectory.cpp

@@ -690,6 +690,38 @@ auto Trajectory::base_iterator<SegT>::operator--(int) -> base_iterator
   return _pimpl->post_decrement<SegT>();
 }
 
+//==============================================================================
+template<typename SegT>
+auto Trajectory::base_iterator<SegT>::operator+(int offset) const
+-> base_iterator
+{
+  const std::size_t N = std::abs(offset);
+  const bool negative = offset < 0;
+
+  base_iterator result = *this;
+  for (std::size_t i=0; i < N; ++i)
+  {
+    if (negative)
+    {
+      --result;
+    }
+    else
+    {
+      ++result;
+    }
+  }
+
+  return result;
+}
+
+//==============================================================================
+template<typename SegT>
+auto Trajectory::base_iterator<SegT>::operator-(int offset) const
+-> base_iterator
+{
+  return *this + (-offset);
+}
+
 //==============================================================================
 #define DEFINE_BASIC_ITERATOR_OP(op) \
   template<typename SegT> \