From 0426bede6d22d48adbf25d58c926781700aee6b4 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Fri, 13 Dec 2024 13:58:33 +0100
Subject: [PATCH] ObjectLastSeenAt: update by AM
---
.../core/ObjectLastSeenAt.cpp | 154 ++++++++++--------
1 file changed, 89 insertions(+), 65 deletions(-)
diff --git a/source/armarx/speech/skills/object_last_seen_at/core/ObjectLastSeenAt.cpp b/source/armarx/speech/skills/object_last_seen_at/core/ObjectLastSeenAt.cpp
index 678ad2b..57f87b0 100644
--- a/source/armarx/speech/skills/object_last_seen_at/core/ObjectLastSeenAt.cpp
+++ b/source/armarx/speech/skills/object_last_seen_at/core/ObjectLastSeenAt.cpp
@@ -30,21 +30,21 @@
#include <cctype>
#include <memory>
#include <unordered_map>
+
#include <SimoxUtility/algorithm/string/string_tools.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem/client/query.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
-#include <RobotAPI/libraries/aron/core/aron_conversions.h>
-#include <RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
-#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/aron/core/aron_conversions.h>
#include <armarx/speech/verbalization/object_verbalization.h>
-
#include <armarx/symbolic_planning/armem/symbolic_scene_memory/client/reader/SymbolicSceneReader.h>
#include <range/v3/algorithm/contains.hpp>
@@ -67,9 +67,13 @@ namespace armarx::speech::object_last_seen_at::core
bool
ObjectLastSeenAt::_execute()
{
- if(simox::alg::contains(this->properties.objectName, "-"))
+
+ //TODO: please fix this!!!
+ //either also remove hyphons from class names or from the object name (Timo)
+ if (simox::alg::contains(this->properties.objectName, "-"))
{
- this->properties.objectName = simox::alg::replace_all(this->properties.objectName, "-", " ");
+ this->properties.objectName =
+ simox::alg::replace_all(this->properties.objectName, "-", " ");
}
// we need to check the remote readers and get scene description and all object classes:
@@ -97,9 +101,8 @@ namespace armarx::speech::object_last_seen_at::core
const auto queryLTM = [this, object_name](const std::string& objectIdString)
-> ::armarx::speech::skills::object_last_seen_at::core::QueryResult
{
- ARMARX_INFO
- << "Object " << object_name << " (" << objectIdString
- << ") has no known location in current memory, searching LTM now";
+ ARMARX_INFO << "Object " << object_name << " (" << objectIdString
+ << ") has no known location in current memory, searching LTM now";
ARMARX_CHECK_NOT_NULL(
this->remote.pythonProxy); //make sure the pythonProxy was correctly set
std::string ltm_base_path = this->properties.ltm_base_path;
@@ -124,16 +127,19 @@ namespace armarx::speech::object_last_seen_at::core
};
//method to get multiple past scene description snapshots
- const auto getMultiplePastSceneDescriptionsFromWM = [this]() -> std::map<armarx::DateTime, scene_representation::symbolic_scene::SceneDescription>
+ const auto getMultiplePastSceneDescriptionsFromWM = [this]()
+ -> std::map<armarx::DateTime, scene_representation::symbolic_scene::SceneDescription>
{
ARMARX_CHECK_NOT_NULL(remote.symbolicSceneReader);
const armarx::DateTime timestampWindowStart = armarx::Clock::Now();
auto duration = armarx::Duration::Minutes(this->properties.wm_search_time);
const armarx::DateTime timestampWindowEnd = timestampWindowStart - duration;
const std::map<armarx::DateTime, scene_representation::symbolic_scene::SceneDescription>
- sceneDescriptions = remote.symbolicSceneReader->getSceneDescriptionWithinTimeRange(
- timestampWindowEnd, timestampWindowStart); // as we search from the end forward we need to switch the timestamps
- ARMARX_INFO << "Got scene descriptions of the last " << duration << " ( " << timestampWindowEnd << " - " << timestampWindowStart << " )";
+ sceneDescriptions = remote.symbolicSceneReader->getSceneDescriptionWithinTimeRange(
+ timestampWindowEnd,
+ timestampWindowStart); // as we search from the end forward we need to switch the timestamps
+ ARMARX_INFO << "Got scene descriptions of the last " << duration << " ( "
+ << timestampWindowEnd << " - " << timestampWindowStart << " )";
ARMARX_INFO << "Number of scene descriptions: " << sceneDescriptions.size();
return sceneDescriptions;
};
@@ -144,20 +150,22 @@ namespace armarx::speech::object_last_seen_at::core
{
auto object_description_name = object.id;
std::transform(object_description_name.begin(),
- object_description_name.end(),
- object_description_name.begin(),
- [](unsigned char c)
- { return std::tolower(c); }); //convert description name to lowercase
-
- if (object_description_name.find(object_name) != std::string::npos && object.objectAt != "(unknown)")
+ object_description_name.end(),
+ object_description_name.begin(),
+ [](unsigned char c)
+ { return std::tolower(c); }); //convert description name to lowercase
+
+ if (object_description_name.find(object_name) != std::string::npos &&
+ object.objectAt != "(unknown)")
{
ARMARX_INFO << "Found object in WM of symbolic scene memory";
- const std::string spokenName = verbalization::getObjectSpokenNames(knownObjectClasses, object.id)[0];
+ const std::string spokenName =
+ verbalization::getObjectSpokenNames(knownObjectClasses, object.id)[0];
const std::string preposition = verbalization::getPreposition(object.objectAt);
- const std::string spokenLocationName
- = verbalization::getObjectSpokenNames(knownObjectClasses, object.objectAt)[0];
- const std::string answer = "I have last seen the " + spokenName + " " + preposition +
- " the " + spokenLocationName;
+ const std::string spokenLocationName =
+ verbalization::getObjectSpokenNames(knownObjectClasses, object.objectAt)[0];
+ const std::string answer = "I have last seen the " + spokenName + " " +
+ preposition + " the " + spokenLocationName;
this->subskills.say(answer);
try
@@ -165,62 +173,77 @@ namespace armarx::speech::object_last_seen_at::core
//obtain the object pose from the object memory:
const armem::MemoryID sourceId = armem::MemoryID::fromString(object.source);
const armem::MemoryID coreSegmentID("Object", "Instance");
- armem::client::Reader reader = this->remote.memoryNameSystem.getReader(coreSegmentID);
+ armem::client::Reader reader =
+ this->remote.memoryNameSystem.getReader(coreSegmentID);
armem::client::QueryBuilder builder;
- builder.coreSegments().withName(coreSegmentID.coreSegmentName)
- .providerSegments().withName(sourceId.providerSegmentName)
- .entities().withName(sourceId.entityName)
- .snapshots().atTime(sourceId.timestamp);
+ builder.coreSegments()
+ .withName(coreSegmentID.coreSegmentName)
+ .providerSegments()
+ .withName(sourceId.providerSegmentName)
+ .entities()
+ .withName(sourceId.entityName)
+ .snapshots()
+ .atTime(sourceId.timestamp);
armem::client::QueryResult result = reader.query(builder.buildQueryInput());
if (result.success)
{
armem::wm::Memory& memory = result.memory;
- memory.forEachInstance([this, &sourceId](const armem::wm::EntityInstance& instance)
- {
-
- const armarx::armem::arondto::ObjectInstance i = armarx::armem::arondto::ObjectInstance::FromAron(instance.data());
- objpose::ObjectPose pose;
- armarx::armem::fromAron(i, pose);
-
- // objpose::ObjectPose pose =
- // armarx::aron::fromAron<objpose::ObjectPose>(
- // armarx::armem::arondto::ObjectInstance::FromAron(instance.data())
- // );
-
- auto currentTime = armarx::armem::Time::Now();
- //auto ageOfObjectInformation = currentTime.toSecondsSinceEpoch() - instance.metadata().sentTime.toSecondsSinceEpoch();
-
- armarx::armem::arondto::ObjectInstance objectInstance;
- armarx::objpose::toAron(objectInstance.pose, pose);
-
- armem::MemoryID newObjectPoseClassID = armem::MemoryID("Object", "Class", "PriorKnowledgeData", sourceId.entityName);
-
- armarx::armem::toAron(objectInstance.classID, newObjectPoseClassID);//objectClassId;
- objectInstance.pose.confidence = 1.0; // we are sure about the location
- objectInstance.pose.timestamp =
- armarx::Clock::Now(); // we must update the timestamp
-
- // store the object in the object memory
- ARMARX_CHECK_NOT_NULL(this->remote.objectInstanceWriter);
- ARMARX_CHECK(this->remote.objectInstanceWriter->commitObject(
- objectInstance, properties.objectProviderName, armarx::Clock::Now()));
- });
+ memory.forEachInstance(
+ [this, &sourceId](const armem::wm::EntityInstance& instance)
+ {
+ const armarx::armem::arondto::ObjectInstance i =
+ armarx::armem::arondto::ObjectInstance::FromAron(
+ instance.data());
+ objpose::ObjectPose pose;
+ armarx::armem::fromAron(i, pose);
+
+ // objpose::ObjectPose pose =
+ // armarx::aron::fromAron<objpose::ObjectPose>(
+ // armarx::armem::arondto::ObjectInstance::FromAron(instance.data())
+ // );
+
+ auto currentTime = armarx::armem::Time::Now();
+ //auto ageOfObjectInformation = currentTime.toSecondsSinceEpoch() - instance.metadata().sentTime.toSecondsSinceEpoch();
+
+ armarx::armem::arondto::ObjectInstance objectInstance;
+ armarx::objpose::toAron(objectInstance.pose, pose);
+
+ armem::MemoryID newObjectPoseClassID =
+ armem::MemoryID("Object",
+ "Class",
+ "PriorKnowledgeData",
+ sourceId.entityName);
+
+ armarx::armem::toAron(objectInstance.classID,
+ newObjectPoseClassID); //objectClassId;
+ objectInstance.pose.confidence =
+ 1.0; // we are sure about the location
+ objectInstance.pose.timestamp =
+ armarx::Clock::Now(); // we must update the timestamp
+
+ // store the object in the object memory
+ ARMARX_CHECK_NOT_NULL(this->remote.objectInstanceWriter);
+ ARMARX_CHECK(this->remote.objectInstanceWriter->commitObject(
+ objectInstance,
+ properties.objectProviderName,
+ armarx::Clock::Now()));
+ });
}
- return true; // for now return here, later put this below
-
- }
+ return true; // for now return here, later put this below
+ }
catch (const std::exception& e)
{
- ARMARX_ERROR << "Error while updating object instance in object memory: " << e.what();
+ ARMARX_ERROR << "Error while updating object instance in object memory: "
+ << e.what();
}
}
}
}
- /** Legacy(only newest snapshot queried)
+ /** Legacy(only newest snapshot queried)
for (auto object_description : object_descriptions)
{
auto object_description_name = object_description.second.id;
@@ -350,7 +373,8 @@ namespace armarx::speech::object_last_seen_at::core
"I cannot remember having seen the " + spokenObjectNameFromStringName(object_name);
//this->subskills.say(answer);
return false;
- } else if (queryResult.locationName == "(aborted)")
+ }
+ else if (queryResult.locationName == "(aborted)")
{
ARMARX_INFO << "Aborted query";
return true;
--
GitLab