diff --git a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp index 77943d887c6833b87a730b1ce45b926273795ad2..f0e3077422c3d81e044806ddb4e12391424ab67e 100644 --- a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp +++ b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp @@ -32,6 +32,7 @@ #include <VirtualRobot/VirtualRobot.h> #include <VirtualRobot/XML/RobotIO.h> +#include "ArmarXCore/core/time/Clock.h" #include <ArmarXCore/core/PackagePath.h> #include <ArmarXCore/core/exceptions/local/ExpressionException.h> #include <ArmarXCore/core/logging/Logging.h> @@ -135,7 +136,7 @@ namespace armarx::articulated_object } auto obj = - VirtualRobot::RobotIO::loadRobot(ArmarXDataPath::resolvePath(it->xml.serialize().path), + VirtualRobot::RobotIO::loadRobot(it->xml.toSystemPath(), VirtualRobot::RobotIO::eStructure); if (not obj) @@ -163,6 +164,11 @@ namespace armarx::articulated_object } } + const auto state = articulatedObjectReaderPlugin->get().queryState(articulatedObject->getType() + "/" + articulatedObject->getName(), Clock::Now()); + + ARMARX_CHECK(state.has_value()); + articulatedObject->setGlobalPose(state->globalPose.matrix()); + ARMARX_DEBUG << "Reporting articulated objects"; const armem::Time now = armem::Time::Now(); @@ -179,7 +185,7 @@ namespace armarx::articulated_object jointValue = node->unscaleJointValue(k, 0, 1); } - articulatedObject->setGlobalPose(simox::math::pose(Eigen::Vector3f(1000, 0, 0))); + // articulatedObject->setGlobalPose(simox::math::pose(Eigen::Vector3f(1000, 0, 0))); articulatedObject->setJointValues(jointValues); auto& articulatedObjectWriter = articulatedObjectWriterPlugin->get(); diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp index 80c3168c5fc5e7c893c2813082171e5cd9c6319b..18d7aa50afd11ba59e9b5d3bb95ceda0a942fe58 100644 --- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp +++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp @@ -231,6 +231,7 @@ namespace armarx::armem::articulated_object cs.dataset = objectId.dataset(); objectInstance.pose.objectID = cs; + objectInstance.pose.providerName = properties.providerName; update.instancesData = {objectInstance.toAron()}; update.referencedTime = timestamp; diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp index 514096731b83167b902dfe4198deae82725837ed..b9abf459ec14789b7e4fe3fccf42ba73db0f5eed 100644 --- a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp +++ b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp @@ -70,6 +70,11 @@ namespace armarx::armem::server::obj::instance std::vector<viz::Layer> layers; for (const auto& [name, poses] : objectPoses) { + if(name.empty()) + { + continue; + } + auto poseHistoryMap = poseHistories.find(name); if (poseHistoryMap != poseHistories.end()) { @@ -92,6 +97,12 @@ namespace armarx::armem::server::obj::instance std::map<std::string, viz::Layer> stage; for (size_t i = 0; i < objectPoses.size(); ++i) { + if(objectPoses.at(i).providerName.empty()) + { + ARMARX_INFO << "Object pose provider not set!"; + continue; + } + visualizeObjectPose(getLayer(objectPoses.at(i).providerName, stage), objectPoses.at(i), poseHistories.at(i),