LocalizationRemoteGui.cpp

Go to the documentation of this file.

#include "LocalizationRemoteGui.h"
 
#include <cmath>
#include <mutex>
 
#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/CycleUtil.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <ArmarXGui/interface/RemoteGuiInterface.h>
#include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder/LayoutWidgets.h>
#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder/StaticWidgets.h>
#include <ArmarXGui/libraries/RemoteGui/WidgetProxy.h>
 
namespace armarx::localization_and_mapping::components::cartographer_localization_and_mapping
{
 
    LocalizationRemoteGui::LocalizationRemoteGui(const RemoteGuiInterfacePrx& remoteGui,
                                                 LocalizationRemoteGuiCallee& callee,
                                                 const Eigen::Isometry3f& initialWorldToMap) :
        remoteGui(remoteGui),
        runningTask(new RunningTask<LocalizationRemoteGui>(this, &LocalizationRemoteGui::run)),
        callee(callee),
        initialWorldToMap(initialWorldToMap),
        lastSentValues(Eigen::Vector3f::Zero())
    {
        createRemoteGuiTab();
        tab = RemoteGui::TabProxy(remoteGui, REMOTE_GUI_TAB_NAME);
        runningTask->start();
    }
 
    LocalizationRemoteGui::~LocalizationRemoteGui()
    {
        if (runningTask->isRunning())
        {
            runningTask->stop();
        }
    }
 
    void
    LocalizationRemoteGui::createRemoteGuiTab()
    {
        ARMARX_TRACE;
        ARMARX_CHECK_NOT_NULL(remoteGui);
 
        const float x0 = initialWorldToMap.translation().x();
        const float y0 = initialWorldToMap.translation().y();
        const float yaw0 =
            std::atan2(initialWorldToMap.linear()(1, 0), initialWorldToMap.linear()(0, 0));
 
        lastSentValues = {x0, y0, yaw0};
 
        guiTab.worldToMap.group.setLabel("World to map registration");
 
        constexpr float posMax = 1e6F; // [mm]
        guiTab.worldToMap.x.setName("x");
        guiTab.worldToMap.x.setRange(-posMax, posMax);
        guiTab.worldToMap.x.setDecimals(1);
        guiTab.worldToMap.x.setSteps(static_cast<int>(posMax / 10));
        guiTab.worldToMap.x.setValue(x0);
 
        guiTab.worldToMap.y.setName("y");
        guiTab.worldToMap.y.setRange(-posMax, posMax);
        guiTab.worldToMap.y.setDecimals(1);
        guiTab.worldToMap.y.setSteps(static_cast<int>(posMax / 10));
        guiTab.worldToMap.y.setValue(y0);
 
        guiTab.worldToMap.yaw.setName("yaw");
        guiTab.worldToMap.yaw.setRange(-static_cast<float>(M_PI), static_cast<float>(M_PI));
        guiTab.worldToMap.yaw.setDecimals(4);
        guiTab.worldToMap.yaw.setSteps(3600);
        guiTab.worldToMap.yaw.setValue(yaw0);
 
        guiTab.worldToMap.applyImmediately.setName("applyImmediately");
        // guiTab.worldToMap.applyImmediately.setLabel("Apply immediately");
        guiTab.worldToMap.applyImmediately.setValue(false);
 
        guiTab.worldToMap.applyButton.setLabel("Apply");
 
        armarx::RemoteGui::Client::GridLayout grid;
        int row = 0;
 
        grid.add(armarx::RemoteGui::Client::Label("x [mm]"), {.row = row, .column = 0})
            .add(guiTab.worldToMap.x, {.row = row, .column = 1});
        ++row;
 
        grid.add(armarx::RemoteGui::Client::Label("y [mm]"), {.row = row, .column = 0})
            .add(guiTab.worldToMap.y, {.row = row, .column = 1});
        ++row;
 
        grid.add(armarx::RemoteGui::Client::Label("yaw [rad]"), {.row = row, .column = 0})
            .add(guiTab.worldToMap.yaw, {.row = row, .column = 1});
        ++row;
 
        grid.add(armarx::RemoteGui::Client::Label("Apply immediately"), {.row = row, .column = 0})
            .add(guiTab.worldToMap.applyImmediately, {.row = row, .column = 1});
        ++row;
 
        grid.add(
            guiTab.worldToMap.applyButton, {.row = row, .column = 0}, {.rows = 1, .columns = 2});
        ++row;
 
        armarx::RemoteGui::Client::VBoxLayout root = {grid, armarx::RemoteGui::Client::VSpacer()};
        guiTab.worldToMap.group.addChild(root);
        guiTab.hbox.addChild(guiTab.worldToMap.group);
 
        guiTab.create(REMOTE_GUI_TAB_NAME, remoteGui, guiTab.hbox);
    }
 
    void
    LocalizationRemoteGui::run()
    {
        constexpr int kCycleDurationMs = 100;
        CycleUtil c(kCycleDurationMs);
 
        while (!runningTask->isStopped())
        {
            {
                std::lock_guard g{mtx};
                tab.receiveUpdates();
                guiTab.receiveUpdates();
            }
 
            handleEvents(tab);
 
            {
                std::lock_guard g{mtx};
                tab.sendUpdates();
                guiTab.sendUpdates();
            }
 
            c.waitForCycleDuration();
        }
    }
 
    void
    LocalizationRemoteGui::handleEvents(RemoteGui::TabProxy& /*tab*/)
    {
        ARMARX_TRACE;
        std::lock_guard g{handleEventsMtx};
 
        const Eigen::Vector3f current{guiTab.worldToMap.x.getValue(),
                                      guiTab.worldToMap.y.getValue(),
                                      guiTab.worldToMap.yaw.getValue()};
 
        const bool valuesChanged = (current - lastSentValues).cwiseAbs().maxCoeff() > 1e-5F;
 
        const auto buildTransform = [&]() -> Eigen::Isometry3f
        {
            Eigen::Isometry3f t = Eigen::Isometry3f::Identity();
            t.translation().x() = current.x();
            t.translation().y() = current.y();
            t.linear() =
                Eigen::AngleAxisf(current.z(), Eigen::Vector3f::UnitZ()).toRotationMatrix();
            return t;
        };
 
        if (valuesChanged and guiTab.worldToMap.applyImmediately.getValue())
        {
            lastSentValues = current;
            callee.onWorldToMapTransformUpdate(buildTransform());
        }
 
        bool wasButtonClicked = false;
        try
        {
            wasButtonClicked = guiTab.worldToMap.applyButton.wasClicked();
        }
        catch (armarx::LocalException&)
        {
            ARMARX_INFO << GetHandledExceptionString();
        }
 
        if (wasButtonClicked)
        {
            lastSentValues = current;
            callee.onWorldToMapTransformUpdate(buildTransform());
        }
    }
 
    void
    LocalizationRemoteGui::shutdown()
    {
        if (!runningTask->isStopped())
        {
            runningTask->stop();
        }
        ARMARX_DEBUG << "Removing tab: " << REMOTE_GUI_TAB_NAME;
        remoteGui->removeTab(REMOTE_GUI_TAB_NAME);
    }
 
    void
    LocalizationRemoteGui::enable()
    {
        ARMARX_TRACE;
        tab.internalSetDisabled(guiTab.worldToMap.applyButton.getName(), false);
        tab.internalSetDisabled(guiTab.worldToMap.applyImmediately.getName(), false);
    }
 
    void
    LocalizationRemoteGui::disable()
    {
        ARMARX_TRACE;
        tab.internalSetDisabled(guiTab.worldToMap.applyButton.getName(), true);
        tab.internalSetDisabled(guiTab.worldToMap.applyImmediately.getName(), true);
    }
 
} // namespace armarx::localization_and_mapping::components::cartographer_localization_and_mapping