LookAtClickWidgetController.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    VisionX::gui-plugins::LookAtClickWidgetController
 * \author     [Author Name] ( [Author Email] )
 * \date       2020
 * \copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "LookAtClickWidgetController.h"
 
#include <algorithm>
#include <iostream>
#include <string>
 
#include <QImage>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
#include <RobotAPI/interface/core/RobotState.h>
 
#include <VisionX/tools/ImageUtil.h>
 
namespace armarx
{
    LookAtClickWidgetController::LookAtClickWidgetController()
    {
        widget.setupUi(getWidget());
 
        _turnOnClick = new LookAtClick;
        widget.verticalLayoutImage->addWidget(_turnOnClick);
        //        _turnOnClick->drawBackground();
        connect(
            _turnOnClick, SIGNAL(clickedAt(const QPoint&)), this, SLOT(clickedAt(const QPoint&)));
        startTimer(10);
    }
 
    LookAtClickWidgetController::~LookAtClickWidgetController()
    {
    }
 
    QPointer<QDialog>
    LookAtClickWidgetController::getConfigDialog(QWidget* parent)
    {
        if (!_dialog)
        {
            _dialog = new SimpleConfigDialog(parent);
            _dialog->addProxyFinder<visionx::ImageProviderInterfacePrx>(
                "ImageProvider", "ImageProvider", "Image*|*Provider");
            _dialog->addProxyFinder<RobotStateComponentInterfacePrx>(
                "rsc", "Robot State Component", "*Component");
            _dialog->addProxyFinder<KinematicUnitInterfacePrx>(
                "KinematicUnitInterface", "Kinematic Unit Interface", "*KinematicUnit");
            _dialog->addLineEdit("pitch", "Pitch Joint (empty = autodetect)", "");
            _dialog->addLineEdit("yaw", "Yaw Joint (empty = autodetect)", "");
        }
        return qobject_cast<SimpleConfigDialog*>(_dialog);
    }
 
    void
    LookAtClickWidgetController::loadSettings(QSettings* settings)
    {
        _imageProviderName = settings->value("_imageProviderName").toString().toStdString();
        _robotStateUnitComponentName = settings->value("rsc").toString().toStdString();
        _kinematicUnitInterfaceName =
            settings->value("KinematicUnitInterface").toString().toStdString();
        _pitchJointName = settings->value("pitch").toString().toStdString();
        _yawJointName = settings->value("yaw").toString().toStdString();
        _inverseYaw = settings->value("inverseYaw").toFloat();
 
        widget.doubleSpinBoxFactorPitch->setValue(settings->value("pitchFactor").toDouble());
        widget.doubleSpinBoxFactorYaw->setValue(settings->value("yawFactor").toDouble());
    }
 
    void
    LookAtClickWidgetController::saveSettings(QSettings* settings)
    {
        settings->setValue("_imageProviderName", QString::fromStdString(_imageProviderName));
        settings->setValue(
            "rsc",
            QString::fromStdString(getRobotStateComponentPlugin().getRobotStateComponentName()));
        settings->setValue("KinematicUnitInterface",
                           QString::fromStdString(_kinematicUnitInterfaceName));
        settings->setValue("pitch", QString::fromStdString(_pitchJointName));
        settings->setValue("yaw", QString::fromStdString(_yawJointName));
        settings->setValue("inverseYaw", _inverseYaw);
 
        settings->setValue("pitchFactor", widget.doubleSpinBoxFactorPitch->value());
        settings->setValue("yawFactor", widget.doubleSpinBoxFactorPitch->value());
    }
 
    void
    LookAtClickWidgetController::onInitImageProcessor()
    {
        usingImageProvider(_imageProviderName);
        usingProxy(_kinematicUnitInterfaceName);
    }
 
    void
    LookAtClickWidgetController::autoDetectRobotSpecificVariables()
    {
        synchronizeLocalClone(_robot);
        if (_yawJointName == "")
        {
            if (_robot->getName() == "Armar6")
            {
                _inverseYaw = -1;
                _yawJointName = "Neck_1_Yaw";
            }
            else if (_robot->getName() == "Armar3")
            {
                _inverseYaw = 1;
                _yawJointName = "Neck_3_Yaw";
            }
            else
            {
                throw std::invalid_argument("Could not autodetect yaw!");
            }
        }
        if (_pitchJointName == "")
        {
            if (_robot->getName() == "Armar6")
            {
                _pitchJointName = "Neck_2_Pitch";
            }
            else if (_robot->getName() == "Armar3")
            {
                _pitchJointName = "Neck_1_Pitch";
            }
            else
            {
                throw std::invalid_argument("Could not autodetect pitch!");
            }
        }
    }
 
    void
    LookAtClickWidgetController::onConnectImageProcessor()
    {
        _robot = addRobot("state robot", VirtualRobot::RobotIO::eStructure);
        try
        {
            autoDetectRobotSpecificVariables();
        }
        catch (...)
        {
            ARMARX_ERROR << "Could not autodetect joints!";
        }
        getProxy(kinematicUnitInterfacePrx, _kinematicUnitInterfaceName);
        //image provider
        {
            _imageProviderInfo = getImageProvider(_imageProviderName, true);
            _imageProvider = _imageProviderInfo.proxy;
            ARMARX_CHECK_GREATER(_imageProviderInfo.numberImages, 0);
            ARMARX_CHECK_EQUAL(_imageProviderInfo.imageFormat.type, visionx::ImageType::eRgb);
            //reserve buffers
            _providerImagesOwner.reserve(_imageProviderInfo.numberImages);
            _providerImages.reserve(_imageProviderInfo.numberImages);
            for (int i = 0; i < _imageProviderInfo.numberImages; ++i)
            {
                _providerImagesOwner.emplace_back(
                    visionx::tools::createByteImage(_imageProviderInfo));
                _providerImages.emplace_back(
                    static_cast<void*>(_providerImagesOwner.back()->pixels));
            }
            _numberOfImages = _imageProviderInfo.numberImages;
        }
    }
 
    void
    LookAtClickWidgetController::process()
    {
        if (!waitForImages(_imageProviderName, 1000))
        {
            return;
        }
        getImages(_providerImages.data());
        const CByteImage& img = *(_providerImagesOwner.at(_imageIndex));
        {
            std::lock_guard<std::mutex> guard{_currentImageMutex};
            _currentImage = QImage(img.width, img.height, QImage::Format_RGB888);
            ARMARX_CHECK_NOT_NULL(_currentImage.bits());
            ARMARX_CHECK_NOT_NULL(img.pixels);
            std::memcpy(_currentImage.bits(), img.pixels, 3 * img.width * img.height);
            _currentImageDirty = true;
        }
    }
 
    void
    LookAtClickWidgetController::configured()
    {
        _imageProviderName = _dialog->getProxyName("ImageProvider");
        _pitchJointName = _dialog->getLineEditText("pitch");
        _yawJointName = _dialog->getLineEditText("yaw");
        _kinematicUnitInterfaceName = _dialog->getProxyName("KinematicUnitInterface");
    }
 
    void
    LookAtClickWidgetController::refreshWidgetData(float* factorPitch, float* factorYaw)
    {
        *factorPitch = widget.doubleSpinBoxFactorPitch->value();
        *factorYaw = widget.doubleSpinBoxFactorYaw->value();
    }
 
    void
    LookAtClickWidgetController::determineNewAngles(const QPoint& offset,
                                                    NameValueMap& jointAngles,
                                                    NameControlModeMap& jointModes,
                                                    float factorPitch,
                                                    float factorYaw)
    {
        jointModes[_pitchJointName] = ePositionControl;
        jointModes[_yawJointName] = ePositionControl;
        ARMARX_INFO << _robot->getRobotNode(_pitchJointName)->getJointValue();
        ARMARX_INFO << _robot->getRobotNode(_yawJointName)->getJointValue();
        auto pitch = _robot->getRobotNode(_pitchJointName);
        auto yaw = _robot->getRobotNode(_yawJointName);
        const float pitchValue = offset.y() * factorPitch + pitch->getJointValue();
        const float yawValue = _inverseYaw * offset.x() * factorYaw + yaw->getJointValue();
        jointAngles[_pitchJointName] =
            std::clamp(pitchValue, pitch->getJointLimitLo(), pitch->getJointLimitHi());
        jointAngles[_yawJointName] =
            std::clamp(yawValue, yaw->getJointLimitLo(), yaw->getJointLimitHi());
        ARMARX_INFO << "pitch value: " << jointAngles[_pitchJointName]
                    << "yaw value: " << jointAngles[_yawJointName];
    }
 
    void
    LookAtClickWidgetController::clickedAt(const QPoint& offset)
    {
        ARMARX_INFO << "ClickedAt called!";
        float factorPitch, factorYaw;
        NameValueMap jointAngles;
        NameControlModeMap jointModes;
 
        refreshWidgetData(&factorPitch, &factorYaw);
        synchronizeLocalClone(_robot);
        determineNewAngles(offset, jointAngles, jointModes, factorPitch, factorYaw);
        try
        {
            kinematicUnitInterfacePrx->switchControlMode(jointModes);
            kinematicUnitInterfacePrx->setJointAngles(jointAngles);
        }
        catch (...)
        {
            ARMARX_ERROR << "Error occurred in clickedAt";
        }
        ARMARX_INFO << "after setjointangles";
    }
 
    void
    LookAtClickWidgetController::timerEvent(QTimerEvent*)
    {
        if (!_currentImageDirty)
        {
            return;
        }
        {
            std::lock_guard<std::mutex> guard{_currentImageMutex};
            _turnOnClick->drawBackground(_currentImage);
            _currentImageDirty = false;
        }
    }
} // namespace armarx