ArticulatedObjectReader.cpp

Go to the documentation of this file.

#include "ArticulatedObjectReader.h"
 
#include <mutex>
#include <optional>
 
#include <VirtualRobot/XML/BaseIO.h>
#include <VirtualRobot/XML/RobotIO.h>
 
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
 
#include <RobotAPI/libraries/ArmarXObjects/ObjectInfo.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/robot_conversions.h>
 
namespace armarx::armem::articulated_object
{
 
    VirtualRobot::RobotPtr
    ArticulatedObjectReader::getArticulatedObject(const std::string& typeName,
                                                  const armem::Time& timestamp,
                                                  const std::optional<std::string>& providerName,
                                                  const std::string& instanceName,
                                                  VirtualRobot::RobotIO::RobotDescription loadMode)
    {
        ARMARX_VERBOSE << "Getting `" << typeName + "/" + instanceName << "`";
 
        const std::optional<ArticulatedObject> articulatedObjectDescription =
            get(typeName + "/" + instanceName, timestamp, providerName);
        ARMARX_CHECK_NOT_NULL(articulatedObjectDescription)
            << "Failed to get articulated object `" << typeName << "/" << instanceName << "`";
 
        auto obj = VirtualRobot::RobotIO::loadRobot(
            articulatedObjectDescription->description.xml.toSystemPath(), loadMode);
 
        if (not obj)
        {
            ARMARX_WARNING << "Failed to load articulated object `" << typeName << "/"
                           << instanceName << "` from file `"
                           << articulatedObjectDescription->description.xml.toSystemPath() << "`.";
 
            return nullptr;
        }
 
        obj->setName(instanceName);
        obj->setType(typeName);
 
        return obj;
    }
 
    bool
    ArticulatedObjectReader::synchronizeArticulatedObject(
        VirtualRobot::Robot& object,
        const armem::Time& timestamp,
        const std::optional<std::string>& providerName)
    {
        const auto objectState =
            queryState(object.getType() + "/" + object.getName(), timestamp, providerName);
        if (not objectState)
        {
            ARMARX_VERBOSE << deactivateSpam(5) << "Querying object state failed for object `"
                           << object.getName() << "` "
                           << "(type `" << object.getType() << "`)!";
            return false;
        }
 
        object.setJointValues(objectState->jointMap);
        object.setGlobalPose(objectState->globalPose.matrix());
 
        return true;
    }
 
} // namespace armarx::armem::articulated_object