diff --git a/scenarios/FamiliarObjectDetectionExample/FamiliarObjectDetectionExample.scx b/scenarios/FamiliarObjectDetectionExample/FamiliarObjectDetectionExample.scx
new file mode 100644
index 0000000000000000000000000000000000000000..512ab2b5d6816bf16f887f59d34447de6fb4739f
--- /dev/null
+++ b/scenarios/FamiliarObjectDetectionExample/FamiliarObjectDetectionExample.scx
@@ -0,0 +1,5 @@
+<?xml version="1.0" encoding="utf-8"?>
+<scenario name="FamiliarObjectDetectionExample" creation="1970-01-01.01:00:00" globalConfigName="./config/global.cfg" package="RobotAPI" nodeName="NodeMain">
+ <application name="FamiliarObjectDetectionExample" instance="" package="RobotAPI" nodeName="" enabled="true"/>
+</scenario>
+
diff --git a/scenarios/FamiliarObjectDetectionExample/config/FamiliarObjectDetectionExample.cfg b/scenarios/FamiliarObjectDetectionExample/config/FamiliarObjectDetectionExample.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..5c0e4e275eab0b8c53fd2d921e3e04622a1b3b6c
--- /dev/null
+++ b/scenarios/FamiliarObjectDetectionExample/config/FamiliarObjectDetectionExample.cfg
@@ -0,0 +1,230 @@
+# ==================================================================
+# FamiliarObjectDetectionExample properties
+# ==================================================================
+
+# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.AutodiscoverPackages: If enabled, will discover all ArmarX packages based on the environment variables. Otherwise, the `DefaultPackages` and `AdditionalPackages` properties are used.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.AutodiscoverPackages = true
+
+
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
+# Attributes:
+# - Default: mongo/.cache
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.CachePath = mongo/.cache
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+# Attributes:
+# - Default: ./config/dependencies.cfg
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
+
+
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
+
+
+# ArmarX.FamiliarObjectDetectionExample.EnableProfiling: enable profiler which is used for logging performance events
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.FamiliarObjectDetectionExample.EnableProfiling = false
+
+
+# ArmarX.FamiliarObjectDetectionExample.MinimumLoggingLevel: Local logging level only for this component
+# Attributes:
+# - Default: Undefined
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.FamiliarObjectDetectionExample.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.FamiliarObjectDetectionExample.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.FamiliarObjectDetectionExample.ObjectName = ""
+
+
+# ArmarX.FamiliarObjectDetectionExample.cmp.FamiliarObjectPoseStorage: Ice object name of the `FamiliarObjectPoseStorage` component.
+# Attributes:
+# - Default: ObjectMemory
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.FamiliarObjectDetectionExample.cmp.FamiliarObjectPoseStorage = ObjectMemory
+
+
+# ArmarX.FamiliarObjectDetectionExample.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
+# Set to false to use this memory as a stand-alone.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.FamiliarObjectDetectionExample.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.FamiliarObjectDetectionExample.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.FamiliarObjectDetectionExample.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.FamiliarObjectDetectionExample.p.robotName:
+# Attributes:
+# - Case sensitivity: yes
+# - Required: yes
+ArmarX.FamiliarObjectDetectionExample.p.robotName = ArmarDE
+
+
+# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoadLibraries = ""
+
+
+# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoggingGroup = ""
+
+
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
+
+
+# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+# Attributes:
+# - Default: 3000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SecondsStartupDelay = 0
+
+
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.StartDebuggerOnCrash = false
+
+
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ThreadPoolSize = 1
+
+
+# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
+
+
+# ArmarX.Verbosity: Global logging level for whole application
+# Attributes:
+# - Default: Info
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Info
+
+
diff --git a/scenarios/FamiliarObjectDetectionExample/config/global.cfg b/scenarios/FamiliarObjectDetectionExample/config/global.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..dce557f2b65f4915bd91332fa9f0fb67ba3bfb60
--- /dev/null
+++ b/scenarios/FamiliarObjectDetectionExample/config/global.cfg
@@ -0,0 +1,4 @@
+# ==================================================================
+# Global Config from Scenario FamiliarObjectDetectionExample
+# ==================================================================
+
diff --git a/source/RobotAPI/components/CMakeLists.txt b/source/RobotAPI/components/CMakeLists.txt
index fcd09e9158a7bb7b36ab3a27a1edb875aaa7b6e6..5771f8021d04318285250d27b96b5dcc468795a6 100644
--- a/source/RobotAPI/components/CMakeLists.txt
+++ b/source/RobotAPI/components/CMakeLists.txt
@@ -5,6 +5,7 @@ add_subdirectory(skills)
add_subdirectory(AronComponentConfigExample)
add_subdirectory(ArticulatedObjectLocalizerExample)
+add_subdirectory(FamiliarObjectDetectionExample)
add_subdirectory(ArViz)
add_subdirectory(CyberGloveObserver)
diff --git a/source/RobotAPI/components/FamiliarObjectDetectionExample/CMakeLists.txt b/source/RobotAPI/components/FamiliarObjectDetectionExample/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..379a61f24d625fc02450a0915403824e6bfa6c3f
--- /dev/null
+++ b/source/RobotAPI/components/FamiliarObjectDetectionExample/CMakeLists.txt
@@ -0,0 +1,26 @@
+set(LIB_NAME FamiliarObjectDetectionExample)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+find_package(OpenCV QUIET)
+armarx_build_if(OpenCV_FOUND "OpenCV not available")
+
+# Add the component
+armarx_add_component(
+ COMPONENT_LIBS
+ ArmarXCore
+ armem_objects
+ RobotAPIArmarXObjects
+ SOURCES
+ FamiliarObjectDetectionExample.cpp
+
+ HEADERS
+ FamiliarObjectDetectionExample.h
+)
+
+# Generate the application
+armarx_generate_and_add_component_executable(
+ # If your component is not defined in ::armarx, specify its namespace here:
+ COMPONENT_NAMESPACE "armarx::familiar_objects"
+)
diff --git a/source/RobotAPI/components/FamiliarObjectDetectionExample/FamiliarObjectDetectionExample.cpp b/source/RobotAPI/components/FamiliarObjectDetectionExample/FamiliarObjectDetectionExample.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..08d848d49fd6ae7e791e093545f137d7e51e198a
--- /dev/null
+++ b/source/RobotAPI/components/FamiliarObjectDetectionExample/FamiliarObjectDetectionExample.cpp
@@ -0,0 +1,294 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2024
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "FamiliarObjectDetectionExample.h"
+
+#include <Eigen/Geometry>
+
+#include <pcl/point_types.h>
+
+#include <opencv2/core/mat.hpp>
+
+#include <IceUtil/Time.h>
+
+#include <SimoxUtility/math/pose/pose.h>
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/VirtualRobot.h>
+#include <VirtualRobot/XML/RobotIO.h>
+
+#include "ArmarXCore/core/time/Clock.h"
+#include <ArmarXCore/core/PackagePath.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+#include <ArmarXCore/core/time/CycleUtil.h>
+
+#include "RobotAPI/libraries/ArmarXObjects/ObjectID.h"
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem_objects/aron/FamiliarObjectInstance.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/types.h>
+// #include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+// #include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
+
+namespace armarx::familiar_objects
+{
+ FamiliarObjectDetectionExample::FamiliarObjectDetectionExample()
+ {
+ addPlugin(familiarObjectInstanceReaderPlugin);
+ }
+
+ armarx::PropertyDefinitionsPtr
+ FamiliarObjectDetectionExample::createPropertyDefinitions()
+ {
+ armarx::PropertyDefinitionsPtr defs =
+ new ComponentPropertyDefinitions(getConfigIdentifier());
+
+ defs->required(p.robotName, "p.robotName");
+ defs->component(familiarObjectPoseStoragePrx, "ObjectMemory");
+
+ return defs;
+ }
+
+ std::string
+ FamiliarObjectDetectionExample::getDefaultName() const
+ {
+ return "FamiliarObjectDetectionExample";
+ }
+
+ void
+ FamiliarObjectDetectionExample::onInitComponent()
+ {
+ exemplaryFamiliarObjectID.dataset = "myDataset";
+ exemplaryFamiliarObjectID.className = "sphere";
+ exemplaryFamiliarObjectID.instanceName = "0";
+ }
+
+ void
+ FamiliarObjectDetectionExample::onConnectComponent()
+ {
+ //
+ // First, we create a familiar object and send it to the memory.
+ //
+
+ ARMARX_IMPORTANT << "Storing exemplary familiar object in memory";
+ storeExemplaryFamiliarObjectInMemory();
+
+ //
+ // Then, we read the familiar object from the memory.
+ //
+ ARMARX_IMPORTANT << "Reading familiar object from memory";
+ readExemplaryFamiliarObjectFromMemory();
+ }
+
+ void
+ FamiliarObjectDetectionExample::storeExemplaryFamiliarObjectInMemory() const
+ {
+
+ ::armarx::objpose::ProvidedFamiliarObjectPoseSeq data;
+
+ armem::arondto::FamiliarObjectInstance familiarObject;
+
+ familiarObject.timestamp = armarx::Clock::Now();
+
+ familiarObject.poseSensFrame.pose =
+ Eigen::Isometry3f{Eigen::Translation3f{0, 0, 1000}}.matrix();
+ familiarObject.poseSensFrame.header.frame = "DepthCameraSim";
+ familiarObject.poseSensFrame.header.agent = p.robotName;
+ // familiarObject.poseSensFrame.header.frame = "AzureKinect_RGB";
+
+
+ familiarObject.objectID = exemplaryFamiliarObjectID;
+
+ familiarObject.confidence = 1.0;
+
+ // familiarObject.bounding_box =
+
+ // familiarObject.depth_image_patch = cv::Mat1f(cv::Size(20, 20));
+ // familiarObject.rgb_image_patch = cv::Mat3b(cv::Size(20, 20));
+
+ const int numPoints = 100;
+
+ // x in red
+ for (int i = 0; i < numPoints; i++)
+ {
+ pcl::PointXYZRGBA point;
+ point.x = static_cast<float>(i) - numPoints / 2;
+ point.y = 0;
+ point.z = 0;
+ point.r = 255;
+ point.g = 0;
+ point.b = 0;
+ familiarObject.points.points.push_back(point);
+ }
+
+ // y in green
+ for (int i = 0; i < numPoints; i++)
+ {
+ pcl::PointXYZRGBA point;
+ point.x = 0;
+ point.y = static_cast<float>(i) - numPoints / 2;
+ point.z = 0;
+ point.r = 0;
+ point.g = 255;
+ point.b = 0;
+ familiarObject.points.points.push_back(point);
+ }
+
+ // z in blue
+ for (int i = 0; i < numPoints; i++)
+ {
+ pcl::PointXYZRGBA point;
+ point.y = 0;
+ point.y = 0;
+ point.z = static_cast<float>(i) - numPoints / 2;
+ point.r = 0;
+ point.g = 0;
+ point.b = 255;
+ familiarObject.points.points.push_back(point);
+ }
+
+ familiarObject.points.header.frame_id = "DepthCameraSim";
+ familiarObject.points.is_dense = true;
+ familiarObject.points.width = familiarObject.points.points.size();
+ familiarObject.points.height = 1;
+
+ familiarObject.bounding_box.center.setZero();
+ familiarObject.bounding_box.extents.setConstant(numPoints);
+
+ data.push_back(familiarObject.toAronDTO());
+
+ ARMARX_INFO << "Sending " << data.size() << " familiar object to the memory";
+ familiarObjectPoseStoragePrx->reportFamiliarObjectPoses(getName(), data);
+ }
+
+ void
+ FamiliarObjectDetectionExample::readExemplaryFamiliarObjectFromMemory() const
+ {
+ ARMARX_CHECK_NOT_NULL(familiarObjectInstanceReaderPlugin);
+
+ const armem::obj::familiar_object_instance::Reader& familiarObjectInstanceReader =
+ familiarObjectInstanceReaderPlugin->get();
+
+
+ //
+ // 1. Read from all providers
+ //
+ ARMARX_IMPORTANT << "Reading from all providers";
+ {
+
+ const auto allFamiliarObjectInstances =
+ familiarObjectInstanceReader.queryAllLatestFamiliarObjectInstances();
+
+ // print
+ for (const auto& [providerName, instances] : allFamiliarObjectInstances)
+ {
+ ARMARX_INFO << "Provider name: " << providerName;
+ for (const auto& instance : instances)
+ {
+ ARMARX_INFO << "- Instance: " << instance.objectID.dataset << "/"
+ << instance.objectID.className << "/"
+ << instance.objectID.instanceName;
+ }
+ }
+ }
+
+ //
+ // 2. Read from a specific provider
+ //
+ ARMARX_IMPORTANT << "Reading from a specific provider";
+ {
+ const std::map<std::string, std::vector<armem::arondto::FamiliarObjectInstance>>
+ familiarObjectInstances =
+ familiarObjectInstanceReader.queryAllLatestFamiliarObjectInstances(getName());
+
+ ARMARX_INFO << "Provider name: " << getName();
+
+ ARMARX_CHECK_EQUAL(familiarObjectInstances.size(), 1);
+ ARMARX_CHECK_EQUAL(familiarObjectInstances.begin()->first, getName());
+
+ const auto& thisFamiliarObjectInstances = familiarObjectInstances.begin()->second;
+ // print
+ for (const auto& instance : thisFamiliarObjectInstances)
+ {
+ ARMARX_INFO << "- Instance: " << instance.objectID.dataset << "/"
+ << instance.objectID.className << "/" << instance.objectID.instanceName;
+ }
+ }
+
+ //
+ // 3. Read all instances of a specific class
+ //
+ ARMARX_IMPORTANT << "Reading all instances of a specific class";
+ {
+ armarx::ObjectID objectId;
+ fromAron(exemplaryFamiliarObjectID, objectId);
+
+ const auto instances =
+ familiarObjectInstanceReader.queryLatestFamiliarObjectInstancesFromClass(
+ objectId.getClassID());
+
+ for (const auto& [instanceName, instancesForProvider] : instances)
+ {
+ for (const auto& instance : instancesForProvider)
+ {
+ ARMARX_INFO << "- Instance: " << instance.objectID.dataset << "/"
+ << instance.objectID.className << "/"
+ << instance.objectID.instanceName;
+ }
+ }
+ }
+
+ //
+ // 4. Read a specific instance
+ //
+ ARMARX_IMPORTANT << "Reading a specific instance";
+ {
+ armarx::ObjectID objectId;
+ fromAron(exemplaryFamiliarObjectID, objectId);
+
+ const std::optional<std::map<std::string, armem::arondto::FamiliarObjectInstance>>
+ instances =
+ familiarObjectInstanceReader.queryLatestFamiliarObjectInstance(objectId);
+
+ ARMARX_CHECK(instances.has_value());
+
+ for (const auto& [instanceName, instance] : instances.value())
+ {
+ ARMARX_INFO << "- Instance: " << instance.objectID.dataset << "/"
+ << instance.objectID.className << "/" << instance.objectID.instanceName;
+ }
+ }
+ }
+
+ void
+ FamiliarObjectDetectionExample::onDisconnectComponent()
+ {
+ // task->stop();
+ }
+
+ void
+ FamiliarObjectDetectionExample::onExitComponent()
+ {
+ }
+
+
+} // namespace armarx::familiar_objects
diff --git a/source/RobotAPI/components/FamiliarObjectDetectionExample/FamiliarObjectDetectionExample.h b/source/RobotAPI/components/FamiliarObjectDetectionExample/FamiliarObjectDetectionExample.h
new file mode 100644
index 0000000000000000000000000000000000000000..39a0e969f82d01f2fd3f154bfa37cd0820d895ef
--- /dev/null
+++ b/source/RobotAPI/components/FamiliarObjectDetectionExample/FamiliarObjectDetectionExample.h
@@ -0,0 +1,81 @@
+
+#pragma once
+
+#include <experimental/memory>
+
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+#include <ArmarXCore/interface/observers/ObserverInterface.h>
+#include <ArmarXCore/util/tasks.h>
+
+#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
+
+#include "RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h"
+#include "RobotAPI/libraries/armem_objects/client/familiar_object_instance/ObjectReader.h"
+#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+#include <RobotAPI/interface/armem/server/MemoryInterface.h>
+#include <RobotAPI/interface/objectpose/FamiliarObjectPoseStorageInterface.h>
+#include <RobotAPI/libraries/armem/client/plugins.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.h>
+#include <RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.h>
+
+namespace armarx::familiar_objects
+{
+
+ /**
+ * @defgroup Component-ExampleClient ExampleClient
+ * @ingroup RobotAPI-Components
+ * A description of the component ExampleClient.
+ *
+ * @class ExampleClient
+ * @ingroup Component-ExampleClient
+ * @brief Brief description of class ExampleClient.
+ *
+ * Detailed description of class ExampleClient.
+ */
+ class FamiliarObjectDetectionExample :
+ virtual public armarx::Component,
+ virtual public armarx::armem::client::ComponentPluginUser
+ {
+ public:
+ FamiliarObjectDetectionExample();
+
+ /// @see armarx::ManagedIceObject::getDefaultName()
+ std::string getDefaultName() const override;
+
+ protected:
+ armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+
+ void onInitComponent() override;
+ void onConnectComponent() override;
+ void onDisconnectComponent() override;
+ void onExitComponent() override;
+
+ void run();
+
+ void storeExemplaryFamiliarObjectInMemory() const;
+ void readExemplaryFamiliarObjectFromMemory() const;
+
+ private:
+ struct Properties
+ {
+ std::string robotName;
+ } p;
+
+ // Writing familiar object instances to memory
+ objpose::FamiliarObjectPoseStorageInterfacePrx familiarObjectPoseStoragePrx;
+
+ // Reading familiar object instances from memory
+ std::experimental::observer_ptr<armem::client::plugins::ReaderWriterPlugin<
+ armarx::armem::obj::familiar_object_instance::Reader>>
+ familiarObjectInstanceReaderPlugin;
+
+
+ armarx::arondto::ObjectID exemplaryFamiliarObjectID;
+ };
+
+} // namespace armarx::familiar_objects
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
index bc8179ff695156ab4691f9a5feae5885699c2c4a..7403e833107e392c8ffdecc4f0544500b13568de 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
@@ -24,6 +24,7 @@
#include <VirtualRobot/VirtualRobot.h>
#include "ArmarXCore/core/time/Clock.h"
+#include <ArmarXCore/core/ManagedIceObject.h>
#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
#include <ArmarXCore/core/time/ice_conversions.h>
@@ -48,6 +49,7 @@ namespace armarx::armem::server::obj
classSegment.defineProperties(defs, prefix + "cls.");
instance::SegmentAdapter::defineProperties(defs, prefix + "inst.");
+ familiar_object_instance::SegmentAdapter::defineProperties(defs, prefix + "fam_obj_inst.");
attachmentSegment.defineProperties(defs, prefix + "attachments.");
@@ -75,6 +77,7 @@ namespace armarx::armem::server::obj
ObjectMemory::ObjectMemory() :
instance::SegmentAdapter(iceAdapter()),
+ familiar_object_instance::SegmentAdapter(iceAdapter()),
classSegment(iceAdapter()),
attachmentSegment(iceAdapter()), virtualRobotReaderPlugin(nullptr)
{
@@ -123,6 +126,7 @@ namespace armarx::armem::server::obj
});
instance::SegmentAdapter::init();
+ familiar_object_instance::SegmentAdapter::init();
initSegmentWithCatch("attachment", [&]()
{
@@ -155,6 +159,13 @@ namespace armarx::armem::server::obj
ArVizComponentPluginUser::getArvizClient(),
debugObserver
);
+
+ familiar_object_instance::SegmentAdapter::connect(
+ std::experimental::make_observer(&virtualRobotReaderPlugin->get()),
+ ArVizComponentPluginUser::getArvizClient(),
+ debugObserver
+ );
+
classSegment.connect(
ArVizComponentPluginUser::getArvizClient()
);
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
index 566d1fec8a6ea1fe39f12edabbaaaa76b08ea9e0..9a97c2374a3010c5003978bf8f0eecdecb4af610 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
@@ -26,6 +26,7 @@
#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
#include <RobotAPI/libraries/armem_objects/server/class/Segment.h>
#include <RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h>
+#include <RobotAPI/libraries/armem_objects/server/familiar_object_instance/SegmentAdapter.h>
#include <RobotAPI/libraries/armem_objects/server/attachments/Segment.h>
#include <RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h>
@@ -63,6 +64,7 @@ namespace armarx::armem::server::obj
, virtual public armarx::armem::server::ObjectMemoryInterface
, virtual public armarx::armem::server::ReadWritePluginUser
, virtual public armarx::armem::server::obj::instance::SegmentAdapter
+ , virtual public armarx::armem::server::obj::familiar_object_instance::SegmentAdapter
, virtual public armarx::LightweightRemoteGuiComponentPluginUser
, virtual public armarx::ArVizComponentPluginUser
{
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
index 2249bdf242a197d1bff3be840d846f58a2487568..09109c2bb721d90890763d7803526f7acfb2bb81 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
@@ -188,6 +188,7 @@ namespace armarx::armem::server::robot_state
startRobotUnitStream();
}
+
}
void
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/BusyWaiting.cpp b/source/RobotAPI/components/skills/SkillProviderExample/BusyWaiting.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..209c73778ec31686d50b9c32d3bb244b13e1862d
--- /dev/null
+++ b/source/RobotAPI/components/skills/SkillProviderExample/BusyWaiting.cpp
@@ -0,0 +1,32 @@
+#include "BusyWaiting.h"
+
+#include <chrono>
+#include <thread>
+
+namespace armarx::skills::provider
+{
+
+ BusyWaitingSkill::BusyWaitingSkill() : SimpleSkill(GetSkillDescription())
+ {
+ }
+
+ SkillDescription
+ BusyWaitingSkill::GetSkillDescription()
+ {
+ return SkillDescription{.skillId = SkillID{.skillName = "BusyWaiting"},
+ .description = "This skill takes two seconds",
+ .timeout = armarx::core::time::Duration::MilliSeconds(10000)};
+ }
+
+ Skill::MainResult
+ BusyWaitingSkill::main(const MainInput& in)
+ {
+ for (unsigned int i = 0; i < 10; i++)
+ {
+ throwIfSkillShouldTerminate();
+ std::this_thread::sleep_for(std::chrono::milliseconds(200));
+ }
+
+ return {TerminatedSkillStatus::Succeeded, nullptr};
+ }
+} // namespace armarx::skills::provider
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/BusyWaiting.h b/source/RobotAPI/components/skills/SkillProviderExample/BusyWaiting.h
new file mode 100644
index 0000000000000000000000000000000000000000..87d00c69fb557749e8129c174c630a3bfc2957cb
--- /dev/null
+++ b/source/RobotAPI/components/skills/SkillProviderExample/BusyWaiting.h
@@ -0,0 +1,42 @@
+
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Andre Meixner ( andre dot meixner at kit dot edu )
+ * @date 2024
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+
+// RobotAPI
+#include <RobotAPI/libraries/skills/provider/SimpleSkill.h>
+
+namespace armarx::skills::provider
+{
+
+ class BusyWaitingSkill : public SimpleSkill
+ {
+ public:
+ BusyWaitingSkill();
+
+ static SkillDescription GetSkillDescription();
+
+ private:
+ Skill::MainResult main(const MainInput&) final;
+ };
+} // namespace armarx::skills::provider
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/CMakeLists.txt b/source/RobotAPI/components/skills/SkillProviderExample/CMakeLists.txt
index b95ebf9bc2058093f917b7f5bfd2dd976df7ae29..cdbb2a67dd8991b0010914c43eed37407878ea29 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/CMakeLists.txt
+++ b/source/RobotAPI/components/skills/SkillProviderExample/CMakeLists.txt
@@ -21,6 +21,10 @@ set(SOURCES
Segfault.cpp
RandomExitStatus.cpp
Sleep.cpp
+ RandomChaining.cpp
+ InstantKill.cpp
+ BusyWaiting.cpp
+ Recursive.cpp
)
set(HEADERS
@@ -33,6 +37,10 @@ set(HEADERS
Segfault.h
RandomExitStatus.h
Sleep.h
+ RandomChaining.h
+ InstantKill.h
+ BusyWaiting.h
+ Recursive.h
)
armarx_add_component("${SOURCES}" "${HEADERS}")
@@ -43,6 +51,7 @@ armarx_enable_aron_file_generation_for_target(
ARON_FILES
aron/HelloWorldAcceptedType.xml
aron/SleepAcceptedType.xml
+ aron/RecursiveSkillParams.xml
)
#generate the application
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/Chaining.cpp b/source/RobotAPI/components/skills/SkillProviderExample/Chaining.cpp
index 983902512c03bc28c8d4429fd22c4690aeb14833..843f1a84dbf224a7af14fb354e0141e2c93fb2df 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/Chaining.cpp
+++ b/source/RobotAPI/components/skills/SkillProviderExample/Chaining.cpp
@@ -12,7 +12,7 @@ namespace armarx::skills::provider
SkillDescription
ChainingSkill::GetSkillDescription()
{
- return SkillDescription{.skillId = armarx::skills::SkillID{.skillName = "ChainingSkill"},
+ return SkillDescription{.skillId = armarx::skills::SkillID{.skillName = "Chaining"},
.description =
"This skill calls the Timeout skill three times. The last "
"execution is aborted due to a timeout of this skill.",
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/Incomplete.cpp b/source/RobotAPI/components/skills/SkillProviderExample/Incomplete.cpp
index 8d6b4f277dcb36adc2b73bfe94b45b9adb500b22..b5afe130d00aed97e443a1263d6db97d313bab39 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/Incomplete.cpp
+++ b/source/RobotAPI/components/skills/SkillProviderExample/Incomplete.cpp
@@ -17,7 +17,7 @@ namespace armarx::skills::provider
IncompleteSkill::GetSkillDescription()
{
auto d = HelloWorldSkill::GetSkillDescription();
- return SkillDescription{.skillId = {.skillName = "IncompleteSkill"},
+ return SkillDescription{.skillId = {.skillName = "Incomplete"},
.description = d.description,
.timeout = d.timeout + armarx::core::time::Duration::Seconds(2),
.parametersType = d.parametersType};
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/InstantKill.cpp b/source/RobotAPI/components/skills/SkillProviderExample/InstantKill.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fa81224c1a659de619a13b5c20133e7d6e24e365
--- /dev/null
+++ b/source/RobotAPI/components/skills/SkillProviderExample/InstantKill.cpp
@@ -0,0 +1,41 @@
+
+#include <cstdlib>
+#include "InstantKill.h"
+
+#include <thread>
+#include <chrono>
+
+namespace armarx::skills::provider
+{
+
+ InstantKillSkill::InstantKillSkill() : SimpleSkill(GetSkillDescription())
+ {
+ }
+
+ SkillDescription
+ InstantKillSkill::GetSkillDescription()
+ {
+ return SkillDescription{.skillId = armarx::skills::SkillID{.skillName = "InstantKill"},
+ .description =
+ "This skill calls Timeout and instantly aboirts it.",
+ .timeout = armarx::core::time::Duration::MilliSeconds(50000)};
+ }
+
+ Skill::MainResult
+ InstantKillSkill::main(const MainInput& in)
+ {
+
+ this->throwIfSkillShouldTerminate();
+
+ SkillProxy prx(
+ manager,
+ skills::SkillID{.providerId = *getSkillId().providerId, .skillName = "Chaining"});
+
+ for (unsigned int i = 0; i < 10; ++i)
+ {
+ auto id = callSubskillAsync(prx);
+ prx.abortSkillAsync(id);
+ }
+ return {TerminatedSkillStatus::Succeeded, nullptr};
+ }
+} // namespace armarx::skills::provider
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/InstantKill.h b/source/RobotAPI/components/skills/SkillProviderExample/InstantKill.h
new file mode 100644
index 0000000000000000000000000000000000000000..d5487d2fca63995554e877cef03c5df1f18ea2e6
--- /dev/null
+++ b/source/RobotAPI/components/skills/SkillProviderExample/InstantKill.h
@@ -0,0 +1,40 @@
+
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Ternava ( fabian dot ternava at kit dot edu )
+ * @date 2024
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+// RobotAPI
+#include <RobotAPI/libraries/skills/provider/SimpleSkill.h>
+
+namespace armarx::skills::provider
+{
+ class InstantKillSkill : public SimpleSkill
+ {
+ public:
+ InstantKillSkill();
+
+ static SkillDescription GetSkillDescription();
+
+ private:
+ Skill::MainResult main(const MainInput&) final;
+ };
+} // namespace armarx::skills::provider
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/RandomChaining.cpp b/source/RobotAPI/components/skills/SkillProviderExample/RandomChaining.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4ca2e3f337bb3bfb41109a525373d2df19d814dd
--- /dev/null
+++ b/source/RobotAPI/components/skills/SkillProviderExample/RandomChaining.cpp
@@ -0,0 +1,72 @@
+
+#include <cstdlib>
+#include "RandomChaining.h"
+
+#include <thread>
+#include <chrono>
+
+namespace armarx::skills::provider
+{
+ namespace util
+ {
+ int randomgen(int max, int min) //Pass in range
+ {
+ srand(time(NULL)); //Changed from rand(). srand() seeds rand for you.
+ int random = rand() % max + min;
+ return random;
+ }
+ }
+
+ RandomChainingSkill::RandomChainingSkill() : SimpleSkill(GetSkillDescription())
+ {
+ }
+
+ SkillDescription
+ RandomChainingSkill::GetSkillDescription()
+ {
+ return SkillDescription{.skillId = armarx::skills::SkillID{.skillName = "RandomChaining"},
+ .description =
+ "This skill calls 100 random subskills from the skillProviderExample excluding the segfault skill.",
+ .timeout = armarx::core::time::Duration::MilliSeconds(120000)};
+ }
+
+ Skill::MainResult
+ RandomChainingSkill::main(const MainInput& in)
+ {
+ std::vector<std::string> subskillNames = {
+ "Timeout", "Chaining", "Foo", "HelloWorld",
+ "Incomplete", "ShowMeCallbacks", "BusyWaiting", "Recursive"
+ };
+
+ ARMARX_CHECK(subskillNames.size() > 0);
+
+ for (unsigned int i = 0; i < 100; ++i)
+ {
+ this->throwIfSkillShouldTerminate();
+
+ auto index = util::randomgen(subskillNames.size() -1, 0);
+
+ auto subskillName = subskillNames[index];
+
+ SkillProxy prx(
+ manager,
+ skills::SkillID{.providerId = *getSkillId().providerId, .skillName = subskillName});
+
+ if (util::randomgen(10, 0) < 2)
+ {
+ callSubskill(prx);
+ }
+ else
+ {
+ callSubskillAsync(prx);
+
+ auto sleep_milliseconds = util::randomgen(1000, 0);
+
+ ARMARX_INFO << "SLEEP FOR " << sleep_milliseconds << "ms";
+ std::this_thread::sleep_for(std::chrono::milliseconds(sleep_milliseconds));
+ }
+ }
+
+ return {TerminatedSkillStatus::Succeeded, nullptr};
+ }
+} // namespace armarx::skills::provider
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/RandomChaining.h b/source/RobotAPI/components/skills/SkillProviderExample/RandomChaining.h
new file mode 100644
index 0000000000000000000000000000000000000000..375a8daf710c42037c5daf1bd0af1a32dbefdf3c
--- /dev/null
+++ b/source/RobotAPI/components/skills/SkillProviderExample/RandomChaining.h
@@ -0,0 +1,40 @@
+
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+// RobotAPI
+#include <RobotAPI/libraries/skills/provider/SimpleSkill.h>
+
+namespace armarx::skills::provider
+{
+ class RandomChainingSkill : public SimpleSkill
+ {
+ public:
+ RandomChainingSkill();
+
+ static SkillDescription GetSkillDescription();
+
+ private:
+ Skill::MainResult main(const MainInput&) final;
+ };
+} // namespace armarx::skills::provider
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/Recursive.cpp b/source/RobotAPI/components/skills/SkillProviderExample/Recursive.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7ad3c457ac45d3cde2daf94fde6e89d11ee04b54
--- /dev/null
+++ b/source/RobotAPI/components/skills/SkillProviderExample/Recursive.cpp
@@ -0,0 +1,49 @@
+#include "Recursive.h"
+
+#include "RobotAPI/libraries/skills/core/SkillDescription.h"
+
+namespace armarx::skills::provider
+{
+
+ RecursiveSkill::RecursiveSkill() : SimpleSpecializedSkill<skills::Example::RecursiveSkillParams>(GetSkillDescription())
+ {
+ }
+
+ SkillDescription
+ RecursiveSkill::GetSkillDescription()
+ {
+ armarx::skills::Example::RecursiveSkillParams root_profile_params;
+ root_profile_params.n = 10;
+
+ return SkillDescription{.skillId = skills::SkillID{.skillName = "Recursive"},
+ .description = "This skill calls itself recursively {n} times",
+ .rootProfileDefaults = root_profile_params.toAron(),
+ .timeout = armarx::core::time::Duration::MilliSeconds(10000),
+ .parametersType =
+ armarx::skills::Example::RecursiveSkillParams::ToAronType(),
+ .resultType = armarx::skills::Example::RecursiveSkillParams::ToAronType()};
+ }
+
+ Skill::MainResult
+ RecursiveSkill::main(const SpecializedMainInput& in)
+ {
+ const int n = in.parameters.n;
+
+ if (n > 0)
+ {
+ SkillProxy prx(manager,
+ skills::SkillID{.providerId = *getSkillId().providerId, .skillName = "Recursive"});
+
+ armarx::skills::Example::RecursiveSkillParams params;
+ params.n = n - 1;
+
+ std::this_thread::sleep_for(std::chrono::milliseconds(100));
+
+ callSubskill(prx, params.toAron());
+ }
+
+ throwIfSkillShouldTerminate();
+
+ return {TerminatedSkillStatus::Succeeded, nullptr};
+ }
+} // namespace armarx::skills::provider
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/Recursive.h b/source/RobotAPI/components/skills/SkillProviderExample/Recursive.h
new file mode 100644
index 0000000000000000000000000000000000000000..ac96cce58a07cea59f6b118f6b410a46d7e09210
--- /dev/null
+++ b/source/RobotAPI/components/skills/SkillProviderExample/Recursive.h
@@ -0,0 +1,42 @@
+
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Andre Meixner ( andre dot meixner at kit dot edu )
+ * @date 2024
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+// RobotAPI
+#include <RobotAPI/libraries/skills/provider/SimpleSpecializedSkill.h>
+
+#include <RobotAPI/components/skills/SkillProviderExample/aron/RecursiveSkillParams.aron.generated.h>
+
+namespace armarx::skills::provider
+{
+ class RecursiveSkill : public SimpleSpecializedSkill<skills::Example::RecursiveSkillParams>
+ {
+ public:
+ RecursiveSkill();
+
+ static SkillDescription GetSkillDescription();
+
+ private:
+ Skill::MainResult main(const SpecializedMainInput&) final;
+ };
+} // namespace armarx::skills::provider
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.cpp b/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.cpp
index 142465a88c63cde5e95294280177b0f0378082d0..6c4dec2e5a648605acc1c92b7d4e12772c0eb918 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.cpp
+++ b/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.cpp
@@ -69,6 +69,19 @@ namespace armarx::skills::provider
// random exit status
ARMARX_INFO << "Adding skill Sleep";
addSkillFactory<Sleep>();
+ // random
+ ARMARX_INFO << "Adding skill RandomChainingSkill";
+ addSkillFactory<RandomChainingSkill>();
+
+ // insta kill
+ ARMARX_INFO << "Adding skill InstaKill";
+ addSkillFactory<InstantKillSkill>();
+
+ ARMARX_INFO << "Adding skill BusyWaiting";
+ addSkillFactory<BusyWaitingSkill>();
+
+ ARMARX_INFO << "Adding skill Recursive";
+ addSkillFactory<RecursiveSkill>();
}
void
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.h b/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.h
index e65cc44ba62144d2797e834e9b96b82d36f332db..d09d00acb56e09da886df0a50e05840d2043efe5 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.h
+++ b/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.h
@@ -37,6 +37,10 @@
#include "Segfault.h"
#include "Sleep.h"
#include "Timeout.h"
+#include "RandomChaining.h"
+#include "InstantKill.h"
+#include "BusyWaiting.h"
+#include "Recursive.h"
namespace armarx::skills::provider
{
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/aron/RecursiveSkillParams.xml b/source/RobotAPI/components/skills/SkillProviderExample/aron/RecursiveSkillParams.xml
new file mode 100644
index 0000000000000000000000000000000000000000..47d69a1b7f090a8ac43adf8ebc3df266a56adf8b
--- /dev/null
+++ b/source/RobotAPI/components/skills/SkillProviderExample/aron/RecursiveSkillParams.xml
@@ -0,0 +1,11 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <GenerateTypes>
+ <Object name='armarx::skills::Example::RecursiveSkillParams'>
+ <ObjectChild key='n'>
+ <int32 />
+ </ObjectChild>
+ </Object>
+ </GenerateTypes>
+
+</AronTypeDefinition>
\ No newline at end of file
diff --git a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
index 16f091a3ac5ca8ae10f5b812761b179b86efc6ea..1daa8bc0f41b165f13c3084fbc7bcf3d6f4f2d15 100755
--- a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
+++ b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
@@ -30,6 +30,7 @@ set(LIB_FILES
SensorValues/SensorValueBase.cpp
Units/RobotUnitSubUnit.cpp
+ Units/DiagnosticsSubUnit.cpp
Units/ForceTorqueSubUnit.cpp
Units/InertialMeasurementSubUnit.cpp
Units/KinematicSubUnit.cpp
@@ -103,11 +104,13 @@ set(LIB_HEADERS
SensorValues/SensorValueBase.h
SensorValues/SensorValue1DoFActuator.h
SensorValues/SensorValueIMU.h
+ SensorValues/SensorValueBattery.h
SensorValues/SensorValueForceTorque.h
SensorValues/SensorValueHolonomicPlatform.h
SensorValues/SensorValueRTThreadTimings.h
Units/RobotUnitSubUnit.h
+ Units/DiagnosticsSubUnit.h
Units/ForceTorqueSubUnit.h
Units/InertialMeasurementSubUnit.h
Units/KinematicSubUnit.h
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
index 6b124cdb76197760261a1b851f7a3b188a978490..09f52252953d5e9d5a1ebfdac591a68745f46137 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
@@ -26,20 +26,21 @@
#include <ArmarXCore/core/IceManager.h>
#include <ArmarXCore/core/time.h>
-#include "RobotAPI/components/units/RobotUnit/SensorValues/SensorValueHolonomicPlatform.h"
-#include "RobotAPI/components/units/RobotUnit/Units/LocalizationSubUnit.h"
#include <RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.h>
-
-#include "../RobotUnit.h"
-#include "../SensorValues/SensorValue1DoFActuator.h"
-#include "../Units/ForceTorqueSubUnit.h"
-#include "../Units/InertialMeasurementSubUnit.h"
-#include "../Units/KinematicSubUnit.h"
-#include "../Units/PlatformSubUnit.h"
-#include "../Units/TCPControllerSubUnit.h"
-#include "../Units/TrajectoryControllerSubUnit.h"
+#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueBattery.h>
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueHolonomicPlatform.h>
+#include <RobotAPI/components/units/RobotUnit/Units/DiagnosticsSubUnit.h>
+#include <RobotAPI/components/units/RobotUnit/Units/ForceTorqueSubUnit.h>
+#include <RobotAPI/components/units/RobotUnit/Units/InertialMeasurementSubUnit.h>
+#include <RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.h>
+#include <RobotAPI/components/units/RobotUnit/Units/LocalizationSubUnit.h>
+#include <RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h>
+#include <RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.h>
+#include <RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.h>
namespace armarx::RobotUnitModule
{
@@ -210,6 +211,10 @@ namespace armarx::RobotUnitModule
throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
ARMARX_INFO << "initializing default units";
+ ARMARX_TRACE;
+ initializeDiagnosticsUnit();
+ ARMARX_DEBUG << "Diagnostics unit initialized.";
+
ARMARX_TRACE;
initializeKinematicUnit();
ARMARX_DEBUG << "KinematicUnit initialized";
@@ -221,8 +226,8 @@ namespace armarx::RobotUnitModule
initializeLocalizationUnit();
ARMARX_DEBUG << "LocalizationUnit initialized";
}
- ARMARX_TRACE;
+ ARMARX_TRACE;
if (_module<RobotData>().arePlatformAndLocalizationUnitsEnabled())
{
ARMARX_DEBUG << "initializing PlatformUnit";
@@ -252,6 +257,43 @@ namespace armarx::RobotUnitModule
<< (TimeUtil::GetTime(true) - beg).toMicroSeconds() << " us";
}
+ void
+ Units::initializeDiagnosticsUnit()
+ {
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ using UnitT = DiagnosticsSubUnit;
+ using IfaceT = DiagnosticsUnitInterface;
+
+ auto guard = getGuard();
+ throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
+ // Check if unit is already added.
+ if (getUnit(IfaceT::ice_staticId()))
+ {
+ return;
+ }
+
+ // Check if it is required.
+ std::map<std::string, std::size_t> batteryDevs;
+ for (auto i : getIndices(_module<Devices>().getSensorDevices().keys()))
+ {
+ const SensorDevicePtr& sensorDevice = _module<Devices>().getSensorDevices().at(i);
+ if (sensorDevice->getSensorValue()->isA<SensorValueBattery>())
+ {
+ batteryDevs[sensorDevice->getDeviceName()] = i;
+ }
+ }
+
+ const std::string configName = getProperty<std::string>("DiagnosticsUnitName");
+ const std::string confPre = getConfigDomain() + "." + configName + ".";
+ Ice::PropertiesPtr properties = getIceProperties()->clone();
+
+ // Create and add unit.
+ IceInternal::Handle<UnitT> unit =
+ Component::create<UnitT>(properties, configName, getConfigDomain());
+ unit->setBatteryManagementDevices(batteryDevs);
+ addUnit(std::move(unit));
+ }
+
void
Units::initializeKinematicUnit()
{
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h
index f33c7ee47065d3bb7c5f1302d806d3af8229920a..45c9b79c76abf3dadbdd45edb015914e5b8ea673 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h
@@ -40,6 +40,8 @@ namespace armarx::RobotUnitModule
defineOptionalProperty<std::string>(
"KinematicUnitNameTopicPrefix", "", "Prefix for the kinematic sensor values topic");
+ defineOptionalProperty<std::string>(
+ "DiagnosticsUnitName", "DiagnosticsUnit", "Name of the diagnostics unit.");
defineOptionalProperty<std::string>(
"PlatformUnitName", "PlatformUnit", "The name of the created platform unit");
@@ -266,6 +268,8 @@ namespace armarx::RobotUnitModule
* - \ref initializeTcpControllerUnit
*/
virtual void initializeDefaultUnits();
+ /// @brief Initialize the diagnostics unit.
+ virtual void initializeDiagnosticsUnit();
/// @brief Initializes the KinematicUnit
virtual void initializeKinematicUnit();
/// @brief Initializes the PlatformUnit
diff --git a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueBattery.h b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueBattery.h
new file mode 100644
index 0000000000000000000000000000000000000000..c91435863d9fb8eef983019d790c39576bb03ea7
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueBattery.h
@@ -0,0 +1,61 @@
+#pragma once
+
+#include <string>
+
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueBase.h>
+
+namespace armarx
+{
+
+ enum class BatteryState : std::uint8_t
+ {
+ unavailable = 0,
+ discharging = 1,
+ charging = 2,
+ notCharging = 3,
+ full = 4
+ };
+
+ class SensorValueBattery : virtual public SensorValueBase
+ {
+ public:
+ DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
+
+ std::uint8_t state;
+ float designEnergy_Wh;
+ float fullChargeEnergy_Wh;
+ float energy_Wh;
+ float energyFromFullCharge_pct;
+ float fullEnergyFromDesignEnergy_pct;
+ float temperature_degC;
+ float ratedVoltage_V;
+ float voltage_V;
+ float current_A;
+ float power_W;
+ float remainingTime_h;
+ bool hasError;
+
+ static SensorValueInfo<SensorValueBattery>
+ GetClassMemberInfo()
+ {
+ SensorValueInfo<SensorValueBattery> svi;
+ svi.addMemberVariable(&SensorValueBattery::state, "state");
+ svi.addMemberVariable(&SensorValueBattery::designEnergy_Wh, "designEnergy_Wh");
+ svi.addMemberVariable(&SensorValueBattery::fullChargeEnergy_Wh, "fullChargeEnergy_Wh");
+ svi.addMemberVariable(&SensorValueBattery::energy_Wh, "energy_Wh");
+ svi.addMemberVariable(&SensorValueBattery::energyFromFullCharge_pct,
+ "energyFromFullCharge_pct");
+ svi.addMemberVariable(&SensorValueBattery::fullEnergyFromDesignEnergy_pct,
+ "fullEnergyFromDesignEnergy_pct");
+ svi.addMemberVariable(&SensorValueBattery::temperature_degC, "temperature_degC");
+ svi.addMemberVariable(&SensorValueBattery::ratedVoltage_V, "ratedVoltage_V");
+ svi.addMemberVariable(&SensorValueBattery::voltage_V, "voltage_V");
+ svi.addMemberVariable(&SensorValueBattery::current_A, "current_A");
+ svi.addMemberVariable(&SensorValueBattery::power_W, "power_W");
+ svi.addMemberVariable(&SensorValueBattery::remainingTime_h, "remainingTime_h");
+ svi.addMemberVariable(&SensorValueBattery::hasError, "hasError");
+ return svi;
+ }
+ };
+
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/DiagnosticsSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/DiagnosticsSubUnit.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..af635065bbbb43bd91cced24212097ee229ed5e0
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/Units/DiagnosticsSubUnit.cpp
@@ -0,0 +1,111 @@
+#include "DiagnosticsSubUnit.h"
+
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueBattery.h>
+
+namespace armarx
+{
+
+ dto::BatteryStatus
+ DiagnosticsSubUnit::getBatteryStatus(const Ice::Current& current) const
+ {
+ // This diagnostics sub unit should rather report the status report of each battery module,
+ // and additionally an accumulated one for easy use. Detailed reports of each battery module
+ // are important in order to identify faulty modules more quickly. At this point, due to
+ // firmware limitations and only being allowed to return one sensor value per device in the
+ // realtime part, the diagnostics unit now only reports the accumulated values and nothing
+ // else. In future, a dto::BatteryStatus struct must be reported for each battery module, in
+ // addition to one dto::BatteryStatus that accumulates all data. For this, the firmware of
+ // the power board needs to be adjusted to report this data, the EtherCAT SDOs need to be
+ // updated, and the device in devices::ethercat needs to be updated to report more than one
+ // sensor value. Additionally, the BatteryManagement Ice interface needs to be adjusted to
+ // report multiple BatteryStatus structs. Similar to this:
+ //
+ // struct BatteryManagementReport
+ // {
+ // map<string, dto::BatteryStatus> detailedReports;
+ // dto::BatteryStatus cumulativeReport;
+ // };
+ //
+ if (_lastBatteryStatuses.size() > 0 and
+ _lastBatteryStatuses.find("PowerBoard") != _lastBatteryStatuses.end())
+ {
+ return _lastBatteryStatuses.at("PowerBoard");
+ }
+
+ return {.name = "PowerBoard",
+ .state = dto::BatteryState::unavailable,
+ .designEnergy_Wh = -1,
+ .fullChargeEnergy_Wh = -1,
+ .energy_Wh = -1,
+ .energyFromFullCharge_pct = 0,
+ .fullEnergyFromDesignEnergy_pct = 0,
+ .temperature_degC = -1,
+ .ratedVoltage_V = -1,
+ .voltage_V = -1,
+ .current_A = -1,
+ .power_W = -1,
+ .remainingTime_h = -1,
+ .hasError = false,
+ .error = ""};
+ }
+
+ std::string
+ DiagnosticsSubUnit::getDefaultName() const
+ {
+ return "DiagnosticsSubUnit";
+ }
+
+ void
+ DiagnosticsSubUnit::update(const SensorAndControl& sc, const JointAndNJointControllers& c)
+ {
+ for (auto& [deviceName, deviceIndex] : _batteryManagementDevices)
+ {
+ const SensorValueBase* sv = sc.sensors.at(deviceIndex).get();
+ ARMARX_CHECK_EXPRESSION(sv->isA<armarx::SensorValueBattery>());
+ const armarx::SensorValueBattery* s = sv->asA<armarx::SensorValueBattery>();
+
+ auto& stat = _lastBatteryStatuses[deviceName];
+ stat.name = deviceName;
+ stat.state = static_cast<dto::BatteryState>(s->state);
+ stat.designEnergy_Wh = s->designEnergy_Wh;
+ stat.fullChargeEnergy_Wh = s->fullChargeEnergy_Wh;
+ stat.energy_Wh = s->energy_Wh;
+ stat.energyFromFullCharge_pct = s->energyFromFullCharge_pct;
+ stat.fullEnergyFromDesignEnergy_pct = s->fullEnergyFromDesignEnergy_pct;
+ stat.temperature_degC = s->temperature_degC;
+ stat.ratedVoltage_V = s->ratedVoltage_V;
+ stat.voltage_V = s->voltage_V;
+ stat.current_A = s->current_A;
+ stat.power_W = s->power_W;
+ stat.remainingTime_h = s->remainingTime_h;
+ stat.hasError = s->hasError;
+ stat.error =
+ ""; // TODO: How to transfer strings from RT? Maybe get rid of this or error codes.
+ }
+ }
+
+ void
+ DiagnosticsSubUnit::setBatteryManagementDevices(
+ std::map<std::string, std::size_t> batteryManagementDevices)
+ {
+ _batteryManagementDevices = batteryManagementDevices;
+
+ for (auto& [deviceName, deviceIndex] : _batteryManagementDevices)
+ {
+ _lastBatteryStatuses[deviceName] = {};
+ }
+ }
+
+ void
+ DiagnosticsSubUnit::onInitComponent()
+ {
+ ;
+ }
+
+ void
+ DiagnosticsSubUnit::onConnectComponent()
+ {
+ ;
+ }
+
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/DiagnosticsSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/DiagnosticsSubUnit.h
new file mode 100644
index 0000000000000000000000000000000000000000..211a88b3be15359f7ad4e730ac88d13a4aedb9ea
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/Units/DiagnosticsSubUnit.h
@@ -0,0 +1,51 @@
+#pragma once
+
+#include <ArmarXCore/core/Component.h>
+
+#include <RobotAPI/components/units/ForceTorqueUnit.h>
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueBattery.h>
+#include <RobotAPI/components/units/RobotUnit/Units/RobotUnitSubUnit.h>
+#include <RobotAPI/interface/units/DiagnosticsUnitInterface.h>
+#include <RobotAPI/interface/units/LocalizationUnitInterface.h>
+
+namespace armarx
+{
+
+ class DiagnosticsSubUnit :
+ virtual public RobotUnitSubUnit,
+ virtual public armarx::DiagnosticsUnitInterface,
+ virtual public Component
+ {
+ public:
+ dto::BatteryStatus getBatteryStatus(const Ice::Current& current) const override;
+
+ std::string getDefaultName() const override;
+
+ void update(const SensorAndControl& sc, const JointAndNJointControllers& c) override;
+
+ void
+ setBatteryManagementDevices(std::map<std::string, std::size_t> batteryManagementDevices);
+
+ protected:
+ void onInitComponent() override;
+ void onConnectComponent() override;
+
+ std::map<std::string, std::size_t> _batteryManagementDevices;
+
+ std::map<std::string, dto::BatteryStatus> _lastBatteryStatuses;
+
+ /*
+ // UnitResourceManagementInterface interface
+ public:
+ void request(const Ice::Current ¤t);
+ void release(const Ice::Current ¤t);
+
+ // UnitExecutionManagementInterface interface
+ public:
+ void init(const Ice::Current ¤t);
+ void start(const Ice::Current ¤t);
+ void stop(const Ice::Current ¤t);
+ UnitExecutionState getExecutionState(const Ice::Current ¤t);*/
+ };
+
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp
old mode 100755
new mode 100644
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h
old mode 100755
new mode 100644
diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index d8f626d9fa4a752cd65eb7cbc95b3074e47c2638..da95d7a17b0a39aa9224a61f574ac61fe51285c9 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -39,8 +39,10 @@ set(SLICE_FILES
objectpose/object_pose_types.ice
objectpose/ObjectPoseStorageInterface.ice
objectpose/ObjectPoseProvider.ice
+ objectpose/FamiliarObjectPoseStorageInterface.ice
units/MultiHandUnitInterface.ice
+ units/DiagnosticsUnitInterface.ice
units/ForceTorqueUnit.ice
units/InertialMeasurementUnit.ice
units/OptoForceUnit.ice
diff --git a/source/RobotAPI/interface/armem/server/ObjectMemoryInterface.ice b/source/RobotAPI/interface/armem/server/ObjectMemoryInterface.ice
index 32e120ae8fdabfbb7b49545a86623f281b6ee159..fdae53a727de5b00ad65e4d9edacca8734ce26fe 100644
--- a/source/RobotAPI/interface/armem/server/ObjectMemoryInterface.ice
+++ b/source/RobotAPI/interface/armem/server/ObjectMemoryInterface.ice
@@ -26,6 +26,7 @@
#include <RobotAPI/interface/armem/server/MemoryInterface.ice>
#include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.ice>
+#include <RobotAPI/interface/objectpose/FamiliarObjectPoseStorageInterface.ice>
module armarx
@@ -41,9 +42,16 @@ module armarx
};
+ interface FamiliarObjectInstanceSegmentInterface extends
+ armarx::objpose::FamiliarObjectPoseStorageInterface
+ {
+
+ };
+
interface ObjectMemoryInterface extends
MemoryInterface
, ObjectInstanceSegmentInterface
+ , FamiliarObjectInstanceSegmentInterface
{
};
@@ -51,4 +59,3 @@ module armarx
};
};
};
-
diff --git a/source/RobotAPI/interface/objectpose/FamiliarObjectPoseStorageInterface.ice b/source/RobotAPI/interface/objectpose/FamiliarObjectPoseStorageInterface.ice
new file mode 100644
index 0000000000000000000000000000000000000000..b7c819ec05d886d40f51723451411070f3930022
--- /dev/null
+++ b/source/RobotAPI/interface/objectpose/FamiliarObjectPoseStorageInterface.ice
@@ -0,0 +1,43 @@
+/**
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package RobotAPI
+* @author Fabian Reister
+* @copyright 2024 Humanoids Group, H2T, KIT
+* @license http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#pragma once
+
+
+#include <RobotAPI/interface/aron/Aron.ice>
+
+
+module armarx
+{
+ module objpose
+ {
+ sequence<::armarx::aron::data::dto::Dict> ProvidedFamiliarObjectPoseSeq;
+
+ interface FamiliarObjectPoseStorageInterface
+ {
+ void reportFamiliarObjectPoses(string providerName, ProvidedFamiliarObjectPoseSeq data);
+ };
+
+ }
+
+};
diff --git a/source/RobotAPI/interface/units/DiagnosticsUnitInterface.ice b/source/RobotAPI/interface/units/DiagnosticsUnitInterface.ice
new file mode 100644
index 0000000000000000000000000000000000000000..9531005d0e1ae86f741e60d37d20045102424a58
--- /dev/null
+++ b/source/RobotAPI/interface/units/DiagnosticsUnitInterface.ice
@@ -0,0 +1,18 @@
+#pragma once
+
+#include <ArmarXCore/interface/components/BatteryManagementInterface.ice>
+#include <ArmarXCore/interface/core/BasicTypes.ice>
+#include <ArmarXCore/interface/core/UserException.ice>
+
+#include <RobotAPI/interface/core/NameValueMap.ice>
+#include <RobotAPI/interface/units/UnitInterface.ice>
+
+module armarx
+{
+
+ interface DiagnosticsUnitInterface extends
+ armarx::BatteryManagementInterface //, SensorActorUnitInterface
+ {
+
+ };
+};
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
index 4f2d0c35fa4c95375cf67d09e332c1ecec5b77fa..cf5eb9b333563c0850e9e4fa5f7b618041958f63 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
@@ -250,6 +250,12 @@ namespace armarx
return select("HandControllerName");
}
+ std::string Arm::getHandUnitName() const
+ {
+ ARMARX_TRACE;
+ return select("HandUnitName");
+ }
+
std::string Arm::getHandRootNode() const
{
ARMARX_TRACE;
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
index 76c9b283f5d915e54d1f240c94d01aa9774d8e08..c1fa9b4d74194e7fd4b8c2a5748d81ef4ccfdcbd 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
@@ -64,6 +64,8 @@ namespace armarx
std::string getHandRootNode() const;
+ std::string getHandUnitName() const;
+
std::string getHandModelPath() const;
std::string getAbsoluteHandModelPath() const;
diff --git a/source/RobotAPI/libraries/armem/client/MemoryNameSystem.cpp b/source/RobotAPI/libraries/armem/client/MemoryNameSystem.cpp
index e07429591954279d478321c1ecc14a76e917b0a3..f879ed5a349cfccb511315371aad7e72ab4363af 100644
--- a/source/RobotAPI/libraries/armem/client/MemoryNameSystem.cpp
+++ b/source/RobotAPI/libraries/armem/client/MemoryNameSystem.cpp
@@ -61,11 +61,32 @@ namespace armarx::armem::client
// Compare ice identities, update if it changed.
auto foo = [](auto & oldProxy, const auto & newProxy)
{
- if (oldProxy->ice_getIdentity() != newProxy->ice_getIdentity())
+ try
{
- // Replace old proxy by new one.
+ newProxy->ice_ping();
+ }
+ catch (const Ice::Exception& e)
+ {
+ // The new proxy must be reachable
+ throw error::MemoryNameSystemQueryFailed(e.what());
+ }
+
+ try
+ {
+ oldProxy->ice_ping();
+
+ if (oldProxy->ice_getIdentity() != newProxy->ice_getIdentity())
+ {
+ // Replace old proxy by new one. This is the case if the identity changed.
+ oldProxy = newProxy;
+ }
+ }
+ catch (const Ice::Exception&)
+ {
+ // if there is any exception, replace the old proxy by the new one
oldProxy = newProxy;
}
+
};
foo(it->second.reading, server.reading);
foo(it->second.writing, server.writing);
@@ -96,27 +117,21 @@ namespace armarx::armem::client
mns::dto::MemoryServerInterfaces
MemoryNameSystem::resolveServer(const MemoryID& memoryID)
{
+ update();
if (auto it = servers.find(memoryID.memoryName); it != servers.end())
{
return it->second;
}
else
{
- update();
- if (auto it = servers.find(memoryID.memoryName); it != servers.end())
- {
- return it->second;
- }
- else
- {
- throw error::CouldNotResolveMemoryServer(memoryID);
- }
+ throw error::CouldNotResolveMemoryServer(memoryID);
}
}
mns::dto::MemoryServerInterfaces MemoryNameSystem::waitForServer(const MemoryID& memoryID)
{
+ update();
if (auto it = servers.find(memoryID.memoryName); it != servers.end())
{
return it->second;
@@ -157,6 +172,7 @@ namespace armarx::armem::client
{
mns::dto::MemoryServerInterfaces server = waitForServer(memoryID);
// Add dependency.
+
component.usingProxy(server.reading->ice_getIdentity().name);
return server;
}
diff --git a/source/RobotAPI/libraries/armem/mns/Registry.cpp b/source/RobotAPI/libraries/armem/mns/Registry.cpp
index 2c0a92da8bb31a2adac56da11d9dd31bd873312f..b11b822285d84ba557e481a0b18ec85469688702 100644
--- a/source/RobotAPI/libraries/armem/mns/Registry.cpp
+++ b/source/RobotAPI/libraries/armem/mns/Registry.cpp
@@ -54,7 +54,7 @@ namespace armarx::armem::mns
info.name = input.name;
info.server = input.server;
info.timeRegistered = armem::Time::Now();
- ARMARX_DEBUG << "Registered memory '" << info.name << "'.";
+ ARMARX_INFO << "Registered memory '" << info.name << "'.";
result.success = true;
return result;
diff --git a/source/RobotAPI/libraries/armem/server/CMakeLists.txt b/source/RobotAPI/libraries/armem/server/CMakeLists.txt
index 0e2544ab302ba69802c699a238a79e71952ccacc..728c22ba6f1b10275ddbc961cbe0f5d095c8f715 100644
--- a/source/RobotAPI/libraries/armem/server/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem/server/CMakeLists.txt
@@ -194,6 +194,7 @@ set(LIB_HEADERS
segment/SpecializedCoreSegment.h
segment/SpecializedProviderSegment.h
segment/SpecializedSegment.h
+ segment/constants.h
query_proc.h
diff --git a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
index 4aee9800a61ce9862b714d0ba3df58b11b08e32d..d878781a7022485802e86620ec06abef6300c8e8 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
+++ b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
@@ -1,6 +1,9 @@
#include "MemoryToIceAdapter.h"
#include <thread>
+#include <list>
+#include <iostream>
+#include <sstream>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
@@ -235,9 +238,21 @@ namespace armarx::armem::server
ARMARX_DEBUG << "The id " << snapshot.id() << " was removed from wm";
}
+ // Consolidate to ltm(s) if recording mode is CLONE_WM
+ if (longtermMemory->isRecording() && longtermMemory->getRecordingMode() == server::ltm::RecordingMode::CONSOLIDATE_ALL)
+ {
+ // convert removedSnapshots to Memory
+ armem::wm::Memory m(longtermMemory->name());
+ m.update(update, true, false);
+ //this removes information about segments, because new memory (wm) is used
- // Consollidate to ltm(s)
- if (longtermMemory->isRecording())
+ // store memory
+ longtermMemory->store(m);
+ }
+
+
+ // Consolidate to ltm(s) if recording mode is CONSOLIDATE_REMOVED
+ if (longtermMemory->isRecording() && longtermMemory->getRecordingMode() == server::ltm::RecordingMode::CONSOLIDATE_REMOVED)
{
// convert removedSnapshots to Memory
armem::wm::Memory m(longtermMemory->name());
@@ -248,6 +263,11 @@ namespace armarx::armem::server
longtermMemory->store(m);
}
+ if (longtermMemory->isRecording() && longtermMemory->getRecordingMode() == server::ltm::RecordingMode::CONSOLIDATE_LATEST)
+ {
+ ARMARX_WARNING << deactivateSpam() << "THIS IS NOT IMPLEMENTED YET!!!";
+ }
+
if (publishUpdates)
{
data::MemoryID& id = updatedIDs.emplace_back();
@@ -380,7 +400,40 @@ namespace armarx::armem::server
return ret;
}
- // LTM LOADING FROM LTM
+ // WM LOADING FROM LTM
+ armem::CommitResult
+ MemoryToIceAdapter::reloadFromLTM()
+ {
+ if(this->longtermMemory->p.importOnStartUp){
+ ARMARX_INFO << "Reloading of data from LTM into WM triggered";
+ //define which core segments to load and how many snapshots:
+ auto coreNames = this->longtermMemory->p.coreSegmentsToLoad;
+ //convert string to list of names:
+ std::list<std::string> names;
+ std::stringstream ss(coreNames);
+ std::string item;
+
+ while (std::getline(ss, item, ',')) {
+ names.push_back(item);
+ }
+ int maxAmountOfSnapshots = this->longtermMemory->p.maxAmountOfSnapshotsLoaded;
+ //create WM and load latest references
+ armem::wm::Memory m;
+ this->longtermMemory->loadLatestNReferences(maxAmountOfSnapshots, m, names);
+ ARMARX_DEBUG << "After loading";
+ //construct a commit of the loaded data and commit it to the working memory
+ auto com = armem::toCommit(m);
+ auto res = this->commit(com);
+ ARMARX_DEBUG << "After commit";
+ //the CommitResult contains some information which might be helpful:
+ return res;
+ } else {
+ ARMARX_INFO << "Not loading initial data from LTM due to importOnStartup being " << this->longtermMemory->p.importOnStartUp;
+ armem::CommitResult r;
+ return r;
+ }
+
+ }
// LTM STORING AND RECORDING
dto::DirectlyStoreResult
@@ -417,18 +470,26 @@ namespace armarx::armem::server
{
ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(longtermMemory);
+ ARMARX_CHECK_NOT_NULL(workingMemory);
ARMARX_IMPORTANT << "Disabling the recording of memory " << longtermMemory->id().str();
+ if(longtermMemory->p.storeOnStop){ //if true this means when stopping LTM recording leftover snapshots are transferred to WM using the simulated consolidation
+ ARMARX_INFO << "Starting to save left-over WM data into LTM";
+ longtermMemory->directlyStore(*workingMemory, true);
+ ARMARX_INFO << "Stored leftover WM data into LTM";
+ } else {
+ ARMARX_INFO << "Not storing WM data into LTM on stop, because storeOnStop is " << longtermMemory->p.storeOnStop;
+ }
+
//put calling stopRecording into a seperate thread and detach to make sure GUI does not freeze
auto ltm = longtermMemory;
+
std::thread stopRecordingThread([<m](){
ltm->stopRecording();
- });
- std::thread savingDataUpdateThread([<m](){
ltm->bufferFinished();
+ ARMARX_INFO << "Storing finished";
});
stopRecordingThread.detach();
- savingDataUpdateThread.detach();
ARMARX_IMPORTANT
<< "Stopped all LTM recordings, please wait with stopping the component until "
diff --git a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.h b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.h
index 0ae95a4207d86842d1b11e82cd97af6030147401..2fb61e6f3ad9c494b99a79ecc106c5f497b18e79 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.h
+++ b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.h
@@ -47,6 +47,7 @@ namespace armarx::armem::server
armem::structure::data::GetServerStructureResult getServerStructure();
// LTM LOADING AND REPLAYING
+ armem::CommitResult reloadFromLTM();
// LTM STORING AND RECORDING
dto::DirectlyStoreResult directlyStore(const dto::DirectlyStoreInput& directlStoreInput);
diff --git a/source/RobotAPI/libraries/armem/server/ltm/CoreSegment.cpp b/source/RobotAPI/libraries/armem/server/ltm/CoreSegment.cpp
index 6ec04595cea542cd62d6245712e25f8adc5b0a1b..ff955d28e1778f3d30fa28ca9dc56345b44273bd 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/CoreSegment.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/CoreSegment.cpp
@@ -30,6 +30,7 @@ namespace armarx::armem::server::ltm
for (const auto& subdirName : getAllDirectories())
{
std::string segmentName = util::fs::detail::unescapeName(subdirName);
+ segmentName = util::fs::detail::extractLastDirectoryFromPath(segmentName);
ProviderSegment c(getMemoryBasePath(),
getSettings(),
getExportName(),
@@ -84,7 +85,7 @@ namespace armarx::armem::server::ltm
e.id() = id().getCoreSegmentID().cleanID();
- ARMARX_INFO << VAROUT(id());
+ //ARMARX_INFO << VAROUT(id());
auto& conv = processors->defaultTypeConverter;
if (fileExists(DiskMemoryItemMixin::TYPE_FILENAME + conv.suffix))
@@ -106,6 +107,33 @@ namespace armarx::armem::server::ltm
});
}
+ void
+ CoreSegment::_loadLatestNReferences(int n, armem::wm::CoreSegment& e)
+ {
+ std::lock_guard l(ltm_mutex);
+
+ e.id() = id().getCoreSegmentID().cleanID();
+
+ auto& conv = processors->defaultTypeConverter;
+ if (fileExists(DiskMemoryItemMixin::TYPE_FILENAME + conv.suffix))
+ {
+ // load and set type
+ auto& conv = processors->defaultTypeConverter;
+
+ auto filecontent = readDataFromFile(DiskMemoryItemMixin::TYPE_FILENAME + conv.suffix);
+ auto aron = conv.convert(filecontent, "");
+ e.aronType() = aron;
+ }
+
+ forEachProviderSegment(
+ [&e, &n](auto& x)
+ {
+ armem::wm::ProviderSegment s;
+ x.loadLatestNReferences(n, s);
+ e.addProviderSegment(s);
+ });
+ }
+
void
CoreSegment::_resolve(armem::wm::CoreSegment& c)
{
@@ -127,7 +155,7 @@ namespace armarx::armem::server::ltm
}
void
- CoreSegment::_store(const armem::wm::CoreSegment& c)
+ CoreSegment::_store(const armem::wm::CoreSegment& c, bool simulatedVersion)
{
std::lock_guard l(ltm_mutex);
@@ -179,7 +207,7 @@ namespace armarx::armem::server::ltm
id().withProviderSegmentName(prov.id().providerSegmentName),
processors);
- c.store(prov);
+ c.store(prov, simulatedVersion);
});
}
} // namespace armarx::armem::server::ltm
diff --git a/source/RobotAPI/libraries/armem/server/ltm/CoreSegment.h b/source/RobotAPI/libraries/armem/server/ltm/CoreSegment.h
index 14d8b0abab73608e6eba0de5fa97734764a1a9ad..1c36d1436f03d185419c638cf88964d4f5a1656f 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/CoreSegment.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/CoreSegment.h
@@ -28,8 +28,9 @@ namespace armarx::armem::server::ltm
protected:
void _loadAllReferences(armem::wm::CoreSegment&) override;
+ void _loadLatestNReferences(int n, armem::wm::CoreSegment& c) override;
void _resolve(armem::wm::CoreSegment&) override;
- void _store(const armem::wm::CoreSegment&) override;
+ void _store(const armem::wm::CoreSegment&, bool simulatedVersion) override;
private:
};
diff --git a/source/RobotAPI/libraries/armem/server/ltm/Entity.cpp b/source/RobotAPI/libraries/armem/server/ltm/Entity.cpp
index ac9433dfb78be5a973925feb9c917a67699f1d7e..7e4bdc4eef7809a7361c01d3b414809734a968fb 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/Entity.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/Entity.cpp
@@ -53,15 +53,6 @@ namespace armarx::armem::server::ltm
}
- // check if this is already a microsec folder (legacy export support)
- //if (std::stol(secName) > 1647524607 ) // the time in us the new export was implemented
- //{
- // EntitySnapshot c(memoryParentPath, id().withTimestamp(timeFromStringMicroSeconds(secName)), processors, currentMode, currentExport);
- // func(c);
- // continue;
- //}
-
-
for (const auto& s : util::fs::getAllDirectories(d)) // seconds
{
if (!util::fs::detail::isNumberString(s.filename()))
@@ -372,6 +363,45 @@ namespace armarx::armem::server::ltm
});
}
+ void
+ Entity::_loadLatestNReferences(int n, armem::wm::Entity& e)
+ {
+ std::lock_guard l(ltm_mutex);
+ e.id() = id().getEntityID().cleanID();
+ int count = 0;
+
+ //this is a little bit ugly, TODO: find an easier way to count the snapshots
+ forEachSnapshot(
+ [&e, &count](auto& x)
+ {
+ count++;
+ }
+ );
+
+ int current = 0;
+
+ forEachSnapshot(
+ [&e, &n, ¤t, &count](auto& x)
+ {
+ if (!e.hasSnapshot(
+ x.id()
+ .timestamp)) // we only load the references if the snapshot is not existant
+ {
+ if (current >= (count - n)){
+ ARMARX_INFO << e.id().coreSegmentName << ": count: " << count << ", n: " << n << ", current: " << current;
+ ARMARX_DEBUG << "Loading snapshot with timestamp " << x.id().timestamp << " into WM";
+ armem::wm::EntitySnapshot s;
+ x.loadAllReferences(s);
+ x.resolve(s);
+ e.addSnapshot(s);
+ } else {
+ ARMARX_DEBUG << "Skipping snapshot with timestamp " << x.id().timestamp;
+ }
+ current++;
+ }
+ });
+ }
+
void
Entity::_resolve(armem::wm::Entity& p)
{
@@ -394,7 +424,7 @@ namespace armarx::armem::server::ltm
}
void
- Entity::_store(const armem::wm::Entity& c)
+ Entity::_store(const armem::wm::Entity& c, bool simulatedVersion)
{
std::lock_guard l(ltm_mutex);
if (id().entityName.empty())
@@ -435,17 +465,19 @@ namespace armarx::armem::server::ltm
for (auto& f : processors->snapFilters)
{
-
- bool accepted = f->accept(snap);
+ bool accepted = f->accept(snap, simulatedVersion);
+
if (!accepted)
{
- //ARMARX_INFO << "Ignoring to put an EntitiySnapshot into the LTM because it got filtered.";
+ ARMARX_DEBUG << "Ignoring to put an EntitySnapshot into the LTM because it got filtered.";
return;
} else {
//ARMARX_INFO << "Storing EntitySnapshot";
}
}
+ ARMARX_DEBUG << "Snapshot: " << c.id().timestamp << " of coreSegment " << c.id().coreSegmentName;
+
c.store(snap);
statistics.recordedSnapshots++;
});
diff --git a/source/RobotAPI/libraries/armem/server/ltm/Entity.h b/source/RobotAPI/libraries/armem/server/ltm/Entity.h
index 6afeed78767669a6ac6a445be7324a53d667967a..a873f444b3f0ea23a404eb66989990803d37edfc 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/Entity.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/Entity.h
@@ -47,8 +47,9 @@ namespace armarx::armem::server::ltm
protected:
void _loadAllReferences(armem::wm::Entity&) override;
+ void _loadLatestNReferences(int n, armem::wm::Entity& e) override;
void _resolve(armem::wm::Entity&) override;
- void _store(const armem::wm::Entity&) override;
+ void _store(const armem::wm::Entity& e, bool simulatedVersion) override;
private:
};
diff --git a/source/RobotAPI/libraries/armem/server/ltm/EntityInstance.cpp b/source/RobotAPI/libraries/armem/server/ltm/EntityInstance.cpp
index 0be6975beb6785dbb2426b74c0d70000edf0ce27..b0302a5cef0178acec1b539500bb591c9eb684f2 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/EntityInstance.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/EntityInstance.cpp
@@ -54,51 +54,6 @@ namespace armarx::armem::server::ltm
aron::data::DictPtr datadict = nullptr;
aron::data::DictPtr metadatadict = nullptr;
- /*if (connected() && collectionExists())
- {
- if (auto d = documentExists(); d)
- {
- nlohmann::json doc = *d;
-
- if (doc.contains(DATA))
- {
- std::vector<nlohmann::json> instances = doc[DATA];
- if (instances.size() > (size_t)id().instanceIndex)
- {
- nlohmann::json data = instances[id().instanceIndex];
- std::string dataStr = data.dump();
-
- std::vector<unsigned char> dataVec{dataStr.begin(), dataStr.end()};
- auto dataaron = dictConverter.convert({dataVec, ""}, {});
- datadict = aron::data::Dict::DynamicCastAndCheck(dataaron);
- }
- else
- {
- ARMARX_ERROR << "Could not find the instance key. Continuing without data.";
- }
- }
- else
- {
- ARMARX_ERROR << "Could not find the data key. Continuing without data.";
- }
-
- if (doc.contains(METADATA))
- {
- nlohmann::json metadata = doc[METADATA];
- std::string metadataStr = metadata.dump();
- std::vector<unsigned char> metadataVec{metadataStr.begin(), metadataStr.end()};
- auto metadataaron = dictConverter.convert({metadataVec, ""}, {});
- metadatadict = aron::data::Dict::DynamicCastAndCheck(metadataaron);
- }
- else
- {
- ARMARX_ERROR << "Could not find the metadata key. Continuing without metadata.";
- }
- }
- } else */
-
- //ARMARX_INFO << VAROUT(getFullPath());
-
if (fullPathExists())
{
diff --git a/source/RobotAPI/libraries/armem/server/ltm/Memory.cpp b/source/RobotAPI/libraries/armem/server/ltm/Memory.cpp
index bb2fd2ceb33dbc846f2d1eb5761008766fed4f78..a950f15410ed4a4283895e896d42856f835c5575 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/Memory.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/Memory.cpp
@@ -62,7 +62,7 @@ namespace armarx::armem::server::ltm
Memory::_disable()
{
BufferedBase::stop();
- MongoDBStorageMixin::stop();
+ //MongoDBStorageMixin::stop();
ARMARX_IMPORTANT << "Storing of data finished, starting to generate and save statistics...";
getAndSaveStatistics();
}
@@ -88,7 +88,8 @@ namespace armarx::armem::server::ltm
{
for (const auto& subdirName : getAllDirectories())
{
- std::string segmentName = util::fs::detail::unescapeName(subdirName);
+ const std::string& segmentName_one = util::fs::detail::unescapeName(subdirName);
+ auto segmentName = util::fs::detail::extractLastDirectoryFromPath(segmentName_one);
CoreSegment c(getMemoryBasePath(),
getSettings(),
getExportName(),
@@ -99,10 +100,14 @@ namespace armarx::armem::server::ltm
}
else
{
- ARMARX_WARNING << "Could not load the core segments of LTM " << id().str() << " as the path " << getFullPath().string() << " does not exist.";
+ ARMARX_WARNING << "Could not load the core segments of LTM "
+ << id().str()
+ << " as the path "
+ << getFullPath().string()
+ << " does not exist.";
}
- ARMARX_INFO << "All CoreSegments handeled";
+ ARMARX_DEBUG << "All CoreSegments handeled";
return true;
}
@@ -176,8 +181,6 @@ namespace armarx::armem::server::ltm
m.id() = id().getMemoryID().cleanID();
- ARMARX_INFO << VAROUT(id());
-
forEachCoreSegment(
[&m](auto& x)
{
@@ -187,11 +190,51 @@ namespace armarx::armem::server::ltm
});
}
+ void
+ Memory::_loadLatestNReferences(int n, armem::wm::Memory& m)
+ {
+ std::lock_guard l(ltm_mutex);
+ m.id() = id().getMemoryID().cleanID();
+
+ forEachCoreSegment(
+ [&m, &n](auto& x)
+ {
+ armem::wm::CoreSegment s;
+ x.loadLatestNReferences(n, s);
+ m.addCoreSegment(s);
+ });
+ }
+
+ void
+ Memory::_loadLatestNReferences(int n, armem::wm::Memory& m, std::list<std::string> coreSegNames)
+ {
+ std::lock_guard l(ltm_mutex);
+ m.id() = id().getMemoryID().cleanID();
+
+ forEachCoreSegment(
+ [&m, &n, &coreSegNames](auto& x)
+ {
+ bool loadCoreSeg = (std::find(coreSegNames.begin(), coreSegNames.end(), x.id().coreSegmentName) != coreSegNames.end());
+ if(loadCoreSeg){
+ armem::wm::CoreSegment s;
+ x.loadLatestNReferences(n, s);
+ m.addCoreSegment(s);
+ } else {
+ ARMARX_DEBUG << "Skipping loading CoreSegment with name "
+ << x.id().coreSegmentName
+ << " from LTM into WM as it is not in the defined list";
+ }
+
+ });
+ }
+
void
Memory::_resolve(armem::wm::Memory& m)
{
std::lock_guard l(ltm_mutex); // we cannot load a memory multiple times simultaneously
+ ARMARX_DEBUG << VAROUT(fullPathExists());
+
if (/*(connected() && collectionExists()) ||*/ fullPathExists())
{
m.forEachCoreSegment(
@@ -205,6 +248,9 @@ namespace armarx::armem::server::ltm
processors);
c.resolve(e);
});
+ } else {
+ ARMARX_WARNING << "You are trying to resolve an LTM from a path that does not exist: "
+ << getFullPath();
}
}
@@ -215,7 +261,7 @@ namespace armarx::armem::server::ltm
}
void
- Memory::_directlyStore(const armem::wm::Memory& memory)
+ Memory::_directlyStore(const armem::wm::Memory& memory, bool simulatedVersion)
{
std::lock_guard l(ltm_mutex); // we cannot store a memory multiple times simultaneously
@@ -234,6 +280,8 @@ namespace armarx::armem::server::ltm
return;
}*/
+ ARMARX_DEBUG << "CoreSegments: " << memory.getCoreSegmentNames().size();
+
memory.forEachCoreSegment(
[&](const auto& core)
{
@@ -244,7 +292,8 @@ namespace armarx::armem::server::ltm
processors);
// 2. store data
- c.store(core);
+ c.store(core, simulatedVersion);
+
// 3. update statistics
statistics.recordedCoreSegments++;
@@ -254,6 +303,12 @@ namespace armarx::armem::server::ltm
//CachedBase::addToCache(memory);
}
+ void
+ Memory::_loadOnStartup(){
+ //not used any more
+
+ }
+
void
Memory::getAndSaveStatistics()
{
diff --git a/source/RobotAPI/libraries/armem/server/ltm/Memory.h b/source/RobotAPI/libraries/armem/server/ltm/Memory.h
index 05d04551d683cb3fb3fb773ca97ebb6cee8c3b6e..bb022c1e2e4e196215eb7288f5dc0a28d70b458a 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/Memory.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/Memory.h
@@ -1,6 +1,7 @@
#pragma once
#include <filesystem>
+#include <list>
// Base Class
#include "detail/MemoryBase.h"
@@ -56,9 +57,12 @@ namespace armarx::armem::server::ltm
protected:
void _loadAllReferences(armem::wm::Memory&) final;
+ void _loadLatestNReferences(int n, armem::wm::Memory& m) final;
+ void _loadLatestNReferences(int n, armem::wm::Memory& m, std::list<std::string> coreSegNames) final;
void _resolve(armem::wm::Memory&) final;
- void _store(const armem::wm::Memory&) final;
- void _directlyStore(const armem::wm::Memory&) final;
+ void _store(const armem::wm::Memory& m) final;
+ void _directlyStore(const armem::wm::Memory& m, bool simulatedVersion) final;
+ void _loadOnStartup() final;
private:
std::time_t current_date;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/ProviderSegment.cpp b/source/RobotAPI/libraries/armem/server/ltm/ProviderSegment.cpp
index 983dfb8943d5d541bbb62973165b78bb1a3160f1..f568b32137ab22872076c72c32e6078b03a2e99c 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/ProviderSegment.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/ProviderSegment.cpp
@@ -26,18 +26,6 @@ namespace armarx::armem::server::ltm
{
std::lock_guard l(ltm_mutex);
- /*if (connected() && collectionExists())
- {
- for (const auto& doc : getAllDocuments())
- {
- std::string id_str = doc[FOREIGN_KEY];
- armem::MemoryID segment_id(id_str);
- Entity c(
- getMemoryBasePath(), getSettings(), getExportName(), segment_id, processors);
- func(c);
- }
- } else */
-
if (fullPathExists())
{
@@ -45,6 +33,7 @@ namespace armarx::armem::server::ltm
{
std::string segmentName = util::fs::detail::unescapeName(subdirName);
+ segmentName = util::fs::detail::extractLastDirectoryFromPath(segmentName);
Entity c(getMemoryBasePath(),
getSettings(),
getExportName(),
@@ -61,16 +50,6 @@ namespace armarx::armem::server::ltm
{
std::lock_guard l(ltm_mutex);
- /*if (connected() && collectionExists())
- {
- auto c = Entity(getMemoryBasePath(),
- getSettings(),
- getExportName(),
- id().withEntityName(name),
- processors);
- return (bool)c.collectionExists();
- }*/
-
if (fullPathExists())
{
ARMARX_INFO << VAROUT(id().getEntityID());
@@ -119,40 +98,38 @@ namespace armarx::armem::server::ltm
e.aronType() = aron;
}
- ARMARX_INFO << VAROUT(id());
-
- /*
+ forEachEntity(
+ [&e](auto& x)
+ {
+ armem::wm::Entity s;
+ x.loadAllReferences(s);
+ e.addEntity(s);
+ });
+ }
- auto& conv = processors->defaultTypeConverter;
- auto setType = [&conv, &e](const std::vector<unsigned char>& aronJson)
- {
- auto typeAron = conv.convert(aronJson, "");
- e.aronType() = aron::type::Object::DynamicCastAndCheck(typeAron);
- };
+ void
+ ProviderSegment::_loadLatestNReferences(int n, armem::wm::ProviderSegment& e)
+ {
+ std::lock_guard l(ltm_mutex);
- */
+ e.id() = id().getProviderSegmentID().cleanID();
- /*if (connected() && collectionExists())
+ auto& conv = processors->defaultTypeConverter;
+ if (fileExists(DiskMemoryItemMixin::TYPE_FILENAME + conv.suffix))
{
- // TODO:
- } else */
-
- /*
+ // load and set type
+ auto& conv = processors->defaultTypeConverter;
- if (std::string filename = TYPE_FILENAME + conv.suffix;
- fullPathExists() && fileExists(filename))
- {
- auto typeFileContent = readDataFromFile(filename);
- setType(typeFileContent);
+ auto filecontent = readDataFromFile(DiskMemoryItemMixin::TYPE_FILENAME + conv.suffix);
+ auto aron = conv.convert(filecontent, "");
+ e.aronType() = aron;
}
- */
-
forEachEntity(
- [&e](auto& x)
+ [&e, &n](auto& x)
{
armem::wm::Entity s;
- x.loadAllReferences(s);
+ x.loadLatestNReferences(n, s);
e.addEntity(s);
});
}
@@ -178,7 +155,7 @@ namespace armarx::armem::server::ltm
}
void
- ProviderSegment::_store(const armem::wm::ProviderSegment& p)
+ ProviderSegment::_store(const armem::wm::ProviderSegment& p, bool simulatedVersion)
{
std::lock_guard l(ltm_mutex);
@@ -229,7 +206,7 @@ namespace armarx::armem::server::ltm
id().withEntityName(e.id().entityName),
processors);
- c.store(e);
+ c.store(e, simulatedVersion);
statistics.recordedEntities++;
});
}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/ProviderSegment.h b/source/RobotAPI/libraries/armem/server/ltm/ProviderSegment.h
index 5f880676c43f483c6ae66a65c41a7a17bf93aa70..056740808ca2508c3eb1e1e1ca99b9ff7f1e1362 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/ProviderSegment.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/ProviderSegment.h
@@ -28,8 +28,9 @@ namespace armarx::armem::server::ltm
protected:
void _loadAllReferences(armem::wm::ProviderSegment&) override;
+ void _loadLatestNReferences(int n, armem::wm::ProviderSegment& p) override;
void _resolve(armem::wm::ProviderSegment&) override;
- void _store(const armem::wm::ProviderSegment&) override;
+ void _store(const armem::wm::ProviderSegment&, bool simulatedVersion) override;
private:
};
diff --git a/source/RobotAPI/libraries/armem/server/ltm/detail/CoreSegmentBase.h b/source/RobotAPI/libraries/armem/server/ltm/detail/CoreSegmentBase.h
index e5a425016e06ff19c46ef7d768cf4e18b8dc8a02..ce70f089d74cb7beae72b037449676f087c4befd 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/detail/CoreSegmentBase.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/detail/CoreSegmentBase.h
@@ -35,6 +35,12 @@ namespace armarx::armem::server::ltm::detail
_loadAllReferences(coreSeg);
}
+ void
+ loadLatestNReferences(int n, armem::wm::CoreSegment& coreSeg)
+ {
+ _loadLatestNReferences(n, coreSeg);
+ }
+
/// convert the references of the input into a wm::Memory
void
resolve(armem::wm::CoreSegment& coreSeg)
@@ -44,9 +50,9 @@ namespace armarx::armem::server::ltm::detail
/// encode the content of a wm::Memory and store
void
- store(const armem::wm::CoreSegment& coreSeg)
+ store(const armem::wm::CoreSegment& coreSeg, bool simulatedVersion)
{
- _store(coreSeg);
+ _store(coreSeg, simulatedVersion);
}
/// statistics
@@ -87,8 +93,9 @@ namespace armarx::armem::server::ltm::detail
protected:
virtual void _loadAllReferences(armem::wm::CoreSegment&) = 0;
+ virtual void _loadLatestNReferences(int n, armem::wm::CoreSegment& c) = 0;
virtual void _resolve(armem::wm::CoreSegment&) = 0;
- virtual void _store(const armem::wm::CoreSegment&) = 0;
+ virtual void _store(const armem::wm::CoreSegment& c, bool simulatedVersion=false) = 0;
protected:
mutable std::recursive_mutex ltm_mutex;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/detail/EntityBase.h b/source/RobotAPI/libraries/armem/server/ltm/detail/EntityBase.h
index ab61c63e0b82cfa6fba6cbb16d76331ff0027d73..3de97348618f77988bf87f92d5c97c2508f08fb5 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/detail/EntityBase.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/detail/EntityBase.h
@@ -35,6 +35,12 @@ namespace armarx::armem::server::ltm::detail
_loadAllReferences(e);
}
+ void
+ loadLatestNReferences(int n, armem::wm::Entity& e)
+ {
+ _loadLatestNReferences(n, e);
+ }
+
/// convert the references of the input into a wm::Memory
void
resolve(armem::wm::Entity& e)
@@ -44,9 +50,9 @@ namespace armarx::armem::server::ltm::detail
/// encode the content of a wm::Memory and store
void
- store(const armem::wm::Entity& e)
+ store(const armem::wm::Entity& e, bool simulatedVersion)
{
- _store(e);
+ _store(e, simulatedVersion);
}
/// statistics
@@ -100,8 +106,9 @@ namespace armarx::armem::server::ltm::detail
protected:
virtual void _loadAllReferences(armem::wm::Entity&) = 0;
+ virtual void _loadLatestNReferences(int n, armem::wm::Entity& e) = 0;
virtual void _resolve(armem::wm::Entity&) = 0;
- virtual void _store(const armem::wm::Entity&) = 0;
+ virtual void _store(const armem::wm::Entity& e, bool simulatedVersion=false) = 0;
protected:
mutable std::recursive_mutex ltm_mutex;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/detail/EntitySnapshotBase.h b/source/RobotAPI/libraries/armem/server/ltm/detail/EntitySnapshotBase.h
index cebbca9555f09e09da6b143056a024ac15b9d6a9..f6488bad8a09dcd3ae9c6204bc2a2babea1467c8 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/detail/EntitySnapshotBase.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/detail/EntitySnapshotBase.h
@@ -78,7 +78,7 @@ namespace armarx::armem::server::ltm::detail
protected:
virtual void _loadAllReferences(armem::wm::EntitySnapshot&) const = 0;
virtual void _resolve(armem::wm::EntitySnapshot&) const = 0;
- virtual void _store(const armem::wm::EntitySnapshot&) = 0;
+ virtual void _store(const armem::wm::EntitySnapshot& s) = 0;
protected:
mutable std::recursive_mutex ltm_mutex;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/detail/MemoryBase.h b/source/RobotAPI/libraries/armem/server/ltm/detail/MemoryBase.h
index 99df8e2e6177be8f39f019342352762724fa7bf1..3450eaaf7fa4ec6bbbc4736cf021fe275354a25a 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/detail/MemoryBase.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/detail/MemoryBase.h
@@ -1,6 +1,7 @@
#pragma once
#include <functional>
+#include <list>
// BaseClass
@@ -20,6 +21,16 @@
#include <RobotAPI/libraries/armem/server/test/ForgettingExperiments.h>
#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
+namespace armarx::armem::server::ltm
+{
+ enum class RecordingMode
+ {
+ CONSOLIDATE_REMOVED, // only store whats removed from the WM
+ CONSOLIDATE_ALL, // store whats in the WM (on commit)
+ CONSOLIDATE_LATEST // store the latest snapshots at a fixed frequency (for now 1Hz)
+ };
+}
+
namespace armarx::armem::server::ltm::detail
{
/// @brief Interface functions for the longterm memory classes
@@ -27,6 +38,8 @@ namespace armarx::armem::server::ltm::detail
class MemoryBase : public MemoryItem
{
public:
+
+
struct Statistics
{
armarx::core::time::DateTime lastEnabled = armarx::core::time::DateTime::Invalid();
@@ -47,6 +60,36 @@ namespace armarx::armem::server::ltm::detail
{
}
+ void setRecordingMode(const std::string& m)
+ {
+ if (m == "CONSOLIDATE_REMOVED")
+ {
+ this->recordingMode = RecordingMode::CONSOLIDATE_REMOVED;
+ }
+ else if (m == "CONSOLIDATE_ALL")
+ {
+ this->recordingMode = RecordingMode::CONSOLIDATE_ALL;
+ }
+ else if (m == "CONSOLIDATE_LATEST")
+ {
+ this->recordingMode = RecordingMode::CONSOLIDATE_LATEST;
+ }
+ else
+ {
+ ARMARX_WARNING << "Unknown recording mode: " << m;
+ }
+ }
+
+ void setRecordingMode(const RecordingMode m)
+ {
+ this->recordingMode = m;
+ }
+
+ RecordingMode getRecordingMode() const
+ {
+ return recordingMode;
+ }
+
/// initialize config
void
configure()
@@ -54,16 +97,19 @@ namespace armarx::armem::server::ltm::detail
bool en = p.enabled_on_startup;
ARMARX_INFO << VAROUT(p.configuration_on_startup);
ARMARX_INFO << VAROUT(p.export_name);
+ ARMARX_INFO << VAROUT(p.export_path);
+
this->setExportName(p.export_name);
+ this->setRecordingMode(p.recordingMode);
try
{
- const auto j = nlohmann::json::parse(p.configuration_on_startup);
+ const auto json = nlohmann::json::parse(p.configuration_on_startup);
// Processors are shared. So we only need to configure the root
- processors->configure(j);
+ processors->configure(json);
- this->_configure(j);
+ this->_configure(json);
}
catch (...)
{
@@ -131,6 +177,33 @@ namespace armarx::armem::server::ltm::detail
TIMING_END_STREAM(LTM_Memory_LoadAll, ARMARX_DEBUG);
}
+ //return the newest part of the ltm as a wm::Memory with only references
+ //will return the newest n snapshots of each entity
+ armem::wm::Memory
+ loadLatestNReferences(int n){
+ armem::wm::Memory ret;
+ loadLatestNReferences(n, ret);
+ return ret;
+ }
+
+ //return the newest part of the ltm as a wm::Memory with only references
+ //will return the newest n snapshots of each entity for only the CoreSegments matching one of the name sin the list
+ void
+ loadLatestNReferences(int n, armem::wm::Memory& memory, std::list<std::string> coreSegNames){
+ TIMING_START(LTM_Memory_loadNewestn_someCoreSegs);
+ _loadLatestNReferences(n, memory, coreSegNames);
+ TIMING_END_STREAM(LTM_Memory_loadNewestn_someCoreSegs, ARMARX_DEBUG);
+ }
+
+ void
+ loadLatestNReferences(int n, armem::wm::Memory& memory){
+ TIMING_START(LTM_Memory_loadNewestn);
+ _loadLatestNReferences(n, memory);
+ TIMING_END_STREAM(LTM_Memory_loadNewestn, ARMARX_DEBUG);
+ }
+
+
+
/// return the full ltm as a wm::Memory and resolves the references
/// the ltm may be huge, use with caution
armem::wm::Memory
@@ -182,8 +255,8 @@ namespace armarx::armem::server::ltm::detail
void
store(const armem::server::wm::Memory& serverMemory)
{
- wm::Memory memory;
- memory.update(armem::toCommit(serverMemory));
+ armem::wm::Memory memory(serverMemory.name());
+ memory.update(armem::toCommit(serverMemory), true, false);
this->store(memory);
}
@@ -201,8 +274,14 @@ namespace armarx::armem::server::ltm::detail
createPropertyDefinitions(PropertyDefinitionsPtr& defs, const std::string& prefix)
{
defs->optional(p.enabled_on_startup, prefix + "enabled");
+ defs->optional(p.recordingMode, prefix + "recordingMode");
defs->optional(p.configuration_on_startup, prefix + "configuration");
defs->optional(p.export_name, prefix + "exportName");
+ defs->optional(p.storeOnStop, prefix + "storeOnStop");
+
+ defs->optional(p.importOnStartUp, prefix + "importOnStartUp");
+ defs->optional(p.maxAmountOfSnapshotsLoaded, prefix + "maxAmountSnapshotsLoaded");
+ defs->optional(p.coreSegmentsToLoad, prefix + "loadedCoreSegments");
}
/// enable/disable
@@ -270,6 +349,12 @@ namespace armarx::armem::server::ltm::detail
return "LT-Memory";
}
+ void
+ loadOnStartup(){
+ _loadOnStartup();
+ }
+
+
protected:
/// configuration
virtual void
@@ -295,18 +380,35 @@ namespace armarx::armem::server::ltm::detail
virtual void _loadAllReferences(armem::wm::Memory& memory) = 0;
virtual void _resolve(armem::wm::Memory& memory) = 0;
virtual void _store(const armem::wm::Memory& memory) = 0;
- virtual void _directlyStore(const armem::wm::Memory& memory) = 0;
+ virtual void _directlyStore(const armem::wm::Memory& memory, bool simulatedVersion=false) = 0;
+ virtual void _loadOnStartup() = 0;
+ virtual void _loadLatestNReferences(int n, armem::wm::Memory& memory) = 0;
+ virtual void _loadLatestNReferences(int n, armem::wm::Memory& memory, std::list<std::string> coreSegNames) = 0;
public:
// stuff for scenario parameters
struct Properties
{
+ // whether the LTM is enabled on startup
bool enabled_on_startup = false;
- std::string configuration_on_startup =
- "{ \"SnapshotFrequencyFilter\": {\"WaitingTimeInMsForFilter\" : 50}, "
- "\"PngConverter\": {}, \"ExrConverter\": {}}"; //record with 20 fps as standard
+
+ // How data is stored, if the LTM is enabled
+ std::string recordingMode = "CONSOLIDATE_REMOVED";
+
+ // Name and export path (TODO: Should this be part of the base class? Export path is a disk memory thing)
std::string export_name = "MemoryExport";
std::string export_path = "/tmp/ltm";
+ bool storeOnStop = false;
+
+ // TODO: belong more to WM, but no good solution for that currently:
+ bool importOnStartUp = false;
+ int maxAmountOfSnapshotsLoaded = 1;
+ std::string coreSegmentsToLoad = "";
+
+ // Other configuration
+ std::string configuration_on_startup =
+ "{ \"SnapshotFrequencyFilter\": {\"WaitingTimeInMsForFilter\" : 50}}"; //record with 20 fps as standard
+
} p;
protected:
@@ -314,6 +416,8 @@ namespace armarx::armem::server::ltm::detail
mutable Statistics statistics;
- std::atomic_bool enabled = false;
+ std::atomic_bool enabled = true;
+
+ RecordingMode recordingMode = RecordingMode::CONSOLIDATE_REMOVED;
};
} // namespace armarx::armem::server::ltm::detail
diff --git a/source/RobotAPI/libraries/armem/server/ltm/detail/MemoryItem.cpp b/source/RobotAPI/libraries/armem/server/ltm/detail/MemoryItem.cpp
index 99dfcfee345d5d2015baffa7ef389a0c6879b3f9..29061d0eed8efc5ab8298daa5f84bb4f572bfd8e 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/detail/MemoryItem.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/detail/MemoryItem.cpp
@@ -26,6 +26,8 @@ namespace armarx::armem::server::ltm::detail
_setExportName(id);
}
+
+
void
MemoryItem::setMemoryID(const MemoryID& id)
{
diff --git a/source/RobotAPI/libraries/armem/server/ltm/detail/ProviderSegmentBase.h b/source/RobotAPI/libraries/armem/server/ltm/detail/ProviderSegmentBase.h
index 86f6306c700b9888636c6d7087cc849558a59417..91f853f92a44cf227f11569bca4442837d11ac8a 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/detail/ProviderSegmentBase.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/detail/ProviderSegmentBase.h
@@ -34,6 +34,11 @@ namespace armarx::armem::server::ltm::detail
_loadAllReferences(provSeg);
}
+ void loadLatestNReferences(int n, armem::wm::ProviderSegment& provSeg)
+ {
+ _loadLatestNReferences(n, provSeg);
+ }
+
/// convert the references of the input into a wm::Memory
void
resolve(armem::wm::ProviderSegment& provSeg)
@@ -43,9 +48,9 @@ namespace armarx::armem::server::ltm::detail
/// encode the content of a wm::Memory and store
void
- store(const armem::wm::ProviderSegment& provSeg)
+ store(const armem::wm::ProviderSegment& provSeg, bool simulatedVersion)
{
- _store(provSeg);
+ _store(provSeg, simulatedVersion);
}
/// statistics
@@ -84,8 +89,9 @@ namespace armarx::armem::server::ltm::detail
protected:
virtual void _loadAllReferences(armem::wm::ProviderSegment&) = 0;
+ virtual void _loadLatestNReferences(int n, armem::wm::ProviderSegment& p) = 0;
virtual void _resolve(armem::wm::ProviderSegment&) = 0;
- virtual void _store(const armem::wm::ProviderSegment&) = 0;
+ virtual void _store(const armem::wm::ProviderSegment& p, bool simulatedVersion=false) = 0;
protected:
mutable std::recursive_mutex ltm_mutex;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/BufferedMemoryMixin.h b/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/BufferedMemoryMixin.h
index 103811f410578b39acf50ab7575b0aa8208f5e6e..0a17f6adcac3f7d491e55047247b83deb10f3aa9 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/BufferedMemoryMixin.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/BufferedMemoryMixin.h
@@ -28,22 +28,23 @@ namespace armarx::armem::server::ltm::detail::mixin
virtual ~BufferedMemoryMixin() = default;
void
- directlyStore(const armem::wm::Memory& memory)
+ directlyStore(const armem::wm::Memory& memory, bool simulatedVersion=false)
{
std::lock_guard l(storeMutex);
TIMING_START(LTM_Memory_DirectlyStore);
- _directlyStore(memory);
+ _directlyStore(memory, simulatedVersion);
TIMING_END_STREAM(LTM_Memory_DirectlyStore, ARMARX_DEBUG);
}
void
- directlyStore(const armem::server::wm::Memory& serverMemory)
+ directlyStore(const armem::server::wm::Memory& serverMemory, bool simulatedVersion=false)
{
- wm::Memory memory;
+ armem::wm::Memory memory;
memory.setName(serverMemory.name());
- memory.update(armem::toCommit(serverMemory));
- this->directlyStore(memory);
+ auto ids = memory.update(armem::toCommit(serverMemory), true);
+ ARMARX_DEBUG << "Amount of ids in update: " << ids.size();
+ this->directlyStore(memory, simulatedVersion);
}
void
@@ -137,7 +138,7 @@ namespace armarx::armem::server::ltm::detail::mixin
defs->optional(storeFrequency, prefix + "storeFrequency");
}
- virtual void _directlyStore(const armem::wm::Memory& memory) = 0;
+ virtual void _directlyStore(const armem::wm::Memory& memory, bool simulatedVersion=false) = 0;
void
addToBuffer(const armem::wm::Memory& memory)
diff --git a/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/DiskStorageMixin.cpp b/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/DiskStorageMixin.cpp
index 19bfcd18bdb28870df2a9e69b1d0c74e387e07ba..a7fbaa4700d3b6908940cfa7185236254ec71ff7 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/DiskStorageMixin.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/DiskStorageMixin.cpp
@@ -12,6 +12,7 @@
// ArmarX
#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/time/TimeUtil.h>
namespace armarx::armem::server::ltm::detail::mixin
@@ -39,20 +40,13 @@ namespace armarx::armem::server::ltm::detail::mixin
std::filesystem::path current_base_path = n;
current_base_path.append(dateString);
- //inform user about change:
- ARMARX_DEBUG << "Changed memory base path to include current date of "
- << dateString
- << " to "
- << this->getMemoryBasePath()
- ;
-
return current_base_path;
}
void
DiskMemoryItemMixin::setMixinExportName(const std::string& n)
{
- ARMARX_INFO << "Currently setting export name to: " << n;
+ ARMARX_DEBUG << "Currently setting export name to: " << n;
exportName = n;
}
@@ -76,24 +70,34 @@ namespace armarx::armem::server::ltm::detail::mixin
Path
DiskMemoryItemMixin::getMemoryBasePath() const
{
- if(memoryBasePathString.empty()){
- return memoryBasePath;
- } else {
- std::filesystem::path newPath;
- newPath.assign(memoryBasePathString);
- return this->addDateToMemoryBasePath(newPath);
- }
+ std::string p = [&](){
+ if(memoryBasePathString.empty()){
+ return memoryBasePath.string();
+ }
+
+ return memoryBasePathString;
+ }();
+
+ ArmarXDataPath::ReplaceEnvVars(p);
+
+ return p;
}
Path
DiskMemoryItemMixin::getFullPath() const
{
auto p = getMemoryBasePath() / exportName;
+ //p = this->addDateToMemoryBasePath(p);
//ARMARX_INFO << VAROUT(_id);
//ARMARX_INFO << VAROUT(_id.cleanID());
auto cleanID = _id.cleanID(); //somehow, the iDs are jumbled when loading the LTM from disk, this solves it for now
- return util::fs::toPath(p, cleanID);
+ auto fullPath = util::fs::toPath(p, cleanID);
+
+ //inform user about change:
+ ARMARX_DEBUG << "Full path is " << fullPath;
+
+ return fullPath;
}
bool
diff --git a/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/util/filesystem.cpp b/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/util/filesystem.cpp
index 0e78e8f01523391210eebe4c46147f41f32188ba..bb8f36bb24159e90bbef87825d5a0b80e97cf3f7 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/util/filesystem.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/util/filesystem.cpp
@@ -40,6 +40,18 @@ namespace armarx::armem::server::ltm ::util::fs
return ret;
}
+ std::string
+ extractLastDirectoryFromPath(const std::string& path)
+ {
+ size_t pos = path.rfind('/');
+
+ if (pos != std::string::npos) {
+ return path.substr(pos + 1);
+ } else {
+ return path;
+ }
+ }
+
std::string
toDayString(const Time& t)
{
diff --git a/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/util/filesystem.h b/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/util/filesystem.h
index 9b5b778cc7ee6b7466685537eebd07b5dfe08326..5bce52edd18af0677d08a458c72ebead6acc80e0 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/util/filesystem.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/util/filesystem.h
@@ -24,6 +24,8 @@ namespace armarx::armem::server::ltm::util::fs
bool isNumberString(const std::string& s);
bool isDateString(const std::string& s);
+
+ std::string extractLastDirectoryFromPath(const std::string& path);
} // namespace detail
std::filesystem::path toPath(const std::filesystem::path& base, const armem::MemoryID& id);
diff --git a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/Filter.h b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/Filter.h
index 7cd2ade2e06a3d623f9138b23bb85cccef9f1dd1..4bf8ec0493bf70a31a402ef7e196e1463c4de515 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/Filter.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/Filter.h
@@ -29,7 +29,7 @@ namespace armarx::armem::server::ltm::processor
SnapshotFilter() = default;
virtual ~SnapshotFilter() = default;
- virtual bool accept(const armem::wm::EntitySnapshot& e) = 0;
+ virtual bool accept(const armem::wm::EntitySnapshot& e, bool simulatedVersion = false) = 0;
virtual void configure(const nlohmann::json& json);
struct FilterStatistics {
diff --git a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/equalityFilter/EqualityFilter.cpp b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/equalityFilter/EqualityFilter.cpp
index 715520f7b51d362cfab9df471ef677e5d926e464..6f55b623a9fdc92a5ba518d63e2ace9c8003826b 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/equalityFilter/EqualityFilter.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/equalityFilter/EqualityFilter.cpp
@@ -9,7 +9,7 @@
namespace armarx::armem::server::ltm::processor::filter
{
bool
- SnapshotSimilarityFilter::accept(const armem::wm::EntitySnapshot& e)
+ SnapshotSimilarityFilter::accept(const armem::wm::EntitySnapshot& e, bool simulatedVersion)
{
auto start = std::chrono::high_resolution_clock::now();
diff --git a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/equalityFilter/EqualityFilter.h b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/equalityFilter/EqualityFilter.h
index e5d3a7b1fac491353895ecfa44fbb9df089b4578..407290de8f18931e71b79a97efe49cd28930405c 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/equalityFilter/EqualityFilter.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/equalityFilter/EqualityFilter.h
@@ -25,7 +25,7 @@ namespace armarx::armem::server::ltm::processor::filter
SnapshotSimilarityFilter() = default;
- virtual bool accept(const armem::wm::EntitySnapshot& e) override;
+ virtual bool accept(const armem::wm::EntitySnapshot& e, bool simulatedVersion) override;
void configure(const nlohmann::json& json) override;
FilterStatistics getFilterStatistics() override;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/frequencyFilter/FrequencyFilter.cpp b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/frequencyFilter/FrequencyFilter.cpp
index 2dd6e1d7c09c0f5757842c5d8bf07d869d51dd68..066272e170c89c44e3bbc8bf3d7dc1d4177d13ca 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/frequencyFilter/FrequencyFilter.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/frequencyFilter/FrequencyFilter.cpp
@@ -6,7 +6,7 @@ namespace armarx::armem::server::ltm::processor::filter
{
bool
- SnapshotFrequencyFilter::accept(const armem::wm::EntitySnapshot &e)
+ SnapshotFrequencyFilter::accept(const armem::wm::EntitySnapshot &e, bool simulatedVersion)
{
//accepting to many elements makes the filter slow and brings problems with the buffer with itself!
@@ -17,13 +17,24 @@ namespace armarx::armem::server::ltm::processor::filter
// get entity id of this snapshot:
auto memID = e.id().getEntityID();
+ //find out if timestamps are corrupted:
+ if(this->nonCorruptedType == TimestampType::NOT_SET){
+ ARMARX_DEBUG << "Setting timestamp type";
+ this->setNonCorruptedTimestampType(e);
+ ARMARX_DEBUG << VAROUT(this->nonCorruptedType);
+ }
+
+ if(this->nonCorruptedType == TimestampType::ALL_CORRUPTED) {
+ return true;
+ }
+
if(this->lastTimesPerEntity.end() == this->lastTimesPerEntity.find(memID)){
//this happens if the key is not in the map, which means this is the first time this
//entity tries to save a snapshot
- ARMARX_INFO << "First time this entity is saved";
+ ARMARX_DEBUG << "First time this entity is saved";
auto firstIndex = e.getInstanceIndices()[0];
auto firstInsance = e.getInstance(firstIndex);
- auto lastT = firstInsance.metadata().sentTime.toMilliSecondsSinceEpoch();
+ auto lastT = this->getNonCorruptedTimestamp(firstInsance, simulatedVersion);
//for statistics sake:
auto end = std::chrono::high_resolution_clock::now();
stats.end_time = end;
@@ -33,16 +44,16 @@ namespace armarx::armem::server::ltm::processor::filter
this->lastTimesPerEntity[memID] = lastT;
return true; //the first one is always accepted
} else {
-
auto lastTime = this->lastTimesPerEntity.at(memID);
// check if any one of the instances for this snapshot were sent in the last x
// milliseconds since a snapshot was accepted last for this entity:
- e.forEachInstance([this, &instances_accepted, ¤t, &lastTime](armem::wm::EntityInstance& i){
- int difference = std::abs(i.metadata().sentTime.toMilliSecondsSinceEpoch() - lastTime);
+ e.forEachInstance([this, &instances_accepted, ¤t, &lastTime, &simulatedVersion](armem::wm::EntityInstance& i){
+ auto t = this->getNonCorruptedTimestamp(i, simulatedVersion);
+ int difference = std::abs(t - lastTime);
if(difference > this->maxDifference){ //at least one instance is older than the last saved instance
instances_accepted = true;
- current = i.metadata().sentTime.toMilliSecondsSinceEpoch();
+ current = this->getNonCorruptedTimestamp(i, simulatedVersion);
}
});
@@ -66,6 +77,99 @@ namespace armarx::armem::server::ltm::processor::filter
return instances_accepted;
}
+ void
+ SnapshotFrequencyFilter::setNonCorruptedTimestampType(const armem::wm::EntitySnapshot &e){
+ auto firstIndex = e.getInstanceIndices()[0];
+ auto firstInsance = e.getInstance(firstIndex);
+ auto sentTime = firstInsance.metadata().sentTime;
+ auto arrivedTime = firstInsance.metadata().arrivedTime;
+ auto referencedTime = firstInsance.metadata().referencedTime;
+
+ ARMARX_DEBUG << VAROUT(sentTime.toMilliSecondsSinceEpoch());
+ ARMARX_DEBUG << VAROUT(arrivedTime.toMilliSecondsSinceEpoch());
+ ARMARX_DEBUG << VAROUT(referencedTime.toMilliSecondsSinceEpoch());
+
+ //we want the referenced time, if not set correctly we take sent time, then arrvived time:
+ if(referencedTime.toMilliSecondsSinceEpoch() < 946688400000){
+ //timestamp corrupted:
+ if(sentTime.toMilliSecondsSinceEpoch() < 946688400000){
+ //sent also corrupted:
+ if(arrivedTime.toMilliSecondsSinceEpoch() < 946688400000){
+ //arrived also corrupted:
+ if(!corruptedWarningGiven){
+ ARMARX_WARNING << "LTM recording does not work correctly, as frequency filter is used, but "
+ << "time sent, arrived and referenced are all corrupted. \n"
+ << "Accepting all snapshots.";
+ this->nonCorruptedType = TimestampType::ALL_CORRUPTED;
+ corruptedWarningGiven = true;
+ }
+ } else {
+ if(!corruptedWarningGiven){ //only print this warning once
+ ARMARX_INFO << "Time referenced and sent for snapshot corrupted, using time arrived for filtering";
+ corruptedWarningGiven = true;
+ }
+ this->nonCorruptedType = TimestampType::ARRIVED;
+ }
+ } else {
+ if(!corruptedWarningGiven){ //only print this warning once
+ ARMARX_INFO << "Time referenced snapshot corrupted, using time sent for filtering";
+ corruptedWarningGiven = true;
+ }
+ this->nonCorruptedType = TimestampType::SENT;
+ }
+ } else {
+ this->nonCorruptedType = TimestampType::REFERENCED;
+ }
+
+ /**
+ if(sentTime.toMilliSecondsSinceEpoch() < 946688400000){
+ // we assume the timestamp does not make sense if it is older than the year 2000
+ if(arrivedTime.toMilliSecondsSinceEpoch() < 946688400000){
+ if(referencedTime.toMilliSecondsSinceEpoch() < 946688400000){
+ ARMARX_WARNING << "LTM recording does not work correctly, as frequency filter is used, but "
+ << "time sent, arrived and referenced are all corrupted. \n"
+ << "Accepting all snapshots.";
+ this->nonCorruptedType = TimestampType::ALL_CORRUPTED;
+ } else {
+ if(!corruptedWarningGiven){ //only print this warning once
+ ARMARX_INFO << "Time sent and arrived for snapshot corrupted, using time referenced for filtering";
+ corruptedWarningGiven = true;
+ }
+ //use time referenced from now on
+ this->nonCorruptedType = TimestampType::REFERENCED;
+ }
+ } else {
+ if(!corruptedWarningGiven){ //only print this warning once
+ ARMARX_INFO << "Time sent for snapshot corrupted, using time arrived for filtering";
+ corruptedWarningGiven = true;
+ }
+ //use time arrived from now on
+ this->nonCorruptedType = TimestampType::ARRIVED;
+ }
+ } else {
+ this->nonCorruptedType = TimestampType::SENT;
+ }
+ */
+ }
+
+ int
+ SnapshotFrequencyFilter::getNonCorruptedTimestamp(const armem::wm::EntityInstance &i, bool simulatedVersion){
+ switch(this->nonCorruptedType) {
+ case TimestampType::SENT:
+ if(!simulatedVersion){
+ return i.metadata().sentTime.toMilliSecondsSinceEpoch();
+ } else {
+ [[fallthrough]];
+ }
+ case TimestampType::ARRIVED:
+ return i.metadata().arrivedTime.toMilliSecondsSinceEpoch();
+ case TimestampType::REFERENCED:
+ return i.metadata().referencedTime.toMilliSecondsSinceEpoch();
+ default:
+ return -1;
+ }
+ }
+
void
SnapshotFrequencyFilter::configure(const nlohmann::json& json)
{
diff --git a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/frequencyFilter/FrequencyFilter.h b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/frequencyFilter/FrequencyFilter.h
index e9c9e0021ac85603a775c02c359a2d700a44f54e..63e4493764e33d009b00b7b1de97001c27c49776 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/frequencyFilter/FrequencyFilter.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/frequencyFilter/FrequencyFilter.h
@@ -5,6 +5,13 @@
namespace armarx::armem::server::ltm::processor::filter
{
+ enum TimestampType{
+ NOT_SET = 0,
+ SENT = 1,
+ ARRIVED = 2,
+ REFERENCED = 3,
+ ALL_CORRUPTED = 4
+ };
class SnapshotFrequencyFilter : public SnapshotFilter
{
@@ -14,14 +21,20 @@ namespace armarx::armem::server::ltm::processor::filter
SnapshotFrequencyFilter() = default;
- virtual bool accept(const armem::wm::EntitySnapshot &e) override;
+ virtual bool accept(const armem::wm::EntitySnapshot &e, bool simulatedVersion) override;
void configure(const nlohmann::json& json) override;
FilterStatistics getFilterStatistics() override;
std::string getName() override;
private:
+ void setNonCorruptedTimestampType(const armem::wm::EntitySnapshot &e);
+ int getNonCorruptedTimestamp(const armem::wm::EntityInstance &i, bool simulatedVersion);
+
std::map<MemoryID, std::int64_t> lastTimesPerEntity;
int maxDifference = 0;
+ bool corruptedWarningGiven = false;
+ TimestampType nonCorruptedType = TimestampType::NOT_SET;
+
};
} // namespace armarx::armem::server::ltm::processor::filter
diff --git a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/importanceFilter/ImportanceFilter.cpp b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/importanceFilter/ImportanceFilter.cpp
index 0ae255a4d4488048c7786aa1fafa3d67f2538f77..06516bd7894d0974f2b9341952cfff1b6bc467de 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/importanceFilter/ImportanceFilter.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/importanceFilter/ImportanceFilter.cpp
@@ -4,7 +4,7 @@
namespace armarx::armem::server::ltm::processor::filter
{
-bool SnapshotImportanceFilter::accept(const armem::wm::EntitySnapshot &e)
+bool SnapshotImportanceFilter::accept(const armem::wm::EntitySnapshot &e, bool simulatedVersion)
{
auto start = std::chrono::high_resolution_clock::now();
bool instances_accepted = false;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/importanceFilter/ImportanceFilter.h b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/importanceFilter/ImportanceFilter.h
index 929bb2e2ac6b03b31f9225cbda74b4a4b34f8e8d..5dcc76b57947a3a208dc1c3bf85ab190fce47dc3 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/importanceFilter/ImportanceFilter.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/importanceFilter/ImportanceFilter.h
@@ -23,7 +23,7 @@ public:
SnapshotImportanceFilter() = default;
- virtual bool accept(const armem::wm::EntitySnapshot& e) override;
+ virtual bool accept(const armem::wm::EntitySnapshot& e, bool simulatedVersion) override;
void configure(const nlohmann::json& json) override;
FilterStatistics getFilterStatistics() override;
diff --git a/source/RobotAPI/libraries/armem/server/plugins/Plugin.cpp b/source/RobotAPI/libraries/armem/server/plugins/Plugin.cpp
index e875bce8dd360038f832f1ff1248acb16e6587cf..ddc51ddbc9b9699302041fc37718983b6c808b4f 100644
--- a/source/RobotAPI/libraries/armem/server/plugins/Plugin.cpp
+++ b/source/RobotAPI/libraries/armem/server/plugins/Plugin.cpp
@@ -76,63 +76,44 @@ namespace armarx::armem::server::plugins
}
void
- Plugin::postOnConnectComponent()
+ Plugin::preOnConnectComponent()
{
Component& parent = this->parent<Component>();
- // register to MNS
+ // register new server info to MNS
if (clientPlugin->isMemoryNameSystemEnabled() and clientPlugin->getMemoryNameSystemClient())
{
- registerServer(parent);
+ registerServer(parent);
}
+ }
+
+ void
+ Plugin::postOnConnectComponent()
+ {
+ Component& parent = this->parent<Component>();
parent.getTopic(memoryTopic, memoryTopicName);
iceAdapter.setMemoryListener(memoryTopic);
+ this->iceAdapter.reloadFromLTM();
+
connected = true;
}
void
Plugin::preOnDisconnectComponent()
{
- /*
- ARMARX_INFO << "Preparing to save statistics for " << this->workingMemory.name();
- try
- {
- auto first_stats = longtermMemory.getFilterStatistics();
- std::map<std::string,
- std::map<std::string, ltm::processor::SnapshotFilter::FilterStatistics>>
- information;
- std::map<std::string, armarx::core::time::DateTime> times;
-
- try
- {
- times["Started LTM1"] = longtermMemory.getStatistics().firstStarted;
- times["Stopped LTM1"] = longtermMemory.getStatistics().firstStopped;
- information["LTM"] = first_stats;
+ //Storing rest of WM into LTM when component is shut down:
+ try{
+ if(longtermMemory.p.storeOnStop){
+ longtermMemory.directlyStore(workingMemory);
+ ARMARX_INFO << "Stored working memory contents into long-term memory on component stop";
+ } else {
+ ARMARX_DEBUG << "Not storing WM into LTM on stop, as longtermMemory.p.storeOnstop is " << longtermMemory.p.storeOnStop;
}
- catch (...)
- {
- ARMARX_INFO << "Something went wrong after getting the statistics";
- }
- auto exportPath = longtermMemory.getMemoryBasePath();
- auto exportName = longtermMemory.getExportName();
- auto recording_started = longtermMemory.getStatistics().firstStarted;
- auto recording_stopped = longtermMemory.getStatistics().firstStopped;
- test::save_statistics(information,
- times,
- recording_started,
- recording_stopped,
- exportPath,
- exportName,
- longtermMemory.name());
+ } catch(...){
+ ARMARX_WARNING << "Could not store working memory into the long-term memory on component stop.";
}
- catch (...)
- {
- ARMARX_INFO << "Something went wrong with the statistics saving process";
- }
-
- statistics_saved = true;
- */
+ //Storing statistics aout LTM recording:
try
{
if (longtermMemory.isRecording())
@@ -147,7 +128,7 @@ namespace armarx::armem::server::plugins
ARMARX_WARNING << "Statistics could not be saved for recording that was interrupted by "
"disconnecting the component";
}
-
+ //removing server:
if (clientPlugin->isMemoryNameSystemEnabled() and clientPlugin->getMemoryNameSystemClient())
{
removeServer();
diff --git a/source/RobotAPI/libraries/armem/server/plugins/Plugin.h b/source/RobotAPI/libraries/armem/server/plugins/Plugin.h
index 0712e4d9f2281adf2c435a7585a019b97907c9d7..0bb86b71baa485fea1a5f6c959ffff1647136445 100644
--- a/source/RobotAPI/libraries/armem/server/plugins/Plugin.h
+++ b/source/RobotAPI/libraries/armem/server/plugins/Plugin.h
@@ -31,6 +31,7 @@ namespace armarx::armem::server::plugins
virtual void preOnInitComponent() override;
virtual void postOnInitComponent() override;
+ virtual void preOnConnectComponent() override;
virtual void postOnConnectComponent() override;
virtual void preOnDisconnectComponent() override;
diff --git a/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.cpp b/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.cpp
index 1c039d569d1dc5007f72e37ca6a69e200ab07699..456d46dcf547004d64df538b851273136481b7e5 100644
--- a/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.cpp
+++ b/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.cpp
@@ -174,4 +174,11 @@ namespace armarx::armem::server::plugins
return getAvailableEngines();
}
+ armem::CommitResult
+ ReadWritePluginUser::reloadFromLTM()
+ {
+ iceAdapter().reloadFromLTM();
+ return armem::CommitResult();
+ }
+
} // namespace armarx::armem::server::plugins
diff --git a/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h b/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h
index 89dcb3239cc6a62275dbd744732f4a262474d447..935fb05989563d7cc4a2fda7cf4dc2bcf6ba5412 100644
--- a/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h
+++ b/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h
@@ -87,6 +87,9 @@ namespace armarx::armem::server::plugins
virtual armem::prediction::data::EngineSupportMap
getAvailableEngines(const ::Ice::Current&) override;
+ //ReloadFromLTMOnStartup
+ armem::CommitResult reloadFromLTM();
+
public:
Plugin& memoryServerPlugin();
diff --git a/source/RobotAPI/libraries/armem/server/segment/SpecializedCoreSegment.h b/source/RobotAPI/libraries/armem/server/segment/SpecializedCoreSegment.h
index 5ddca3e7d1dc0820a29039dba17af1e956d9897c..e34a65795c142a4629591e3f285bc58f9a869353 100644
--- a/source/RobotAPI/libraries/armem/server/segment/SpecializedCoreSegment.h
+++ b/source/RobotAPI/libraries/armem/server/segment/SpecializedCoreSegment.h
@@ -27,7 +27,7 @@ namespace armarx::armem::server::segment
MemoryToIceAdapter& iceMemory,
const std::string& defaultCoreSegmentName = "",
aron::type::ObjectPtr coreSegmentAronType = nullptr,
- int defaultMaxHistorySize = -1,
+ int defaultMaxHistorySize = 10,
const std::vector<PredictionEngine>& predictionEngines = {});
virtual ~SpecializedCoreSegment() override;
diff --git a/source/RobotAPI/libraries/armem/server/segment/SpecializedProviderSegment.h b/source/RobotAPI/libraries/armem/server/segment/SpecializedProviderSegment.h
index 3cb50c52f42ba2e2ca35a5024755729b9226336f..6cbac5cad70e5590e2ac190768d9aa38c86542a2 100644
--- a/source/RobotAPI/libraries/armem/server/segment/SpecializedProviderSegment.h
+++ b/source/RobotAPI/libraries/armem/server/segment/SpecializedProviderSegment.h
@@ -29,7 +29,7 @@ namespace armarx::armem::server::segment
const std::string& defaultCoreSegmentName = "",
aron::type::ObjectPtr providerSegmentAronType = nullptr,
aron::type::ObjectPtr coreSegmentAronType = nullptr,
- int defaultMaxHistorySize = -1,
+ int defaultMaxHistorySize = 10,
const std::vector<PredictionEngine>& providerSegmentPredictionEngines = {},
const std::vector<PredictionEngine>& coreSegmentPredictionEngines = {});
diff --git a/source/RobotAPI/libraries/armem/server/segment/constants.h b/source/RobotAPI/libraries/armem/server/segment/constants.h
new file mode 100644
index 0000000000000000000000000000000000000000..99c0d35d2eef902689a8290cc5798553efac4855
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/segment/constants.h
@@ -0,0 +1,4 @@
+namespace armarx::armem::server::segment::constants
+{
+ const int DEFAULT_MAX_HISTORY_SIZE = 10;
+}
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem/server/wm/detail/MaxHistorySize.cpp b/source/RobotAPI/libraries/armem/server/wm/detail/MaxHistorySize.cpp
index cccf47eef46ad8fd9f5d4990f000232b731dea19..bd6096c2fa6470d764b107046418435c8d9b0b38 100644
--- a/source/RobotAPI/libraries/armem/server/wm/detail/MaxHistorySize.cpp
+++ b/source/RobotAPI/libraries/armem/server/wm/detail/MaxHistorySize.cpp
@@ -5,11 +5,20 @@
namespace armarx::armem::server::wm::detail
{
- void MaxHistorySize::setMaxHistorySize(long maxSize)
+ void MaxHistorySize::setMaxHistorySize(long maxSize, const std::string& additionalInfo)
{
if (maxSize < 0){
- ARMARX_WARNING << "The maxHistorySize for this core segment is set to < 0. This means nothing will ever be forgotten in working memory. "
- << "This may slow down the memory server.";
+ if(additionalInfo == ""){
+ ARMARX_DEBUG << "The maxHistorySize for this memory entity is set to < 0. "
+ << "This means nothing will ever be forgotten in working memory. "
+ << "This may slow down the memory server. \n"
+ << "It is better to set the maxHistorySize per Provider- or CoreSegment";
+ } else {
+ ARMARX_DEBUG << "The maxHistorySize for this memory entity is set to < 0. "
+ << "This means nothing will ever be forgotten in working memory. "
+ << "This may slow down the memory server."
+ << "The entity name is " << additionalInfo;
+ }
}
this->_maxHistorySize = maxSize;
}
diff --git a/source/RobotAPI/libraries/armem/server/wm/detail/MaxHistorySize.h b/source/RobotAPI/libraries/armem/server/wm/detail/MaxHistorySize.h
index 64eb07a63d4d8087378856623ea47c2edcee3b2f..7d0bbd6691f7b0a57ffe231fe2bb4faf828fd488 100644
--- a/source/RobotAPI/libraries/armem/server/wm/detail/MaxHistorySize.h
+++ b/source/RobotAPI/libraries/armem/server/wm/detail/MaxHistorySize.h
@@ -12,7 +12,7 @@ namespace armarx::armem::server::wm::detail
* @brief Set the maximum number of snapshots to be contained in an entity.
* Affected entities are to be update right away.
*/
- void setMaxHistorySize(long maxSize);
+ void setMaxHistorySize(long maxSize, const std::string& additionalInfo = "");
long getMaxHistorySize() const;
diff --git a/source/RobotAPI/libraries/armem/server/wm/memory_definitions.cpp b/source/RobotAPI/libraries/armem/server/wm/memory_definitions.cpp
index 973e3ca5c21f1913f5abb84718789b4d4d27b2d2..b4dfb1cf6de2a847c5285208f8f157e9d57409ad 100644
--- a/source/RobotAPI/libraries/armem/server/wm/memory_definitions.cpp
+++ b/source/RobotAPI/libraries/armem/server/wm/memory_definitions.cpp
@@ -16,7 +16,8 @@ namespace armarx::armem::server::wm
void Entity::setMaxHistorySize(long maxSize)
{
- MaxHistorySize::setMaxHistorySize(maxSize);
+ std::string containerID = (this->id()).str();
+ MaxHistorySize::setMaxHistorySize(maxSize, containerID);
truncate();
}
diff --git a/source/RobotAPI/libraries/armem/server/wm/memory_definitions.h b/source/RobotAPI/libraries/armem/server/wm/memory_definitions.h
index 35015353a2653ca957bb19fe29c7e8430790885d..1249dcf039eac37545cc3b93cdf978eaa61be57f 100644
--- a/source/RobotAPI/libraries/armem/server/wm/memory_definitions.h
+++ b/source/RobotAPI/libraries/armem/server/wm/memory_definitions.h
@@ -101,7 +101,15 @@ namespace armarx::armem::server::wm
ProviderSegment& addProviderSegment(const std::string& name, Args... args)
{
ProviderSegmentT& added = CoreSegmentBase::addProviderSegment(name, args...);
- added.setMaxHistorySize(this->getMaxHistorySize());
+ int maxHistorySize = this->getMaxHistorySize();
+ if(maxHistorySize < 0){
+ ARMARX_INFO << "The maxHistorySize for this core segment is set to < 0. "
+ << "This means nothing will ever be forgotten in working memory. "
+ << "This may slow down the memory server. \n"
+ << "Core Segment Name: "
+ << (this->id()).str();
+ }
+ added.setMaxHistorySize(maxHistorySize);
return added;
}
diff --git a/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.cpp b/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.cpp
index 12dc0c0abe73d495df37a7bea6fcec19197baeb3..3e1741e91c0d1d70c81c0780e8689be3f8d171fc 100644
--- a/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.cpp
+++ b/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.cpp
@@ -31,6 +31,9 @@ namespace armarx::armem::gui
vboxlayout->setMargin(1);
vboxlayout->setSizeConstraint(QLayout::SizeConstraint::SetMinAndMaxSize);
vboxlayout->setAlignment(Qt::AlignTop);
+ _de_selectMemoryServers = new QPushButton(selectText);
+ allMemoryServersSelected = false;
+ vboxlayout->addWidget(_de_selectMemoryServers);
_availableMemoriesGroupBox->setLayout(vboxlayout);
hlayout->addWidget(_availableMemoriesGroupBox);
}
@@ -138,6 +141,8 @@ namespace armarx::armem::gui
connect(
_dataCheckBox, &QCheckBox::stateChanged, this, &This::setRecursionDepthSpinnerEnabled);
+ connect(_de_selectMemoryServers, &QPushButton::pressed, this, &This::deSelectMemoryServers);
+
setLayout(vlayout);
}
@@ -148,8 +153,8 @@ namespace armarx::armem::gui
std::vector<std::string> alreadyPresentMemoryCheckboxes;
// get information about memories already present and activate inactive ones if necessary
- int maxIndex = _availableMemoriesGroupBox->layout()->count();
- for (int i = 0; i < maxIndex; ++i)
+ int maxIndex = _availableMemoriesGroupBox->layout()->count(); //as the (de)select button is counted as well
+ for (int i = 1; i < maxIndex; ++i)
{
auto w = _availableMemoriesGroupBox->layout()->itemAt(i)->widget();
QCheckBox* box = static_cast<QCheckBox*>(w);
@@ -184,7 +189,7 @@ namespace armarx::armem::gui
{
// new memory available
auto box = new QCheckBox(QString::fromStdString(memoryName));
- box->setChecked(false); // we do not want all memories to be enabled on startup, to reduce communication overhead
+ box->setChecked(true); // we do not want all memories to be enabled on startup, to reduce communication overhead
box->setVisible(true);
box->setEnabled(true);
_availableMemoriesGroupBox->layout()->addWidget(box);
@@ -218,7 +223,7 @@ namespace armarx::armem::gui
std::map<std::string, QueryWidget::ActiveMemoryState> states;
int maxIndex = _availableMemoriesGroupBox->layout()->count();
- for (int i = 0; i < maxIndex; ++i)
+ for (int i = 1; i < maxIndex; ++i)
{
auto w = _availableMemoriesGroupBox->layout()->itemAt(i)->widget();
QCheckBox* box = static_cast<QCheckBox*>(w);
@@ -246,7 +251,7 @@ namespace armarx::armem::gui
std::vector<std::string> enabledMemoryCheckboxes;
int maxIndex = _availableMemoriesGroupBox->layout()->count();
- for (int i = 0; i < maxIndex; ++i)
+ for (int i = 1; i < maxIndex; ++i)
{
auto w = _availableMemoriesGroupBox->layout()->itemAt(i)->widget();
QCheckBox* box = static_cast<QCheckBox*>(w);
@@ -280,4 +285,45 @@ namespace armarx::armem::gui
break;
}
}
+
+ void
+ QueryWidget::deSelectMemoryServers()
+ {
+ if(!allMemoryServersSelected){
+ //ARMARX_INFO << "Selecting all memory servers";
+ //not all servers are selected -> select all memory servers:
+ std::scoped_lock l(enabledMemoriesMutex);
+
+ // get information about memories already present and activate inactive ones if necessary
+ int maxIndex = _availableMemoriesGroupBox->layout()->count(); //as the (de)select button is counted as well
+ for (int i = 1; i < maxIndex; ++i)
+ {
+ auto w = _availableMemoriesGroupBox->layout()->itemAt(i)->widget();
+ QCheckBox* box = static_cast<QCheckBox*>(w);
+ std::string memoryName = box->text().toStdString();
+ box->setChecked(true);
+ }
+ //set variables and text:
+ allMemoryServersSelected = true;
+ _de_selectMemoryServers->setText(deselectText);
+
+ } else {
+ //ARMARX_INFO << "Deselecting all memory servers";
+ //memories should be deselected:
+ std::scoped_lock l(enabledMemoriesMutex);
+
+ // get information about memories already present and activate inactive ones if necessary
+ int maxIndex = _availableMemoriesGroupBox->layout()->count(); //as the (de)select button is counted as well
+ for (int i = 1; i < maxIndex; ++i)
+ {
+ auto w = _availableMemoriesGroupBox->layout()->itemAt(i)->widget();
+ QCheckBox* box = static_cast<QCheckBox*>(w);
+ box->setChecked(false);
+ }
+
+ allMemoryServersSelected = false;
+ _de_selectMemoryServers->setText(selectText);
+ }
+ }
+
} // namespace armarx::armem::gui
diff --git a/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.h b/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.h
index 3cad51e99c69452de5541dbf10c8d21bcf4f4267..e1139a41df0eafe09c9004508fa7a92756093576 100644
--- a/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.h
+++ b/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.h
@@ -12,6 +12,7 @@ class QTabWidget;
class QPushButton;
class QGroupBox;
class QLineEdit;
+class QString;
namespace armarx::armem::gui
{
@@ -60,6 +61,8 @@ namespace armarx::armem::gui
private slots:
void setRecursionDepthSpinnerEnabled(int state);
+ void deSelectMemoryServers();
+
signals:
@@ -87,6 +90,11 @@ namespace armarx::armem::gui
QGroupBox* _additionalSettingsGroupBox;
QGroupBox* _availableMemoriesGroupBox;
+ QPushButton* _de_selectMemoryServers;
+ bool allMemoryServersSelected = false;
+ QString selectText = "Select all Memory Servers";
+ QString deselectText = "Deselect all Memory Servers";
+
mutable std::mutex enabledMemoriesMutex;
};
diff --git a/source/RobotAPI/libraries/armem_locations/CMakeLists.txt b/source/RobotAPI/libraries/armem_locations/CMakeLists.txt
index 38d927aa828a2e1d554492527093c0cb21b09035..474b117bc6a7fc5701938de7f4f24eed5c9a40e1 100644
--- a/source/RobotAPI/libraries/armem_locations/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_locations/CMakeLists.txt
@@ -12,7 +12,7 @@ armarx_add_library(
RobotAPI::Core
RobotAPI::armem
- armem_objects
+ RobotAPI::armem_objects
armem_robot_state
# aronjsonconverter
diff --git a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
index 70eab90df6a473558a51ff94fcdecd5446ba27f1..67c04e9c2c2157eea574359d116de9630b670801 100644
--- a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
@@ -3,6 +3,8 @@ set(LIB_NAME armem_objects)
armarx_component_set_name("${LIB_NAME}")
armarx_set_target("Library: ${LIB_NAME}")
+find_package(PCL QUIET)
+armarx_build_if(PCL_FOUND "PCL not available")
armarx_add_library(
LIBS
@@ -18,6 +20,8 @@ armarx_add_library(
RobotAPI::armem_robot_state
# armem_robot_state_aron
+ ${OpenCV_LIBS}
+
HEADERS
aron_conversions.h
aron_forward_declarations.h
@@ -53,6 +57,9 @@ armarx_add_library(
client/instance/ObjectReader.h
client/instance/ObjectWriter.h
+ client/familiar_object_instance/ObjectReader.h
+ client/familiar_object_instance/ObjectWriter.h
+
SOURCES
aron_conversions.cpp
memory_ids.cpp
@@ -74,9 +81,13 @@ armarx_add_library(
client/instance/ObjectReader.cpp
client/instance/ObjectWriter.cpp
+ client/familiar_object_instance/ObjectReader.cpp
+ client/familiar_object_instance/ObjectWriter.cpp
+
ARON_FILES
aron/ObjectClass.xml
aron/ObjectInstance.xml
+ aron/FamiliarObjectInstance.xml
aron/Attachment.xml
# aron/Constraint.xml
@@ -84,6 +95,14 @@ armarx_add_library(
aron/Marker.xml
)
+# ARON dependency
+if(PCL_FOUND)
+ target_include_directories(armem_objects
+ SYSTEM PUBLIC
+ ${PCL_INCLUDE_DIRS}
+ )
+endif()
+
add_library(${PROJECT_NAME}::armem_objects ALIAS armem_objects)
diff --git a/source/RobotAPI/libraries/armem_objects/aron/FamiliarObjectInstance.xml b/source/RobotAPI/libraries/armem_objects/aron/FamiliarObjectInstance.xml
new file mode 100644
index 0000000000000000000000000000000000000000..c583f1eedb1e774dd189c7298d9c0ec921f4a58a
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/aron/FamiliarObjectInstance.xml
@@ -0,0 +1,89 @@
+<!--
+Core segment type of Object/Instance.
+-->
+<?xml version="1.0" encoding="UTF-8"?>
+<AronTypeDefinition>
+ <AronIncludes>
+ <!-- <PackagePath package="RobotAPI" path="libraries/ArmarXObjects/aron/ObjectPose.xml" /> -->
+ <PackagePath package="RobotAPI" path="libraries/armem/aron/MemoryID.xml" />
+ <PackagePath package="RobotAPI" path="libraries/ArmarXObjects/aron/ObjectID.xml" />
+ <PackagePath package="RobotAPI" path="libraries/aron/common/aron/AxisAlignedBoundingBox.xml" />
+ </AronIncludes>
+
+ <GenerateTypes>
+
+ <Object name="armarx::armem::arondto::FamiliarObjectInstance">
+
+ <!-- id -->
+ <ObjectChild key="objectID">
+ <armarx::arondto::ObjectID />
+ </ObjectChild>
+
+ <!-- the object pose in the sensor frame, e.g., camera frame -->
+ <ObjectChild key="poseSensFrame">
+ <FramedPose />
+ </ObjectChild>
+
+ <!-- the object pose in the global frame -->
+ <ObjectChild key="poseGlobal">
+ <FramedPose optional="true" />
+ </ObjectChild>
+
+ <!-- the object class (can be anything) -->
+ <!-- <ObjectChild key="className">
+ <string />
+ </ObjectChild> -->
+
+ <!-- if the object classes / categories are from a dataset, list it here (e.g., COCO) -->
+ <!-- <ObjectChild key="dataset">
+ <string />
+ </ObjectChild> -->
+
+ <!-- point cloud representing the spatial knowledge about the object from a single frame
+ (or aggregated?) -->
+ <!-- Relative to poseSensFrame -->
+ <ObjectChild key="points">
+ <PointCloud type="PointXYZRGBA" />
+ </ObjectChild>
+
+ <!-- bounding box in the image -->
+ <!-- <ObjectChild key="bounding_box_img">
+ <PointCloud type="PointXYZRGB"/>
+ </ObjectChild> -->
+
+ <!-- bounding box in local sensFrame (see poseSensFrame) -->
+ <ObjectChild key="bounding_box">
+ <simox::arondto::AxisAlignedBoundingBox />
+ </ObjectChild>
+
+ <!-- cropped RGB image (based on bounding_box_img) -->
+ <!-- <ObjectChild key='rgb_image_patch'> -->
+ <!-- <Image type="rgb24" /> -->
+ <!-- <Image type="rgb24" optional="true"/> -->
+ <!-- </ObjectChild> -->
+
+ <!-- cropped depth image (based on bounding_box_img) -->
+ <!-- <ObjectChild key='depth_image_patch'> -->
+ <!-- <Image type="depth32" /> -->
+ <!-- <Image type="depth32" optional="true"/> -->
+ <!-- </ObjectChild> -->
+
+ <!-- reference for the source image (pair) -->
+ <ObjectChild key="imageSourceID">
+ <armarx::armem::arondto::MemoryID />
+ </ObjectChild>
+
+ <!-- timestmap -->
+ <ObjectChild key="timestamp">
+ <Time />
+ </ObjectChild>
+
+ <!-- confidence -->
+ <ObjectChild key="confidence">
+ <float32 />
+ </ObjectChild>
+
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
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 24e2a29d828214237ed8b9114fad4bbffd645762..e0ce311486d06c5dc859422976f4c96aa9d037dc 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
@@ -54,7 +54,7 @@ namespace armarx::armem::articulated_object
id.coreSegmentName = objects::constants::CoreClassSegmentName;
// listen to all provider segments!
- memoryNameSystem.subscribe(id, this, &Reader::updateKnownObjects);
+ // memoryNameSystem.subscribe(id, this, &Reader::updateKnownObjects);
}
void
diff --git a/source/RobotAPI/libraries/armem_objects/client/familiar_object_instance/ObjectReader.cpp b/source/RobotAPI/libraries/armem_objects/client/familiar_object_instance/ObjectReader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..02a62148d1c1e38d62687243526191a10dea4783
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/client/familiar_object_instance/ObjectReader.cpp
@@ -0,0 +1,239 @@
+#include "ObjectReader.h"
+
+#include <optional>
+
+#include <ArmarXCore/core/PackagePath.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/util/util.h>
+#include <RobotAPI/libraries/armem_objects/aron/Attachment.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron/FamiliarObjectInstance.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+#include <RobotAPI/libraries/armem_objects/constants.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+
+namespace armarx::armem::obj::familiar_object_instance
+{
+ void
+ Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ {
+ const std::string prefix = propertyPrefix;
+
+ def->optional(p.memoryName, prefix + "MemoryName");
+ }
+
+ void
+ Reader::connect(armem::client::MemoryNameSystem& memoryNameSystem)
+ {
+ ARMARX_IMPORTANT << "Reader: Waiting for memory '" << objects::constants::MemoryName
+ << "' ...";
+ try
+ {
+ memoryReader = memoryNameSystem.useReader(objects::constants::MemoryName);
+ ARMARX_IMPORTANT << "Reader: Connected to memory '" << objects::constants::MemoryName
+ << "'";
+ }
+ catch (const armem::error::CouldNotResolveMemoryServer& e)
+ {
+ ARMARX_ERROR << e.what();
+ return;
+ }
+ }
+
+ inline std::optional<arondto::FamiliarObjectInstance>
+ convert(const wm::EntityInstance& instance)
+ {
+ arondto::FamiliarObjectInstance aronBo;
+ try
+ {
+ aronBo.fromAron(instance.data());
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << "Conversion to ObjectPose failed!";
+ return std::nullopt;
+ }
+
+ return aronBo;
+ }
+
+ std::optional<std::map<std::string, arondto::FamiliarObjectInstance>>
+ Reader::queryLatestFamiliarObjectInstance(const ObjectID& instanceId,
+ const std::optional<std::string>& providerName) const
+ {
+
+ ARMARX_TRACE;
+
+ armem::client::query::Builder qb;
+ {
+ auto& cs = qb.coreSegments().withName(
+ objects::constants::CoreFamiliarObjectInstanceSegmentName);
+
+ client::query::ProviderSegmentSelector* providerSegments = nullptr;
+ if (providerName.has_value())
+ {
+ providerSegments = &cs.providerSegments().withName(providerName.value());
+ }
+ else
+ {
+ providerSegments = &cs.providerSegments().all();
+ }
+
+ providerSegments->entities().withName(instanceId.str()).snapshots().latest();
+ }
+
+
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+
+ if (not qResult.success) /* c++20 [[unlikely]] */
+ {
+ ARMARX_WARNING << qResult.errorMessage;
+ return std::nullopt;
+ }
+
+
+ std::map<std::string, arondto::FamiliarObjectInstance> result;
+
+ qResult.memory.forEachProviderSegment(
+ [&result](const wm::ProviderSegment& ps)
+ {
+ auto& thisResult = result[ps.name()];
+
+ ARMARX_CHECK_EQUAL(ps.getEntityNames().size(), 1)
+ << "Query returned more than one entity";
+
+ ps.forEachInstance(
+ [&thisResult](const wm::EntityInstance& e)
+ {
+ if (auto converted = convert(e))
+ {
+ thisResult = converted.value();
+ }
+ });
+ });
+
+ return result;
+ }
+
+ std::map<std::string, std::vector<arondto::FamiliarObjectInstance>>
+ Reader::queryAllLatestFamiliarObjectInstances(
+ const std::optional<std::string>& providerName) const
+ {
+ ARMARX_TRACE;
+
+ armem::client::query::Builder qb;
+ {
+ auto& cs = qb.coreSegments().withName(
+ objects::constants::CoreFamiliarObjectInstanceSegmentName);
+
+ client::query::ProviderSegmentSelector* providerSegments = nullptr;
+ if (providerName.has_value())
+ {
+ providerSegments = &cs.providerSegments().withName(providerName.value());
+ }
+ else
+ {
+ providerSegments = &cs.providerSegments().all();
+ }
+
+ providerSegments->entities().all().snapshots().latest();
+ }
+
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+
+ if (not qResult.success) /* c++20 [[unlikely]] */
+ {
+ ARMARX_WARNING << qResult.errorMessage;
+ return {};
+ }
+
+ std::map<std::string, std::vector<arondto::FamiliarObjectInstance>> result;
+
+ qResult.memory.forEachProviderSegment(
+ [&result](const wm::ProviderSegment& ps)
+ {
+ auto& thisResult = result[ps.name()];
+
+ ps.forEachInstance(
+ [&thisResult](const wm::EntityInstance& e)
+ {
+ if (auto converted = convert(e))
+ {
+ thisResult.push_back(converted.value());
+ }
+ });
+ });
+
+ return result;
+ }
+
+ std::map<std::string, std::vector<arondto::FamiliarObjectInstance>>
+ Reader::queryLatestFamiliarObjectInstancesFromClass(
+ const ObjectID& classId,
+ const std::optional<std::string>& providerName) const
+ {
+ ARMARX_TRACE;
+
+ armem::client::query::Builder qb;
+ {
+ auto& cs = qb.coreSegments().withName(
+ objects::constants::CoreFamiliarObjectInstanceSegmentName);
+
+ client::query::ProviderSegmentSelector* providerSegments = nullptr;
+ if (providerName.has_value())
+ {
+ providerSegments = &cs.providerSegments().withName(providerName.value());
+ }
+ else
+ {
+ providerSegments = &cs.providerSegments().all();
+ }
+
+ providerSegments->entities()
+ .withNamesStartingWith(classId.getClassID().str())
+ .snapshots()
+ .latest();
+ }
+
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+
+ if (not qResult.success) /* c++20 [[unlikely]] */
+ {
+ ARMARX_WARNING << qResult.errorMessage;
+ return {};
+ }
+
+ std::map<std::string, std::vector<arondto::FamiliarObjectInstance>> result;
+
+ qResult.memory.forEachProviderSegment(
+ [&result](const wm::ProviderSegment& ps)
+ {
+ auto& thisResult = result[ps.name()];
+
+ ps.forEachInstance(
+ [&thisResult](const wm::EntityInstance& e)
+ {
+ if (auto converted = convert(e))
+ {
+ thisResult.push_back(converted.value());
+ }
+ });
+ });
+
+
+ return result;
+ }
+
+} // namespace armarx::armem::obj::familiar_object_instance
diff --git a/source/RobotAPI/libraries/armem_objects/client/familiar_object_instance/ObjectReader.h b/source/RobotAPI/libraries/armem_objects/client/familiar_object_instance/ObjectReader.h
new file mode 100644
index 0000000000000000000000000000000000000000..48a165c43dc833535059898f86d92695f99f3a4f
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/client/familiar_object_instance/ObjectReader.h
@@ -0,0 +1,98 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <optional>
+
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+
+#include <RobotAPI/interface/objectpose/FamiliarObjectPoseStorageInterface.h>
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+#include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem_objects/aron/FamiliarObjectInstance.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/types.h>
+
+// The object pose provider. As long as the provider is not connected to armem we need the second proxy
+#include <RobotAPI/interface/objectpose/ObjectPoseProvider.h>
+#include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.h>
+
+namespace armarx::armem::obj::familiar_object_instance
+{
+ class Reader
+ {
+ public:
+ // using StoragePrxType = objpose::FamiliarObjectPoseStorageInterfacePrx;
+
+ struct Properties
+ {
+ std::string memoryName = "Object";
+ };
+
+ Reader() = default;
+ virtual ~Reader() = default;
+
+ void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
+ void connect(armem::client::MemoryNameSystem& memoryNameSystem);
+
+ // query existing instance from the memory. Requires an instance index to be set.
+ std::optional<std::map<std::string, arondto::FamiliarObjectInstance>>
+ queryLatestFamiliarObjectInstance(
+ const ObjectID& instanceId,
+ const std::optional<std::string>& providerName = std::nullopt) const;
+
+ // query the latest instances. Ignores the instance index.
+ std::map<std::string, std::vector<arondto::FamiliarObjectInstance>>
+ queryAllLatestFamiliarObjectInstances(
+ const std::optional<std::string>& providerName = std::nullopt) const;
+
+ // query all latest object instances.
+ std::map<std::string, std::vector<arondto::FamiliarObjectInstance>>
+ queryLatestFamiliarObjectInstancesFromClass(
+ const ObjectID& classId,
+ const std::optional<std::string>& providerName = std::nullopt) const;
+
+ Properties
+ getProperties()
+ {
+ return this->p;
+ }
+
+ // StoragePrxType
+ // getFamiliarObjectPoseStorage() const
+ // {
+ // return famObjPosStorage;
+ // }
+
+ private:
+ Properties p;
+
+ const std::string propertyPrefix = "mem.obj.familiar_object_instance.";
+
+ armem::client::Reader memoryReader;
+
+
+ // StoragePrxType famObjPosStorage;
+ };
+
+
+} // namespace armarx::armem::obj::familiar_object_instance
diff --git a/source/RobotAPI/libraries/armem_objects/client/familiar_object_instance/ObjectWriter.cpp b/source/RobotAPI/libraries/armem_objects/client/familiar_object_instance/ObjectWriter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f5a6e267ef75ad1e125e9ec44f8b2d02dded1188
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/client/familiar_object_instance/ObjectWriter.cpp
@@ -0,0 +1,79 @@
+#include "ObjectWriter.h"
+
+#include <ArmarXCore/core/PackagePath.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/util/util.h>
+#include <RobotAPI/libraries/armem_objects/aron/Attachment.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+
+namespace armarx::armem::obj::familiar_object_instance
+{
+ Writer::Writer() = default;
+
+ void
+ Writer::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ {
+ ARMARX_DEBUG << "Writer: registerPropertyDefinitions";
+
+ const std::string prefix = propertyPrefix;
+
+ def->optional(properties.memoryName, prefix + "MemoryName");
+
+ def->optional(properties.coreSegmentName,
+ prefix + "CoreSegment",
+ "Name of the memory core segment to use for object instances.");
+ }
+
+ void
+ Writer::connect(armem::client::MemoryNameSystem& memoryNameSystem)
+ {
+ // Wait for the memory to become available and add it as dependency.
+ ARMARX_IMPORTANT << "Writer: Waiting for memory '" << properties.memoryName << "' ...";
+ try
+ {
+ memoryWriter = memoryNameSystem.useWriter(properties.memoryName);
+ ARMARX_IMPORTANT << "Writer: Connected to memory '" << properties.memoryName << "'";
+ }
+ catch (const armem::error::CouldNotResolveMemoryServer& e)
+ {
+ ARMARX_ERROR << e.what();
+ return;
+ }
+ }
+
+ bool
+ Writer::commitObject(const armem::arondto::FamiliarObjectInstance& inst,
+ const std::string& provider,
+ const armem::Time& t)
+ {
+ armem::Commit c;
+ auto& e = c.add();
+
+ e.entityID.memoryName = properties.memoryName;
+ e.entityID.coreSegmentName = properties.coreSegmentName;
+ e.entityID.providerSegmentName = provider;
+
+ e.entityID.entityName = inst.objectID.dataset + "/" + inst.objectID.className + "/" +
+ inst.objectID.instanceName;
+ e.referencedTime = t;
+ e.sentTime = armem::Time::Now();
+ e.instancesData = {inst.toAron()};
+
+ auto res = memoryWriter.commit(c);
+
+ if (!res.allSuccess())
+ {
+ ARMARX_ERROR << "Failed to commit a FamiliarObjectInstance to memory: "
+ << res.allErrorMessages();
+ return false;
+ }
+ return true;
+ }
+
+} // namespace armarx::armem::obj::familiar_object_instance
diff --git a/source/RobotAPI/libraries/armem_objects/client/familiar_object_instance/ObjectWriter.h b/source/RobotAPI/libraries/armem_objects/client/familiar_object_instance/ObjectWriter.h
new file mode 100644
index 0000000000000000000000000000000000000000..ad6ffc6e5a309d4e67717479e617b9abff65f524
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/client/familiar_object_instance/ObjectWriter.h
@@ -0,0 +1,59 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+#include <RobotAPI/libraries/armem/client/Writer.h>
+#include <RobotAPI/libraries/armem_objects/aron/FamiliarObjectInstance.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/types.h>
+
+namespace armarx::armem::obj::familiar_object_instance
+{
+ class Writer
+ {
+ public:
+ Writer();
+ virtual ~Writer() = default;
+
+ void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
+ void connect(armem::client::MemoryNameSystem& memoryNameSystem);
+
+ bool commitObject(const armem::arondto::FamiliarObjectInstance& inst,
+ const std::string& provider,
+ const armem::Time&);
+
+ private:
+ struct Properties
+ {
+ std::string memoryName = "Object";
+ std::string coreSegmentName = "FamiliarObjectInstance";
+ } properties;
+
+ const std::string propertyPrefix = "mem.obj.familiar_object_instance.";
+
+ armem::client::Writer memoryWriter;
+ };
+
+
+} // namespace armarx::armem::obj::familiar_object_instance
diff --git a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp
index 131fdc655569a16b5d80488289ad2e0fa0d90983..05e850869fac0ce879b1a9b186b5acea99f98890 100644
--- a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp
@@ -26,25 +26,34 @@ namespace armarx::armem::obj::instance
{
const std::string prefix = propertyPrefix;
- def->optional(p.memoryName, prefix + "MemoryName");
+ def->optional(properties.memoryName, prefix + "MemoryName");
}
void
Reader::connect(armem::client::MemoryNameSystem& memoryNameSystem)
{
// Wait for the memory to become available and add it as dependency.
- ARMARX_IMPORTANT << "Waiting for memory '" << p.memoryName << "' ...";
+ ARMARX_IMPORTANT << "Waiting for memory '" << properties.memoryName << "' ...";
try
{
// simply wait until memory is ready. Do nothing with reader but get prx
- auto r = memoryNameSystem.useReader(p.memoryName);
+ auto r = memoryNameSystem.useReader(properties.memoryName);
+
+ // cast MemoryPrx to objPoseStoragePrx --> NOT WORKING DUE TO SOME ICE CACHING OR SO.
+ // Explanation:
+ // When the readingPrx disconnects and reconnects, the cast fails as it uses internally
+ // a ice_isA call which results in connection refused. The reason is, that for some
+ // reason the mns thinks that the reconnected prx is similar to the old info
+ // (check ice_identity) so it returnes the cached prx in its server map. Could be fixed
+ // by always querying the mns component for new proxies, but this may slow the system
+ // down.
+ //this->objPoseStorage =
+ // objpose::ObjectPoseStorageInterfacePrx::uncheckedCast(r.readingPrx);
+ // Current fix: Get prx from mns:
+ this->readingPrx = r.readingPrx;
+
+ ARMARX_INFO << "Connected to Memory '" << properties.memoryName << "'";
- // cast MemoryPrx to objPoseStoragePrx
- this->objPoseStorage =
- objpose::ObjectPoseStorageInterfacePrx::checkedCast(r.readingPrx);
-
- ARMARX_IMPORTANT << "Connected to Memory and ObjectPoseStorage '" << p.memoryName
- << "'";
}
catch (const armem::error::CouldNotResolveMemoryServer& e)
{
@@ -58,7 +67,7 @@ namespace armarx::armem::obj::instance
const armarx::core::time::Duration& until)
{
std::map<std::string, bool> ret;
- auto providers = objPoseStorage->getAvailableProvidersInfo();
+ auto providers = getObjectPoseStorage()->getAvailableProvidersInfo();
for (const auto& [k, p] : providers)
{
// TODO: check supported objects?
@@ -81,7 +90,7 @@ namespace armarx::armem::obj::instance
req.request.objectIDs = {requestObject};
req.request.relativeTimeoutMS = until.toMilliSeconds();
- auto requestResult = objPoseStorage->requestObjects(req);
+ auto requestResult = getObjectPoseStorage()->requestObjects(req);
if (requestResult.results.count(requestObject))
{
@@ -95,7 +104,7 @@ namespace armarx::armem::obj::instance
{
// TODO: Shall we throw an exception if no instance index is set?
- auto objectPoses = objPoseStorage->getObjectPoses();
+ auto objectPoses = getObjectPoseStorage()->getObjectPoses();
for (const auto& pose : objectPoses)
{
ObjectID oid;
@@ -114,7 +123,7 @@ namespace armarx::armem::obj::instance
Reader::queryLatestObjectInstances()
{
std::map<std::string, objpose::ObjectPose> ret;
- auto objectPoses = objPoseStorage->getObjectPoses();
+ auto objectPoses = getObjectPoseStorage()->getObjectPoses();
for (const auto& pose : objectPoses)
{
ObjectID oid;
@@ -128,7 +137,7 @@ namespace armarx::armem::obj::instance
Reader::queryLatestObjectInstances(const ObjectID& classId)
{
std::map<std::string, objpose::ObjectPose> ret;
- auto objectPoses = objPoseStorage->getObjectPoses();
+ auto objectPoses = getObjectPoseStorage()->getObjectPoses();
for (const auto& pose : objectPoses)
{
ObjectID oid;
diff --git a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h
index 18f52a6111d96525c0b5940b2e03084b8ff29a96..41769504c5bcf3096371dd7c4da774b3133ff7e6 100644
--- a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h
+++ b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h
@@ -34,6 +34,7 @@
// The object pose provider. As long as the provider is not connected to armem we need the second proxy
#include <RobotAPI/interface/objectpose/ObjectPoseProvider.h>
#include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.h>
+#include <RobotAPI/interface/armem/server/MemoryInterface.h>
namespace armarx::armem::obj::instance
{
@@ -71,21 +72,22 @@ namespace armarx::armem::obj::instance
Properties
getProperties()
{
- return this->p;
+ return this->properties;
}
objpose::ObjectPoseStorageInterfacePrx
getObjectPoseStorage() const
{
- return objPoseStorage;
+ // TODO: see problem in source file. This performs a ice_isA every time this method is called
+ return objpose::ObjectPoseStorageInterfacePrx::checkedCast(this->readingPrx);
}
private:
- Properties p;
+ Properties properties;
const std::string propertyPrefix = "mem.obj.instance.";
- objpose::ObjectPoseStorageInterfacePrx objPoseStorage;
+ armarx::armem::server::ReadingMemoryInterfacePrx readingPrx;
};
diff --git a/source/RobotAPI/libraries/armem_objects/constants.h b/source/RobotAPI/libraries/armem_objects/constants.h
index 2417346925c1b6352c3c425f3cb44c87dc0507e7..517d68bcf18af3a6e2e03a29d76c46d118157acb 100644
--- a/source/RobotAPI/libraries/armem_objects/constants.h
+++ b/source/RobotAPI/libraries/armem_objects/constants.h
@@ -25,6 +25,7 @@ namespace armarx::armem::objects::constants
{
inline const std::string MemoryName = "Object";
inline const std::string CoreInstanceSegmentName = "Instance";
+ inline const std::string CoreFamiliarObjectInstanceSegmentName = "FamiliarObjectInstance";
inline const std::string CoreClassSegmentName = "Class";
} // namespace armarx::armem_objects::constants
diff --git a/source/RobotAPI/libraries/armem_objects/memory_ids.cpp b/source/RobotAPI/libraries/armem_objects/memory_ids.cpp
index ad77419a2e5d3288aefae77a038cca62808e81dd..0ee0f6c5a9030eaa7e6be4cfd773a25d349bc100 100644
--- a/source/RobotAPI/libraries/armem_objects/memory_ids.cpp
+++ b/source/RobotAPI/libraries/armem_objects/memory_ids.cpp
@@ -31,5 +31,6 @@ namespace armarx::armem
const MemoryID objects::attachmentsSegmentID = memoryID.withCoreSegmentName("Attachments");
const MemoryID objects::classSegmentID = memoryID.withCoreSegmentName("Class");
const MemoryID objects::instaceSegmentID = memoryID.withCoreSegmentName("Instance");
+ const MemoryID objects::familiarObjectInstaceSegmentID = memoryID.withCoreSegmentName("FamiliarObjectInstance");
}
diff --git a/source/RobotAPI/libraries/armem_objects/memory_ids.h b/source/RobotAPI/libraries/armem_objects/memory_ids.h
index 50408b82f550fe2ecf80b4cc5d4930ee9b69b012..a39fb87ceee2b08de59664fa7f3fe2cbf619e183 100644
--- a/source/RobotAPI/libraries/armem_objects/memory_ids.h
+++ b/source/RobotAPI/libraries/armem_objects/memory_ids.h
@@ -33,6 +33,7 @@ namespace armarx::armem::objects
extern const MemoryID attachmentsSegmentID;
extern const MemoryID classSegmentID;
extern const MemoryID instaceSegmentID;
+ extern const MemoryID familiarObjectInstaceSegmentID;
} // namespace armarx::armem::objects
diff --git a/source/RobotAPI/libraries/armem_objects/server/CMakeLists.txt b/source/RobotAPI/libraries/armem_objects/server/CMakeLists.txt
index a04d95ca88d86fd722373456cccccddbd82c09a7..9731acbf802d5abe6234b562d79682ff251afe23 100644
--- a/source/RobotAPI/libraries/armem_objects/server/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_objects/server/CMakeLists.txt
@@ -37,6 +37,11 @@ armarx_add_library(
instance/visu/LinearPredictionsVisu.h
+ familiar_object_instance/Segment.h
+ familiar_object_instance/SegmentAdapter.h
+ familiar_object_instance/Decay.h
+ familiar_object_instance/Visu.h
+
attachments/Segment.h
SOURCES
@@ -52,6 +57,11 @@ armarx_add_library(
instance/visu/LinearPredictionsVisu.cpp
+ familiar_object_instance/Segment.cpp
+ familiar_object_instance/SegmentAdapter.cpp
+ familiar_object_instance/Decay.cpp
+ familiar_object_instance/Visu.cpp
+
attachments/Segment.cpp
)
diff --git a/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Decay.cpp b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Decay.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a24a023ec12f5edf595769949b1b5403dcdf8608
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Decay.cpp
@@ -0,0 +1,134 @@
+#include "Decay.h"
+
+#include <SimoxUtility/math/scale_value.h>
+
+#include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/core/time/DateTime.h>
+
+
+namespace armarx::armem::server::obj::familiar_object_instance
+{
+
+ void Decay::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ defs->optional(enabled, prefix + "enabled",
+ "If true, object poses decay over time when not localized anymore.");
+ defs->optional(delaySeconds, prefix + "delaySeconds",
+ "Duration after latest localization before decay starts.");
+ defs->optional(durationSeconds, prefix + "durationSeconds",
+ "How long to reach minimal confidence.");
+ defs->optional(maxConfidence, prefix + "maxConfidence",
+ "Confidence when decay starts.");
+ defs->optional(minConfidence, prefix + "minConfidence",
+ "Confidence after decay duration.");
+ defs->optional(removeObjectsBelowConfidence, prefix + "removeObjectsBelowConfidence",
+ "Remove objects whose confidence is lower than this value.");
+ }
+
+ void Decay::updateConfidence(objpose::ObjectPose& pose, const DateTime& now) const
+ {
+ if (pose.attachment or pose.isStatic)
+ {
+ pose.confidence = 1.0;
+ }
+ else
+ {
+ pose.confidence = calculateConfidence(pose.timestamp, now);
+ }
+ }
+
+ void Decay::updateConfidences(objpose::ObjectPoseSeq& objectPoses, const DateTime& now) const
+ {
+ for (objpose::ObjectPose& pose : objectPoses)
+ {
+ updateConfidence(pose, now);
+ }
+ }
+
+ float Decay::calculateConfidence(const DateTime& localization, const DateTime& now) const
+ {
+ const float duration = static_cast<float>((now - localization).toSecondsDouble());
+ if (duration < delaySeconds)
+ {
+ return maxConfidence;
+ }
+ else if (duration > delaySeconds + this->durationSeconds)
+ {
+ return minConfidence;
+ }
+ else
+ {
+ return simox::math::scale_value_from_to(
+ duration,
+ delaySeconds, delaySeconds + this->durationSeconds,
+ maxConfidence, minConfidence);
+ }
+ }
+
+
+
+ void Decay::RemoteGui::setup(const Decay& decay)
+ {
+ using namespace armarx::RemoteGui::Client;
+
+ enabled.setValue(decay.enabled);
+ {
+ float max = 1e6;
+ delaySeconds.setRange(0, max);
+ delaySeconds.setDecimals(2);
+ delaySeconds.setSteps(int(10 * max)); // = 0.1 steps
+ delaySeconds.setValue(decay.delaySeconds);
+ }
+ {
+ float max = 1e6;
+ durationSeconds.setRange(0, max);
+ durationSeconds.setDecimals(2);
+ durationSeconds.setSteps(int(10 * max)); // = 0.1 steps
+ durationSeconds.setValue(decay.durationSeconds);
+ }
+ {
+ maxConfidence.setRange(0, 1);
+ maxConfidence.setSteps(100);
+ maxConfidence.setValue(decay.maxConfidence);
+ }
+ {
+ minConfidence.setRange(0, 1);
+ minConfidence.setSteps(100);
+ minConfidence.setValue(decay.minConfidence);
+ }
+ {
+ removeObjectsBelowConfidence.setRange(0, 1);
+ removeObjectsBelowConfidence.setSteps(100);
+ removeObjectsBelowConfidence.setValue(decay.removeObjectsBelowConfidence);
+ }
+
+ GridLayout grid;
+ int row = 0;
+ grid.add(Label("Enabled"), {row, 0}).add(enabled, {row, 1});
+ row++;
+ grid.add(Label("Delay [sec]"), {row, 0}).add(delaySeconds, {row, 1});
+ row++;
+ grid.add(Label("Duration [sec]"), {row, 0}).add(durationSeconds, {row, 1});
+ row++;
+ grid.add(Label("Max Confidence"), {row, 0}).add(maxConfidence, {row, 1});
+ row++;
+ grid.add(Label("Min Confidence"), {row, 0}).add(minConfidence, {row, 1});
+ row++;
+ grid.add(Label("Remove Objects with Confidence < "), {row, 0}).add(removeObjectsBelowConfidence, {row, 1});
+ row++;
+
+ group.setLabel("Decay");
+ group.addChild(grid);
+ }
+
+ void Decay::RemoteGui::update(Decay& decay)
+ {
+ decay.enabled = enabled.getValue();
+ decay.delaySeconds = delaySeconds.getValue();
+ decay.durationSeconds = durationSeconds.getValue();
+ decay.maxConfidence = maxConfidence.getValue();
+ decay.minConfidence = minConfidence.getValue();
+ decay.removeObjectsBelowConfidence = removeObjectsBelowConfidence.getValue();
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Decay.h b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Decay.h
new file mode 100644
index 0000000000000000000000000000000000000000..6fc4967cf94bff14ac0ff2d0c576ab130525575f
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Decay.h
@@ -0,0 +1,67 @@
+#pragma once
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/services/tasks/TaskUtil.h>
+#include <ArmarXCore/core/time/forward_declarations.h>
+
+#include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
+
+
+namespace armarx::armem::server::obj::familiar_object_instance
+{
+
+ /**
+ * @brief Models decay of object localizations by decreasing the confidence
+ * the longer the object was not localized.
+ */
+ class Decay : public armarx::Logging
+ {
+ public:
+
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "decay.");
+
+ void updateConfidence(objpose::ObjectPose& pose, const DateTime& now) const;
+ void updateConfidences(objpose::ObjectPoseSeq& objectPoses, const DateTime& now) const;
+
+ private:
+
+ float calculateConfidence(const DateTime& localization, const DateTime& now) const;
+
+
+ public:
+
+ bool enabled = false;
+
+ /// Duration after latest localization before decay starts.
+ float delaySeconds = 5.0;
+ /// How long to reach minConfidence.
+ float durationSeconds = 20.0;
+
+ float maxConfidence = 1.0;
+ float minConfidence = 0.0f;
+
+ float removeObjectsBelowConfidence = 0.1f;
+
+
+ struct RemoteGui
+ {
+ armarx::RemoteGui::Client::GroupBox group;
+
+ armarx::RemoteGui::Client::CheckBox enabled;
+
+ armarx::RemoteGui::Client::FloatSpinBox delaySeconds;
+ armarx::RemoteGui::Client::FloatSpinBox durationSeconds;
+ armarx::RemoteGui::Client::FloatSlider maxConfidence;
+ armarx::RemoteGui::Client::FloatSlider minConfidence;
+
+ armarx::RemoteGui::Client::FloatSlider removeObjectsBelowConfidence;
+
+ void setup(const Decay& decay);
+ void update(Decay& decay);
+ };
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Segment.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a141b97e4e8b9d4b169ff5b9118c330b71080d5a
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Segment.cpp
@@ -0,0 +1,275 @@
+#include "Segment.h"
+
+#include <filesystem>
+#include <sstream>
+
+#include <sys/inotify.h>
+
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
+
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <SimoxUtility/algorithm/string.h>
+#include <SimoxUtility/json.h>
+#include <SimoxUtility/math/pose/pose.h>
+#include <SimoxUtility/math/regression/linear.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#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>
+#include <ArmarXCore/core/time/ice_conversions.h>
+
+#include "RobotAPI/libraries/ArmarXObjects/ObjectID.h"
+#include "RobotAPI/libraries/armem/server/wm/memory_definitions.h"
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+#include <RobotAPI/libraries/ArmarXObjects/Scene.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
+#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
+#include <RobotAPI/libraries/ArmarXObjects/json_conversions.h>
+#include <RobotAPI/libraries/armem/client/Writer.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/query/query_fns.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+#include <RobotAPI/libraries/armem/util/util.h>
+#include <RobotAPI/libraries/armem_objects/aron/FamiliarObjectInstance.aron.generated.h>
+#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/aron/common/aron_conversions.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
+
+namespace armarx::armem::server::obj::familiar_object_instance
+{
+
+ void
+ Segment::Calibration::defineProperties(PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ defs->optional(robotName,
+ prefix + "robotName",
+ "Name of robot whose note can be calibrated.\n"
+ "If not given, the 'fallbackName' is used.");
+ defs->optional(robotNode, prefix + "robotNode", "Robot node which can be calibrated.");
+ defs->optional(offset, prefix + "offset", "Offset for the node to be calibrated.");
+ }
+
+ Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
+ SpecializedCoreSegment(memoryToIceAdapter,
+ objects::familiarObjectInstaceSegmentID.coreSegmentName,
+ arondto::FamiliarObjectInstance::ToAronType(),
+ 64)
+ {
+ }
+
+ Segment::~Segment()
+ {
+ }
+
+ void
+ Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ SpecializedCoreSegment::defineProperties(defs, prefix);
+
+
+ defs->optional(robots.fallbackName,
+ prefix + "robots.FallbackName",
+ "Robot name to use as fallback if the robot name is not specified "
+ "in a provided object pose.");
+
+
+ decay.defineProperties(defs, prefix + "decay.");
+ }
+
+ void
+ Segment::init()
+ {
+ SpecializedCoreSegment::init();
+
+ robots.setTag(Logging::tag);
+ }
+
+ void
+ Segment::connect(viz::Client arviz)
+ {
+ (void)arviz;
+ // ARMARX_INFO << "ArticulatedObjectVisu";
+ // this->visu = std::make_unique<ArticulatedObjectVisu>(arviz, *this);
+ // visu->init();
+ }
+
+ void
+ Segment::commit(
+ const std::vector<armarx::armem::arondto::FamiliarObjectInstance>& familiarObjectInstances,
+ const std::string& providerName)
+ {
+ ARMARX_CHECK_NOT_EMPTY(providerName);
+
+ Time now = Time::Now();
+
+ ARMARX_CHECK_NOT_NULL(segmentPtr);
+ const MemoryID coreSegmentID = segmentPtr->id();
+
+ Commit commit;
+ for (const armarx::armem::arondto::FamiliarObjectInstance& familiarObjectInstance :
+ familiarObjectInstances)
+ {
+ EntityUpdate& update = commit.updates.emplace_back();
+
+ const MemoryID providerID = coreSegmentID.withProviderSegmentName(providerName);
+
+ armarx::ObjectID objectID;
+ fromAron(familiarObjectInstance.objectID, objectID);
+
+ update.entityID = providerID.withEntityName(objectID.str());
+ update.arrivedTime = now;
+ update.referencedTime = familiarObjectInstance.timestamp;
+ update.confidence = familiarObjectInstance.confidence;
+
+ update.instancesData.push_back(familiarObjectInstance.toAron());
+ }
+ // Commit non-locking.
+ ARMARX_INFO << "Committing " << commit.updates.size()
+ << " familiar object instances from provider " << providerName;
+ iceMemory.commit(commit);
+ }
+
+
+ std::vector<armarx::armem::arondto::FamiliarObjectInstance>
+ Segment::getFamiliarObjects(const DateTime& now)
+ {
+ std::vector<armarx::armem::arondto::FamiliarObjectInstance> familiarObjects;
+
+ segmentPtr->forEachEntity(
+ [&familiarObjects, &now](const wm::Entity& entity)
+ {
+ entity.getLatestSnapshot().forEachInstance(
+ [&familiarObjects, &now](const wm::EntityInstance& instance)
+ {
+ armarx::armem::arondto::FamiliarObjectInstance dto;
+ dto.fromAron(instance.data());
+
+ if (dto.timestamp <= now)
+ {
+ familiarObjects.push_back(dto);
+ }
+ });
+ });
+
+ return familiarObjects;
+ }
+
+ std::map<std::string, std::vector<armarx::armem::arondto::FamiliarObjectInstance>>
+ Segment::getFamiliarObjectsByProvider(const DateTime& now)
+ {
+ std::map<std::string, std::vector<armarx::armem::arondto::FamiliarObjectInstance>>
+ familiarObjectsByProvider;
+
+ segmentPtr->forEachProviderSegment(
+
+ [&now, &familiarObjectsByProvider](const wm::ProviderSegment& ps)
+ {
+ auto& familiarObjects = familiarObjectsByProvider[ps.name()];
+ ps.forEachEntity(
+ [&familiarObjects, &now](const wm::Entity& entity)
+ {
+ entity.getLatestSnapshot().forEachInstance(
+ [&familiarObjects, &now](const wm::EntityInstance& instance)
+ {
+ armarx::armem::arondto::FamiliarObjectInstance dto;
+ dto.fromAron(instance.data());
+
+ if (dto.timestamp <= now)
+ {
+ familiarObjects.push_back(dto);
+ }
+ });
+ });
+ }
+
+ );
+
+ return familiarObjectsByProvider;
+ }
+
+
+ VirtualRobot::RobotPtr
+ Segment::RobotsCache::get(const std::string& _robotName, const std::string& providerName)
+ {
+ std::string robotName = _robotName;
+
+ if (robotName.empty())
+ {
+ auto antiSpam = deactivateSpam(10 * 60); // 10 minutes.
+
+ std::stringstream ss;
+ if (providerName.empty())
+ {
+ ss << "The provided object pose";
+ }
+ else
+ {
+ ss << "The object pose provided by '" << providerName << "'";
+ }
+ ss << " does not specify a robot name";
+
+ if (fallbackName.empty())
+ {
+ ss << ", and no fallback robot name was configured (e.g. via the properties).\n"
+ << "For these object poses, the object instance segment is not able "
+ << "to provide transformed object poses (global and in robot root frame).";
+ ARMARX_INFO << antiSpam << ss.str();
+
+ return nullptr;
+ }
+ else
+ {
+ ss << ". The object instance segment is falling back to '" << fallbackName << "'.";
+ ARMARX_INFO << antiSpam << ss.str();
+
+ robotName = fallbackName;
+ }
+ }
+ ARMARX_CHECK_NOT_EMPTY(robotName);
+
+ // Lookup in cache.
+ if (auto it = loaded.find(robotName); it != loaded.end())
+ {
+ ARMARX_CHECK_NOT_NULL(it->second);
+ return it->second;
+ }
+ else
+ {
+ // Try to fetch the robot.
+ ARMARX_CHECK_NOT_NULL(reader);
+ VirtualRobot::RobotPtr robot = reader->getRobot(
+ robotName, Clock::Now(), VirtualRobot::RobotIO::RobotDescription::eStructure);
+
+ if (robot)
+ {
+ bool synchronized = reader->synchronizeRobot(*robot, Clock::Now());
+ if (not synchronized)
+ {
+ ARMARX_INFO << "The robot '" << robotName << "' could be loaded, but not"
+ << " synchronized successfully (e.g., the global localization "
+ "could be missing). "
+ << "Make sure to synchronize it before use if necessary.";
+ }
+ // Store robot if valid.
+ loaded.emplace(robotName, robot);
+ }
+ return robot; // valid or nullptr
+ }
+ }
+
+
+} // namespace armarx::armem::server::obj::familiar_object_instance
diff --git a/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Segment.h b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Segment.h
new file mode 100644
index 0000000000000000000000000000000000000000..f5fa90ad48d7b7a8d5529fc7b46e5d876f6b845f
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Segment.h
@@ -0,0 +1,123 @@
+#pragma once
+
+#include <experimental/memory>
+#include <map>
+#include <string>
+
+#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>
+#include <RobotAPI/interface/core/RobotState.h>
+#include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
+#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
+#include <RobotAPI/libraries/armem/core/Prediction.h>
+#include <RobotAPI/libraries/armem/server/segment/SpecializedSegment.h>
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem_objects/aron/FamiliarObjectInstance.aron.generated.h>
+
+#include "Decay.h"
+
+namespace armarx::armem::arondto
+{
+ class ObjectInstance;
+}
+
+namespace armarx::armem::server::obj::familiar_object_instance
+{
+
+ class Segment : public segment::SpecializedCoreSegment
+ {
+ public:
+ struct CommitStats
+ {
+ int numUpdated = 0;
+ };
+
+ using ObjectPose = objpose::ObjectPose;
+ using ObjectPoseSeq = objpose::ObjectPoseSeq;
+ using ObjectPoseMap = std::map<ObjectID, ObjectPose>;
+
+ struct Calibration
+ {
+ std::string robotName = "";
+ std::string robotNode = "Neck_2_Pitch";
+ float offset = 0.0f;
+
+ void defineProperties(armarx::PropertyDefinitionsPtr defs,
+ const std::string& prefix = "calibration.");
+ };
+
+
+ public:
+ Segment(server::MemoryToIceAdapter& iceMemory);
+ virtual ~Segment() override;
+
+
+ void defineProperties(armarx::PropertyDefinitionsPtr defs,
+ const std::string& prefix = "") override;
+ void init() override;
+ void connect(viz::Client arviz);
+
+ void commit(const std::vector<armarx::armem::arondto::FamiliarObjectInstance>&
+ familiarObjectInstances,
+ const std::string& providerName);
+
+
+ std::vector<armarx::armem::arondto::FamiliarObjectInstance>
+ getFamiliarObjects(const DateTime& now);
+
+ std::map<std::string, std::vector<armarx::armem::arondto::FamiliarObjectInstance>>
+ getFamiliarObjectsByProvider(const DateTime& now);
+
+ private:
+
+ friend struct DetachVisitor;
+
+
+ public:
+ /// Loaded robot models identified by the robot name.
+ struct RobotsCache : public armarx::Logging
+ {
+ std::experimental::observer_ptr<robot_state::VirtualRobotReader> reader;
+ std::string fallbackName;
+
+ std::map<std::string, VirtualRobot::RobotPtr> loaded;
+
+ VirtualRobot::RobotPtr get(const std::string& robotName,
+ const std::string& providerName = "");
+ };
+
+ RobotsCache robots;
+
+
+ objpose::ProviderInfoMap providers;
+
+
+ ObjectFinder objectFinder;
+
+ /// Decay model.
+ Decay decay;
+
+
+ private:
+ struct Properties
+ {
+
+ };
+
+ Properties p;
+
+
+ static const std::string timestampPlaceholder;
+
+
+ };
+
+} // namespace armarx::armem::server::obj::familiar_object_instance
diff --git a/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/SegmentAdapter.cpp b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/SegmentAdapter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a0e9444a3c80d78e1e8cb32bf3c925ac9c359a73
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/SegmentAdapter.cpp
@@ -0,0 +1,224 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::armem_objects::SegmentAdapter
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2024
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "SegmentAdapter.h"
+
+#include <iterator>
+#include <memory>
+#include <optional>
+
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <VirtualRobot/Robot.h>
+
+#include "ArmarXCore/core/time/Clock.h"
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/core/time/DateTime.h>
+#include <ArmarXCore/core/time/Duration.h>
+#include <ArmarXCore/core/time/Frequency.h>
+#include <ArmarXCore/core/time/Metronome.h>
+#include <ArmarXCore/core/time/ice_conversions.h>
+#include <ArmarXCore/observers/variant/Variant.h>
+
+#include "RobotAPI/libraries/core/FramedPose.h"
+#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
+#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
+#include <RobotAPI/libraries/ArmarXObjects/predictions.h>
+#include <RobotAPI/libraries/armem_objects/aron/FamiliarObjectInstance.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+#include <RobotAPI/libraries/aron/core/aron_conversions.h>
+#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
+
+namespace armarx::armem::server::obj::familiar_object_instance
+{
+
+ SegmentAdapter::SegmentAdapter(MemoryToIceAdapter& iceMemory) : segment(iceMemory)
+ {
+ }
+
+ std::string
+ SegmentAdapter::getName() const
+ {
+ return Logging::tag.tagName;
+ }
+
+ void
+ SegmentAdapter::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ segment.defineProperties(defs, prefix);
+ visu.defineProperties(defs, prefix + "visu.");
+ }
+
+ void
+ SegmentAdapter::init()
+ {
+ segment.setTag(getName());
+ segment.decay.setTag(getName());
+ // segment.setPredictionEngines(predictionEngines);
+ visu.setTag(getName());
+
+ segment.init();
+ }
+
+ void
+ SegmentAdapter::connect(
+ std::experimental::observer_ptr<robot_state::VirtualRobotReader> virtualRobotReader,
+ viz::Client arviz,
+ DebugObserverInterfacePrx debugObserver)
+ {
+ this->debugObserver = debugObserver;
+ this->arviz = arviz;
+
+ ARMARX_CHECK_NOT_NULL(virtualRobotReader);
+ segment.robots.reader = virtualRobotReader;
+
+ visu.arviz = arviz;
+
+ if (!visu.updateTask)
+ {
+ ARMARX_IMPORTANT << "Enabled visu";
+ visu.updateTask = new SimpleRunningTask<>([this]() { this->visualizeRun(); });
+ visu.updateTask->start();
+ }
+
+ segment.connect(arviz);
+ }
+
+ void
+ SegmentAdapter::reportFamiliarObjectPoses(
+ const std::string& providerName,
+ const ::armarx::objpose::ProvidedFamiliarObjectPoseSeq& data,
+ const Ice::Current&)
+ {
+
+ if (data.empty())
+ {
+ return;
+ }
+
+ // Note, that in general, timestamps in `data` can differ. Therefore, we maintain the sync timestamp
+ // and check whether we have to update the robot. Otherwise, we can avoid these Ice calls.
+ std::optional<armarx::DateTime> syncTimestamp;
+
+
+ const auto convertAndFillFamiliarObjectInstances =
+ [this, providerName, &syncTimestamp](const ::armarx::aron::data::dto::DictPtr& data)
+ -> armarx::armem::arondto::FamiliarObjectInstance
+ {
+ auto famObjInstance = armarx::armem::arondto::FamiliarObjectInstance::FromAron(data);
+
+
+ armarx::FramedPose framedPose(famObjInstance.poseSensFrame.pose,
+ famObjInstance.poseSensFrame.header.frame,
+ famObjInstance.poseSensFrame.header.agent);
+
+ ARMARX_CHECK_NOT_EMPTY(famObjInstance.poseSensFrame.header.agent);
+ ARMARX_CHECK_NOT_EMPTY(famObjInstance.poseSensFrame.header.frame);
+
+ VirtualRobot::RobotPtr robot =
+ segment.robots.get(famObjInstance.poseSensFrame.header.agent, providerName);
+ ARMARX_CHECK_NOT_NULL(robot) << famObjInstance.poseSensFrame.header.agent;
+
+
+ if (not syncTimestamp.has_value() or syncTimestamp.value() != famObjInstance.timestamp)
+ {
+ ARMARX_CHECK(
+ segment.robots.reader->synchronizeRobot(*robot, famObjInstance.timestamp));
+ syncTimestamp = famObjInstance.timestamp;
+ }
+
+
+ // fill global pose info if not set yet
+ if (not famObjInstance.poseGlobal.has_value())
+ {
+ famObjInstance.poseGlobal.emplace();
+ famObjInstance.poseGlobal->pose = framedPose.toGlobalEigen(robot);
+ famObjInstance.poseGlobal->header.agent = famObjInstance.poseSensFrame.header.agent;
+ famObjInstance.poseGlobal->header.frame = armarx::GlobalFrame;
+ }
+ else
+ {
+ ARMARX_CHECK_EQUAL(famObjInstance.poseGlobal->header.frame, armarx::GlobalFrame);
+ }
+
+
+ return famObjInstance;
+ };
+
+ std::vector<armarx::armem::arondto::FamiliarObjectInstance> familiarObjectInstances;
+ familiarObjectInstances.reserve(data.size());
+
+ std::transform(std::begin(data),
+ std::end(data),
+ std::back_inserter(familiarObjectInstances),
+ convertAndFillFamiliarObjectInstances);
+
+ segment.commit(familiarObjectInstances, providerName);
+ }
+
+ void
+ SegmentAdapter::handleProviderUpdate(const std::string& providerName)
+ {
+ // Initialized to 0 on first access.
+ if (segment.providers.count(providerName) == 0)
+ {
+ segment.providers[providerName] = objpose::ProviderInfo();
+ }
+ }
+
+ void
+ SegmentAdapter::visualizeRun()
+ {
+ if (not visu.enabled)
+ {
+ ARMARX_INFO << "Visu disabled";
+ return;
+ }
+
+ armarx::Metronome metronome(armarx::Frequency::HertzDouble(visu.frequencyHz));
+
+ while (visu.updateTask && !visu.updateTask->isStopped())
+ {
+ {
+ std::scoped_lock lock(visuMutex);
+
+ const std::map<std::string,
+ std::vector<armarx::armem::arondto::FamiliarObjectInstance>>
+ familiarObjectsByProvider = segment.doLocked(
+ [this]()
+ { return segment.getFamiliarObjectsByProvider(armarx::Clock::Now()); });
+
+ ARMARX_VERBOSE << "Visualizing " << familiarObjectsByProvider.size()
+ << " providers.";
+ visu.visualizeFamiliarObjectsByProvider(familiarObjectsByProvider);
+ }
+
+ metronome.waitForNextTick();
+ }
+ }
+
+
+} // namespace armarx::armem::server::obj::familiar_object_instance
diff --git a/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/SegmentAdapter.h b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/SegmentAdapter.h
new file mode 100644
index 0000000000000000000000000000000000000000..572939d96701734be8d972a0b31ecfaa918aac6d
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/SegmentAdapter.h
@@ -0,0 +1,113 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::armem_objects::Adapter
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2024
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+
+#include <experimental/memory>
+#include <mutex>
+
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
+
+#include "RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h"
+#include <RobotAPI/components/ArViz/Client/Client.h>
+#include <RobotAPI/interface/armem/server/ObjectMemoryInterface.h>
+#include <RobotAPI/interface/core/RobotState.h>
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+#include <RobotAPI/libraries/armem_objects/server/familiar_object_instance/Decay.h>
+#include <RobotAPI/libraries/armem_objects/server/familiar_object_instance/Segment.h>
+#include <RobotAPI/libraries/armem_objects/server/familiar_object_instance/Visu.h>
+
+
+#define ICE_CURRENT_ARG const Ice::Current& = Ice::emptyCurrent
+
+namespace armarx::armem::server::obj::familiar_object_instance
+{
+
+ /**
+ * @brief Helps implementing the `armarx::armem::server::ObjectInstanceSegmentInterface`.
+ */
+ class SegmentAdapter :
+ virtual public armarx::Logging,
+ virtual public armarx::armem::server::FamiliarObjectInstanceSegmentInterface
+ {
+ public:
+ SegmentAdapter(MemoryToIceAdapter& iceMemory);
+
+ std::string getName() const;
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
+
+ void init();
+ void connect(std::experimental::observer_ptr<robot_state::VirtualRobotReader> virtualRobotReader,
+ // KinematicUnitObserverInterfacePrx kinematicUnitObserver,
+ viz::Client arviz,
+ DebugObserverInterfacePrx debugObserver);
+
+
+ // ObjectPoseTopic interface
+ public:
+ // void reportProviderAvailable(const std::string& providerName,
+ // const objpose::ProviderInfo& info,
+ // ICE_CURRENT_ARG) override;
+ void reportFamiliarObjectPoses(const std::string& providerName,
+ const ::armarx::objpose::ProvidedFamiliarObjectPoseSeq& data,
+ ICE_CURRENT_ARG) override;
+
+ // ObjectInstanceSegmentInterface interface
+
+
+ private:
+ void updateProviderInfo(const std::string& providerName, const objpose::ProviderInfo& info);
+
+ void updateObjectPoses(const std::string& providerName,
+ const objpose::data::ProvidedObjectPoseSeq& providedPoses);
+ void handleProviderUpdate(const std::string& providerName);
+
+
+ // Visualization
+
+ void visualizeRun();
+
+
+ public:
+ static const std::string linearPredictionEngineID;
+ static const std::vector<PredictionEngine> predictionEngines;
+
+
+ private:
+ viz::Client arviz;
+ DebugObserverInterfacePrx debugObserver;
+
+ familiar_object_instance::Segment segment;
+
+
+ familiar_object_instance::Visu visu;
+ std::mutex visuMutex;
+
+
+ };
+
+} // namespace armarx::armem::server::obj::familiar_object_instance
+
+#undef ICE_CURRENT_ARG
diff --git a/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Visu.cpp b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Visu.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..345703832fd49d3b07739fe6efb3715bf5c4a82d
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Visu.cpp
@@ -0,0 +1,114 @@
+#include "Visu.h"
+
+#include <Eigen/src/Geometry/Transform.h>
+#include <Eigen/src/Geometry/Translation.h>
+
+#include <SimoxUtility/color/Color.h>
+#include <SimoxUtility/math/pose.h>
+#include <SimoxUtility/math/rescale.h>
+
+#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/core/time/ice_conversions.h>
+
+#include "RobotAPI/components/ArViz/Client/Elements.h"
+#include "RobotAPI/components/ArViz/Client/elements/PointCloud.h"
+#include "RobotAPI/libraries/ArmarXObjects/ObjectID.h"
+#include "RobotAPI/libraries/aron/core/aron_conversions.h"
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
+#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
+#include <RobotAPI/libraries/ArmarXObjects/predictions.h>
+
+namespace armarx::armem::server::obj::familiar_object_instance
+{
+
+ void
+ Visu::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ defs->optional(enabled, prefix + "enabled", "Enable or disable visualization of objects.");
+ defs->optional(frequencyHz, prefix + "frequenzyHz", "Frequency of visualization.");
+ defs->optional(inGlobalFrame,
+ prefix + "inGlobalFrame",
+ "If true, show global poses. If false, show poses in robot frame.");
+ defs->optional(alpha, prefix + "alpha", "Alpha of objects (1 = solid, 0 = transparent).");
+ defs->optional(alphaByConfidence,
+ prefix + "alphaByConfidence",
+ "If true, use the pose confidence as alpha (if < 1.0).");
+ defs->optional(oobbs, prefix + "oobbs", "Enable showing oriented bounding boxes.");
+ defs->optional(sizePixel, prefix + "sizePixel", "Pixel size of point cloud.");
+
+ defs->optional(visualizePointCloud, prefix + "visualizePointCloud", "");
+ defs->optional(visualizePose, prefix + "visualizePose", "");
+ defs->optional(visualizeBoundingBox, prefix + "visualizeBoundingBox", "");
+
+ defs->optional(objectFrames, prefix + "objectFrames", "Enable showing object frames.");
+ defs->optional(
+ objectFramesScale, prefix + "objectFramesScale", "Scaling of object frames.");
+ }
+
+ void
+ Visu::visualizeFamiliarObjectsByProvider(
+ const std::map<std::string, std::vector<armarx::armem::arondto::FamiliarObjectInstance>>&
+ familiarObjectsByProvider)
+ {
+
+ std::vector<viz::Layer> layers;
+
+ for (const auto& [providerName, familiarObjects] : familiarObjectsByProvider)
+ {
+ auto layerPose = arviz.layer("familiar_objects/pose/" + providerName);
+ auto layerPointCloud = arviz.layer("familiar_objects/points/" + providerName);
+ auto layerBox = arviz.layer("familiar_objects/box/" + providerName);
+
+ for (const auto& familiarObject : familiarObjects)
+ {
+ armarx::ObjectID objectId;
+ fromAron(familiarObject.objectID, objectId);
+
+ if (familiarObject.poseGlobal.has_value())
+ {
+ if (visualizePose)
+ {
+ auto pose = viz::Pose(objectId.str()).pose(familiarObject.poseGlobal->pose);
+ layerPose.add(pose);
+ }
+
+ const int alpha = static_cast<int>(255 * this->alpha);
+
+ if (visualizePointCloud and not familiarObject.points.empty())
+ {
+ auto points = viz::PointCloud(objectId.str())
+ .pointCloud(familiarObject.points)
+ .pose(familiarObject.poseGlobal->pose);
+
+ points.pointSizeInPixels(sizePixel);
+ layerPointCloud.add(points);
+ }
+
+ if (visualizeBoundingBox)
+ {
+
+ const Eigen::Isometry3f global_T_bb =
+ Eigen::Isometry3f{familiarObject.poseGlobal->pose} *
+ Eigen::Translation3f{familiarObject.bounding_box.center};
+
+ auto box = viz::Box(objectId.str());
+ box.pose(global_T_bb.matrix());
+ box.size(familiarObject.bounding_box.extents);
+ box.color(simox::Color::gray(128, alpha));
+ layerBox.add(box);
+ }
+ }
+ }
+
+ layers.push_back(layerPose);
+ layers.push_back(layerPointCloud);
+ layers.push_back(layerBox);
+ }
+
+ arviz.commit(layers);
+ }
+
+
+} // namespace armarx::armem::server::obj::familiar_object_instance
diff --git a/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Visu.h b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Visu.h
new file mode 100644
index 0000000000000000000000000000000000000000..b3bececb885a0f47bd04fb54185dd797a356ade3
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Visu.h
@@ -0,0 +1,66 @@
+#pragma once
+
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/services/tasks/TaskUtil.h>
+
+#include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
+
+#include <RobotAPI/components/ArViz/Client/Client.h>
+#include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
+#include <RobotAPI/libraries/armem_objects/aron/FamiliarObjectInstance.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/server/instance/visu/LinearPredictionsVisu.h>
+
+namespace armarx
+{
+ class ObjectFinder;
+}
+
+namespace armarx::armem::server::obj::familiar_object_instance
+{
+
+ /**
+ * @brief Models decay of object localizations by decreasing the confidence
+ * the longer the object was not localized.
+ */
+ class Visu : public armarx::Logging
+ {
+ public:
+ void defineProperties(armarx::PropertyDefinitionsPtr defs,
+ const std::string& prefix = "visu.");
+
+
+ void visualizeFamiliarObjectsByProvider(
+ const std::map<std::string,
+ std::vector<armarx::armem::arondto::FamiliarObjectInstance>>&
+ familiarObjectsByProvider);
+
+
+ viz::Client arviz;
+
+ bool enabled = true;
+ float frequencyHz = 25;
+
+ bool inGlobalFrame = true;
+ float minConfidence = -1;
+ float alpha = 1.0;
+ bool alphaByConfidence = false;
+ bool oobbs = false;
+
+ std::uint8_t sizePixel = 5;
+
+ bool visualizePointCloud = true;
+ bool visualizeBoundingBox = true;
+ bool visualizePose = true;
+
+ bool objectFrames = false;
+ float objectFramesScale = 1.0;
+
+
+
+ SimpleRunningTask<>::pointer_type updateTask;
+
+ };
+
+} // namespace armarx::armem::server::obj::familiar_object_instance
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron/RobotDescription.xml b/source/RobotAPI/libraries/armem_robot_state/aron/RobotDescription.xml
index aae0bba8efb2b42d6b426698911b36e6331ade27..3fb008da6919db746fd335735c6c2aa094e58a12 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron/RobotDescription.xml
+++ b/source/RobotAPI/libraries/armem_robot_state/aron/RobotDescription.xml
@@ -27,6 +27,9 @@
<ObjectChild key='end_effector'>
<string />
</ObjectChild>
+ <ObjectChild key='hand_unit'>
+ <string />
+ </ObjectChild>
<!-- Legacy old memory -->
<!-- <ObjectChild key='memory_hand_name'>
<string />
@@ -50,6 +53,21 @@
</Object>
+ <Object name='armarx::armem::arondto::ManipulationCapability'>
+ <ObjectChild key="affordance">
+ <string />
+ </ObjectChild>
+ <ObjectChild key="tcp">
+ <string optional="true"/>
+ </ObjectChild>
+ <ObjectChild key="shape">
+ <string optional="true"/>
+ </ObjectChild>
+ <ObjectChild key="type">
+ <string optional="true"/>
+ </ObjectChild>
+ </Object>
+
<Object name='armarx::armem::arondto::RobotInfo'>
<ObjectChild key="parts">
<Dict>
@@ -121,6 +139,14 @@
<armarx::armem::arondto::RobotInfo />
</ObjectChild>
+ <ObjectChild key='manipulationCapabilities'>
+ <Dict optional="true">
+ <List>
+ <armarx::armem::arondto::ManipulationCapability />
+ </List>
+ </Dict>
+ </ObjectChild>
+
<!-- <ObjectChild key='scaling'>
<float32 />
</ObjectChild> -->
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
index 9d03afe51e81fa99cf707eaf09ad517825d245f6..41f86a758d9ee8137d54351162ef42ac23a83bbf 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
@@ -117,6 +117,7 @@ namespace armarx::armem::robot_state::description
fromAron(dto.xml, bo.xml);
aron::fromAron(dto.visualization, bo.visualization);
aron::fromAron(dto.info, bo.info);
+ aron::fromAron(dto.manipulationCapabilities, bo.manipulationCapabilities);
}
void
@@ -126,6 +127,7 @@ namespace armarx::armem::robot_state::description
toAron(dto.xml, bo.xml);
aron::toAron(dto.visualization, bo.visualization);
aron::toAron(dto.info, bo.info);
+ aron::toAron(dto.manipulationCapabilities, bo.manipulationCapabilities);
}
} // namespace armarx::armem::robot_state::description
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
index 0fdb753ba78f100ed62681da36dc5673a3bb1af8..ef4c89379d9f243e1c7b22699b93c983370f5614 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
@@ -3,6 +3,7 @@
#include <filesystem>
#include <SimoxUtility/algorithm/string/string_tools.h>
+#include <VirtualRobot/XML/RobotIO.h>
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/application/properties/PluginAll.h>
@@ -20,6 +21,7 @@
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/RobotDescription.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/memory_ids.h>
#include <RobotAPI/libraries/armem_robot_state/robot_conversions.h>
@@ -187,6 +189,10 @@ namespace armarx::armem::server::robot_state::description
e.torso_kinematic_chain =
tryGet([&]() { return arm.getTorsoKinematicChain(); });
+ e.hand_unit =
+ tryGet([&]() { return arm.getHandUnitName(); });
+
+
info.parts.emplace(side + "Arm", e);
}
}
@@ -208,11 +214,51 @@ namespace armarx::armem::server::robot_state::description
}
}
+ std::optional<
+ std::map<std::string, std::vector<armarx::armem::arondto::ManipulationCapability>>>
+ allManipulationCapabilities;
+ {
+ const auto robot = VirtualRobot::RobotIO::loadRobot(robotPath.toSystemPath());
+ ARMARX_CHECK_NOT_NULL(robot) << robotPath.toSystemPath();
+
+ for (const auto& eef : robot->getEndEffectors())
+ {
+ const auto& capabilities = eef->getManipulationCapabilities();
+ if (capabilities.has_value())
+ {
+ std::vector<armarx::armem::arondto::ManipulationCapability>
+ manipulationCapabilities;
+
+ for (const auto& vrCap : capabilities->capabilities)
+ {
+ arondto::ManipulationCapability dto;
+
+ dto.affordance = vrCap.affordance;
+ dto.tcp = vrCap.tcp;
+ dto.shape = vrCap.shape;
+ dto.type = vrCap.type;
+
+ manipulationCapabilities.push_back(dto);
+ }
+
+ // initialize on demand
+ if (not allManipulationCapabilities)
+ {
+ allManipulationCapabilities.emplace();
+ }
+
+ allManipulationCapabilities->emplace(eef->getName(),
+ manipulationCapabilities);
+ }
+ }
+ }
+
const armem::robot_state::description::RobotDescription robotDescription{
.name = kinematicUnit->getRobotName(),
.xml = {robotPath.serialize().package, robotPath.serialize().path},
.visualization = visualization,
- .info = info};
+ .info = info,
+ .manipulationCapabilities = allManipulationCapabilities};
// make sure that the package path is valid
ARMARX_CHECK(std::filesystem::exists(robotDescription.xml.toSystemPath()));
diff --git a/source/RobotAPI/libraries/armem_robot_state/types.h b/source/RobotAPI/libraries/armem_robot_state/types.h
index 80d11cc4f556da58b9489b4fd452f12e632b093e..aa30157a5bf548f7cbcad4c63650e089d1a3730d 100644
--- a/source/RobotAPI/libraries/armem_robot_state/types.h
+++ b/source/RobotAPI/libraries/armem_robot_state/types.h
@@ -23,6 +23,7 @@
#include <filesystem>
#include <map>
+#include <optional>
#include <vector>
#include <Eigen/Geometry>
@@ -49,6 +50,9 @@ namespace armarx::armem::robot_state
arondto::RobotDescriptionVisualization visualization;
arondto::RobotInfo info;
+ std::optional<
+ std::map<std::string, std::vector<armarx::armem::arondto::ManipulationCapability>>>
+ manipulationCapabilities = std::nullopt;
};
using RobotDescriptions = std::vector<RobotDescription>;
diff --git a/source/RobotAPI/libraries/armem_skills/server/segment/ExecutableSkillLibrarySegment.cpp b/source/RobotAPI/libraries/armem_skills/server/segment/ExecutableSkillLibrarySegment.cpp
index 2f4c14e82d1c701363fbc81e3d974b47a0a43cd1..bde88afce40524ef09bb216e31a1e856311b1724 100644
--- a/source/RobotAPI/libraries/armem_skills/server/segment/ExecutableSkillLibrarySegment.cpp
+++ b/source/RobotAPI/libraries/armem_skills/server/segment/ExecutableSkillLibrarySegment.cpp
@@ -54,7 +54,7 @@ namespace armarx::skills::segment
entityUpdate.entityID = provId.withEntityName(d.skillId.skillName);
// Commit data to memory and notify
- iceMemory.commit(commit);
+ iceMemory.commitLocking(commit);
}
}
diff --git a/source/RobotAPI/libraries/armem_skills/server/segment/SkillEventSegment.cpp b/source/RobotAPI/libraries/armem_skills/server/segment/SkillEventSegment.cpp
index ba3d3b6aa9ddd54c24622d8163d575c11cb502c8..057c7573743497ff8f518bd0ab2d1d0d90153b8a 100644
--- a/source/RobotAPI/libraries/armem_skills/server/segment/SkillEventSegment.cpp
+++ b/source/RobotAPI/libraries/armem_skills/server/segment/SkillEventSegment.cpp
@@ -20,7 +20,7 @@ namespace armarx::skills::segment
void
SkillEventCoreSegment::defineProperties(PropertyDefinitionsPtr defs, const std::string& prefix)
{
- this->setDefaultMaxHistorySize(10);
+ this->setDefaultMaxHistorySize(300);
Base::defineProperties(defs, prefix);
}
@@ -60,7 +60,8 @@ namespace armarx::skills::segment
auto coreSegment = this->segmentPtr;
ARMARX_CHECK(coreSegment);
- coreSegment->forEachInstance(
+ coreSegment->doLocked([&](){
+ coreSegment->forEachInstance(
[&](const armem::wm::EntityInstance& i)
{
auto event = i.dataAs<armarx::skills::arondto::SkillStatusUpdate>();
@@ -75,6 +76,7 @@ namespace armarx::skills::segment
// set or replace
ret[up.executionId] = up;
});
+ });
// for (const auto& [k, v] : ret)
// {
@@ -91,7 +93,8 @@ namespace armarx::skills::segment
auto coreSegment = this->segmentPtr;
ARMARX_CHECK(coreSegment);
- coreSegment->forEachInstance(
+ coreSegment->doLocked([&](){
+ coreSegment->forEachInstance(
[&](const armem::wm::EntityInstance& i)
{
auto event = i.dataAs<armarx::skills::arondto::SkillStatusUpdate>();
@@ -106,6 +109,7 @@ namespace armarx::skills::segment
}
}
});
+ });
return ret;
}
} // namespace armarx::skills::segment
diff --git a/source/RobotAPI/libraries/armem_skills/server/segment/SkillExecutionRequestSegment.cpp b/source/RobotAPI/libraries/armem_skills/server/segment/SkillExecutionRequestSegment.cpp
index a19463b9d5a2f58ba0713d422ad7575f3077b785..1fd19f7ff174f430c799b0e7c8fa88c7696541a0 100644
--- a/source/RobotAPI/libraries/armem_skills/server/segment/SkillExecutionRequestSegment.cpp
+++ b/source/RobotAPI/libraries/armem_skills/server/segment/SkillExecutionRequestSegment.cpp
@@ -20,7 +20,7 @@ namespace armarx::skills::segment
SkillExecutionRequestCoreSegment::defineProperties(PropertyDefinitionsPtr defs,
const std::string& prefix)
{
- this->setDefaultMaxHistorySize(10);
+ this->setDefaultMaxHistorySize(50);
Base::defineProperties(defs, prefix);
}
@@ -86,6 +86,6 @@ namespace armarx::skills::segment
}
- iceMemory.commit(comm);
+ iceMemory.commitLocking(comm);
}
} // namespace armarx::skills::segment
diff --git a/source/RobotAPI/libraries/aron/common/rw/eigen.h b/source/RobotAPI/libraries/aron/common/rw/eigen.h
index 7cf56de94f1ecf81937bc2729f3b664d597e4866..fee5d0f9f2e3e2b3f645b1192024f0bb3e8cc18b 100644
--- a/source/RobotAPI/libraries/aron/common/rw/eigen.h
+++ b/source/RobotAPI/libraries/aron/common/rw/eigen.h
@@ -27,8 +27,8 @@ namespace armarx
aron_r.readNDArray(input, shape, typeAsString, data);
std::stringstream ss;
- ss << "Received wrong dimensions for matrix member. Dimensions are " << shape.at(0)
- << "," << shape.at(1) << " but should be " << ret.rows() << "/" << ret.cols();
+ ss << "Received wrong dimensions for matrix member. Dimensions are (" << shape.at(0)
+ << "," << shape.at(1) << ") but should be (" << ret.rows() << "," << ret.cols() << ").";
ARMARX_CHECK_AND_THROW(
typeAsString == TypeName<EigenT>::Get(),
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.cpp b/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.cpp
index dfad132e5f6a45f828dd00942b8c5de4c1f427f3..38de3e29214ef427687533308c06cdd46aa075f4 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.cpp
@@ -217,8 +217,9 @@ namespace armarx::aron::data
{
auto casted = type::Matrix::DynamicCastAndCheck(type);
ARMARX_TRACE;
- return (aron->shape.size() == 3 && aron->shape[0] == casted->getRows() &&
- aron->shape[1] == casted->getCols());
+ return (aron->shape.size() == 3 &&
+ (aron->shape[0] == casted->getRows() || casted->getRows() == -1) &&
+ (aron->shape[1] == casted->getCols() || casted->getCols() == -1));
}
case type::Descriptor::QUATERNION:
{
diff --git a/source/RobotAPI/libraries/core/RobotPool.cpp b/source/RobotAPI/libraries/core/RobotPool.cpp
index 2b9444a4d594a6483c435638fad3385f9b544f57..0b6d58afe038069766bcd96751fcccadfd7056ec 100644
--- a/source/RobotAPI/libraries/core/RobotPool.cpp
+++ b/source/RobotAPI/libraries/core/RobotPool.cpp
@@ -51,10 +51,12 @@ namespace armarx
return r;
}
}
+
+ ARMARX_INFO << "Cloning robot";
auto newRobot = baseRobot->clone(baseRobot->getName(), VirtualRobot::CollisionCheckerPtr(new VirtualRobot::CollisionChecker()));
newRobot->inflateCollisionModel(inflation);
ARMARX_CHECK_EQUAL(newRobot.use_count(), 1);
- ARMARX_DEBUG << "created new robot clone n with inflation " << inflation;
+ ARMARX_INFO << "created new robot clone n with inflation " << inflation;
robots[inflation].push_back(newRobot);
return newRobot;
}
diff --git a/source/RobotAPI/libraries/skills/core/Skill.cpp b/source/RobotAPI/libraries/skills/core/Skill.cpp
index a01eb4908baed4e5f6dc18c76f5493053d4bb886..62fee239e4bf83fdfe6cb3d9a09428484280d4aa 100644
--- a/source/RobotAPI/libraries/skills/core/Skill.cpp
+++ b/source/RobotAPI/libraries/skills/core/Skill.cpp
@@ -38,13 +38,23 @@ namespace armarx
return ret;
}
+
skills::SkillExecutionID
- Skill::callSubskillAsync(const skills::SkillProxy& prx, const aron::data::DictPtr& params)
+ Skill::callSubskillAsync(const skills::SkillProxy& prx)
{
- std::unique_lock l(subskillsMutex);
+ return callSubskillAsync(prx, prx.getRootProfileParameters());
+ }
+ skills::SkillExecutionID
+ Skill::callSubskillAsync(const skills::SkillProxy& prx, const aron::data::DictPtr& params)
+ {
std::string executorHistory = this->executorName + "->" + getSkillId().toString();
+
+ throwIfSkillShouldTerminate();
auto eid = prx.executeSkillAsync(executorHistory, params);
+
+ std::unique_lock l(subskillsMutex);
+ throwIfSkillShouldTerminate([&](){prx.abortSkillAsync(eid);}); // also notify newly added skill as it was not added to subskills list yet
this->subskills.push_back(eid);
return eid;
}
@@ -331,6 +341,11 @@ namespace armarx
void
Skill::notifySkillToStop()
{
+ if (stopped)
+ {
+ // skill already got stopped. Ignore
+ return;
+ }
std::scoped_lock l(subskillsMutex);
stopped = true;
_onStopRequested();
@@ -340,6 +355,12 @@ namespace armarx
void
Skill::notifyTimeoutReached()
{
+ if (stopped || timeoutReached)
+ {
+ // skill already got timeoutReached. Ignore
+ return;
+ }
+
std::scoped_lock l(subskillsMutex);
timeoutReached = true;
_onTimeoutReached();
@@ -356,10 +377,13 @@ namespace armarx
void
Skill::_onTimeoutReached()
{
+ // WE ASSUME THAT THE LOCK IS ALREADY TAKEN
+
if (!manager)
{
return;
}
+
for (const auto& execId : subskills)
{
manager->abortSkillAsync(execId.toManagerIce());
@@ -369,10 +393,13 @@ namespace armarx
void
Skill::_onStopRequested()
{
+ // WE ASSUME THAT THE LOCK IS ALREADY TAKEN
+
if (!manager)
{
return;
}
+
for (const auto& execId : subskills)
{
manager->abortSkillAsync(execId.toManagerIce());
diff --git a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp
index 6ca4c7def47d736e35b501c2f37b85eec81cdf8f..4a666401364a659cbd55d7fc87c655958a190154 100644
--- a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp
@@ -195,7 +195,7 @@ namespace armarx::plugins
auto async =
provider->begin_executeSkill(provider_executionRequest.toProviderIce());
- l.unlock(); // allow parallel e.g. stopping
+ l.unlock(); // allow parallel e.g. stopping. Otherwise the manager would lock himself in nested calls
auto provider_statusUpdate_ice = provider->end_executeSkill(async);
// convert to manager view
@@ -203,17 +203,11 @@ namespace armarx::plugins
skills::SkillStatusUpdate::FromIce(provider_statusUpdate_ice, provderId);
return statusUpdate;
}
- catch (...)
+ catch (const std::exception& e)
{
- ARMARX_WARNING << __PRETTY_FUNCTION__
- << ": Found disconnected or buggy skill provider '" << provderId
- << "' during execution. Removing it from skills.";
- skillProviderMap.erase(it);
+ l.lock();
- throw skills::error::SkillException(
- __PRETTY_FUNCTION__,
- "Skill execution failed. Could not execute a skill of provider '" +
- provderId.toString() + "' because the provider does not exist.");
+ handleExceptionNonLockingThrow(__PRETTY_FUNCTION__, e, provderId);
}
}
else
@@ -269,7 +263,7 @@ namespace armarx::plugins
auto async =
provider->begin_executeSkillAsync(provider_executionRequest.toProviderIce());
- l.unlock(); // allow parallel e.g. stopping
+ l.unlock(); // allow parallel e.g. stopping. Otherwise the manager would lock himself in nested calls
auto provider_executionID_ice = provider->end_executeSkillAsync(async);
// convert to manager view
@@ -278,17 +272,11 @@ namespace armarx::plugins
executionId.skillId.providerId = provderId;
return executionId;
}
- catch (...)
+ catch (const std::exception& e)
{
- ARMARX_WARNING << __PRETTY_FUNCTION__
- << ": Found disconnected or buggy skill provider '" << provderId
- << "' during execution. Removing it from skills.";
- skillProviderMap.erase(it);
+ l.lock();
- throw skills::error::SkillException(
- __PRETTY_FUNCTION__,
- "Skill execution failed. Could not execute a skill of provider '" +
- provderId.toString() + "' because the provider does not exist.");
+ handleExceptionNonLockingThrow(__PRETTY_FUNCTION__, e, provderId);
}
}
else
@@ -304,6 +292,8 @@ namespace armarx::plugins
SkillManagerComponentPlugin::updateSkillParameters(const skills::SkillExecutionID& executionId,
const aron::data::DictPtr& data)
{
+ ARMARX_DEBUG << "updateSkillParameters for skill " << executionId.skillId;
+
ARMARX_CHECK(executionId.skillId.isFullySpecified())
<< "Got: " << executionId.skillId.toString();
@@ -326,14 +316,16 @@ namespace armarx::plugins
{
auto async = provider->begin_updateSkillParameters(executionId.toProviderIce(),
data->toAronDictDTO());
- l.unlock(); // allow parallel e.g. stopping
+ l.unlock(); // allow parallel e.g. stopping. Otherwise the manager would lock himself in nested calls
auto r = provider->end_updateSkillParameters(async);
return r.success;
}
- catch (...)
+ catch (const std::exception& e)
{
- ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found buggy skill provider '"
- << providerId << "'. Removing it from skills on next execute.";
+ l.lock();
+
+ handleExceptionNonLocking(__PRETTY_FUNCTION__, e, providerId);
+
return false;
}
}
@@ -346,6 +338,8 @@ namespace armarx::plugins
bool
SkillManagerComponentPlugin::abortSkill(const skills::SkillExecutionID& executionId)
{
+ ARMARX_DEBUG << "abortSkill for skill " << executionId.skillId;
+
ARMARX_CHECK(executionId.skillId.isFullySpecified())
<< "Got: " << executionId.skillId.toString();
@@ -367,14 +361,16 @@ namespace armarx::plugins
try
{
auto async = provider->begin_abortSkill(executionId.toProviderIce());
- l.unlock(); // allow parallel e.g. stopping
+ l.unlock(); // allow parallel e.g. stopping. Otherwise the manager would lock himself in nested calls
auto r = provider->end_abortSkill(async);
return r.success;
}
- catch (...)
+ catch (const std::exception& e)
{
- ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found buggy skill provider '"
- << providerId << "'. Removing it from skills on next execute.";
+ l.lock();
+
+ handleExceptionNonLocking(__PRETTY_FUNCTION__, e, providerId);
+
return false;
}
}
@@ -410,10 +406,10 @@ namespace armarx::plugins
auto async = provider->begin_abortSkill(executionId.toProviderIce());
return true;
}
- catch (...)
+ catch (const std::exception& e)
{
- ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found buggy skill provider '"
- << providerId << "'. Removing it from skills on next execute.";
+ handleExceptionNonLocking(__PRETTY_FUNCTION__, e, providerId);
+
return false;
}
}
@@ -426,6 +422,8 @@ namespace armarx::plugins
std::optional<skills::SkillDescription>
SkillManagerComponentPlugin::getSkillDescription(const skills::SkillID& skillId)
{
+ ARMARX_DEBUG << "getSkillDescription for skill " << skillId;
+
ARMARX_CHECK(skillId.isFullySpecified()) << "Got: " << skillId.toString();
std::unique_lock l(skillProviderMapMutex);
@@ -450,7 +448,7 @@ namespace armarx::plugins
try
{
auto async = provider->begin_getSkillDescription(skillId.toProviderIce());
- l.unlock(); // allow parallel e.g. stopping
+ l.unlock(); // allow parallel e.g. stopping. Otherwise the manager would lock himself in nested calls
auto provider_desc_ice = provider->end_getSkillDescription(async);
if (not provider_desc_ice)
@@ -463,18 +461,9 @@ namespace armarx::plugins
skills::SkillDescription::FromIce(provider_desc_ice.value(), providerId);
return desc;
}
- catch (...)
+ catch (const std::exception& e)
{
- ARMARX_WARNING << __PRETTY_FUNCTION__
- << ": Found disconnected or buggy skill provider '" << providerId
- << "' during execution. Removing it from skills.";
- skillProviderMap.erase(it);
-
- throw skills::error::SkillException(__PRETTY_FUNCTION__,
- "Skill execution failed. Could not query a "
- "status update of a skill of provider '" +
- providerId.toString() +
- "' because the provider does not exist.");
+ handleExceptionNonLockingThrow(__PRETTY_FUNCTION__, e, providerId);
}
}
else
@@ -515,11 +504,12 @@ namespace armarx::plugins
++it;
}
- catch (...)
+ catch (const std::exception& e)
{
- ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found buggy skill provider '"
- << providerId << "'. Removing it from skills.";
- it = skillProviderMap.erase(it);
+ if (auto _it = handleExceptionNonLocking(__PRETTY_FUNCTION__, e, providerId))
+ {
+ it = _it.value(); // next element
+ }
}
}
return ret;
@@ -555,7 +545,7 @@ namespace armarx::plugins
try
{
auto async = provider->begin_getSkillExecutionStatus(executionId.toProviderIce());
- l.unlock(); // allow parallel e.g. stopping
+ l.unlock(); // allow parallel e.g. stopping. Otherwise the manager would lock himself in nested calls
auto provider_statusUpdate_ice = provider->end_getSkillExecutionStatus(async);
if (not provider_statusUpdate_ice)
@@ -570,18 +560,11 @@ namespace armarx::plugins
// ARMARX_WARNING << "Status update: " << statusUpdate.result;
return statusUpdate;
}
- catch (...)
+ catch (const std::exception& e)
{
- ARMARX_WARNING << __PRETTY_FUNCTION__
- << ": Found disconnected or buggy skill provider '" << providerId
- << "' during execution. Removing it from skills.";
- skillProviderMap.erase(it);
+ l.lock();
- throw skills::error::SkillException(__PRETTY_FUNCTION__,
- "Skill execution failed. Could not query a "
- "status update of a skill of provider '" +
- providerId.toString() +
- "' because the provider does not exist.");
+ handleExceptionNonLockingThrow(__PRETTY_FUNCTION__, e, providerId);
}
}
else
@@ -622,16 +605,58 @@ namespace armarx::plugins
}
it++;
}
- catch (...)
+ catch (const std::exception& e)
{
- ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found buggy skill provider '"
- << providerId << "'. Removing it from skills.";
- it = skillProviderMap.erase(it);
+ if (auto _it = handleExceptionNonLocking(__PRETTY_FUNCTION__, e, providerId))
+ {
+ it = _it.value(); // next element
+ }
}
}
return ret;
}
+ std::optional<
+ std::map<skills::ProviderID, skills::provider::dti::SkillProviderInterfacePrx>::iterator>
+ SkillManagerComponentPlugin::handleExceptionNonLocking(const char* funcName,
+ const std::exception& e,
+ skills::ProviderID providerId,
+ bool eraseSkillProvider)
+ {
+ // NON LOCKING! WE ASSERT THAT THE CALLER HOLDS LOCK
+ if (auto it = skillProviderMap.find(providerId);
+ eraseSkillProvider and it != skillProviderMap.end())
+ {
+ ARMARX_WARNING << funcName << ": Found disconnected or buggy skill provider '"
+ << providerId << "' during execution. Removing it from skills. "
+ << "Error: " << e.what();
+ return skillProviderMap.erase(it);
+ }
+ else
+ {
+ ARMARX_WARNING << funcName << ": Found disconnected or buggy skill provider '"
+ << providerId
+ << "' during execution. However, it already got removed... "
+ << "Error: " << e.what();
+ return std::nullopt;
+ }
+ }
+
+ void
+ SkillManagerComponentPlugin::handleExceptionNonLockingThrow(const char* funcName,
+ const std::exception& e,
+ skills::ProviderID providerId,
+ bool eraseSkillProvider)
+ {
+ // NON LOCKING! WE ASSERT THAT THE CALLER HOLDS LOCK
+ handleExceptionNonLocking(funcName, e, providerId, eraseSkillProvider);
+
+ throw skills::error::SkillException(
+ funcName,
+ "Skill execution failed. Could not execute a skill of provider '" +
+ providerId.toString() + "' because the provider does not exist.");
+ }
+
//****************************//
//** Fluxio related methods **//
//****************************//
diff --git a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.h b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.h
index 29afa7da8299ea1036fa6545f413da4f21ebe2cd..f5f016f678d4e979a53c60292a8acf2b3767ff3a 100644
--- a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.h
+++ b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.h
@@ -1,5 +1,6 @@
#pragma once
+#include <exception>
#include <memory>
#include <mutex>
#include <optional>
@@ -27,6 +28,7 @@
#include <RobotAPI/libraries/skills/core/ProviderInfo.h>
#include <RobotAPI/libraries/skills/core/SkillExecutionRequest.h>
#include <RobotAPI/libraries/skills/core/SkillStatusUpdate.h>
+#include <RobotAPI/libraries/skills/core/error/Exception.h>
#include <RobotAPI/libraries/skills/core/executor/FluxioExecutor.h>
#include "SkillManagerComponentPluginUser.h"
@@ -73,6 +75,7 @@ namespace armarx::plugins
std::map<skills::SkillExecutionID, skills::SkillStatusUpdate> getSkillExecutionStatuses();
+ protected:
skills::ProviderID getFirstProviderNameThatHasSkill(const skills::SkillID& skillid);
//****************************//
@@ -162,6 +165,17 @@ namespace armarx::plugins
};
FluxioDataCollector fluxioDC = {};
+ [[noreturn]] void handleExceptionNonLockingThrow(const char* funcName,
+ const std::exception& e,
+ skills::ProviderID providerId,
+ bool eraseSkillProvider = true);
+
+ std::optional<std::map<skills::ProviderID,
+ skills::provider::dti::SkillProviderInterfacePrx>::iterator>
+ handleExceptionNonLocking(const char* funcName,
+ const std::exception& e,
+ skills::ProviderID providerId,
+ bool eraseSkillProvider = true);
skills::manager::dti::SkillManagerInterfacePrx myPrx;
diff --git a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp
index 730ce95a67671fad8464f4b886431a544a6410e3..09b9d248be794aa673eea7db7a051d405004d739 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp
@@ -129,6 +129,7 @@ namespace armarx::plugins
return std::nullopt;
}
+ std::scoped_lock l2{skillExecutions.at(execId).skillStatusesMutex};
return skillExecutions.at(execId).statusUpdate;
}
@@ -223,9 +224,16 @@ namespace armarx::plugins
[&]()
{
// execute waits until the previous execution finishes.
- auto x = wrapper->executeSkill();
- ret.result = x.result;
- ret.status = armarx::skills::toSkillStatus(x.status);
+ try
+ {
+ auto x = wrapper->executeSkill();
+ ret.result = x.result;
+ ret.status = armarx::skills::toSkillStatus(x.status);
+ }
+ catch(std::exception& e)
+ {
+ ARMARX_WARNING << "Got an uncatched Exception when executing a skill. Exception was: " << e.what();
+ }
});
} // release lock. We don't know how long the skill needs to finish and we have to release the lock for being able to abort the execution
@@ -249,19 +257,28 @@ namespace armarx::plugins
{
ARMARX_CHECK(executionRequest.skillId.isFullySpecified());
- skills::SkillExecutionID executionId{executionRequest.skillId,
- executionRequest.executorName,
- armarx::core::time::DateTime::Now()};
+ skills::SkillExecutionID executionId;
+
skills::detail::SkillRuntime* wrapper;
{
auto l1 = std::unique_lock{skillFactoriesMutex};
- const auto& fac = getSkillFactory(executionId.skillId);
- ARMARX_CHECK(fac) << "Could not find a factory for skill " << executionId.skillId;
+ const auto& fac = getSkillFactory(executionRequest.skillId);
+ ARMARX_CHECK(fac) << "Could not find a factory for skill " << executionRequest.skillId;
{
const std::unique_lock l2{skillExecutionsMutex};
+
+ executionId = skills::SkillExecutionID{executionRequest.skillId,
+ executionRequest.executorName,
+ armarx::core::time::DateTime::Now()};
+
+ if (skillExecutions.count(executionId) > 0)
+ {
+ ARMARX_ERROR << "SkillsExecutionID already exists! This is undefined behaviour and should not occur!";
+ }
+
auto it =
skillExecutions.emplace(std::piecewise_construct,
std::make_tuple(executionId),
@@ -276,8 +293,15 @@ namespace armarx::plugins
wrapper->execution = std::thread(
[&]()
{
- // execute waits until the previous execution finishes.
- auto x = wrapper->executeSkill();
+ try
+ {
+ // execute waits until the previous execution finishes.
+ auto x = wrapper->executeSkill();
+ }
+ catch(std::exception& e)
+ {
+ ARMARX_WARNING << "Got an uncatched Exception when executing a skill. Exception was: " << e.what();
+ }
});
}
diff --git a/source/RobotAPI/libraries/skills_gui/memory/SkillManagerWrapper.cpp b/source/RobotAPI/libraries/skills_gui/memory/SkillManagerWrapper.cpp
index 1b7b492b5c24dcc87f536eccb2dffae29a94ca89..d19f45df54da7f914889b7243f72df840e183f42 100644
--- a/source/RobotAPI/libraries/skills_gui/memory/SkillManagerWrapper.cpp
+++ b/source/RobotAPI/libraries/skills_gui/memory/SkillManagerWrapper.cpp
@@ -26,7 +26,7 @@ namespace armarx::skills::gui
// we return this map
StatusMap statusMap;
- auto currentManagerStatuses = memory->getSkillExecutionStatuses();
+ auto currentManagerStatuses = memory->ice_invocationTimeout(10000)->getSkillExecutionStatuses();
// iterate over raw data and convert to common types
for (const auto& [k, v] : currentManagerStatuses)
@@ -136,7 +136,7 @@ namespace armarx::skills::gui
SkillMap skills;
- auto managerSkills = memory->getSkillDescriptions();
+ auto managerSkills = memory->ice_invocationTimeout(5000)->getSkillDescriptions();
for (const auto& [sid, desc] : managerSkills)
{
@@ -295,7 +295,7 @@ namespace armarx::skills::gui
{
ARMARX_INFO << "Aborting skill '" << executionId.skillId.skillName << "'...";
std::scoped_lock l(mutex_memory);
- this->memory->abortSkillAsync(executionId.toManagerIce());
+ this->memory->ice_invocationTimeout(5000)->abortSkillAsync(executionId.toManagerIce());
}
catch (Ice::Exception const& e)
{
@@ -381,7 +381,7 @@ namespace armarx::skills::gui
try
{
std::scoped_lock l(mutex_memory);
- memory->executeSkillAsync(req.toManagerIce());
+ memory->ice_invocationTimeout(5000)->executeSkillAsync(req.toManagerIce());
}
catch (Ice::Exception const& e)
{