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;
}
}
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 f5ae8c13b383662411bc4d33399cc87ec3048a79..b8fb2887e5c1c6c129da78cbf6b41cd313750732 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
@@ -68,46 +68,18 @@ namespace armarx::armem::articulated_object
const auto resultCoreInstanceSegmentName =
memoryWriter.addSegment(properties.coreInstanceSegmentName, properties.providerName);
-
- armem::MemoryID refId = armem::MemoryID(resultCoreClassSegment.segmentID);
-
- armem::MemoryID id;
- id.setCoreSegmentID(refId); // listen to all provider segments!
-
- updateKnownObjects();
- memoryNameSystem.subscribe(id, this, &Writer::updateKnownObjects);
- }
-
- void
- Writer::updateKnownObject(const armem::MemoryID& snapshotId)
- {
- arondto::RobotDescription aronArticulatedObjectDescription;
- // aronArticulatedObjectDescription.fromAron(snapshotId.ent);
-
- // TODO(fabian.reister): implement
}
- void
- Writer::updateKnownObjects(const armem::MemoryID& subscriptionID,
- const std::vector<armem::MemoryID>& snapshotIDs)
- {
- ARMARX_INFO << "New objects available!";
- updateKnownObjects();
- }
-
- void
- Writer::updateKnownObjects()
- {
- knownObjects = queryDescriptions(Time::Now());
-
- ARMARX_INFO << "Known articulated objects " << simox::alg::get_keys(knownObjects);
- }
std::optional<armem::MemoryID>
Writer::storeOrGetClass(const ArticulatedObject& obj) const
{
ARMARX_TRACE;
+ // key: name of object: RobotDescription::name
+ const std::unordered_map<std::string, MemoryID> knownObjects = queryDescriptions(Time::Now());
+ ARMARX_VERBOSE << "Known articulated objects " << simox::alg::get_keys(knownObjects);
+
const auto objectId = knownObjects.find(obj.description.name);
// check if exists
@@ -172,9 +144,6 @@ namespace armarx::armem::articulated_object
return std::nullopt;
}
- // update cache (TODO: likely remove this)
- knownObjects[obj.description.name] = updateResult.snapshotID;
-
return updateResult.snapshotID;
}
@@ -276,7 +245,7 @@ namespace armarx::armem::articulated_object
if (not classId)
{
ARMARX_WARNING << "Could not get class id for object " << obj.description.name << "! "
- << "Known classes are " << simox::alg::get_keys(knownObjects);
+ << "Known classes are " << simox::alg::get_keys(queryDescriptions(Time::Now()));
return false;
}
@@ -333,7 +302,7 @@ namespace armarx::armem::articulated_object
}
std::unordered_map<std::string, armem::MemoryID>
- Writer::queryDescriptions(const armem::Time& timestamp)
+ Writer::queryDescriptions(const armem::Time& timestamp) const
{
// Query all entities from provider.
armem::client::query::Builder qb;
@@ -343,7 +312,7 @@ namespace armarx::armem::articulated_object
.coreSegments().withName(properties.coreClassSegmentName)
.providerSegments().all()
.entities().all()
- .snapshots().latest(); // TODO beforeTime(timestamp);
+ .snapshots().beforeTime(timestamp);
// clang-format on
const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
index 568bec35dad164f665cd41f9067d92b16693e380..c31f5ce72eb4c95038b82e01b3ea73641352544d 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
@@ -59,14 +59,9 @@ namespace armarx::armem::articulated_object
private:
std::optional<armem::MemoryID> storeOrGetClass(const ArticulatedObject& obj) const;
- void updateKnownObjects(const armem::MemoryID& subscriptionID,
- const std::vector<armem::MemoryID>& snapshotIDs);
- void updateKnownObjects();
- void updateKnownObject(const armem::MemoryID& snapshotId);
-
// TODO duplicate
std::unordered_map<std::string, armem::MemoryID>
- queryDescriptions(const armem::Time& timestamp);
+ queryDescriptions(const armem::Time& timestamp) const;
std::optional<robot_state::description::RobotDescription>
getRobotDescription(const armarx::armem::wm::Memory& memory) const;
std::unordered_map<std::string, armem::MemoryID>
@@ -89,9 +84,6 @@ namespace armarx::armem::articulated_object
armem::client::Reader memoryReader;
mutable std::mutex memoryReaderMutex;
-
- // key: name of object: RobotDescription::name
- mutable std::unordered_map<std::string, MemoryID> knownObjects;
};