GraspCandidateViewerWidgetController.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * \package    RobotAPI::gui-plugins::GraspCandidateViewerWidgetController
 * \author     Raphael Grimm ( raphael dot grimm at kit dot edu )
 * \date       2020
 * \copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "GraspCandidateViewerWidgetController.h"
 
#include <string>
 
#include <SimoxUtility/math/convert/quat_to_rpy.h>
 
#include <RobotAPI/libraries/GraspingUtility/grasp_candidate_drawer.h>
 
//setting management
namespace armarx
{
    void
    GraspCandidateViewerWidgetController::loadSettings(QSettings* settings)
    {
        ARMARX_TRACE;
        std::lock_guard g{_mutex};
        getRobotStateComponentPlugin().setRobotStateComponentName(
            settings->value("rsc", "Armar6StateComponent").toString().toStdString());
        getGraspCandidateObserverComponentPlugin().setGraspCandidateObserverName(
            settings->value("gco", "GraspCandidateObserver").toString().toStdString());
    }
 
    void
    GraspCandidateViewerWidgetController::saveSettings(QSettings* settings)
    {
        ARMARX_TRACE;
        std::lock_guard g{_mutex};
        settings->setValue(
            "rsc",
            QString::fromStdString(getRobotStateComponentPlugin().getRobotStateComponentName()));
        settings->setValue(
            "rsc",
            QString::fromStdString(
                getGraspCandidateObserverComponentPlugin().getGraspCandidateObserverName()));
    }
 
    QPointer<QDialog>
    GraspCandidateViewerWidgetController::getConfigDialog(QWidget* parent)
    {
        ARMARX_TRACE;
        std::lock_guard g{_mutex};
        if (!_dialog)
        {
            _dialog = new SimpleConfigDialog(parent);
            _dialog->addProxyFinder<RobotStateComponentInterfacePrx>(
                "rsc", "Robot State Component", "*Component");
            _dialog->addProxyFinder<grasping::GraspCandidateObserverInterfacePrx>(
                "gco", "Grasp Candidate Observer", "*Observer");
        }
        return qobject_cast<SimpleConfigDialog*>(_dialog);
    }
 
    void
    GraspCandidateViewerWidgetController::configured()
    {
        ARMARX_TRACE;
        std::lock_guard g{_mutex};
        getRobotStateComponentPlugin().setRobotStateComponentName(_dialog->getProxyName("rsc"));
        getGraspCandidateObserverComponentPlugin().setGraspCandidateObserverName(
            _dialog->getProxyName("gco"));
    }
} // namespace armarx
 
//ctor
namespace armarx
{
    GraspCandidateViewerWidgetController::GraspCandidateViewerWidgetController()
    {
        ARMARX_TRACE;
        std::lock_guard g{_mutex};
        _ui.setupUi(getWidget());
 
        connect(_ui.pushButtonUpdateGC,
                &QPushButton::clicked,
                this,
                &GraspCandidateViewerWidgetController::update_gc);
        connect(_ui.treeWidgetGC,
                &QTreeWidget::itemSelectionChanged,
                this,
                &GraspCandidateViewerWidgetController::selected_item_changed);
 
        connect(_ui.treeWidgetGC,
                SIGNAL(itemChanged(QTreeWidgetItem*, int)),
                this,
                SLOT(check_state_changed()));
    }
 
    GraspCandidateViewerWidgetController::~GraspCandidateViewerWidgetController()
    {
    }
 
    void
    GraspCandidateViewerWidgetController::update_gc()
    {
        ARMARX_TRACE;
        std::lock_guard g{_mutex};
        if (!getGraspCandidateObserver())
        {
            return;
        }
        //clean
        {
            _ui.treeWidgetGC->clear();
            for (auto& [prname, pr] : _providers)
            {
                pr.candidates.clear();
                pr.item = nullptr;
            }
            _tree_item_to_gc.clear();
        }
        //fill
        {
            for (const auto& gc : getGraspCandidateObserver()->getAllCandidates())
            {
                ARMARX_CHECK_NOT_NULL(gc);
                const auto& pname = gc->providerName;
                auto& pr = _providers[pname];
                pr.provider_name = pname;
                if (!pr.item)
                {
                    pr.item = new QTreeWidgetItem;
                    pr.item->setCheckState(0, Qt::Unchecked);
                    _ui.treeWidgetGC->addTopLevelItem(pr.item);
                    pr.item->setText(0, QString::fromStdString(pname));
                }
 
                auto& gcdata = pr.candidates.emplace_back();
                gcdata.gc = gc;
                gcdata.idx = pr.candidates.size() - 1;
                gcdata.provider = &pr;
                gcdata.item = new QTreeWidgetItem;
                gcdata.item->setCheckState(0, Qt::Unchecked);
                pr.item->addChild(gcdata.item);
                gcdata.item->setText(0, QString::fromStdString(gcdata.name()));
                _tree_item_to_gc[gcdata.item] = &gcdata;
            }
        }
    }
 
    void
    GraspCandidateViewerWidgetController::check_state_changed()
    {
        ARMARX_TRACE;
        std::lock_guard g{_mutex};
        synchronizeLocalClone(_robot);
        armarx::grasp_candidate_drawer gc_drawer{getRobotNameHelper(), _robot};
 
        std::vector<viz::Layer> layers;
        for (const auto& [pname, pr] : _providers)
        {
            layers.emplace_back(getArvizClient().layer(pname));
            auto& l = layers.back();
            for (const auto& gc : pr.candidates)
            {
                if (gc.item->checkState(0) == Qt::Unchecked)
                {
                    gc_drawer.draw(gc.gc, l);
                }
            }
        }
        getArvizClient().commit(layers);
    }
 
    void
    GraspCandidateViewerWidgetController::selected_item_changed()
    {
        ARMARX_TRACE;
        std::lock_guard g{_mutex};
        const auto& selected = _ui.treeWidgetGC->selectedItems();
        ARMARX_CHECK_GREATER(2, selected.size());
        if (selected.empty())
        {
            show_entry(nullptr);
            //clear
        }
        else if (_tree_item_to_gc.count(selected.value(0)))
        {
            show_entry(_tree_item_to_gc.at(selected.value(0)));
        }
        //provider item selected -> do nothing
    }
 
    void
    GraspCandidateViewerWidgetController::show_entry(entry_gc* e)
    {
        if (!e)
        {
            _ui.labelGCIdx->setText("-");
            _ui.labelNumGC->setText("-");
 
            _ui.labelGPoseTX->setText("-");
            _ui.labelGPoseTY->setText("-");
            _ui.labelGPoseTZ->setText("-");
            _ui.labelGPoseRX->setText("-");
            _ui.labelGPoseRY->setText("-");
            _ui.labelGPoseRZ->setText("-");
 
            _ui.labelRPoseTX->setText("-");
            _ui.labelRPoseTY->setText("-");
            _ui.labelRPoseTZ->setText("-");
            _ui.labelRPoseRX->setText("-");
            _ui.labelRPoseRY->setText("-");
            _ui.labelRPoseRZ->setText("-");
 
            _ui.labelGApprTX->setText("-");
            _ui.labelGApprTY->setText("-");
            _ui.labelGApprTZ->setText("-");
 
            _ui.labelSrcFrame->setText("-");
            _ui.labelTrgFrame->setText("-");
            _ui.labelSide->setText("-");
            _ui.labelGraspProb->setText("-");
            _ui.labelGraspgroupNr->setText("-");
            _ui.labelGraspObjType->setText("-");
            _ui.labelGraspProviderName->setText("-");
            return;
        }
        ARMARX_CHECK_NOT_NULL(e->gc);
        ARMARX_CHECK_NOT_NULL(e->gc->graspPose->position);
        ARMARX_CHECK_NOT_NULL(e->gc->robotPose->position);
        ARMARX_CHECK_NOT_NULL(e->gc->graspPose->orientation);
        ARMARX_CHECK_NOT_NULL(e->gc->robotPose->orientation);
        ARMARX_CHECK_NOT_NULL(e->gc->approachVector);
        _ui.labelGCIdx->setText(QString::number(e->idx + 1));
        _ui.labelNumGC->setText(QString::number(e->provider->candidates.size()));
 
        _ui.labelGPoseTX->setText(QString::number(e->gc->graspPose->position->x));
        _ui.labelGPoseTY->setText(QString::number(e->gc->graspPose->position->y));
        _ui.labelGPoseTZ->setText(QString::number(e->gc->graspPose->position->z));
        const Eigen::Vector3f vec_g = simox::math::quat_to_rpy({e->gc->graspPose->orientation->qw,
                                                                e->gc->graspPose->orientation->qx,
                                                                e->gc->graspPose->orientation->qy,
                                                                e->gc->graspPose->orientation->qz
 
        });
        _ui.labelGPoseRX->setText(QString::number(vec_g(0)));
        _ui.labelGPoseRY->setText(QString::number(vec_g(1)));
        _ui.labelGPoseRZ->setText(QString::number(vec_g(2)));
 
        _ui.labelRPoseTX->setText(QString::number(e->gc->robotPose->position->x));
        _ui.labelRPoseTY->setText(QString::number(e->gc->robotPose->position->y));
        _ui.labelRPoseTZ->setText(QString::number(e->gc->robotPose->position->z));
        const Eigen::Vector3f vec_r = simox::math::quat_to_rpy({e->gc->robotPose->orientation->qw,
                                                                e->gc->robotPose->orientation->qx,
                                                                e->gc->robotPose->orientation->qy,
                                                                e->gc->robotPose->orientation->qz
 
        });
        _ui.labelRPoseRX->setText(QString::number(vec_r(0)));
        _ui.labelRPoseRY->setText(QString::number(vec_r(1)));
        _ui.labelRPoseRZ->setText(QString::number(vec_r(2)));
 
        _ui.labelGApprTX->setText(QString::number(e->gc->approachVector->x));
        _ui.labelGApprTY->setText(QString::number(e->gc->approachVector->y));
        _ui.labelGApprTZ->setText(QString::number(e->gc->approachVector->z));
 
        _ui.labelSrcFrame->setText(QString::fromStdString(e->gc->sourceFrame));
        _ui.labelTrgFrame->setText(QString::fromStdString(e->gc->targetFrame));
        _ui.labelSide->setText(QString::fromStdString(e->gc->side));
        _ui.labelGraspProb->setText(QString::number(e->gc->graspSuccessProbability));
        _ui.labelGraspgroupNr->setText(QString::number(e->gc->groupNr));
        switch (e->gc->objectType)
        {
            case objpose::ObjectType::AnyObject:
                _ui.labelGraspObjType->setText("AnyObject");
                break;
            case objpose::ObjectType::KnownObject:
                _ui.labelGraspObjType->setText("KnownObject");
                break;
            case objpose::ObjectType::UnknownObject:
                _ui.labelGraspObjType->setText("UnknownObject");
                break;
        }
        _ui.labelGraspProviderName->setText(QString::fromStdString(e->gc->providerName));
    }
 
} // namespace armarx