feat: add function that returns the total trajectory duration

aica-technology · Jan 31, 2025 · bd0c01c · bd0c01c
1 parent 822401b
commit bd0c01c
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 0 deletions.
diff --git a/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp b/source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp
@@ -69,6 +69,12 @@ class TrajectoryBase : public State {
    */
   const std::vector<std::chrono::nanoseconds> get_times_from_start() const;
 
+  /**
+   * @brief Get the total duration of the trajectory
+   * @return the total duration of the trajectory
+   */
+  const std::chrono::nanoseconds get_trajectory_duration() const;
+
   /**
    * @brief Get number of points in trajectory
    * @return the number of points in trajectory
@@ -367,6 +373,11 @@ inline const std::vector<std::chrono::nanoseconds> TrajectoryBase<TrajectoryT>::
   return times_from_start;
 }
 
+template<typename TrajectoryT>
+inline const std::chrono::nanoseconds TrajectoryBase<TrajectoryT>::get_trajectory_duration() const {
+  return this->get_size() ? this->get_times_from_start().back() : std::chrono::nanoseconds(0);
+}
+
 template<typename TrajectoryT>
 inline unsigned int TrajectoryBase<TrajectoryT>::get_size() const {
   return this->points_.size();

diff --git a/source/state_representation/test/tests/test_trajectory.cpp b/source/state_representation/test/tests/test_trajectory.cpp
@@ -357,6 +357,7 @@ TYPED_TEST_P(TrajectoryTest, Getters) {
     EXPECT_EQ(this->trajectory->get_time_from_start(i), time_from_start);
     EXPECT_EQ(this->trajectory->get_duration(i), durations[i]);
   }
+  EXPECT_EQ(this->trajectory->get_trajectory_duration(), time_from_start);
 
   this->trajectory->reset();
   EXPECT_EQ(this->trajectory->get_size(), 0);