TreeTypedDataVisitor.cpp

Go to the documentation of this file.

#include "TreeTypedDataVisitor.h"
 
#include <iomanip> // std::setprecision
 
#include <SimoxUtility/algorithm/string.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
#include <RobotAPI/libraries/armem/core.h>
#include <RobotAPI/libraries/aron/converter/eigen/EigenConverter.h>
#include <RobotAPI/libraries/aron/core/Exception.h>
 
namespace armarx::armem::gui::instance
{
 
    /*void TreeTypedDataVisitor::streamValueText(const aron::data::Long& data, const aron::type::DateTime& type, std::stringstream& ss) const
    {
        (void) type;
        armem::Time time { armem::Duration::MicroSeconds(data.getValue()) };
        armem::toDateTimeMilliSeconds(time);
        ss << armem::toDateTimeMilliSeconds(time);
    }*/
 
    void
    TreeTypedDataVisitor::streamValueText(const aron::data::NDArray& data,
                                          const aron::type::Matrix& type,
                                          std::stringstream& ss) const
    {
        auto shape = data.getShape();
 
        // TODO: Remove hardcoded stuff
        if (shape.size() == 3)
        {
            if (shape[0] == 3 && shape[1] == 1 && shape[2] == 4)
            {
                streamPositionText(data, type, ss);
                return;
            }
 
            if (shape[0] == 4 && shape[1] == 4 && shape[2] == 4)
            {
                streamPoseText(data, type, ss);
                return;
            }
 
            if (shape[0] == 4 && shape[1] == 1 && shape[2] == 4)
            {
                streamOrientationText(data, type, ss);
                return;
            }
        }
        TreeDataVisitorBase::streamValueText(data, ss); // fallback
    }
 
    void
    TreeTypedDataVisitor::streamPoseText(const aron::data::NDArray& data,
                                         const aron::type::Matrix& type,
                                         std::stringstream& ss) const
    {
        (void)type;
        const Eigen::Matrix4f pose =
            aron::data::converter::AronEigenConverter::ConvertToMatrix4f(data);
        ss << std::setprecision(2) << std::fixed;
        ss << pose.format(
            Eigen::IOFormat(Eigen::StreamPrecision, 0, coeffSep, "\n", "", "", "", ""));
    }
 
    void
    TreeTypedDataVisitor::streamPositionText(const aron::data::NDArray& data,
                                             const aron::type::Matrix& type,
                                             std::stringstream& ss) const
    {
        (void)type;
        const Eigen::Vector3f pos =
            aron::data::converter::AronEigenConverter::ConvertToVector3f(data);
        ss << std::setprecision(2) << std::fixed;
        ss << pos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, "", coeffSep, "", "", "", ""));
    }
 
    void
    TreeTypedDataVisitor::streamOrientationText(const aron::data::NDArray& data,
                                                const aron::type::Matrix& type,
                                                std::stringstream& ss) const
    {
        (void)type;
        const Eigen::Quaternionf quat =
            aron::data::converter::AronEigenConverter::ConvertToQuaternionf(data);
        ss << std::setprecision(2) << std::fixed;
        ss << quat.w() << coeffSep << "|" << coeffSep << quat.x() << coeffSep << quat.y()
           << coeffSep << quat.z();
    }
 
} // namespace armarx::armem::gui::instance