TypedDataDisplayVisitor.cpp

Go to the documentation of this file.

#include "TypedDataDisplayVisitor.h"
 
#include <iomanip> // std::setprecision
 
#include <SimoxUtility/algorithm/string.h>
#include <SimoxUtility/math/pose/pose.h>
 
#include <ArmarXCore/core/logging/Logging.h>
 
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/aron/converter/eigen/EigenConverter.h>
#include <RobotAPI/libraries/aron/core/data/variant/All.h>
 
#include "DataDisplayVisitor.h"
 
namespace armarx::aron
{
 
    std::string
    TypedDataDisplayVisitor::getValue(const type::VariantPtr& type, const data::VariantPtr& data)
    {
        TypedDataDisplayVisitor v;
        data::visit(v, data, type);
        return v.value.str();
    }
 
    TypedDataDisplayVisitor::TypedDataDisplayVisitor()
    {
    }
 
    void
    TypedDataDisplayVisitor::visitDict(const data::VariantPtr& data, const type::VariantPtr& type)
    {
        value << DataDisplayVisitor::getValue(data);
    }
 
    void
    TypedDataDisplayVisitor::visitObject(const data::VariantPtr& data, const type::VariantPtr& type)
    {
        value << DataDisplayVisitor::getValue(data);
    }
 
    void
    TypedDataDisplayVisitor::visitList(const data::VariantPtr& data, const type::VariantPtr& type)
    {
        value << DataDisplayVisitor::getValue(data);
    }
 
    void
    TypedDataDisplayVisitor::visitTuple(const data::VariantPtr& data, const type::VariantPtr& type)
    {
        value << DataDisplayVisitor::getValue(data);
    }
 
    void
    TypedDataDisplayVisitor::visitBool(const data::VariantPtr& data, const type::VariantPtr& type)
    {
        value << DataDisplayVisitor::getValue(data);
    }
 
    void
    TypedDataDisplayVisitor::visitDouble(const data::VariantPtr& data, const type::VariantPtr& type)
    {
        setStreamPrecision();
        value << DataDisplayVisitor::getValue(data);
    }
 
    void
    TypedDataDisplayVisitor::visitFloat(const data::VariantPtr& data, const type::VariantPtr& type)
    {
        setStreamPrecision();
        value << DataDisplayVisitor::getValue(data);
    }
 
    void
    TypedDataDisplayVisitor::visitInt(const data::VariantPtr& data, const type::VariantPtr& type)
    {
        value << DataDisplayVisitor::getValue(data);
    }
 
    void
    TypedDataDisplayVisitor::visitLong(const data::VariantPtr& data, const type::VariantPtr& type)
    {
        value << DataDisplayVisitor::getValue(data);
    }
 
    void
    TypedDataDisplayVisitor::visitString(const data::VariantPtr& data, const type::VariantPtr& type)
    {
        value << DataDisplayVisitor::getValue(data);
    }
 
    void
    TypedDataDisplayVisitor::visitIntEnum(const data::VariantPtr& data,
                                          const type::VariantPtr& type)
    {
        type::IntEnumPtr enumType = type::IntEnum::DynamicCast(type);
        data::IntPtr enumData = data::Int::DynamicCast(data);
 
        if (enumType == nullptr or enumData == nullptr)
        {
            ARMARX_WARNING << "Enum Type or Data is NULL";
            return;
        }
 
        std::string name = enumType->getValueName(enumData->getValue());
        value << name;
    }
 
    /*void TypedDataDisplayVisitor::visitDateTime(const data::VariantPtr& data, const type::VariantPtr& type)
    {
        auto l = data::Long::DynamicCastAndCheck(data);
        armem::Time time { armem::Duration::MicroSeconds(l->getValue()) };
        value << armem::toDateTimeMilliSeconds(time);
    }*/
 
 
    template <typename ScalarT>
    void
    TypedDataDisplayVisitor::processMatrix(const type::Matrix& type, const data::NDArray& data)
    {
        Eigen::Map<Eigen::Matrix<ScalarT, Eigen::Dynamic, Eigen::Dynamic>> m(
            reinterpret_cast<ScalarT*>(data.getData()),
            data.getShape().at(0),
            data.getShape().at(1));
        value << m.format(eigenIof);
    }
 
    void
    TypedDataDisplayVisitor::processQuaternion(const data::NDArray& data)
    {
        const Eigen::Quaternionf quat =
            aron::data::converter::AronEigenConverter::ConvertToQuaternionf(data);
        setStreamPrecision();
        value << quat.w() << coeffSep << "|" << coeffSep << quat.x() << coeffSep << quat.y()
              << coeffSep << quat.z();
    }
 
    void
    TypedDataDisplayVisitor::processPose(const type::Matrix&, const data::NDArray& d)
    {
        const Eigen::IOFormat eigenIof(Eigen::StreamPrecision, 0, " ", "\n", "( ", " )", "", "");
        const std::string cdot = "\u00B7"; // center dot: https://unicode-table.com/de/00B7/
 
        auto getLines = [&](auto&& mat)
        {
            std::stringstream ss;
            setStreamPrecision(ss);
            ss << mat.format(eigenIof);
            std::vector<std::string> lines = simox::alg::split(ss.str(), "\n");
            ARMARX_CHECK_EQUAL(lines.size(), 3);
            return lines;
        };
 
        const Eigen::Matrix4f pose =
            aron::data::converter::AronEigenConverter::ConvertToMatrix4f(d);
        const std::vector<std::string> r = getLines(simox::math::orientation(pose));
        const std::vector<std::string> t = getLines(simox::math::position(pose));
 
        std::vector<std::string> lines;
        lines.push_back(r.at(0) + "      \t" + t.at(0));
        lines.push_back(r.at(1) + " " + cdot + " x + \t" + t.at(1));
        lines.push_back(r.at(2) + "      \t" + t.at(2));
 
        value << simox::alg::join(lines, "\n");
    }
 
    void
    TypedDataDisplayVisitor::processPosition(const type::Matrix&, const data::NDArray& d)
    {
        const Eigen::Vector3f pos = aron::data::converter::AronEigenConverter::ConvertToVector3f(d);
        setStreamPrecision();
        value << pos.format(eigenIof);
    }
 
    void
    TypedDataDisplayVisitor::visitMatrix(const data::VariantPtr& data, const type::VariantPtr& type)
    {
        value << DataDisplayVisitor::getValue(data);
    }
 
    void
    TypedDataDisplayVisitor::visitQuaternion(const data::VariantPtr& data, const type::VariantPtr&)
    {
        auto d = *data::NDArray::DynamicCastAndCheck(data);
        processQuaternion(d);
    }
 
    void
    TypedDataDisplayVisitor::visitImage(const data::VariantPtr& data, const type::VariantPtr& type)
    {
        // aron::typenavigator::ImagePixelType pixelType = ImageNavigator::pixelTypeFromName(data.getType());
        //value << DataDisplayVisitor::getValue(data) << " pixel type: " << data.getType()";
        value << DataDisplayVisitor::getValue(data);
    }
 
    void
    TypedDataDisplayVisitor::visitPointCloud(const data::VariantPtr& data,
                                             const type::VariantPtr& type)
    {
        value << DataDisplayVisitor::getValue(data);
    }
 
} // namespace armarx::aron