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/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)
    {
        const auto descriptions = queryDescriptions(timestamp, providerName);
 
        ARMARX_INFO << "Found " << descriptions.size() << " articulated object descriptions";
 
        const auto it = std::find_if(
            descriptions.begin(),
            descriptions.end(),
            [&](const armem::articulated_object::ArticulatedObjectDescription& desc) -> bool
            { return desc.name == typeName; });
 
        if (it == descriptions.end())
        {
            ARMARX_WARNING << "Description for articulate object with type <" << typeName
                           << "> not (yet) available!";
            return nullptr;
        }
 
        ARMARX_DEBUG << "Description for articulate object with type <" << typeName
                     << "> available!";
 
        auto obj = VirtualRobot::RobotIO::loadRobot(it->xml.toSystemPath(), loadMode);
 
        if (not obj)
        {
            ARMARX_WARNING << "Failed to load description for articulated object <" << typeName
                           << ">!";
            return nullptr;
        }
 
        obj->setName(instanceName);
        obj->setType(it->name);
 
        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