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/ArViz/Client/elements/Robot.h b/source/RobotAPI/components/ArViz/Client/elements/Robot.h
index 1c90e0209498ae0d6c37d80a19905df32f7565d9..53c2976cafd2e235290dea6b1ea969408725471d 100644
--- a/source/RobotAPI/components/ArViz/Client/elements/Robot.h
+++ b/source/RobotAPI/components/ArViz/Client/elements/Robot.h
@@ -1,5 +1,6 @@
#pragma once
+#include <ArmarXCore/core/PackagePath.h>
#include "ElementOps.h"
@@ -19,6 +20,14 @@ namespace armarx::viz
return *this;
}
+ Robot& file(const armarx::PackagePath& packagePath)
+ {
+ data_->project = packagePath.serialize().package;
+ data_->filename = packagePath.serialize().package + "/" + packagePath.serialize().path;
+
+ return *this;
+ }
+
Robot& useCollisionModel()
{
data_->drawStyle |= data::ModelDrawStyle::COLLISION;
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 56bfd4d8af2866f732345b2e65f8ab6ce6463fff..fbebe63f4288a73b4a445d98769020f68830225d 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/CMakeLists.txt
+++ b/source/RobotAPI/components/skills/SkillProviderExample/CMakeLists.txt
@@ -19,6 +19,10 @@ set(SOURCES
Callback.cpp
Timeout.cpp
Segfault.cpp
+ RandomChaining.cpp
+ InstantKill.cpp
+ BusyWaiting.cpp
+ Recursive.cpp
)
set(HEADERS
@@ -29,6 +33,10 @@ set(HEADERS
Callback.h
Timeout.h
Segfault.h
+ RandomChaining.h
+ InstantKill.h
+ BusyWaiting.h
+ Recursive.h
)
armarx_add_component("${SOURCES}" "${HEADERS}")
@@ -38,6 +46,7 @@ armarx_enable_aron_file_generation_for_target(
${ARMARX_COMPONENT_NAME}
ARON_FILES
aron/HelloWorldAcceptedType.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 d292d7465bd8df09806fbe77b235478425f3442b..fe98dc157eb556fb51991d48f5f4428155117950 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.cpp
+++ b/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.cpp
@@ -61,6 +61,20 @@ namespace armarx::skills::provider
// segfault
ARMARX_INFO << "Adding skill SegfaultSkill";
addSkillFactory<SegfaultSkill>();
+
+ // 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 be0c6b3704daafcbec412fd0b9dfd9690485e8f1..63168c11da2be2a36eb6a0a911dab85d9cf1ae01 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.h
+++ b/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.h
@@ -35,6 +35,10 @@
#include "Incomplete.h"
#include "Segfault.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/NJointControllers/NJointTaskSpaceImpedanceController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp
index ee59658b4c78c60e3187d4e4a4c1db21a0bf25d7..e4b850fcaea1b866c2b6c6394d77934ffdfd5998 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp
@@ -54,11 +54,11 @@ NJointTaskSpaceImpedanceController::NJointTaskSpaceImpedanceController(
std::string jointName = rns->getNode(i)->getName();
jointNames.push_back(jointName);
ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
- ARMARX_CHECK_EXPRESSION(ct);
+ ARMARX_CHECK_EXPRESSION(ct) << jointName;
const SensorValueBase* sv = useSensorValue(jointName);
- ARMARX_CHECK_EXPRESSION(sv);
+ ARMARX_CHECK_EXPRESSION(sv) << jointName;
auto casted_ct = ct->asA<ControlTarget1DoFActuatorTorque>();
- ARMARX_CHECK_EXPRESSION(casted_ct);
+ ARMARX_CHECK_EXPRESSION(casted_ct) << jointName;
targets.push_back(casted_ct);
const SensorValue1DoFActuatorTorque* torqueSensor =
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
index 88cf11c532113f593f4dfa9056e4469fbc9a7992..6f01acc17ee6c7cf5ede5925d4deffa68ceeca4e 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
@@ -996,14 +996,14 @@ namespace armarx::RobotUnitModule
}
void
- ControlThread::setEmergencyStopState(EmergencyStopState state, const Ice::Current&)
+ ControlThread::setEmergencyStopState(EmergencyStopState state)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
_module<Units>().getEmergencyStopMaster()->setEmergencyStopState(state);
}
EmergencyStopState
- ControlThread::getEmergencyStopState(const Ice::Current&) const
+ ControlThread::getEmergencyStopState() const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return emergencyStop ? EmergencyStopState::eEmergencyStopActive
@@ -1011,7 +1011,7 @@ namespace armarx::RobotUnitModule
}
EmergencyStopState
- ControlThread::getRtEmergencyStopState(const Ice::Current&) const
+ ControlThread::getRtEmergencyStopState() const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return rtIsInEmergencyStop() ? EmergencyStopState::eEmergencyStopActive
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h
index af1990164ed130b335b6e8c26b540d165694b51e..4c06316e0f60f76cac6f1912a739f6109573d656 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h
@@ -128,20 +128,19 @@ namespace armarx::RobotUnitModule
* @brief Sets the \ref EmergencyStopState
* @param state The \ref EmergencyStopState to set
*/
- void setEmergencyStopState(EmergencyStopState state,
- const Ice::Current& = Ice::emptyCurrent) override;
+ void setEmergencyStopState(EmergencyStopState state);
+
/**
* @brief Returns the \ref ControlThread's target \ref EmergencyStopState
* @return The \ref EmergencyStopState
*/
- EmergencyStopState
- getEmergencyStopState(const Ice::Current& = Ice::emptyCurrent) const override;
+ EmergencyStopState getEmergencyStopState() const;
+
/**
* @brief Returns the \ref ControlThread's \ref EmergencyStopState
* @return The \ref EmergencyStopState
*/
- EmergencyStopState
- getRtEmergencyStopState(const Ice::Current& = Ice::emptyCurrent) const override;
+ EmergencyStopState getRtEmergencyStopState() const;
// //////////////////////////////////////////////////////////////////////////////////////// //
// /////////////////////////////////// Module interface /////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
index cc1e89e592642c8adbd137c071023a472284e4ec..b2ae894908ffc85a0bc906254f93bc4a31f1f5c5 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
@@ -175,6 +175,7 @@ namespace armarx::RobotUnitModule
// read the robot
try
{
+ // FIXME change the following to VirtualRobot::RobotIO::eStructure and see whether it breaks something
robot =
VirtualRobot::RobotIO::loadRobot(robotFileName, VirtualRobot::RobotIO::eFull);
if (robotName.empty())
@@ -197,7 +198,7 @@ namespace armarx::RobotUnitModule
<< "\n\tRobotNodeSet : " << robotNodeSetName << "\n\tNodeNames : "
<< robot->getRobotNodeSet(robotNodeSetName)->getNodeNames();
ARMARX_CHECK_NOT_NULL(robot);
- robotPool.reset(new RobotPool(robot, 10));
+ robotPool.reset(new RobotPool(robot, 20));
}
}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
index 34168e9a242f8c43da13eb43e2a809594ebbc226..09f52252953d5e9d5a1ebfdac591a68745f46137 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
@@ -24,21 +24,23 @@
#include <ArmarXCore/core/IceGridAdmin.h>
#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
{
@@ -78,18 +80,49 @@ namespace armarx::RobotUnitModule
setEmergencyStopState(EmergencyStopState state,
const Ice::Current& = Ice::emptyCurrent) final override
{
- if (getEmergencyStopState() == state)
+ EmergencyStopState currentState = getEmergencyStopState();
+
+ const std::unique_lock lock{ss2StateMutex};
+
+ if (currentState == state)
{
return;
}
+
+ currentState = state;
+
+ if (currentState == EmergencyStopState::eEmergencyStopActive)
+ {
+ lastSS2Activation = armarx::core::time::DateTime::Now();
+ }
+
ControlThreadAttorneyForRobotUnitEmergencyStopMaster::
- SetEmergencyStopStateNoReportToTopic(controlThreadModule, state);
- emergencyStopTopic->reportEmergencyStopState(state);
+ SetEmergencyStopStateNoReportToTopic(controlThreadModule, currentState);
+ emergencyStopTopic->reportEmergencyStopState(currentState);
+ }
+
+ EmergencyStopState
+ trySetEmergencyStopState(EmergencyStopState targetState,
+ const ::Ice::Current&) final override
+ {
+ armarx::core::time::Duration safeDuration =
+ armarx::core::time::Duration::MilliSeconds(500);
+
+ // If the SS2 should be released (target state = inactive) we check whether the last
+ // activation happened at least before safeDuration.
+ if (targetState == EmergencyStopState::eEmergencyStopInactive and
+ armarx::core::time::DateTime::Now() > lastSS2Activation + safeDuration)
+ {
+ setEmergencyStopState(targetState);
+ }
+
+ return getEmergencyStopState();
}
EmergencyStopState
getEmergencyStopState(const Ice::Current& = Ice::emptyCurrent) const final override
{
+ const std::shared_lock lock{ss2StateMutex};
return controlThreadModule->getEmergencyStopState();
}
@@ -118,6 +151,8 @@ namespace armarx::RobotUnitModule
ControlThread* const controlThreadModule;
const std::string emergencyStopTopicName;
EmergencyStopListenerPrx emergencyStopTopic;
+ mutable std::shared_mutex ss2StateMutex;
+ armarx::core::time::DateTime lastSS2Activation;
};
} // namespace armarx::RobotUnitModule
@@ -176,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";
@@ -187,8 +226,8 @@ namespace armarx::RobotUnitModule
initializeLocalizationUnit();
ARMARX_DEBUG << "LocalizationUnit initialized";
}
- ARMARX_TRACE;
+ ARMARX_TRACE;
if (_module<RobotData>().arePlatformAndLocalizationUnitsEnabled())
{
ARMARX_DEBUG << "initializing PlatformUnit";
@@ -218,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/interface/units/RobotUnit/RobotUnitInterface.ice b/source/RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice
index 97a40339d29419d83e1ce8af190f13bd30cfc516..517ecf266df944b5673f28a8e1742ddbe2ca6841 100644
--- a/source/RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice
@@ -22,30 +22,23 @@
#pragma once
-#include <ArmarXCore/interface/core/UserException.ice>
+#include <ArmarXCore/interface/components/EmergencyStopInterface.ice>
#include <ArmarXCore/interface/core/BasicTypes.ice>
#include <ArmarXCore/interface/core/RemoteReferenceCount.ice>
-#include <ArmarXCore/interface/components/EmergencyStopInterface.ice>
+#include <ArmarXCore/interface/core/UserException.ice>
#include <ArmarXCore/interface/observers/ObserverInterface.ice>
#include <ArmarXGui/interface/WidgetDescription.ice>
-#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
-
-#include <RobotAPI/interface/units/KinematicUnitInterface.ice>
+#include <RobotAPI/interface/components/RobotHealthInterface.ice>
+#include <RobotAPI/interface/components/TrajectoryPlayerInterface.ice>
#include <RobotAPI/interface/units/ForceTorqueUnit.ice>
#include <RobotAPI/interface/units/InertialMeasurementUnit.ice>
+#include <RobotAPI/interface/units/KinematicUnitInterface.ice>
#include <RobotAPI/interface/units/PlatformUnitInterface.ice>
+#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
#include <RobotAPI/interface/units/TCPControlUnit.ice>
-#include <RobotAPI/interface/components/TrajectoryPlayerInterface.ice>
-
#include <RobotAPI/interface/visualization/DebugDrawerInterface.ice>
-#include <RobotAPI/interface/components/RobotHealthInterface.ice>
-#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
-
-#include <ArmarXCore/interface/core/BasicTypes.ice>
-#include <ArmarXCore/interface/core/UserException.ice>
-#include <ArmarXGui/interface/WidgetDescription.ice>
//NJointController
@@ -61,7 +54,9 @@ module armarx
string hardwareControlMode;
string targetType;
};
- dictionary<string, HWControlModeAndTargetType> ControlModeToHWControlModeAndTargetTypeDictionary;
+
+ dictionary<string, HWControlModeAndTargetType>
+ ControlModeToHWControlModeAndTargetTypeDictionary;
struct ControlDeviceDescription
{
@@ -69,7 +64,9 @@ module armarx
ControlModeToHWControlModeAndTargetTypeDictionary contolModeToTargetType;
Ice::StringSeq tags;
};
+
sequence<ControlDeviceDescription> ControlDeviceDescriptionSeq;
+
struct ControlDeviceStatus
{
string deviceName;
@@ -78,6 +75,7 @@ module armarx
StringToStringVariantBaseMapMap controlTargetValues;
long timestampUSec = 0;
};
+
sequence<ControlDeviceStatus> ControlDeviceStatusSeq;
}
//RobotUnit Utility types - SensorDevice
@@ -89,13 +87,16 @@ module armarx
string sensorValueType;
Ice::StringSeq tags;
};
+
sequence<SensorDeviceDescription> SensorDeviceDescriptionSeq;
+
struct SensorDeviceStatus
{
string deviceName;
StringVariantBaseMap sensorValue;
long timestampUSec = 0;
};
+
sequence<SensorDeviceStatus> SensorDeviceStatusSeq;
}
//RobotUnit Utility types - NJointController
@@ -106,6 +107,7 @@ module armarx
string className;
WidgetDescription::Widget configDescription;
};
+
sequence<NJointControllerClassDescription> NJointControllerClassDescriptionSeq;
}
//RobotUnit Utility types - data streaming
@@ -126,9 +128,10 @@ module armarx
struct DataEntry
{
- DataEntryType type;
- long index = -1;
+ DataEntryType type;
+ long index = -1;
};
+
dictionary<string, DataEntry> StringDataEntryMap;
struct DataStreamingDescription
@@ -136,20 +139,20 @@ module armarx
StringDataEntryMap entries;
};
-
struct TimeStep
{
long iterationId;
long timestampUSec;
long timesSinceLastIterationUSec;
- Ice::BoolSeq bools;
- Ice::ByteSeq bytes;
- Ice::ShortSeq shorts;
- Ice::IntSeq ints;
- Ice::LongSeq longs;
- Ice::FloatSeq floats;
+ Ice::BoolSeq bools;
+ Ice::ByteSeq bytes;
+ Ice::ShortSeq shorts;
+ Ice::IntSeq ints;
+ Ice::LongSeq longs;
+ Ice::FloatSeq floats;
Ice::DoubleSeq doubles;
};
+
sequence<TimeStep> TimeStepSeq;
interface Receiver
@@ -193,15 +196,23 @@ module armarx
{
//rt-logging
["cpp:const"] idempotent Ice::StringSeq getLoggingNames();
- SimpleRemoteReferenceCounterBase startRtLogging(string filePathFormatString, Ice::StringSeq loggingNames) throws LogicError, InvalidArgumentException;
- SimpleRemoteReferenceCounterBase startRtLoggingWithAliasNames(string filePathFormatString, StringStringDictionary aliasNames) throws LogicError, InvalidArgumentException;
- void addMarkerToRtLog(SimpleRemoteReferenceCounterBase token, string marker) throws LogicError;
+ SimpleRemoteReferenceCounterBase startRtLogging(
+ string filePathFormatString, Ice::StringSeq loggingNames) throws LogicError,
+ InvalidArgumentException;
+ SimpleRemoteReferenceCounterBase startRtLoggingWithAliasNames(
+ string filePathFormatString, StringStringDictionary aliasNames) throws LogicError,
+ InvalidArgumentException;
+ void addMarkerToRtLog(SimpleRemoteReferenceCounterBase token, string marker)
+ throws LogicError;
void stopRtLogging(SimpleRemoteReferenceCounterBase token) throws LogicError;
- ["cpp:const"] void writeRecentIterationsToFile(string filePathFormatString) throws LogicError, InvalidArgumentException;
+ ["cpp:const"] void writeRecentIterationsToFile(string filePathFormatString)
+ throws LogicError,
+ InvalidArgumentException;
- RobotUnitDataStreaming::DataStreamingDescription startDataStreaming(RobotUnitDataStreaming::Receiver* receiver, RobotUnitDataStreaming::Config config);
- void stopDataStreaming(RobotUnitDataStreaming::Receiver* receiver);
+ RobotUnitDataStreaming::DataStreamingDescription startDataStreaming(
+ RobotUnitDataStreaming::Receiver * receiver, RobotUnitDataStreaming::Config config);
+ void stopDataStreaming(RobotUnitDataStreaming::Receiver * receiver);
};
interface RobotUnitUnitInterface
{
@@ -227,17 +238,29 @@ module armarx
interface RobotUnitDevicesInterface
{
//devices
- ["cpp:const"] idempotent Ice::StringSeq getControlDeviceNames() throws LogicError;
- ["cpp:const"] idempotent ControlDeviceDescription getControlDeviceDescription(string name) throws InvalidArgumentException, LogicError;
- ["cpp:const"] idempotent ControlDeviceDescriptionSeq getControlDeviceDescriptions() throws LogicError;
- ["cpp:const"] idempotent ControlDeviceStatus getControlDeviceStatus(string name) throws InvalidArgumentException, LogicError;
- ["cpp:const"] idempotent ControlDeviceStatusSeq getControlDeviceStatuses() throws LogicError;
-
- ["cpp:const"] idempotent Ice::StringSeq getSensorDeviceNames() throws LogicError;
- ["cpp:const"] idempotent SensorDeviceDescription getSensorDeviceDescription(string name) throws InvalidArgumentException, LogicError;
- ["cpp:const"] idempotent SensorDeviceDescriptionSeq getSensorDeviceDescriptions() throws LogicError;
- ["cpp:const"] idempotent SensorDeviceStatus getSensorDeviceStatus(string name) throws InvalidArgumentException, LogicError;
- ["cpp:const"] idempotent SensorDeviceStatusSeq getSensorDeviceStatuses() throws LogicError;
+ ["cpp:const"] idempotent Ice::StringSeq getControlDeviceNames() throws LogicError;
+ ["cpp:const"] idempotent ControlDeviceDescription getControlDeviceDescription(
+ string name) throws InvalidArgumentException,
+ LogicError;
+ ["cpp:const"] idempotent ControlDeviceDescriptionSeq getControlDeviceDescriptions()
+ throws LogicError;
+ ["cpp:const"] idempotent ControlDeviceStatus getControlDeviceStatus(string name)
+ throws InvalidArgumentException,
+ LogicError;
+ ["cpp:const"] idempotent ControlDeviceStatusSeq getControlDeviceStatuses()
+ throws LogicError;
+
+ ["cpp:const"] idempotent Ice::StringSeq getSensorDeviceNames() throws LogicError;
+ ["cpp:const"] idempotent SensorDeviceDescription getSensorDeviceDescription(string name)
+ throws InvalidArgumentException,
+ LogicError;
+ ["cpp:const"] idempotent SensorDeviceDescriptionSeq getSensorDeviceDescriptions()
+ throws LogicError;
+ ["cpp:const"] idempotent SensorDeviceStatus getSensorDeviceStatus(string name)
+ throws InvalidArgumentException,
+ LogicError;
+ ["cpp:const"] idempotent SensorDeviceStatusSeq getSensorDeviceStatuses()
+ throws LogicError;
};
interface RobotUnitControllerManagementInterface
{
@@ -246,42 +269,76 @@ module armarx
["cpp:const"] idempotent Ice::StringSeq getRequestedNJointControllerNames();
["cpp:const"] idempotent Ice::StringSeq getActivatedNJointControllerNames();
//proxy/information
- ["cpp:const"] idempotent NJointControllerInterface* getNJointController(string name);
- ["cpp:const"] idempotent StringNJointControllerPrxDictionary getAllNJointControllers();
+ ["cpp:const"] idempotent NJointControllerInterface* getNJointController(string name);
+ ["cpp:const"] idempotent StringNJointControllerPrxDictionary getAllNJointControllers();
- ["cpp:const"] idempotent NJointControllerStatus getNJointControllerStatus(string name) throws InvalidArgumentException;
- ["cpp:const"] idempotent NJointControllerStatusSeq getNJointControllerStatuses();
+ ["cpp:const"] idempotent NJointControllerStatus getNJointControllerStatus(string name)
+ throws InvalidArgumentException;
+ ["cpp:const"] idempotent NJointControllerStatusSeq getNJointControllerStatuses();
- ["cpp:const"] idempotent NJointControllerDescription getNJointControllerDescription(string name) throws InvalidArgumentException;
- ["cpp:const"] idempotent NJointControllerDescriptionSeq getNJointControllerDescriptions();
+ ["cpp:const"] idempotent NJointControllerDescription getNJointControllerDescription(
+ string name) throws InvalidArgumentException;
+ ["cpp:const"] idempotent NJointControllerDescriptionSeq
+ getNJointControllerDescriptions();
- ["cpp:const"] idempotent NJointControllerDescriptionWithStatus getNJointControllerDescriptionWithStatus(string name) throws InvalidArgumentException;
- ["cpp:const"] idempotent NJointControllerDescriptionWithStatusSeq getNJointControllerDescriptionsWithStatuses();
+ ["cpp:const"] idempotent NJointControllerDescriptionWithStatus
+ getNJointControllerDescriptionWithStatus(string name) throws InvalidArgumentException;
+ ["cpp:const"] idempotent NJointControllerDescriptionWithStatusSeq
+ getNJointControllerDescriptionsWithStatuses();
//classes
- ["cpp:const"] idempotent Ice::StringSeq getNJointControllerClassNames();
- ["cpp:const"] idempotent NJointControllerClassDescription getNJointControllerClassDescription(string name) throws InvalidArgumentException;
- ["cpp:const"] idempotent NJointControllerClassDescriptionSeq getNJointControllerClassDescriptions();
+ ["cpp:const"] idempotent Ice::StringSeq getNJointControllerClassNames();
+ ["cpp:const"] idempotent NJointControllerClassDescription
+ getNJointControllerClassDescription(string name) throws InvalidArgumentException;
+ ["cpp:const"] idempotent NJointControllerClassDescriptionSeq
+ getNJointControllerClassDescriptions();
//switching
- void switchNJointControllerSetup(Ice::StringSeq newSetup) throws InvalidArgumentException, LogicError;
-
- void activateNJointController(string controllerInstanceName) throws InvalidArgumentException, LogicError;
- void activateNJointControllers(Ice::StringSeq controllerInstanceNames) throws InvalidArgumentException, LogicError;
- void deactivateNJointController(string controllerInstanceName)throws InvalidArgumentException, LogicError;
- void deactivateNJointControllers(Ice::StringSeq controllerInstanceNames)throws InvalidArgumentException, LogicError;
+ void switchNJointControllerSetup(Ice::StringSeq newSetup)
+ throws InvalidArgumentException,
+ LogicError;
+
+ void activateNJointController(string controllerInstanceName)
+ throws InvalidArgumentException,
+ LogicError;
+ void activateNJointControllers(Ice::StringSeq controllerInstanceNames)
+ throws InvalidArgumentException,
+ LogicError;
+ void deactivateNJointController(string controllerInstanceName)
+ throws InvalidArgumentException,
+ LogicError;
+ void deactivateNJointControllers(Ice::StringSeq controllerInstanceNames)
+ throws InvalidArgumentException,
+ LogicError;
//creting controllers
- NJointControllerInterface* createNJointController(string className, string instanceName, NJointControllerConfig config) throws InvalidArgumentException, LogicError;
- NJointControllerInterface* createNJointControllerFromVariantConfig(string className, string instanceName, StringVariantBaseMap config) throws InvalidArgumentException, LogicError;
-
- NJointControllerInterface* createOrReplaceNJointController(string className, string instanceName, NJointControllerConfig config) throws InvalidArgumentException, LogicError;
+ NJointControllerInterface *createNJointController(
+ string className, string instanceName, NJointControllerConfig config)
+ throws InvalidArgumentException,
+ LogicError;
+ NJointControllerInterface *createNJointControllerFromVariantConfig(
+ string className, string instanceName, StringVariantBaseMap config)
+ throws InvalidArgumentException,
+ LogicError;
+
+ NJointControllerInterface *createOrReplaceNJointController(
+ string className, string instanceName, NJointControllerConfig config)
+ throws InvalidArgumentException,
+ LogicError;
//deleting controllers
- void deleteNJointController(string controllerInstanceName)throws InvalidArgumentException, LogicError;
- void deleteNJointControllers(Ice::StringSeq controllerInstanceNames)throws InvalidArgumentException, LogicError;
-
- void deactivateAndDeleteNJointController(string controllerInstanceName)throws InvalidArgumentException, LogicError;
- void deactivateAndDeleteNJointControllers(Ice::StringSeq controllerInstanceNames)throws InvalidArgumentException, LogicError;
+ void deleteNJointController(string controllerInstanceName)
+ throws InvalidArgumentException,
+ LogicError;
+ void deleteNJointControllers(Ice::StringSeq controllerInstanceNames)
+ throws InvalidArgumentException,
+ LogicError;
+
+ void deactivateAndDeleteNJointController(string controllerInstanceName)
+ throws InvalidArgumentException,
+ LogicError;
+ void deactivateAndDeleteNJointControllers(Ice::StringSeq controllerInstanceNames)
+ throws InvalidArgumentException,
+ LogicError;
};
interface RobotUnitSelfCollisionCheckerInterface
{
@@ -291,11 +348,10 @@ module armarx
idempotent void setSelfCollisionAvoidanceFrequency(float freq);
idempotent void setSelfCollisionAvoidanceDistance(float dist);
};
- interface RobotUnitControlThreadInterface
- {
- idempotent void setEmergencyStopState(EmergencyStopState state);
+ interface RobotUnitControlThreadInterface{
+ /*idempotent void setEmergencyStopState(EmergencyStopState state);
["cpp:const"] idempotent EmergencyStopState getEmergencyStopState();
- ["cpp:const"] idempotent EmergencyStopState getRtEmergencyStopState();
+ ["cpp:const"] idempotent EmergencyStopState getRtEmergencyStopState();*/
};
};
}
@@ -303,16 +359,11 @@ module armarx
//RobotUnit
module armarx
{
- interface RobotUnitInterface extends
- RobotUnitModule::RobotUnitUnitInterface,
- RobotUnitModule::RobotUnitDevicesInterface,
- RobotUnitModule::RobotUnitLoggingInterface,
+ interface RobotUnitInterface extends RobotUnitModule::RobotUnitUnitInterface,
+ RobotUnitModule::RobotUnitDevicesInterface, RobotUnitModule::RobotUnitLoggingInterface,
RobotUnitModule::RobotUnitPublishingInterface,
RobotUnitModule::RobotUnitManagementInterface,
RobotUnitModule::RobotUnitControlThreadInterface,
RobotUnitModule::RobotUnitControllerManagementInterface,
- RobotUnitModule::RobotUnitSelfCollisionCheckerInterface
- {
- };
+ RobotUnitModule::RobotUnitSelfCollisionCheckerInterface{};
};
-
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
index 8b61c578cca4070197d4b57bcc8a353ff8ceee2c..1167db200b5cacded7c52f265e8c2b9261b85bed 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
@@ -285,7 +285,9 @@ namespace armarx
objects.emplace_back(armem::articulated_object::ArticulatedObjectDescription
{
.name = object.idStr(),
- .xml = {modelFile->package, modelFile->relativePath}
+ .xml = {modelFile->package, modelFile->relativePath},
+ .visualization = {},
+ .info = {}
// .dataset = dataset
});
}
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/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..3ec7db60a82604d2f87f4476d592197184daeccc 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>
@@ -380,7 +383,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
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 36ab32c123f81a221a19fc272dd4f0ffdffa79d8..3f8d4f8d62fbdcefe48b1e76f5798303e0dfe336 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/CoreSegment.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/CoreSegment.cpp
@@ -17,7 +17,7 @@ namespace armarx::armem::server::ltm
DiskMemoryItemMixin(p, exportName, id),
MongoDBStorageMixin(s, exportName, id)
{
- start();
+ //start();
}
bool
@@ -25,10 +25,12 @@ namespace armarx::armem::server::ltm
{
std::lock_guard l(ltm_mutex);
- if (connected() && collectionExists()) {
- //for (const auto &subCollection : getSubCollection())
+ if (fullPathExists())
+ {
+ for (const auto& subdirName : getAllDirectories())
{
- std::string segmentName = "SubCollection";
+ std::string segmentName = util::fs::detail::unescapeName(subdirName);
+ segmentName = util::fs::detail::extractLastDirectoryFromPath(segmentName);
ProviderSegment c(getMemoryBasePath(),
getSettings(),
getExportName(),
@@ -36,19 +38,6 @@ namespace armarx::armem::server::ltm
processors);
func(c);
}
-
- } else {
- if (fullPathExists()) {
- for (const auto &subdirName: getAllDirectories()) {
- std::string segmentName = util::fs::detail::unescapeName(subdirName);
- ProviderSegment c(getMemoryBasePath(),
- getSettings(),
- getExportName(),
- id().withProviderSegmentName(segmentName),
- processors);
- func(c);
- }
- }
}
return true;
}
@@ -58,25 +47,17 @@ namespace armarx::armem::server::ltm
{
std::lock_guard l(ltm_mutex);
- if(connected() && collectionExists()) {
+ if (fullPathExists())
+ {
auto c = ProviderSegment(getMemoryBasePath(),
getSettings(),
getExportName(),
id().withProviderSegmentName(name),
processors);
- return (bool)c.collectionExists();
- } else {
- if (fullPathExists()) {
- auto c = ProviderSegment(getMemoryBasePath(),
- getSettings(),
- getExportName(),
- id().withProviderSegmentName(name),
- processors);
-
- return c.fullPathExists();
- }
+ return c.fullPathExists();
}
+
return false;
}
@@ -104,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))
@@ -126,12 +107,39 @@ 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)
{
std::lock_guard l(ltm_mutex);
- if ((connected() && collectionExists()) || fullPathExists())
+ if (/*(connected() && collectionExists()) ||*/ fullPathExists())
{
c.forEachProviderSegment(
[&](auto& e)
@@ -160,11 +168,11 @@ namespace armarx::armem::server::ltm
id().coreSegmentName = c.id().coreSegmentName;
};
- if (!connected())
+ /*if (!connected())
{
ARMARX_WARNING << "LTM CORE SEGMENT NOT CONNECTED ALTHOUGH ENABLED " << id().str();
return;
- }
+ }*/
// add foreign key to memory collection
if (c.hasAronType())
@@ -174,10 +182,10 @@ namespace armarx::armem::server::ltm
auto [vec, modeSuffix] = conv.convert(c.aronType());
ARMARX_CHECK_EMPTY(modeSuffix);
- std::string dataStr{vec.begin(), vec.end()};
- auto dataJson = nlohmann::json::parse(dataStr);
+ //std::string dataStr{vec.begin(), vec.end()};
+ /*auto dataJson = nlohmann::json::parse(dataStr);
- writeForeignKeyToPreviousDocument(dataJson);
+ writeForeignKeyToPreviousDocument(dataJson);*/
ensureFullPathExists(true);
std::string filename = (TYPE_FILENAME + conv.suffix);
@@ -187,7 +195,7 @@ namespace armarx::armem::server::ltm
{
ARMARX_INFO << "CoreSegment does not have aron type, so aron type information "
"cannot be exported";
- writeForeignKeyToPreviousDocument();
+ /*writeForeignKeyToPreviousDocument();*/
}
c.forEachProviderSegment(
diff --git a/source/RobotAPI/libraries/armem/server/ltm/CoreSegment.h b/source/RobotAPI/libraries/armem/server/ltm/CoreSegment.h
index 14d8b0abab73608e6eba0de5fa97734764a1a9ad..062ce79c398577de0cc6927315a042d141e5b587 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/CoreSegment.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/CoreSegment.h
@@ -28,6 +28,7 @@ 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;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/Entity.cpp b/source/RobotAPI/libraries/armem/server/ltm/Entity.cpp
index bc2aa20d1a399f6631a793545dfa514d0efa1332..a96723691c635f2bf43be25a9075c76de63685fa 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,44 @@ 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);
+ e.addSnapshot(s);
+ } else {
+ ARMARX_DEBUG << "Skipping snapshot with timestamp " << x.id().timestamp;
+ }
+ current++;
+ }
+ });
+ }
+
void
Entity::_resolve(armem::wm::Entity& p)
{
diff --git a/source/RobotAPI/libraries/armem/server/ltm/Entity.h b/source/RobotAPI/libraries/armem/server/ltm/Entity.h
index 6afeed78767669a6ac6a445be7324a53d667967a..82f22a63c1e8f6829d6f6538b91a44ea68a34a65 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/Entity.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/Entity.h
@@ -47,6 +47,7 @@ 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;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/EntityInstance.cpp b/source/RobotAPI/libraries/armem/server/ltm/EntityInstance.cpp
index ee9f13f8c1391acc506f0b8c62ad7ead01a19739..b0302a5cef0178acec1b539500bb591c9eb684f2 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/EntityInstance.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/EntityInstance.cpp
@@ -23,7 +23,7 @@ namespace armarx::armem::server::ltm
DiskMemoryItemMixin(p, exportName, id),
MongoDBStorageMixin(s, exportName, id)
{
- start();
+ //start();
}
void
@@ -54,77 +54,38 @@ namespace armarx::armem::server::ltm
aron::data::DictPtr datadict = nullptr;
aron::data::DictPtr metadatadict = nullptr;
- if (connected() && collectionExists())
+ if (fullPathExists())
{
- 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.";
- }
+ std::string dataFilename = (DATA_FILENAME + dictConverter.suffix);
+ std::string metadataFilename = (METADATA_FILENAME + dictConverter.suffix);
+ std::filesystem::path dataPath = getFullPath() / dataFilename;
+ std::filesystem::path metadataPath = getFullPath() / metadataFilename;
- 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.";
- }
+ if (util::fs::fileExists(dataPath))
+ {
+ //ARMARX_INFO << "Convert data for entity " << id();
+ auto datafilecontent = readDataFromFile(dataFilename);
+ auto dataaron = dictConverter.convert({datafilecontent, ""}, {});
+ datadict = aron::data::Dict::DynamicCastAndCheck(dataaron);
+ }
+ else
+ {
+ ARMARX_ERROR << "Could not find the data file '" << dataPath.string()
+ << "'. Continuing without data.";
}
- } else {
-
- //ARMARX_INFO << VAROUT(getFullPath());
-
- if (fullPathExists()) {
-
- std::string dataFilename = (DATA_FILENAME + dictConverter.suffix);
- std::string metadataFilename = (METADATA_FILENAME + dictConverter.suffix);
- std::filesystem::path dataPath = getFullPath() / dataFilename;
- std::filesystem::path metadataPath = getFullPath() / metadataFilename;
-
- if (util::fs::fileExists(dataPath)) {
- //ARMARX_INFO << "Convert data for entity " << id();
- auto datafilecontent = readDataFromFile(dataFilename);
- auto dataaron = dictConverter.convert({datafilecontent, ""}, {});
- datadict = aron::data::Dict::DynamicCastAndCheck(dataaron);
- } else {
- ARMARX_ERROR << "Could not find the data file '" << dataPath.string()
- << "'. Continuing without data.";
- }
- //ARMARX_INFO << "Convert metadata for entity " << id();
- if (util::fs::fileExists(metadataPath)) {
- auto metadatafilecontent = readDataFromFile(metadataFilename);
- auto metadataaron = dictConverter.convert({metadatafilecontent, ""}, {});
- metadatadict = aron::data::Dict::DynamicCastAndCheck(metadataaron);
- } else {
- ARMARX_ERROR << "Could not find the metadata file '" << metadataPath.string()
- << "'. Continuing without metadata.";
- }
+ //ARMARX_INFO << "Convert metadata for entity " << id();
+ if (util::fs::fileExists(metadataPath))
+ {
+ auto metadatafilecontent = readDataFromFile(metadataFilename);
+ auto metadataaron = dictConverter.convert({metadatafilecontent, ""}, {});
+ metadatadict = aron::data::Dict::DynamicCastAndCheck(metadataaron);
+ }
+ else
+ {
+ ARMARX_ERROR << "Could not find the metadata file '" << metadataPath.string()
+ << "'. Continuing without metadata.";
}
}
@@ -174,11 +135,11 @@ namespace armarx::armem::server::ltm
auto& dictConverter = processors->defaultObjectConverter;
- if (!connected())
+ /*if (!connected())
{
ARMARX_WARNING << "LTM ENTITY INSTANCE NOT CONNECTED ALTHOUGH ENABLED " << id().str();
return {};
- }
+ }*/
ensureFullPathExists(true);
diff --git a/source/RobotAPI/libraries/armem/server/ltm/Memory.cpp b/source/RobotAPI/libraries/armem/server/ltm/Memory.cpp
index 72faded127dda572353efa77cdd60bc9e218b1ae..39e18a3724a652aa6a44cbce5e78889ec33a6407 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/Memory.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/Memory.cpp
@@ -55,7 +55,7 @@ namespace armarx::armem::server::ltm
Memory::_enable()
{
BufferedBase::start();
- MongoDBStorageMixin::start();
+ //MongoDBStorageMixin::start();
}
void
@@ -83,10 +83,13 @@ namespace armarx::armem::server::ltm
{
std::lock_guard l(ltm_mutex);
- if (connected() && collectionExists()) {
- for (const auto& subCollection : getAllSubCollections())
+ // legacy: check fs
+ if (fullPathExists())
+ {
+ for (const auto& subdirName : getAllDirectories())
{
- std::string segmentName = util::mongodb::detail::unescapeName(subCollection);
+ const std::string& segmentName_one = util::fs::detail::unescapeName(subdirName);
+ auto segmentName = util::fs::detail::extractLastDirectoryFromPath(segmentName_one);
CoreSegment c(getMemoryBasePath(),
getSettings(),
getExportName(),
@@ -94,25 +97,17 @@ namespace armarx::armem::server::ltm
processors);
func(c);
}
- } else {
- // legacy: check fs
- if (fullPathExists()) {
- for (const auto &subdirName: getAllDirectories()) {
- std::string segmentName = util::fs::detail::unescapeName(subdirName);
- CoreSegment c(getMemoryBasePath(),
- getSettings(),
- getExportName(),
- id().withCoreSegmentName(segmentName),
- processors);
- func(c);
- }
- } else {
- ARMARX_WARNING << "Could not load the core segments of LTM " << id().str() << " as the path "
- << getFullPath().string() << " does not exist.";
- }
+ }
+ else
+ {
+ 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;
}
@@ -128,7 +123,7 @@ namespace armarx::armem::server::ltm
}
// check if collection exists
- if (connected() && collectionExists())
+ /*if (connected() && collectionExists())
{
auto c = CoreSegment(getMemoryBasePath(),
getSettings(),
@@ -137,17 +132,18 @@ namespace armarx::armem::server::ltm
processors);
return (bool)c.collectionExists();
- } else {
- // legacy: check if segment is stored on hard drive without db
- if (fullPathExists()) {
- auto c = CoreSegment(getMemoryBasePath(),
- getSettings(),
- getExportName(),
- id().withCoreSegmentName(name),
- processors);
-
- return c.fullPathExists();
- }
+ }*/
+
+ // legacy: check if segment is stored on hard drive without db
+ if (fullPathExists())
+ {
+ auto c = CoreSegment(getMemoryBasePath(),
+ getSettings(),
+ getExportName(),
+ id().withCoreSegmentName(name),
+ processors);
+
+ return c.fullPathExists();
}
return false;
@@ -185,8 +181,6 @@ namespace armarx::armem::server::ltm
m.id() = id().getMemoryID().cleanID();
- ARMARX_INFO << VAROUT(id());
-
forEachCoreSegment(
[&m](auto& x)
{
@@ -196,12 +190,52 @@ 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
- if ((connected() && collectionExists()) || fullPathExists())
+ ARMARX_DEBUG << VAROUT(fullPathExists());
+
+ if (/*(connected() && collectionExists()) ||*/ fullPathExists())
{
m.forEachCoreSegment(
[&](auto& e)
@@ -214,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();
}
}
@@ -237,11 +274,11 @@ namespace armarx::armem::server::ltm
setMemoryID(memory.id());
}
- if (!connected())
+ /*if (!connected())
{
ARMARX_WARNING << "LTM NOT CONNECTED ALTHOUGH ENABLED " << id().str();
return;
- }
+ }*/
memory.forEachCoreSegment(
[&](const auto& core)
@@ -254,6 +291,7 @@ namespace armarx::armem::server::ltm
// 2. store data
c.store(core);
+
// 3. update statistics
statistics.recordedCoreSegments++;
@@ -263,6 +301,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..4c7b287fc15e3f713250e2e082250e8baf309661 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 _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 1f4ad4b8cc85d837de31a5fd8c3290746c98a915..8978aecedae25da25c3dba5abbc89bfd6793be3a 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/ProviderSegment.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/ProviderSegment.cpp
@@ -18,7 +18,7 @@ namespace armarx::armem::server::ltm
DiskMemoryItemMixin(p, exportName, id),
MongoDBStorageMixin(s, exportName, id)
{
- start();
+ //start();
}
bool
@@ -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(),
@@ -53,7 +42,6 @@ namespace armarx::armem::server::ltm
func(c);
}
}
- }
return true;
}
@@ -62,27 +50,17 @@ namespace armarx::armem::server::ltm
{
std::lock_guard l(ltm_mutex);
- if (connected() && collectionExists())
+ if (fullPathExists())
{
+ ARMARX_INFO << VAROUT(id().getEntityID());
auto c = Entity(getMemoryBasePath(),
getSettings(),
getExportName(),
id().withEntityName(name),
processors);
- return (bool)c.collectionExists();
- } else
- {
- if (fullPathExists())
- {
- ARMARX_INFO << VAROUT(id().getEntityID());
- auto c = Entity(getMemoryBasePath(),
- getSettings(),
- getExportName(),
- id().withEntityName(name),
- processors);
- return c.fullPathExists();
- }
+ return c.fullPathExists();
}
+
return false;
}
@@ -120,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);
});
}
@@ -163,7 +139,7 @@ namespace armarx::armem::server::ltm
{
std::lock_guard l(ltm_mutex);
- if ((connected() && collectionExists()) || fullPathExists())
+ if (/*(connected() && collectionExists()) ||*/ fullPathExists())
{
p.forEachEntity(
[&](auto& e)
@@ -192,11 +168,11 @@ namespace armarx::armem::server::ltm
id().providerSegmentName = p.id().providerSegmentName;
}
- if (!connected())
+ /*if (!connected())
{
ARMARX_WARNING << "LTM PROVIDER SEGMENT NOT CONNECTED ALTHOUGH ENABLED " << id().str();
return;
- }
+ }*/
// add foreign key to memory collection
if (p.hasAronType())
@@ -206,10 +182,10 @@ namespace armarx::armem::server::ltm
auto [vec, modeSuffix] = conv.convert(p.aronType());
ARMARX_CHECK_EMPTY(modeSuffix);
- std::string dataStr{vec.begin(), vec.end()};
- auto dataJson = nlohmann::json::parse(dataStr);
+ //std::string dataStr{vec.begin(), vec.end()};
+ //auto dataJson = nlohmann::json::parse(dataStr);
- writeForeignKeyToPreviousDocument(dataJson);
+ //writeForeignKeyToPreviousDocument(dataJson);
ensureFullPathExists(true);
std::string filename = (TYPE_FILENAME + conv.suffix);
@@ -218,7 +194,7 @@ namespace armarx::armem::server::ltm
else
{
ARMARX_INFO << "ProviderSegment does not seem to have an aron type, so aron type information connot be exported";
- writeForeignKeyToPreviousDocument();
+ //writeForeignKeyToPreviousDocument();
}
p.forEachEntity(
diff --git a/source/RobotAPI/libraries/armem/server/ltm/ProviderSegment.h b/source/RobotAPI/libraries/armem/server/ltm/ProviderSegment.h
index 5f880676c43f483c6ae66a65c41a7a17bf93aa70..1d6ac9d36a5a75edb80d0ab609518aeef07407f5 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/ProviderSegment.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/ProviderSegment.h
@@ -28,6 +28,7 @@ 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;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/detail/CoreSegmentBase.h b/source/RobotAPI/libraries/armem/server/ltm/detail/CoreSegmentBase.h
index e5a425016e06ff19c46ef7d768cf4e18b8dc8a02..79a5cc7742dbd1c44f5998fd5a14b3cb86dc9de5 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)
@@ -87,6 +93,7 @@ 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;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/detail/EntityBase.h b/source/RobotAPI/libraries/armem/server/ltm/detail/EntityBase.h
index ab61c63e0b82cfa6fba6cbb16d76331ff0027d73..3bf90a7f48c0743d8aeea804be40d8cbf477475d 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)
@@ -100,6 +106,7 @@ 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;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/detail/MemoryBase.h b/source/RobotAPI/libraries/armem/server/ltm/detail/MemoryBase.h
index 99df8e2e6177be8f39f019342352762724fa7bf1..33b1e46d7ed76680085768170e7a34bcd5010477 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
@@ -54,6 +55,8 @@ 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);
try
@@ -75,6 +78,7 @@ namespace armarx::armem::server::ltm::detail
{
this->startRecording();
}
+
}
/// enable this LTM
@@ -131,6 +135,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
@@ -203,6 +234,11 @@ namespace armarx::armem::server::ltm::detail
defs->optional(p.enabled_on_startup, prefix + "enabled");
defs->optional(p.configuration_on_startup, prefix + "configuration");
defs->optional(p.export_name, prefix + "exportName");
+
+ defs->optional(p.importOnStartUp, prefix + "importOnStartUp");
+ defs->optional(p.maxAmountOfSnapshotsLoaded, prefix + "maxAmountSnapshotsLoaded");
+ defs->optional(p.coreSegmentsToLoad, prefix + "loadedCoreSegments");
+
}
/// enable/disable
@@ -270,6 +306,12 @@ namespace armarx::armem::server::ltm::detail
return "LT-Memory";
}
+ void
+ loadOnStartup(){
+ _loadOnStartup();
+ }
+
+
protected:
/// configuration
virtual void
@@ -296,6 +338,9 @@ namespace armarx::armem::server::ltm::detail
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 _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
@@ -307,13 +352,19 @@ namespace armarx::armem::server::ltm::detail
"\"PngConverter\": {}, \"ExrConverter\": {}}"; //record with 20 fps as standard
std::string export_name = "MemoryExport";
std::string export_path = "/tmp/ltm";
+
+ //belong more to WM, but no good solution for that currently:
+ bool importOnStartUp = false;
+ int maxAmountOfSnapshotsLoaded = 10;
+ std::string coreSegmentsToLoad = "Localization";
} p;
+
protected:
mutable std::recursive_mutex ltm_mutex;
mutable Statistics statistics;
- std::atomic_bool enabled = false;
+ std::atomic_bool enabled = true;
};
} // namespace armarx::armem::server::ltm::detail
diff --git a/source/RobotAPI/libraries/armem/server/ltm/detail/ProviderSegmentBase.h b/source/RobotAPI/libraries/armem/server/ltm/detail/ProviderSegmentBase.h
index 86f6306c700b9888636c6d7087cc849558a59417..0aa25896fbbe481fdc682525b77224301882f38f 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)
@@ -84,6 +89,7 @@ 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;
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/plugins/Plugin.cpp b/source/RobotAPI/libraries/armem/server/plugins/Plugin.cpp
index e875bce8dd360038f832f1ff1248acb16e6587cf..193ca45e3c646bb372c684ea36644e7c5f08e450 100644
--- a/source/RobotAPI/libraries/armem/server/plugins/Plugin.cpp
+++ b/source/RobotAPI/libraries/armem/server/plugins/Plugin.cpp
@@ -88,6 +88,8 @@ namespace armarx::armem::server::plugins
parent.getTopic(memoryTopic, memoryTopicName);
iceAdapter.setMemoryListener(memoryTopic);
+ this->iceAdapter.reloadFromLTM();
+
connected = true;
}
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..289db221609ee4f157126a3fb898df00fd78f58f 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;
+ //test
+ 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 2e94c2cdb47a1fb883a6e790240be4c76a21b5e6..bd6096c2fa6470d764b107046418435c8d9b0b38 100644
--- a/source/RobotAPI/libraries/armem/server/wm/detail/MaxHistorySize.cpp
+++ b/source/RobotAPI/libraries/armem/server/wm/detail/MaxHistorySize.cpp
@@ -1,10 +1,25 @@
#include "MaxHistorySize.h"
+#include <ArmarXCore/core/logging/Logging.h>
+
namespace armarx::armem::server::wm::detail
{
- void MaxHistorySize::setMaxHistorySize(long maxSize)
+ void MaxHistorySize::setMaxHistorySize(long maxSize, const std::string& additionalInfo)
{
+ if (maxSize < 0){
+ 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 d8f01c81be35b546d25d83e749a5e4770eee045e..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);
@@ -169,7 +174,7 @@ namespace armarx::armem::gui
{
// checkbox is disabled but memory is available again
box->setVisible(true);
- box->setChecked(true);
+ box->setChecked(false);
box->setEnabled(true);
}
alreadyPresentMemoryCheckboxes.push_back(memoryName);
@@ -184,7 +189,7 @@ namespace armarx::armem::gui
{
// new memory available
auto box = new QCheckBox(QString::fromStdString(memoryName));
- box->setChecked(true);
+ 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/ArticulatedObjectWriter.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
index d4cf12feebd5e996353ff08fc174720981d96256..f293741f9ba9baa56853717f1984389ce30ad1d1 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
@@ -38,7 +38,9 @@ namespace armarx::armem::articulated_object
.description = {.name = obj.getType(),
.xml = PackagePath(
armarx::ArmarXDataPath::getProject({package}, fileRelPath),
- obj.getFilename())},
+ obj.getFilename()),
+ .visualization = {},
+ .info = {}},
.instance = obj.getName(),
.config = {.timestamp = timestamp,
.globalPose = Eigen::Isometry3f(obj.getRootNode()->getGlobalPose()),
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/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 325075db3994c74ee21bafa949b37a7a35f66b58..3fb008da6919db746fd335735c6c2aa094e58a12 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron/RobotDescription.xml
+++ b/source/RobotAPI/libraries/armem_robot_state/aron/RobotDescription.xml
@@ -7,6 +7,120 @@
<GenerateTypes>
+ <Object name='armarx::armem::arondto::RobotInfoElement'>
+
+ <ObjectChild key='kinematic_chain'>
+ <string />
+ </ObjectChild>
+ <ObjectChild key='torso_kinematic_chain'>
+ <string />
+ </ObjectChild>
+ <ObjectChild key='tcp'>
+ <string />
+ </ObjectChild>
+ <ObjectChild key='force_torque_sensor'>
+ <string />
+ </ObjectChild>
+ <ObjectChild key='force_torque_sensor_frame'>
+ <string />
+ </ObjectChild>
+ <ObjectChild key='end_effector'>
+ <string />
+ </ObjectChild>
+ <ObjectChild key='hand_unit'>
+ <string />
+ </ObjectChild>
+ <!-- Legacy old memory -->
+ <!-- <ObjectChild key='memory_hand_name'>
+ <string />
+ </ObjectChild> -->
+ <ObjectChild key='hand_controller_name'>
+ <string />
+ </ObjectChild>
+ <ObjectChild key='hand_root_node'>
+ <string />
+ </ObjectChild>
+ <ObjectChild key='hand_model'>
+ <armarx::arondto::PackagePath />
+ </ObjectChild>
+ <ObjectChild key='palm_collision_model'>
+ <string />
+ </ObjectChild>
+ <ObjectChild key='full_hand_collision_model'>
+ <armarx::arondto::PackagePath />
+ </ObjectChild>
+
+
+ </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>
+ <armarx::armem::arondto::RobotInfoElement />
+ </Dict>
+ </ObjectChild>
+ </Object>
+
+ <Object name='armarx::armem::arondto::BodyPartDescription'>
+ <ObjectChild key='xml'>
+ <armarx::arondto::PackagePath />
+ </ObjectChild>
+
+ <ObjectChild key='root_node_name'>
+ <string/>
+ </ObjectChild>
+ </Object>
+
+
+ <Object name='armarx::armem::arondto::RobotDescriptionVisualization'>
+
+ <ObjectChild key="body_parts">
+ <Dict>
+ <armarx::armem::arondto::BodyPartDescription />
+ </Dict>
+ </ObjectChild>
+
+
+ <!-- <ObjectChild key='right_hand_closed_joints'>
+ <Dict>
+ <float32 />
+ </Dict>
+ </ObjectChild>
+ <ObjectChild key='left_hand_closed_joints'>
+ <Dict>
+ <float32 />
+ </Dict>
+ </ObjectChild>
+ <ObjectChild key='right_hand_opened_joints'>
+ <Dict>
+ <float32 />
+ </Dict>
+ </ObjectChild>
+ <ObjectChild key='left_hand_opened_joints'>
+ <Dict>
+ <float32 />
+ </Dict>
+ </ObjectChild> -->
+
+
+ </Object>
+
+
<Object name='armarx::armem::arondto::RobotDescription'>
<ObjectChild key='name'>
@@ -17,6 +131,22 @@
<armarx::arondto::PackagePath />
</ObjectChild>
+ <ObjectChild key='visualization'>
+ <armarx::armem::arondto::RobotDescriptionVisualization />
+ </ObjectChild>
+
+ <ObjectChild key='info'>
+ <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 e00b27c3d77fb6130171e06244cffc1181c8fbc1..41f86a758d9ee8137d54351162ef42ac23a83bbf 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
@@ -115,6 +115,9 @@ namespace armarx::armem::robot_state::description
{
aron::fromAron(dto.name, bo.name);
fromAron(dto.xml, bo.xml);
+ aron::fromAron(dto.visualization, bo.visualization);
+ aron::fromAron(dto.info, bo.info);
+ aron::fromAron(dto.manipulationCapabilities, bo.manipulationCapabilities);
}
void
@@ -122,6 +125,9 @@ namespace armarx::armem::robot_state::description
{
aron::toAron(dto.name, bo.name);
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/client/common/VirtualRobotWriter.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.cpp
index 48dc75da8b590436b65c5799591a5f25ad18be69..68d02546ea59f5bd3ee1e89989837c8da05b8fd5 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.cpp
@@ -50,8 +50,11 @@ namespace armarx::armem::robot_state
{
const auto filename = robot.getFilename();
+ // FIXME maybe visu and info optional?
const description::RobotDescription desc{.name = robot.getType(),
- .xml = resolvePackagePath(filename)};
+ .xml = resolvePackagePath(filename),
+ .visualization = {},
+ .info = {}};
return RobotWriter::storeDescription(desc, timestamp);
}
@@ -60,10 +63,9 @@ namespace armarx::armem::robot_state
VirtualRobotWriter::storeState(const VirtualRobot::Robot& robot, const armem::Time& timestamp)
{
const RobotState robotState{.timestamp = timestamp,
- .globalPose =
- RobotState::Pose(robot.getGlobalPose()),
- .jointMap = robot.getJointValues(),
- .proprioception = std::nullopt};
+ .globalPose = RobotState::Pose(robot.getGlobalPose()),
+ .jointMap = robot.getJointValues(),
+ .proprioception = std::nullopt};
return RobotWriter::storeState(
robotState,
diff --git a/source/RobotAPI/libraries/armem_robot_state/robot_conversions.cpp b/source/RobotAPI/libraries/armem_robot_state/robot_conversions.cpp
index 22790b52cc21f8af15877f147d620e735b79f520..042315b10fa3d8137874acbffd46a6fda96c4f94 100644
--- a/source/RobotAPI/libraries/armem_robot_state/robot_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/robot_conversions.cpp
@@ -32,7 +32,9 @@ namespace armarx::armem::robot_state
description::RobotDescription robotDescription{
.name = "",
- .xml = ::armarx::PackagePath("", fs::path("")) // initialize empty, no default c'tor
+ .xml = ::armarx::PackagePath("", fs::path("")), // initialize empty, no default c'tor
+ .visualization = {},
+ .info = {}
};
fromAron(aronRobotDescription, robotDescription);
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 13587664c03a0c31ab83e8295561372e07630223..ef4c89379d9f243e1c7b22699b93c983370f5614 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
@@ -1,43 +1,47 @@
#include "Segment.h"
+
#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>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/time/TimeUtil.h>
-#include <RobotAPI/libraries/armem_robot_state/types.h>
-#include <RobotAPI/libraries/aron/common/aron_conversions.h>
-
-#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.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/MemoryID.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
#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>
-#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/types.h>
-#include <RobotAPI/libraries/armem_robot_state/memory_ids.h>
-
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
namespace armarx::armem::server::robot_state::description
{
Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
- Base(memoryToIceAdapter, armem::robot_state::descriptionSegmentID.coreSegmentName, arondto::RobotDescription::ToAronType())
+ Base(memoryToIceAdapter,
+ armem::robot_state::descriptionSegmentID.coreSegmentName,
+ arondto::RobotDescription::ToAronType())
{
}
Segment::~Segment() = default;
-
- void Segment::onConnect(const RobotUnitInterfacePrx& robotUnitPrx)
+ void
+ Segment::onConnect(const RobotUnitInterfacePrx& robotUnitPrx)
{
robotUnit = robotUnitPrx;
@@ -45,15 +49,17 @@ namespace armarx::armem::server::robot_state::description
updateRobotDescription();
}
-
- void Segment::commitRobotDescription(const armem::robot_state::description::RobotDescription& robotDescription)
+ void
+ Segment::commitRobotDescription(
+ const armem::robot_state::description::RobotDescription& robotDescription)
{
const Time now = Time::Now();
const MemoryID providerID = segmentPtr->id().withProviderSegmentName(robotDescription.name);
if (not segmentPtr->hasProviderSegment(providerID.providerSegmentName))
{
- segmentPtr->addProviderSegment(providerID.providerSegmentName, arondto::RobotDescription::ToAronType());
+ segmentPtr->addProviderSegment(providerID.providerSegmentName,
+ arondto::RobotDescription::ToAronType());
}
@@ -63,18 +69,40 @@ namespace armarx::armem::server::robot_state::description
arondto::RobotDescription dto;
toAron(dto, robotDescription);
- update.instancesData =
- {
- dto.toAron()
- };
+ update.instancesData = {dto.toAron()};
Commit commit;
commit.updates.push_back(update);
iceMemory.commitLocking(commit);
}
+ // Helper function to handle exceptions
+ // template <typename T>
+ // std::optional<T> tryGet(const std::function<T()>& func) {
+ // try {
+ // return func();
+ // } catch (const std::exception& ex) {
+ // // Handle exception or log error
+ // std::cerr << "Error: " << ex.what() << std::endl;
+ // return std::nullopt; // Return an empty optional if an exception occurs
+ // }
+ // }
+
+ std::string
+ tryGet(const std::function<std::string()>& func)
+ {
+ try
+ {
+ return func();
+ }
+ catch (const std::exception& ex)
+ {
+ return ""; // Return an empty string if an exception occurs
+ }
+ }
- void Segment::updateRobotDescription()
+ void
+ Segment::updateRobotDescription()
{
ARMARX_CHECK_NOT_NULL(robotUnit);
KinematicUnitInterfacePrx kinematicUnit = robotUnit->getKinematicUnit();
@@ -83,7 +111,8 @@ namespace armarx::armem::server::robot_state::description
const std::string robotName = kinematicUnit->getRobotName();
const std::string robotFilename = kinematicUnit->getRobotFilename();
- const std::vector<std::string> packages = armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
+ const std::vector<std::string> packages =
+ armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
// const std::string package = armarx::ArmarXDataPath::getProject(packages, robotFilename);
const std::string package = simox::alg::split(robotFilename, "/").front();
@@ -93,25 +122,143 @@ namespace armarx::armem::server::robot_state::description
ARMARX_INFO << VAROUT(robotFilename);
// make sure that the relative path is without the 'package/' prefix
- const std::string robotFileRelPath = [&robotFilename, &package]()-> std::string {
+ const auto fixPath = [](armarx::PackagePath& pp)
+ {
+ auto spp = pp.serialize();
+ if (simox::alg::starts_with(spp.path, spp.package))
+ {
+ // remove "package" + "/"
+ const std::string fixedPath = spp.path.substr(spp.package.size() + 1, -1);
+
+ spp.path = fixedPath;
- if(simox::alg::starts_with(robotFilename, package))
+ pp = armarx::PackagePath{spp};
+ }
+ };
+
+ armarx::PackagePath robotPath{package, robotFilename};
+ fixPath(robotPath);
+
+ const auto fixPathDto = [](armarx::arondto::PackagePath& pp)
+ {
+ if (simox::alg::starts_with(pp.path, pp.package))
{
// remove "package" + "/"
- const std::string fixedRobotFilename = robotFilename.substr(package.size() + 1, -1);
- return fixedRobotFilename;
+ const std::string fixedPath = pp.path.substr(pp.package.size() + 1, -1);
+
+ pp.path = fixedPath;
}
-
- return robotFilename;
- }();
+ };
- ARMARX_INFO << "Robot description '" << VAROUT(robotFileRelPath) << "' found in package " << package;
+ ARMARX_INFO << "Robot description '" << robotPath << "'";
- const armem::robot_state::description::RobotDescription robotDescription
+ auto robotNameHelper = RobotNameHelper::Create(robotPath.toSystemPath());
+
+ arondto::RobotInfo info;
{
+ for (const auto& side :
+ {RobotNameHelper::LocationLeft, RobotNameHelper::LocationRight})
+ {
+ const auto arm = robotNameHelper->getArm(side);
+
+ arondto::RobotInfoElement e;
+ e.end_effector = tryGet([&]() { return arm.getEndEffector(); });
+ e.force_torque_sensor = tryGet([&]() { return arm.getForceTorqueSensor(); });
+ e.force_torque_sensor_frame =
+ tryGet([&]() { return arm.getForceTorqueSensorFrame(); });
+
+ e.full_hand_collision_model.package =
+ tryGet([&]() { return arm.getHandModelPackage(); });
+ e.full_hand_collision_model.path =
+ tryGet([&]() { return arm.getFullHandCollisionModel(); });
+ fixPathDto(e.full_hand_collision_model);
+
+
+ e.hand_controller_name = tryGet([&]() { return arm.getHandControllerName(); });
+
+ e.hand_model.package = tryGet([&]() { return arm.getHandModelPackage(); });
+ e.hand_model.path = tryGet([&]() { return arm.getHandModelPath(); });
+ fixPathDto(e.hand_model);
+
+ e.hand_root_node = tryGet([&]() { return arm.getHandRootNode(); });
+ e.kinematic_chain = tryGet([&]() { return arm.getKinematicChain(); });
+
+ e.palm_collision_model = tryGet([&]() { return arm.getPalmCollisionModel(); });
+
+ e.tcp = tryGet([&]() { return arm.getTCP(); });
+ e.torso_kinematic_chain =
+ tryGet([&]() { return arm.getTorsoKinematicChain(); });
+
+ e.hand_unit =
+ tryGet([&]() { return arm.getHandUnitName(); });
+
+
+ info.parts.emplace(side + "Arm", e);
+ }
+ }
+
+ arondto::RobotDescriptionVisualization visualization;
+ {
+ for (const auto& side :
+ {RobotNameHelper::LocationLeft, RobotNameHelper::LocationRight})
+ {
+ const auto arm = robotNameHelper->getArm(side);
+
+ armarx::armem::arondto::BodyPartDescription bpd;
+ bpd.root_node_name = tryGet([&]() { return arm.getHandRootNode(); });
+ bpd.xml.package = tryGet([&]() { return arm.getHandModelPackage(); });
+ bpd.xml.path = tryGet([&]() { return arm.getHandModelPath(); });
+ fixPathDto(bpd.xml);
+
+ visualization.body_parts.emplace(side + "Hand", bpd);
+ }
+ }
+
+ 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 = {package, robotFileRelPath}
- };
+ .xml = {robotPath.serialize().package, robotPath.serialize().path},
+ .visualization = visualization,
+ .info = info,
+ .manipulationCapabilities = allManipulationCapabilities};
// make sure that the package path is valid
ARMARX_CHECK(std::filesystem::exists(robotDescription.xml.toSystemPath()));
@@ -120,45 +267,45 @@ namespace armarx::armem::server::robot_state::description
}
else
{
- ARMARX_WARNING << "Robot unit '" << robotUnit->ice_getIdentity().name << "' does not have a kinematic unit."
+ ARMARX_WARNING << "Robot unit '" << robotUnit->ice_getIdentity().name
+ << "' does not have a kinematic unit."
<< "\n Cannot commit robot description.";
}
}
-
RobotDescriptionMap
Segment::getRobotDescriptionsLocking(const armem::Time& timestamp) const
{
- return doLocked([this, ×tamp]()
- {
- return getRobotDescriptions(timestamp);
- });
+ return doLocked([this, ×tamp]() { return getRobotDescriptions(timestamp); });
}
-
RobotDescriptionMap
Segment::getRobotDescriptions(const armem::Time& timestamp) const
{
ARMARX_CHECK_NOT_NULL(segmentPtr);
- (void) timestamp; // Unused
+ (void)timestamp; // Unused
RobotDescriptionMap robotDescriptions;
- segmentPtr->forEachEntity([this, &robotDescriptions](const wm::Entity & entity)
- {
- const wm::EntityInstance& entityInstance = entity.getLatestSnapshot().getInstance(0);
- const auto description = ::armarx::armem::robot_state::convertRobotDescription(entityInstance);
- if (description)
+ segmentPtr->forEachEntity(
+ [this, &robotDescriptions](const wm::Entity& entity)
{
- ARMARX_DEBUG << "Key is " << armem::MemoryID(entity.id());
- robotDescriptions.emplace(description->name, *description);
- }
- else
- {
- ARMARX_WARNING << "Could not convert entity instance to 'RobotDescription'";
- }
- });
+ const wm::EntityInstance& entityInstance =
+ entity.getLatestSnapshot().getInstance(0);
+ const auto description =
+ ::armarx::armem::robot_state::convertRobotDescription(entityInstance);
+ if (description)
+ {
+ ARMARX_DEBUG << "Key is " << armem::MemoryID(entity.id());
+ robotDescriptions.emplace(description->name, *description);
+ }
+ else
+ {
+ ARMARX_WARNING << "Could not convert entity instance to 'RobotDescription'";
+ }
+ });
- ARMARX_VERBOSE << deactivateSpam(60) << "Number of known robot descriptions: " << robotDescriptions.size();
+ ARMARX_VERBOSE << deactivateSpam(60)
+ << "Number of known robot descriptions: " << robotDescriptions.size();
return robotDescriptions;
}
diff --git a/source/RobotAPI/libraries/armem_robot_state/types.h b/source/RobotAPI/libraries/armem_robot_state/types.h
index ef1b9c1e9fff8e8cb09cdf1dfa2c869231ec0423..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>
@@ -33,6 +34,7 @@
#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/RobotDescription.aron.generated.h>
namespace armarx::armem::robot_state
{
@@ -45,6 +47,12 @@ namespace armarx::armem::robot_state
std::string name;
PackagePath xml{"", std::filesystem::path("")};
+
+ 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 14e4e64e8a0ba1af5d8a22a824bc76543cc91fe2..c0d4757462c142c3792a323bc44c09407c2431a5 100644
--- a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp
@@ -124,7 +124,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
@@ -132,17 +132,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
@@ -198,7 +192,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
@@ -207,17 +201,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
@@ -233,6 +221,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();
@@ -255,14 +245,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;
}
}
@@ -275,6 +267,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();
@@ -296,14 +290,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;
}
}
@@ -339,10 +335,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;
}
}
@@ -355,6 +351,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);
@@ -379,7 +377,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)
@@ -392,18 +390,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
@@ -444,11 +433,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;
@@ -484,7 +474,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)
@@ -497,18 +487,11 @@ namespace armarx::plugins
provider_statusUpdate_ice.value(), providerId);
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
@@ -549,16 +532,53 @@ 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.");
+ }
+
} // namespace armarx::plugins
namespace armarx
diff --git a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.h b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.h
index 8aeea6f1ab7d387ce6c46e4212e1455c1c2a9435..261037f624c2821d15d79560d24f127125740f8b 100644
--- a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.h
+++ b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.h
@@ -1,11 +1,14 @@
#pragma once
+#include <exception>
#include <mutex>
+#include <optional>
#include <ArmarXCore/core/ComponentPlugin.h>
#include <ArmarXCore/core/ManagedIceObject.h>
#include <RobotAPI/interface/skills/SkillManagerInterface.h>
+#include <RobotAPI/libraries/skills/core/error/Exception.h>
#include <RobotAPI/libraries/skills/core/ProviderID.h>
#include <RobotAPI/libraries/skills/core/ProviderInfo.h>
#include <RobotAPI/libraries/skills/core/SkillExecutionRequest.h>
@@ -53,9 +56,17 @@ namespace armarx::plugins
std::map<skills::SkillExecutionID, skills::SkillStatusUpdate> getSkillExecutionStatuses();
+ protected:
skills::ProviderID getFirstProviderNameThatHasSkill(const skills::SkillID& skillid);
private:
+ [[ 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;
std::mutex skillProviderMapMutex;
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)
{