planpanel.cpp

Go to the documentation of this file.

/* Author: Constantinos Chamzas, Zachary Kingston */

#include <QString>
#include <QLineEdit>

#include <se2ez/core/robot.h>
#include <se2ez/core/frame.h>
#include <se2ez/core/state.h>
#include <se2ez/core/log.h>
#include <se2ez/core/io.h>

#include <se2ez/plan/helper.h>

#include <se2ez/gui/planpanel.h>
#include <se2ez/gui/jointpanel.h>
#include <se2ez/gui/scenepanel.h>

#include "ui_planpanel.h"

using namespace se2ez;

gui::PlanPanel::PlanPanel(RobotPtr robot, std::string name,
                          std::map<std::string, ompl::base::ConstraintPtr> constraints, QWidget *parent)
  : Panel(name, parent), ui_(new Ui::PlanPanel), robot_(robot), constraints_(constraints)
{
    {
        std::unique_lock<std::mutex> lock(planmutex_);

        ui_->setupUi(this);

        planner_ = std::make_shared<plan::EZPlans>(robot_);
        planner_->initialize();
        planner_->createPlanners();

        // allocating space for the  states
        pstate_ = std::make_shared<State>(robot_);

        // Populate constraint list
        ui_->constraints->addItem("<none>");
        for (const auto &constraint : constraints)
            ui_->constraints->addItem(QString(constraint.first.c_str()));

        // Populate planner list
        for (const auto &name : planner_->getPlanners())
            ui_->planners->addItem(QString(name.c_str()));

        // Populate graph end-effector list
        for (const auto &name : robot_->getFrameNames())
        {
            const auto &data = robot_->getFrameDataConst(name);
            if (data->movable)
                ui_->ees->addItem(QString(name.c_str()));
        }

        // Animation slider bookkeeping
        ui_->pathSlider->setEnabled(false);

        anim_.setTargetObject(ui_->pathSlider);
        anim_.setPropertyName("value");

        // Setup all signals
        connect(ui_->plan, SIGNAL(clicked()), this, SLOT(plan()));
        connect(ui_->pathSlider, SIGNAL(valueChanged(int)), this, SLOT(setState(int)));
        connect(ui_->animateBox, SIGNAL(stateChanged(int)), this, SLOT(animate(int)));
        connect(ui_->animTime, SIGNAL(valueChanged(double)), this, SLOT(animateTime()));
        connect(ui_->planners, SIGNAL(currentTextChanged(const QString &)), this,
                SLOT(plannerChanged(const QString &)));
        connect(ui_->swept, SIGNAL(stateChanged(int)), this, SLOT(updateState()));
        connect(ui_->sweptCount, SIGNAL(valueChanged(int)), this, SLOT(updateState()));
        connect(ui_->draw, SIGNAL(toggled(bool)), this, SLOT(updateState()));
        connect(ui_->constraints, SIGNAL(currentTextChanged(const QString &)), this,
                SLOT(setConstraints(const QString &)));

        connect(ui_->ees, SIGNAL(currentTextChanged(const QString &)), this, SLOT(setEE(const QString &)));
        connect(ui_->gorn, SIGNAL(toggled(bool)), this, SLOT(updateState()));
        connect(ui_->gstate, SIGNAL(toggled(bool)), this, SLOT(updateState()));
        connect(ui_->gedge, SIGNAL(toggled(bool)), this, SLOT(updateState()));
    }

    ui_->ees->setCurrentIndex(ui_->ees->count() - 1);
    setEE(ui_->ees->currentText());

    int index = ui_->planners->findText("rrt:RRTConnect", Qt::MatchExactly);
    if (index >= 0)
        ui_->planners->setCurrentIndex(index);
}

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

void gui::PlanPanel::updateState()
{
    emit stateChanged();
}

void gui::PlanPanel::setEE(const QString &text)
{
    {
        std::unique_lock<std::mutex> lock(planmutex_);
        ee_ = text.toStdString();
    }

    updateState();
}

void gui::PlanPanel::setConstraints(const QString &text)
{
    {
        std::unique_lock<std::mutex> lock(planmutex_);

        if (ui_->constraints->currentIndex() != 0)
        {
            const auto &constraint = text.toStdString();
            planner_ = std::make_shared<plan::EZPlansConstraint>(robot_, constraints_[constraint],
                                                                 plan::ConstraintType::PROJECTION);
        }
        else
            planner_ = std::make_shared<plan::EZPlans>(robot_);

        planner_->initialize();
        planner_->createPlanners();
    }

    plannerChanged(ui_->planners->currentText());
    updateState();
}

void gui::PlanPanel::plannerChanged(const QString &text)
{
    std::unique_lock<std::mutex> lock(planmutex_);

    while (ui_->parameters->count())
    {
        QLayoutItem *child = ui_->parameters->takeAt(0);
        delete child->widget();
        delete child;
    }

    set_.clear();

    const auto &planner = text.toStdString();
    planner_->setPlanner(planner);

    // HACK: PRM's parameters do not play nice with this interface
    if (planner == "prm:PRM")
        return;

    set_ = planner_->setup->getPlanner()->params();
    for (const auto &entry : set_.getParams())
    {
        const auto &name = entry.first;
        std::string formatted_name = name;
        std::replace(formatted_name.begin(), formatted_name.end(), '_', ' ');
        if (formatted_name.size() > 28)
        {
            formatted_name.resize(25);
            formatted_name += "...";
        }

        auto &param = entry.second;
        const auto &range = param->getRangeSuggestion();

        // Check if range is for a boolean value
        auto maybeBool = io::tokenize(range, ",");
        if (maybeBool.size() == 2)
        {
            auto box = new QCheckBox();
            connect(box, &QCheckBox::stateChanged, [&, name](int state) {
                std::string value = (state == Qt::Unchecked) ? "0" : "1";
                set_.setParam(name, value);
            });

            box->setCheckState(boost::lexical_cast<bool>(param->getValue()) ? Qt::Checked : Qt::Unchecked);
            ui_->parameters->addRow(QString(formatted_name.c_str()), box);
        }
        else
        {
            // Try to guess type based upon value or range
            const auto &vt = param->getValue();
            bool isDouble =
                range.find_first_of(".") != std::string::npos || vt.find_first_of(".") != std::string::npos;

            // Tokenize range to extract
            auto numbers = io::tokenize(range, ":");

            // If we guess a double type, create a double spin box
            if (isDouble)
            {
                double min = 0, max = 1, step = 0.1;
                if (numbers.size() == 3)
                {
                    min = boost::lexical_cast<double>(numbers[0]);
                    step = boost::lexical_cast<double>(numbers[1]);
                    max = boost::lexical_cast<double>(numbers[2]);
                }
                else if (numbers.size() == 2)
                {
                    min = boost::lexical_cast<double>(numbers[0]);
                    max = boost::lexical_cast<double>(numbers[1]);
                }

                auto box = new QDoubleSpinBox();
                box->setMaximum(max);
                box->setMinimum(min);
                box->setSingleStep(step);

                connect(box, static_cast<void (QDoubleSpinBox::*)(double)>(&QDoubleSpinBox::valueChanged),
                        [&, name](double value) {
                            try
                            {
                                set_.setParam(name, std::to_string(value));
                            }
                            catch (std::exception &e)
                            {
                                SE2EZ_ERROR(e.what());
                            }
                        });

                box->setValue(boost::lexical_cast<double>(param->getValue()));
                ui_->parameters->addRow(QString(formatted_name.c_str()), box);
            }
            // If a range is specified but there is no `.`, we are integral
            else if (not numbers.empty())
            {
                int min = 0, max = 100, step = 1;
                if (numbers.size() == 3)
                {
                    min = boost::lexical_cast<int>(numbers[0]);
                    step = boost::lexical_cast<int>(numbers[1]);
                    max = boost::lexical_cast<int>(numbers[2]);
                }
                else if (numbers.size() == 2)
                {
                    min = boost::lexical_cast<int>(numbers[0]);
                    max = boost::lexical_cast<int>(numbers[1]);
                }

                auto box = new QSpinBox();
                box->setMaximum(max);
                box->setMinimum(min);
                box->setSingleStep(step);

                connect(box, static_cast<void (QSpinBox::*)(int)>(&QSpinBox::valueChanged),
                        [&, name](int value) {
                            try
                            {
                                set_.setParam(name, std::to_string(value));
                            }
                            catch (std::exception &e)
                            {
                                SE2EZ_ERROR(e.what());
                            }
                        });

                box->setValue(boost::lexical_cast<int>(param->getValue()));
                ui_->parameters->addRow(QString(formatted_name.c_str()), box);
            }
            // If nothing else, just add a text box
            else
            {
                auto line = new QLineEdit(QString(param->getValue().c_str()));
                ui_->parameters->addWidget(line);
                connect(line, &QLineEdit::textChanged, [&, name](const QString &newValue) {
                    const auto value = newValue.toStdString();
                    set_.setParam(name, value);
                });

                ui_->parameters->addRow(QString(formatted_name.c_str()), line);
            }
        }

        auto label =
            ui_->parameters->itemAt(ui_->parameters->rowCount() - 1, QFormLayout::LabelRole)->widget();
        auto widget =
            ui_->parameters->itemAt(ui_->parameters->rowCount() - 1, QFormLayout::FieldRole)->widget();
        label->setToolTip(QString(name.c_str()));
        widget->setToolTip(QString(name.c_str()));
    }
}

void gui::PlanPanel::animate(int state)
{
    if (state == Qt::Checked)
    {
        if (path_.size())
        {
            startAnimation();
            anim_.start();
        }
    }
    else
        pauseAnimation();
}

void gui::PlanPanel::animateTime()
{
    if (ui_->animateBox->isChecked())
    {
        pauseAnimation();
        startAnimation();
        anim_.start();
    }
}

void gui::PlanPanel::pauseAnimation()
{
    anim_.stop();
}

void gui::PlanPanel::startAnimation()
{
    anim_.setDuration(ui_->animTime->value() * 1000);
    anim_.setLoopCount(-1);

    anim_.setStartValue(ui_->pathSlider->minimum());
    // anim_.setKeyValueAt(0.5, ui_->pathSlider->maximum());
    anim_.setEndValue(ui_->pathSlider->maximum() + 1);
}

void gui::PlanPanel::setState(int index)
{
    pstate_ = path_[index];
    updateState();
}

void gui::PlanPanel::setPath(const std::vector<StatePtr> path)
{
    std::unique_lock<std::mutex> lock(pathmutex_);
    path_ = path;

    if (path_.size())
    {
        double l = 0;
        for (unsigned int i = 0; i < path_.size() - 1; ++i)
            l += robot_->distance(path_[i], path_[i + 1]);
        ui_->length->setText(QString(log::format("%1$0.3f", l).c_str()));
        ui_->states->setText(QString(log::format("%1%", path_.size()).c_str()));

        ui_->pathSlider->setMaximum(path_.size() - 1);
        ui_->pathSlider->setEnabled(true);

        setState(ui_->pathSlider->value());

        animateTime();
    }

    emit newPlan();
    updateState();
}

const std::vector<StatePtr> &gui::PlanPanel::getPath() const
{
    return path_;
}

ompl::base::PlannerDataPtr gui::PlanPanel::getPlannerData()
{
    std::unique_lock<std::mutex> lock(planmutex_);

    if (ui_->constraints->currentIndex() == 0)
    {
        auto pd = std::make_shared<ompl::base::PlannerData>(planner_->info);
        planner_->setup->getPlannerData(*pd);
        pd->decoupleFromPlanner();

        return pd;
    }

    return nullptr;
}

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

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

    if (snames_.find(name) != snames_.end())
        robot_->getNamedState(name, state);

    return state;
}

void gui::PlanPanel::plan()
{
    if (planning_)
    {
        planning_->join();
        delete planning_;
    }

    {
        std::unique_lock<std::mutex> lock(planmutex_);
        robot_->compileTree();
        planner_->updateValidityChecker();

        ui_->setbox->setEnabled(false);
        ui_->planbox->setEnabled(false);
        ui_->scrollArea->setEnabled(false);
    }

    planning_ = new std::thread([&]() {
        std::unique_lock<std::mutex> lock(planmutex_);

        // Clear previous paths
        if (ui_->clear->isChecked())
            planner_->setup->clear();
        else
        {
            const auto &cptr = std::static_pointer_cast<ompl::base::ProblemDefinition>(
                planner_->setup->getProblemDefinition());
            auto *pdef = const_cast<ompl::base::ProblemDefinition *>(cptr.get());

            pdef->clearGoal();
            pdef->clearSolutionPaths();

            planner_->setup->getPlanner()->clearQuery();
        }

        planner_->info->setStateValidityCheckingResolution(ui_->valid->value());
        planner_->setStart(getState(ui_->startComboBox->currentText()));
        planner_->setGoal(getState(ui_->goalComboBox->currentText()));

        planner_->setup->getPlanner()->params().include(set_);

        ompl::base::PlannerStatus solved = planner_->setup->solve(ui_->time->value());
        const bool display =
            ui_->approx->isChecked() ? solved : solved == ompl::base::PlannerStatus::EXACT_SOLUTION;

        if (display)
        {
            if (ui_->simplify->isChecked())
                planner_->setup->simplifySolution();

            if (ui_->interpolate->isChecked())
            {
                auto &path = planner_->setup->getSolutionPath();
                path.interpolate();
            }

            ui_->tts->setText(
                QString(log::format("%1$0.3f s", planner_->setup->getLastPlanComputationTime()).c_str()));
            setPath(planner_->extractPath());
        }
        else
        {
            SE2EZ_WARN("No solution found");
            setPath({});
        }

        lock.unlock();

        {
            std::unique_lock<std::mutex> plock(pdmutex_);
            pd_ = getPlannerData();

            if (pd_)
            {
                ui_->gstatel->setText(QString(log::format("%1%", pd_->numVertices()).c_str()));
                ui_->gedgel->setText(QString(log::format("%1%", pd_->numEdges()).c_str()));
            }
            else
            {
                ui_->gstatel->setText("0");
                ui_->gedgel->setText("0");
            }
        }

        ui_->setbox->setEnabled(true);
        ui_->planbox->setEnabled(true);
        ui_->scrollArea->setEnabled(true);

        updateState();
    });
}

void gui::PlanPanel::draw(QPainter &painter, RenderArea *canvas)
{
    // Draw path
    bool path = ui_->draw->isChecked();
    if (path && path_.size())
    {
        std::unique_lock<std::mutex> lock(pathmutex_);
        bool swept = ui_->swept->isChecked();
        unsigned int count = ui_->sweptCount->value();

        if (swept)
        {
            for (unsigned int i = 0; i < path_.size() - 1; i += count)
                canvas->drawGeometry(painter, robot_, path_[i]);

            canvas->drawGeometry(painter, robot_, path_.back());
        }

        else
        {
            pstate_->fk(robot_);
            canvas->drawGeometry(painter, robot_, pstate_);
        }
    }

    bool edges = ui_->gedge->isChecked();
    bool states = ui_->gstate->isChecked();
    bool orn = ui_->gorn->isChecked();
    if (pd_ && (edges || states || orn))
    {
        std::unique_lock<std::mutex> plock(pdmutex_);

        painter.save();
        painter.resetTransform();

        for (unsigned int i = 0; i < pd_->numVertices(); ++i)
        {
            const auto &vert = pd_->getVertex(i);
            const auto &state = planner_->getStateConst(vert.getState());

            QColor color(0, 255, 164);
            if (vert.getTag() == 2)
                color = QColor(0, 164, 255);

            QPen pen(color);
            painter.setPen(pen);

            state->fk(robot_);
            auto iso = state->getPose(robot_, ee_);
            auto pose = iso.translation();

            int x, y;
            canvas->toScreen(pose[0], pose[1], x, y);

            if (states)
                painter.fillRect(x - 3, y - 3, 6, 6, color);

            if (orn)
            {
                Eigen::Vector2d rot{0.15, 0};
                rot = iso * rot;

                int xa, ya;
                canvas->toScreen(rot[0], rot[1], xa, ya);

                painter.drawLine(x, y, xa, ya);
            }

            if (edges)
            {
                std::vector<unsigned int> edge;
                unsigned int m = pd_->getEdges(i, edge);

                for (unsigned int j = 0; j < m; ++j)
                {
                    const auto &next = planner_->getStateConst(pd_->getVertex(edge[j]).getState());

                    next->fk(robot_);
                    auto npose = next->getPose(robot_, ee_).translation();

                    int xa, ya;
                    canvas->toScreen(npose[0], npose[1], xa, ya);

                    painter.drawLine(x, y, xa, ya);
                }
            }
        }

        painter.restore();
    }

    // Draw constraints
    if (ui_->constraints->currentIndex() != 0)
    {
        const auto &text = ui_->constraints->currentText();
        auto constraint = constraints_[text.toStdString()];
        auto line = std::dynamic_pointer_cast<plan::LineConstraint>(constraint);

        painter.save();
        painter.resetTransform();
        QPen pen(QColor(255, 164, 0));
        pen.setWidth(3);
        painter.setPen(pen);

        if (line)
        {
            Eigen::Vector2d a, b;
            line->getEndpoints(a, b, pstate_);

            int ax, ay, bx, by;
            canvas->toScreen(a[0], a[1], ax, ay);
            canvas->toScreen(b[0], b[1], bx, by);

            painter.drawLine(ax, ay, bx, by);
            painter.drawText(bx + 3, by, text);
        }

        auto point = std::dynamic_pointer_cast<plan::PointConstraint>(constraint);
        if (point)
        {
            Eigen::Vector2d a;
            point->getPoint(a, pstate_);

            int ax, ay;
            canvas->toScreen(a[0], a[1], ax, ay);
            painter.drawEllipse(ax, ay, 5, 5);
            painter.drawText(ax + 7, ay, text);
        }

        auto circle = std::dynamic_pointer_cast<plan::CircleConstraint>(constraint);
        if (circle)
        {
            painter.save();

            double v;
            Eigen::Vector2d p, r;
            circle->getCircle(p, r, v, pstate_);

            int px, py;
            canvas->toScreen(p[0], p[1], px, py);
            int rx, ry, ox, oy;
            canvas->toScreen(r[0], r[1], rx, ry);
            canvas->toScreen(0, 0, ox, oy);

            rx -= ox;
            ry -= oy;

            painter.translate(px, py);
            painter.rotate(v);

            painter.drawEllipse(-rx, -ry, 2*rx, 2*ry);
            painter.restore();

            painter.drawText(px + 7, py, text);
        }

        painter.restore();
    }
}

void gui::PlanPanel::update(boost::posix_time::ptime /*last*/, boost::posix_time::ptime /*current*/)
{
    bool update = false;

    // Storing the named states
    for (const auto &name : robot_->getNamedStates())
    {
        auto result = snames_.find(name);
        if (result == snames_.end())
        {
            snames_.emplace(name);
            update = true;
        }
    }
    // Storing the states from joint panels
    for (const auto &panel : *panels)
    {
        if (panelnames_.emplace(panel.first).second)  // If new panel was added
        {
            auto jpanel = dynamic_cast<gui::JointPanel *>(panel.second);  // check if it is jpanel
            if (jpanel)
            {
                // If the joint panel controls the robot
                if (jpanel->getSignature().compare(robot_->getSignature()) == 0)
                {
                    jnames_.emplace(panel.first);
                    update = true;
                }
            }
        }
    }

    // update
    if (update)
    {
        ui_->startComboBox->clear();
        ui_->goalComboBox->clear();

        // Adding the named states
        for (const auto &name : snames_)
        {
            ui_->startComboBox->addItem(QString(name.c_str()));
            ui_->goalComboBox->addItem(QString(name.c_str()));
        }

        int count = ui_->startComboBox->count();
        ui_->startComboBox->insertSeparator(count);
        ui_->goalComboBox->insertSeparator(count);

        // Adding the states from jointPanels
        for (const auto &name : jnames_)
        {
            ui_->startComboBox->addItem(QString(name.c_str()));
            ui_->goalComboBox->addItem(QString(name.c_str()));
        }

        count = ui_->startComboBox->count();
        if (count)
        {
            ui_->startComboBox->setCurrentIndex(count - 2);
            ui_->goalComboBox->setCurrentIndex(count - 1);
        }
    }
}