trajectory_player.cpp

Go to the documentation of this file.

 
#include <tesseract_common/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <memory>
TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
#include <tesseract_visualization/trajectory_player.h>
#include <tesseract_visualization/trajectory_interpolator.h>
 
namespace tesseract_visualization
{
TrajectoryPlayer::TrajectoryPlayer() = default;
TrajectoryPlayer::~TrajectoryPlayer() = default;
 
void TrajectoryPlayer::setTrajectory(const tesseract_common::JointTrajectory& trajectory)
{
  // Prepare the new trajectory message
  trajectory_ = std::make_unique<TrajectoryInterpolator>(trajectory);
 
  // Get the duration
  trajectory_duration_start_ = trajectory_->getStateDuration(0);
  trajectory_duration_end_ = trajectory_->getStateDuration(trajectory_->getStateCount() - 1);
 
  // Reset state
  reset();
}
 
void TrajectoryPlayer::setScale(double scale) { scale_ = scale; }
 
tesseract_common::JointState TrajectoryPlayer::setCurrentDurationByIndex(long index)
{
  if (!trajectory_ || trajectory_->empty())
    throw std::runtime_error("Trajectory is empty!");
 
  if (trajectory_->getStateCount() > 0)
  {
    if (index > 0)
      current_duration_ = trajectory_->getStateDuration(index);
    else
      current_duration_ = 0;
  }
 
  start_time_ = std::chrono::high_resolution_clock::now() -
                std::chrono::duration_cast<std::chrono::high_resolution_clock::duration>(
                    std::chrono::duration<double>(current_duration_));
 
  // Compute the interpolated state
  return trajectory_->getState(current_duration_);
}
 
tesseract_common::JointState TrajectoryPlayer::setCurrentDuration(double duration)
{
  if (!trajectory_ || trajectory_->empty())
    throw std::runtime_error("Trajectory is empty!");
 
  finished_ = false;
  if (duration > trajectory_duration_end_)
  {
    current_duration_ = trajectory_duration_end_;
    finished_ = true;
  }
  else if (duration < trajectory_duration_start_)
    current_duration_ = trajectory_duration_start_;
  else
    current_duration_ = duration;
 
  start_time_ = std::chrono::high_resolution_clock::now() -
                std::chrono::duration_cast<std::chrono::high_resolution_clock::duration>(
                    std::chrono::duration<double>(current_duration_));
 
  // Compute the interpolated state
  return trajectory_->getState(current_duration_);
}
 
tesseract_common::JointState TrajectoryPlayer::getNext()
{
  if (!trajectory_ || trajectory_->empty())
    throw std::runtime_error("Trajectory is empty!");
 
  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_end_)
  {
    current_duration_ = trajectory_duration_end_;
 
    // Compute the interpolated state
    auto mi = trajectory_->getState(current_duration_);
 
    // Reset the player
    if (loop_)
      reset();
    else
      finished_ = true;
 
    return mi;
  }
 
  return trajectory_->getState(current_duration_);
}
 
tesseract_common::JointState TrajectoryPlayer::getByIndex(long index) const
{
  return trajectory_->getState(trajectory_->getStateDuration(index));
}
 
double TrajectoryPlayer::currentDuration() const { return current_duration_; }
 
double TrajectoryPlayer::trajectoryDurationBegin() const { return trajectory_duration_start_; }
 
double TrajectoryPlayer::trajectoryDurationEnd() const { return trajectory_duration_end_; }
 
bool TrajectoryPlayer::isFinished() const { return finished_; }
 
void TrajectoryPlayer::enableLoop(bool loop) { loop_ = loop; }
 
bool TrajectoryPlayer::isLoopEnabled() const { return loop_; }
 
void TrajectoryPlayer::reset()
{
  // Reset state associated with trajectory playback
  current_duration_ = trajectory_duration_start_;
 
  // Get the chrono time
  start_time_ = std::chrono::high_resolution_clock::now();
 
  // Reset finished
  finished_ = false;
}
 
long TrajectoryPlayer::size() const { return (trajectory_) ? trajectory_->getStateCount() : 0; }
 
}  // namespace tesseract_visualization