SetDesiredPoseDialog.cpp

Go to the documentation of this file.

#include "SetDesiredPoseDialog.h"
#include <RobotComponents/gui-plugins/RobotIKPlugin/ui_SetDesiredPoseDialog.h>
 
#include <ArmarXCore/core/util/StringHelpers.h>
 
#include <QMessageBox>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
static std::vector<std::string> expectedKeys({"agent", "frame", "qw", "qx", "qy", "qz", "x", "y", "z"});
 
SetDesiredPoseDialog::SetDesiredPoseDialog(QWidget* parent) :
    QDialog(parent),
    ui(new Ui::SetDesiredPoseDialog)
{
    ui->setupUi(this);
 
    connect(ui->pushButton_formatJSON, SIGNAL(clicked()), this, SLOT(formatInput()));
    connect(ui->plainTextEdit, SIGNAL(textChanged()), this, SLOT(checkJSON()));
    connect(ui->buttonBox->button(QDialogButtonBox::Ok), SIGNAL(clicked()), this, SLOT(parseInputAndSetPose()));
 
    checkJSON();
}
 
SetDesiredPoseDialog::~SetDesiredPoseDialog()
{
    delete ui;
}
 
armarx::FramedPosePtr SetDesiredPoseDialog::getDesiredPose()
{
    return result;
}
 
void SetDesiredPoseDialog::checkJSON()
{
    std::string text = ui->plainTextEdit->toPlainText().toUtf8().data();
 
    armarx::StructuralJsonParser parser(text, false);
    parser.parse();
 
    // Check for keys
    std::string errorMsgPart;
    armarx::JsonObjectPtr json = std::dynamic_pointer_cast<armarx::JsonObject>(parser.parsedJson);
    if (json)
    {
        std::vector<std::string> keys = json->getKeys();
        for (std::string& k : expectedKeys)
        {
            if (std::find(keys.begin(), keys.end(), k) == keys.end())
            {
                errorMsgPart += k;
                errorMsgPart += " ,";
            }
        }
    }
    else
    {
        for (std::string& k : expectedKeys)
        {
            errorMsgPart += k;
            errorMsgPart += " ,";
        }
    }
    if (errorMsgPart.size() >= 2)
    {
        errorMsgPart = errorMsgPart.substr(0, errorMsgPart.size() - 2);
    }
 
    // Set error message
    if (!parser.iserr() && errorMsgPart.empty())
    {
        ui->buttonBox->button(QDialogButtonBox::Ok)->setEnabled(true);
        ui->label_jsonValid->setStyleSheet("QLabel { color : green; }");
        ui->label_jsonValid->setText("JSON-Format: valid");
    }
    else
    {
        ui->buttonBox->button(QDialogButtonBox::Ok)->setEnabled(false);
        ui->label_jsonValid->setStyleSheet("QLabel { color : red; }");
        if (errorMsgPart.empty())
        {
            ui->label_jsonValid->setText("JSON-Format: not valid, Error at " + QString(parser.getlongerrposstr().c_str()));
        }
        else
        {
 
            ui->label_jsonValid->setText("JSON-Format: not valid, Missing following keys: \n{ " + QString(errorMsgPart.c_str()) + " }");
        }
    }
}
 
void SetDesiredPoseDialog::formatInput()
{
    std::string text = ui->plainTextEdit->toPlainText().toUtf8().data();
    if (text.empty())
    {
        text += "{\n}"; // Minimal requirements for successful parsing
    }
    armarx::JsonObjectPtr json;
    if (stringToJSON(text, json))
    {
        std::vector<std::string> keys = json->getKeys();
        for (std::string& k : expectedKeys)
        {
            if (std::find(keys.begin(), keys.end(), k) == keys.end())
            {
                if (k == "agent" || k == "frame")
                {
                    json->add(k, armarx::JsonValue(""));
                }
                else
                {
                    json->add(k, armarx::JsonValue(0.0f));
                }
            }
        }
        ui->plainTextEdit->setPlainText(QString(json->toJsonString(2).c_str()));
    }
    else
    {
        QMessageBox msgBox;
        msgBox.setText("Input cannot be parsed into json and therefore not formated.");
        msgBox.setIcon(QMessageBox::Warning);
        msgBox.setStandardButtons(QMessageBox::Ok);
        msgBox.setDefaultButton(QMessageBox::Ok);
        msgBox.exec();
    }
}
 
void SetDesiredPoseDialog::parseInputAndSetPose()
{
    std::string text = ui->plainTextEdit->toPlainText().toUtf8().data();
    armarx::JsonObjectPtr json;
    ARMARX_CHECK_EXPRESSION(stringToJSON(text, json));
 
    float x = armarx::toFloat((std::dynamic_pointer_cast<armarx::JsonValue>(json->get("x")))->rawValue());
    float y = armarx::toFloat((std::dynamic_pointer_cast<armarx::JsonValue>(json->get("y")))->rawValue());
    float z = armarx::toFloat((std::dynamic_pointer_cast<armarx::JsonValue>(json->get("z")))->rawValue());
    float qw = armarx::toFloat((std::dynamic_pointer_cast<armarx::JsonValue>(json->get("qw")))->rawValue());
    float qx = armarx::toFloat((std::dynamic_pointer_cast<armarx::JsonValue>(json->get("qx")))->rawValue());
    float qy = armarx::toFloat((std::dynamic_pointer_cast<armarx::JsonValue>(json->get("qy")))->rawValue());
    float qz = armarx::toFloat((std::dynamic_pointer_cast<armarx::JsonValue>(json->get("qz")))->rawValue());
    std::string agent = (std::dynamic_pointer_cast<armarx::JsonValue>(json->get("agent")))->asString();
    std::string frame = (std::dynamic_pointer_cast<armarx::JsonValue>(json->get("frame")))->asString();
 
    armarx::Vector3Ptr pos = new armarx::Vector3(x, y, z);
    armarx::QuaternionPtr quat = new armarx::Quaternion(qw, qx, qy, qz);
 
    if (pos && quat)
    {
        armarx::FramedPosePtr pose = new armarx::FramedPose(pos, quat, frame, agent);
        this->result = pose;
    }
}
 
 
bool SetDesiredPoseDialog::stringToJSON(std::string string, armarx::JsonObjectPtr& result) const
{
    armarx::StructuralJsonParser parser(string, false);
    parser.parse();
    result = std::dynamic_pointer_cast<armarx::JsonObject>(parser.parsedJson);
    return !parser.iserr();
}