Trajectory player fixes

tesseract_visualization/include/tesseract_visualization/trajectory_player.h

@@ -87,10 +87,16 @@ class TrajectoryPlayer
   double currentDuration() const;
 
   /**
-   * @brief Get the trajectory duration
+   * @brief Get the trajectory duration at the start state
    * @return The trajectory duration
    */
-  double trajectoryDuration() const;
+  double trajectoryDurationStart() const;
+
+  /**
+   * @brief Get the trajectory duration at the end state
+   * @return The trajectory duration
+   */
+  double trajectoryDurationEnd() const;
 
   /**
    * @brief Check if the player has the reached the end of the trajectory
@@ -118,7 +124,8 @@ class TrajectoryPlayer
 
 private:
   TrajectoryInterpolator::UPtr trajectory_{ nullptr };
-  double trajectory_duration_{ 0 };
+  double trajectory_duration_start_{ 0 };
+  double trajectory_duration_end_{ 0 };
   double current_duration_{ 0 };
   double scale_{ 1 };
   bool loop_{ false };

tesseract_visualization/src/trajectory_interpolator.cpp

@@ -38,7 +38,7 @@ TrajectoryInterpolator::TrajectoryInterpolator(tesseract_common::JointTrajectory
   double total_time = 0;
   bool overwrite_dt = false;
   // Check if time is populated
-  if (!trajectory_.empty() && (trajectory_.back().time - trajectory_.front().time) < 1e-3)
+  if ((trajectory_.size() > 1) && (trajectory_.back().time - trajectory_.front().time) < 1e-3)
     overwrite_dt = true;
 
   bool initial_state = true;

tesseract_visualization/src/trajectory_player.cpp

@@ -39,7 +39,8 @@ void TrajectoryPlayer::setTrajectory(const tesseract_common::JointTrajectory& tr
   trajectory_ = std::make_unique<TrajectoryInterpolator>(trajectory);
 
   // Get the duration
-  trajectory_duration_ = trajectory_->getStateDuration(trajectory_->getStateCount() - 1);
+  trajectory_duration_start_ = trajectory_->getStateDuration(0);
+  trajectory_duration_end_ = trajectory_->getStateDuration(trajectory_->getStateCount() - 1);
 
   // Reset state
   reset();
@@ -74,13 +75,13 @@ tesseract_common::JointState TrajectoryPlayer::setCurrentDuration(double duratio
     throw std::runtime_error("Trajectory is empty!");
 
   finished_ = false;
-  if (duration > trajectory_duration_)
+  if (duration > trajectory_duration_end_)
   {
-    current_duration_ = trajectory_duration_;
+    current_duration_ = trajectory_duration_end_;
     finished_ = true;
   }
-  else if (duration < 0)
-    current_duration_ = 0;
+  else if (duration < trajectory_duration_start_)
+    current_duration_ = trajectory_duration_start_;
   else
     current_duration_ = duration;
 
@@ -100,9 +101,9 @@ tesseract_common::JointState TrajectoryPlayer::getNext()
   auto current_time = std::chrono::high_resolution_clock::now();
   current_duration_ = (scale_ * std::chrono::duration<double>(current_time - start_time_).count());
 
-  if (current_duration_ > trajectory_duration_)
+  if (current_duration_ > trajectory_duration_end_)
   {
-    current_duration_ = trajectory_duration_;
+    current_duration_ = trajectory_duration_end_;
 
     // Compute the interpolated state
     auto mi = trajectory_->getState(current_duration_);
@@ -126,7 +127,9 @@ tesseract_common::JointState TrajectoryPlayer::getByIndex(long index) const
 
 double TrajectoryPlayer::currentDuration() const { return current_duration_; }
 
-double TrajectoryPlayer::trajectoryDuration() const { return trajectory_duration_; }
+double TrajectoryPlayer::trajectoryDurationStart() const { return trajectory_duration_start_; }
+
+double TrajectoryPlayer::trajectoryDurationEnd() const { return trajectory_duration_end_; }
 
 bool TrajectoryPlayer::isFinished() const { return finished_; }
 
@@ -137,7 +140,7 @@ bool TrajectoryPlayer::isLoopEnabled() const { return loop_; }
 void TrajectoryPlayer::reset()
 {
   // Reset state associated with trajectory playback
-  current_duration_ = 0.0;
+  current_duration_ = trajectory_duration_start_;
 
   // Get the chrono time
   start_time_ = std::chrono::high_resolution_clock::now();

tesseract_visualization/test/trajectory_player_unit.cpp

@@ -52,7 +52,7 @@ TEST(TesseracTrajectoryPlayerUnit, TrajectoryTest)  // NOLINT
   TrajectoryPlayer player;
   player.setTrajectory(trajectory);
 
-  EXPECT_NEAR(player.trajectoryDuration(), 9, 1e-5);
+  EXPECT_NEAR(player.trajectoryDurationEnd(), 9, 1e-5);
   EXPECT_NEAR(player.currentDuration(), 0, 1e-5);
 
   for (long i = 0; i < 10; ++i)