From e3713eb4d95680f87e9f812beb6fee159be7fff1 Mon Sep 17 00:00:00 2001 From: Fabian Reister <fabian.reister@kit.edu> Date: Wed, 17 May 2023 12:11:49 +0200 Subject: [PATCH] fix: articulated objects in memory --- .../ArticulatedObjectLocalizerExample.cpp | 10 ++++++++-- .../client/articulated_object/Writer.cpp | 1 + .../libraries/armem_objects/server/instance/Visu.cpp | 11 +++++++++++ 3 files changed, 20 insertions(+), 2 deletions(-) diff --git a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp index 77943d887..f0e307742 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 80c3168c5..18d7aa50a 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 514096731..b9abf459e 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), -- GitLab