LocalizationRemoteGui.cpp

Go to the documentation of this file.

#include "LocalizationRemoteGui.h"
 
#include <cmath>
#include <mutex>
 
#include "ArmarXCore/core/time/CycleUtil.h"
#include "ArmarXCore/statechart/ParameterMapping.h"
#include "ArmarXCore/util/CPPUtility/trace.h"
#include <ArmarXCore/core/exceptions/LocalException.h>
 
#include "ArmarXGui/libraries/RemoteGui/WidgetBuilder/IntegerWidgets.h"
#include "ArmarXGui/libraries/RemoteGui/WidgetBuilder/LayoutWidgets.h"
#include "ArmarXGui/libraries/RemoteGui/WidgetBuilder/StaticWidgets.h"
#include <ArmarXGui/interface/RemoteGuiInterface.h>
#include <ArmarXGui/libraries/RemoteGui/WidgetProxy.h>
 
namespace armarx::localization_and_mapping::components::cartographer_localization_and_mapping
{
 
    LocalizationRemoteGui::LocalizationRemoteGui(const RemoteGuiInterfacePrx& remoteGui,
                                                 LocalizationRemoteGuiCallee& callee) :
        remoteGui(remoteGui),
        runningTask(new RunningTask<LocalizationRemoteGui>(this, &LocalizationRemoteGui::run)),
        callee(callee)
    {
        createRemoteGuiTab();
 
        tab = RemoteGui::TabProxy(remoteGui, REMOTE_GUI_TAB_MAME);
 
        runningTask->start();
    }
 
    LocalizationRemoteGui::~LocalizationRemoteGui()
    {
        if (runningTask->isRunning())
        {
            runningTask->stop();
        }
    }
 
    void
    LocalizationRemoteGui::createRemoteGuiTab()
    {
        ARMARX_TRACE;
 
        ARMARX_CHECK_NOT_NULL(remoteGui); // createRemoteGui() has to be called first
 
        {
            guiTab.localizationCorrection.group.setLabel("Localization correction");
 
            const float posMax = 10'000;
            guiTab.localizationCorrection.x.setName("x");
            guiTab.localizationCorrection.x.setRange(-posMax, posMax);
            guiTab.localizationCorrection.x.setDecimals(0);
            guiTab.localizationCorrection.x.setSteps(int(posMax / 10));
            guiTab.localizationCorrection.x.setValue(0.F);
 
            guiTab.localizationCorrection.y.setName("y");
            guiTab.localizationCorrection.y.setRange(-posMax, posMax);
            guiTab.localizationCorrection.y.setDecimals(0);
            guiTab.localizationCorrection.y.setSteps(int(posMax / 10));
            guiTab.localizationCorrection.y.setValue(0.F);
 
            const float angleMax = M_PIf32;
 
            guiTab.localizationCorrection.yaw.setName("yaw");
            guiTab.localizationCorrection.yaw.setRange(-angleMax, angleMax);
            // guiTab.localizationCorrection.yaw.setDecimals(2);
            guiTab.localizationCorrection.yaw.setSteps(360);
            guiTab.localizationCorrection.yaw.setValue(0.F);
 
            guiTab.controlGroup.applyButton.setLabel("Apply");
 
            armarx::RemoteGui::Client::GridLayout grid;
            int row = 10;
            {
 
                grid.add(guiTab.localizationCorrection.x, {row, 1});
                ++row;
 
                grid.add(guiTab.localizationCorrection.y, {row, 1});
                ++row;
 
                grid.add(guiTab.localizationCorrection.yaw, {row, 1});
                ++row;
 
                grid.add(guiTab.controlGroup.applyButton, {row, 1});
                ++row;
            }
 
            armarx::RemoteGui::Client::VBoxLayout root = {grid,
                                                          armarx::RemoteGui::Client::VSpacer()};
            guiTab.localizationCorrection.group.addChild(root);
        }
 
        guiTab.hbox.addChild(guiTab.localizationCorrection.group);
 
        guiTab.create(REMOTE_GUI_TAB_MAME, remoteGui, guiTab.hbox);
 
        // boxLayout.addChild(line);
        // remoteGui->createTab(REMOTE_GUI_TAB_MAME, guiTab.hbox);
    }
 
    void
    LocalizationRemoteGui::run()
    {
        constexpr int kCycleDurationMs = 100;
 
        CycleUtil c(kCycleDurationMs);
        while (!runningTask->isStopped())
        {
            {
 
                {
                    std::lock_guard g{mtx};
 
                    ARMARX_TRACE;
                    tab.receiveUpdates();
                    guiTab.receiveUpdates();
                }
 
                ARMARX_TRACE;
                handleEvents(tab);
 
                {
                    std::lock_guard g{mtx};
 
                    ARMARX_TRACE;
                    tab.sendUpdates();
                    guiTab.sendUpdates();
                }
            }
 
            c.waitForCycleDuration();
        }
    }
 
    void
    LocalizationRemoteGui::handleEvents(RemoteGui::TabProxy& tab)
    {
        ARMARX_TRACE;
 
        std::lock_guard g{handleEventsMtx};
 
        const Eigen::Vector3f correction{guiTab.localizationCorrection.x.getValue(),
                                         guiTab.localizationCorrection.y.getValue(),
                                         guiTab.localizationCorrection.yaw.getValue()};
        // ARMARX_INFO << correction;
 
        const Eigen::Vector3f diff = correction - localizationCorrection;
 
        // check if values have changed
        if ((diff.cwiseAbs().array() > 0.01F).any())
        {
            localizationCorrection = correction;
 
            const Eigen::Isometry3f trafo =
                Eigen::Translation3f(Eigen::Vector3f{correction.x(), correction.y(), 0.F}) *
                Eigen::Isometry3f(Eigen::AngleAxisf(correction.z(), Eigen::Vector3f::UnitZ()));
            // ARMARX_INFO << "Localization correction";
            callee.onLocalizationCorrection(trafo);
        }
 
        bool wasButtonClicked = false;
 
        // wasClicked can throw: "Reason: Could find value with name: 6"
        try
        {
            wasButtonClicked = guiTab.controlGroup.applyButton.wasClicked();
        }
        catch (armarx::LocalException& e)
        {
            ARMARX_INFO << GetHandledExceptionString();
        }
 
        if (wasButtonClicked)
        {
            ARMARX_TRACE;
            callee.onApplyLocalizationCorrection();
        }
    }
 
    void
    LocalizationRemoteGui::shutdown()
    {
        // std::lock_guard g{mtx};
 
        if (!runningTask->isStopped())
        {
            runningTask->stop();
        }
 
        ARMARX_DEBUG << "Removing tab: " << REMOTE_GUI_TAB_MAME;
        remoteGui->removeTab(REMOTE_GUI_TAB_MAME);
    }
 
    void
    LocalizationRemoteGui::enable()
    {
        ARMARX_TRACE;
 
        // std::lock_guard g{mtx};
        tab.internalSetDisabled(guiTab.controlGroup.applyButton.getName(), false);
    }
 
    void
    LocalizationRemoteGui::disable()
    {
        ARMARX_TRACE;
 
        // std::unique_lock g{mtx};
 
        // it might happen, that this function is triggered from handleEvents
        // std::unique_lock ul{handleEventsMtx};
        // ul.try_lock();
 
        tab.internalSetDisabled(guiTab.controlGroup.applyButton.getName(), true);
    }
 
    void
    LocalizationRemoteGui::reset()
    {
        ARMARX_TRACE;
 
        // std::lock_guard g{mtx};
 
        guiTab.localizationCorrection.x.setValue(0.F);
        guiTab.localizationCorrection.y.setValue(0.F);
        guiTab.localizationCorrection.yaw.setValue(0.F);
 
        tab.sendUpdates();
        guiTab.sendUpdates();
    }
 
} // namespace armarx::localization_and_mapping::components::cartographer_localization_and_mapping