diff --git a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp index 35f54669285b085c2c14fb7929585f2bba6bc0c0..1f812f23af7684ca55576fef2c9a01e5c4838cee 100644 --- a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp +++ b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp @@ -22,6 +22,7 @@ #include "ArticulatedObjectLocalizerExample.h" #include <memory> +#include <optional> #include <Eigen/Geometry> @@ -38,6 +39,9 @@ #include <ArmarXCore/core/logging/Logging.h> #include <ArmarXCore/core/services/tasks/PeriodicTask.h> #include <ArmarXCore/core/time/CycleUtil.h> +#include <ArmarXCore/core/time/Duration.h> +#include <ArmarXCore/core/time/ScopedStopWatch.h> +#include <ArmarXCore/core/time/StopWatch.h> #include <RobotAPI/libraries/armem/core/Time.h> #include <RobotAPI/libraries/armem_objects/types.h> @@ -162,13 +166,32 @@ namespace armarx::articulated_object } } - const auto state = articulatedObjectReaderPlugin->get().queryState( - articulatedObject->getType() + "/" + articulatedObject->getName(), - Clock::Now(), - p.obj.readProviderName); + const armem::robot_state::RobotState state = [this]() + { + // Query state from memory + { + const std::optional<armem::robot_state::RobotState> state = + articulatedObjectReaderPlugin->get().queryState( + articulatedObject->getType() + "/" + articulatedObject->getName(), + Clock::Now(), + p.obj.readProviderName); + + if (state) + { + return state.value(); + } + } + + // The object does not exist in the memory (yet). Therefore, we create an initial state. + armem::robot_state::RobotState state{.timestamp = Clock::Now(), + .globalPose = Eigen::Isometry3f::Identity(), + .jointMap = articulatedObject->getJointValues(), + .proprioception = std::nullopt}; + + return state; + }(); - ARMARX_CHECK(state.has_value()); - articulatedObject->setGlobalPose(state->globalPose.matrix()); + articulatedObject->setGlobalPose(state.globalPose.matrix()); ARMARX_DEBUG << "Reporting articulated objects"; @@ -189,8 +212,14 @@ namespace armarx::articulated_object // articulatedObject->setGlobalPose(simox::math::pose(Eigen::Vector3f(1000, 0, 0))); articulatedObject->setJointValues(jointValues); - auto& articulatedObjectWriter = articulatedObjectWriterPlugin->get(); - articulatedObjectWriter.storeArticulatedObject(articulatedObject, now); + { + core::time::ScopedStopWatch sw( + [this](const armarx::Duration& dur) + { ARMARX_INFO << "Storing object took " << dur << "."; }); + + auto& articulatedObjectWriter = articulatedObjectWriterPlugin->get(); + articulatedObjectWriter.storeArticulatedObject(articulatedObject, now); + } } } // namespace armarx::articulated_object diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp index 55a10158299de3cd3a64883ea44fb957bd0c355a..4feb69e7672a23669680b7997754f111b3b91e9d 100644 --- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp +++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp @@ -352,7 +352,7 @@ namespace armarx::armem::articulated_object } catch (...) { - ARMARX_FATAL << "Failed to obtain robot state"; + ARMARX_VERBOSE << "Failed to obtain robot state"; return std::nullopt; } }