local trajectory: ensuring timestamp is in boulds

source/armarx/navigation/core/Trajectory.cpp

@@ -18,10 +18,11 @@
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 #include <ArmarXCore/core/logging/Logging.h>
 
-#include "range/v3/algorithm/none_of.hpp"
 #include <armarx/navigation/conversions/eigen.h>
 #include <armarx/navigation/core/basic_types.h>
 #include <armarx/navigation/core/types.h>
+
+#include <range/v3/algorithm/none_of.hpp>
 #include <range/v3/action/insert.hpp>
 #include <range/v3/numeric/accumulate.hpp>
 #include <range/v3/range/conversion.hpp>
@@ -157,7 +158,6 @@ namespace armarx::navigation::core
 
     } // namespace conv
 
-
     GlobalTrajectory::InternalProjection
     GlobalTrajectory::projectInternal(const Position& point,
                                       const VelocityInterpolation& velocityInterpolation) const
@@ -641,7 +641,6 @@ namespace armarx::navigation::core
         return pts;
     }
 
-
     float
     GlobalTrajectory::duration(const core::VelocityInterpolation interpolation) const
     {
@@ -748,8 +747,11 @@ namespace armarx::navigation::core
     }
 
     Evaluation
-    LocalTrajectory::evaluate(const armarx::core::time::DateTime& timestamp) const
+    LocalTrajectory::evaluate(const armarx::core::time::DateTime& ts) const
     {
+        const auto& finalTimestamp = points().back().timestamp;
+        const auto timestamp = ts > finalTimestamp ? finalTimestamp - Duration::MilliSeconds(1) : ts;
+
         const auto cmp = [](const core::LocalTrajectoryPoint& lhs,
                             const DateTime& timestamp) -> bool
         { return lhs.timestamp < timestamp; };