diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp index a2b0ddd6b2985997156cfa6c82998de249409e5a..a7938a92e3810781adfe2ff8e12aced95fcf9c86 100644 --- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp +++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp @@ -25,10 +25,11 @@ namespace armarx::armem::articulated_object { VirtualRobot::RobotPtr ArticulatedObjectReader::getArticulatedObject( - const std::string& name, - const armem::Time& timestamp) + const std::string& typeName, + const armem::Time& timestamp, + const std::string& instanceName) { - const auto descriptions = queryDescriptions(armem::Time::Now()); + const auto descriptions = queryDescriptions(timestamp); ARMARX_INFO << "Found " << descriptions.size() << " articulated object descriptions"; @@ -36,15 +37,15 @@ namespace armarx::armem::articulated_object descriptions.begin(), descriptions.end(), [&](const armem::articulated_object::ArticulatedObjectDescription & desc) -> bool - { return desc.name == name; }); + { return desc.name == typeName; }); if (it == descriptions.end()) { - ARMARX_WARNING << "Articulated object " << name << " not (yet) available"; + ARMARX_WARNING << "Description for articulate object with type <" << typeName << "> not (yet) available!"; return nullptr; } - ARMARX_DEBUG << "Object " << name << " available"; + ARMARX_DEBUG << "Description for articulate object with type <" << typeName << "> available!"; auto obj = VirtualRobot::RobotIO::loadRobot( it->xml.toSystemPath(), @@ -52,14 +53,29 @@ namespace armarx::armem::articulated_object if (not obj) { - ARMARX_WARNING << "Failed to load object: " << name; + ARMARX_WARNING << "Failed to load description for articulated object <" << typeName << ">!"; return nullptr; } - obj->setName(""); + obj->setName(instanceName); obj->setType(it->name); return obj; } + bool ArticulatedObjectReader::synchronizeArticulatedObject(VirtualRobot::Robot& object, const armem::Time& timestamp) { + const auto objectState = queryState(object.getType() + "/" + object.getName(), timestamp); + 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 diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.h index 65e8f2fba2ba962429e040e1d0e1eefc514e2355..02a1b905454db60354a1e8173994eeb63b98f1a0 100644 --- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.h +++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.h @@ -11,7 +11,11 @@ namespace armarx::armem::articulated_object using Reader::Reader; VirtualRobot::RobotPtr getArticulatedObject( - const std::string& name, - const armem::Time& timestamp); + const std::string& typeName, + const armem::Time& timestamp, + const std::string& instanceName = ""); + bool synchronizeArticulatedObject( + VirtualRobot::Robot& object, + const armem::Time& timestamp); }; } // namespace armarx::armem::articulated_object