DesignerTrajectoryPlayer.cpp

Go to the documentation of this file.

 
#include "DesignerTrajectoryPlayer.h"
 
#include <QTimer>
 
 
using namespace armarx;
using namespace VirtualRobot;
 
DesignerTrajectoryPlayer::DesignerTrajectoryPlayer(RobotVisualizationPtr viewer,
                                                   VirtualRobot::RobotPtr robot)
{
    this->viewer = viewer;
    this->robot = robot;
    this->currentTime = 0;
    this->fps = 60;
    this->trajectories = std::vector<DesignerTrajectoryPtr>();
    this->timeOptimalTrajectories = std::vector<TrajectoryPtr>();
}
 
void
DesignerTrajectoryPlayer::updateLoop()
{
    bool endReached = true;
    int i = 0;
    //ARMARX_INFO << "AT TIME: " + std::to_string(currentTime);
    for (TrajectoryPtr currentTrajectory : this->timeOptimalTrajectories)
    {
        if (!currentTrajectory)
        {
            continue;
        }
        if (currentTime < currentTrajectory->getTimeLength() * 1000)
        {
            Ice::DoubleSeq ice_JA = currentTrajectory->getStates(currentTime / 1000, 0);
            //ARMARX_INFO << ice_JA;
            std::vector<float> jA = std::vector<float>(ice_JA.begin(), ice_JA.end());
            robot->getRobotNodeSet(trajectories[i]->getRns()->getName())->setJointValues(jA);
            endReached = false;
        }
        i++;
    }
    viewer->updateRobotVisualization();
    currentTime = currentTime + (1000.0 / fps);
    if (endReached)
    {
        emit finishedPlayback();
    }
}
 
void
DesignerTrajectoryPlayer::addTrajectory(armarx::DesignerTrajectoryPtr trajectory)
{
    this->trajectories.push_back(trajectory);
}
 
void
DesignerTrajectoryPlayer::startPlayback()
{
    //Setup timer
    timer = std::shared_ptr<QTimer>(new QTimer(this));
    connect(timer.get(), SIGNAL(timeout()), this, SLOT(updateLoop()));
    connect(this, SIGNAL(finishedPlayback()), timer.get(), SLOT(stop()));
 
    //get all finalTrajectories
    for (DesignerTrajectoryPtr current : trajectories)
    {
        if (current && current->getNrOfUserWaypoints() != 1)
        {
            TrajectoryPtr traj = current->getFinalTrajectory();
            timeOptimalTrajectories.push_back(traj);
        }
        else
        {
            timeOptimalTrajectories.push_back(TrajectoryPtr(new Trajectory()));
        }
    }
 
    //start Timer
    timer->start(1000.0 / fps);
}
 
void
DesignerTrajectoryPlayer::setFPS(int fps)
{
    this->fps = fps;
}