ArticulatedObjectWriter.cpp

Go to the documentation of this file.

#include "ArticulatedObjectWriter.h"
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <VirtualRobot/Robot.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
 
namespace armarx::armem::articulated_object
{
    armem::articulated_object::ArticulatedObject
    convert(const VirtualRobot::Robot& obj, const armem::Time& timestamp)
    {
        ARMARX_DEBUG << "Filename is " << obj.getFilename();
 
        const std::vector<std::string> packages =
            armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
        const std::string package = armarx::ArmarXDataPath::getProject(packages, obj.getFilename());
 
        // make sure that the relative path is without the 'package/' prefix
        const std::string fileRelPath = [&obj, &package]() -> std::string
        {
            if (simox::alg::starts_with(obj.getFilename(), package))
            {
                // remove "package" + "/"
                const std::string fixedFilename = obj.getFilename().substr(package.size() + 1, -1);
                return fixedFilename;
            }
 
            return obj.getFilename();
        }();
 
        return armem::articulated_object::ArticulatedObject{
            .description = {.name = obj.getType(),
                            .xml = PackagePath(
                                armarx::ArmarXDataPath::getProject({package}, fileRelPath),
                                obj.getFilename())},
            .instance = obj.getName(),
            .config = {.timestamp = timestamp,
                       .globalPose = Eigen::Affine3f(obj.getRootNode()->getGlobalPose()),
                       .jointMap = obj.getJointValues(),
                       .proprioception = std::nullopt},
            .timestamp = timestamp};
    }
 
    bool
    ArticulatedObjectWriter::storeArticulatedObject(const VirtualRobot::RobotPtr& articulatedObject,
                                                    const armem::Time& timestamp)
    {
 
        ARMARX_CHECK_NOT_NULL(articulatedObject);
 
        armarx::armem::articulated_object::ArticulatedObject armemArticulatedObject =
            convert(*articulatedObject, Time::Now());
 
        return store(armemArticulatedObject);
    }
} // namespace armarx::armem::articulated_object