RobotViewerGuiPlugin.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * 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
 * @author
 * @date
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
#include "RobotViewerGuiPlugin.h"
 
#include <VirtualRobot/CollisionDetection/CollisionModel.h>
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
#include <VirtualRobot/Visualization/VisualizationFactory.h>
#include <VirtualRobot/XML/RobotIO.h>
 
#include <ArmarXCore/core/ArmarXManager.h>
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
 
#include <RobotAPI/libraries/core/FramedPose.h>
 
// Qt headers
#include <QCheckBox>
#include <QClipboard>
#include <QPushButton>
#include <QScrollBar>
#include <Qt>
#include <QtGlobal>
 
#include <ArmarXCore/util/json/JSONObject.h>
 
#include <ArmarXGui/libraries/StructuralJson/JsonWriter.h>
 
#include <Inventor/Qt/SoQt.h>
#include <Inventor/SoDB.h>
 
// System
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
 
#include <cmath>
#include <filesystem>
#include <iostream>
#include <string>
 
#define TIMER_MS 33
#define ROBOTSTATE_NAME_DEFAULT "RobotStateComponent"
 
using namespace armarx;
using namespace VirtualRobot;
 
enum RobotStateOutputType
{
    eJointConfiguration,
    eFramedPositionTCP,
    eFramedOrientationTCP,
    eFramedPoseTCP,
 
    eRobotStateOutputTypeSize
};
 
const QString ROBOT_STATE_OUTPUT_TYPE_NAMES[eRobotStateOutputTypeSize] = {
    "Joint Configuration",
    "Framed Position (TCP)",
    "Framed Orientation (TCP)",
    "Framed Pose (TCP)",
};
 
RobotViewerGuiPlugin::RobotViewerGuiPlugin()
{
    addWidget<RobotViewerWidgetController>();
}
 
RobotViewerWidgetController::RobotViewerWidgetController() :
    rootVisu(nullptr), robotVisu(nullptr), timerSensor(nullptr), debugLayerVisu(nullptr)
{
    ui.setupUi(getWidget());
    getWidget()->setEnabled(false);
 
    QComboBox* outputTypes = ui.outputTypeComboBox;
    for (int typeIndex = 0; typeIndex < eRobotStateOutputTypeSize; ++typeIndex)
    {
        auto& typeName = ROBOT_STATE_OUTPUT_TYPE_NAMES[typeIndex];
        outputTypes->addItem(typeName);
    }
    outputTypes->setCurrentIndex(0);
}
 
void
RobotViewerWidgetController::onInitComponent()
{
    bool createDebugDrawer = true;
    verbose = true;
 
    usingProxy(robotStateComponentName);
 
    rootVisu = new SoSeparator();
    rootVisu->ref();
 
    robotVisu = new SoSeparator;
    robotVisu->ref();
    rootVisu->addChild(robotVisu);
 
    // create the debugdrawer component
    if (createDebugDrawer)
    {
        std::string debugDrawerComponentName = "RobotViewerGUIDebugDrawer_" + getName();
        ARMARX_INFO << "Creating component " << debugDrawerComponentName;
        debugDrawer =
            Component::create<DebugDrawerComponent>(getIceProperties(), debugDrawerComponentName);
 
        if (mutex3D)
        {
            debugDrawer->setMutex(mutex3D);
        }
        else
        {
            ARMARX_ERROR << " No 3d mutex available...";
        }
 
        ArmarXManagerPtr m = getArmarXManager();
        m->addObject(debugDrawer);
 
 
        {
            std::unique_lock lock(*mutex3D);
            debugLayerVisu = new SoSeparator();
            debugLayerVisu->ref();
            debugLayerVisu->addChild(debugDrawer->getVisualization());
            rootVisu->addChild(debugLayerVisu);
        }
    }
 
    showRoot(true);
}
 
void
RobotViewerWidgetController::onConnectComponent()
{
    robotStateComponentPrx = getProxy<RobotStateComponentInterfacePrx>(robotStateComponentName);
    usingTopic(robotStateComponentPrx->getRobotStateTopicName());
    if (robotVisu)
    {
        robotVisu->removeAllChildren();
    }
 
    robot.reset();
 
    std::string rfile;
    Ice::StringSeq includePaths;
 
    // get robot filename
    try
    {
 
        Ice::StringSeq packages = robotStateComponentPrx->getArmarXPackages();
        packages.push_back(Application::GetProjectName());
        ARMARX_VERBOSE << "ArmarX packages " << packages;
 
        for (const std::string& projectName : packages)
        {
            if (projectName.empty())
            {
                continue;
            }
 
            CMakePackageFinder project(projectName);
            auto pathsString = project.getDataDir();
            ARMARX_VERBOSE << "Data paths of ArmarX package " << projectName << ": " << pathsString;
            Ice::StringSeq projectIncludePaths = Split(pathsString, ";,", true, true);
            ARMARX_VERBOSE << "Result: Data paths of ArmarX package " << projectName << ": "
                           << projectIncludePaths;
            includePaths.insert(
                includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
        }
 
        rfile = robotStateComponentPrx->getRobotFilename();
        ARMARX_VERBOSE << "Relative robot file " << rfile;
        ArmarXDataPath::getAbsolutePath(rfile, rfile, includePaths);
        ARMARX_VERBOSE << "Absolute robot file " << rfile;
    }
    catch (...)
    {
        ARMARX_ERROR << "Unable to retrieve robot filename";
    }
 
    try
    {
        ARMARX_INFO << "Loading robot from file " << rfile;
        robot = loadRobotFile(rfile);
    }
    catch (...)
    {
        ARMARX_ERROR << "Failed to init robot";
    }
 
    if (!robot)
    {
        getObjectScheduler()->terminate();
 
        if (getWidget()->parentWidget())
        {
            getWidget()->parentWidget()->close();
        }
 
        std::cout << "returning" << std::endl;
        return;
    }
 
    setRobotVisu(ui.radioButtonCol->isChecked());
    showRobot(ui.cbRobot->isChecked());
 
    // start update timer
    SoSensorManager* sensor_mgr = SoDB::getSensorManager();
    timerSensor = new SoTimerSensor(timerCB, this);
    timerSensor->setInterval(SbTime(TIMER_MS / 1000.0f));
    sensor_mgr->insertTimerSensor(timerSensor);
 
    // Initialize the copy state GUI
    {
        auto sharedRobot = robotStateComponentPrx->getSynchronizedRobot();
 
        ui.kinematicChainComboBox->addItem("<All Nodes>");
        robotNodeSetNames = sharedRobot->getRobotNodeSets();
        for (std::string const& nodeSetName : robotNodeSetNames)
        {
            ui.kinematicChainComboBox->addItem(QString::fromStdString(nodeSetName));
        }
        ui.kinematicChainComboBox->setCurrentIndex(0);
        ui.tcpComboBox->addItem("<default>");
        for (auto& node : sharedRobot->getRobotNodes())
        {
            ui.tcpComboBox->addItem(QString::fromStdString(node));
        }
        ui.tcpComboBox->setCurrentIndex(0);
 
        ui.frameComboBox->addItem("<Global>");
        robotNodeNames = sharedRobot->getRobotNodes();
        for (std::string const& nodeName : robotNodeNames)
        {
            ui.frameComboBox->addItem(QString::fromStdString(nodeName));
        }
        ui.frameComboBox->setCurrentIndex(0);
    }
 
    connectSlots();
    enableMainWidgetAsync(true);
}
 
void
RobotViewerWidgetController::onDisconnectComponent()
{
 
    ARMARX_INFO << "Disconnecting component";
 
    // stop update timer
    if (timerSensor)
    {
        SoSensorManager* sensor_mgr = SoDB::getSensorManager();
        sensor_mgr->removeTimerSensor(timerSensor);
    }
 
    ARMARX_INFO << "Disconnecting component: timer stopped";
 
    robotStateComponentPrx = nullptr;
 
    {
        std::unique_lock lock(*mutex3D);
 
        if (robotVisu)
        {
            ARMARX_INFO << "Disconnecting component: removing visu";
            robotVisu->removeAllChildren();
        }
 
        robot.reset();
    }
    ARMARX_INFO << "Disconnecting component: finished";
}
 
void
RobotViewerWidgetController::onExitComponent()
{
    enableMainWidgetAsync(false);
 
    {
        std::unique_lock lock(*mutex3D);
 
        if (debugLayerVisu)
        {
            debugLayerVisu->removeAllChildren();
            debugLayerVisu->unref();
            debugLayerVisu = NULL;
        }
 
        if (robotVisu)
        {
            robotVisu->removeAllChildren();
            robotVisu->unref();
            robotVisu = NULL;
        }
 
        if (rootVisu)
        {
            rootVisu->removeAllChildren();
            rootVisu->unref();
            rootVisu = NULL;
        }
    }
 
    /*
        if (debugDrawer && debugDrawer->getObjectScheduler())
        {
            ARMARX_INFO << "Removing DebugDrawer component...";
            debugDrawer->getObjectScheduler()->terminate();
            ARMARX_INFO << "Removing DebugDrawer component...done";
        }
    */
}
 
QPointer<QDialog>
RobotViewerWidgetController::getConfigDialog(QWidget* parent)
{
    if (!dialog)
    {
        dialog = new SimpleConfigDialog(parent);
        dialog->addProxyFinder<RobotStateComponentInterfacePrx>(
            {"RobotStateComponent", "", "RobotState*"});
    }
    return qobject_cast<SimpleConfigDialog*>(dialog);
}
 
void
RobotViewerWidgetController::configured()
{
    robotStateComponentName = dialog->getProxyName("RobotStateComponent");
}
 
void
RobotViewerWidgetController::loadSettings(QSettings* settings)
{
    robotStateComponentName =
        settings->value("RobotStateComponent", QString::fromStdString(ROBOTSTATE_NAME_DEFAULT))
            .toString()
            .toStdString();
 
    bool showRob = settings->value("showRobot", QVariant(true)).toBool();
    bool fullMod = settings->value("fullModel", QVariant(true)).toBool();
    ui.cbRobot->setChecked(showRob);
    ui.radioButtonFull->setChecked(fullMod);
    ui.radioButtonCol->setChecked(!fullMod);
}
 
void
RobotViewerWidgetController::saveSettings(QSettings* settings)
{
    settings->setValue("RobotStateComponent", QString::fromStdString(robotStateComponentName));
    settings->setValue("showRobot", ui.cbRobot->isChecked());
    settings->setValue("fullModel", ui.radioButtonFull->isChecked());
}
 
void
RobotViewerWidgetController::setRobotVisu(bool colModel)
{
    robotVisu->removeAllChildren();
 
    if (!robot)
    {
        return;
    }
 
    std::unique_lock lock(*mutex3D);
 
    VirtualRobot::SceneObject::VisualizationType v = VirtualRobot::SceneObject::Full;
 
    if (colModel)
    {
        v = VirtualRobot::SceneObject::Collision;
    }
 
    CoinVisualizationPtr robotViewerVisualization = robot->getVisualization(v);
 
    if (robotViewerVisualization)
    {
        robotVisu->addChild(robotViewerVisualization->getCoinVisualization());
    }
    else
    {
        ARMARX_WARNING << "no robot visu available...";
    }
}
 
void
RobotViewerWidgetController::showVisuLayers(bool show)
{
    if (debugDrawer)
    {
        if (show)
        {
            debugDrawer->enableAllLayers();
        }
        else
        {
            debugDrawer->disableAllLayers();
        }
    }
}
 
void
RobotViewerWidgetController::showRoot(bool show)
{
    if (!debugDrawer)
    {
        return;
    }
 
    std::string poseName("root");
 
    if (show)
    {
        Eigen::Matrix4f gp = Eigen::Matrix4f::Identity();
        PosePtr gpP(new Pose(gp));
        debugDrawer->setPoseDebugLayerVisu(poseName, gpP);
    }
    else
    {
        debugDrawer->removePoseDebugLayerVisu(poseName);
    }
}
 
void
RobotViewerWidgetController::showRobot(bool show)
{
    if (!robotVisu)
    {
        return;
    }
 
    std::unique_lock lock(*mutex3D);
 
    if (show && rootVisu->findChild(robotVisu) < 0)
    {
        rootVisu->addChild(robotVisu);
    }
    else if (!show && rootVisu->findChild(robotVisu) >= 0)
    {
        rootVisu->removeChild(robotVisu);
    }
}
 
SoNode*
RobotViewerWidgetController::getScene()
{
    return rootVisu;
}
 
void
RobotViewerWidgetController::timerCB(void* data, SoSensor* sensor)
{
    RobotViewerWidgetController* controller = static_cast<RobotViewerWidgetController*>(data);
 
    if (!controller)
    {
        return;
    }
 
    controller->updateRobotVisu();
}
 
void
RobotViewerWidgetController::connectSlots()
{
    // Robot viewer GUI (top part)
    connect(this, SIGNAL(robotStatusUpdated()), this, SLOT(updateRobotVisu()));
    connect(ui.cbDebugLayer,
            SIGNAL(toggled(bool)),
            this,
            SLOT(showVisuLayers(bool)),
            Qt::QueuedConnection);
    connect(ui.cbRoot, SIGNAL(toggled(bool)), this, SLOT(showRoot(bool)), Qt::QueuedConnection);
    connect(ui.cbRobot, SIGNAL(toggled(bool)), this, SLOT(showRobot(bool)), Qt::QueuedConnection);
    connect(
        ui.radioButtonCol, SIGNAL(toggled(bool)), this, SLOT(colModel(bool)), Qt::QueuedConnection);
    connect(ui.radioButtonFull,
            SIGNAL(toggled(bool)),
            this,
            SLOT(colModel(bool)),
            Qt::QueuedConnection);
    connect(ui.horizontalSliderCollisionModelInflation,
            SIGNAL(sliderMoved(int)),
            this,
            SLOT(inflateCollisionModel(int)),
            Qt::QueuedConnection);
 
    // Copy state GUI (bottom part)
    connect(ui.kinematicChainComboBox,
            SIGNAL(currentIndexChanged(int)),
            this,
            SLOT(updateStateSettings(int)));
    connect(
        ui.frameComboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(updateStateSettings(int)));
    connect(ui.outputTypeComboBox,
            SIGNAL(currentIndexChanged(int)),
            this,
            SLOT(updateStateSettings(int)));
    connect(ui.copyToClipboardButton, SIGNAL(clicked()), this, SLOT(copyToClipboard()));
 
    connect(this,
            SIGNAL(configurationChanged()),
            this,
            SLOT(onConfigurationChanged()),
            Qt::QueuedConnection);
}
 
VirtualRobot::RobotPtr
RobotViewerWidgetController::loadRobotFile(std::string fileName)
{
    VirtualRobot::RobotPtr robot;
 
    if (!ArmarXDataPath::getAbsolutePath(fileName, fileName))
    {
        ARMARX_INFO << "Could not find Robot XML file with name " << fileName;
    }
 
    robot = RobotIO::loadRobot(fileName);
 
    if (!robot)
    {
        ARMARX_INFO << "Could not find Robot XML file with name " << fileName;
    }
 
    return robot;
}
 
void
RobotViewerWidgetController::colModel(bool c)
{
    bool colModel = false;
 
    if (ui.radioButtonCol->isChecked())
    {
        colModel = true;
    }
    ui.horizontalSliderCollisionModelInflation->setEnabled(ui.radioButtonCol->isChecked());
 
    setRobotVisu(colModel);
}
 
void
RobotViewerWidgetController::updateStateSettings(int)
{
    updateState();
}
 
void
RobotViewerWidgetController::copyToClipboard()
{
    QClipboard* clipboard = QApplication::clipboard();
    if (clipboard)
    {
        QString currentText = ui.previewTextBox->document()->toPlainText();
        clipboard->setText(currentText);
    }
    else
    {
        ARMARX_ERROR << "Could not copy text to clipboard";
    }
}
 
static std::string
writeJointConfigurationToJson(VirtualRobot::Robot& robot,
                              VirtualRobot::RobotNodeSetPtr const& robotNodeSet)
{
    int indenting = 2; // Magic value for correct indenting
    JsonWriter writer(indenting);
    writer.startObject();
 
    if (robotNodeSet)
    {
        for (RobotNodePtr const& node : *robotNodeSet)
        {
            writer.writeKey(node->getName());
            writer.writeRawValue(to_string(node->getJointValue()));
        }
    }
    else // Write all rotational and translational joints
    {
        for (RobotNodePtr const& node : robot.getRobotNodes())
        {
            if (node->isRotationalJoint() || node->isTranslationalJoint())
            {
                writer.writeKey(node->getName());
                writer.writeRawValue(to_string(node->getJointValue()));
            }
        }
    }
 
    writer.endObject();
    return writer.toString();
}
 
template <typename FrameType>
static std::string
writeFramedTCP(VirtualRobot::RobotPtr const& robot,
               VirtualRobot::RobotNodeSetPtr const& nodeSet,
               std::string const& frameName,
               const std::string& tcpName)
{
    if (nodeSet)
    {
        auto tcp = tcpName.empty() ? nodeSet->getTCP() : robot->getRobotNode(tcpName);
        Eigen::Matrix4f tcpMatrix = tcp->getPoseInRootFrame();
        IceInternal::Handle<FrameType> position =
            new FrameType(tcpMatrix, robot->getRootNode()->getName(), robot->getName());
        position->changeFrame(robot, frameName);
 
        JSONObjectPtr object = new JSONObject;
        object->serializeIceObject(position);
        return object->asString(true);
    }
    else
    {
        return "{}";
    }
}
 
void
RobotViewerWidgetController::updateState()
{
    if (!robot)
    {
        return;
    }
 
    std::string kinematicChainName = ui.kinematicChainComboBox->currentText().toStdString();
    VirtualRobot::RobotNodeSetPtr robotNodeSet;
    if (ui.kinematicChainComboBox->currentIndex() > 0)
    {
        robotNodeSet = robot->getRobotNodeSet(kinematicChainName);
    }
 
    std::string frameName = ui.frameComboBox->currentText().toStdString();
    if (ui.frameComboBox->currentIndex() <= 0)
    {
        frameName = "Global";
    }
 
    int selectedOutputType = ui.outputTypeComboBox->currentIndex();
    if (selectedOutputType < 0 || selectedOutputType > eRobotStateOutputTypeSize)
    {
        selectedOutputType = eJointConfiguration;
    }
    RobotStateOutputType outputType = static_cast<RobotStateOutputType>(selectedOutputType);
 
    // Set the locale so that the floats get converted correctly
    const char* oldLocale = std::setlocale(LC_ALL, "en_US.UTF-8");
    std::string tcpName =
        ui.tcpComboBox->currentIndex() == 0 ? "" : ui.tcpComboBox->currentText().toStdString();
    std::string output;
    switch (outputType)
    {
        case eJointConfiguration:
            output = writeJointConfigurationToJson(*robot, robotNodeSet);
            break;
 
        case eFramedPositionTCP:
            output = writeFramedTCP<FramedPosition>(robot, robotNodeSet, frameName, tcpName);
            break;
 
        case eFramedOrientationTCP:
            output = writeFramedTCP<FramedOrientation>(robot, robotNodeSet, frameName, tcpName);
            break;
 
        case eFramedPoseTCP:
            output = writeFramedTCP<FramedPose>(robot, robotNodeSet, frameName, tcpName);
            break;
 
        default:
            ARMARX_ERROR << "Output type not supported: " << outputType;
            break;
    }
 
    QString jsonOutput = QString::fromStdString(output);
    QPlainTextEdit* previewTextBox = ui.previewTextBox;
    QTextDocument* document = previewTextBox->document();
    if (document->toPlainText() != jsonOutput)
    {
        QScrollBar* scrollBar = previewTextBox->verticalScrollBar();
        int oldScrollValue = scrollBar->value();
 
        document->setPlainText(jsonOutput);
 
        int newScrollValue = std::min(oldScrollValue, scrollBar->maximum());
        scrollBar->setValue(newScrollValue);
    }
 
    std::setlocale(LC_ALL, oldLocale);
}
 
void
RobotViewerWidgetController::onConfigurationChanged()
{
    if (ui.autoUpdateCheckBox->isChecked())
    {
        updateState();
    }
}
 
void
RobotViewerWidgetController::updateRobotVisu()
{
    std::unique_lock lock(*mutex3D);
 
    if (!robotStateComponentPrx || !robot)
    {
        return;
    }
 
    //    try
    //    {
    //        RemoteRobot::synchronizeLocalClone(robot, robotStateComponentPrx);
    //    }
    //    catch (...)
    //    {
    //        ARMARX_INFO << deactivateSpam(5) << "Robot synchronization failed";
    //        return;
    //    }
 
    Eigen::Matrix4f gp = robot->getGlobalPose();
    QString roboInfo("Robot Pose (global): pos: ");
    roboInfo += QString::number(gp(0, 3), 'f', 2);
    roboInfo += QString(", ");
    roboInfo += QString::number(gp(1, 3), 'f', 2);
    roboInfo += QString(", ");
    roboInfo += QString::number(gp(2, 3), 'f', 2);
    roboInfo += QString(", rot:");
    Eigen::Vector3f rpy;
    VirtualRobot::MathTools::eigen4f2rpy(gp, rpy);
    roboInfo += QString::number(rpy(0), 'f', 2);
    roboInfo += QString(", ");
    roboInfo += QString::number(rpy(1), 'f', 2);
    roboInfo += QString(", ");
    roboInfo += QString::number(rpy(2), 'f', 2);
    ui.leRobotInfo->setText(roboInfo);
 
    emit configurationChanged();
}
 
void
RobotViewerWidgetController::inflateCollisionModel(int inflationValueMM)
{
    std::unique_lock lock(*mutex3D);
 
    for (auto& model : robot->getCollisionModels())
    {
        model->inflateModel(inflationValueMM);
    }
    ui.label_collisionModelInflationValue->setText(QString::number(inflationValueMM) + " mm");
    setRobotVisu(true);
}
 
void
RobotViewerWidgetController::setMutex3D(RecursiveMutexPtr const& mutex3D)
{
    //ARMARX_IMPORTANT << "set mutex3d :" << mutex3D.get();
    this->mutex3D = mutex3D;
 
    if (debugDrawer)
    {
        debugDrawer->setMutex(mutex3D);
    }
}
 
void
armarx::RobotViewerWidgetController::reportGlobalRobotRootPose(const FramedPoseBasePtr& pose,
                                                               Ice::Long timestamp,
                                                               bool poseChanged,
                                                               const Ice::Current&)
{
    std::unique_lock lock(*mutex3D);
    if (!robotStateComponentPrx || !robot)
    {
        return;
    }
    Eigen::Matrix4f newPose = PosePtr::dynamicCast(pose)->toEigen();
 
    if (!robot->getGlobalPose().isApprox(newPose))
    {
        robot->setGlobalPose(newPose);
    }
}
 
void
armarx::RobotViewerWidgetController::reportJointValues(const NameValueMap& jointAngles,
                                                       Ice::Long,
                                                       bool aValueChanged,
                                                       const Ice::Current&)
{
    std::unique_lock lock(*mutex3D);
 
    if (!robotStateComponentPrx || !robot || !aValueChanged)
    {
        return;
    }
    robot->setJointValues(jointAngles);
}