scenepanel.cpp

Go to the documentation of this file.

/* Author: Constantinos Chamzas */

#include <se2ez/core/log.h>
#include <se2ez/core/io.h>
#include <se2ez/core/robot.h>
#include <se2ez/core/state.h>
#include <se2ez/core/scene.h>
#include <se2ez/core/frame.h>
#include <se2ez/core/ik.h>
#include <se2ez/core/box2d/collision.h>
#include <se2ez/core/motionbuilder.h>

#include "se2ez/gui/scenepanel.h"
#include "se2ez/gui/jointpanel.h"
#include <se2ez/gui/renderarea.h>
#include "ui_scenepanel.h"

using namespace se2ez;

gui::ScenePanel::ScenePanel(ScenePtr scene, std::string name, QWidget *parent)
  : Panel(name, parent), ui_(new Ui::ScenePanel), scene_(scene)
{
    ui_->setupUi(this);
    sstate_ = std::make_shared<State>(scene_->getSceneRobot());

    cm_ = std::make_shared<Box2DCollisionManager>(scene_->getSceneRobot());
    cm_->compile();

    // // connecting the signals
    connect(ui_->geometry, SIGNAL(stateChanged(int)), this, SLOT(updateScene()));
    connect(ui_->frames, SIGNAL(stateChanged(int)), this, SLOT(updateScene()));
    connect(ui_->collide, SIGNAL(stateChanged(int)), this, SLOT(updateScene()));
    connect(ui_->generateGoalsButton, SIGNAL(pressed()), this, SLOT(generateGoals()));
}

void gui::ScenePanel::update(boost::posix_time::ptime /*last*/, boost::posix_time::ptime /*current*/)
{
    bool update = false;
    for (const auto &panel : *panels)
    {
        if (panelnames_.emplace(panel.first).second)  // If new panel was added
        {
            auto jpanel = dynamic_cast<gui::JointPanel *>(panel.second);
            if (jpanel)
            {
                // when a passive jpanel updates the scene should get updated too
                connect(jpanel, SIGNAL(stateChanged()), this, SLOT(updateScene()));
                // If the joint panel controls the active robot or an environment
                bool isActive = (jpanel->getSignature().compare(scene_->getSignature()) == 0);
                isActive ? anames_.emplace(panel.first) : pnames_.emplace(panel.first);
                update = true;
            }
        }
    }

    if (update)
    {
        ui_->robotComboBox->clear();
        // Adding the active robots
        for (const auto &name : anames_)
            ui_->robotComboBox->addItem(tr(name.c_str()));
        // Adding the passive robots
        for (const auto &name : pnames_)
        {
            auto box = new QCheckBox(tr(name.c_str()));
            box->setChecked(true);
            connect(box, SIGNAL(clicked()), this, SLOT(updateScene()));
            ui_->passiveBoxes->addWidget(box);
        }
    }
}

void gui::ScenePanel::updateScene()
{
    std::lock_guard<std::mutex> lock(mutex_);

    scene_->clear();
    // adding the passive robots
    for (int i = 0; i < ui_->passiveBoxes->count(); i++)
    {
        auto box = static_cast<QCheckBox *>(ui_->passiveBoxes->itemAt(i)->widget());
        auto name = box->text();
        if (box->isChecked())
            scene_->addRobot(getRobot(name), name.toStdString(), getState(name));
    }
    scene_->compileScene();

    cm_->compile();
    if (ui_->collide->isChecked())
    {
        cm_->collide(sstate_, colliding_);
    }

    // setting the active robots state
    sstate_->copy(getState(ui_->robotComboBox->currentText()));

    emit stateChanged();
}

void gui::ScenePanel::generateGoals()
{
    auto ee = getEE(ui_->robotComboBox->currentText());
    auto prefix = ui_->lineEditPrefix->text().toStdString();
    auto builder = std::make_shared<MotionBuilder>(scene_->getSceneRobot(), ee, prefix);

    auto goals = builder->genGoals(MotionBuilder::FRAME);
    scene_->getSceneRobot()->setNamedStates(goals);
}

void gui::ScenePanel::draw(QPainter &painter, gui::RenderArea *canvas)
{
    std::lock_guard<std::mutex> lock(mutex_);

    auto srobot = scene_->getSceneRobot();

    if (ui_->geometry->isChecked())
    {
        canvas->drawGeometry(painter, srobot, sstate_,
                             std::make_shared<RenderArea::CollideDrawer>(ui_->collide->isChecked(),
                                                                         colliding_, srobot, sstate_));
    }

    if (ui_->frames->isChecked())
        canvas->drawAllFrames(painter, srobot, sstate_);
}

const StatePtr gui::ScenePanel::getState(const QString &s)
{
    auto state = std::make_shared<State>(scene_->getSceneRobot());
    const std::string &name = s.toStdString();

    if (pnames_.find(name) != anames_.end())
    {
        auto entry = panels->find(name);
        auto jp = dynamic_cast<gui::JointPanel *>(entry->second);
        return jp->getState();
    }

    SE2EZ_WARN("JointPanel %s does not exist, returning empty state", name);
    return state;
}

const std::string gui::ScenePanel::getEE(const QString &s)
{
    const std::string &name = s.toStdString();

    if (anames_.find(name) != anames_.end())
    {
        auto entry = panels->find(name);
        auto jp = dynamic_cast<JointPanel *>(entry->second);
        return jp->getEE();
    }

    std::string ee = scene_->getActiveRobot()->getEEs()[0];
    SE2EZ_WARN("JointPanel %s does not exist, returning the first EE %s from the robot", name, ee);
    return ee;
}

const RobotPtr gui::ScenePanel::getRobot(const QString &s)
{
    const std::string &name = s.toStdString();

    if (pnames_.find(name) != pnames_.end())
    {
        auto entry = panels->find(name);
        auto jp = dynamic_cast<JointPanel *>(entry->second);
        return jp->getRobot();
    }

    SE2EZ_WARN("JointPanel %s does not exist, returning empty Robot", name);
    return std::make_shared<Robot>();
}

gui::ScenePanel::~ScenePanel()
{
    delete ui_;
}