NJointControllersWidget.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::RobotUnitPlugin
 * \author     Raphael Grimm ( raphael dot grimm at kit dot edu )
 * \date       2017
 * \copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
#include "NJointControllersWidget.h"
 
#include <ArmarXCore/core/logging/Logging.h>
 
#include "StyleSheets.h"
 
namespace armarx
{
    NJointControllersWidget::NJointControllersWidget(QWidget* parent) :
        RobotUnitWidgetTemplateBase("NJointControllersWidget", parent)
    {
        ui->treeWidget->setColumnCount(11);
        QTreeWidgetItem* head = ui->treeWidget->headerItem();
 
        head->setText(idxName, "Name");
        head->setText(idxClass, "Class");
        head->setText(idxActive, "Act");
        head->setText(idxRequested, "Req");
        head->setText(idxError, "Err");
        head->setText(idxInternal, "Int");
        head->setText(idxCtrlDev, "Devices");
        head->setText(idxCtrlMode, "Modes");
        head->setText(idxActivate, "");
        head->setText(idxDeactivate, "");
        head->setText(idxDelete, "");
 
        head->setToolTip(idxName,
                         "The controllers instance name (green: ok, red: active and requested "
                         "state missmatch or there is an error)");
        head->setToolTip(idxClass, "The controllers class name");
        head->setToolTip(idxActive,
                         "Whether the controller is active (green: ok, red: active and requested "
                         "state missmatch)");
        head->setToolTip(idxRequested,
                         "Whether the controller is requested (green: ok, red: active and "
                         "requested state missmatch)");
        head->setToolTip(idxError,
                         "Whether the controller is deactivated due to an error (green: ok, red: "
                         "there is an error)");
        head->setToolTip(idxInternal,
                         "Whether the controller is internal (e.g. used by the kinematic unit)");
        head->setToolTip(idxCtrlDev, "The control devices used by this controller");
        head->setToolTip(idxCtrlMode,
                         "The controll mode for the control devices used by this controller");
        head->setToolTip(idxActivate, "Button to activate the Controller");
        head->setToolTip(idxDeactivate, "Button to deactivate the Controller");
        head->setToolTip(idxDelete, "Button to delete the Controller");
 
        ui->treeWidget->setColumnWidth(idxName, 400);
        ui->treeWidget->setColumnWidth(idxClass, 200);
        ui->treeWidget->setColumnWidth(idxCtrlDev, 130);
        ui->treeWidget->setColumnWidth(idxCtrlMode, 150);
        ui->treeWidget->setColumnWidth(idxActivate, 40);
        ui->treeWidget->setColumnWidth(idxDeactivate, 40);
        ui->treeWidget->setColumnWidth(idxDelete, 40);
        ui->treeWidget->setColumnWidth(idxRequested, 40);
        ui->treeWidget->setColumnWidth(idxActive, 40);
        ui->treeWidget->setColumnWidth(idxError, 40);
        ui->treeWidget->setColumnWidth(idxInternal, 40);
 
        ui->treeWidget->header()->setResizeMode(idxActivate, QHeaderView::Fixed);
        ui->treeWidget->header()->setResizeMode(idxDeactivate, QHeaderView::Fixed);
        ui->treeWidget->header()->setResizeMode(idxDelete, QHeaderView::Fixed);
        ui->treeWidget->header()->setResizeMode(idxRequested, QHeaderView::Fixed);
        ui->treeWidget->header()->setResizeMode(idxActive, QHeaderView::Fixed);
        ui->treeWidget->header()->setResizeMode(idxError, QHeaderView::Fixed);
        ui->treeWidget->header()->setResizeMode(idxInternal, QHeaderView::Fixed);
 
        ui->pushButtonStopAll->setIcon(QIcon{QString{":/icons/media-playback-pause.ico"}});
        ui->pushButtonRemoveAll->setIcon(QIcon{QString{":/icons/Trash.svg"}});
        connect(
            ui->pushButtonStopAll, SIGNAL(clicked()), this, SLOT(onPushButtonStopAll_clicked()));
        connect(ui->pushButtonRemoveAll,
                SIGNAL(clicked()),
                this,
                SLOT(onPushButtonRemoveAll_clicked()));
    }
 
    NJointControllersWidget::~NJointControllersWidget()
    {
        delete ui;
    }
 
    void
    NJointControllersWidget::nJointControllerStatusChanged(
        const NJointControllerStatusSeq& allStatus)
    {
        std::unique_lock<std::recursive_timed_mutex> guard{mutex, std::defer_lock};
        if (!guard.try_lock_for(std::chrono::microseconds(100)))
        {
            return;
        }
        for (const auto& status : allStatus)
        {
            if (statusUpdates[status.instanceName].timestampUSec < status.timestampUSec)
            {
                statusUpdates[status.instanceName] = status;
                if (doMetaCall)
                {
                    QMetaObject::invokeMethod(this, "updateContent", Qt::QueuedConnection);
                }
            }
        }
    }
 
    void
    NJointControllersWidget::add(const NJointControllerDescriptionWithStatus& ds)
    {
        if (!entries.count(ds.description.instanceName))
        {
            entries[ds.description.instanceName] =
                new NJointControllersWidgetEntry(*this, *(ui->treeWidget), ds.description);
        }
        entries.at(ds.description.instanceName)->update(ds.status);
    }
 
    void
    NJointControllersWidget::onPushButtonStopAll_clicked()
    {
        if (robotUnit)
        {
            robotUnit->switchNJointControllerSetup({});
        }
    }
 
    void
    NJointControllersWidget::onPushButtonRemoveAll_clicked()
    {
        for (auto& pair : entries)
        {
            try
            {
                if (pair.second->deletable)
                {
                    pair.second->deleteController();
                }
            }
            catch (...)
            {
            }
        }
    }
 
    void
    NJointControllersWidget::nJointControllerCreated(std::string name)
    {
        RobotUnitInterfacePrx ru;
        std::unique_lock<std::recursive_timed_mutex> guard{mutex};
        if (isResetting || !robotUnit || entries.count(name))
        {
            return;
        }
        ru = robotUnit;
        guard.unlock();
        auto data = ru->getNJointControllerDescriptionWithStatus(name);
        guard.lock();
        if (!entries.count(name))
        {
            controllersCreated[name] = std::move(data);
            if (doMetaCall)
            {
                QMetaObject::invokeMethod(this, "updateContent", Qt::QueuedConnection);
            }
        }
    }
 
    void
    NJointControllersWidget::nJointControllerDeleted(std::string name)
    {
        std::unique_lock<std::recursive_timed_mutex> guard{mutex};
        controllersDeleted.emplace(name);
        if (doMetaCall)
        {
            QMetaObject::invokeMethod(this, "updateContent", Qt::QueuedConnection);
        }
    }
 
    void
    NJointControllersWidget::loadSettings(QSettings* settings)
    {
        ui->treeWidget->setColumnWidth(idxName, settings->value("ctrlColWName", 400).toInt());
        ui->treeWidget->setColumnWidth(idxClass, settings->value("ctrlColWClass", 200).toInt());
        ui->treeWidget->setColumnWidth(idxCtrlDev, settings->value("ctrlColWCDen", 130).toInt());
        ui->treeWidget->setColumnWidth(idxCtrlMode, settings->value("ctrlColWCMod", 150).toInt());
    }
 
    void
    NJointControllersWidget::saveSettings(QSettings* settings)
    {
        settings->setValue("ctrlColWName", ui->treeWidget->columnWidth(idxName));
        settings->setValue("ctrlColWClass", ui->treeWidget->columnWidth(idxClass));
        settings->setValue("ctrlColWCDen", ui->treeWidget->columnWidth(idxCtrlDev));
        settings->setValue("ctrlColWCMod", ui->treeWidget->columnWidth(idxCtrlMode));
    }
 
    void
    NJointControllersWidget::clearAll()
    {
        entries.clear();
        controllersCreated.clear();
        statusUpdates.clear();
        controllersDeleted.clear();
    }
 
    void
    NJointControllersWidget::doContentUpdate()
    {
        for (const auto& pair : controllersCreated)
        {
            add(pair.second);
        }
        controllersCreated.clear();
        for (const auto& pair : statusUpdates)
        {
            const auto& name = pair.second.instanceName;
            if (controllersDeleted.count(name))
            {
                continue;
            }
            auto it = entries.find(name);
            if (it != entries.end())
            {
                it->second->update(pair.second);
            }
            else
            {
                NJointControllerDescriptionWithStatus stat;
                stat.status = pair.second;
                ARMARX_INFO << "loding description for the missing entry '" << name << "'";
                stat.description = robotUnit->getNJointControllerDescription(name);
                add(stat);
            }
        }
        statusUpdates.clear();
        for (const auto& name : controllersDeleted)
        {
            auto it = entries.find(name);
            if (it != entries.end())
            {
                it->second->deleteContent();
                it->second->deleteLater();
                entries.erase(it);
            }
        }
        controllersDeleted.clear();
    }
 
    void
    NJointControllersWidget::getResetData()
    {
        auto temp = robotUnit->getNJointControllerDescriptionsWithStatuses();
        {
            std::unique_lock<std::recursive_timed_mutex> guard{mutex};
            for (NJointControllerDescriptionWithStatus& ds : temp)
            {
                controllersCreated[ds.description.instanceName] = std::move(ds);
            }
        }
    }
 
    bool
    NJointControllersWidget::addOneFromResetData()
    {
        if (controllersCreated.empty())
        {
            return true;
        }
        add(controllersCreated.begin()->second);
        controllersCreated.erase(controllersCreated.begin());
        return false;
    }
 
    NJointControllersWidgetEntry::NJointControllersWidgetEntry(
        NJointControllersWidget& parent,
        QTreeWidget& treeWidget,
        const NJointControllerDescription& desc) :
        QObject(&parent), deletable{desc.deletable}, controller{desc.controller}
    {
        ARMARX_CHECK_EXPRESSION(controller);
        header = new QTreeWidgetItem;
        treeWidget.addTopLevelItem(header);
        header->setText(NJointControllersWidget::idxName,
                        QString::fromStdString(desc.instanceName));
        header->setText(NJointControllersWidget::idxClass, QString::fromStdString(desc.className));
        header->setText(NJointControllersWidget::idxActive, "?");
        header->setText(NJointControllersWidget::idxRequested, "?");
        header->setText(NJointControllersWidget::idxError, "?");
        header->setText(NJointControllersWidget::idxInternal, desc.internal ? "X" : " ");
 
        const std::size_t numDev = desc.controlModeAssignment.size();
        const std::size_t numMod =
            getMapValues<StringStringDictionary, std::set>(desc.controlModeAssignment).size();
        //device list
        {
            //buttons
            QWidget* widgDev = new QWidget{&parent};
            QWidget* widgMod = new QWidget{&parent};
            treeWidget.setItemWidget(header, NJointControllersWidget::idxCtrlDev, widgDev);
            treeWidget.setItemWidget(header, NJointControllersWidget::idxCtrlMode, widgMod);
            QVBoxLayout* layDev = new QVBoxLayout;
            QVBoxLayout* layMod = new QVBoxLayout;
            layDev->setContentsMargins(0, 0, 0, 0);
            layMod->setContentsMargins(0, 0, 0, 0);
            widgDev->setLayout(layDev);
            widgMod->setLayout(layMod);
            boxDev = new QCheckBox{QString::number(numDev) + " Device" +
                                   (numDev > 1 ? QString{"s"} : QString{""})};
            boxMod = new QCheckBox{QString::number(numMod) + " Mode" +
                                   (numMod > 1 ? QString{"s"} : QString{""})};
            layDev->addWidget(boxDev);
            layMod->addWidget(boxMod);
            boxDev->setStyleSheet(checkboxStyleSheet());
            boxMod->setStyleSheet(checkboxStyleSheet());
            boxDev->setChecked(false);
            boxMod->setChecked(false);
            connect(boxDev, SIGNAL(clicked(bool)), this, SLOT(hideDeviceList()));
            connect(boxMod, SIGNAL(clicked(bool)), this, SLOT(hideDeviceList()));
            //list
            devsToModes.reserve(desc.controlModeAssignment.size());
            for (const auto& pair : desc.controlModeAssignment)
            {
                QTreeWidgetItem* child = new QTreeWidgetItem;
                header->addChild(child);
                child->setText(NJointControllersWidget::idxCtrlDev,
                               QString::fromStdString(pair.first));
                child->setText(NJointControllersWidget::idxCtrlMode,
                               QString::fromStdString(pair.second));
                devsToModes.emplace_back(child);
                child->setHidden(true);
            }
        }
        QPushButton* act = new QPushButton;
        QPushButton* dec = new QPushButton;
        act->setToolTip("Activate the Controller");
        dec->setToolTip("Deactivate the Controller");
        treeWidget.setItemWidget(header, NJointControllersWidget::idxActivate, act);
        treeWidget.setItemWidget(header, NJointControllersWidget::idxDeactivate, dec);
        act->setIcon(QIcon{QString{":/icons/media-playback-start.ico"}});
        dec->setIcon(QIcon{QString{":/icons/media-playback-pause.ico"}});
        act->setFixedWidth(40);
        dec->setFixedWidth(40);
        act->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Minimum);
        dec->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Minimum);
        connect(act, SIGNAL(clicked(bool)), this, SLOT(activateController()));
        connect(dec, SIGNAL(clicked(bool)), this, SLOT(deactivateController()));
        if (desc.deletable)
        {
            QPushButton* del = new QPushButton;
            treeWidget.setItemWidget(header, NJointControllersWidget::idxDelete, del);
            del->setToolTip("Delete the Controller");
            del->setIcon(QIcon{QString{":/icons/Trash.svg"}});
            del->setFixedWidth(40);
            del->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Minimum);
            connect(del, SIGNAL(clicked(bool)), this, SLOT(deleteController()));
        }
 
        //remote functions
        {
            for (const auto& descr : controller->getFunctionDescriptions())
            {
                new NJointControllersWidgetRemoteFunction{
                    treeWidget,
                    *header,
                    descr.first,
                    controller,
                    descr.second}; //adds it self to the widget
            }
        }
    }
 
    void
    NJointControllersWidgetEntry::update(const NJointControllerStatus& status)
    {
        header->setText(NJointControllersWidget::idxActive, status.active ? "X" : " ");
        header->setText(NJointControllersWidget::idxRequested, status.requested ? "X" : " ");
        header->setText(NJointControllersWidget::idxError, status.error ? "X" : " ");
        auto acReqColor = (status.active != status.requested) ? red() : green();
        header->setBackground(NJointControllersWidget::idxActive, {acReqColor});
        header->setBackground(NJointControllersWidget::idxRequested, {acReqColor});
        header->setBackground(NJointControllersWidget::idxError, {status.error ? red() : green()});
        //name
        header->setBackground(NJointControllersWidget::idxName,
                              {status.error ? red() : acReqColor});
    }
 
    bool
    NJointControllersWidgetEntry::matchName(const QString& name)
    {
        return header->text(NJointControllersWidget::idxName).contains(name, Qt::CaseInsensitive);
    }
 
    bool
    NJointControllersWidgetEntry::matchClass(const QString& name)
    {
        return header->text(NJointControllersWidget::idxClass).contains(name, Qt::CaseInsensitive);
    }
 
    bool
    NJointControllersWidgetEntry::isActiveState(const QString& state)
    {
        return header->text(NJointControllersWidget::idxActive)
            .contains(state, Qt::CaseInsensitive);
    }
 
    bool
    NJointControllersWidgetEntry::isRequestedState(const QString& state)
    {
        return header->text(NJointControllersWidget::idxRequested)
            .contains(state, Qt::CaseInsensitive);
    }
 
    bool
    NJointControllersWidgetEntry::isErrorState(const QString& state)
    {
        return header->text(NJointControllersWidget::idxError).contains(state, Qt::CaseInsensitive);
    }
 
    bool
    NJointControllersWidgetEntry::matchDevice(const QString& dev)
    {
        for (const auto& elem : devsToModes)
        {
            if (elem->text(NJointControllersWidget::idxCtrlDev).contains(dev, Qt::CaseInsensitive))
            {
                return true;
            }
        }
        return false;
    }
 
    bool
    NJointControllersWidgetEntry::matchMode(const QString& mode)
    {
        for (const auto& elem : devsToModes)
        {
            if (elem->text(NJointControllersWidget::idxCtrlMode)
                    .contains(mode, Qt::CaseInsensitive))
            {
                return true;
            }
        }
        return false;
    }
 
    void
    NJointControllersWidgetEntry::deleteContent()
    {
        for (auto item : devsToModes)
        {
            delete item;
        }
        delete header;
    }
 
    void
    NJointControllersWidgetEntry::setVisible(bool vis)
    {
        setDeviceListVisible(vis);
        header->setHidden(!vis);
    }
 
    void
    NJointControllersWidgetEntry::activateController()
    {
        ARMARX_CHECK_EXPRESSION(controller);
        controller->activateController();
    }
 
    void
    NJointControllersWidgetEntry::deactivateController()
    {
        ARMARX_CHECK_EXPRESSION(controller);
        controller->deactivateController();
    }
 
    void
    NJointControllersWidgetEntry::deleteController()
    {
        ARMARX_CHECK_EXPRESSION(controller);
        controller->deleteController();
    }
 
    void
    NJointControllersWidgetEntry::hideDeviceList()
    {
        QCheckBox* box = dynamic_cast<QCheckBox*>(sender());
        if (box)
        {
            bool checked = box->isChecked();
            boxDev->setChecked(checked);
            boxMod->setChecked(checked);
            for (QTreeWidgetItem* dev : devsToModes)
            {
                dev->setHidden(!checked);
            }
            if (checked)
            {
                header->setExpanded(true);
            }
        }
    }
 
    void
    NJointControllersWidgetEntry::setDeviceListVisible(bool vis)
    {
        boxDev->setChecked(vis);
        boxMod->setChecked(vis);
        for (QTreeWidgetItem* dev : devsToModes)
        {
            dev->setHidden(!vis);
        }
    }
 
    NJointControllersWidgetRemoteFunction::NJointControllersWidgetRemoteFunction(
        QTreeWidget& treeWidget,
        QTreeWidgetItem& header,
        const std::string& name,
        const NJointControllerInterfacePrx& ctrl,
        const WidgetDescription::WidgetPtr& w) :
        functionName{name}, ctrl{ctrl}
    {
        //tree widget item
        {
            setObjectName(QString::fromStdString(ctrl->getInstanceName() + "_" + name));
            functionHeader = new QTreeWidgetItem;
            header.addChild(functionHeader);
            functionHeader->setFirstColumnSpanned(true);
            treeWidget.setItemWidget(functionHeader, 0, this);
 
            QHBoxLayout* l = new QHBoxLayout;
            setLayout(l);
            l->setContentsMargins(0, 0, 0, 0);
            //exec button
            {
                QPushButton* execute = new QPushButton;
                execute->setIcon(QIcon{QString{":/icons/media-playback-start.ico"}});
                execute->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Minimum);
                execute->setFixedWidth(40);
                execute->setToolTip("Execute '" + QString::fromStdString(name) + "' function");
                connect(execute, SIGNAL(clicked(bool)), this, SLOT(execFunction()));
                l->addWidget(execute);
            }
            //param change box
            execOnParamChange = new QCheckBox{"Call function on parameter changes"};
            execOnParamChange->setToolTip("Execute '" + QString::fromStdString(name) +
                                          "' function whenever a parameter changes");
            l->addWidget(execOnParamChange);
            if (!w)
            {
                //this way we always have the correct horizontal spacing
                execOnParamChange->setFixedHeight(0);
                execOnParamChange->setSizePolicy(execOnParamChange->sizePolicy().horizontalPolicy(),
                                                 QSizePolicy::Fixed);
            }
            //name
            l->addWidget(new QLabel{"Remote function: " + QString::fromStdString(name)});
            //spacer
            l->addSpacerItem(new QSpacerItem{0, 0, QSizePolicy::Expanding});
        }
        //add widget as child
        if (w)
        {
            QTreeWidgetItem* fnitem = new QTreeWidgetItem;
            functionHeader->addChild(fnitem);
            fnitem->setFirstColumnSpanned(true);
            QWidget* container = new QWidget;
            treeWidget.setItemWidget(fnitem, 0, container);
            QHBoxLayout* compress = new QHBoxLayout;
            compress->setContentsMargins(0, 0, 0, 0);
            container->setLayout(compress);
            //params
            params = WidgetDescription::makeDescribedWidget(w, this);
            paramValues = params->getVariants();
            params->setContentsMargins(0, 0, 0, 0);
            compress->addWidget(params);
            //add spacer
            compress->addItem(new QSpacerItem{
                0, 0, QSizePolicy::MinimumExpanding, QSizePolicy::MinimumExpanding});
        }
    }
 
    void
    NJointControllersWidgetRemoteFunction::execFunction()
    {
        if (paramValues.empty())
        {
            ARMARX_INFO << deactivateSpam(1, functionName) << "calling function " << functionName
                        << " with no parameters\n";
        }
        else
        {
            std::stringstream ss;
            ss << "calling function " << functionName << " with parameters:\n";
            for (const auto& pair : paramValues)
            {
                if (pair.second)
                {
 
                    if (pair.second->data)
                    {
                        ss << "    '" << pair.first << "' of type " << pair.second->data->ice_id()
                           << "\n";
                    }
                    else
                    {
                        ss << "    '" << pair.first << "' nullptr data \n";
                    }
                }
                else
                {
                    ss << "    '" << pair.first << "' nullptr\n";
                }
            }
            ARMARX_INFO << deactivateSpam(1, functionName) << ss.str();
        }
        ctrl->callDescribedFunction(functionName, paramValues);
    }
 
    void
    NJointControllersWidgetRemoteFunction::valueChangedSlot(std::string name, VariantBasePtr value)
    {
        paramValues[std::move(name)] = std::move(value);
        if (execOnParamChange->isChecked())
        {
            execFunction();
        }
    }
} // namespace armarx