diff --git a/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp b/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp
index 786ca15e36b15d16ebc008c165d19ffd132d6ce9..358954ae904939c74c8c9c943e7dc00e2a6ef4e2 100644
--- a/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp
+++ b/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp
@@ -2,20 +2,27 @@
#include <cstring>
#include <filesystem>
+#include <map>
#include <optional>
#include <string>
#include <vector>
+#include <sys/inotify.h>
+#include <sys/types.h>
+#include <unistd.h>
+
#include <SimoxUtility/algorithm/string/string_tools.h>
-#include <VirtualRobot/Grasping/Grasp.h>
-#include <VirtualRobot/Grasping/GraspSet.h>
-#include <VirtualRobot/ManipulationObject.h>
+#include <VirtualRobot/Grasping/Grasp.h> // IWYU pragma: keep
+#include <VirtualRobot/Grasping/GraspSet.h> // IWYU pragma: keep
+#include <VirtualRobot/ManipulationObject.h> // IWYU pragma: keep
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/XML/ObjectIO.h>
#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/services/tasks/TaskUtil.h>
+#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/core/util/StringHelpers.h>
#include "RobotAPI/libraries/ArmarXObjects/ObjectFinder.h"
@@ -26,6 +33,8 @@
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
#include <RobotAPI/libraries/armem_grasping/aron/KnownGraspCandidate.aron.generated.h>
+#include <linux/limits.h>
+
namespace armarx::armem::grasping::segment
{
@@ -41,183 +50,189 @@ namespace armarx::armem::grasping::segment
Base::init();
loadMemory();
+
+ installFileWatcher();
}
std::optional<arondto::KnownGraspInfo>
KnownGraspProviderSegment::knownGraspInfoFromObjectInfo(const ObjectInfo& info)
{
- std::string objectClassName = info.className();
- auto fileLocInfo = info.file(".xml", "_Grasps");
- std::filesystem::path graspFilePath = fileLocInfo.absolutePath;
+ return knownGraspInfoFromFile(GraspFileInfo::FromObjectInfo(info));
+ }
+
+ std::optional<arondto::KnownGraspInfo>
+ KnownGraspProviderSegment::knownGraspInfoFromFile(const GraspFileInfo& graspFileInfo)
+ {
- if (std::filesystem::is_regular_file(graspFilePath))
+ const std::filesystem::path graspFilePath = graspFileInfo.fileLocInfo.absolutePath;
+ if (not std::filesystem::is_regular_file(graspFilePath))
{
- ARMARX_INFO << "loading " << graspFilePath;
- try
- {
- auto manipulationObject =
- VirtualRobot::ObjectIO::loadManipulationObject(graspFilePath);
+ return std::nullopt; // file does not exist
+ }
- if (manipulationObject == nullptr)
- {
- ARMARX_WARNING << "Invalid file content: " << graspFilePath;
- return std::nullopt;
- }
+ ARMARX_INFO << "loading " << graspFilePath;
+ try
+ {
+ const std::string objectClassName = graspFileInfo.objectId.className();
- arondto::KnownGraspInfo ret;
- ret.correspondingObject.memoryName = "Object";
- ret.correspondingObject.coreSegmentName = "Class";
- ret.correspondingObject.providerSegmentName = "PriorKnowledgeData";
- ret.correspondingObject.entityName = info.idStr();
- ret.xml.package = fileLocInfo.package;
- ret.xml.path = fileLocInfo.relativePath;
+ auto manipulationObject = VirtualRobot::ObjectIO::loadManipulationObject(graspFilePath);
- for (const VirtualRobot::GraspSetPtr& graspSet :
- manipulationObject->getAllGraspSets())
- {
- ARMARX_CHECK_NOT_NULL(graspSet);
+ if (manipulationObject == nullptr)
+ {
+ ARMARX_WARNING << "Invalid file content: " << graspFilePath;
+ return std::nullopt;
+ }
- arondto::KnownGraspSet retGraspSet;
+ arondto::KnownGraspInfo ret;
+ ret.correspondingObject.memoryName = "Object";
+ ret.correspondingObject.coreSegmentName = "Class";
+ ret.correspondingObject.providerSegmentName = "PriorKnowledgeData";
+ ret.correspondingObject.entityName = graspFileInfo.objectId.str();
+ ret.xml.package = graspFileInfo.fileLocInfo.package;
+ ret.xml.path = graspFileInfo.fileLocInfo.relativePath;
- retGraspSet.name = graspSet->getName();
+ for (const VirtualRobot::GraspSetPtr& graspSet : manipulationObject->getAllGraspSets())
+ {
+ ARMARX_CHECK_NOT_NULL(graspSet);
- retGraspSet.robot = simox::alg::split(graspSet->getRobotType(), " ").front();
+ arondto::KnownGraspSet retGraspSet;
- retGraspSet.endeffector = graspSet->getEndEffector();
+ retGraspSet.name = graspSet->getName();
- VirtualRobot::GraspSetPtr preGraspSet = [&graspSet]()
- {
- std::vector<VirtualRobot::GraspPtr> preGrasps;
- for (const auto& grasp : graspSet->getGrasps())
- {
- // check if grasp is a prepose by checking if grasp name ends with "_Prepose"
- if (simox::alg::ends_with(grasp->getName(), PREPOSE_SUFFIX))
- {
- preGrasps.emplace_back(grasp);
- }
+ retGraspSet.robot = simox::alg::split(graspSet->getRobotType(), " ").front();
- // everything else is assumed to be a grasp
- }
+ retGraspSet.endeffector = graspSet->getEndEffector();
- // create a new grasp set with the preposes
- VirtualRobot::GraspSetPtr preGraspSet = graspSet->clone();
- preGraspSet->removeAllGrasps(); // just keep the EEF info etc
- for (const auto& preGrasp : preGrasps)
+ VirtualRobot::GraspSetPtr preGraspSet = [&graspSet]()
+ {
+ std::vector<VirtualRobot::GraspPtr> preGrasps;
+ for (const auto& grasp : graspSet->getGrasps())
+ {
+ // check if grasp is a prepose by checking if grasp name ends with "_Prepose"
+ if (simox::alg::ends_with(grasp->getName(), PREPOSE_SUFFIX))
{
- preGraspSet->addGrasp(preGrasp);
+ preGrasps.emplace_back(grasp);
}
- return preGraspSet;
- }();
-
-
- ARMARX_DEBUG << VAROUT(preGraspSet->getSize());
+ // everything else is assumed to be a grasp
+ }
- // remove all preposes from the grasp set
- for (const auto& preGrasp : preGraspSet->getGrasps())
+ // create a new grasp set with the preposes
+ VirtualRobot::GraspSetPtr preGraspSet = graspSet->clone();
+ preGraspSet->removeAllGrasps(); // just keep the EEF info etc
+ for (const auto& preGrasp : preGrasps)
{
- graspSet->removeGrasp(preGrasp);
+ preGraspSet->addGrasp(preGrasp);
}
+ return preGraspSet;
+ }();
- for (const VirtualRobot::GraspPtr& grasp : graspSet->getGrasps())
- {
- ARMARX_CHECK_NOT_NULL(grasp);
- arondto::KnownGrasp retGrasp;
+ ARMARX_DEBUG << VAROUT(preGraspSet->getSize());
- retGrasp.name = grasp->getName();
- retGrasp.quality = grasp->getQuality();
- retGrasp.creator = grasp->getCreationMethod();
- retGrasp.pose = grasp->getTransformation();
- retGrasp.prepose.reset();
+ // remove all preposes from the grasp set
+ for (const auto& preGrasp : preGraspSet->getGrasps())
+ {
+ graspSet->removeGrasp(preGrasp);
+ }
- // check if grasp has a prepose by checking if grasp name ends with "_Prepose"
- {
- const std::string prePoseName = retGrasp.name + PREPOSE_SUFFIX;
- ARMARX_DEBUG << "Checking for prepose '" << prePoseName << "' ...";
+ for (const VirtualRobot::GraspPtr& grasp : graspSet->getGrasps())
+ {
+ ARMARX_CHECK_NOT_NULL(grasp);
- if (preGraspSet->hasGrasp(prePoseName))
- {
- retGrasp.prepose =
- preGraspSet->getGrasp(prePoseName)->getTransformation();
+ arondto::KnownGrasp retGrasp;
- // remove the prepose from the set as it found its match
- preGraspSet->removeGrasp(preGraspSet->getGrasp(prePoseName));
+ retGrasp.name = grasp->getName();
+ retGrasp.quality = grasp->getQuality();
+ retGrasp.creator = grasp->getCreationMethod();
+ retGrasp.pose = grasp->getTransformation();
+ retGrasp.prepose.reset();
+ // check if grasp has a prepose by checking if grasp name ends with "_Prepose"
+ {
+ const std::string prePoseName = retGrasp.name + PREPOSE_SUFFIX;
- ARMARX_DEBUG << "Found prepose `" + prePoseName + "` for grasp '"
- << retGrasp.name << "' in set '" << retGraspSet.name
- << "' for obj '" << objectClassName << "' with pose \n"
- << retGrasp.prepose.value();
- }
- }
+ ARMARX_DEBUG << "Checking for prepose '" << prePoseName << "' ...";
- // check if grasp has a prepose for a grasp with a specific name, e.g., "XY_Grasp" (GRASP_OPTIONAL_SUFFIX)
- if ( // not retGrasp.prepose.has_value() and
- simox::alg::ends_with(retGrasp.name, GRASP_OPTIONAL_SUFFIX))
+ if (preGraspSet->hasGrasp(prePoseName))
{
- const std::string prePoseName =
- retGrasp.name.substr(
- 0, retGrasp.name.size() - std::strlen(GRASP_OPTIONAL_SUFFIX)) +
- PREPOSE_SUFFIX;
+ retGrasp.prepose =
+ preGraspSet->getGrasp(prePoseName)->getTransformation();
- ARMARX_DEBUG << "Checking for prepose '" << prePoseName << "' ...";
-
- if (preGraspSet->hasGrasp(prePoseName))
- {
- retGrasp.prepose =
- preGraspSet->getGrasp(prePoseName)->getTransformation();
+ // remove the prepose from the set as it found its match
+ preGraspSet->removeGrasp(preGraspSet->getGrasp(prePoseName));
- // remove the prepose from the set as it found its match
- preGraspSet->removeGrasp(preGraspSet->getGrasp(prePoseName));
- ARMARX_DEBUG << "Found prepose `" + prePoseName + "` for grasp '"
- << retGrasp.name << "' in set '" << retGraspSet.name
- << "' for obj '" << objectClassName << "' with pose \n"
- << retGrasp.prepose.value();
- }
+ ARMARX_DEBUG << "Found prepose `" + prePoseName + "` for grasp '"
+ << retGrasp.name << "' in set '" << retGraspSet.name
+ << "' for obj '" << objectClassName << "' with pose \n"
+ << retGrasp.prepose.value();
}
-
- ARMARX_DEBUG << "Found grasp '" << retGrasp.name << "' in set '"
- << retGraspSet.name << "' for obj '" << objectClassName
- << "' with pose \n"
- << retGrasp.pose;
-
- retGraspSet.grasps.push_back(retGrasp);
}
- // Now, check if there are any preposes left in the set. This should not have happened.
- if (preGraspSet->getSize() > 0)
+ // check if grasp has a prepose for a grasp with a specific name, e.g., "XY_Grasp" (GRASP_OPTIONAL_SUFFIX)
+ if ( // not retGrasp.prepose.has_value() and
+ simox::alg::ends_with(retGrasp.name, GRASP_OPTIONAL_SUFFIX))
{
- ARMARX_WARNING << "Found " << preGraspSet->getSize()
- << " preposes in the grasp set '" << retGraspSet.name
- << "' for obj '" << objectClassName
- << "' that do not have a corresponding grasp!";
- for (const auto& preGrasp : preGraspSet->getGrasps())
+ const std::string prePoseName =
+ retGrasp.name.substr(
+ 0, retGrasp.name.size() - std::strlen(GRASP_OPTIONAL_SUFFIX)) +
+ PREPOSE_SUFFIX;
+
+ ARMARX_DEBUG << "Checking for prepose '" << prePoseName << "' ...";
+
+ if (preGraspSet->hasGrasp(prePoseName))
{
- ARMARX_WARNING << "Prepose '" << preGrasp->getName();
+ retGrasp.prepose =
+ preGraspSet->getGrasp(prePoseName)->getTransformation();
+
+ // remove the prepose from the set as it found its match
+ preGraspSet->removeGrasp(preGraspSet->getGrasp(prePoseName));
+
+ ARMARX_DEBUG << "Found prepose `" + prePoseName + "` for grasp '"
+ << retGrasp.name << "' in set '" << retGraspSet.name
+ << "' for obj '" << objectClassName << "' with pose \n"
+ << retGrasp.prepose.value();
}
}
- ARMARX_CHECK(ret.graspSets.count(retGraspSet.robot + "/" + retGraspSet.name) ==
- 0)
- << "The grasp set `" << retGraspSet.robot + "/" + retGraspSet.name
- << "` was defined twice!";
+ ARMARX_DEBUG << "Found grasp '" << retGrasp.name << "' in set '"
+ << retGraspSet.name << "' for obj '" << objectClassName
+ << "' with pose \n"
+ << retGrasp.pose;
- ret.graspSets[retGraspSet.robot + "/" + retGraspSet.name] = retGraspSet;
+ retGraspSet.grasps.push_back(retGrasp);
}
- return ret;
- }
- catch (...)
- {
- ARMARX_WARNING << graspFilePath << " is not a manipulation object!"
- << GetHandledExceptionString();
- return std::nullopt;
+
+ // Now, check if there are any preposes left in the set. This should not have happened.
+ if (preGraspSet->getSize() > 0)
+ {
+ ARMARX_WARNING << "Found " << preGraspSet->getSize()
+ << " preposes in the grasp set '" << retGraspSet.name
+ << "' for obj '" << objectClassName
+ << "' that do not have a corresponding grasp!";
+ for (const auto& preGrasp : preGraspSet->getGrasps())
+ {
+ ARMARX_WARNING << "Prepose '" << preGrasp->getName();
+ }
+ }
+
+ ARMARX_CHECK(ret.graspSets.count(retGraspSet.robot + "/" + retGraspSet.name) == 0)
+ << "The grasp set `" << retGraspSet.robot + "/" + retGraspSet.name
+ << "` was defined twice!";
+
+ ret.graspSets[retGraspSet.robot + "/" + retGraspSet.name] = retGraspSet;
}
+ return ret;
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << graspFilePath << " is not a manipulation object!"
+ << GetHandledExceptionString();
+ return std::nullopt;
}
- return std::nullopt;
}
void
@@ -268,4 +283,152 @@ namespace armarx::armem::grasping::segment
ARMARX_WARNING << "Got errors for commit: " << result.allErrorMessages();
}
}
+
+ void
+ KnownGraspProviderSegment::installFileWatcher()
+ {
+ // FIXME improve this implementation. Merge it with the code above
+
+ // load data from prior knowledge
+ const ObjectFinder objectFinder;
+ const std::vector<ObjectInfo> infos = objectFinder.findAllObjects(false);
+
+ const bool autoReloadSceneSnapshotsOnFileChange = true; // FIXME param
+
+ if (autoReloadSceneSnapshotsOnFileChange)
+ {
+ int inotifyFd;
+
+ // init inotify
+ {
+
+ inotifyFd = inotify_init();
+ if (inotifyFd == -1)
+ {
+ ARMARX_WARNING << "inotify_init failed";
+ }
+ }
+
+ std::map<int, GraspFileInfo> wds;
+
+ // set up inotify watchers
+ for (const auto& info : infos)
+ {
+ const auto graspFileInfo = GraspFileInfo::FromObjectInfo(info);
+
+ if (std::filesystem::exists(graspFileInfo.fileLocInfo.absolutePath))
+ {
+ auto wd = inotify_add_watch(
+ inotifyFd, graspFileInfo.fileLocInfo.absolutePath.c_str(), IN_MODIFY);
+ if (wd == -1)
+ {
+ ARMARX_WARNING << "inotify_add_watch for grasp file: "
+ << graspFileInfo.fileLocInfo.absolutePath << "` failed.";
+ }
+
+ ARMARX_INFO << "inotify_add_watch for grasp file: "
+ << graspFileInfo.fileLocInfo.absolutePath << "` added.";
+ wds.emplace(wd, graspFileInfo);
+ }
+ else
+ {
+ ARMARX_WARNING << "Faild to add file watcher for grasp file: `"
+ << graspFileInfo.fileLocInfo.absolutePath << "`.";
+ }
+ }
+
+ ARMARX_INFO << "Set up " << wds.size() << " inotify events.";
+
+ fileWatcherTask = new SimpleRunningTask<>(
+ [this, inotifyFd, wds]()
+ {
+ ObjectFinder objectFinder;
+
+ constexpr std::size_t BUF_LEN = (10 * (sizeof(inotify_event) + NAME_MAX + 1));
+ char buf[BUF_LEN];
+
+ for (;;)
+ {
+ ssize_t numRead;
+
+ numRead = read(inotifyFd, buf, BUF_LEN);
+ if (numRead == 0)
+ {
+ ARMARX_WARNING << "read() from inotify fd returned 0!";
+ }
+
+ if (numRead == -1)
+ {
+ ARMARX_VERBOSE << "read";
+ }
+
+ ARMARX_DEBUG << VAROUT(numRead);
+
+ for (char* p = buf; p < buf + numRead;)
+ {
+ auto* event = reinterpret_cast<inotify_event*>(p);
+
+ const auto& graspFileInfo = wds.at(event->wd);
+ ARMARX_INFO << "File changed: "
+ << VAROUT(graspFileInfo.fileLocInfo.absolutePath);
+
+ p += sizeof(struct inotify_event) + event->len;
+
+ const bool lockMemory = true;
+
+ // read updated grasp file and store it in the memory
+ {
+ const MemoryID providerID =
+ segmentPtr->id().withProviderSegmentName(
+ objectFinder.getPackageName());
+
+ // create commit from new file content
+ Commit commit;
+ {
+ if (auto knownGraspCandidate =
+ knownGraspInfoFromFile(graspFileInfo);
+ knownGraspCandidate)
+ {
+ EntityUpdate& update = commit.add();
+ update.entityID =
+ providerID.withEntityName(graspFileInfo.objectId.str());
+ update.entityID.timestamp = update.arrivedTime =
+ update.referencedTime = update.sentTime =
+ armarx::Clock::Now();
+
+ update.instancesData = {knownGraspCandidate->toAron()};
+
+ ARMARX_DEBUG
+ << VAROUT(knownGraspCandidate->graspSets.size());
+ for (const auto& gs : knownGraspCandidate->graspSets)
+ {
+ ARMARX_DEBUG << VAROUT(gs.second.grasps.size());
+ for (const auto& grasp : gs.second.grasps)
+ {
+ ARMARX_DEBUG << VAROUT(grasp.name);
+ }
+ }
+ }
+ }
+
+ // now store commit in memory
+ if (not commit.updates.empty())
+ {
+ ARMARX_VERBOSE << "Storing update in memory";
+
+ const auto result = iceMemory.commit(commit);
+ if (not result.allSuccess())
+ {
+ ARMARX_WARNING << "Got errors for commit: "
+ << result.allErrorMessages();
+ }
+ }
+ }
+ }
+ }
+ });
+
+ fileWatcherTask->start();
+ }
+ }
} // namespace armarx::armem::grasping::segment
diff --git a/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.h b/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.h
index 36504338a877c42932c646521bb0e9c556ab7cc4..e70f675b3a2a552f3713f8050f51f6549164a53e 100644
--- a/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.h
+++ b/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.h
@@ -1,7 +1,14 @@
#pragma once
-#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
-#include <RobotAPI/libraries/armem/server/segment/SpecializedSegment.h>
+#include <filesystem>
+#include <optional>
+
+#include <ArmarXCore/core/services/tasks/TaskUtil.h>
+
+#include "RobotAPI/libraries/ArmarXObjects/ObjectID.h"
+#include "RobotAPI/libraries/ArmarXObjects/ObjectInfo.h"
+#include "RobotAPI/libraries/armem/server/forward_declarations.h"
+#include "RobotAPI/libraries/armem/server/segment/SpecializedProviderSegment.h"
#include <RobotAPI/libraries/armem_grasping/aron/KnownGraspCandidate.aron.generated.h>
namespace armarx::armem::grasping::segment
@@ -19,6 +26,27 @@ namespace armarx::armem::grasping::segment
void loadMemory();
std::optional<arondto::KnownGraspInfo> knownGraspInfoFromObjectInfo(const ObjectInfo&);
+ struct GraspFileInfo
+ {
+ PackageFileLocation fileLocInfo;
+ ObjectID objectId;
+
+ static GraspFileInfo FromObjectInfo(const ObjectInfo& objectInfo)
+ {
+ const std::string objectClassName = objectInfo.className();
+ PackageFileLocation fileLocInfo = objectInfo.file(".xml", "_Grasps");
+
+ return GraspFileInfo{.fileLocInfo = fileLocInfo, .objectId = objectInfo.id()};
+ }
+ };
+
+ std::optional<arondto::KnownGraspInfo>
+ knownGraspInfoFromFile(const GraspFileInfo& graspFileInfo);
+
+ void installFileWatcher();
+
+ SimpleRunningTask<>::pointer_type fileWatcherTask;
+
public:
static const constexpr char* CORE_SEGMENT_NAME = "KnownGraspCandidate";
static const constexpr char* PROVIDER_SEGMENT_NAME = "PriorKnowledgeData";