From 4c65e565a059f2efaf883e465af6c7d71bd752b7 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 28 Mar 2024 08:39:16 +0100
Subject: [PATCH] ObjectMemory: adding file watcher to reload scene snapshots
on file change
---
.../armem_objects/server/instance/Segment.cpp | 89 ++++++++++++++++++-
.../armem_objects/server/instance/Segment.h | 5 ++
2 files changed, 93 insertions(+), 1 deletion(-)
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index ebbac6dcf..5f0487883 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -3,6 +3,8 @@
#include <filesystem>
#include <sstream>
+#include <sys/inotify.h>
+
#include <Eigen/Dense>
#include <Eigen/Geometry>
@@ -14,6 +16,7 @@
#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/services/tasks/TaskUtil.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/core/time/DateTime.h>
@@ -35,9 +38,9 @@
#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+#include <RobotAPI/libraries/armem_objects/client/articulated_object/aron_conversions.h>
#include <RobotAPI/libraries/armem_objects/memory_ids.h>
#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
-#include <RobotAPI/libraries/armem_objects/client/articulated_object/aron_conversions.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
@@ -139,6 +142,9 @@ namespace armarx::armem::server::obj::instance
prefix + "scene.12_SnapshotToLoad",
simox::alg::join(sceneSnapshotToLoadDescription, " \n"));
+ defs->optional(p.autoReloadSceneSnapshotsOnFileChange,
+ prefix + "scene.13_autoReloadSceneSnapshotsOnFileChange");
+
decay.defineProperties(defs, prefix + "decay.");
}
@@ -166,6 +172,87 @@ namespace armarx::armem::server::obj::instance
}
robots.setTag(Logging::tag);
+
+ if (p.autoReloadSceneSnapshotsOnFileChange)
+ {
+
+ int inotifyFd;
+
+ // init inotify
+ {
+
+ inotifyFd = inotify_init();
+ if (inotifyFd == -1)
+ {
+ ARMARX_WARNING << "inotify_init failed";
+ }
+ }
+
+ std::map<int, std::string> wds;
+
+ // set up inotify watchers
+ for (const auto& scene : scenes)
+ {
+ if (std::optional<std::filesystem::path> path = resolveSceneFilepath(scene))
+ {
+ auto wd = inotify_add_watch(inotifyFd, path.value().c_str(), IN_MODIFY);
+ if (wd == -1)
+ {
+ ARMARX_WARNING << "inotify_add_watch for scene: " << scene << "` failed.";
+ }
+
+ ARMARX_INFO << "inotify_add_watch for scene: " << scene << "` added.";
+ wds.emplace(wd, scene);
+ }
+ else
+ {
+ ARMARX_WARNING << "Faild to add file watcher for scene: `" << scene << "`.";
+ }
+ }
+
+ ARMARX_INFO << "Set up inotify events.";
+
+ fileWatcherTask = new SimpleRunningTask<>(
+ [this, inotifyFd, wds]()
+ {
+ 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& scene = wds.at(event->wd);
+ ARMARX_INFO << "File changed: " << VAROUT(scene);
+
+ p += sizeof(struct inotify_event) + event->len;
+
+ const bool lockMemory = true;
+ commitSceneSnapshotFromFilename(scene, lockMemory);
+ }
+ }
+ });
+
+ fileWatcherTask->start();
+ }
}
void
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
index b922f1211..458117502 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
@@ -7,6 +7,7 @@
#include <SimoxUtility/caching/CacheMap.h>
#include <SimoxUtility/shapes/OrientedBox.h>
+#include <ArmarXCore/core/services/tasks/TaskUtil.h>
#include "RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h"
#include <RobotAPI/components/ArViz/Client/Client.h>
@@ -195,6 +196,8 @@ namespace armarx::armem::server::obj::instance
std::string sceneSnapshotsDirectory = "scenes";
std::string sceneSnapshotsToLoad = "";
+ bool autoReloadSceneSnapshotsOnFileChange = false;
+
std::vector<std::string> getSceneSnapshotsToLoad() const;
};
@@ -234,6 +237,8 @@ namespace armarx::armem::server::obj::instance
private:
std::unique_ptr<ArticulatedObjectVisu> visu;
+
+ SimpleRunningTask<>::pointer_type fileWatcherTask;
};
} // namespace armarx::armem::server::obj::instance
--
GitLab