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 <string>
 
#include <SimoxUtility/math/convert/quat_to_rpy.h>
 
#include <RobotAPI/libraries/GraspingUtility/grasp_candidate_drawer.h>
#include "GraspCandidateViewerWidgetController.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"));
    }
}
//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));
    }
 
}