ArticulatedObjectWriter.cpp

Go to the documentation of this file.

#include "ArticulatedObjectWriter.h"
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <SimoxUtility/algorithm/string/string_tools.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#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
{
 
    // Slow version with filesystem access. It runs CMake for each project / environment variable.
    static armem::articulated_object::ArticulatedObject
    convert(const VirtualRobot::Robot& obj, const armem::Time& timestamp)
    {
        ARMARX_DEBUG << "Filename is " << obj.getFilename();
 
        // this is very inefficient. It runs CMake for each project
        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()),
                            .visualization = {},
                            .info = {}},
            .instance = obj.getName(),
            .config = {.timestamp = timestamp,
                       .globalPose = Eigen::Isometry3f(obj.getRootNode()->getGlobalPose()),
                       .jointMap = obj.getJointValues(),
                       .proprioception = std::nullopt},
            .timestamp = timestamp};
    }
 
    // fast version without filesystem (CMake) access
    static armem::articulated_object::ArticulatedObject
    convertWithoutPackagePath(const VirtualRobot::Robot& obj, const armem::Time& timestamp)
    {
        ARMARX_DEBUG << "Filename is " << obj.getFilename();
 
        return armem::articulated_object::ArticulatedObject{
            .description = {.name = obj.getType(), .xml = {}, .visualization = {}, .info = {}},
            .instance = obj.getName(),
            .config = {.timestamp = timestamp,
                       .globalPose = Eigen::Isometry3f(obj.getRootNode()->getGlobalPose()),
                       .jointMap = obj.getJointValues(),
                       .proprioception = std::nullopt},
            .timestamp = timestamp};
    }
 
    bool
    ArticulatedObjectWriter::storeArticulatedObjectWithObjectClass(
        const VirtualRobot::RobotPtr& articulatedObject,
        const armem::Time& timestamp,
        const bool isStatic)
    {
 
        ARMARX_CHECK_NOT_NULL(articulatedObject);
 
        armarx::armem::articulated_object::ArticulatedObject armemArticulatedObject =
            convert(*articulatedObject, timestamp);
 
        return store(armemArticulatedObject, isStatic);
    }
 
    bool
    ArticulatedObjectWriter::storeArticulatedObject(const VirtualRobot::RobotPtr& articulatedObject,
                                                    const armem::Time& timestamp,
                                                    const bool isStatic)
    {
 
        ARMARX_CHECK_NOT_NULL(articulatedObject);
 
        armarx::armem::articulated_object::ArticulatedObject armemArticulatedObject =
            convertWithoutPackagePath(*articulatedObject, timestamp);
 
        return storeInstance(armemArticulatedObject, isStatic);
    }
 
} // namespace armarx::armem::articulated_object