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 dc3d9fea1e2c57f70512d7df7e729bba5fac0c81..3437637d9437e32a5387b2a65f7a863fef651056 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
@@ -40,6 +40,7 @@
#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 <armarx/symbolic_planning/armem/symbolic_scene_memory/client/reader/SymbolicSceneReader.h>
@@ -189,17 +190,6 @@ namespace armarx::speech::object_last_seen_at::core
ARMARX_DEBUG << "Got description of objects";
- // Collect current objects from working memory
- // auto objectPoseStorage = remote.objectInstanceReader->getObjectPoseStorage();
- // std::unordered_map<std::string, armarx::objpose::data::ObjectPoseSeq> objectPosesByProvider;
- // for (const auto providerName : objectPoseStorage->getAvailableProviderNames())
- // {
- // if (providerName != properties.objectProviderName)
- // {
- // objectPosesByProvider[providerName] = objectPoseStorage->getObjectPosesByProvider(providerName);
- // }
- // }
-
for (auto object_description : object_descriptions)
{
auto object_description_name = object_description.second.id;
@@ -209,8 +199,6 @@ namespace armarx::speech::object_last_seen_at::core
[](unsigned char c)
{ return std::tolower(c); }); //convert description name to lowercase
- armem::MemoryID newObjectPoseClassID = armem::MemoryID("Object", "Class", "", "");
-
ARMARX_DEBUG << object_description_name;
if (object_description_name.find(object_name) != std::string::npos)
@@ -229,7 +217,7 @@ namespace armarx::speech::object_last_seen_at::core
return true; // for now return here, later put this below
- /**
+
try{
//obtain the object pose from the object memory:
const armem::MemoryID sourceId = armem::MemoryID::fromString(object_description.second.source);
@@ -242,25 +230,33 @@ namespace armarx::speech::object_last_seen_at::core
.snapshots().atTime(sourceId.timestamp);
armem::client::QueryResult result = reader.query(builder.buildQueryInput());
+
+
if(result.success)
{
armem::wm::Memory& memory = result.memory;
- memory.forEachInstance([this](const armem::wm::EntityInstance& instance)
+ memory.forEachInstance([this, &sourceId](const armem::wm::EntityInstance& instance)
{
- armarx::objpose::ObjectPose pose =
- armarx::aron::fromAron<armarx::objpose::ObjectPose>(
- armarx::armem::arondto::ObjectInstance::FromAron(instance.data())
- );
+ 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();
+ //auto ageOfObjectInformation = currentTime.toSecondsSinceEpoch() - instance.metadata().sentTime.toSecondsSinceEpoch();
armarx::armem::arondto::ObjectInstance objectInstance;
armarx::objpose::toAron(objectInstance.pose, pose);
- objectInstance.classID = ""; //objectClassId;
+ 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
@@ -276,12 +272,12 @@ namespace armarx::speech::object_last_seen_at::core
ARMARX_ERROR << "Error while updating object instance in object memory: " << e.what();
}
- */
}
{ // if the location is unkown, we need to query the LTM
+ ARMARX_INFO << "Information not in WM, searching LTM";
const auto queryResult = queryLTM(object_description.second.id);
ARMARX_INFO << VAROUT(queryResult.locationName);
@@ -303,6 +299,7 @@ namespace armarx::speech::object_last_seen_at::core
}
// object is not in symbolic scene memory (-> currently not visible) and we need to leverage the LTM
+ ARMARX_INFO << "Information not in WM, searching LTM";
const auto queryResult = queryLTM(object_name);
ARMARX_INFO << VAROUT(queryResult.locationName);
if (queryResult.locationName == "(unknown)")
@@ -341,17 +338,8 @@ namespace armarx::speech::object_last_seen_at::core
ARMARX_INFO << "Object instance is not available";
}
-
return true;
}
-
- // ARMARX_WARNING << "Could not find an object matching the name " << object_name
- // << " in the symbolic scene memory";
-
- // std::string answer = "I cannot remember having seen the " +
- // spokenObjectNameFromStringName(object_name) + " today.";
- // this->subskills.say(answer);
- // return false;
}
} // namespace armarx::speech::object_last_seen_at::core
diff --git a/source/armarx/speech/skills/scene_knowledge_verbalization/skills/CMakeLists.txt b/source/armarx/speech/skills/scene_knowledge_verbalization/skills/CMakeLists.txt
index 2b4f4cc787283fed689136f2e2c6c751bf08b130..58c18fb30c61c0942ee1ad053c5a18ce6de4b16c 100644
--- a/source/armarx/speech/skills/scene_knowledge_verbalization/skills/CMakeLists.txt
+++ b/source/armarx/speech/skills/scene_knowledge_verbalization/skills/CMakeLists.txt
@@ -15,11 +15,13 @@ armarx_add_library(scene_knowledge_verbalization_skills
RobotAPISkills
armem_objects
+ armarx_symbolic_planning
+
# armarx_speech
armarx_speech::scene_knowledge_verbalization_constants
armarx_speech::scene_knowledge_verbalization_aron
- armarx_symbolic_planning::symbolic_scene_memory_client
+
DEPENDENCIES_PRIVATE
range-v3::range-v3
diff --git a/source/armarx/speech/skills/scene_knowledge_verbalization/skills/ObjectsAtLocation.cpp b/source/armarx/speech/skills/scene_knowledge_verbalization/skills/ObjectsAtLocation.cpp
index 536e02ed0d443a5c5c26c34bb0a52277de21b7c8..77439791441fab340d6260753dbdf4c174b05a3d 100644
--- a/source/armarx/speech/skills/scene_knowledge_verbalization/skills/ObjectsAtLocation.cpp
+++ b/source/armarx/speech/skills/scene_knowledge_verbalization/skills/ObjectsAtLocation.cpp
@@ -279,11 +279,12 @@ namespace armarx::speech::scene_knowledge_verbalization::skills
spokenObjectNames.push_back("the " + getObjectSpokenName(objectId));
}
+ //const auto locationInstanceName = getLocationInstanceName(locationId);
+
ss << getSpokenList(spokenObjectNames);
+ ss << in.parameters.preposition; //<< " the " << locationInstanceName;
ss << ".";
- // Add the location of where you searched for objects
-
say(ss.str());
return MakeSucceededResult();