From 91ca0ac159ac4da6f602f06f668c740a01fa598f Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Mon, 27 Jan 2025 19:40:43 +0100
Subject: [PATCH] fixing example
---
.../ArticulatedObjectLocalizerExample.cpp | 45 +++++++++++++++----
.../client/articulated_object/Reader.cpp | 2 +-
2 files changed, 38 insertions(+), 9 deletions(-)
diff --git a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
index 35f546692..1f812f23a 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 55a101582..4feb69e76 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;
}
}
--
GitLab