diff --git a/data/RobotAPI/VariantInfo-RobotAPI.xml b/data/RobotAPI/VariantInfo-RobotAPI.xml
index 8b869fe31552807e5b47222e396eefa9d564092f..e09c9aa52c8ab75de5ca845a812fc6fd38aa4d4e 100644
--- a/data/RobotAPI/VariantInfo-RobotAPI.xml
+++ b/data/RobotAPI/VariantInfo-RobotAPI.xml
@@ -17,6 +17,13 @@
<Variant baseType="::armarx::TrajectoryBase" dataType="::armarx::Trajectory" humanName="Trajectory" include="RobotAPI/libraries/core/Trajectory.h"/>
</Lib>
<Lib name="RobotAPIInterfaces">
+
+ <Class typeName="NJointCartesianWaypointControllerConfig" include="RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.h" />
+ <Class typeName="FramedPoseBase" include="RobotAPI/interface/core/FramedPoseBase.h" />
+ <Class typeName="Vector3Base" include="RobotAPI/libraries/core/Pose.h" />
+ <Class typeName="QuaternionBase" include="RobotAPI/libraries/core/Pose.h" />
+ <Class typeName="armarx::viz::data::Element" include="RobotAPI/interface/ArViz/Elements.h" />
+ <Class typeName="armarx::viz::data::LayerUpdate" include="RobotAPI/interface/ArViz/Component.h" />
<Proxy include="RobotAPI/interface/units/KinematicUnitInterface.h"
humanName="Kinematic Unit"
typeName="KinematicUnitInterfacePrx"
@@ -195,6 +202,16 @@
propertyName="NJointTrajectoryControllerName"
propertyIsOptional="true"
propertyDefaultValue="NJointTrajectoryController" />
+ <Proxy include="RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.h"
+ humanName="NJoint Cartesian Waypoint Controller"
+ typeName="NJointCartesianControllerInterfacePrx"
+ memberName="NJointCartesianWaypointController"
+ getterName="getNJointCartesianWaypointController"
+ propertyName="NJointCartesianWaypointControllerName"
+ propertyIsOptional="true"
+ propertyDefaultValue="NJointCartesianWaypointController" />
+
+
<Proxy include="RobotAPI/interface/core/RobotState.h"
humanName="Robot State Component"
typeName="RobotStateComponentInterfacePrx"
@@ -264,6 +281,17 @@
propertyIsOptional="true"
propertyDefaultValue="ArVizTopic">
</Topic>
+ <Proxy include="RobotAPI/interface/ArViz/Component.h"
+ humanName="ArViz Storage"
+ typeName="armarx::viz::StorageInterfacePrx"
+ memberName="storageProxy"
+ getterName="getStorageProxy"
+ propertyName="StorageName"
+ propertyIsOptional="true"
+ propertyDefaultValue="ArViz">
+ </Proxy>
+
+
<Topic include="RobotAPI/interface/speech/SpeechInterface.h"
humanName="Text to Speech Topic"
typeName="TextListenerInterfacePrx"
@@ -298,7 +326,7 @@
propertyDefaultValue="DynamicObstacleManager" />
<Proxy include="RobotAPI/interface/observers/GraspCandidateObserverInterface.h"
humanName="Grasp Candidate Observer"
- typeName="grasping::GraspCandidateObserverInterfacePrx"
+ typeName="armarx::grasping::GraspCandidateObserverInterfacePrx"
memberName="graspCandidateObserver"
getterName="getGraspCandidateObserver"
propertyName="GraspCandidateObserverName"
diff --git a/scenarios/IkDemo/IkDemo.scx b/scenarios/IkDemo/IkDemo.scx
new file mode 100644
index 0000000000000000000000000000000000000000..a6ae92de68fb42a32bae8a0e2ecd4f8c1785a067
--- /dev/null
+++ b/scenarios/IkDemo/IkDemo.scx
@@ -0,0 +1,5 @@
+<?xml version="1.0" encoding="utf-8"?>
+<scenario name="IkDemo" creation="2022-03-01.11:51:52" globalConfigName="./config/global.cfg" package="RobotAPI" deploymentType="local" nodeName="NodeMain">
+ <application name="ik_demo" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+</scenario>
+
diff --git a/scenarios/IkDemo/config/global.cfg b/scenarios/IkDemo/config/global.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..87eb82db50df2df5b80b0dab8ee03aa81318c849
--- /dev/null
+++ b/scenarios/IkDemo/config/global.cfg
@@ -0,0 +1,4 @@
+# ==================================================================
+# Global Config from Scenario IkDemo
+# ==================================================================
+
diff --git a/scenarios/IkDemo/config/ik_demo.cfg b/scenarios/IkDemo/config/ik_demo.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..47f70e0264c4550308bb307c41cebc709d03b6d7
--- /dev/null
+++ b/scenarios/IkDemo/config/ik_demo.cfg
@@ -0,0 +1,222 @@
+# ==================================================================
+# ik_demo 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.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.IkDemo.ArVizStorageName: Name of the ArViz storage
+# Attributes:
+# - Default: ArVizStorage
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.IkDemo.ArVizStorageName = ArVizStorage
+
+
+# ArmarX.IkDemo.ArVizTopicName: Name of the ArViz topic
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.IkDemo.ArVizTopicName = ArVizTopic
+
+
+# ArmarX.IkDemo.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.IkDemo.EnableProfiling = false
+
+
+# ArmarX.IkDemo.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.IkDemo.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.IkDemo.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.IkDemo.ObjectName = ""
+
+
+# ArmarX.IkDemo.p.robotFile: The ArmarX data path to the robot XML file.
+# For ARMAR-III: 'RobotAPI/robots/Armar3/ArmarIII.xml'
+# For ARMAR-6: 'armar6_rt/robotmodel/Armar6-SH/Armar6-SH.xml'
+# Attributes:
+# - Default: armar6_rt/robotmodel/Armar6-SH/Armar6-SH.xml
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.IkDemo.p.robotFile = armar6_rt/robotmodel/Armar6-SH/Armar6-SH.xml
+
+
+# ArmarX.IkDemo.p.robotNodeSetNames: Names of robot node sets for TCPs.
+# Attributes:
+# - Default: LeftArm; RightArm
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.IkDemo.p.robotNodeSetNames = LeftArm; RightArm
+
+
+# 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/source/RobotAPI/components/CMakeLists.txt b/source/RobotAPI/components/CMakeLists.txt
index 4a6fbed267afc454f8d65c305270ea7ba0105f05..7debb7e6f4aa280c4e28cc1a40bbc59af0bf25c3 100644
--- a/source/RobotAPI/components/CMakeLists.txt
+++ b/source/RobotAPI/components/CMakeLists.txt
@@ -3,6 +3,7 @@ add_subdirectory(units)
add_subdirectory(armem)
add_subdirectory(skills)
+add_subdirectory(ArticulatedObjectLocalizerExample)
add_subdirectory(ArViz)
add_subdirectory(CyberGloveObserver)
@@ -17,6 +18,7 @@ add_subdirectory(EarlyVisionGraph)
# add_subdirectory(FrameTracking)
add_subdirectory(GamepadControlUnit)
+add_subdirectory(ik_demo)
add_subdirectory(KITHandUnit)
add_subdirectory(KITProstheticHandUnit)
add_subdirectory(MultiHandUnit)
@@ -30,5 +32,3 @@ add_subdirectory(RobotToArViz)
add_subdirectory(StatechartExecutorExample)
add_subdirectory(TopicTimingTest)
add_subdirectory(ViewSelection)
-
-add_subdirectory(ArticulatedObjectLocalizerExample)
diff --git a/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.cpp b/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.cpp
index 43f32295717ff031d2ac9b6f54184c21b8c60e73..6359111c24ef51d35360ff8def6853f9fdae55ae 100644
--- a/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.cpp
+++ b/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.cpp
@@ -70,7 +70,9 @@ namespace armarx
while (objectProcessingTask && !objectProcessingTask->isStopped())
{
- const objpose::ObjectPoseSeq objectPoses = ObjectPoseClient::getObjectPoses();
+ // This client can be copied to other classes to give them access to the object pose storage.
+ objpose::ObjectPoseClient client = getClient();
+ const objpose::ObjectPoseSeq objectPoses = client.fetchObjectPoses();
ARMARX_VERBOSE << "Received poses of " << objectPoses.size() << " objects.";
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
index 19d3e19df844731d355a05a95986216d68ead88a..4d4d2399bb3ee5059d9788fb60f61cf23f78c2b0 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
@@ -356,10 +356,10 @@ namespace armarx
insertPose(time, globalRobotPose.transform);
_synchronized->setGlobalPose(globalRobotPose.transform);
- _sharedRobotServant->setGlobalPose(new Pose(globalRobotPose.transform));
if (_sharedRobotServant)
{
+ _sharedRobotServant->setGlobalPose(new Pose(globalRobotPose.transform));
_sharedRobotServant->setTimestamp(time);
}
}
@@ -703,6 +703,7 @@ namespace armarx
const Eigen::Matrix4f globalPose = math::Helpers::Pose(position, orientation);
IceUtil::Time time = IceUtil::Time::microSeconds(currentPose.timestampInMicroSeconds);
+ // ARMARX_IMPORTANT << VAROUT(currentPose.timestampInMicroSeconds);
TransformStamped stamped;
stamped.header.frame = armarx::GlobalFrame;
diff --git a/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp
index 10b31920d68599552ab3dc0713a41bf8ab56033f..db3fb7ee4c04b359d97ca606e54fe2961750043c 100644
--- a/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp
+++ b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp
@@ -174,6 +174,10 @@ namespace armarx
{
//commitExampleImages();
}
+ if (true)
+ {
+ commitExamplesWithUntypedData();
+ }
CycleUtil c(static_cast<int>(1000 / p.commitFrequency));
while (!task->isStopped())
@@ -602,6 +606,37 @@ namespace armarx
}
}
+ void ExampleMemoryClient::commitExamplesWithUntypedData()
+ {
+ const armem::Time time = armem::Time::now();
+
+ armem::Commit commit;
+ {
+ armem::EntityUpdate& update = commit.add();
+ update.entityID = exampleDataProviderID.withEntityName("unexpected_data");
+ update.timeCreated = time;
+
+ armem::example::ExampleData data;
+ toAron(data.memoryLink, armem::MemoryID()); // ////1/1
+
+ aron::data::DictPtr aron = data.toAron();
+ aron->addElement("unexpectedString", std::make_shared<aron::data::String>("unexpected value"));
+ aron->addElement("unexpectedDict", std::make_shared<aron::data::Dict>(
+ std::map<std::string, aron::data::VariantPtr>{
+ { "key43", std::make_shared<aron::data::Int>(43) },
+ { "keyABC", std::make_shared<aron::data::String>("ABC") },
+ }
+ ));
+ update.instancesData = { aron };
+ }
+
+ armem::CommitResult commitResult = memoryWriter.commit(commit);
+ if (!commitResult.allSuccess())
+ {
+ ARMARX_WARNING << commitResult.allErrorMessages();
+ }
+ }
+
void ExampleMemoryClient::processExampleEntityUpdate(
const armem::MemoryID& subscriptionID, const std::vector<armem::MemoryID>& snapshotIDs)
diff --git a/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.h b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.h
index 0adda091a10f7c07585fe1a34d00ee3d7cae0737..930abfea7618c4ece3f7298c97376a54b201bac6 100644
--- a/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.h
+++ b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.h
@@ -102,6 +102,8 @@ namespace armarx
void commitExampleImages();
+ void commitExamplesWithUntypedData();
+
private:
diff --git a/source/RobotAPI/components/armem/server/CMakeLists.txt b/source/RobotAPI/components/armem/server/CMakeLists.txt
index 58c3758d1ed5e56127d702137a0746ddfcd454a3..7ddc79776f7e56d2c71596f4e3c0c5656f18875f 100644
--- a/source/RobotAPI/components/armem/server/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/server/CMakeLists.txt
@@ -1,6 +1,7 @@
add_subdirectory(ExampleMemory)
add_subdirectory(GeneralPurposeMemory)
add_subdirectory(ObjectMemory)
+add_subdirectory(ReasoningMemory)
add_subdirectory(RobotStateMemory)
add_subdirectory(SkillsMemory)
add_subdirectory(GraspMemory)
diff --git a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
index 98788a3a67c1803f64d97d65a101223f0685e810..4adee0b0fb84415490dadf5f8394716086eb3adf 100644
--- a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
@@ -72,7 +72,8 @@ namespace armarx
for (const std::string& name : p.core.defaultCoreSegments)
{
- workingMemory().addCoreSegment(name);
+ auto& c = workingMemory().addCoreSegment(name);
+ c.setMaxHistorySize(100);
}
}
diff --git a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp
index 412b526217bec76a64391cca1de105840032778a..ef87c2417ddfbd11b8eb14c739382154fb129ad8 100644
--- a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp
+++ b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp
@@ -23,7 +23,7 @@ namespace armarx::armem::server::grasp
defs->topic(debugObserver);
defs->optional(memoryName, "memory.Name", "Name of this memory server.");
-
+ defs->optional(enableRemoteGui, "remoteGui.enable", "Enable/Disable Remote GUI");
return defs;
}
@@ -54,10 +54,11 @@ namespace armarx::armem::server::grasp
void GraspMemory::onConnectComponent()
{
-
- createRemoteGuiTab();
- RemoteGui_startRunningTask();
-
+ if (enableRemoteGui)
+ {
+ createRemoteGuiTab();
+ RemoteGui_startRunningTask();
+ }
}
void GraspMemory::onDisconnectComponent()
diff --git a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h
index 867bf0a62575f2affd51e1842d6631ceb0204e8e..b40af2691ee8bddf31c86816ae436752b4c36351 100644
--- a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h
+++ b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h
@@ -99,5 +99,7 @@ namespace armarx::armem::server::grasp
};
GuiInfo gui;
+ bool enableRemoteGui{true};
+
};
}
diff --git a/source/RobotAPI/components/armem/server/ReasoningMemory/CMakeLists.txt b/source/RobotAPI/components/armem/server/ReasoningMemory/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..6d31cfd8a9646d5df26104efb45cfe855d6a6214
--- /dev/null
+++ b/source/RobotAPI/components/armem/server/ReasoningMemory/CMakeLists.txt
@@ -0,0 +1,28 @@
+armarx_component_set_name("ReasoningMemory")
+
+
+set(COMPONENT_LIBS
+ ArmarXCore ArmarXCoreInterfaces # for DebugObserverInterface
+ ArmarXGuiComponentPlugins
+ RobotAPICore RobotAPIInterfaces armem_server
+ RobotAPIComponentPlugins # for ArViz and other plugins
+ armem_reasoning
+ armem_reasoning_server
+)
+
+set(SOURCES
+ ReasoningMemory.cpp
+)
+set(HEADERS
+ ReasoningMemory.h
+)
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
+
+#generate the application
+armarx_generate_and_add_component_executable(
+ COMPONENT_NAMESPACE ::armarx::armem::server
+)
+
+
+
diff --git a/source/RobotAPI/components/armem/server/ReasoningMemory/ReasoningMemory.cpp b/source/RobotAPI/components/armem/server/ReasoningMemory/ReasoningMemory.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7ea316348abfa01cf08700ea362a6faa57a3afb9
--- /dev/null
+++ b/source/RobotAPI/components/armem/server/ReasoningMemory/ReasoningMemory.cpp
@@ -0,0 +1,77 @@
+/*
+ * 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::ArmarXObjects::ExampleMemory
+ * @author Markus Grotz ( markus dot grotz at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "ReasoningMemory.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <SimoxUtility/algorithm/string.h>
+
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+
+
+namespace armarx::armem::server
+{
+ armarx::PropertyDefinitionsPtr ReasoningMemory::createPropertyDefinitions()
+ {
+ armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
+
+ setMemoryName("Reasoning");
+ return defs;
+ }
+
+
+ ReasoningMemory::ReasoningMemory() :
+ anticipationSegment(iceAdapter())
+ {
+
+ }
+
+ std::string ReasoningMemory::getDefaultName() const
+ {
+ return "ReasoningMemory";
+ }
+
+ void ReasoningMemory::onInitComponent()
+ {
+
+ {
+ wm::CoreSegment& cs = workingMemory().addCoreSegment("Anticipation", armarx::reasoning::arondto::Anticipation::ToAronType());
+ cs.setMaxHistorySize(20);
+ }
+
+
+ }
+
+ void ReasoningMemory::onConnectComponent()
+ {
+ }
+
+ void ReasoningMemory::onDisconnectComponent()
+ {
+ }
+
+ void ReasoningMemory::onExitComponent()
+ {
+ }
+}
diff --git a/source/RobotAPI/components/armem/server/ReasoningMemory/ReasoningMemory.h b/source/RobotAPI/components/armem/server/ReasoningMemory/ReasoningMemory.h
new file mode 100644
index 0000000000000000000000000000000000000000..2ad670842bb15e1159326a977d23497af714aaa8
--- /dev/null
+++ b/source/RobotAPI/components/armem/server/ReasoningMemory/ReasoningMemory.h
@@ -0,0 +1,55 @@
+/*
+ * 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::ArmarXObjects::ExampleMemory
+ * @author Markus Grotz ( markus dot grotz at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <ArmarXCore/core/Component.h>
+
+#include <RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h>
+#include <RobotAPI/libraries/armem_reasoning/server/AnticipationSegment.h>
+
+namespace armarx::armem::server
+{
+ class ReasoningMemory :
+ virtual public armarx::Component,
+ virtual public armem::server::ReadWritePluginUser
+ {
+ public:
+ ReasoningMemory();
+
+ /// @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;
+
+
+ private:
+ reasoning::AnticipationSegment anticipationSegment;
+ };
+}
diff --git a/source/RobotAPI/components/ik_demo/CMakeLists.txt b/source/RobotAPI/components/ik_demo/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..68bdb926aa9da39ecd30029a936067a717e781df
--- /dev/null
+++ b/source/RobotAPI/components/ik_demo/CMakeLists.txt
@@ -0,0 +1,72 @@
+armarx_component_set_name(ik_demo)
+
+set(COMPONENT_LIBS
+ # DecoupledSingleComponent
+ ArViz
+ RobotAPIComponentPlugins
+ diffik
+)
+
+set(SOURCES
+ ik_demo.cpp
+ IkDemo.cpp
+ PoseGizmo.cpp
+)
+set(HEADERS
+ ik_demo.h
+ IkDemo.h
+ PoseGizmo.h
+)
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
+
+
+# add unit tests
+# add_subdirectory(test)
+
+# generate the application
+armarx_generate_and_add_component_executable(
+ COMPONENT_NAMESPACE ::armar6::skills::components::armar6_ik_demo
+)
+
+
+
+if (false) # Modern
+ armarx_add_component(ik_demo
+ ICE_FILES
+ ComponentInterface.ice
+
+ ICE_DEPENDENCIES
+ ArmarXCoreInterfaces
+ RobotAPIInterfaces
+
+ SOURCES
+ ik_demo.cpp
+ IkDemo.cpp
+ PoseGizmo.cpp
+
+ HEADERS
+ ik_demo.h
+ IkDemo.h
+ PoseGizmo.h
+
+ DEPENDENCIES
+ # ArmarXCore
+ ArmarXCore
+ ## ArmarXCoreComponentPlugins # For DebugObserver plugin.
+ # ArmarXGui
+ ## ArmarXGuiComponentPlugins # For RemoteGui plugin.
+ # RobotAPI
+ ## RobotAPICore
+ # RobotAPIInterfaces
+ RobotAPIComponentPlugins # For ArViz and other plugins.
+
+ diffik
+
+ # DEPENDENCIES_LEGACY
+ ## Add libraries that do not provide any targets but ${FOO_*} variables.
+ # FOO
+ # If you need a separate shared component library you can enable it with the following flag.
+ # SHARED_COMPONENT_LIBRARY
+ )
+endif()
diff --git a/source/RobotAPI/components/ik_demo/ComponentInterface.ice b/source/RobotAPI/components/ik_demo/ComponentInterface.ice
new file mode 100644
index 0000000000000000000000000000000000000000..edde6d507af83320f080fb20c61856a4036747b3
--- /dev/null
+++ b/source/RobotAPI/components/ik_demo/ComponentInterface.ice
@@ -0,0 +1,35 @@
+/*
+ * 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 skills::Armar6IkDemo
+ * author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * date 2022
+ * copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#pragma once
+
+
+module armar6 { module skills { module components { module armar6_ik_demo
+{
+
+ interface ComponentInterface
+ {
+ // Define your interface here.
+ };
+
+};};};};
diff --git a/source/RobotAPI/components/ik_demo/IkDemo.cpp b/source/RobotAPI/components/ik_demo/IkDemo.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2d60ef2feeae53056890925f44a799a7ea8f5b10
--- /dev/null
+++ b/source/RobotAPI/components/ik_demo/IkDemo.cpp
@@ -0,0 +1,436 @@
+#include "IkDemo.h"
+
+#include <SimoxUtility/algorithm/string.h>
+#include <SimoxUtility/meta/EnumNames.hpp>
+#include <SimoxUtility/math/pose/invert.h>
+
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/XML/RobotIO.h>
+#include <VirtualRobot/IK/CompositeDiffIK/CompositeDiffIK.h>
+#include <VirtualRobot/IK/CompositeDiffIK/ManipulabilityNullspaceGradient.h>
+#include <VirtualRobot/IK/CompositeDiffIK/SoechtingNullspaceGradient.h>
+
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+
+#include <RobotAPI/components/ArViz/Client/Client.h>
+#include <RobotAPI/libraries/diffik/SimpleDiffIK.h>
+
+#include "PoseGizmo.h"
+
+
+namespace viz = armarx::viz;
+
+
+namespace armar6::skills::components::armar6_ik_demo
+{
+
+ enum class IkMethod
+ {
+ SimpleDiffIk,
+ CompositeDiffIk,
+ };
+ const simox::meta::EnumNames<IkMethod> IkMethodNames =
+ {
+ { IkMethod::SimpleDiffIk, "Simple Diff IK" },
+ { IkMethod::CompositeDiffIk, "Composite Diff IK" },
+ };
+
+ struct Manipulator
+ {
+ Manipulator()
+ {
+ gizmo.box.color(simox::Color::cyan(255, 64));
+ }
+ virtual ~Manipulator()
+ {
+ }
+
+ virtual bool handle(viz::InteractionFeedback const& interaction, viz::StagedCommit* stage)
+ {
+ bool updated = false;
+ if (interaction.layer() == gizmo.layer.data_.name)
+ {
+ updated |= gizmo.handleInteraction(interaction, stage);
+ }
+ return updated;
+ }
+
+ virtual void visualize(viz::Client& arviz) = 0;
+ virtual void runIk(IkDemo::Robot& robot) = 0;
+
+ viz::PoseGizmo gizmo;
+ };
+
+
+ struct TcpManipulator : public Manipulator
+ {
+ TcpManipulator()
+ {
+ std::vector<std::string> options;
+ for (IkMethod method : IkMethodNames.values())
+ {
+ options.push_back("Use " + IkMethodNames.to_name(method));
+ }
+ gizmo.box
+ .size({75, 100, 200})
+ .enable(viz::interaction().selection().transform().hideDuringTransform()
+ .contextMenu(options));
+ };
+
+ void visualize(viz::Client& arviz) override
+ {
+ gizmo.setLayer(arviz.layer(tcp->getName()));
+ gizmo.update();
+ }
+
+ bool handle(viz::InteractionFeedback const& interaction, viz::StagedCommit* stage) override
+ {
+ bool updated = Manipulator::handle(interaction, stage);
+
+ if (interaction.layer() == gizmo.layer.data_.name)
+ {
+ switch (interaction.type())
+ {
+ case viz::InteractionFeedbackType::ContextMenuChosen:
+ {
+ int i = 0;
+ for (IkMethod method : IkMethodNames.values())
+ {
+ if (i == interaction.chosenContextMenuEntry())
+ {
+ this->method = method;
+ updated |= true;
+ ARMARX_IMPORTANT << "[" << tcp->getName() << "] Using " << IkMethodNames.to_name(method);
+ break;
+ }
+ ++i;
+ }
+ } break;
+ default:
+ break;
+ }
+ }
+
+ return updated;
+ }
+
+ void runIk(IkDemo::Robot& robot) override
+ {
+ const Eigen::Matrix4f tcpPoseInRobotFrame =
+ simox::math::inverted_pose(robot.robot->getGlobalPose()) * gizmo.getCurrent();
+
+ switch (method)
+ {
+ case IkMethod::SimpleDiffIk:
+ {
+ armarx::SimpleDiffIK ik;
+ armarx::SimpleDiffIK::Result result = ik.CalculateDiffIK(tcpPoseInRobotFrame, nodeSet, tcp);
+
+ if (result.reached)
+ {
+ gizmo.box.color(simox::Color::kit_green(64));
+
+ std::map<std::string, float> jointValues;
+ size_t i = 0;
+ for (const auto& rn : nodeSet->getAllRobotNodes())
+ {
+ jointValues[rn->getName()] = result.jointValues(i);
+ ++i;
+ }
+
+ robot.robot->setJointValues(jointValues);
+ }
+ else
+ {
+ gizmo.box.color(simox::Color::red(255, 64));
+ }
+ } break;
+
+ case IkMethod::CompositeDiffIk:
+ {
+ // Code taken from: simox/VirtualRobot/examples/RobotViewer/DiffIKWidget.cpp
+
+ const float jointLimitAvoidance = 0;
+ const float kGainManipulabilityAsNullspace = 0.01;
+ const float kSoechtingAsNullspace = 0.0;
+ const int steps = 100;
+
+ VirtualRobot::CompositeDiffIK ik(nodeSet);
+ Eigen::Matrix4f pose = tcpPoseInRobotFrame;
+
+ const bool ori = true;
+ VirtualRobot::CompositeDiffIK::TargetPtr target1 = ik.addTarget(
+ tcp, pose, ori ? VirtualRobot::IKSolver::All : VirtualRobot::IKSolver::Position);
+
+
+ if (jointLimitAvoidance > 0)
+ {
+ VirtualRobot::CompositeDiffIK::NullspaceJointLimitAvoidancePtr nsjla(
+ new VirtualRobot::CompositeDiffIK::NullspaceJointLimitAvoidance(nodeSet));
+ nsjla->kP = jointLimitAvoidance;
+ for (auto node : nodeSet->getAllRobotNodes())
+ {
+ if (node->isLimitless())
+ {
+ nsjla->setWeight(node->getName(), 0);
+ }
+ }
+ ik.addNullspaceGradient(nsjla);
+ }
+
+ VirtualRobot::NullspaceManipulabilityPtr nsman = nullptr;
+ if (kGainManipulabilityAsNullspace > 0)
+ {
+#if 0
+ std::cout << "Adding manipulability as nullspace target" << std::endl;
+ auto manipTracking = getManipulabilityTracking(nodeSet, nullptr);
+ if (!manipTracking)
+ {
+ std::cout << "Manip tracking zero!" << std::endl;
+ return;
+ }
+ Eigen::MatrixXd followManip = readFollowManipulability();
+ if (followManip.rows() != manipTracking->getTaskVars())
+ {
+ std::cout << "Wrong manipulability matrix!" << std::endl;
+ return;
+ }
+ nsman = VirtualRobot::NullspaceManipulabilityPtr(new VirtualRobot::NullspaceManipulability(manipTracking, followManip, Eigen::MatrixXd(), true));
+ nsman->kP = kGainManipulabilityAsNullspace;
+ ik.addNullspaceGradient(nsman);
+#endif
+ }
+
+ if (kSoechtingAsNullspace > 0)
+ {
+ if (robot.robot->getName() == "Armar6" and nodeSet->getName() == "RightArm")
+ {
+ std::cout << "Adding soechting nullspace" << std::endl;
+ VirtualRobot::SoechtingNullspaceGradient::ArmJoints armjoints;
+ armjoints.clavicula = robot.robot->getRobotNode("ArmR1_Cla1");
+ armjoints.shoulder1 = robot.robot->getRobotNode("ArmR2_Sho1");
+ armjoints.shoulder2 = robot.robot->getRobotNode("ArmR3_Sho2");
+ armjoints.shoulder3 = robot.robot->getRobotNode("ArmR4_Sho3");
+ armjoints.elbow = robot.robot->getRobotNode("ArmR5_Elb1");
+
+ auto gradient = std::make_shared<VirtualRobot::SoechtingNullspaceGradient>(
+ target1, "ArmR2_Sho1", VirtualRobot::Soechting::ArmType::Right, armjoints);
+
+ gradient->kP = kSoechtingAsNullspace;
+ ik.addNullspaceGradient(gradient);
+ }
+ else if (robot.robot->getName() == "Armar6" and nodeSet->getName() == "LeftArm")
+ {
+ std::cout << "Adding soechting nullspace" << std::endl;
+ VirtualRobot::SoechtingNullspaceGradient::ArmJoints armjoints;
+ armjoints.clavicula = robot.robot->getRobotNode("ArmL1_Cla1");
+ armjoints.shoulder1 = robot.robot->getRobotNode("ArmL2_Sho1");
+ armjoints.shoulder2 = robot.robot->getRobotNode("ArmL3_Sho2");
+ armjoints.shoulder3 = robot.robot->getRobotNode("ArmL4_Sho3");
+ armjoints.elbow = robot.robot->getRobotNode("ArmL5_Elb1");
+
+ auto gradient = std::make_shared<VirtualRobot::SoechtingNullspaceGradient>(
+ target1, "ArmL2_Sho1", VirtualRobot::Soechting::ArmType::Left, armjoints);
+ gradient->kP = kSoechtingAsNullspace;
+ ik.addNullspaceGradient(gradient);
+ }
+ else
+ {
+ ARMARX_INFO << "Soechting currently supports only Armar6 and RightArm/LeftArm robot node set "
+ "for first robot node set for demonstration purposes.";
+ }
+ }
+
+ {
+ VirtualRobot::CompositeDiffIK::Parameters cp;
+ cp.resetRnsValues = false;
+ cp.returnIKSteps = true;
+ cp.steps = 1;
+ VirtualRobot::CompositeDiffIK::SolveState state;
+ ik.solve(cp, state);
+
+ int i = 0;
+ while (i < steps or (steps < 0 and not ik.getLastResult().reached and i < 1000))
+ {
+ ik.step(cp, state, i);
+ i++;
+ }
+
+ if (ik.getLastResult().reached)
+ {
+ gizmo.box.color(simox::Color::kit_green(64));
+ robot.robot->setJointValues(ik.getRobotNodeSet()->getJointValueMap());
+ }
+ else
+ {
+ gizmo.box.color(simox::Color::red(255, 64));
+ }
+ }
+ } break;
+ }
+ }
+
+
+ VirtualRobot::RobotNodeSetPtr nodeSet;
+ VirtualRobot::RobotNodePtr tcp;
+
+ IkMethod method = IkMethod::SimpleDiffIk;
+ };
+
+
+ struct PlatformManipulator : public Manipulator
+ {
+ PlatformManipulator()
+ {
+ gizmo.box.size({1000, 1000, 100});
+ }
+
+ void visualize(viz::Client& arviz) override
+ {
+ gizmo.setLayer(arviz.layer(root->getName()));
+ gizmo.update();
+ }
+ void runIk(IkDemo::Robot& robot) override
+ {
+ robot.robot->setGlobalPose(gizmo.getCurrent());
+ }
+
+ VirtualRobot::RobotNodePtr root;
+ };
+
+
+
+ IkDemo::IkDemo()
+ {
+ }
+
+ IkDemo::~IkDemo()
+ {
+ }
+
+
+ IkDemo::Params::Params() :
+ robotFile("armar6_rt/robotmodel/Armar6-SH/Armar6-SH.xml"),
+ robotNodeSetNamesStr(simox::alg::join({"LeftArm", "RightArm"}, "; "))
+ {
+ }
+
+
+ std::vector<std::string> IkDemo::Params::robotNodeSetNames() const
+ {
+ bool trim = true;
+ return simox::alg::split(robotNodeSetNamesStr, ";", trim);
+ }
+
+
+ void IkDemo::defineProperties(armarx::PropertyDefinitionContainer& defs)
+ {
+ defs.optional(params.robotFile, "p.robotFile",
+ "The ArmarX data path to the robot XML file."
+ "\n For ARMAR-III: 'RobotAPI/robots/Armar3/ArmarIII.xml'"
+ "\n For ARMAR-6: 'armar6_rt/robotmodel/Armar6-SH/Armar6-SH.xml'"
+ );
+ defs.optional(params.robotNodeSetNamesStr, "p.robotNodeSetNames",
+ "Names of robot node sets for TCPs.");
+ }
+
+
+ void IkDemo::start()
+ {
+ this->stage.reset();
+
+ {
+ params.robotFile = armarx::ArmarXDataPath::resolvePath(params.robotFile);
+ robot.robot = VirtualRobot::RobotIO::loadRobot(params.robotFile, VirtualRobot::RobotIO::eStructure);
+
+ robot.layer = remote.arviz->layer("Robot");
+
+ robot.visu.file("", robot.robot->getFilename()).joints(robot.robot->getJointValues());
+ robot.layer.add(robot.visu);
+ stage.add(robot.layer);
+ }
+
+ {
+ auto root = robot.robot->getRootNode();
+
+ std::unique_ptr<PlatformManipulator> manip = std::make_unique<PlatformManipulator>();
+
+ manip->gizmo.initial = root->getGlobalPose();
+ manip->root = root;
+
+ manipulators.emplace_back(std::move(manip));
+ }
+
+ for (const std::string& rnsName : params.robotNodeSetNames())
+ {
+ ARMARX_CHECK(robot.robot->hasRobotNodeSet(rnsName)) << VAROUT(rnsName) << " must exist.";
+
+ const auto& rns = robot.robot->getRobotNodeSet(rnsName);
+ const auto tcp = rns->getTCP();
+ ARMARX_CHECK_NOT_NULL(tcp) << VAROUT(rnsName) << " must have a TCP.";
+
+ std::unique_ptr<TcpManipulator> manip = std::make_unique<TcpManipulator>();
+
+ manip->gizmo.initial = tcp->getGlobalPose();
+ manip->tcp = tcp;
+ manip->nodeSet = rns;
+
+ manipulators.emplace_back(std::move(manip));
+ }
+
+ for (auto& manip : manipulators)
+ {
+ manip->visualize(remote.arviz.value());
+ stage.add(manip->gizmo.layer);
+ }
+
+ runIk();
+
+ viz::CommitResult result = remote.arviz->commit(stage);
+ ARMARX_VERBOSE << "Initial commit at revision: " << result.revision();
+ }
+
+
+ void IkDemo::update()
+ {
+ viz::CommitResult result = remote.arviz->commit(stage);
+
+ // Reset the stage, so that it can be rebuild during the interaction handling
+ stage.reset();
+
+ for (auto& manip : manipulators)
+ {
+ stage.requestInteraction(manip->gizmo.layer);
+ }
+
+ viz::InteractionFeedbackRange interactions = result.interactions();
+
+ bool updated = false;
+ for (viz::InteractionFeedback const& inter : interactions)
+ {
+ for (auto& manip : manipulators)
+ {
+ updated |= manip->handle(inter, &stage);
+ }
+ }
+ if (updated)
+ {
+ runIk();
+ }
+ }
+
+
+ void IkDemo::runIk()
+ {
+ for (auto& manip : manipulators)
+ {
+ manip->runIk(robot);
+ }
+
+ robot.visu.pose(robot.robot->getGlobalPose());
+ robot.visu.joints(robot.robot->getJointValues());
+ stage.add(robot.layer);
+ }
+
+}
diff --git a/source/RobotAPI/components/ik_demo/IkDemo.h b/source/RobotAPI/components/ik_demo/IkDemo.h
new file mode 100644
index 0000000000000000000000000000000000000000..b48b040efe2ee6f10272edea446701c7695c14d0
--- /dev/null
+++ b/source/RobotAPI/components/ik_demo/IkDemo.h
@@ -0,0 +1,68 @@
+#pragma once
+
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <ArmarXCore/core/application/properties/forward_declarations.h>
+
+#include <RobotAPI/components/ArViz/Client/Client.h>
+
+
+namespace armar6::skills::components::armar6_ik_demo
+{
+
+ struct Manipulator;
+
+
+ class IkDemo
+ {
+ public:
+
+ struct Params
+ {
+ Params();
+
+ std::string robotFile;
+ std::string robotNodeSetNamesStr;
+ std::vector<std::string> robotNodeSetNames() const;
+ };
+ struct Remote
+ {
+ std::optional<armarx::viz::Client> arviz;
+ };
+ struct Robot
+ {
+ VirtualRobot::RobotPtr robot;
+
+ armarx::viz::Layer layer;
+ armarx::viz::Robot visu { "robot" };
+ };
+
+
+ public:
+
+ IkDemo();
+ ~IkDemo();
+
+ void defineProperties(armarx::PropertyDefinitionContainer& defs);
+
+ void connect(Remote&& remote);
+
+ void start();
+ void update();
+
+ void runIk();
+
+
+ public:
+
+ Remote remote;
+ Params params;
+ Robot robot;
+
+ armarx::viz::StagedCommit stage;
+
+ std::vector<std::unique_ptr<Manipulator>> manipulators;
+
+ };
+
+}
diff --git a/source/RobotAPI/components/ik_demo/PoseGizmo.cpp b/source/RobotAPI/components/ik_demo/PoseGizmo.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..04153d3f8e8c2b412b948f3037c002b3d78e52ab
--- /dev/null
+++ b/source/RobotAPI/components/ik_demo/PoseGizmo.cpp
@@ -0,0 +1,102 @@
+#include "PoseGizmo.h"
+
+
+namespace viz = armarx::viz;
+
+
+namespace armarx::viz
+{
+
+ PoseGizmo::PoseGizmo()
+ {
+ box.enable(viz::interaction().selection().transform().hideDuringTransform());
+ }
+
+ void PoseGizmo::setLayer(const Layer& layer_)
+ {
+ this->layer = layer_;
+ this->layer.add(box);
+ this->layer.add(pose);
+ }
+
+ void PoseGizmo::update()
+ {
+ const Eigen::Matrix4f current = getCurrent();
+ box.pose(current);
+ pose.pose(current);
+ }
+
+ void PoseGizmo::updateDuringTransform()
+ {
+ const Eigen::Matrix4f current = getCurrent();
+ box.pose(initial);
+ pose.pose(current);
+ }
+
+ bool
+ PoseGizmo::handleInteraction(
+ const InteractionFeedback& interaction,
+ StagedCommit* stage)
+ {
+ if (interaction.element() != box.data_->id)
+ {
+ return false;
+ }
+
+ switch (interaction.type())
+ {
+ case viz::InteractionFeedbackType::Select:
+ {
+ // Nothing to do.
+ } break;
+
+ case viz::InteractionFeedbackType::Transform:
+ {
+ // Update state of tcp object
+ transform = interaction.transformation();
+
+ if (interaction.isTransformDuring())
+ {
+ updateDuringTransform();
+ }
+
+ stage->add(layer);
+ return true;
+ } break;
+
+ case viz::InteractionFeedbackType::Deselect:
+ {
+ // If an object is deselected, we apply the transformation
+
+ initial = getCurrent();
+ transform.setIdentity();
+
+ update();
+ stage->add(layer);
+
+ return true;
+ } break;
+
+ case viz::InteractionFeedbackType::ContextMenuChosen:
+ {
+ }
+
+ default:
+ {
+ // Ignore other interaction types
+ } break;
+
+ }
+
+ return false;
+ }
+
+
+ Eigen::Matrix4f PoseGizmo::getCurrent() const
+ {
+ return transform * initial;
+ }
+
+}
+
+
diff --git a/source/RobotAPI/components/ik_demo/PoseGizmo.h b/source/RobotAPI/components/ik_demo/PoseGizmo.h
new file mode 100644
index 0000000000000000000000000000000000000000..482eea5aa420ee1d302f7c2d5f6f94bfd944b1dc
--- /dev/null
+++ b/source/RobotAPI/components/ik_demo/PoseGizmo.h
@@ -0,0 +1,40 @@
+#pragma once
+
+#include <RobotAPI/components/ArViz/Client/Elements.h>
+#include <RobotAPI/components/ArViz/Client/Layer.h>
+#include <RobotAPI/components/ArViz/Client/Client.h>
+
+
+namespace armarx::viz
+{
+
+ class PoseGizmo
+ {
+ public:
+
+ PoseGizmo();
+
+ void setLayer(const viz::Layer& layer);
+
+ void update();
+ void updateDuringTransform();
+
+ bool handleInteraction(const viz::InteractionFeedback& interaction,
+ viz::StagedCommit* stage);
+
+ Eigen::Matrix4f getCurrent() const;
+
+
+ public:
+
+ Eigen::Matrix4f initial = Eigen::Matrix4f::Identity();
+ Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
+
+ viz::Layer layer;
+
+ viz::Box box { "box" };
+ viz::Pose pose { "pose" };
+
+ };
+
+}
diff --git a/source/RobotAPI/components/ik_demo/ik_demo.cpp b/source/RobotAPI/components/ik_demo/ik_demo.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..73b838422a9f579a290dcce2814e1b5c236173b6
--- /dev/null
+++ b/source/RobotAPI/components/ik_demo/ik_demo.cpp
@@ -0,0 +1,227 @@
+/**
+ * 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 skills::ArmarXObjects::armar6_ik_demo
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#include "ik_demo.h"
+
+#include <Eigen/Core>
+
+#include <SimoxUtility/color/Color.h>
+
+// #include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
+#include <ArmarXCore/core/time/CycleUtil.h>
+
+#include <RobotAPI/components/ArViz/Client/Client.h>
+
+#include "IkDemo.h"
+
+
+namespace viz = armarx::viz;
+
+
+namespace armar6::skills::components::armar6_ik_demo
+{
+
+ const std::string
+ ik_demo::defaultName = "IkDemo";
+
+
+ ik_demo::ik_demo() : impl(new IkDemo)
+ {
+ }
+
+
+ armarx::PropertyDefinitionsPtr
+ ik_demo::createPropertyDefinitions()
+ {
+ armarx::PropertyDefinitionsPtr def = new armarx::ComponentPropertyDefinitions(getConfigIdentifier());
+
+ // Publish to a topic (passing the TopicListenerPrx).
+ // def->topic(myTopicListener);
+
+ // Subscribe to a topic (passing the topic name).
+ // def->topic<PlatformUnitListener>("MyTopic");
+
+ // Use (and depend on) another component (passing the ComponentInterfacePrx).
+ // def->component(myComponentProxy)
+
+ impl->defineProperties(*def);
+
+
+ // Add a required property. (The component won't start without a value being set.)
+ // def->required(properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz.");
+
+ // Add an optionalproperty.
+ // def->optional(properties.numBoxes, "p.box.Number", "Number of boxes to draw in ArViz.");
+
+ return def;
+ }
+
+
+ void
+ ik_demo::onInitComponent()
+ {
+ // Topics and properties defined above are automagically registered.
+
+ // Keep debug observer data until calling `sendDebugObserverBatch()`.
+ // (Requies the armarx::DebugObserverComponentPluginUser.)
+ // setDebugObserverBatchModeEnabled(true);
+ }
+
+
+ void
+ ik_demo::onConnectComponent()
+ {
+ impl->remote.arviz = arviz;
+
+ task = new armarx::SimpleRunningTask<>([this]() { this->run(); });
+ task->start();
+
+ /* (Requies the armarx::DebugObserverComponentPluginUser.)
+ // Use the debug observer to log data over time.
+ // The data can be viewed in the ObserverView and the LivePlotter.
+ // (Before starting any threads, we don't need to lock mutexes.)
+ {
+ setDebugObserverDatafield("numBoxes", properties.numBoxes);
+ setDebugObserverDatafield("boxLayerName", properties.boxLayerName);
+ sendDebugObserverBatch();
+ }
+ */
+
+ /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
+ // Setup the remote GUI.
+ {
+ createRemoteGuiTab();
+ RemoteGui_startRunningTask();
+ }
+ */
+ }
+
+
+ void
+ ik_demo::onDisconnectComponent()
+ {
+ const bool join = true;
+ task->stop(join);
+ task = nullptr;
+ }
+
+
+ void
+ ik_demo::onExitComponent()
+ {
+ }
+
+
+ void ik_demo::run()
+ {
+ impl->start();
+
+ armarx::CycleUtil cycle(10.0f);
+ while (!task->isStopped())
+ {
+ impl->update();
+
+ cycle.waitForCycleDuration();
+ }
+ }
+
+
+ std::string
+ ik_demo::getDefaultName() const
+ {
+ return ik_demo::defaultName;
+ }
+
+
+ std::string
+ ik_demo::GetDefaultName()
+ {
+ return ik_demo::defaultName;
+ }
+
+
+ /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
+ void
+ Component::createRemoteGuiTab()
+ {
+ using namespace armarx::RemoteGui::Client;
+
+ // Setup the widgets.
+
+ tab.boxLayerName.setValue(properties.boxLayerName);
+
+ tab.numBoxes.setValue(properties.numBoxes);
+ tab.numBoxes.setRange(0, 100);
+
+ tab.drawBoxes.setLabel("Draw Boxes");
+
+ // Setup the layout.
+
+ GridLayout grid;
+ int row = 0;
+ {
+ grid.add(Label("Box Layer"), {row, 0}).add(tab.boxLayerName, {row, 1});
+ ++row;
+
+ grid.add(Label("Num Boxes"), {row, 0}).add(tab.numBoxes, {row, 1});
+ ++row;
+
+ grid.add(tab.drawBoxes, {row, 0}, {2, 1});
+ ++row;
+ }
+
+ VBoxLayout root = {grid, VSpacer()};
+ RemoteGui_createTab(getName(), root, &tab);
+ }
+
+
+ void
+ Component::RemoteGui_update()
+ {
+ if (tab.boxLayerName.hasValueChanged() || tab.numBoxes.hasValueChanged())
+ {
+ std::scoped_lock lock(propertiesMutex);
+ properties.boxLayerName = tab.boxLayerName.getValue();
+ properties.numBoxes = tab.numBoxes.getValue();
+
+ {
+ setDebugObserverDatafield("numBoxes", properties.numBoxes);
+ setDebugObserverDatafield("boxLayerName", properties.boxLayerName);
+ sendDebugObserverBatch();
+ }
+ }
+ if (tab.drawBoxes.wasClicked())
+ {
+ // Lock shared variables in methods running in seperate threads
+ // and pass them to functions. This way, the called functions do
+ // not need to think about locking.
+ std::scoped_lock lock(propertiesMutex, arvizMutex);
+ drawBoxes(properties, arviz);
+ }
+ }
+ */
+
+
+ // ARMARX_REGISTER_COMPONENT_EXECUTABLE(ik_demo, ik_demo::GetDefaultName());
+
+} // namespace armar6::skills::components::armar6_ik_demo
diff --git a/source/RobotAPI/components/ik_demo/ik_demo.h b/source/RobotAPI/components/ik_demo/ik_demo.h
new file mode 100644
index 0000000000000000000000000000000000000000..45eeb06a386e8dcf67fd2c11e337b24e858d68a1
--- /dev/null
+++ b/source/RobotAPI/components/ik_demo/ik_demo.h
@@ -0,0 +1,137 @@
+/**
+ * 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 skills::ArmarXObjects::armar6_ik_demo
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#pragma once
+
+#include <memory>
+
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/services/tasks/TaskUtil.h>
+
+// #include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
+
+// #include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
+
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+
+// #include <armar6/skills/components/armar6_ik_demo/ComponentInterface.h>
+
+
+namespace armar6::skills::components::armar6_ik_demo
+{
+
+ class IkDemo;
+
+
+ class ik_demo :
+ virtual public armarx::Component
+ // , virtual public armar6::skills::components::armar6_ik_demo::ComponentInterface
+ // , virtual public armarx::DebugObserverComponentPluginUser
+ // , virtual public armarx::LightweightRemoteGuiComponentPluginUser
+ , virtual public armarx::ArVizComponentPluginUser
+ {
+ public:
+
+ /// @see armarx::ManagedIceObject::getDefaultName()
+ std::string getDefaultName() const override;
+
+ /// Get the component's default name.
+ static std::string GetDefaultName();
+
+ ik_demo();
+
+
+ protected:
+
+ /// @see PropertyUser::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+
+ /// @see armarx::ManagedIceObject::onInitComponent()
+ void onInitComponent() override;
+
+ /// @see armarx::ManagedIceObject::onConnectComponent()
+ void onConnectComponent() override;
+
+ /// @see armarx::ManagedIceObject::onDisconnectComponent()
+ void onDisconnectComponent() override;
+
+ /// @see armarx::ManagedIceObject::onExitComponent()
+ void onExitComponent() override;
+
+
+ /* (Requires armarx::LightweightRemoteGuiComponentPluginUser.)
+ /// This function should be called once in onConnect() or when you
+ /// need to re-create the Remote GUI tab.
+ void createRemoteGuiTab();
+
+ /// After calling `RemoteGui_startRunningTask`, this function is
+ /// called periodically in a separate thread. If you update variables,
+ /// make sure to synchronize access to them.
+ void RemoteGui_update() override;
+ */
+
+
+ private:
+
+ void run();
+
+ // Private methods go here.
+
+ // Forward declare `Properties` if you used it before its defined.
+ // struct Properties;
+
+ /* (Requires the armarx::ArVizComponentPluginUser.)
+ /// Draw some boxes in ArViz.
+ void drawBoxes(const Properties& p, viz::Client& arviz);
+ */
+
+
+ private:
+
+ static const std::string defaultName;
+
+ std::unique_ptr<IkDemo> impl;
+ armarx::SimpleRunningTask<>::pointer_type task;
+
+
+ /// Properties shown in the Scenario GUI.
+ struct Properties
+ {
+ };
+ Properties properties;
+
+ /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
+ /// Tab shown in the Remote GUI.
+ struct RemoteGuiTab : armarx::RemoteGui::Client::Tab
+ {
+ armarx::RemoteGui::Client::LineEdit boxLayerName;
+ armarx::RemoteGui::Client::IntSpinBox numBoxes;
+
+ armarx::RemoteGui::Client::Button drawBoxes;
+ };
+ RemoteGuiTab tab;
+ */
+
+ };
+
+} // namespace armar6::skills::components::armar6_ik_demo
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/CMakeLists.txt b/source/RobotAPI/components/skills/SkillProviderExample/CMakeLists.txt
index ee1ea4e51bb0156822513f8481cc13338cc79b62..afbb7ef2abd07d6e718c7480ba7d84455574041d 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/CMakeLists.txt
+++ b/source/RobotAPI/components/skills/SkillProviderExample/CMakeLists.txt
@@ -6,7 +6,7 @@ set(COMPONENT_LIBS
ArmarXGuiComponentPlugins
RobotAPICore
RobotAPIInterfaces
- RobotAPI::skills
+ RobotAPISkills
aron
aronjsonconverter
)
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.cpp b/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.cpp
index 1ddbdb5a9efa1c2c98b4baefdb1e805a9af90fb8..ab6944e97cbcd54f16a6a6319928474e67ec7910 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.cpp
+++ b/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.cpp
@@ -19,7 +19,7 @@ namespace armarx::skills::provider
armarx::skills::Example::HelloWorldAcceptedType::ToAronType()
})
{}
- Skill::Status HelloWorldSkill::_execute(const aron::data::DictPtr& d, const CallbackT&)
+ Skill::Status HelloWorldSkill::execute(const aron::data::DictPtr& d, const CallbackT&)
{
ARMARX_IMPORTANT << "Hi, from the Hello World Skill.\n" <<
"I received the following data: \n" <<
@@ -28,6 +28,42 @@ namespace armarx::skills::provider
return Skill::Status::Succeeded;
}
+ ChainingSkill::ChainingSkill() :
+ Skill(SkillDescription{
+ "ChainingSkill",
+ "This skill calls the HelloWorld skill three times.",
+ {},
+ 3000,
+ nullptr
+ })
+ {}
+ Skill::Status ChainingSkill::execute(const aron::data::DictPtr& d, const CallbackT&)
+ {
+ armarx::skills::Example::HelloWorldAcceptedType exec1;
+ armarx::skills::Example::HelloWorldAcceptedType exec2;
+ armarx::skills::Example::HelloWorldAcceptedType exec3;
+
+ exec1.some_text = "Hello from the ChainingSkill 1";
+ exec2.some_text = "Hello from the ChainingSkill 2";
+ exec3.some_text = "Hello from the ChainingSkill 3";
+
+ manager::dto::SkillExecutionInfo exec;
+ exec.providerName = "SkillProviderExample";
+ exec.skillName = "HelloWorld";
+ exec.waitUntilSkillFinished = false;
+
+ exec.params = exec1.toAron()->toAronDictDTO();
+ ownerManager->executeSkill(exec);
+
+ exec.params = exec2.toAron()->toAronDictDTO();
+ ownerManager->executeSkill(exec);
+
+ exec.params = exec3.toAron()->toAronDictDTO();
+ ownerManager->executeSkill(exec);
+
+ return Skill::Status::Succeeded;
+ }
+
TimeoutSkill::TimeoutSkill() :
Skill(SkillDescription{
"Timeout",
@@ -37,7 +73,7 @@ namespace armarx::skills::provider
nullptr
})
{}
- Skill::Status TimeoutSkill::_execute(const aron::data::DictPtr& d, const CallbackT&)
+ Skill::Status TimeoutSkill::execute(const aron::data::DictPtr& d, const CallbackT&)
{
int i = 0;
while (!timeoutReached)
@@ -64,7 +100,7 @@ namespace armarx::skills::provider
nullptr
})
{}
- Skill::Status CallbackSkill::_execute(const aron::data::DictPtr& d, const CallbackT& callback)
+ Skill::Status CallbackSkill::execute(const aron::data::DictPtr& d, const CallbackT& callback)
{
ARMARX_IMPORTANT << "Logging three updates via the callback";
auto up1 = std::make_shared<aron::data::Dict>();
@@ -101,7 +137,7 @@ namespace armarx::skills::provider
void SkillProviderExample::onInitComponent()
{
// Add example skill
- addSkill(std::make_shared<HelloWorldSkill>());
+ addSkill(std::make_unique<HelloWorldSkill>());
// Add another lambda example skill
{
@@ -114,10 +150,13 @@ namespace armarx::skills::provider
}
// Add another example skill
- addSkill(std::make_shared<CallbackSkill>());
+ addSkill(std::make_unique<CallbackSkill>());
// Add timeout skill
- addSkill(std::make_shared<TimeoutSkill>());
+ addSkill(std::make_unique<TimeoutSkill>());
+
+ // chaining
+ addSkill(std::make_unique<ChainingSkill>());
}
void SkillProviderExample::onConnectComponent()
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.h b/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.h
index 2cd96b5ebea1cd9ab860521f1cc48b103ee21fec..57853d6420b99f99df2a15cd9523a88c48cc031f 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.h
+++ b/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.h
@@ -36,7 +36,14 @@ namespace armarx::skills::provider
{
public:
HelloWorldSkill();
- Status _execute(const aron::data::DictPtr&, const CallbackT&) final;
+ Status execute(const aron::data::DictPtr&, const CallbackT&) final;
+ };
+
+ class ChainingSkill : public Skill
+ {
+ public:
+ ChainingSkill();
+ Status execute(const aron::data::DictPtr&, const CallbackT&) final;
};
@@ -44,7 +51,7 @@ namespace armarx::skills::provider
{
public:
TimeoutSkill();
- Status _execute(const aron::data::DictPtr&, const CallbackT&) final;
+ Status execute(const aron::data::DictPtr&, const CallbackT&) final;
};
@@ -52,7 +59,7 @@ namespace armarx::skills::provider
{
public:
CallbackSkill();
- Status _execute(const aron::data::DictPtr&, const CallbackT&) final;
+ Status execute(const aron::data::DictPtr&, const CallbackT&) final;
};
/**
diff --git a/source/RobotAPI/components/units/PlatformUnitSimulation.cpp b/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
index 15e2f998f3a78716bbe0c6da08187a047f6b5ee9..3c9cde6b67a1106976beee9032795262755a4be3 100644
--- a/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
+++ b/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
@@ -176,7 +176,7 @@ namespace armarx
Eigen::Rotation2Df rot(currentRotation);
targetVel = rot * targetVel;
velPID->update(timeDeltaInSeconds, currentTranslationVelocity, targetVel);
- currentTranslationVelocity += timeDeltaInSeconds * velPID->getControlValue();
+ currentTranslationVelocity = timeDeltaInSeconds * velPID->getControlValue();
currentPositionX += timeDeltaInSeconds * currentTranslationVelocity[0];
currentPositionY += timeDeltaInSeconds * currentTranslationVelocity[1];
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
index 6ad411f67b743fecaf8ed8fa82a31f0d3e0d0b8c..4658f41d72431535920580dbb6bda4e0c70c0b6e 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
@@ -468,4 +468,9 @@ namespace armarx
}
}
}
+
+ int NJointCartesianWaypointController::getCurrentWaypointIndex(const Ice::Current&)
+ {
+ return _publishWpsCur;
+ }
}
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h
index 9ed0b775c33ca9bc3a32c239d6635fd9d021044e..4933edd9d22e5138a7ef06d09fa15aa4f908e339 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h
@@ -51,6 +51,7 @@ namespace armarx
public:
bool hasReachedTarget(const Ice::Current& = Ice::emptyCurrent) override;
bool hasReachedForceLimit(const Ice::Current& = Ice::emptyCurrent) override;
+ int getCurrentWaypointIndex(const Ice::Current& = Ice::emptyCurrent) override;
void setConfig(const NJointCartesianWaypointControllerRuntimeConfig& cfg, const Ice::Current& = Ice::emptyCurrent) override;
void setWaypoints(const std::vector<Eigen::Matrix4f>& wps, const Ice::Current& = Ice::emptyCurrent) override;
diff --git a/source/RobotAPI/gui-plugins/HapticUnitPlugin/MatrixDisplayWidget.h b/source/RobotAPI/gui-plugins/HapticUnitPlugin/MatrixDisplayWidget.h
index 1c09b0b5c18ce6af4917da48c1254d3358a1bc82..562c6f5665fbc88e8ca101a47da2c1fe0acf308b 100644
--- a/source/RobotAPI/gui-plugins/HapticUnitPlugin/MatrixDisplayWidget.h
+++ b/source/RobotAPI/gui-plugins/HapticUnitPlugin/MatrixDisplayWidget.h
@@ -25,7 +25,7 @@
#include <QWidget>
#include <QMutex>
-#include <eigen3/Eigen/Dense>
+#include <Eigen/Dense>
#include <valarray>
using Eigen::MatrixXf;
diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
index 5312cde46d0afb5a0ce77e07462c664703d187f7..144dacf08d6f3f2fdbb33aab1e73d1d67052b1b1 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
@@ -37,6 +37,7 @@
#include <VirtualRobot/XML/RobotIO.h>
#include <SimoxUtility/json.h>
+#include <SimoxUtility/algorithm/string.h>
// Qt headers
#include <Qt>
@@ -226,7 +227,7 @@ void KinematicUnitWidgetController::onConnectComponent()
}
// check robot name and disable setZero Button if necessary
- if (robot->getName() != "Armar3")
+ if (not simox::alg::starts_with(robot->getName(), "Armar3"))
{
ARMARX_IMPORTANT << "Disable the SetZero button because the robot name is " << robot->getName();
ui.pushButtonKinematicUnitPos1->setDisabled(true);
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/CMakeLists.txt b/source/RobotAPI/gui-plugins/SkillManagerPlugin/CMakeLists.txt
index 749a9e26330f60a2ffaee4ae251a65495887f771..1d427101fc58c6b36acf68979ff0a16c2f890bfc 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/CMakeLists.txt
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/CMakeLists.txt
@@ -37,7 +37,13 @@ set(GUI_UIS
)
# Add more libraries you depend on here, e.g. ${QT_LIBRARIES}.
-set(COMPONENT_LIBS RobotAPIInterfaces aron skills aronjsonconverter SimpleConfigDialog)
+set(COMPONENT_LIBS
+ RobotAPIInterfaces
+ aron
+ RobotAPISkills
+ aronjsonconverter
+ SimpleConfigDialog
+)
if(ArmarXGui_FOUND)
armarx_gui_plugin("${LIB_NAME}" "${SOURCES}" "" "${GUI_UIS}" "" "${COMPONENT_LIBS}")
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidget.ui b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidget.ui
index 15678facbdf134eb61542300528e6aa3290a0dc9..59089db2066050f0ffac50cd63c7af87dbcfea51 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidget.ui
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidget.ui
@@ -70,7 +70,7 @@
<item row="6" column="0">
<widget class="QPushButton" name="pushButtonStopSkill">
<property name="text">
- <string>Stop</string>
+ <string>Stop current skill</string>
</property>
</widget>
</item>
@@ -108,7 +108,7 @@
<item row="6" column="1">
<widget class="QPushButton" name="pushButtonExecuteSkill">
<property name="text">
- <string>Execute</string>
+ <string>Request Execution</string>
</property>
</widget>
</item>
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp
index 4a458d690633e611e64a4402238ec6793ae388d6..431ce92cade5f9d8fc92d8ac7bd80fd402e84c51 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp
@@ -148,6 +148,15 @@ namespace armarx
void SkillManagerMonitorWidgetController::refreshSkills()
{
+ static std::map<skills::provider::dto::Execution::Status, std::string> ExecutionStatus2String = {
+ {skills::provider::dto::Execution::Status::Aborted, "Aborted"},
+ {skills::provider::dto::Execution::Status::Failed, "Failed"},
+ {skills::provider::dto::Execution::Status::Idle, "Not yet started"},
+ {skills::provider::dto::Execution::Status::Running, "Running"},
+ {skills::provider::dto::Execution::Status::Scheduled, "Scheduled"},
+ {skills::provider::dto::Execution::Status::Succeeded, "Succeeded"}
+ };
+
if (!connected)
return;
@@ -178,6 +187,7 @@ namespace armarx
SkillProviderData providerData;
providerData.providerName = providerName;
providerData.skillDescriptions = provider->getSkills();
+ providerData.skillProviderPrx = provider;
skills.insert(std::make_pair(providerName, providerData));
newProviders.push_back(providerName);
@@ -185,30 +195,61 @@ namespace armarx
}
// remove providers from tree
- for (int i = 0; i < widget.treeWidgetSkills->topLevelItemCount(); ++i)
+ int i = 0;
+ while (i < widget.treeWidgetSkills->topLevelItemCount())
{
QTreeWidgetItem* item = widget.treeWidgetSkills->topLevelItem(i);
if (std::find(removedProviders.begin(), removedProviders.end(), item->text(0).toStdString()) != removedProviders.end())
{
delete widget.treeWidgetSkills->takeTopLevelItem(i);
}
+ else
+ {
+ ++i;
+ }
}
// add new providers
for (const auto& [providerName, provider] : skills)
{
- if (std::find(newProviders.begin(), newProviders.end(), providerName) != newProviders.end())
+ if (auto it = std::find(newProviders.begin(), newProviders.end(), providerName); it != newProviders.end())
{
- auto it = new QTreeWidgetItem(widget.treeWidgetSkills);
- it->setText(0, QString::fromStdString(providerName));
+ auto item = new QTreeWidgetItem(widget.treeWidgetSkills);
+ item->setText(0, QString::fromStdString(providerName));
for (const auto& [name, sk] : provider.skillDescriptions)
{
- auto itsk = new QTreeWidgetItem(it);
- it->addChild(itsk);
+ auto itsk = new QTreeWidgetItem(item);
+ item->addChild(itsk);
itsk->setText(0, QString::fromStdString(name));
}
}
}
+
+ // update status
+ for (int i = 0; i < widget.treeWidgetSkills->topLevelItemCount(); ++i)
+ {
+ try
+ {
+ QTreeWidgetItem* item = widget.treeWidgetSkills->topLevelItem(i);
+ auto providerName = item->text(0).toStdString();
+ for (int j = 0; j < item->childCount(); ++j)
+ {
+ QTreeWidgetItem* skillItem = item->child(j);
+ auto skillName = skillItem->text(0).toStdString();
+
+ auto& providerPrx = skills.at(providerName).skillProviderPrx;
+ auto statusUpdate = providerPrx->getSkillExecutionStatus(skillName);
+
+ skillItem->setText(2, QString::fromStdString(ExecutionStatus2String.at(statusUpdate.status)));
+ }
+ }
+ catch (const std::exception& e)
+ {
+ // Perhaps the skill provider died after the check at the beginning of this method
+ // Continue
+ continue;
+ }
+ }
}
void SkillManagerMonitorWidgetController::executeSkill()
@@ -227,11 +268,14 @@ namespace armarx
auto data = getConfigAsAron();
skills::manager::dto::SkillExecutionInfo exInfo;
+ exInfo.waitUntilSkillFinished = false;
exInfo.providerName = selectedSkill.providerName;
exInfo.skillName = selectedSkill.skillName;
exInfo.params = aron::data::Dict::ToAronDictDTO(data);
ARMARX_INFO << "Executing skill from GUI: " << selectedSkill.providerName << "/" << selectedSkill.skillName;
+ // Note that we execute the skill in a seperate thread so that the GUI thread does not freeze.
+ //executions.emplace_back([&](){ manager->executeSkill(exInfo); });
manager->executeSkill(exInfo);
}
@@ -248,9 +292,8 @@ namespace armarx
return;
}
- // TODO
ARMARX_INFO << "Stopping skill from GUI: " << selectedSkill.providerName << "/" << selectedSkill.skillName;
- //observer->abortSkill(skills.selectedSkill);
+ manager->abortSkill(selectedSkill.providerName, selectedSkill.skillName);
}
void SkillManagerMonitorWidgetController::skillSelectionChanged(QTreeWidgetItem* current, QTreeWidgetItem*)
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.h
index 9ba0d8a378411a688f650012ec96d55525a164c2..debeaedff268559d70438cfd3c453cee77f9d3cb 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.h
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.h
@@ -22,6 +22,8 @@
#pragma once
#include <stack>
+#include <vector>
+#include <thread>
#include <ArmarXCore/core/system/ImportExportComponent.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
@@ -95,7 +97,7 @@ namespace armarx
* Widget Form
*/
Ui::SkillManagerMonitorWidget widget;
- QPointer<SimpleConfigDialog> dialog;
+ QPointer<SimpleConfigDialog> dialog;
std::string observerName = "SkillManager";
skills::manager::dti::SkillManagerInterfacePrx manager = nullptr;
@@ -110,6 +112,7 @@ namespace armarx
{
std::string providerName;
skills::provider::dto::SkillDescriptionMap skillDescriptions;
+ skills::provider::dti::SkillProviderInterfacePrx skillProviderPrx;
};
// Data taken from observer
@@ -125,6 +128,9 @@ namespace armarx
// others
std::atomic_bool connected = false;
QTimer* refreshSkillsResultTimer;
+
+ // skillExecutions
+ std::vector<std::thread> executions;
};
}
diff --git a/source/RobotAPI/interface/skills/SkillManagerInterface.ice b/source/RobotAPI/interface/skills/SkillManagerInterface.ice
index ea2ffed638075444a61c17b7952da90ae09708e3..9f6bd73148b7de3a7f043359775155d33a3227ff 100644
--- a/source/RobotAPI/interface/skills/SkillManagerInterface.ice
+++ b/source/RobotAPI/interface/skills/SkillManagerInterface.ice
@@ -39,6 +39,7 @@ module armarx
string providerName;
string skillName;
aron::data::dto::Dict params;
+ bool waitUntilSkillFinished;
};
struct ProviderInfo
@@ -57,6 +58,7 @@ module armarx
void removeProvider(string providerName);
provider::dti::SkillProviderMap getSkillProviders();
void executeSkill(dto::SkillExecutionInfo skillExecutionInfo);
+ void abortSkill(string providerName, string skillName);
};
}
}
diff --git a/source/RobotAPI/interface/skills/SkillProviderInterface.ice b/source/RobotAPI/interface/skills/SkillProviderInterface.ice
index ff147cc4a9dcc7447af016d9c0f80251ce02db62..c6eec573654ae0b8a4a6b7811fe273eb189033ad 100644
--- a/source/RobotAPI/interface/skills/SkillProviderInterface.ice
+++ b/source/RobotAPI/interface/skills/SkillProviderInterface.ice
@@ -68,6 +68,7 @@ module armarx
string skillName;
aron::data::dto::Dict params;
callback::dti::SkillProviderCallbackInterface* callbackInterface; // use nullptr if you do not want to have callbacks
+ bool waitUntilSkillFinished;
};
// The status enum of a skill
@@ -108,7 +109,7 @@ module armarx
dto::SkillDescriptionMap getSkills();
dto::SkillStatusUpdate getSkillExecutionStatus(string name);
void executeSkill(dto::SkillExecutionInfo executionInfo);
- dto::SkillStatusUpdate abortSkill(string skill);
+ void abortSkill(string skill, bool waitUntilSkillFinished);
};
dictionary<string, SkillProviderInterface*> SkillProviderMap;
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.ice
index 712399c5f77ec1a7c116b3e77b0ad2e4df616a87..bf00909d6f5b95ee23fec2f982aae5d4d6ed6ec8 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.ice
@@ -51,6 +51,7 @@ module armarx
{
idempotent bool hasReachedTarget();
idempotent bool hasReachedForceLimit();
+ idempotent int getCurrentWaypointIndex();
void setConfig(NJointCartesianWaypointControllerRuntimeConfig cfg);
void setWaypoints(Eigen::Matrix4fSeq wps);
diff --git a/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt b/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
index 20f30486fd3249168f6f80a9353d6dd78cc7b2ea..2b2fb5f9d4ea899930351b1234873b2354cb50bf 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
+++ b/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
@@ -30,6 +30,8 @@ set(LIB_FILES
plugins/ObjectPoseProviderPlugin.cpp
plugins/ObjectPoseClientPlugin.cpp
plugins/RequestedObjects.cpp
+
+ util.cpp
)
set(LIB_HEADERS
ArmarXObjects.h
@@ -54,6 +56,8 @@ set(LIB_HEADERS
plugins/ObjectPoseProviderPlugin.h
plugins/ObjectPoseClientPlugin.h
plugins/RequestedObjects.h
+
+ util.h
)
armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.cpp
index 8710af33c07522713bd833e53c7f1e982754e3ec..13de0bb80cccb9d9e7a63819907d3b7d81688fbd 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.cpp
@@ -1,4 +1,6 @@
#include "ObjectPoseClient.h"
+#include <optional>
+#include "RobotAPI/libraries/ArmarXObjects/ObjectPose.h"
namespace armarx::objpose
@@ -45,7 +47,8 @@ namespace armarx::objpose
}
- ObjectPoseMap ObjectPoseClient::fetchObjectPosesAsMap()
+ ObjectPoseMap
+ ObjectPoseClient::fetchObjectPosesAsMap()
{
ObjectPoseMap map;
for (auto& pose : fetchObjectPoses())
@@ -54,8 +57,24 @@ namespace armarx::objpose
}
return map;
}
+
- ObjectPoseSeq ObjectPoseClient::fetchObjectPosesFromProvider(const std::string& providerName)
+ std::optional<ObjectPose>
+ ObjectPoseClient::fetchObjectPose(const ObjectID& objectID)
+ {
+ const auto *object = findObjectPoseByID(fetchObjectPoses(), objectID);
+
+ if(object != nullptr)
+ {
+ return *object;
+ }
+
+ return std::nullopt;
+ }
+
+
+ ObjectPoseSeq
+ ObjectPoseClient::fetchObjectPosesFromProvider(const std::string& providerName)
{
if (!objectPoseStorage)
{
@@ -66,7 +85,6 @@ namespace armarx::objpose
}
-
const ObjectPoseStorageInterfacePrx&
ObjectPoseClient::getObjectPoseStorage() const
{
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h
index 33f9a6dd7b40fd933a451d50d093afc937a08635..a5b5fd18d7686e0cc46cd7e0154588ca3b13b40f 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h
@@ -1,6 +1,9 @@
#pragma once
+#include <optional>
+
#include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
@@ -16,28 +19,83 @@ namespace armarx::objpose
{
public:
+ /// Construct a disconnected client.
ObjectPoseClient();
- ObjectPoseClient(const ObjectPoseStorageInterfacePrx& objectPoseStorage,
- const ObjectFinder& finder = {});
-
- void connect(const ObjectPoseStorageInterfacePrx& objectPoseStorage);
-
- bool isConnected() const;
-
-
+ /// Construct a client and connect it to the object pose storage.
+ ObjectPoseClient(
+ const ObjectPoseStorageInterfacePrx& objectPoseStorage,
+ const ObjectFinder& finder = {}
+ );
+
+ /**
+ * @brief Connect to the given object pose storage.
+ *
+ * This function can be used after default-constructing the client.
+ *
+ * @param objectPoseStorage The object pose storage.
+ */
+ void
+ connect(const ObjectPoseStorageInterfacePrx& objectPoseStorage);
+
+ /**
+ * @brief Indicate whether this client is connected to an object pose
+ * storage.
+ *
+ * That is, whether its proxy has been set via the constructor or
+ * `connect()`.
+ *
+ * If false, all `fetch*()` functions will return empty results.
+ *
+ * @return True if connected
+ */
+ bool
+ isConnected() const;
+
+
+ /**
+ * @brief Fetch all known object poses.
+ * @return The known object poses.
+ */
ObjectPoseSeq
fetchObjectPoses();
+ /**
+ * @brief Fetch all known object poses.
+ * @return The known object poses, with object ID as key.
+ */
ObjectPoseMap
fetchObjectPosesAsMap();
+ /**
+ * @brief Fetch the pose of a single object.
+ *
+ * This is a network call. If you need multiple object poses, use
+ * `fetchObjectPoses()` instead.
+ *
+ * @param objectID The object's ID.
+ * @return The object's pose, if known.
+ */
+ std::optional<ObjectPose>
+ fetchObjectPose(const ObjectID& objectID);
+
+ /**
+ * @brief Fetch object poses from a specific provider.
+ * @param providerName The provider's name.
+ * @return The object poses from that provider.
+ */
ObjectPoseSeq
fetchObjectPosesFromProvider(const std::string& providerName);
+ /**
+ * @brief Get the object pose storage's proxy.
+ */
const ObjectPoseStorageInterfacePrx&
getObjectPoseStorage() const;
+ /**
+ * @brief Get the internal object finder.
+ */
const ObjectFinder&
getObjectFinder() const;
diff --git a/source/RobotAPI/libraries/ArmarXObjects/Scene.h b/source/RobotAPI/libraries/ArmarXObjects/Scene.h
index a4cc41d80b5fccc87b1e9d871808c685ce332401..3c2767c017ce2ac19faef9bde78b7ede5bba8b13 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/Scene.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/Scene.h
@@ -24,6 +24,7 @@
#include <map>
#include <string>
#include <vector>
+#include <optional>
#include <Eigen/Core>
#include <Eigen/Geometry>
@@ -43,6 +44,7 @@ namespace armarx::objects
Eigen::Vector3f position = Eigen::Vector3f::Zero();
Eigen::Quaternionf orientation = Eigen::Quaternionf::Identity();
+ std::optional<bool> isStatic;
std::map<std::string, float> jointValues;
ObjectID getClassID() const;
diff --git a/source/RobotAPI/libraries/ArmarXObjects/json_conversions.cpp b/source/RobotAPI/libraries/ArmarXObjects/json_conversions.cpp
index 617baca941ebb86be7e3b05ecac1e79efc23490c..6080cfda786c3d6115c6647fe7decd4e5faa6b03 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/json_conversions.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/json_conversions.cpp
@@ -9,7 +9,7 @@
#include "ice_conversions.h"
-void armarx::to_json(nlohmann::json& j, const ObjectID& id)
+void armarx::to_json(simox::json::json& j, const ObjectID& id)
{
j["dataset"] = id.dataset();
j["className"] = id.className();
@@ -17,7 +17,7 @@ void armarx::to_json(nlohmann::json& j, const ObjectID& id)
j["str"] = id.str();
}
-void armarx::from_json(const nlohmann::json& j, ObjectID& id)
+void armarx::from_json(const simox::json::json& j, ObjectID& id)
{
id =
{
@@ -27,7 +27,7 @@ void armarx::from_json(const nlohmann::json& j, ObjectID& id)
};
}
-void armarx::objpose::to_json(nlohmann::json& j, const ObjectPose& op)
+void armarx::objpose::to_json(simox::json::json& j, const ObjectPose& op)
{
j["providerName"] = op.providerName;
j["objectType"] = ObjectTypeNames.to_name(op.objectType);
@@ -52,7 +52,7 @@ void armarx::objpose::to_json(nlohmann::json& j, const ObjectPose& op)
}
-void armarx::objpose::from_json(const nlohmann::json& j, ObjectPose& op)
+void armarx::objpose::from_json(const simox::json::json& j, ObjectPose& op)
{
op.providerName = j.at("providerName");
op.objectType = ObjectTypeNames.from_name(j.at("objectType"));
@@ -77,44 +77,64 @@ void armarx::objpose::from_json(const nlohmann::json& j, ObjectPose& op)
}
-void armarx::objects::to_json(nlohmann::json& j, const SceneObject& rhs)
+void armarx::objects::to_json(simox::json::json& j, const SceneObject& rhs)
{
- // j["instanceID"] = rhs.instanceID;
j["class"] = rhs.className;
j["instanceName"] = rhs.instanceName;
j["collection"] = rhs.collection;
j["position"] = rhs.position;
j["orientation"] = rhs.orientation;
+ if (rhs.isStatic.has_value())
+ {
+ j["isStatic"] = rhs.isStatic.value();
+ }
j["jointValues"] = rhs.jointValues;
}
-void armarx::objects::from_json(const nlohmann::json& j, SceneObject& rhs)
+void armarx::objects::from_json(const simox::json::json& j, SceneObject& rhs)
{
- // j.at("instanceID").get_to(rhs.instanceID);
j.at("class").get_to(rhs.className);
if (j.count("instanceName"))
{
j["instanceName"].get_to(rhs.instanceName);
}
+ else
+ {
+ rhs.instanceName.clear();
+ }
j.at("collection").get_to(rhs.collection);
j.at("position").get_to(rhs.position);
j.at("orientation").get_to(rhs.orientation);
+
+ if (j.count("isStatic"))
+ {
+ rhs.isStatic = j.at("isStatic").get<bool>();
+ }
+ else
+ {
+ rhs.isStatic = std::nullopt;
+ }
+
if (j.count("jointValues"))
{
j.at("jointValues").get_to(rhs.jointValues);
}
+ else
+ {
+ rhs.jointValues.clear();
+ }
}
-void armarx::objects::to_json(nlohmann::json& j, const Scene& rhs)
+void armarx::objects::to_json(simox::json::json& j, const Scene& rhs)
{
j["objects"] = rhs.objects;
}
-void armarx::objects::from_json(const nlohmann::json& j, Scene& rhs)
+void armarx::objects::from_json(const simox::json::json& j, Scene& rhs)
{
j.at("objects").get_to(rhs.objects);
}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/json_conversions.h b/source/RobotAPI/libraries/ArmarXObjects/json_conversions.h
index 608ad14424234ef4120f117d511310a42be71bcc..0a5e83b61aa9479d3d67344f585564c2c60d2d75 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/json_conversions.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/json_conversions.h
@@ -1,28 +1,28 @@
#pragma once
-#include <SimoxUtility/json/json.hpp>
+#include <SimoxUtility/json/json.h>
#include "forward_declarations.h"
namespace armarx
{
- void to_json(nlohmann::json& j, const ObjectID& value);
- void from_json(const nlohmann::json& j, ObjectID& value);
+ void to_json(simox::json::json& j, const ObjectID& value);
+ void from_json(const simox::json::json& j, ObjectID& value);
}
namespace armarx::objpose
{
- void to_json(nlohmann::json& j, const ObjectPose& op);
- void from_json(const nlohmann::json& j, ObjectPose& op);
+ void to_json(simox::json::json& j, const ObjectPose& op);
+ void from_json(const simox::json::json& j, ObjectPose& op);
}
namespace armarx::objects
{
- void to_json(nlohmann::json& j, const SceneObject& rhs);
- void from_json(const nlohmann::json& j, SceneObject& rhs);
+ void to_json(simox::json::json& j, const SceneObject& rhs);
+ void from_json(const simox::json::json& j, SceneObject& rhs);
- void to_json(nlohmann::json& j, const Scene& rhs);
- void from_json(const nlohmann::json& j, Scene& rhs);
+ void to_json(simox::json::json& j, const Scene& rhs);
+ void from_json(const simox::json::json& j, Scene& rhs);
}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/util.cpp b/source/RobotAPI/libraries/ArmarXObjects/util.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3dd90c141d81b77fcce1e0b91f8a5963d84a2726
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/util.cpp
@@ -0,0 +1,88 @@
+/**
+ * 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 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "util.h"
+
+#include <string>
+
+#include <Eigen/Geometry>
+
+#include <VirtualRobot/ManipulationObject.h>
+#include <VirtualRobot/SceneObjectSet.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+
+namespace armarx::objpose
+{
+
+ objpose::ObjectPoseSeq
+ filterObjects(objpose::ObjectPoseSeq objects, const std::vector<std::string>& datasetBlocklist)
+ {
+ const auto isBlacklisted = [&datasetBlocklist](const objpose::ObjectPose& objectPose)
+ {
+ const auto dataset = objectPose.objectID.dataset();
+
+ return std::find(datasetBlocklist.begin(), datasetBlocklist.end(), dataset) !=
+ datasetBlocklist.end();
+ };
+
+ objects.erase(std::remove_if(objects.begin(), objects.end(), isBlacklisted), objects.end());
+ return objects;
+ }
+
+
+ VirtualRobot::ManipulationObjectPtr
+ asManipulationObject(const objpose::ObjectPose& objectPose)
+ {
+ ObjectFinder finder;
+
+ VirtualRobot::SceneObjectSetPtr sceneObjects(new VirtualRobot::SceneObjectSet);
+ if (auto obstacle = finder.loadManipulationObject(objectPose))
+ {
+ obstacle->setGlobalPose(objectPose.objectPoseGlobal);
+ return obstacle;
+ }
+
+ ARMARX_WARNING << "Failed to load scene object `" << objectPose.objectID << "`";
+ return nullptr;
+ }
+
+
+ VirtualRobot::SceneObjectSetPtr
+ asSceneObjects(const objpose::ObjectPoseSeq& objectPoses)
+ {
+ ObjectFinder finder;
+
+ VirtualRobot::SceneObjectSetPtr sceneObjects(new VirtualRobot::SceneObjectSet);
+ for (const auto& objectPose : objectPoses)
+ {
+ if (auto obstacle = finder.loadManipulationObject(objectPose))
+ {
+ obstacle->setGlobalPose(objectPose.objectPoseGlobal);
+ sceneObjects->addSceneObject(obstacle);
+ }
+ }
+
+ return sceneObjects;
+ }
+
+
+} // namespace armarx::objpose
diff --git a/source/RobotAPI/libraries/ArmarXObjects/util.h b/source/RobotAPI/libraries/ArmarXObjects/util.h
new file mode 100644
index 0000000000000000000000000000000000000000..bd8ebf0c8397ae1a26b899a8d48c1f9a303377f9
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/util.h
@@ -0,0 +1,36 @@
+/**
+ * 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 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
+
+namespace armarx::objpose
+{
+ objpose::ObjectPoseSeq filterObjects(objpose::ObjectPoseSeq objects,
+ const std::vector<std::string>& datasetBlocklist);
+
+ VirtualRobot::ManipulationObjectPtr asManipulationObject(const objpose::ObjectPose& objectPose);
+ VirtualRobot::SceneObjectSetPtr asSceneObjects(const objpose::ObjectPoseSeq& objectPoses);
+
+} // namespace armarx::objpose
diff --git a/source/RobotAPI/libraries/CMakeLists.txt b/source/RobotAPI/libraries/CMakeLists.txt
index 86bdc8fa48e612b256365f1beb1c43bfd1d09ec2..099ad8b4b981aeed1cc9e274c25672a1014ee120 100644
--- a/source/RobotAPI/libraries/CMakeLists.txt
+++ b/source/RobotAPI/libraries/CMakeLists.txt
@@ -23,6 +23,7 @@ add_subdirectory(armem_gui)
add_subdirectory(armem_motions)
add_subdirectory(armem_mps)
add_subdirectory(armem_objects)
+add_subdirectory(armem_reasoning)
add_subdirectory(armem_robot)
add_subdirectory(armem_robot_state)
add_subdirectory(armem_skills)
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.cpp
index d0c7371051effc8f00a007687723ece0e2fa5cb9..fc83132850cfb652359c0ef7cca81d89e3ff3aac 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.cpp
@@ -7,92 +7,79 @@
namespace armarx::plugins
{
- void
- HeartbeatComponentPlugin::configureHeartbeatChannel(const std::string& channel,
- const RobotHealthHeartbeatArgs& args)
+ void HeartbeatComponentPlugin::configureHeartbeatChannel(const std::string& channel,
+ const RobotHealthHeartbeatArgs& args)
{
channelHeartbeatConfig.emplace(channel, args);
}
- void
- HeartbeatComponentPlugin::heartbeat()
+ void HeartbeatComponentPlugin::heartbeat()
{
if (robotHealthTopic)
{
robotHealthTopic->heartbeat(componentName, heartbeatArgs);
- }
- else
+ } else
{
ARMARX_WARNING << "No robot health topic available!";
}
}
- void
- HeartbeatComponentPlugin::heartbeat(const std::string& channel)
+ void HeartbeatComponentPlugin::heartbeat(const std::string& channel)
{
const auto argsIt = channelHeartbeatConfig.find(channel);
- ARMARX_CHECK(argsIt != channelHeartbeatConfig.end())
- << "heartbeat() called for unknown channel '" << channel << "'."
- << "You must register the config using configureHeartbeatChannel(channel) first!";
+ ARMARX_CHECK(argsIt != channelHeartbeatConfig.end()) << "heartbeat() called for unknown channel '" << channel
+ << "'."
+ << "You must register the config using configureHeartbeatChannel(channel) first!";
const auto& args = argsIt->second;
if (robotHealthTopic)
{
robotHealthTopic->heartbeat(componentName + "_" + channel, args);
- }
- else
+ } else
{
ARMARX_WARNING << "No robot health topic available!";
}
}
- void
- HeartbeatComponentPlugin::preOnInitComponent()
+ void HeartbeatComponentPlugin::preOnInitComponent()
{
- if (topicName.empty())
- {
- parent<Component>().getProperty(topicName, makePropertyName(topicPropertyName));
- }
- parent<Component>().offeringTopic(topicName);
+ // if (topicName.empty())
+ // {
+ // parent<Component>().getProperty(topicName, makePropertyName(topicPropertyName));
+ // }
+ // parent<Component>().offeringTopic(topicName);
}
- void
- HeartbeatComponentPlugin::postOnInitComponent()
+ void HeartbeatComponentPlugin::postOnInitComponent()
{
}
- void
- HeartbeatComponentPlugin::preOnConnectComponent()
+ void HeartbeatComponentPlugin::preOnConnectComponent()
{
- robotHealthTopic = parent<Component>().getTopic<RobotHealthInterfacePrx>(topicName);
+ // robotHealthTopic = parent<Component>().getTopic<RobotHealthInterfacePrx>(topicName);
componentName = parent<Component>().getName();
}
- void
- HeartbeatComponentPlugin::postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties)
+ void HeartbeatComponentPlugin::postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties)
{
if (!properties->hasDefinition(makePropertyName(topicPropertyName)))
{
- properties->defineOptionalProperty<std::string>(
- makePropertyName(topicPropertyName),
- "DebugObserver",
- "Name of the topic the DebugObserver listens on");
+ properties->topic(robotHealthTopic, topicName, topicPropertyName,
+ "Name of the topic the DebugObserver listens on");
}
if (not properties->hasDefinition(makePropertyName(maximumCycleTimeWarningMSPropertyName)))
{
- properties->defineRequiredProperty<std::string>(
- makePropertyName(maximumCycleTimeWarningMSPropertyName),
- "TODO: maximumCycleTimeWarningMS");
+ properties->required(heartbeatArgs.maximumCycleTimeWarningMS, maximumCycleTimeWarningMSPropertyName,
+ "maximum cycle time before warning is emitted");
}
if (not properties->hasDefinition(makePropertyName(maximumCycleTimeErrorMSPropertyName)))
{
- properties->defineRequiredProperty<std::string>(
- makePropertyName(maximumCycleTimeErrorMSPropertyName),
- "TODO: maximumCycleTimeErrorMS");
+ properties->required(heartbeatArgs.maximumCycleTimeErrorMS, maximumCycleTimeErrorMSPropertyName,
+ "maximum cycle time before error is emitted");
}
}
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h
index 6a4a6c2bd692484c94bbd42bdfff399b0f5a0aa9..80cf546726085b6ca03406a7dc1abc91e647c11c 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h
@@ -66,7 +66,7 @@ namespace armarx::plugins
private:
//! heartbeat topic name (outgoing)
- std::string topicName;
+ std::string topicName{"DebugObserver"};
//! name of this component used as identifier for heartbeats
std::string componentName;
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
index 57a714eacc92f1243dcd31d30ea08b1d48648dc0..f5a88c0ae45c0960938f1ef819002a563bc198c1 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
@@ -226,6 +226,12 @@ namespace armarx
return select("ForceTorqueSensor");
}
+ std::string RobotNameHelper::Arm::getForceTorqueSensorFrame() const
+ {
+ ARMARX_TRACE;
+ return select("ForceTorqueSensorFrame");
+ }
+
std::string RobotNameHelper::Arm::getEndEffector() const
{
ARMARX_TRACE;
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
index 1c40eac0d8c07c476129e0e852f4e29f57d20144..92330d05d3e18b1447e68a9d36bb4c8b7472ad64 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
@@ -73,6 +73,7 @@ namespace armarx
std::string getTorsoKinematicChain() const;
std::string getTCP() const;
std::string getForceTorqueSensor() const;
+ std::string getForceTorqueSensorFrame() const;
std::string getEndEffector() const;
std::string getMemoryHandName() const;
std::string getHandControllerName() const;
diff --git a/source/RobotAPI/libraries/armem/server/CMakeLists.txt b/source/RobotAPI/libraries/armem/server/CMakeLists.txt
index 45b4dbc7222bf4483597314b7cc7c587c134a84b..34b02a4da78015ccdb32398a3f1de7f42c0311e3 100644
--- a/source/RobotAPI/libraries/armem/server/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem/server/CMakeLists.txt
@@ -7,11 +7,22 @@ SET(INSTALL_SCRIPT_MSG
"Please use the installation script in RobotAPI/etc/mongocxx to install libmongocxx and libbsoncxx."
)
-find_package(libmongocxx QUIET)
-armarx_build_if(libmongocxx_FOUND "libmongocxx not available. ${INSTALL_SCRIPT_MSG}")
+
+
+# MongoLTM
+#find_package(libmongocxx QUIET)
+#armarx_build_if(libmongocxx_FOUND "libmongocxx not available. ${INSTALL_SCRIPT_MSG}")
+
+# DiskLTM TODO: switch to FindMinizip file?
+INCLUDE (FindPkgConfig)
+if (PKG_CONFIG_FOUND)
+ PKG_CHECK_MODULES(UNZIP minizip)
+endif (PKG_CONFIG_FOUND)
+armarx_build_if(UNZIP_FOUND "MiniZip not available.")
+
+# LTM Encoding stuff
find_package(libbsoncxx QUIET)
armarx_build_if(libbsoncxx_FOUND "libbsoncxx not available. ${INSTALL_SCRIPT_MSG}")
-
armarx_build_if(OpenCV_FOUND "OpenCV not available")
set(LIBS
@@ -21,11 +32,12 @@ set(LIBS
aron
RobotAPI::armem
- # Needed for LTM
+ # LTM
RobotAPI::aron::converter::json
RobotAPI::aron::converter::opencv
- ${LIBMONGOCXX_LIBRARIES}
+ #${LIBMONGOCXX_LIBRARIES}
${LIBBSONCXX_LIBRARIES}
+ ${UNZIP_LIBRARIES}
)
set(LIB_FILES
@@ -33,6 +45,7 @@ set(LIB_FILES
MemoryRemoteGui.cpp
RemoteGuiAronDataVisitor.cpp
+ ltm/base/detail/Processors.cpp
ltm/base/detail/MemoryItem.cpp
ltm/base/detail/MemoryBase.cpp
ltm/base/detail/BufferedMemoryBase.cpp
@@ -44,21 +57,28 @@ set(LIB_FILES
ltm/base/filter/Filter.cpp
ltm/base/filter/frequencyFilter/FrequencyFilter.cpp
+ ltm/base/filter/equalityFilter/EqualityFilter.cpp
ltm/base/extractor/Extractor.cpp
ltm/base/extractor/imageExtractor/ImageExtractor.cpp
- ltm/base/extractor/noExtractor/NoExtractor.cpp
-
- ltm/base/converter/dict/Converter.cpp
- ltm/base/converter/dict/json/JsonConverter.cpp
- ltm/base/converter/dict/bson/BsonConverter.cpp
+ ltm/base/extractor/imageExtractor/DepthImageExtractor.cpp
+
+ ltm/base/converter/Converter.cpp
+ ltm/base/typeConverter/Converter.cpp
+ ltm/base/typeConverter/json/JsonConverter.cpp
+ ltm/base/converter/object/Converter.cpp
+ ltm/base/converter/object/json/JsonConverter.cpp
+ ltm/base/converter/object/bson/BsonConverter.cpp
ltm/base/converter/image/Converter.cpp
ltm/base/converter/image/png/PngConverter.cpp
+ ltm/base/converter/image/exr/ExrConverter.cpp
ltm/base/forgetter/Forgetter.cpp
ltm/base/forgetter/LRUForgetter/LRUForgetter.cpp
- ltm/disk/detail/Data.cpp
+ ltm/disk/detail/util/util.cpp
+ ltm/disk/detail/util/filesystem_util.cpp
+ ltm/disk/detail/util/minizip_util.cpp
ltm/disk/detail/DiskStorage.cpp
ltm/disk/Memory.cpp
ltm/disk/CoreSegment.cpp
@@ -105,6 +125,7 @@ set(LIB_HEADERS
MemoryRemoteGui.h
RemoteGuiAronDataVisitor.h
+ ltm/base/detail/Processors.h
ltm/base/detail/MemoryItem.h
ltm/base/detail/MemoryBase.h
ltm/base/detail/BufferedMemoryBase.h
@@ -116,22 +137,29 @@ set(LIB_HEADERS
ltm/base/filter/Filter.h
ltm/base/filter/frequencyFilter/FrequencyFilter.h
+ ltm/base/filter/equalityFilter/EqualityFilter.h
ltm/base/extractor/Extractor.h
ltm/base/extractor/imageExtractor/ImageExtractor.h
- ltm/base/extractor/noExtractor/NoExtractor.h
-
- ltm/base/converter/dict/Converter.h
- ltm/base/converter/dict/json/JsonConverter.h
- ltm/base/converter/dict/bson/BsonConverter.h
+ ltm/base/extractor/imageExtractor/DepthImageExtractor.h
+
+ ltm/base/converter/Converter.h
+ ltm/base/typeConverter/Converter.h
+ ltm/base/typeConverter/json/JsonConverter.h
+ ltm/base/converter/object/Converter.h
+ ltm/base/converter/object/json/JsonConverter.h
+ ltm/base/converter/object/bson/BsonConverter.h
ltm/base/converter/image/Converter.h
ltm/base/converter/image/png/PngConverter.h
+ ltm/base/converter/image/exr/ExrConverter.h
ltm/base/forgetter/Forgetter.h
ltm/base/forgetter/LRUForgetter/LRUForgetter.h
- ltm/disk/detail/Data.h
+ ltm/disk/detail/util/util.h
+ ltm/disk/detail/util/filesystem_util.h
+ ltm/disk/detail/util/minizip_util.h
ltm/disk/detail/DiskStorage.h
ltm/disk/Memory.h
ltm/disk/CoreSegment.h
diff --git a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
index d6e93314555a33c689bf7ec8a38d1db01fad094e..d5318d3f7e457065493ba38cfcdc671a29e2b9fb 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
+++ b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
@@ -221,6 +221,7 @@ namespace armarx::armem::server
e->addSnapshot(snapshot);
}
+ // store memory
longtermMemory->store(m);
}
@@ -280,7 +281,7 @@ namespace armarx::armem::server
{
ARMARX_INFO << "The LTM returned data after query";
- longtermMemory->load(ltmResult); // convert memory ==> meaning resolving references
+ longtermMemory->resolve(ltmResult); // convert memory ==> meaning resolving references
wmResult.append(ltmResult);
if (wmResult.empty())
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/Converter.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/converter/Converter.cpp
similarity index 100%
rename from source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/Converter.cpp
rename to source/RobotAPI/libraries/armem/server/ltm/base/converter/Converter.cpp
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/Converter.h b/source/RobotAPI/libraries/armem/server/ltm/base/converter/Converter.h
new file mode 100644
index 0000000000000000000000000000000000000000..c42e9c0d57d64f4776f7c1f586ea3eba3a4af75a
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/converter/Converter.h
@@ -0,0 +1,38 @@
+#pragma once
+
+// STD/STL
+#include <memory>
+
+// ArmarX
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+
+namespace armarx::armem::server::ltm
+{
+ class Converter
+ {
+ public:
+ enum class ConverterType
+ {
+ Str,
+ Binary
+ };
+
+ Converter(const ConverterType t, const std::string& id, const std::string& s, const aron::type::Descriptor c):
+ type(t),
+ identifier(id),
+ suffix(s),
+ convertsType(c)
+ {}
+ virtual ~Converter() = default;
+
+ virtual std::pair<std::vector<unsigned char>, std::string> convert(const aron::data::VariantPtr& data) = 0;
+ virtual aron::data::VariantPtr convert(const std::vector<unsigned char>& data, const std::string&) = 0;
+
+ public:
+ const ConverterType type;
+ const std::string identifier;
+ const std::string suffix;
+ const aron::type::Descriptor convertsType;
+ bool enabled = false;
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/Converter.h b/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/Converter.h
deleted file mode 100644
index 776b7c05ca15ba98c7350977c1fb4762e8e0a949..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/Converter.h
+++ /dev/null
@@ -1,35 +0,0 @@
-#pragma once
-
-// STD/STL
-#include <memory>
-
-// ArmarX
-#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
-
-namespace armarx::armem::server::ltm
-{
- class DictConverter;
- using DictConverterPtr = std::shared_ptr<DictConverter>;
-
- class DictConverter
- {
- public:
- enum class ConverterType
- {
- Str,
- Binary
- };
-
- DictConverter(const ConverterType t, const std::string& s):
- type(t),
- suffix(s)
- {}
- virtual ~DictConverter() = default;
-
- virtual std::vector<unsigned char> convert(const aron::data::DictPtr& data) = 0;
- virtual aron::data::DictPtr convert(const std::vector<unsigned char>& data) = 0;
-
- const ConverterType type;
- const std::string suffix;
- };
-}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/bson/BsonConverter.h b/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/bson/BsonConverter.h
deleted file mode 100644
index 0d6f858b0605edbe0496b279928b6cea6370e21c..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/bson/BsonConverter.h
+++ /dev/null
@@ -1,27 +0,0 @@
-#pragma once
-
-// Base Class
-#include "../Converter.h"
-
-// ArmarX
-#include "../json/JsonConverter.h"
-
-namespace armarx::armem::server::ltm::converter::dict
-{
- class BsonConverter;
- using BsonConverterPtr = std::shared_ptr<BsonConverter>;
-
- class BsonConverter : public DictConverter
- {
- public:
- BsonConverter() :
- DictConverter(ConverterType::Binary, ".bson")
- {}
-
- virtual std::vector<unsigned char> convert(const aron::data::DictPtr& data) override;
- virtual aron::data::DictPtr convert(const std::vector<unsigned char>& data) override;
-
- private:
- JsonConverter jsonConverter;
- };
-}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/json/JsonConverter.h b/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/json/JsonConverter.h
deleted file mode 100644
index fad44583f452740684724e45e47f74138c63697c..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/json/JsonConverter.h
+++ /dev/null
@@ -1,24 +0,0 @@
-#pragma once
-
-// Base Class
-#include "../Converter.h"
-
-// Simox
-#include <SimoxUtility/json.h>
-
-namespace armarx::armem::server::ltm::converter::dict
-{
- class JsonConverter;
- using JsonConverterPtr = std::shared_ptr<JsonConverter>;
-
- class JsonConverter : public DictConverter
- {
- public:
- JsonConverter() :
- DictConverter(ConverterType::Str, ".json")
- {}
-
- virtual std::vector<unsigned char> convert(const aron::data::DictPtr& data) override;
- virtual aron::data::DictPtr convert(const std::vector<unsigned char>& data) override;
- };
-}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/Converter.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/Converter.cpp
index fd719807eb069bcf62db065ed42bb649c38d225d..39b4d94b015841467ccadf14e34bf6ab1bf00ef0 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/Converter.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/Converter.cpp
@@ -1 +1,18 @@
#include "Converter.h"
+
+namespace armarx::armem::server::ltm
+{
+
+ std::pair<std::vector<unsigned char>, std::string> ImageConverter::convert(const aron::data::VariantPtr& data)
+ {
+ auto d = aron::data::NDArray::DynamicCastAndCheck(data);
+ return _convert(d);
+ }
+
+ aron::data::VariantPtr ImageConverter::convert(const std::vector<unsigned char>& data, const std::string& m)
+ {
+ auto d = _convert(data, m);
+ return d;
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/Converter.h b/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/Converter.h
index 740757ef84d98aa38ab1c2d65ab9e881bc215090..a2b2df04565bbe7940cbcc37fa5e49166d554df6 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/Converter.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/Converter.h
@@ -3,33 +3,28 @@
// STD/STL
#include <memory>
+// BaseClass
+#include "../Converter.h"
+
// ArmarX
#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
namespace armarx::armem::server::ltm
{
- class ImageConverter;
- using ImageConverterPtr = std::shared_ptr<ImageConverter>;
-
- class ImageConverter
+ class ImageConverter : public Converter
{
public:
- enum class ConverterType
- {
- Str,
- Binary
- };
-
- ImageConverter(const ConverterType t, const std::string& s):
- type(t),
- suffix(s)
+ ImageConverter(const ConverterType t, const std::string& id, const std::string& s):
+ Converter(t, id, s, aron::type::Descriptor::eImage)
{}
+
virtual ~ImageConverter() = default;
- virtual std::vector<unsigned char> convert(const aron::data::NDArrayPtr& data) = 0;
- virtual aron::data::NDArrayPtr convert(const std::vector<unsigned char>& data) = 0;
+ std::pair<std::vector<unsigned char>, std::string> convert(const aron::data::VariantPtr& data) final;
+ aron::data::VariantPtr convert(const std::vector<unsigned char>& data, const std::string&) final;
- const ConverterType type;
- const std::string suffix;
+ protected:
+ virtual std::pair<std::vector<unsigned char>, std::string> _convert(const aron::data::NDArrayPtr& data) = 0;
+ virtual aron::data::NDArrayPtr _convert(const std::vector<unsigned char>& data, const std::string&) = 0;
};
}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/exr/ExrConverter.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/exr/ExrConverter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c5eee777b60ac5b7e1c97606e5902107afdb0221
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/exr/ExrConverter.cpp
@@ -0,0 +1,33 @@
+#include "ExrConverter.h"
+
+// ArmarX
+#include <RobotAPI/libraries/aron/converter/opencv/OpenCVConverter.h>
+
+#include <opencv2/opencv.hpp>
+#include <opencv2/imgcodecs.hpp>
+#include <opencv2/imgproc.hpp>
+
+
+namespace armarx::armem::server::ltm::converter::image
+{
+ std::pair<std::vector<unsigned char>, std::string> ExrConverter::_convert(const aron::data::NDArrayPtr& data)
+ {
+ ARMARX_CHECK_NOT_NULL(data);
+
+ auto img = aron::converter::AronOpenCVConverter::ConvertToMat(data);
+ std::vector<unsigned char> buffer;
+
+ auto shape = data->getShape(); // we know from the extraction that the shape has 3 elements
+ ARMARX_CHECK_EQUAL(shape.size(), 3);
+ ARMARX_CHECK_EQUAL(shape[2], 4);
+
+ cv::imencode(".exr", img, buffer);
+ return std::make_pair(buffer, "");
+ }
+
+ aron::data::NDArrayPtr ExrConverter::_convert(const std::vector<unsigned char>& data, const std::string& m)
+ {
+ cv::Mat img = cv::imdecode(data, cv::IMREAD_ANYDEPTH);
+ return aron::converter::AronOpenCVConverter::ConvertFromMat(img);
+ }
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/exr/ExrConverter.h b/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/exr/ExrConverter.h
new file mode 100644
index 0000000000000000000000000000000000000000..ad4b1bfa2f4b517a55136ace72942d4440404c9f
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/exr/ExrConverter.h
@@ -0,0 +1,21 @@
+#pragma once
+
+// Base Class
+#include "../Converter.h"
+
+namespace armarx::armem::server::ltm::converter::image
+{
+ class ExrConverter : public ImageConverter
+ {
+ public:
+ ExrConverter() :
+ ImageConverter(ConverterType::Binary, "depthimage", ".exr")
+ {
+ enabled = true; // enabled by default
+ }
+
+ protected:
+ std::pair<std::vector<unsigned char>, std::string> _convert(const aron::data::NDArrayPtr& data) final;
+ aron::data::NDArrayPtr _convert(const std::vector<unsigned char>& data, const std::string&) final;
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/png/PngConverter.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/png/PngConverter.cpp
index 3658def03e62aff1fed98e21272c82fe31636ab5..e8536cf64841d23712e7264ce76282039b893189 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/png/PngConverter.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/png/PngConverter.cpp
@@ -10,20 +10,52 @@
namespace armarx::armem::server::ltm::converter::image
{
- std::vector<unsigned char> PngConverter::convert(const aron::data::NDArrayPtr& data)
+ std::pair<std::vector<unsigned char>, std::string> PngConverter::_convert(const aron::data::NDArrayPtr& data)
{
ARMARX_CHECK_NOT_NULL(data);
auto img = aron::converter::AronOpenCVConverter::ConvertToMat(data);
std::vector<unsigned char> buffer;
- cv::imencode(".png", img, buffer);
- return buffer;
+
+
+ auto shape = data->getShape(); // we know from the extraction that the shape has 3 elements
+ ARMARX_CHECK_EQUAL(shape.size(), 3);
+
+ if (shape[2] == 3) // its probably a rgb image
+ {
+ cv::cvtColor(img, img, CV_RGB2BGR);
+ cv::imencode(suffix, img, buffer);
+ return std::make_pair(buffer, ".rgb");
+ }
+
+ if (shape[2] == 1) // its probably a grayscale image
+ {
+ cv::imencode(suffix, img, buffer);
+ return std::make_pair(buffer, ".gs");
+ }
+
+ // try to export without conversion
+ cv::imencode(suffix, img, buffer);
+ return std::make_pair(buffer, "");
}
- aron::data::NDArrayPtr PngConverter::convert(const std::vector<unsigned char>& data)
+ aron::data::NDArrayPtr PngConverter::_convert(const std::vector<unsigned char>& data, const std::string& m)
{
- cv::Mat img = cv::imdecode(data, cv::IMREAD_COLOR);
- auto aron = aron::converter::AronOpenCVConverter::ConvertFromMat(img);
- return aron;
+ if (m == ".rgb")
+ {
+ cv::Mat img = cv::imdecode(data, cv::IMREAD_COLOR);
+ cv::cvtColor(img, img, CV_BGR2RGB);
+ return aron::converter::AronOpenCVConverter::ConvertFromMat(img);
+ }
+
+ if (m == ".gs")
+ {
+ cv::Mat img = cv::imdecode(data, cv::IMREAD_GRAYSCALE);
+ return aron::converter::AronOpenCVConverter::ConvertFromMat(img);
+ }
+
+ // try to load without conversion
+ cv::Mat img = cv::imdecode(data, cv::IMREAD_ANYCOLOR);
+ return aron::converter::AronOpenCVConverter::ConvertFromMat(img);
}
}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/png/PngConverter.h b/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/png/PngConverter.h
index 13581518a38a05a4ff1a2caaa8ddf7e25a8383af..5b6c80134319a52b77565dba322349985fed2267 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/png/PngConverter.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/converter/image/png/PngConverter.h
@@ -5,17 +5,17 @@
namespace armarx::armem::server::ltm::converter::image
{
- class PngConverter;
- using PngConverterPtr = std::shared_ptr<PngConverter>;
-
class PngConverter : public ImageConverter
{
public:
PngConverter() :
- ImageConverter(ConverterType::Str, ".png")
- {}
+ ImageConverter(ConverterType::Binary, "image", ".png")
+ {
+ enabled = true; // enabled by default
+ }
- virtual std::vector<unsigned char> convert(const aron::data::NDArrayPtr& data) override;
- virtual aron::data::NDArrayPtr convert(const std::vector<unsigned char>& data) override;
+ protected:
+ std::pair<std::vector<unsigned char>, std::string> _convert(const aron::data::NDArrayPtr& data) final;
+ aron::data::NDArrayPtr _convert(const std::vector<unsigned char>& data, const std::string&) final;
};
}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/object/Converter.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/converter/object/Converter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4f0808a6f03f1739e76cf6fc02a9a155be334be8
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/converter/object/Converter.cpp
@@ -0,0 +1,18 @@
+#include "Converter.h"
+
+namespace armarx::armem::server::ltm
+{
+
+ std::pair<std::vector<unsigned char>, std::string> ObjectConverter::convert(const aron::data::VariantPtr& data)
+ {
+ auto d = aron::data::Dict::DynamicCastAndCheck(data);
+ return _convert(d);
+ }
+
+ aron::data::VariantPtr ObjectConverter::convert(const std::vector<unsigned char>& data, const std::string& m)
+ {
+ auto d = _convert(data, m);
+ return d;
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/object/Converter.h b/source/RobotAPI/libraries/armem/server/ltm/base/converter/object/Converter.h
new file mode 100644
index 0000000000000000000000000000000000000000..e5c8e9f80a04d7bba1b2bca9e79986d66fb2d84a
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/converter/object/Converter.h
@@ -0,0 +1,30 @@
+#pragma once
+
+// STD/STL
+#include <memory>
+
+// BaseClass
+#include "../Converter.h"
+
+// ArmarX
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+
+namespace armarx::armem::server::ltm
+{
+ class ObjectConverter : public Converter
+ {
+ public:
+ ObjectConverter(const ConverterType t, const std::string& id, const std::string& s):
+ Converter(t, id, s, aron::type::Descriptor::eObject)
+ {}
+
+ virtual ~ObjectConverter() = default;
+
+ std::pair<std::vector<unsigned char>, std::string> convert(const aron::data::VariantPtr& data) final;
+ aron::data::VariantPtr convert(const std::vector<unsigned char>& data, const std::string&) final;
+
+ protected:
+ virtual std::pair<std::vector<unsigned char>, std::string> _convert(const aron::data::DictPtr& data) = 0;
+ virtual aron::data::DictPtr _convert(const std::vector<unsigned char>& data, const std::string&) = 0;
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/bson/BsonConverter.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/converter/object/bson/BsonConverter.cpp
similarity index 63%
rename from source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/bson/BsonConverter.cpp
rename to source/RobotAPI/libraries/armem/server/ltm/base/converter/object/bson/BsonConverter.cpp
index e78913abd351657aaa73c852f7e336acd1c3c7f1..8465201f3bd078b7bd93bec2cbd26378dfd119ab 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/bson/BsonConverter.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/converter/object/bson/BsonConverter.cpp
@@ -5,14 +5,14 @@
#include <bsoncxx/builder/stream/document.hpp>
#include <bsoncxx/builder/stream/array.hpp>
-namespace armarx::armem::server::ltm::converter::dict
+namespace armarx::armem::server::ltm::converter::object
{
namespace bsoncxxbuilder = bsoncxx::builder::stream;
namespace bsoncxxdoc = bsoncxx::document;
- std::vector<unsigned char> BsonConverter::convert(const aron::data::DictPtr& data)
+ std::pair<std::vector<unsigned char>, std::string> BsonConverter::_convert(const aron::data::DictPtr& data)
{
- std::vector<unsigned char> jsonVec = jsonConverter.convert(data);
+ auto [jsonVec, str] = jsonConverter.convert(data);
std::string json(jsonVec.begin(), jsonVec.end());
auto view = bsoncxx::from_json(json).view();
@@ -21,15 +21,16 @@ namespace armarx::armem::server::ltm::converter::dict
{
std::memcpy(bson.data(), view.data(), view.length());
}
- return bson;
+ return std::make_pair(bson, str);
}
- aron::data::DictPtr BsonConverter::convert(const std::vector<unsigned char>& data)
+ aron::data::DictPtr BsonConverter::_convert(const std::vector<unsigned char>& data, const std::string& m)
{
bsoncxx::document::view view(data.data(), data.size());
nlohmann::json json = bsoncxx::to_json(view);
std::string str = json.dump(2);
std::vector<unsigned char> jsonVec(str.begin(), str.end());
- return jsonConverter.convert(jsonVec);
+ auto v = jsonConverter.convert(jsonVec, m);
+ return aron::data::Dict::DynamicCast(v);
}
}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/object/bson/BsonConverter.h b/source/RobotAPI/libraries/armem/server/ltm/base/converter/object/bson/BsonConverter.h
new file mode 100644
index 0000000000000000000000000000000000000000..6eb183be2c5998a3eb43251ff7018a6e336260ea
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/converter/object/bson/BsonConverter.h
@@ -0,0 +1,28 @@
+#pragma once
+
+// Base Class
+#include "../Converter.h"
+
+// ArmarX
+#include "../json/JsonConverter.h"
+
+namespace armarx::armem::server::ltm::converter::object
+{
+ class BsonConverter;
+ using BsonConverterPtr = std::shared_ptr<BsonConverter>;
+
+ class BsonConverter : public ObjectConverter
+ {
+ public:
+ BsonConverter() :
+ ObjectConverter(ConverterType::Binary, "dict", ".bson")
+ {}
+
+ protected:
+ std::pair<std::vector<unsigned char>, std::string> _convert(const aron::data::DictPtr& data) final;
+ aron::data::DictPtr _convert(const std::vector<unsigned char>& data, const std::string&) final;
+
+ private:
+ JsonConverter jsonConverter;
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/json/JsonConverter.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/converter/object/json/JsonConverter.cpp
similarity index 55%
rename from source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/json/JsonConverter.cpp
rename to source/RobotAPI/libraries/armem/server/ltm/base/converter/object/json/JsonConverter.cpp
index 379caf9b76c45a997e7700a039290abe08726fd7..f2baa4865ff5d2a832e61acc857863d2c6ca545a 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/converter/dict/json/JsonConverter.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/converter/object/json/JsonConverter.cpp
@@ -2,16 +2,16 @@
#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
-namespace armarx::armem::server::ltm::converter::dict
+namespace armarx::armem::server::ltm::converter::object
{
- std::vector<unsigned char> JsonConverter::convert(const aron::data::DictPtr& data)
+ std::pair<std::vector<unsigned char>, std::string> JsonConverter::_convert(const aron::data::DictPtr& data)
{
nlohmann::json j = aron::converter::AronNlohmannJSONConverter::ConvertToNlohmannJSON(data);
auto str = j.dump(2);
- return std::vector<unsigned char>(str.begin(), str.end());
+ return std::make_pair(std::vector<unsigned char>(str.begin(), str.end()), "");
}
- aron::data::DictPtr JsonConverter::convert(const std::vector<unsigned char>& data)
+ aron::data::DictPtr JsonConverter::_convert(const std::vector<unsigned char>& data, const std::string&)
{
std::string str(data.begin(), data.end());
nlohmann::json j = nlohmann::json::parse(str);
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/object/json/JsonConverter.h b/source/RobotAPI/libraries/armem/server/ltm/base/converter/object/json/JsonConverter.h
new file mode 100644
index 0000000000000000000000000000000000000000..1b9f86ee93cda46983201be6e7910e4596734b3e
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/converter/object/json/JsonConverter.h
@@ -0,0 +1,24 @@
+#pragma once
+
+// Base Class
+#include "../Converter.h"
+
+// Simox
+#include <SimoxUtility/json.h>
+
+namespace armarx::armem::server::ltm::converter::object
+{
+ class JsonConverter : public ObjectConverter
+ {
+ public:
+ JsonConverter() :
+ ObjectConverter(ConverterType::Str, "dict", ".json")
+ {
+ enabled = true; // always true!
+ }
+
+ protected:
+ std::pair<std::vector<unsigned char>, std::string> _convert(const aron::data::DictPtr& data) final;
+ aron::data::DictPtr _convert(const std::vector<unsigned char>& data, const std::string&) final;
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/detail/BufferedMemoryBase.h b/source/RobotAPI/libraries/armem/server/ltm/base/detail/BufferedMemoryBase.h
index 29f57896b60082c871de07b9cf07efa8e7b57db0..4dade14f3b917b81eaf84721c998904f6b3b8be3 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/detail/BufferedMemoryBase.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/detail/BufferedMemoryBase.h
@@ -13,56 +13,73 @@ namespace armarx::armem::server::ltm
public:
BufferedMemoryBase() :
- Base()
+ BufferedMemoryBase({})
{
}
BufferedMemoryBase(const MemoryID& id) :
- Base(id),
- buffer(id)
+ Base(id)
{
+ buffer = std::make_shared<armem::wm::Memory>(id);
+ to_store = std::make_shared<armem::wm::Memory>(id);
}
virtual ~BufferedMemoryBase() = default;
+ void setMemoryID(const MemoryID& id) override
+ {
+ ARMARX_CHECK_NOT_EMPTY(id.memoryName);
+
+ Base::setMemoryID(id.getMemoryID());
+
+ buffer->id() = id.getMemoryID();
+ to_store->id() = id.getMemoryID();
+ }
+
armem::wm::Memory getBuffer() const
{
- return buffer;
+ std::lock_guard l(bufferMutex);
+ return *buffer;
}
void directlyStore(const armem::wm::Memory& memory)
{
TIMING_START(LTM_Memory_DirectlyStore);
- if (!this->pipeline.memFreqFilter->accept(memory))
+ for (auto& f : this->processors->memFilters)
{
- ARMARX_WARNING << deactivateSpam() << "Ignoring to put a Memory into the LTM because it got filtered.";
- return;
+ if (!f->accept(memory))
+ {
+ ARMARX_WARNING << deactivateSpam() << "Ignoring to commit a Memory into the LTM because the full commit got filtered.";
+ return;
+ }
}
- else
- {
- _directlyStore(memory);
- }
- TIMING_END(LTM_Memory_DirectlyStore);
+ _directlyStore(memory);
+ TIMING_END_STREAM(LTM_Memory_DirectlyStore, ARMARX_DEBUG);
}
void storeBuffer()
{
- std::lock_guard l(bufferMutex);
- if (buffer.empty())
+ std::lock_guard l(storeMutex);
+ {
+ std::lock_guard l(bufferMutex);
+ to_store = buffer;
+ buffer = std::make_shared<armem::wm::Memory>(this->id());
+ }
+
+ if (to_store->empty())
{
ARMARX_INFO << deactivateSpam() << "Cannot store an empty buffer. Ignoring.";
return;
}
- this->directlyStore(buffer);
- buffer.clear();
+ this->directlyStore(*to_store);
}
virtual void createPropertyDefinitions(PropertyDefinitionsPtr& defs, const std::string& prefix) override
{
Base::createPropertyDefinitions(defs, prefix);
- defs->optional(storeFrequency, prefix + ".buffer.storeFreq", "Frequency to store the buffer to the LTM in Hz.");
+ defs->optional(storeFrequency, prefix + ".buffer.storeFreq", "Frequency to store the buffer to the LTM in Hz.").setMin(1).setMax(1000);
}
protected:
@@ -71,7 +88,9 @@ namespace armarx::armem::server::ltm
void _store(const armem::wm::Memory& memory) override
{
std::lock_guard l(bufferMutex);
- buffer.append(memory);
+ buffer->append(memory);
+
+ // create task if not already exists
if (!task)
{
int waitingTimeMs = 1000.f / storeFrequency;
@@ -81,12 +100,12 @@ namespace armarx::armem::server::ltm
}
protected:
-
- /// Internal memory for data consolidated from wm to ltm (buffer)
+ /// Internal memory for data consolidated from wm to ltm (double-buffer)
/// The to-put-to-ltm buffer (contains data in plain text)
- /// This buffer may still be filtered.
+ /// This buffer may still be filtered (e.g. snapshot filters).
/// This means that it is not guaranteed that all data in the buffer will be stored in the ltm
- armem::wm::Memory buffer;
+ std::shared_ptr<armem::wm::Memory> buffer;
+ std::shared_ptr<armem::wm::Memory> to_store;
/// The periodic'task to store the content of the buffer to the ltm
typename armarx::PeriodicTask<BufferedMemoryBase>::pointer_type task = nullptr;
@@ -95,7 +114,8 @@ namespace armarx::armem::server::ltm
float storeFrequency = 10;
/// a mutex to access the buffer object
- std::mutex bufferMutex;
+ mutable std::mutex bufferMutex;
+ mutable std::mutex storeMutex;
};
} // namespace armarx::armem::server::ltm
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/detail/CoreSegmentBase.h b/source/RobotAPI/libraries/armem/server/ltm/base/detail/CoreSegmentBase.h
index 47a6278ab7c98739e58675164d9b6b0a65431b6d..d34d693af901d09d6cc7f8b9c045522e564501ec 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/detail/CoreSegmentBase.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/detail/CoreSegmentBase.h
@@ -24,15 +24,15 @@ namespace armarx::armem::server::ltm
/// return the full sub-ltm as a wm::CoreSegment with only references
/// the ltm may be huge, use with caution
- void loadAll(armem::wm::CoreSegment& coreSeg)
+ void loadAllReferences(armem::wm::CoreSegment& coreSeg)
{
- _loadAll(coreSeg);
+ _loadAllReferences(coreSeg);
}
/// convert the references of the input into a wm::Memory
- void load(armem::wm::CoreSegment& coreSeg)
+ void resolve(armem::wm::CoreSegment& coreSeg)
{
- _load(coreSeg);
+ _resolve(coreSeg);
}
/// encode the content of a wm::Memory and store
@@ -41,6 +41,12 @@ namespace armarx::armem::server::ltm
_store(coreSeg);
}
+ /// store the type of the core segment
+ void storeType(const armem::wm::CoreSegment& coreSeg)
+ {
+ _storeType(coreSeg);
+ }
+
/// iterate over all provider segments of this ltm
virtual bool forEachProviderSegment(std::function<void(ProviderSegmentT&)>&& func) const = 0;
@@ -60,9 +66,10 @@ namespace armarx::armem::server::ltm
}
protected:
- virtual void _loadAll(armem::wm::CoreSegment&) = 0;
- virtual void _load(armem::wm::CoreSegment&) = 0;
+ virtual void _loadAllReferences(armem::wm::CoreSegment&) = 0;
+ virtual void _resolve(armem::wm::CoreSegment&) = 0;
virtual void _store(const armem::wm::CoreSegment&) = 0;
+ virtual void _storeType(const armem::wm::CoreSegment&) = 0;
protected:
mutable std::recursive_mutex ltm_mutex;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/detail/EntityBase.h b/source/RobotAPI/libraries/armem/server/ltm/base/detail/EntityBase.h
index 38d3dec99173d19ccc00f9a8569f585cd700a27b..5f92e1a202d008d4bd6791a8527d9246812690d7 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/detail/EntityBase.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/detail/EntityBase.h
@@ -24,15 +24,15 @@ namespace armarx::armem::server::ltm
/// return the full sub-ltm as a wm::Entity with only references
/// the ltm may be huge, use with caution
- void loadAll(armem::wm::Entity& e)
+ void loadAllReferences(armem::wm::Entity& e)
{
- _loadAll(e);
+ _loadAllReferences(e);
}
/// convert the references of the input into a wm::Memory
- void load(armem::wm::Entity& e)
+ void resolve(armem::wm::Entity& e)
{
- _load(e);
+ _resolve(e);
}
/// encode the content of a wm::Memory and store
@@ -62,8 +62,8 @@ namespace armarx::armem::server::ltm
}
protected:
- virtual void _loadAll(armem::wm::Entity&) = 0;
- virtual void _load(armem::wm::Entity&) = 0;
+ virtual void _loadAllReferences(armem::wm::Entity&) = 0;
+ virtual void _resolve(armem::wm::Entity&) = 0;
virtual void _store(const armem::wm::Entity&) = 0;
protected:
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/detail/EntitySnapshotBase.h b/source/RobotAPI/libraries/armem/server/ltm/base/detail/EntitySnapshotBase.h
index f7cd75f6801d7dfef1c397e8d9f81881c654c294..5ebca19436bade8482c4e60fcb1219db10282bc4 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/detail/EntitySnapshotBase.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/detail/EntitySnapshotBase.h
@@ -20,15 +20,15 @@ namespace armarx::armem::server::ltm
/// return the full sub-ltm as a wm::EntitySnapshot with only references
/// the ltm may be huge, use with caution
- void loadAll(armem::wm::EntitySnapshot& e) const
+ void loadAllReferences(armem::wm::EntitySnapshot& e) const
{
- _loadAll(e);
+ _loadAllReferences(e);
}
/// convert the references of the input into a wm::Memory
- void load(armem::wm::EntitySnapshot& e) const
+ void resolve(armem::wm::EntitySnapshot& e) const
{
- _load(e);
+ _resolve(e);
}
/// encode the content of a wm::Memory and store
@@ -40,8 +40,8 @@ namespace armarx::armem::server::ltm
static std::string getLevelName();
protected:
- virtual void _loadAll(armem::wm::EntitySnapshot&) const = 0;
- virtual void _load(armem::wm::EntitySnapshot&) const = 0;
+ virtual void _loadAllReferences(armem::wm::EntitySnapshot&) const = 0;
+ virtual void _resolve(armem::wm::EntitySnapshot&) const = 0;
virtual void _store(const armem::wm::EntitySnapshot&) const = 0;
protected:
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryBase.h b/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryBase.h
index e615f9064df703495c86c0ed1db7b55ccd8e026b..d7ec68da708f72cca8ad0655571c4a73d7f48d0a 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryBase.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryBase.h
@@ -9,7 +9,6 @@
#include "CoreSegmentBase.h"
// ArmarX
-#include <ArmarXCore/core/application/properties/Properties.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/logging/LoggingUtil.h>
@@ -37,43 +36,62 @@ namespace armarx::armem::server::ltm
MemoryBase(const MemoryID& id) :
MemoryItem(id)
{
- pipeline.memFreqFilter = std::make_shared<filter::MemoryFrequencyFilter>();
- pipeline.snapFreqFilter = std::make_shared<filter::SnapshotFrequencyFilter>();
- pipeline.imageExtractor = std::make_shared<extractor::ImageExtractor>();
- pipeline.dictConverter = std::make_shared<converter::dict::JsonConverter>();
- pipeline.imgConverter = std::make_shared<converter::image::PngConverter>();
}
/// return the full ltm as a wm::Memory with only references
/// the ltm may be huge, use with caution
- void loadAll(armem::wm::Memory& memory)
+ armem::wm::Memory loadAllReferences()
+ {
+ armem::wm::Memory ret;
+ loadAllReferences(ret);
+ return ret;
+ }
+ void loadAllReferences(armem::wm::Memory& memory)
{
TIMING_START(LTM_Memory_LoadAll);
- _loadAll(memory);
- TIMING_END(LTM_Memory_LoadAll);
+ _loadAllReferences(memory);
+ TIMING_END_STREAM(LTM_Memory_LoadAll, 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 loadAllAndResolve()
+ {
+ armem::wm::Memory ret;
+ loadAllAndResolve(ret);
+ return ret;
+ }
+
+ void loadAllAndResolve(armem::wm::Memory& memory)
+ {
+ TIMING_START(LTM_Memory_LoadAllAndResolve);
+ _loadAllReferences(memory);
+ _resolve(memory);
+ TIMING_END_STREAM(LTM_Memory_LoadAllAndResolve, ARMARX_DEBUG);
}
/// convert the references of the input into a wm::Memory
- void load(armem::wm::Memory& memory)
+ void resolve(armem::wm::Memory& memory)
{
TIMING_START(LTM_Memory_Load);
- _load(memory);
- TIMING_END(LTM_Memory_Load);
+ _resolve(memory);
+ TIMING_END_STREAM(LTM_Memory_Load, ARMARX_DEBUG);
}
/// append a wm::Memory instance to the ltm
void store(const armem::wm::Memory& memory)
{
TIMING_START(LTM_Memory_Append);
- if (pipeline.memFreqFilter->accept(memory))
+ for (auto& f : processors->memFilters)
{
- _store(memory);
+ if (f->enabled && !f->accept(memory))
+ {
+ ARMARX_INFO << deactivateSpam() << "Ignoring to put a Memory into the LTM because it got filtered.";
+ return;
+ }
}
- else
- {
- ARMARX_WARNING << deactivateSpam() << "Ignoring to put a Memory into the LTM because it got filtered.";
- }
- TIMING_END(LTM_Memory_Append);
+ _store(memory);
+ TIMING_END_STREAM(LTM_Memory_Append, ARMARX_DEBUG);
}
/// append a wm::Memory instance to the ltm
@@ -93,9 +111,7 @@ namespace armarx::armem::server::ltm
/// parameters
virtual void createPropertyDefinitions(PropertyDefinitionsPtr& defs, const std::string& prefix)
{
- defs->optional(pipeline.memFreqFilter->waitingTimeInMs, prefix + "memFreqFilter.WaitingTime", "Withdraw time in MS after each LTM update.");
- defs->optional(pipeline.snapFreqFilter->waitingTimeInMs, prefix + "memSnapFilter.WaitingTime", "Withdraw time in MS after each Entity update.");
- defs->optional(pipeline.snapFreqFilter->waitingTimeInMs, prefix + "memSnapFilter.WaitingTime", "Withdraw time in MS after each Entity update.");
+ processors->createPropertyDefinitions(defs, prefix);
}
/// get level name
@@ -105,8 +121,8 @@ namespace armarx::armem::server::ltm
}
protected:
- virtual void _loadAll(armem::wm::Memory& memory) = 0;
- virtual void _load(armem::wm::Memory& memory) = 0;
+ virtual void _loadAllReferences(armem::wm::Memory& memory) = 0;
+ virtual void _resolve(armem::wm::Memory& memory) = 0;
virtual void _store(const armem::wm::Memory& memory) = 0;
public:
@@ -114,5 +130,6 @@ namespace armarx::armem::server::ltm
protected:
mutable std::recursive_mutex ltm_mutex;
+
};
} // namespace armarx::armem::server::ltm
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryItem.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryItem.cpp
index c932b5690f1ef5cd2f33dea87cf13b9381bfeed3..6f38d6ea21317b44d37045c985f15d5efe2103da 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryItem.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryItem.cpp
@@ -8,12 +8,13 @@
namespace armarx::armem::server::ltm
{
MemoryItem::MemoryItem(const MemoryID& id) :
+ processors(std::make_shared<Processors>()),
_id(id)
{
}
- MemoryItem::MemoryItem(const MemoryID& id, const LongTermMemoryPipeline& p) :
- pipeline(p),
+ MemoryItem::MemoryItem(const MemoryID& id, const std::shared_ptr<Processors>& p) :
+ processors(p),
_id(id)
{
}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryItem.h b/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryItem.h
index 913c0ecdfaaae1afb828c3291f0fd69e18ddbfb5..570231509806a4f6a56d55ab4a4a2d7442de8807 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryItem.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryItem.h
@@ -5,45 +5,16 @@
#include <optional>
#include <string>
-#include "../filter/frequencyFilter/FrequencyFilter.h"
-#include "../extractor/imageExtractor/ImageExtractor.h"
-#include "../converter/dict/json/JsonConverter.h"
-#include "../converter/image/png/PngConverter.h"
-
-#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include "Processors.h"
namespace armarx::armem::server::ltm
{
- /// all necessary classes to convert an entry of the ltm to some other format(s)
- struct LongTermMemoryPipeline
- {
- // Memory Filters
- /// filter based on update frequency (removes full updates)
- filter::MemoryFrequencyFilterPtr memFreqFilter;
-
- // Snapshot filters
- /// filter based on latest snapshot stored (per entity)
- filter::SnapshotFrequencyFilterPtr snapFreqFilter;
-
- // Extractors
- /// Extract NDArrays with 2 dims and at least 10 values (Images)
- extractor::ImageExtractorPtr imageExtractor;
-
- // Converters
- /// Convert dicts (everything that has not been extracted) to JSON
- converter::dict::JsonConverterPtr dictConverter;
- /// Convert images to png
- converter::image::PngConverterPtr imgConverter;
-
- };
-
/// @brief Interface functions for the longterm memory classes
class MemoryItem
{
public:
- MemoryItem() = default;
- MemoryItem(const MemoryID&);
- MemoryItem(const MemoryID&, const LongTermMemoryPipeline&);
+ MemoryItem(const MemoryID&); // only used by memory
+ MemoryItem(const MemoryID&, const std::shared_ptr<Processors>&); // used by all other segments
virtual ~MemoryItem() = default;
MemoryID id() const;
@@ -52,7 +23,7 @@ namespace armarx::armem::server::ltm
virtual void setMemoryID(const MemoryID&);
protected:
- LongTermMemoryPipeline pipeline;
+ std::shared_ptr<Processors> processors;
private:
MemoryID _id;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/detail/Processors.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/detail/Processors.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6e176bf7914c9d5dc59082f227206c313715c873
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/detail/Processors.cpp
@@ -0,0 +1,35 @@
+#include "Processors.h"
+
+namespace armarx::armem::server::ltm
+{
+ Processors::Processors()
+ {
+ // setup containers
+ memFilters.push_back(&memFreqFilter);
+ snapFilters.push_back(&snapFreqFilter);
+ snapFilters.push_back(&snapEqFilter);
+ extractors.push_back(&imageExtractor);
+ extractors.push_back(&depthImageExtractor);
+ converters.insert({pngConverter.identifier, &pngConverter});
+ converters.insert({exrConverter.identifier, &exrConverter});
+ }
+
+ void Processors::createPropertyDefinitions(PropertyDefinitionsPtr& defs, const std::string& prefix)
+ {
+ // filters
+ defs->optional(memFreqFilter.enabled, prefix + "memFreqFilter.Enabled");
+ defs->optional(memFreqFilter.waitingTimeInMs, prefix + "memFreqFilter.WaitingTime", "Waiting time in MS after each LTM update.");
+ defs->optional(snapFreqFilter.enabled, prefix + "snapFreqFilter.Enabled");
+ defs->optional(snapFreqFilter.waitingTimeInMs, prefix + "snapFreqFilter.WaitingTime", "Waiting time in MS after each Entity update.");
+ defs->optional(snapEqFilter.enabled, prefix + "snapEqFilter.Enabled");
+ defs->optional(snapEqFilter.maxWaitingTimeInMs, prefix + "snapEqFilter.MaxWaitingTime", "Max Waiting time in MS after each Entity update.");
+
+ // extractors
+ defs->optional(imageExtractor.enabled, prefix + "imageExtractor.Enabled");
+ defs->optional(depthImageExtractor.enabled, prefix + "depthImageExtractor.Enabled");
+
+ // converters
+ defs->optional(pngConverter.enabled, prefix + "pngConverter.Enabled");
+ defs->optional(exrConverter.enabled, prefix + "exrConverter.Enabled");
+ }
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/detail/Processors.h b/source/RobotAPI/libraries/armem/server/ltm/base/detail/Processors.h
new file mode 100644
index 0000000000000000000000000000000000000000..cc10b538d5f6b14af4e55fef61bb826508d4ec7d
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/detail/Processors.h
@@ -0,0 +1,54 @@
+#pragma once
+
+#include <map>
+#include <mutex>
+#include <optional>
+#include <string>
+
+#include <ArmarXCore/core/application/properties/Properties.h>
+
+#include "../filter/frequencyFilter/FrequencyFilter.h"
+#include "../filter/equalityFilter/EqualityFilter.h"
+#include "../extractor/imageExtractor/ImageExtractor.h"
+#include "../extractor/imageExtractor/DepthImageExtractor.h"
+#include "../converter/object/json/JsonConverter.h"
+#include "../converter/image/png/PngConverter.h"
+#include "../converter/image/exr/ExrConverter.h"
+#include "../typeConverter/json/JsonConverter.h"
+
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+
+namespace armarx::armem::server::ltm
+{
+ /// all necessary classes to filter and convert an entry of the ltm to some other format(s)
+ class Processors
+ {
+ public:
+ Processors();
+ void createPropertyDefinitions(PropertyDefinitionsPtr& defs, const std::string& prefix);
+
+ public:
+ // Unique Memory Filters
+ std::vector<MemoryFilter*> memFilters;
+ filter::MemoryFrequencyFilter memFreqFilter;
+
+ // Unique Snapshot filters
+ std::vector<SnapshotFilter*> snapFilters;
+ filter::SnapshotFrequencyFilter snapFreqFilter;
+ filter::SnapshotEqualityFilter snapEqFilter;
+
+ // Special Extractors
+ std::vector<Extractor*> extractors;
+ extractor::ImageExtractor imageExtractor;
+ extractor::DepthImageExtractor depthImageExtractor;
+
+ // Special Converters
+ std::map<std::string, Converter*> converters;
+ converter::image::PngConverter pngConverter;
+ converter::image::ExrConverter exrConverter;
+
+ // Default converter
+ converter::object::JsonConverter defaultObjectConverter;
+ converter::type::JsonConverter defaultTypeConverter;
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/detail/ProviderSegmentBase.h b/source/RobotAPI/libraries/armem/server/ltm/base/detail/ProviderSegmentBase.h
index 167858dc748b5c8289bc62ab1ae12d4329c95a97..b3c4be572317fa587b29864fe8981b5e11706cf1 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/detail/ProviderSegmentBase.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/detail/ProviderSegmentBase.h
@@ -24,15 +24,15 @@ namespace armarx::armem::server::ltm
/// return the full sub-ltm as a wm::ProviderSegment with only references
/// the ltm may be huge, use with caution
- void loadAll(armem::wm::ProviderSegment& provSeg)
+ void loadAllReferences(armem::wm::ProviderSegment& provSeg)
{
- _loadAll(provSeg);
+ _loadAllReferences(provSeg);
}
/// convert the references of the input into a wm::Memory
- void load(armem::wm::ProviderSegment& provSeg)
+ void resolve(armem::wm::ProviderSegment& provSeg)
{
- _load(provSeg);
+ _resolve(provSeg);
}
/// encode the content of a wm::Memory and store
@@ -41,6 +41,12 @@ namespace armarx::armem::server::ltm
_store(provSeg);
}
+ /// store the type of the segment
+ void storeType(const armem::wm::ProviderSegment& coreSeg)
+ {
+ _storeType(coreSeg);
+ }
+
/// iterate over all core segments of this ltm
virtual bool forEachEntity(std::function<void(EntityT&)>&& func) const = 0;
@@ -58,9 +64,10 @@ namespace armarx::armem::server::ltm
}
protected:
- virtual void _loadAll(armem::wm::ProviderSegment&) = 0;
- virtual void _load(armem::wm::ProviderSegment&) = 0;
+ virtual void _loadAllReferences(armem::wm::ProviderSegment&) = 0;
+ virtual void _resolve(armem::wm::ProviderSegment&) = 0;
virtual void _store(const armem::wm::ProviderSegment&) = 0;
+ virtual void _storeType(const armem::wm::ProviderSegment&) = 0;
protected:
mutable std::recursive_mutex ltm_mutex;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/extractor/Extractor.h b/source/RobotAPI/libraries/armem/server/ltm/base/extractor/Extractor.h
index 6da8399303d0d1bda7681ec5e88c9e79625595c9..0fd5ca5adc0f82010d1a4f0ac85cc836301bac4f 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/extractor/Extractor.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/extractor/Extractor.h
@@ -9,9 +9,6 @@
namespace armarx::armem::server::ltm
{
- class Extractor;
- typedef std::shared_ptr<Extractor> ExtractorPtr;
-
class Extractor
{
public:
@@ -21,10 +18,14 @@ namespace armarx::armem::server::ltm
std::map<std::string, aron::data::VariantPtr> extraction;
};
- Extractor() = default;
+ Extractor(const aron::type::Descriptor t, const std::string& id) : extractsType(t), identifier(id) {};
virtual ~Extractor() = default;
virtual Extraction extract(aron::data::DictPtr& data) = 0;
virtual aron::data::DictPtr merge(Extraction& encoding) = 0;
+
+ const aron::type::Descriptor extractsType;
+ const std::string identifier;
+ bool enabled = false;
};
}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/extractor/imageExtractor/DepthImageExtractor.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/extractor/imageExtractor/DepthImageExtractor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4640cca47ec3401f8effc043536ac1f7244f93fb
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/extractor/imageExtractor/DepthImageExtractor.cpp
@@ -0,0 +1,48 @@
+#include "DepthImageExtractor.h"
+
+
+namespace armarx::armem::server::ltm::extractor
+{
+ void DepthImageExtractorVisitor::visitDictOnEnter(Input& data)
+ {
+ ARMARX_CHECK_NOT_NULL(data);
+
+ auto dict = aron::data::Dict::DynamicCastAndCheck(data);
+ for (const auto& [key, child] : dict->getElements())
+ {
+ if (child && child->getDescriptor() == aron::data::Descriptor::eNDArray)
+ {
+ auto ndarray = aron::data::NDArray::DynamicCastAndCheck(child);
+ auto shape = ndarray->getShape();
+ if (shape.size() == 3 && shape[2] == 4 && std::accumulate(std::begin(shape), std::end(shape), 1, std::multiplies<int>()) > 200) // must be big enough to assume an image (instead of 4x4x4 poses)
+ {
+ depthImages[key] = ndarray;
+ dict->setElement(key, nullptr);
+ }
+ }
+ }
+ }
+
+ void DepthImageExtractorVisitor::visitUnknown(Input&)
+ {
+ // A member is null. Simply ignore...
+ }
+
+ Extractor::Extraction DepthImageExtractor::extract(aron::data::DictPtr& data)
+ {
+ DepthImageExtractorVisitor visitor;
+ aron::data::VariantPtr var = std::static_pointer_cast<aron::data::Variant>(data);
+ aron::data::VariantPtr p;
+ aron::data::visitRecursive(visitor, var);
+
+ Extraction encoding;
+ encoding.dataWithoutExtraction = data;
+ encoding.extraction = visitor.depthImages;
+ return encoding;
+ }
+
+ aron::data::DictPtr DepthImageExtractor::merge(Extraction& encoding)
+ {
+ return encoding.dataWithoutExtraction;
+ }
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/extractor/imageExtractor/DepthImageExtractor.h b/source/RobotAPI/libraries/armem/server/ltm/base/extractor/imageExtractor/DepthImageExtractor.h
new file mode 100644
index 0000000000000000000000000000000000000000..09b305e3ca412be13f9b0bf3caf348273fba23e4
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/extractor/imageExtractor/DepthImageExtractor.h
@@ -0,0 +1,31 @@
+#pragma once
+
+// Base Class
+#include "../Extractor.h"
+
+#include <RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.h>
+
+namespace armarx::armem::server::ltm::extractor
+{
+ class DepthImageExtractorVisitor : public aron::data::RecursiveVariantVisitor
+ {
+ public:
+ std::map<std::string, aron::data::VariantPtr> depthImages;
+
+ void visitDictOnEnter(Input& data);
+ void visitUnknown(Input& data);
+ };
+
+ class DepthImageExtractor : public Extractor
+ {
+ public:
+ DepthImageExtractor() :
+ Extractor(aron::type::Descriptor::eImage, "depthimage")
+ {
+ enabled = true;
+ };
+
+ virtual Extraction extract(aron::data::DictPtr& data) override;
+ virtual aron::data::DictPtr merge(Extraction& encoding) override;
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/extractor/imageExtractor/ImageExtractor.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/extractor/imageExtractor/ImageExtractor.cpp
index ff86e06243bbee18abfafdfe781ffa50d7126ee6..c646a4f474019742d0db6094245543463ccc553c 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/extractor/imageExtractor/ImageExtractor.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/extractor/imageExtractor/ImageExtractor.cpp
@@ -14,7 +14,7 @@ namespace armarx::armem::server::ltm::extractor
{
auto ndarray = aron::data::NDArray::DynamicCastAndCheck(child);
auto shape = ndarray->getShape();
- if (shape.size() == 3 && std::accumulate(std::begin(shape), std::end(shape), 1, std::multiplies<int>()) > 200) // must be big enough to assume an image (instead of 4x4x4 poses)
+ if (shape.size() == 3 && (shape[2] == 3 || shape[2] == 1 /* 3 channel color or grayscale */) && std::accumulate(std::begin(shape), std::end(shape), 1, std::multiplies<int>()) > 200) // must be big enough to assume an image (instead of 4x4x4 poses)
{
images[key] = ndarray;
dict->setElement(key, nullptr);
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/extractor/imageExtractor/ImageExtractor.h b/source/RobotAPI/libraries/armem/server/ltm/base/extractor/imageExtractor/ImageExtractor.h
index 901bebb8b87f16bb3fc0c425343e1b89d2f9b2be..be9c80ba7d8138857aa54edefbb3d61a2d461e39 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/extractor/imageExtractor/ImageExtractor.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/extractor/imageExtractor/ImageExtractor.h
@@ -7,9 +7,6 @@
namespace armarx::armem::server::ltm::extractor
{
- class ImageExtractor;
- typedef std::shared_ptr<ImageExtractor> ImageExtractorPtr;
-
class ImageExtractorVisitor : public aron::data::RecursiveVariantVisitor
{
public:
@@ -22,12 +19,13 @@ namespace armarx::armem::server::ltm::extractor
class ImageExtractor : public Extractor
{
public:
- ImageExtractor() = default;
+ ImageExtractor() :
+ Extractor(aron::type::Descriptor::eImage, "image")
+ {
+ enabled = true;
+ };
virtual Extraction extract(aron::data::DictPtr& data) override;
virtual aron::data::DictPtr merge(Extraction& encoding) override;
-
- public:
- bool enabled;
};
}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/extractor/noExtractor/NoExtractor.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/extractor/noExtractor/NoExtractor.cpp
deleted file mode 100644
index 58f36e588390296b73d0899520afc78c02e61b3a..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/ltm/base/extractor/noExtractor/NoExtractor.cpp
+++ /dev/null
@@ -1,17 +0,0 @@
-#include "NoExtractor.h"
-
-
-namespace armarx::armem::server::ltm::extractor
-{
- Extractor::Extraction NoExtractor::extract(aron::data::DictPtr& data)
- {
- Extraction encoding;
- encoding.dataWithoutExtraction = data;
- return encoding;
- }
-
- aron::data::DictPtr NoExtractor::merge(Extraction& encoding)
- {
- return encoding.dataWithoutExtraction;
- }
-}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/extractor/noExtractor/NoExtractor.h b/source/RobotAPI/libraries/armem/server/ltm/base/extractor/noExtractor/NoExtractor.h
deleted file mode 100644
index fdcc6668471815f8a8c645e19a002adb60275beb..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/ltm/base/extractor/noExtractor/NoExtractor.h
+++ /dev/null
@@ -1,19 +0,0 @@
-#pragma once
-
-// Base Class
-#include "../Extractor.h"
-
-namespace armarx::armem::server::ltm::extractor
-{
- class NoExtractor;
- typedef std::shared_ptr<NoExtractor> NoExtractorPtr;
-
- class NoExtractor : public Extractor
- {
- public:
- NoExtractor() = default;
-
- virtual Extraction extract(aron::data::DictPtr& data) override;
- virtual aron::data::DictPtr merge(Extraction& encoding) override;
- };
-}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/filter/Filter.h b/source/RobotAPI/libraries/armem/server/ltm/base/filter/Filter.h
index f5b068600ac0bb1a89215d81861c2a7a0e10bc6f..e75969bfa50b86379b1a0db47ab99c9f533ddc05 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/filter/Filter.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/filter/Filter.h
@@ -9,12 +9,6 @@
namespace armarx::armem::server::ltm
{
- class SnapshotFilter;
- typedef std::shared_ptr<SnapshotFilter> SnapshotFilterPtr;
-
- class MemoryFilter;
- typedef std::shared_ptr<MemoryFilter> MemoryFilterPtr;
-
class MemoryFilter
{
public:
@@ -22,6 +16,8 @@ namespace armarx::armem::server::ltm
virtual ~MemoryFilter() = default;
virtual bool accept(const armem::wm::Memory& e) = 0;
+
+ bool enabled = false;
};
class SnapshotFilter
@@ -31,5 +27,7 @@ namespace armarx::armem::server::ltm
virtual ~SnapshotFilter() = default;
virtual bool accept(const armem::wm::EntitySnapshot& e) = 0;
+
+ bool enabled = false;
};
}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/filter/equalityFilter/EqualityFilter.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/filter/equalityFilter/EqualityFilter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ec52015e2c9ca62a3815dc3c9f62634078e2ba59
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/filter/equalityFilter/EqualityFilter.cpp
@@ -0,0 +1,52 @@
+#include "EqualityFilter.h"
+
+#include <IceUtil/Time.h>
+
+namespace armarx::armem::server::ltm::filter
+{
+ bool SnapshotEqualityFilter::accept(const armem::wm::EntitySnapshot& e)
+ {
+ auto entityID = e.id().getEntityID();
+ auto genMs = e.time().toMilliSeconds();
+
+ long lastMs = 0;
+ std::vector<aron::data::DictPtr> lastData;
+ if (timestampLastCommitInMs.count(entityID) > 0)
+ {
+ lastData = dataLastCommit.at(entityID);
+ lastMs = timestampLastCommitInMs.at(entityID);
+ }
+
+ auto timePassedSinceLastStored = genMs - lastMs;
+ if (maxWaitingTimeInMs < 0 || timePassedSinceLastStored > maxWaitingTimeInMs)
+ {
+ bool accept = false;
+ std::vector<aron::data::DictPtr> genData;
+ for (unsigned int i = 0; i != e.size(); ++i)
+ {
+ const auto& d = e.getInstance(i).data();
+ genData.push_back(d);
+
+ if (lastMs == 0 || e.size() != lastData.size()) // nothing stored yet or we cannot compare
+ {
+ accept = true;
+ break;
+ }
+
+ const auto& el = lastData.at(i);
+ if ((!d and el) || (d and !el) || (d && el && !(*d == *el))) // data unequal?
+ {
+ accept = true;
+ break;
+ }
+ }
+
+ if (!accept) return false;
+
+ dataLastCommit[entityID] = genData;
+ timestampLastCommitInMs[entityID] = genMs;
+ return true;
+ }
+ return false;
+ }
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/filter/equalityFilter/EqualityFilter.h b/source/RobotAPI/libraries/armem/server/ltm/base/filter/equalityFilter/EqualityFilter.h
new file mode 100644
index 0000000000000000000000000000000000000000..1ad630b4fb4add2cd318b6b3e8cdf5eddd52e5fa
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/filter/equalityFilter/EqualityFilter.h
@@ -0,0 +1,29 @@
+#pragma once
+
+#include <vector>
+#include <map>
+
+// Base Class
+#include "../Filter.h"
+
+// Aron
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+
+namespace armarx::armem::server::ltm::filter
+{
+ class SnapshotEqualityFilter :
+ public SnapshotFilter
+ {
+ public:
+ SnapshotEqualityFilter() = default;
+
+ virtual bool accept(const armem::wm::EntitySnapshot& e) override;
+
+ public:
+ int maxWaitingTimeInMs = -1;
+
+ private:
+ std::map<MemoryID, std::vector<aron::data::DictPtr>> dataLastCommit;
+ std::map<MemoryID, long> timestampLastCommitInMs;
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/filter/frequencyFilter/FrequencyFilter.h b/source/RobotAPI/libraries/armem/server/ltm/base/filter/frequencyFilter/FrequencyFilter.h
index 2da5ea95712598ec7b78e7ca0342063473a89bf8..f8427b2b70e41cd603702cb60b5664ddef3126da 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/filter/frequencyFilter/FrequencyFilter.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/filter/frequencyFilter/FrequencyFilter.h
@@ -1,16 +1,12 @@
#pragma once
+#include <map>
+
// Base Class
#include "../Filter.h"
namespace armarx::armem::server::ltm::filter
{
- class SnapshotFrequencyFilter;
- typedef std::shared_ptr<SnapshotFrequencyFilter> SnapshotFrequencyFilterPtr;
-
- class MemoryFrequencyFilter;
- typedef std::shared_ptr<MemoryFrequencyFilter> MemoryFrequencyFilterPtr;
-
class MemoryFrequencyFilter :
public MemoryFilter
{
@@ -35,9 +31,9 @@ namespace armarx::armem::server::ltm::filter
virtual bool accept(const armem::wm::EntitySnapshot& e) override;
public:
- int waitingTimeInMs = 1000;
+ int waitingTimeInMs = -1;
private:
- std::unordered_map<MemoryID, long> timestampLastCommitInMs;
+ std::map<MemoryID, long> timestampLastCommitInMs;
};
}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/typeConverter/Converter.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/typeConverter/Converter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c846adcbd45ddb152a4525207a9bbde30bd897af
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/typeConverter/Converter.cpp
@@ -0,0 +1,6 @@
+#include "Converter.h"
+
+namespace armarx::armem::server::ltm
+{
+
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/typeConverter/Converter.h b/source/RobotAPI/libraries/armem/server/ltm/base/typeConverter/Converter.h
new file mode 100644
index 0000000000000000000000000000000000000000..8290e17c03f3e32e4d7b417b2269699869a85a0a
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/typeConverter/Converter.h
@@ -0,0 +1,28 @@
+#pragma once
+
+// STD/STL
+#include <memory>
+
+// ArmarX
+#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h>
+
+namespace armarx::armem::server::ltm
+{
+ class TypeConverter
+ {
+ public:
+ TypeConverter(const std::string& id, const std::string& s):
+ identifier(id),
+ suffix(s)
+ {}
+ virtual ~TypeConverter() = default;
+
+ virtual std::pair<std::vector<unsigned char>, std::string> convert(const aron::type::ObjectPtr& data) = 0;
+ virtual aron::type::ObjectPtr convert(const std::vector<unsigned char>& data, const std::string&) = 0;
+
+ public:
+ const std::string identifier;
+ const std::string suffix;
+ bool enabled = false;
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/typeConverter/json/JsonConverter.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/typeConverter/json/JsonConverter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..cc558297e32f82b6db21a2649f283292a0a11518
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/typeConverter/json/JsonConverter.cpp
@@ -0,0 +1,20 @@
+#include "JsonConverter.h"
+
+#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
+
+namespace armarx::armem::server::ltm::converter::type
+{
+ std::pair<std::vector<unsigned char>, std::string> JsonConverter::convert(const aron::type::ObjectPtr& data)
+ {
+ nlohmann::json j = aron::converter::AronNlohmannJSONConverter::ConvertToNlohmannJSON(data);
+ auto str = j.dump(2);
+ return std::make_pair(std::vector<unsigned char>(str.begin(), str.end()), "");
+ }
+
+ aron::type::ObjectPtr JsonConverter::convert(const std::vector<unsigned char>& data, const std::string&)
+ {
+ std::string str(data.begin(), data.end());
+ nlohmann::json j = nlohmann::json::parse(str);
+ return aron::converter::AronNlohmannJSONConverter::ConvertFromNlohmannJSONTypeObject(j);
+ }
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/typeConverter/json/JsonConverter.h b/source/RobotAPI/libraries/armem/server/ltm/base/typeConverter/json/JsonConverter.h
new file mode 100644
index 0000000000000000000000000000000000000000..1f95674b5398d910468b1335617efdb3591a9f7c
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/typeConverter/json/JsonConverter.h
@@ -0,0 +1,23 @@
+#pragma once
+
+// Base Class
+#include "../Converter.h"
+
+// Simox
+#include <SimoxUtility/json.h>
+
+namespace armarx::armem::server::ltm::converter::type
+{
+ class JsonConverter : public TypeConverter
+ {
+ public:
+ JsonConverter() :
+ TypeConverter("dict", ".json")
+ {
+ enabled = true; // always true!
+ }
+
+ std::pair<std::vector<unsigned char>, std::string> convert(const aron::type::ObjectPtr& data) final;
+ aron::type::ObjectPtr convert(const std::vector<unsigned char>& data, const std::string&) final;
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/CoreSegment.cpp b/source/RobotAPI/libraries/armem/server/ltm/disk/CoreSegment.cpp
index 1a3f17e7dce50eae95409cb2028fed86dd1ac7e7..ee45273c7517d48e432b321144afc7a0bc0a364a 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/CoreSegment.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/CoreSegment.cpp
@@ -8,96 +8,156 @@
namespace armarx::armem::server::ltm::disk
{
- namespace
- {
- MemoryID getMemoryIDFromPath(const std::filesystem::path& p)
- {
- util::ensureFolderExists(p);
-
- MemoryID m;
- m.memoryName = p.parent_path().filename();
- m.coreSegmentName = p.filename();
- return m;
- }
- }
-
- CoreSegment::CoreSegment(const std::filesystem::path& p, const LongTermMemoryPipeline& pipe) :
- CoreSegmentBase(getMemoryIDFromPath(p), pipe),
- DiskMemoryItem(p.parent_path())
+ CoreSegment::CoreSegment(const std::filesystem::path& p, const MemoryID& id, const std::shared_ptr<Processors>& filters, const DiskMemoryItem::MemoryEncodingMode mode, const unsigned long e) :
+ CoreSegmentBase(id, filters),
+ DiskMemoryItem(p, EscapeSegmentName(id.memoryName), std::filesystem::path(EscapeSegmentName(id.coreSegmentName))),
+ currentMode(mode),
+ currentExport(e)
{
}
bool CoreSegment::forEachProviderSegment(std::function<void(ProviderSegment&)>&& func) const
{
- if (!checkPath(id().coreSegmentName))
+ auto mPath = getMemoryBasePathForMode(currentMode, currentExport);
+ auto relPath = getRelativePathForMode(currentMode);
+ if (!util::checkIfBasePathExists(mPath) || !util::checkIfFolderExists(mPath, relPath))
{
return false;
}
- std::filesystem::path p = path;
- p = p / id().coreSegmentName;
- util::ensureFolderExists(p, false);
-
- for (const auto& subdir : std::filesystem::directory_iterator(p))
+ for (const auto& subdirName : util::getAllDirectories(mPath, relPath))
{
- std::filesystem::path subdirPath = subdir.path();
- ProviderSegment c(subdirPath, pipeline);
+ std::string segmentName = UnescapeSegmentName(subdirName);
+ ProviderSegment c(memoryParentPath, id().withProviderSegmentName(segmentName), processors, currentMode, currentExport);
func(c);
}
return true;
}
- std::shared_ptr<ProviderSegment> CoreSegment::findProviderSegment(const std::string& n) const
+ std::shared_ptr<ProviderSegment> CoreSegment::findProviderSegment(const std::string& providerSegmentName) const
{
- if (!checkPath(id().coreSegmentName))
+ auto mPath = getMemoryBasePathForMode(currentMode, currentExport);
+ auto relPath = getRelativePathForMode(currentMode);
+ if (!util::checkIfBasePathExists(mPath) || !util::checkIfFolderExists(mPath, relPath))
{
- return {};
+ return nullptr;
}
- std::filesystem::path p = path;
- p = p / id().coreSegmentName;
- util::ensureFolderExists(p, false);
+ auto c = std::make_shared<ProviderSegment>(memoryParentPath, id().withProviderSegmentName(providerSegmentName), processors, currentMode, currentExport);
+ if (!util::checkIfFolderExists(mPath, c->getRelativePathForMode(currentMode)))
+ {
+ return nullptr;
+ }
- std::filesystem::path subpath = p / n;
- util::ensureFolderExists(subpath, false);
- auto c = std::make_shared<ProviderSegment>(subpath, pipeline);
return c;
}
- std::string CoreSegment::getExpectedFolderName() const
+ void CoreSegment::_loadAllReferences(armem::wm::CoreSegment& e)
{
- return name();
- }
+ auto mPath = getMemoryBasePathForMode(currentMode, currentExport);
+ auto relPath = getRelativePathForMode(currentMode);
+ if (!util::checkIfBasePathExists(mPath) || !util::checkIfFolderExists(mPath, relPath))
+ {
+ return;
+ }
- void CoreSegment::_loadAll(armem::wm::CoreSegment& e)
- {
e.id() = id();
- forEachProviderSegment([&e](ProviderSegment& x) {
+ auto& conv = processors->defaultTypeConverter;
+ auto relTPath = relPath / (constantes::TYPE_FILENAME + conv.suffix);
+
+ if (util::checkIfFileExists(mPath, relTPath))
+ {
+ auto typeFileContent = util::readDataFromFile(mPath, relTPath);
+ auto typeAron = conv.convert(typeFileContent, "");
+ e.aronType() = aron::type::Object::DynamicCastAndCheck(typeAron);
+ }
+
+ forEachProviderSegment([&e](auto& x) {
armem::wm::ProviderSegment s;
- x.loadAll(s);
+ x.loadAllReferences(s);
e.addProviderSegment(s);
});
}
- void CoreSegment::_load(armem::wm::CoreSegment& c)
+ void CoreSegment::_resolve(armem::wm::CoreSegment& c)
{
- c.forEachProviderSegment([this](armem::wm::ProviderSegment& e)
+ auto mPath = getMemoryBasePathForMode(currentMode, currentExport);
+ auto relPath = getRelativePathForMode(currentMode);
+ if (!util::checkIfBasePathExists(mPath) || !util::checkIfFolderExists(mPath, relPath))
{
- util::ensureFolderExists(std::filesystem::path(path) / id().coreSegmentName / e.id().providerSegmentName, false);
- ProviderSegment c((std::filesystem::path(path) / id().coreSegmentName / e.id().providerSegmentName), pipeline);
- c.load(e);
+ return;
+ }
+
+ c.forEachProviderSegment([&](auto& e)
+ {
+ ProviderSegment c(memoryParentPath, id().withProviderSegmentName(e.id().providerSegmentName), processors, currentMode, currentExport);
+ if (util::checkIfFolderExists(mPath, c.getRelativePathForMode(currentMode)))
+ {
+ c.resolve(e);
+ }
});
}
void CoreSegment::_store(const armem::wm::CoreSegment& c)
{
- c.forEachProviderSegment([this](const auto& provSegment)
+ auto currentMaxExport = currentExport;
+ auto encodingModeOfPast = currentMode;
+
+ if (id().coreSegmentName.empty())
{
- util::ensureFolderExists(std::filesystem::path(path) / id().coreSegmentName / provSegment.id().providerSegmentName);
- ProviderSegment c((std::filesystem::path(path) / id().coreSegmentName / provSegment.id().providerSegmentName), pipeline);
- c.store(provSegment);
+ ARMARX_WARNING << "During storage of segment '" << c.id().str() << "' I noticed that the corresponding LTM has no id set. " <<
+ "I set the id of the LTM to the same name, however this should not happen!";
+ id().coreSegmentName = c.id().coreSegmentName;
+ }
+
+ auto defaultMode = MemoryEncodingMode::FILESYSTEM;
+
+ auto defaultMPath = getMemoryBasePathForMode(defaultMode, 0);
+ auto defaultRelPath = getRelativePathForMode(defaultMode);
+ if (!util::checkIfFolderExists(defaultMPath, defaultRelPath))
+ {
+ ARMARX_WARNING << "The segment folder for segment '"+id().str()+"'was not created. I will create the folder by myself, however it seems like there is a bug in the ltm pipeline.";
+ util::ensureFolderExists(defaultMPath, defaultRelPath, true);
+ }
+
+ c.forEachProviderSegment([&](const auto& prov)
+ {
+ ProviderSegment c(memoryParentPath, id().withProviderSegmentName(prov.id().providerSegmentName), processors, encodingModeOfPast, currentMaxExport);
+ util::ensureFolderExists(defaultMPath, c.getRelativePathForMode(defaultMode), true);
+
+ c.storeType(prov);
+ c.store(prov);
});
}
+ void CoreSegment::_storeType(const armem::wm::CoreSegment& c)
+ {
+ if (id().coreSegmentName.empty())
+ {
+ ARMARX_WARNING << "During storage of segment '" << c.id().str() << "' I noticed that the corresponding LTM has no id set. " <<
+ "I set the id of the LTM to the same name, however this should not happen!";
+ id().coreSegmentName = c.id().coreSegmentName;
+ }
+
+ auto defaultMode = MemoryEncodingMode::FILESYSTEM;
+
+ auto defaultMPath = getMemoryBasePathForMode(defaultMode, 0);
+ auto defaultRelPath = getRelativePathForMode(defaultMode);
+ if (!util::checkIfFolderExists(defaultMPath, defaultRelPath))
+ {
+ ARMARX_WARNING << "The segment folder for segment '"+id().str()+"'was not created. I will create the folder by myself, however it seems like there is a bug in the ltm pipeline.";
+ util::ensureFolderExists(defaultMPath, defaultRelPath, true);
+ }
+
+ if(c.hasAronType())
+ {
+ auto& conv = processors->defaultTypeConverter;
+
+ auto [vec, modeSuffix] = conv.convert(c.aronType());
+ ARMARX_CHECK_EMPTY(modeSuffix);
+ std::filesystem::path relTypePath = defaultRelPath / (constantes::TYPE_FILENAME + conv.suffix);
+ util::writeDataToFileRepeated(defaultMPath, relTypePath, vec);
+ }
+ }
}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/CoreSegment.h b/source/RobotAPI/libraries/armem/server/ltm/disk/CoreSegment.h
index b5d5d6c59a89482f7ebc5b779826737a062542d1..6dfc00e7e5e37814027cfc10a3be53dd83dff603 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/CoreSegment.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/CoreSegment.h
@@ -16,19 +16,21 @@ namespace armarx::armem::server::ltm::disk
{
public:
- CoreSegment(const std::filesystem::path&, const LongTermMemoryPipeline& p);
+ CoreSegment(const std::filesystem::path& parentPath, const MemoryID& id, const std::shared_ptr<Processors>& p, const DiskMemoryItem::MemoryEncodingMode mode, const unsigned long e);
bool forEachProviderSegment(std::function<void(ProviderSegment&)>&& func) const override;
std::shared_ptr<ProviderSegment> findProviderSegment(const std::string&) const override;
protected:
- void _loadAll(armem::wm::CoreSegment&) override;
- void _load(armem::wm::CoreSegment&) override;
+ void _loadAllReferences(armem::wm::CoreSegment&) override;
+ void _resolve(armem::wm::CoreSegment&) override;
void _store(const armem::wm::CoreSegment&) override;
+ void _storeType(const armem::wm::CoreSegment&) override;
- std::string getExpectedFolderName() const override;
-
+ private:
+ MemoryEncodingMode currentMode;
+ unsigned long currentExport;
};
} // namespace armarx::armem::server::ltm::disk
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/Entity.cpp b/source/RobotAPI/libraries/armem/server/ltm/disk/Entity.cpp
index 15972407317946524a4ccfd525f84405199e076a..ac1703ac910796f139b81b39aa3504e1fa21b731 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/Entity.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/Entity.cpp
@@ -12,150 +12,324 @@
namespace armarx::armem::server::ltm::disk
{
-
- namespace
- {
- MemoryID getMemoryIDFromPath(const std::filesystem::path& p)
- {
- util::ensureFolderExists(p);
-
- MemoryID m;
- m.memoryName = p.parent_path().parent_path().parent_path().filename();
- m.coreSegmentName = p.parent_path().parent_path().filename();
- m.providerSegmentName = p.parent_path().filename();
- m.entityName = p.filename();
- return m;
- }
- }
-
- Entity::Entity(const std::filesystem::path& p, const LongTermMemoryPipeline& pipe) :
- EntityBase(getMemoryIDFromPath(p), pipe),
- DiskMemoryItem(p.parent_path())
+ Entity::Entity(const std::filesystem::path& p, const MemoryID& id, const std::shared_ptr<Processors>& filters, const DiskMemoryItem::MemoryEncodingMode mode, const unsigned long e) :
+ EntityBase(id, filters),
+ DiskMemoryItem(p, EscapeSegmentName(id.memoryName), std::filesystem::path(EscapeSegmentName(id.coreSegmentName)) / EscapeSegmentName(id.providerSegmentName) / EscapeSegmentName(id.entityName)),
+ currentMode(mode),
+ currentExport(e)
{
}
- std::string Entity::getExpectedFolderName() const
- {
- return name();
- }
-
bool Entity::forEachSnapshot(std::function<void(EntitySnapshot&)>&& func) const
{
- if (!checkPath(id().entityName))
+ auto mPath = getMemoryBasePathForMode(currentMode, currentExport);
+ auto relPath = getRelativePathForMode(currentMode);
+ if (!util::checkIfBasePathExists(mPath) || !util::checkIfFolderExists(mPath, relPath))
{
return false;
}
- std::filesystem::path p = path;
- p = p / id().entityName;
- util::ensureFolderExists(p, false);
-
- for (const auto& subdir : std::filesystem::directory_iterator(p))
+ for (const auto& hName : util::getAllDirectories(mPath, relPath))
{
- std::filesystem::path subdirPath = subdir.path();
- EntitySnapshot c(subdirPath, pipeline);
- func(c);
+ if (!util::isNumber(hName))
+ {
+ ARMARX_WARNING << "Found a non-timestamp folder inside an entity '" << id().str() << "' with name '" << hName << "'. " <<
+ "Ignoring this folder, however this is a bad situation.";
+ continue;
+ }
+
+ // 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;
+ //}
+
+ auto hRelPath = relPath / hName;
+ for (const auto& secName : util::getAllDirectories(mPath, hRelPath))
+ {
+ if (!util::isNumber(secName))
+ {
+ ARMARX_WARNING << "Found a non-timestamp folder inside an entity '" << id().str() << "' with name '" << secName << "'. " <<
+ "Ignoring this folder, however this is a bad situation.";
+ continue;
+ }
+
+ auto secRelPath = hRelPath / secName;
+ for (const auto& usecName : util::getAllDirectories(mPath, secRelPath))
+ {
+ if (!util::isNumber(usecName))
+ {
+ ARMARX_WARNING << "Found a non-timestamp folder inside an entity '" << id().str() << "' with name '" << usecName << "'. " <<
+ "Ignoring this folder, however this is a bad situation.";
+ continue;
+ }
+
+ EntitySnapshot c(memoryParentPath, id().withTimestamp(timeFromStringMicroSeconds(usecName)), processors, currentMode, currentExport);
+ func(c);
+ }
+ }
}
return true;
}
bool Entity::forEachSnapshotInIndexRange(long first, long last, std::function<void(EntitySnapshot&)>&& func) const
{
- return true;
+ throw LocalException("NOT IMPLEMENTED YET BECAUSE THE DIRECTORY ITERATOR IS UNSORTED!");
}
bool Entity::forEachSnapshotInTimeRange(const Time& min, const Time& max, std::function<void(EntitySnapshot&)>&& func) const
{
- return true;
+ auto f = [&](EntitySnapshot& e) {
+ auto ts = e.id().timestamp;
+ if (ts >= min && ts <= max)
+ {
+ func(e);
+ }
+ };
+
+ return forEachSnapshot(std::move(f));
}
bool Entity::forEachSnapshotBeforeOrAt(const Time& time, std::function<void(EntitySnapshot&)>&& func) const
{
- return true;
+ auto f = [&](EntitySnapshot& e) {
+ auto ts = e.id().timestamp;
+ if (ts <= time)
+ {
+ func(e);
+ }
+ };
+
+ return forEachSnapshot(std::move(f));
}
bool Entity::forEachSnapshotBefore(const Time& time, std::function<void(EntitySnapshot&)>&& func) const
{
- return true;
+ auto f = [&](EntitySnapshot& e) {
+ auto ts = e.id().timestamp;
+ if (ts < time)
+ {
+ func(e);
+ }
+ };
+
+ return forEachSnapshot(std::move(f));
}
std::shared_ptr<EntitySnapshot> Entity::findSnapshot(const Time& n) const
{
- if (!checkPath(id().entityName))
+ auto mPath = getMemoryBasePathForMode(currentMode, currentExport);
+ auto relPath = getRelativePathForMode(currentMode);
+ if (!util::checkIfBasePathExists(mPath) || !util::checkIfFolderExists(mPath, relPath))
{
- return {};
+ return nullptr;
}
- std::filesystem::path p = path;
- p = p / id().entityName;
- util::ensureFolderExists(p, false);
+ auto c = std::make_shared<EntitySnapshot>(memoryParentPath, id().withTimestamp(n), processors, currentMode, currentExport);
+ if (!util::checkIfFolderExists(mPath, c->getRelativePathForMode(currentMode)))
+ {
+ return nullptr;
+ }
- std::filesystem::path subpath = p / std::to_string(n.toMicroSeconds());
- util::ensureFolderExists(subpath, false);
- auto c = std::make_shared<EntitySnapshot>(subpath, pipeline);
return c;
}
std::shared_ptr<EntitySnapshot> Entity::findLatestSnapshot() const
{
- return {};
+ Time bestMatch = Time::microSeconds(-1);
+ auto f = [&](EntitySnapshot& e) {
+ auto ts = e.id().timestamp;
+ if (ts > bestMatch)
+ {
+ bestMatch = ts;
+ }
+ };
+
+ forEachSnapshot(std::move(f));
+
+ if (bestMatch == Time::microSeconds(-1))
+ {
+ return nullptr;
+ }
+
+ return std::make_shared<EntitySnapshot>(memoryParentPath, id().withTimestamp(bestMatch), processors, currentMode, currentExport);
}
std::shared_ptr<EntitySnapshot> Entity::findLatestSnapshotBefore(const Time& time) const
{
- return {};
+ Time bestMatch = Time::microSeconds(-1);
+ auto f = [&](EntitySnapshot& e) {
+ auto ts = e.id().timestamp;
+ if (ts < time && ts > bestMatch)
+ {
+ bestMatch = ts;
+ }
+ };
+
+ forEachSnapshot(std::move(f));
+
+ if (bestMatch == Time::microSeconds(-1))
+ {
+ return nullptr;
+ }
+
+ return std::make_shared<EntitySnapshot>(memoryParentPath, id().withTimestamp(bestMatch), processors, currentMode, currentExport);
}
std::shared_ptr<EntitySnapshot> Entity::findLatestSnapshotBeforeOrAt(const Time& time) const
{
- return {};
+ Time bestMatch = Time::microSeconds(-1);
+ auto f = [&](EntitySnapshot& e) {
+ auto ts = e.id().timestamp;
+ if (ts <= time && ts > bestMatch)
+ {
+ bestMatch = ts;
+ }
+ };
+
+ forEachSnapshot(std::move(f));
+
+ if (bestMatch == Time::microSeconds(-1))
+ {
+ return nullptr;
+ }
+
+ return std::make_shared<EntitySnapshot>(memoryParentPath, id().withTimestamp(bestMatch), processors, currentMode, currentExport);
}
std::shared_ptr<EntitySnapshot> Entity::findFirstSnapshotAfter(const Time& time) const
{
- return {};
+ Time bestMatch = Time::microSeconds(std::numeric_limits<long>::max());
+ auto f = [&](EntitySnapshot& e) {
+ auto ts = e.id().timestamp;
+ if (ts > time && ts < bestMatch)
+ {
+ bestMatch = ts;
+ }
+ };
+
+ forEachSnapshot(std::move(f));
+
+ if (bestMatch == Time::microSeconds(std::numeric_limits<long>::max()))
+ {
+ return nullptr;
+ }
+
+ return std::make_shared<EntitySnapshot>(memoryParentPath, id().withTimestamp(bestMatch), processors, currentMode, currentExport);
}
std::shared_ptr<EntitySnapshot> Entity::findFirstSnapshotAfterOrAt(const Time& time) const
{
- return {};
+ Time bestMatch = Time::microSeconds(std::numeric_limits<long>::max());
+ auto f = [&](EntitySnapshot& e) {
+ auto ts = e.id().timestamp;
+ if (ts >= time && ts < bestMatch)
+ {
+ bestMatch = ts;
+ }
+ };
+
+ forEachSnapshot(std::move(f));
+
+ if (bestMatch == Time::microSeconds(std::numeric_limits<long>::max()))
+ {
+ return nullptr;
+ }
+
+ return std::make_shared<EntitySnapshot>(memoryParentPath, id().withTimestamp(bestMatch), processors, currentMode, currentExport);
}
- void Entity::_loadAll(armem::wm::Entity& e)
+ void Entity::_loadAllReferences(armem::wm::Entity& e)
{
+ auto mPath = getMemoryBasePathForMode(currentMode, currentExport);
+ auto relPath = getRelativePathForMode(currentMode);
+ if (!util::checkIfBasePathExists(mPath) || !util::checkIfFolderExists(mPath, relPath))
+ {
+ return;
+ }
+
e.id() = id();
- forEachSnapshot([&e](EntitySnapshotBase& x) {
- armem::wm::EntitySnapshot s;
- x.loadAll(s);
- e.addSnapshot(s);
+ forEachSnapshot([&e](auto& x) {
+ if (!e.hasSnapshot(x.id().timestamp)) // we only load the references if the snapshot is not existant
+ {
+ armem::wm::EntitySnapshot s;
+ x.loadAllReferences(s);
+ e.addSnapshot(s);
+ }
});
}
- void Entity::_load(armem::wm::Entity& p)
+ void Entity::_resolve(armem::wm::Entity& p)
{
- p.forEachSnapshot([this](armem::wm::EntitySnapshot& e)
+ auto mPath = getMemoryBasePathForMode(currentMode, currentExport);
+ auto relPath = getRelativePathForMode(currentMode);
+ if (!util::checkIfBasePathExists(mPath) || !util::checkIfFolderExists(mPath, relPath))
{
- util::ensureFolderExists(std::filesystem::path(path) / id().entityName / std::to_string(e.id().timestamp.toMicroSeconds()), false);
- EntitySnapshot c((std::filesystem::path(path) / id().entityName / std::to_string(e.id().timestamp.toMicroSeconds())), pipeline);
- c.load(e);
+ return;
+ }
+
+ p.forEachSnapshot([&](auto& e)
+ {
+ EntitySnapshot c(memoryParentPath, id().withTimestamp(e.id().timestamp), processors, currentMode, currentExport);
+ if (util::checkIfFolderExists(mPath, c.getRelativePathForMode(currentMode)))
+ {
+ c.resolve(e);
+ }
});
}
- void Entity::_store(const armem::wm::Entity& entity)
+ void Entity::_store(const armem::wm::Entity& c)
{
- entity.forEachSnapshot([this](armem::wm::EntitySnapshot& e)
+ if (id().entityName.empty())
+ {
+ ARMARX_WARNING << "During storage of segment '" << c.id().str() << "' I noticed that the corresponding LTM has no id set. " <<
+ "I set the id of the LTM to the same name, however this should not happen!";
+ id().entityName = c.id().entityName;
+ }
+
+ auto defaultMode = MemoryEncodingMode::FILESYSTEM;
+
+ auto defaultMPath = getMemoryBasePathForMode(defaultMode, 0);
+ auto defaultRelPath = getRelativePathForMode(defaultMode);
+ if (!util::checkIfFolderExists(defaultMPath, defaultRelPath))
+ {
+ ARMARX_WARNING << "The segment folder for segment '"+id().str()+"'was not created. I will create the folder by myself, however it seems like there is a bug in the ltm pipeline.";
+ util::ensureFolderExists(defaultMPath, defaultRelPath, true);
+ }
+
+ c.forEachSnapshot([&](const auto& snap)
{
- if (!pipeline.snapFreqFilter->accept(e))
+ auto currentMaxExport = currentExport;
+ auto encodingModeOfPast = currentMode;
+
+ for (unsigned long i = 0; i < currentMaxExport; ++i)
{
- ARMARX_WARNING << deactivateSpam() << "Ignoring to put an EntitiySnapshot into the LTM because it got filtered.";
- return;
+ MemoryEncodingMode mode = i == 0 ? defaultMode : encodingModeOfPast;
+ auto mPath = getMemoryBasePathForMode(mode, i);
+
+ EntitySnapshot c(memoryParentPath, id().withTimestamp(snap.id().timestamp), processors, mode, i);
+ if (util::checkIfFolderExists(mPath, c.getRelativePathForMode(mode)))
+ {
+ ARMARX_INFO << deactivateSpam() << "Ignoring to put an EntitiySnapshot into the LTM because the timestamp already existed (we assume snapshots are const and do not change outside the ltm).";
+ return;
+ }
}
- util::ensureFolderExists(std::filesystem::path(path) / id().entityName / std::to_string(e.id().timestamp.toMicroSeconds()));
- EntitySnapshot c((std::filesystem::path(path) / id().entityName / std::to_string(e.id().timestamp.toMicroSeconds())), pipeline);
- c.store(e);
+ for (auto& f : processors->snapFilters)
+ {
+ if (f->enabled && !f->accept(snap))
+ {
+ ARMARX_INFO << deactivateSpam() << "Ignoring to put an EntitiySnapshot into the LTM because it got filtered.";
+ return;
+ }
+ }
+
+ EntitySnapshot c(memoryParentPath, id().withTimestamp(snap.id().timestamp), processors, encodingModeOfPast, currentMaxExport);
+ util::ensureFolderExists(defaultMPath, c.getRelativePathForMode(defaultMode));
+ c.store(snap);
});
}
-
}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/Entity.h b/source/RobotAPI/libraries/armem/server/ltm/disk/Entity.h
index 3e0414339218e4d58173597004fecb36c899f059..759b22733612c700ec0461541976cf3872d9df07 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/Entity.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/Entity.h
@@ -16,7 +16,7 @@ namespace armarx::armem::server::ltm::disk
public DiskMemoryItem
{
public:
- Entity(const std::filesystem::path&, const LongTermMemoryPipeline& p);
+ Entity(const std::filesystem::path& parentPath, const MemoryID& id, const std::shared_ptr<Processors>& p, const DiskMemoryItem::MemoryEncodingMode mode, const unsigned long e);
bool forEachSnapshot(std::function<void(EntitySnapshot&)>&& func) const override;
bool forEachSnapshotInIndexRange(long first, long last, std::function<void(EntitySnapshot&)>&& func) const override;
@@ -32,12 +32,13 @@ namespace armarx::armem::server::ltm::disk
std::shared_ptr<EntitySnapshot> findFirstSnapshotAfterOrAt(const Time& time) const override;
protected:
- void _loadAll(armem::wm::Entity&) override;
- void _load(armem::wm::Entity&) override;
+ void _loadAllReferences(armem::wm::Entity&) override;
+ void _resolve(armem::wm::Entity&) override;
void _store(const armem::wm::Entity&) override;
- std::string getExpectedFolderName() const override;
-
+ private:
+ MemoryEncodingMode currentMode;
+ unsigned long currentExport;
};
} // namespace armarx::armem::server::ltm::disk
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/EntitySnapshot.cpp b/source/RobotAPI/libraries/armem/server/ltm/disk/EntitySnapshot.cpp
index f34f2d11b01a7f0678cbc33ae478730421b539e8..a4aece1fdc87714ffa91452d44ef573dc31b3e1d 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/EntitySnapshot.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/EntitySnapshot.cpp
@@ -12,61 +12,29 @@
namespace armarx::armem::server::ltm::disk
{
-
- namespace
- {
- MemoryID getMemoryIDFromPath(const std::filesystem::path& p)
- {
- util::ensureFolderExists(p);
-
- MemoryID m;
- m.memoryName = p.parent_path().parent_path().parent_path().parent_path().filename();
- m.coreSegmentName = p.parent_path().parent_path().parent_path().filename();
- m.providerSegmentName = p.parent_path().parent_path().filename();
- m.entityName = p.parent_path().filename();
- m.timestamp = IceUtil::Time::microSeconds(std::stol(p.filename()));
- return m;
- }
-
- void writeDataToFile(const std::filesystem::path& path, const std::vector<unsigned char>& data)
- {
- std::ofstream dataofs;
- dataofs.open(path);
- dataofs.write(reinterpret_cast<const char*>(data.data()), data.size());
- dataofs.close();
- }
-
- std::vector<unsigned char> readDataFromFile(const std::filesystem::path path)
- {
- std::ifstream dataifs(path);
- std::vector<unsigned char> datafilecontent((std::istreambuf_iterator<char>(dataifs)), (std::istreambuf_iterator<char>()));
- return datafilecontent;
- }
- }
-
- EntitySnapshot::EntitySnapshot(const std::filesystem::path& p, const LongTermMemoryPipeline& pipe) :
- EntitySnapshotBase(getMemoryIDFromPath(p), pipe),
- DiskMemoryItem(p.parent_path())
- {
-
- }
-
- std::string EntitySnapshot::getExpectedFolderName() const
+ EntitySnapshot::EntitySnapshot(const std::filesystem::path& p, const MemoryID& id, const std::shared_ptr<Processors>& filters, const DiskMemoryItem::MemoryEncodingMode mode, const unsigned long e) :
+ EntitySnapshotBase(id, filters),
+ DiskMemoryItem(p, EscapeSegmentName(id.memoryName), std::filesystem::path(EscapeSegmentName(id.coreSegmentName)) / EscapeSegmentName(id.providerSegmentName) / EscapeSegmentName(id.entityName) / std::to_string(id.timestamp.toSeconds() / 3600 /* hours */) / std::to_string(id.timestamp.toSeconds()) / id.timestampStr()),
+ currentMode(mode),
+ currentExport(e)
{
- return name();
}
- void EntitySnapshot::_loadAll(armem::wm::EntitySnapshot& e) const
+ void EntitySnapshot::_loadAllReferences(armem::wm::EntitySnapshot& e) const
{
- std::filesystem::path p = path;
- p = p / id().timestampStr();
- util::ensureFolderExists(p, false);
+ auto mPath = getMemoryBasePathForMode(currentMode, currentExport);
+ auto relPath = getRelativePathForMode(currentMode);
+ if (!util::checkIfBasePathExists(mPath) || !util::checkIfFolderExists(mPath, relPath))
+ {
+ return;
+ }
e.id() = id();
for (unsigned int i = 0; i < 1000; ++i) // 1000 is max size for instances in a single timestamp
{
- if (!util::checkIfFolderExists(p / std::to_string(i)))
+ std::filesystem::path relIPath = relPath / std::to_string(i) / "";
+ if (!util::checkIfFolderExists(mPath, relIPath))
{
break;
}
@@ -76,95 +44,176 @@ namespace armarx::armem::server::ltm::disk
}
}
- void EntitySnapshot::_load(armem::wm::EntitySnapshot& e) const
+ void EntitySnapshot::_resolve(armem::wm::EntitySnapshot& e) const
{
- // Get data from disk
- std::filesystem::path p = path;
- p = p / id().timestampStr();
- util::ensureFolderExists(p, false);
-
- for (unsigned int i = 0; i < e.size(); ++i)
+ auto mPath = getMemoryBasePathForMode(currentMode, currentExport);
+ auto relPath = getRelativePathForMode(currentMode);
+ if (!util::checkIfBasePathExists(mPath) || !util::checkIfFolderExists(mPath, relPath))
{
- util::ensureFolderExists(p / std::to_string(i), false);
+ return;
+ }
- std::filesystem::path data = p / std::to_string(i) / (constantes::DATA_FILENAME + pipeline.dictConverter->suffix);
- std::filesystem::path metadata = p / std::to_string(i) / (constantes::METADATA_FILENAME + pipeline.dictConverter->suffix);
+ auto& dictConverter = processors->defaultObjectConverter;
- util::ensureFileExists(data);
+ // Get data from disk
+ for (unsigned int i = 0; i < e.size(); ++i)
+ {
+ std::filesystem::path relIPath = relPath / std::to_string(i) / "";
+ if (util::checkIfFolderExists(mPath, relIPath))
+ {
+ std::string dataFilename = (constantes::DATA_FILENAME + dictConverter.suffix);
+ std::string metadataFilename = (constantes::METADATA_FILENAME + dictConverter.suffix);
+ std::filesystem::path relDataPath = relIPath / dataFilename;
+ std::filesystem::path relMetadataPath = relIPath / metadataFilename;
- auto& ins = e.getInstance(i);
+ auto& ins = e.getInstance(i);
+ aron::data::DictPtr datadict = nullptr;
+ aron::data::DictPtr metadatadict = nullptr;
- auto datafilecontent = readDataFromFile(data);
- auto dataaron = pipeline.dictConverter->convert(datafilecontent);
+ // get list of all files. This ensures that we only have to call fs::exists once for each file
+ auto allFilesInIndexFolder = util::getAllFiles(mPath, relIPath);
- // check for special members
- for (const auto& [key, m] : dataaron->getElements())
- {
- if (!m)
+ if (std::find(allFilesInIndexFolder.begin(), allFilesInIndexFolder.end(), dataFilename) != allFilesInIndexFolder.end())
{
- // check for image
- std::filesystem::path member = p / std::to_string(i) / (key + pipeline.imgConverter->suffix);
- if (std::filesystem::exists(member) && std::filesystem::is_regular_file(member))
+ auto datafilecontent = util::readDataFromFile(mPath, relDataPath);
+ auto dataaron = dictConverter.convert(datafilecontent, "");
+ datadict = aron::data::Dict::DynamicCastAndCheck(dataaron);
+
+ // check for special members
+ for (const auto& [key, m] : datadict->getElements())
{
- auto memberfilecontent = readDataFromFile(member);
- auto memberaron = pipeline.imgConverter->convert(memberfilecontent);
- dataaron->setElement(key, memberaron);
+ for (auto& [t, f] : processors->converters)
+ {
+ for (const auto& filename : allFilesInIndexFolder) // iterate over all files and search for matching ones
+ {
+ if (simox::alg::starts_with(filename, key) and simox::alg::ends_with(filename, f->suffix))
+ {
+ std::filesystem::path relMemberPath = relIPath / filename;
+ std::string mode = simox::alg::remove_suffix(simox::alg::remove_prefix(filename, key), f->suffix);
+
+ auto memberfilecontent = util::readDataFromFile(mPath, relMemberPath);
+ auto memberaron = f->convert(memberfilecontent, mode);
+ datadict->setElement(key, memberaron);
+ break;
+ }
+ }
+ }
}
}
- }
+ else
+ {
+ ARMARX_ERROR << "Could not find the data file '" << relDataPath.string() << "'. Continuing without data.";
+ }
- auto metadatafilecontent = readDataFromFile(metadata);
- auto metadataaron = pipeline.dictConverter->convert(metadatafilecontent);
+ if (std::find(allFilesInIndexFolder.begin(), allFilesInIndexFolder.end(), metadataFilename) != allFilesInIndexFolder.end())
+ {
+ auto metadatafilecontent = util::readDataFromFile(mPath, relMetadataPath);
+ auto metadataaron = dictConverter.convert(metadatafilecontent, "");
+ metadatadict = aron::data::Dict::DynamicCastAndCheck(metadataaron);
+ }
+ else
+ {
+ ARMARX_ERROR << "Could not find the metadata file '" << relMetadataPath.string() << "'. Continuing without metadata.";
+ }
- from_aron(metadataaron, dataaron, ins);
+ from_aron(metadatadict, datadict, ins);
+ }
+ else
+ {
+ ARMARX_WARNING << "Could not find the index segment folder for segment '" << e.id().str() << "'.";
+ }
}
}
void EntitySnapshot::_store(const armem::wm::EntitySnapshot& e) const
{
- std::filesystem::path p = path;
- p = p / id().timestampStr();
- util::ensureFolderExists(p);
+ //auto currentMaxExport = currentExport;
+ //auto encodingModeOfPast = currentMode;
+
+ if (id().timestampStr().empty())
+ {
+ ARMARX_WARNING << "During storage of segment '" << e.id().str() << "' I noticed that the corresponding LTM has no id set. " <<
+ "I set the id of the LTM to the same name, however this should not happen!";
+ id().timestamp = e.id().timestamp;
+ }
+
+ auto defaultMode = MemoryEncodingMode::FILESYSTEM;
+
+ auto& dictConverter = processors->defaultObjectConverter;
+
+ auto defaultMPath = getMemoryBasePathForMode(defaultMode, 0);
+ auto defaultRelPath = getRelativePathForMode(defaultMode);
+ if (!util::checkIfFolderExists(defaultMPath, defaultRelPath))
+ {
+ ARMARX_WARNING << "The segment folder for segment '"+id().str()+"'was not created. I will create the folder by myself, however it seems like there is a bug in the ltm pipeline.";
+ util::ensureFolderExists(defaultMPath, defaultRelPath, true);
+ }
for (unsigned int i = 0; i < e.size(); ++i)
{
- std::filesystem::path instancePath = p / std::to_string(i);
- util::ensureFolderExists(instancePath);
+ std::filesystem::path defaultRelIPath = defaultRelPath / std::to_string(i) / "";
- std::filesystem::path dataPath = instancePath / (constantes::DATA_FILENAME + pipeline.dictConverter->suffix);
- std::filesystem::path metadataPath = instancePath / (constantes::METADATA_FILENAME + pipeline.dictConverter->suffix);
+ // For performance reasons we skip to check whether the index folder already exists somewhere.
+ // We already check if the ts exists on entity level.
- if (util::checkIfFileExists(dataPath) or util::checkIfFileExists(metadataPath))
+ if (!util::checkIfFolderExists(defaultMPath, defaultRelIPath))
{
- continue;
- }
+ util::ensureFolderExists(defaultMPath, defaultRelIPath);
- auto& ins = e.getInstance(i);
+ std::filesystem::path relDataPath = defaultRelIPath / (constantes::DATA_FILENAME + dictConverter.suffix);
+ std::filesystem::path relMetadataPath = defaultRelIPath / (constantes::METADATA_FILENAME + dictConverter.suffix);
- // data
- auto dataAron = std::make_shared<aron::data::Dict>();
- auto metadataAron = std::make_shared<aron::data::Dict>();
- to_aron(metadataAron, dataAron, ins);
+ auto& ins = e.getInstance(i);
- // extract
- auto dataExt = pipeline.imageExtractor->extract(dataAron);
- auto metadataExt = pipeline.imageExtractor->extract(metadataAron); // useless
+ // data
+ auto dataAron = std::make_shared<aron::data::Dict>();
+ auto metadataAron = std::make_shared<aron::data::Dict>();
+ to_aron(metadataAron, dataAron, ins);
- // convert images
- for (const auto& [memberName, var] : dataExt.extraction)
- {
- ARMARX_CHECK_NOT_NULL(var);
- auto img = aron::data::NDArray::DynamicCastAndCheck(var);
- std::filesystem::path imgPath = instancePath / (memberName + pipeline.imgConverter->suffix);
- auto imgVec = pipeline.imgConverter->convert(img);
- writeDataToFile(imgPath, imgVec);
- }
+ // check special members for special extractions
+ for (auto& x : processors->extractors)
+ {
+ if (!x->enabled) continue;
+
+ const auto& t = x->identifier;
+
+ Converter* conv = nullptr; // find suitable converter
+ for (const auto& [ct, c] : processors->converters)
+ {
+ if (!c->enabled) continue;
+ if (t != ct) continue;
+ conv = c;
+ }
+
+ if (conv)
+ {
+ auto dataExt = x->extract(dataAron);
- // convert dict and metadata
- auto dataVec = pipeline.dictConverter->convert(dataExt.dataWithoutExtraction);
- auto metadataVec = pipeline.dictConverter->convert(metadataExt.dataWithoutExtraction);
- writeDataToFile(dataPath, dataVec);
- writeDataToFile(metadataPath, metadataVec);
+ for (const auto& [memberName, var] : dataExt.extraction)
+ {
+ ARMARX_CHECK_NOT_NULL(var);
+
+ auto [memberDataVec, memberDataModeSuffix] = conv->convert(var);
+ std::filesystem::path relMemberPath = defaultRelIPath / (memberName + memberDataModeSuffix + conv->suffix);
+
+ util::writeDataToFileRepeated(defaultMPath, relMemberPath, memberDataVec);
+ }
+
+ dataAron = dataExt.dataWithoutExtraction;
+ }
+ // else we could not convert the extracted data so it makes no sense to extract it at all...
+ }
+
+ // convert dict and metadata
+ auto [dataVec, dataVecModeSuffix] = dictConverter.convert(dataAron);
+ auto [metadataVec, metadataVecModeSuffix] = dictConverter.convert(metadataAron);
+ ARMARX_CHECK_EMPTY(dataVecModeSuffix);
+ ARMARX_CHECK_EMPTY(metadataVecModeSuffix);
+
+ util::writeDataToFileRepeated(defaultMPath, relDataPath, dataVec);
+ util::writeDataToFileRepeated(defaultMPath, relMetadataPath, metadataVec);
+ }
+ // Ignore if the full index already exists. Actually this should not happen since the existence of the ts folder is checked on entity level
}
}
}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/EntitySnapshot.h b/source/RobotAPI/libraries/armem/server/ltm/disk/EntitySnapshot.h
index 690d948251099033b453d37f0d72a89cc3df5e80..b91c69b78dca573935b7d51c665ca67e6b499fcc 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/EntitySnapshot.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/EntitySnapshot.h
@@ -14,15 +14,16 @@ namespace armarx::armem::server::ltm::disk
public DiskMemoryItem
{
public:
- EntitySnapshot(const std::filesystem::path&, const LongTermMemoryPipeline& p);
+ EntitySnapshot(const std::filesystem::path& parentPath, const MemoryID& id, const std::shared_ptr<Processors>& p, const DiskMemoryItem::MemoryEncodingMode mode, const unsigned long e);
protected:
- void _loadAll(armem::wm::EntitySnapshot&) const override;
- void _load(armem::wm::EntitySnapshot&) const override;
+ void _loadAllReferences(armem::wm::EntitySnapshot&) const override;
+ void _resolve(armem::wm::EntitySnapshot&) const override;
void _store(const armem::wm::EntitySnapshot&) const override;
- std::string getExpectedFolderName() const override;
-
+ private:
+ MemoryEncodingMode currentMode;
+ unsigned long currentExport;
};
} // namespace armarx::armem::server::ltm::disk
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/Memory.cpp b/source/RobotAPI/libraries/armem/server/ltm/disk/Memory.cpp
index d2e5bad13a6ca55b08aaa4256559550a552e54bb..0eccbbf1a4435d301e05fea5ea0f64d2abf6eef0 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/Memory.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/Memory.cpp
@@ -5,155 +5,194 @@
#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
-// ArmarX
-#include "../base/filter/frequencyFilter/FrequencyFilter.h"
-#include "../base/extractor/noExtractor/NoExtractor.h"
-#include "../base/extractor/imageExtractor/ImageExtractor.h"
-#include "../base/converter/dict/json/JsonConverter.h"
-#include "../base/converter/dict/bson/BsonConverter.h"
-#include "../base/converter/image/png/PngConverter.h"
-
namespace armarx::armem::server::ltm::disk
{
- namespace
- {
- MemoryID getMemoryIDFromPath(const std::filesystem::path& p)
- {
- ARMARX_CHECK(!p.empty());
-
- MemoryID m;
- m.memoryName = p.filename();
- return m;
- }
-
- std::filesystem::path getDefaultStoragePath()
- {
- /*std::string armarx_home = std::string(getenv("HOME")) + "/.armarx";
- if (getenv("ARMARX_DEFAULTS_DIR"))
- {
- armarx_home = getenv("ARMARX_DEFAULTS_DIR");
- }
- path = armarx_home + "/armem/disk/data/db";*/
- return "/tmp/MemoryExport/Test";
- }
- }
-
void Memory::createPropertyDefinitions(PropertyDefinitionsPtr& properties, const std::string& prefix)
{
Base::createPropertyDefinitions(properties, prefix);
- properties->optional(path, prefix + "storagepath", "The path to the memory storage.");
+ properties->optional(memoryParentPath, prefix + "storagepath", "The path to the memory storage (the memory will be stored in a seperate subfolder).");
+ properties->optional(sizeToCompressDataInMegaBytes, prefix + "sizeToCompressDataInMegaBytes", "The size in MB to compress away the current export. Exports are numbered (lower number means newer).");
}
Memory::Memory() :
- Memory(getDefaultStoragePath())
+ Memory(std::filesystem::path("/tmp/MemoryExport"), "Test")
{
}
- Memory::Memory(const std::filesystem::path& p) :
- Base(getMemoryIDFromPath(p)),
- DiskMemoryItem(p.parent_path())
+ Memory::Memory(const std::filesystem::path& p, const std::string& memoryName /* UNESCAPED */, const MemoryEncodingMode defaultEncodingMode) :
+ Base(MemoryID(memoryName, "")),
+ DiskMemoryItem(p, EscapeSegmentName(memoryName), ""),
+ exportEncodingMode(defaultEncodingMode)
{
+ setMemoryID(MemoryID(memoryName, "")); // set Memory id and search for old exports
}
void Memory::setMemoryID(const MemoryID& id)
{
ARMARX_CHECK_NOT_EMPTY(id.memoryName);
-
Base::setMemoryID(id.getMemoryID());
- buffer.id() = id.getMemoryID();
- }
+ escapedMemoryName = EscapeSegmentName(id.memoryName);
- std::string Memory::getExpectedFolderName() const
- {
- return id().memoryName;
+ // Discover how many exports already exists
+ for (unsigned long i = 1; i < 10000; ++i)
+ {
+ std::filesystem::path exportPath = getMemoryBasePathForMode(exportEncodingMode, i);
+ if (!std::filesystem::exists(exportPath))
+ {
+ break;
+ }
+ maxExportIndex = i;
+ }
}
bool Memory::forEachCoreSegment(std::function<void(CoreSegment&)>&& func) const
{
- if (!checkPath(id().memoryName))
+ for (unsigned long i = 0; i <= maxExportIndex; ++i)
{
- return false;
+ MemoryEncodingMode mode = i == 0 ? MemoryEncodingMode::FILESYSTEM : exportEncodingMode;
+ auto mPath = getMemoryBasePathForMode(mode, i);
+ if (util::checkIfBasePathExists(mPath))
+ {
+ for (const auto& subdirName : util::getAllDirectories(mPath, getRelativePathForMode(mode)))
+ {
+ std::string segmentName = UnescapeSegmentName(subdirName);
+ CoreSegment c(memoryParentPath, id().withCoreSegmentName(segmentName), processors, mode, i);
+ func(c);
+ }
+ }
}
- std::filesystem::path p = path;
- p = p / id().memoryName;
- util::ensureFolderExists(p, false);
-
- for (const auto& subdir : std::filesystem::directory_iterator(p))
- {
- std::filesystem::path subdirPath = subdir.path();
- CoreSegment c(subdirPath, pipeline);
- func(c);
- }
return true;
}
- std::shared_ptr<CoreSegment> Memory::findCoreSegment(const std::string& n) const
+ std::shared_ptr<CoreSegment> Memory::findCoreSegment(const std::string& coreSegmentName) const
{
- if (!checkPath(id().memoryName))
+ for (unsigned long i = 0; i <= maxExportIndex; ++i)
{
- return {};
- }
+ MemoryEncodingMode mode = i == 0 ? MemoryEncodingMode::FILESYSTEM : exportEncodingMode;
+
+ auto mPath = getMemoryBasePathForMode(mode, i);
+ if (!util::checkIfBasePathExists(mPath))
+ {
+ continue;
+ }
- std::filesystem::path p = path;
- p = p / id().memoryName;
- util::ensureFolderExists(p, false);
+ auto c = std::make_shared<CoreSegment>(memoryParentPath, id().withCoreSegmentName(coreSegmentName), processors, mode, i);
+ if (!util::checkIfFolderExists(mPath, c->getRelativePathForMode(mode)))
+ {
+ continue;
+ }
- std::filesystem::path subpath = p / n;
- util::ensureFolderExists(subpath, false);
- auto c = std::make_shared<CoreSegment>(subpath, pipeline);
- return c;
+ return c;
+ }
+ return nullptr;
}
- void Memory::_loadAll(armem::wm::Memory& m)
+ void Memory::_loadAllReferences(armem::wm::Memory& m)
{
m.id() = id();
- forEachCoreSegment([&m](CoreSegment& x) {
+ forEachCoreSegment([&m](auto& x) {
armem::wm::CoreSegment s;
- x.loadAll(s);
+ x.loadAllReferences(s);
m.addCoreSegment(s);
});
}
- void Memory::_load(armem::wm::Memory& m)
+ void Memory::_resolve(armem::wm::Memory& m)
{
- if (!checkPath(id().memoryName))
- {
- return;
- }
+ std::lock_guard l(ltm_mutex); // we cannot load a memory multiple times simultaneously
- std::lock_guard l(ltm_mutex);
- m.forEachCoreSegment([this](armem::wm::CoreSegment& e)
+ for (unsigned long i = 0; i <= maxExportIndex; ++i)
{
- util::ensureFolderExists(std::filesystem::path(path) / id().memoryName / e.id().coreSegmentName, false);
- CoreSegment c((std::filesystem::path(path) / id().memoryName / e.id().coreSegmentName), pipeline);
- c.load(e);
- });
+ MemoryEncodingMode mode = i == 0 ? MemoryEncodingMode::FILESYSTEM : exportEncodingMode;
+
+ auto mPath = getMemoryBasePathForMode(mode, i);
+ if (!util::checkIfBasePathExists(mPath))
+ {
+ continue;
+ }
+
+ m.forEachCoreSegment([&](auto& e)
+ {
+ CoreSegment c(memoryParentPath, id().withCoreSegmentName(e.id().coreSegmentName), processors, mode, i);
+ if (util::checkIfFolderExists(mPath, c.getRelativePathForMode(mode)))
+ {
+ c.resolve(e);
+ }
+ });
+ }
}
void Memory::_directlyStore(const armem::wm::Memory& memory)
{
- if (!checkPath(id().memoryName))
+ std::lock_guard l(ltm_mutex); // we cannot store a memory multiple times simultaneously
+
+ MemoryEncodingMode defaultMode = MemoryEncodingMode::FILESYSTEM;
+ MemoryEncodingMode encodeModeOfPast = exportEncodingMode;
+ // Storage will always be in filesystem mode!
+ // Somehow, minizip was not able to write data to images. It always created a folder named xyz.png without any data in it...
+ // Another problem is that storing data directly in compressed format will require a lot of time when the compressed file is big (>20MB)
+
+ if (id().memoryName.empty())
{
- return;
+ ARMARX_WARNING << "During storage of memory '" << memory.id().str() << "' I noticed that the corresponding LTM has no id set. " <<
+ "I set the id of the LTM to the same name, however this should not happen!";
+ setMemoryID(memory.id());
}
- std::lock_guard l(ltm_mutex);
+ auto defaultMPath = getMemoryBasePathForMode(defaultMode, 0);
- memory.forEachCoreSegment([this](const auto& core)
+ // Check if we have to compress the memory!
+ // See above mentioned issues with directly compressing the data. Therefore we store data in plain text and compress from time to time
+ // using system calls. Also increase the index of all old exports
+ auto size = filesystem::util::getSizeOfDirectory(defaultMPath);
+ //std::cout << "Current maxExportIndex is: " << maxExportIndex << std::endl;
+ if (size >= (sizeToCompressDataInMegaBytes * 1024 * 1024))
{
- util::ensureFolderExists(std::filesystem::path(path) / id().memoryName / core.id().coreSegmentName);
- CoreSegment c((std::filesystem::path(path) / id().memoryName / core.id().coreSegmentName), pipeline);
+ ARMARX_INFO << "Compressen of memory " + id().memoryName + " needed because the size of last export is " + std::to_string(size / 1024.f / 1024.f) + " (>= " + std::to_string(sizeToCompressDataInMegaBytes) + ")";
+
+ // increase index of old memories
+ for (unsigned long i = maxExportIndex; i >= 1; --i)
+ {
+ ARMARX_INFO << "Increasing the index of old compressed memory " + id().memoryName + " (" + std::to_string(i) + " to " + std::to_string(i+1) + ")";
+ auto exportPath = getMemoryBasePathForMode(encodeModeOfPast, i);
+ auto newExportPath = getMemoryBasePathForMode(encodeModeOfPast, i+1);
+ std::string moveCommand = "mv " + exportPath.string() + " " + newExportPath.string();
+ //std::cout << "Exec command: " << moveCommand << std::endl;
+ int ret = system(moveCommand.c_str());
+ (void) ret;
+ }
+
+ // zip away current export
+ ARMARX_INFO << "Compressing the last export of " + id().memoryName;
+ auto newExportPath = getMemoryBasePathForMode(encodeModeOfPast, 1); // 1 will be the new export
+ std::string zipCommand = "cd " + memoryParentPath.string() + " && zip -r " + newExportPath.string() + " " + escapedMemoryName;
+ //std::cout << "Exec command: " << zipCommand << std::endl;
+ int ret = system(zipCommand.c_str());
+ (void) ret;
+
+ // remove unzipped memory export
+ ARMARX_INFO << "Removing the last export of " + id().memoryName;
+ std::string rmCommand = "rm -r " + defaultMPath.string();
+ ret = system(rmCommand.c_str());
+ (void) ret;
+
+ maxExportIndex++;
+ }
+
+ util::ensureBasePathExists(defaultMPath, true); // create if not exists
+
+ memory.forEachCoreSegment([&](const auto& core)
+ {
+ CoreSegment c(memoryParentPath, id().withCoreSegmentName(core.id().coreSegmentName), processors, encodeModeOfPast, 0 /* how far to look back in past on enity level. For full lookup use maxExportIndex. */);
+ util::ensureFolderExists(defaultMPath, c.getRelativePathForMode(defaultMode), true); // create subfolder
+
+ // 1. store type
+ c.storeType(core);
+
+ // 2. store data
c.store(core);
});
}
-
- void Memory::setPath(const std::string& ps)
- {
- std::filesystem::path p = ps;
- DiskMemoryItem::setPath(p.parent_path());
- setMemoryID(getMemoryIDFromPath(p));
- }
-
}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/Memory.h b/source/RobotAPI/libraries/armem/server/ltm/disk/Memory.h
index fc0fd793d90817cf97c83aa35515e6f773650235..085a56459efd9118d1c9afa4f7bd85f64f6ba38f 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/Memory.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/Memory.h
@@ -21,23 +21,26 @@ namespace armarx::armem::server::ltm::disk
using Base = BufferedMemoryBase<CoreSegment>;
Memory();
- Memory(const std::filesystem::path&);
+ Memory(const std::filesystem::path&, const std::string&, const MemoryEncodingMode defaultEncodingMode = MemoryEncodingMode::MINIZIP);
void setMemoryID(const MemoryID& id) override;
void createPropertyDefinitions(PropertyDefinitionsPtr& properties, const std::string& prefix) override;
bool forEachCoreSegment(std::function<void(CoreSegment&)>&& func) const override;
- std::shared_ptr<CoreSegment> findCoreSegment(const std::string&) const override;
-
- void setPath(const std::string&) override;
+ std::shared_ptr<CoreSegment> findCoreSegment(const std::string& coreSegmentName) const override;
protected:
- void _loadAll(armem::wm::Memory&) override;
- void _load(armem::wm::Memory&) override;
+ void _loadAllReferences(armem::wm::Memory&) override;
+ void _resolve(armem::wm::Memory&) override;
void _directlyStore(const armem::wm::Memory&) override;
- std::string getExpectedFolderName() const override;
+ private:
+ MemoryEncodingMode exportEncodingMode = MemoryEncodingMode::MINIZIP;
+ unsigned long maxExportIndex = 0;
+ unsigned long sizeToCompressDataInMegaBytes = long(1) * 1024; // 1GB
+ public:
+ static const int DEPTH_TO_DATA_FILES = 7; // from memory folder = 1 (cseg) + 1 (pseg) + 1 (ent) + 3 (snap) + 1 (inst)
};
} // namespace armarx::armem::server::ltm::disk
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/ProviderSegment.cpp b/source/RobotAPI/libraries/armem/server/ltm/disk/ProviderSegment.cpp
index 1645f9b1275a24220f67e243dd5bcde88e860586..df52832d1bfdc35e4f19a35c220b2c232a97bdc5 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/ProviderSegment.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/ProviderSegment.cpp
@@ -10,96 +10,154 @@
namespace armarx::armem::server::ltm::disk
{
- namespace
- {
- MemoryID getMemoryIDFromPath(const std::filesystem::path& p)
- {
- util::ensureFolderExists(p);
-
- MemoryID m;
- m.memoryName = p.parent_path().parent_path().filename();
- m.coreSegmentName = p.parent_path().filename();
- m.providerSegmentName = p.filename();
- return m;
- }
- }
-
- ProviderSegment::ProviderSegment(const std::filesystem::path& p, const LongTermMemoryPipeline& pipe) :
- ProviderSegmentBase(getMemoryIDFromPath(p), pipe),
- DiskMemoryItem(p.parent_path())
+ ProviderSegment::ProviderSegment(const std::filesystem::path& p, const MemoryID& id, const std::shared_ptr<Processors>& filters, const DiskMemoryItem::MemoryEncodingMode mode, const unsigned long e) :
+ ProviderSegmentBase(id, filters),
+ DiskMemoryItem(p, EscapeSegmentName(id.memoryName), std::filesystem::path(EscapeSegmentName(id.coreSegmentName)) / EscapeSegmentName(id.providerSegmentName)),
+ currentMode(mode),
+ currentExport(e)
{
}
bool ProviderSegment::forEachEntity(std::function<void(Entity&)>&& func) const
{
- if (!checkPath(id().providerSegmentName))
+ auto mPath = getMemoryBasePathForMode(currentMode, currentExport);
+ auto relPath = getRelativePathForMode(currentMode);
+ if (!util::checkIfBasePathExists(mPath) || !util::checkIfFolderExists(mPath, relPath))
{
return false;
}
- std::filesystem::path p = path;
- p = p / id().providerSegmentName;
- util::ensureFolderExists(p, false);
-
- for (const auto& subdir : std::filesystem::directory_iterator(p))
+ for (const auto& subdirName : util::getAllDirectories(mPath, relPath))
{
- std::filesystem::path subdirPath = subdir.path();
- Entity c(subdirPath, pipeline);
+ std::string segmentName = UnescapeSegmentName(subdirName);
+ Entity c(memoryParentPath, id().withEntityName(subdirName), processors, currentMode, currentExport);
func(c);
}
return true;
}
- std::shared_ptr<Entity> ProviderSegment::findEntity(const std::string& n) const
+ std::shared_ptr<Entity> ProviderSegment::findEntity(const std::string& entityName) const
{
- if (!checkPath(id().providerSegmentName))
+ auto mPath = getMemoryBasePathForMode(currentMode, currentExport);
+ auto relPath = getRelativePathForMode(currentMode);
+ if (!util::checkIfBasePathExists(mPath) || !util::checkIfFolderExists(mPath, relPath))
{
- return {};
+ return nullptr;
}
- std::filesystem::path p = path;
- p = p / id().providerSegmentName;
- util::ensureFolderExists(p, false);
+ auto c = std::make_shared<Entity>(memoryParentPath, id().withEntityName(entityName), processors, currentMode, currentExport);
+ if (!util::checkIfFolderExists(mPath, c->getRelativePathForMode(currentMode)))
+ {
+ return nullptr;
+ }
- std::filesystem::path subpath = p / n;
- util::ensureFolderExists(subpath, false);
- auto c = std::make_shared<Entity>(subpath, pipeline);
return c;
}
- std::string ProviderSegment::getExpectedFolderName() const
+ void ProviderSegment::_loadAllReferences(armem::wm::ProviderSegment& e)
{
- return name();
- }
+ auto mPath = getMemoryBasePathForMode(currentMode, currentExport);
+ auto relPath = getRelativePathForMode(currentMode);
+ if (!util::checkIfBasePathExists(mPath) || !util::checkIfFolderExists(mPath, relPath))
+ {
+ return;
+ }
- void ProviderSegment::_loadAll(armem::wm::ProviderSegment& e)
- {
e.id() = id();
- forEachEntity([&e](Entity& x) {
+ auto& conv = processors->defaultTypeConverter;
+ auto relTPath = relPath / (constantes::TYPE_FILENAME + conv.suffix);
+
+ if (util::checkIfFileExists(mPath, relTPath))
+ {
+ auto typeFileContent = util::readDataFromFile(mPath, relTPath);
+ auto typeAron = conv.convert(typeFileContent, "");
+ e.aronType() = aron::type::Object::DynamicCastAndCheck(typeAron);
+ }
+
+ forEachEntity([&e](auto& x) {
armem::wm::Entity s;
- x.loadAll(s);
+ x.loadAllReferences(s);
e.addEntity(s);
});
}
- void ProviderSegment::_load(armem::wm::ProviderSegment& p)
+ void ProviderSegment::_resolve(armem::wm::ProviderSegment& p)
{
- p.forEachEntity([this](armem::wm::Entity& e)
+ auto mPath = getMemoryBasePathForMode(currentMode, currentExport);
+ auto relPath = getRelativePathForMode(currentMode);
+ if (!util::checkIfBasePathExists(mPath) || !util::checkIfFolderExists(mPath, relPath))
{
- util::ensureFolderExists(std::filesystem::path(path) / id().providerSegmentName / e.id().entityName, false);
- Entity c((std::filesystem::path(path) / id().providerSegmentName / e.id().entityName), pipeline);
- c.load(e);
+ return;
+ }
+
+ p.forEachEntity([&](auto& e)
+ {
+ Entity c(memoryParentPath, id().withEntityName(e.id().entityName), processors, currentMode, currentExport);
+ if (util::checkIfFolderExists(mPath, c.getRelativePathForMode(currentMode)))
+ {
+ c.resolve(e);
+ }
});
}
- void ProviderSegment::_store(const armem::wm::ProviderSegment& providerSegment)
+ void ProviderSegment::_store(const armem::wm::ProviderSegment& p)
{
- providerSegment.forEachEntity([this](const auto& entity)
+ auto currentMaxExport = currentExport;
+ auto encodingModeOfPast = currentMode;
+
+ if (id().providerSegmentName.empty())
{
- util::ensureFolderExists(std::filesystem::path(path) / id().providerSegmentName / entity.id().entityName);
- Entity c((std::filesystem::path(path) / id().providerSegmentName / entity.id().entityName), pipeline);
- c.store(entity);
+ ARMARX_WARNING << "During storage of segment '" << p.id().str() << "' I noticed that the corresponding LTM has no id set. " <<
+ "I set the id of the LTM to the same name, however this should not happen!";
+ id().providerSegmentName = p.id().providerSegmentName;
+ }
+
+ auto defaultMode = MemoryEncodingMode::FILESYSTEM;
+
+ auto defaultMPath = getMemoryBasePathForMode(defaultMode, 0);
+ auto defaultRelPath = getRelativePathForMode(defaultMode);
+ if (!util::checkIfFolderExists(defaultMPath, defaultRelPath))
+ {
+ ARMARX_WARNING << "The segment folder for segment '"+id().str()+"'was not created. I will create the folder by myself, however it seems like there is a bug in the ltm pipeline.";
+ util::ensureFolderExists(defaultMPath, defaultRelPath, true);
+ }
+
+ p.forEachEntity([&](const auto& e)
+ {
+ Entity c(memoryParentPath, id().withEntityName(e.id().entityName), processors, encodingModeOfPast, currentMaxExport);
+ util::ensureFolderExists(defaultMPath, c.getRelativePathForMode(defaultMode), true);
+ c.store(e);
});
}
+
+ void ProviderSegment::_storeType(const armem::wm::ProviderSegment& p)
+ {
+ if (id().providerSegmentName.empty())
+ {
+ ARMARX_WARNING << "During storage of segment '" << p.id().str() << "' I noticed that the corresponding LTM has no id set. " <<
+ "I set the id of the LTM to the same name, however this should not happen!";
+ id().providerSegmentName = p.id().providerSegmentName;
+ }
+
+ auto defaultMode = MemoryEncodingMode::FILESYSTEM;
+
+ auto defaultMPath = getMemoryBasePathForMode(defaultMode, 0);
+ auto defaultRelPath = getRelativePathForMode(defaultMode);
+ if (!util::checkIfFolderExists(defaultMPath, defaultRelPath))
+ {
+ ARMARX_WARNING << "The segment folder for segment '"+id().str()+"'was not created. I will create the folder by myself, however it seems like there is a bug in the ltm pipeline.";
+ util::ensureFolderExists(defaultMPath, defaultRelPath, true);
+ }
+
+ if(p.hasAronType())
+ {
+ auto& conv = processors->defaultTypeConverter;
+
+ auto [vec, modeSuffix] = conv.convert(p.aronType());
+ ARMARX_CHECK_EMPTY(modeSuffix);
+ std::filesystem::path relTypePath = defaultRelPath / (constantes::TYPE_FILENAME + conv.suffix);
+ util::writeDataToFileRepeated(defaultMPath, relTypePath, vec);
+ }
+ }
}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/ProviderSegment.h b/source/RobotAPI/libraries/armem/server/ltm/disk/ProviderSegment.h
index 9683715ef66b94d46a95e6e7c102fad7621561b5..0402f4b98408b73719ea2d2889d02c195186aac6 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/ProviderSegment.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/ProviderSegment.h
@@ -15,19 +15,21 @@ namespace armarx::armem::server::ltm::disk
public DiskMemoryItem
{
public:
- ProviderSegment(const std::filesystem::path&, const LongTermMemoryPipeline& p);
+ ProviderSegment(const std::filesystem::path& parentPath, const MemoryID& id, const std::shared_ptr<Processors>& p, const DiskMemoryItem::MemoryEncodingMode mode, const unsigned long e);
bool forEachEntity(std::function<void(Entity&)>&& func) const override;
std::shared_ptr<Entity> findEntity(const std::string&) const override;
protected:
- void _loadAll(armem::wm::ProviderSegment&) override;
- void _load(armem::wm::ProviderSegment&) override;
+ void _loadAllReferences(armem::wm::ProviderSegment&) override;
+ void _resolve(armem::wm::ProviderSegment&) override;
void _store(const armem::wm::ProviderSegment&) override;
+ void _storeType(const armem::wm::ProviderSegment&) override;
- std::string getExpectedFolderName() const override;
-
+ private:
+ MemoryEncodingMode currentMode;
+ unsigned long currentExport;
};
} // namespace armarx::armem::server::ltm::disk
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/detail/Data.cpp b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/Data.cpp
deleted file mode 100644
index d6eef0874de238366f4946cff7bb673b4b1f16c6..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/detail/Data.cpp
+++ /dev/null
@@ -1,7 +0,0 @@
-// Header
-#include "Data.h"
-
-namespace armarx::armem::server::ltm::disk
-{
-
-}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/detail/Data.h b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/Data.h
deleted file mode 100644
index b5b2fbf41adc7fffc0f72792ad4b1c09cf023024..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/detail/Data.h
+++ /dev/null
@@ -1,65 +0,0 @@
-#pragma once
-
-#include <filesystem>
-
-#include "../../../../core/error.h"
-
-namespace armarx::armem::server::ltm::disk
-{
- namespace constantes
- {
- const std::string TYPE_FILENAME = "type.aron";
- const std::string DATA_FILENAME = "data.aron";
- const std::string METADATA_FILENAME = "metadata.aron";
- }
-
- namespace util
- {
- // check whether a string is a number (e.g. timestamp folders)
- inline bool isNumber(const std::string& s)
- {
- for (char const& ch : s)
- {
- if (std::isdigit(ch) == 0)
- {
- return false;
- }
- }
- return true;
- }
-
- inline bool checkIfFolderExists(const std::filesystem::path& p)
- {
- return std::filesystem::exists(p) and std::filesystem::is_directory(p);
- }
-
- inline void ensureFolderExists(const std::filesystem::path& p, bool createIfNotExistent = true)
- {
- if (!std::filesystem::exists(p))
- {
- if (createIfNotExistent)
- {
- std::filesystem::create_directories(p);
- }
- }
- if (!std::filesystem::is_directory(p))
- {
- throw error::ArMemError("Could not find folder: " + p.string());
- }
- }
-
- inline bool checkIfFileExists(const std::filesystem::path& p)
- {
- return std::filesystem::exists(p) and std::filesystem::is_regular_file(p);
- }
-
- inline void ensureFileExists(const std::filesystem::path& p)
- {
- if (!std::filesystem::exists(p) || !std::filesystem::is_regular_file(p))
- {
- // not found
- throw error::ArMemError("Could not find file: " + p.string());
- }
- }
- }
-} // namespace armarx::armem::server::ltm::disk
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/detail/DiskStorage.cpp b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/DiskStorage.cpp
index b30b7cde323e011ea9ae715233323542e717dbec..284f289842b88c888195ac4ac8f5d48a196e9a66 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/detail/DiskStorage.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/DiskStorage.cpp
@@ -1,46 +1,79 @@
// Header
#include "DiskStorage.h"
+// Simox
+#include <SimoxUtility/algorithm/string.h>
+
// ArmarX
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/exceptions/LocalException.h>
namespace armarx::armem::server::ltm::disk
{
- DiskMemoryItem::DiskMemoryItem(const std::filesystem::path& p) :
- path(p.string())
+ //const std::string DiskMemoryItem::Prefix = "_";
+ //const std::string DiskMemoryItem::PrefixEscaped = "__";
+
+ const std::string DiskMemoryItem::MEMORY_EXPORT_SUFFIX = "_";
+ const std::map<std::string, std::string> DiskMemoryItem::EscapeTable = {
+ {"/", "|"}
+ };
+
+ DiskMemoryItem::DiskMemoryItem(const std::filesystem::path& memoryParentPath, const std::string& memoryName, const std::filesystem::path relativePath) :
+ memoryParentPath(memoryParentPath),
+ escapedMemoryName(memoryName),
+ relativeSegmentPath(relativePath)
{
}
- bool DiskMemoryItem::checkPath(const std::string& suffix) const
+ std::filesystem::path DiskMemoryItem::getMemoryBasePathForMode(const MemoryEncodingMode m, const unsigned long e) const
{
- std::filesystem::path memoryP(path);
- memoryP = memoryP / suffix;
-
- // Check connection:
- if (!std::filesystem::exists(path))
+ std::string escapedMemoryNameSuffixed = escapedMemoryName;
+ if (e > 0)
{
- std::filesystem::create_directories(path);
- return true;
+ escapedMemoryNameSuffixed = escapedMemoryName + MEMORY_EXPORT_SUFFIX + std::to_string(e);
}
- else if (std::string expectedFolderName = getExpectedFolderName(); !std::filesystem::is_directory(path) || memoryP.filename() != expectedFolderName)
+
+ switch(m)
{
- ARMARX_WARNING << deactivateSpam("LTM_PathError_" + expectedFolderName)
- << "The entered path is not valid. Please use a path leading to a memory folder with name: " << expectedFolderName << ". The used path was: " << getPath()
- << "\n\n";
- return false;
+ case MemoryEncodingMode::FILESYSTEM:
+ return memoryParentPath / escapedMemoryNameSuffixed;
+ case MemoryEncodingMode::MINIZIP:
+ return memoryParentPath / (escapedMemoryNameSuffixed + minizip::util::MINIZIP_SUFFIX);
}
+ return "";
+ }
- return true;
+ std::filesystem::path DiskMemoryItem::getRelativePathForMode(const MemoryEncodingMode m) const
+ {
+ switch(m) // ensure a tailing "/"
+ {
+ case MemoryEncodingMode::FILESYSTEM:
+ return relativeSegmentPath / "";
+ case MemoryEncodingMode::MINIZIP:
+ return std::filesystem::path(escapedMemoryName) / relativeSegmentPath / "";
+ }
+ return "";
}
- std::string DiskMemoryItem::getPath() const
+ std::string DiskMemoryItem::EscapeSegmentName(const std::string& segmentName)
{
- return path;
+ std::string ret = segmentName;
+ //simox::alg::replace_all(ret, Prefix, PrefixEscaped);
+ for (const auto& [s, r] : EscapeTable)
+ {
+ ret = simox::alg::replace_all(ret, s, r);
+ }
+ return ret;
}
- void DiskMemoryItem::setPath(const std::string& ps)
+ std::string DiskMemoryItem::UnescapeSegmentName(const std::string& escapedName)
{
- path = ps;
+ std::string ret = escapedName;
+ for (const auto& [s, r] : EscapeTable) // Here we assume that noone uses the replaced char usually in the segment name... TODO
+ {
+ ret = simox::alg::replace_all(ret, r, s);
+ }
+ return ret;
}
}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/detail/DiskStorage.h b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/DiskStorage.h
index 01142b930e6d7d3751c8a01af6d613c2bc3f1501..e4bc43b0b173908bdcc471b4033145cf5fdfd63a 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/detail/DiskStorage.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/DiskStorage.h
@@ -1,26 +1,42 @@
#pragma once
#include <filesystem>
+#include <map>
+#include <string>
-#include "Data.h"
+// util
+#include "util/util.h"
namespace armarx::armem::server::ltm::disk
{
class DiskMemoryItem
{
public:
+ enum class MemoryEncodingMode
+ {
+ FILESYSTEM = 0,
+ MINIZIP = 1
+ };
+
DiskMemoryItem() = default;
- DiskMemoryItem(const std::filesystem::path&);
+ DiskMemoryItem(const std::filesystem::path& memoryParentPath, const std::string& escapedMemoryName, const std::filesystem::path relativePath);
virtual ~DiskMemoryItem() = default;
- virtual void setPath(const std::string&);
- std::string getPath() const;
+ std::filesystem::path getMemoryBasePathForMode(const MemoryEncodingMode m, const unsigned long e) const;
+ std::filesystem::path getRelativePathForMode(const MemoryEncodingMode m) const;
protected:
- bool checkPath(const std::string&) const;
- virtual std::string getExpectedFolderName() const = 0;
+ static std::string EscapeSegmentName(const std::string& segmentName);
+ static std::string UnescapeSegmentName(const std::string& escapedName);
protected:
- std::string path;
+ //static const std::string PREFIX;
+ //static const std::string PREFIX_ESCAPED;
+ static const std::string MEMORY_EXPORT_SUFFIX;
+ static const std::map<std::string, std::string> EscapeTable;
+
+ std::filesystem::path memoryParentPath;
+ std::string escapedMemoryName;
+ std::filesystem::path relativeSegmentPath;
};
} // namespace armarx::armem::server::ltm::disk
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/detail/util/filesystem_util.cpp b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/util/filesystem_util.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..eceff8d47b41654b64e78212e125aafd7196dfa3
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/util/filesystem_util.cpp
@@ -0,0 +1,114 @@
+#include "filesystem_util.h"
+
+namespace armarx::armem::server::ltm::disk
+{
+ namespace filesystem::util
+ {
+ size_t getSizeOfDirectory(const std::filesystem::path& p)
+ {
+ if (!std::filesystem::exists(p) || !std::filesystem::is_directory(p))
+ {
+ return 0;
+ }
+
+ // command to be executed. Taken from https://stackoverflow.com/questions/15495756/how-can-i-find-the-size-of-all-files-located-inside-a-folder/15501719
+ std::string cmd = "du -sb " + p.string() + " | cut -f1 2>&1";
+
+ // execute above command and get the output
+ FILE *stream = popen(cmd.c_str(), "r");
+ if (stream) {
+ const int max_size = 256;
+ char readbuf[max_size];
+ if (fgets(readbuf, max_size, stream) != NULL)
+ {
+ return atoll(readbuf);
+ }
+ pclose(stream);
+ }
+
+ // return error val
+ return 0;
+ }
+
+ bool checkIfFolderInFilesystemExists(const std::filesystem::path& p)
+ {
+ return std::filesystem::exists(p) and std::filesystem::is_directory(p);
+ }
+
+ void ensureFolderInFilesystemExists(const std::filesystem::path& p, bool createIfNotExistent)
+ {
+ if (!std::filesystem::exists(p))
+ {
+ if (createIfNotExistent)
+ {
+ std::filesystem::create_directories(p);
+ }
+ }
+ if (!std::filesystem::is_directory(p))
+ {
+ throw error::ArMemError("Could not find folder: " + p.string());
+ }
+ }
+
+ bool checkIfFileInFilesystemExists(const std::filesystem::path& p)
+ {
+ return std::filesystem::exists(p) and std::filesystem::is_regular_file(p);
+ }
+
+ void ensureFileInFilesystemExists(const std::filesystem::path& p)
+ {
+ if (!std::filesystem::exists(p) || !std::filesystem::is_regular_file(p))
+ {
+ // not found
+ throw error::ArMemError("Could not find file: " + p.string());
+ }
+ }
+
+ void writeDataInFilesystemFile(const std::filesystem::path& p, const std::vector<unsigned char>& data)
+ {
+ std::ofstream dataofs;
+ dataofs.open(p);
+ if (!dataofs)
+ {
+ throw error::ArMemError("Could not write data to filesystem file '" + p.string() + "'. Skipping this file.");
+ }
+ dataofs.write(reinterpret_cast<const char*>(data.data()), data.size());
+ dataofs.close();
+ }
+
+ std::vector<unsigned char> readDataFromFilesystemFile(const std::filesystem::path path)
+ {
+ std::ifstream dataifs(path);
+ std::vector<unsigned char> datafilecontent((std::istreambuf_iterator<char>(dataifs)), (std::istreambuf_iterator<char>()));
+ return datafilecontent;
+ }
+
+ std::vector<std::string> getAllFilesystemDirectories(const std::filesystem::path path)
+ {
+ std::vector<std::string> ret;
+ for (const auto& subdir : std::filesystem::directory_iterator(path))
+ {
+ std::filesystem::path subdirPath = subdir.path();
+ if (std::filesystem::is_directory(subdirPath))
+ {
+ ret.push_back(subdirPath.filename());
+ }
+ }
+ return ret;
+ }
+
+ std::vector<std::string> getAllFilesystemFiles(const std::filesystem::path path)
+ {
+ std::vector<std::string> ret;
+ for (const auto& subdir : std::filesystem::directory_iterator(path))
+ {
+ std::filesystem::path subdirPath = subdir.path();
+ if (std::filesystem::is_regular_file(subdirPath))
+ {
+ ret.push_back(subdirPath.filename());
+ }
+ }
+ return ret;
+ }
+ }
+} // namespace armarx::armem::server::ltm::disk
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/detail/util/filesystem_util.h b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/util/filesystem_util.h
new file mode 100644
index 0000000000000000000000000000000000000000..b229a7b7f8ac115318fb142c7a6d0f065b344d86
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/util/filesystem_util.h
@@ -0,0 +1,32 @@
+#pragma once
+
+// STD / STL
+#include <filesystem>
+#include <iostream>
+#include <fstream>
+
+#include "../../../../../core/error.h"
+
+namespace armarx::armem::server::ltm::disk
+{
+ namespace filesystem::util
+ {
+ size_t getSizeOfDirectory(const std::filesystem::path& p);
+
+ bool checkIfFolderInFilesystemExists(const std::filesystem::path& p);
+
+ void ensureFolderInFilesystemExists(const std::filesystem::path& p, bool createIfNotExistent = true);
+
+ bool checkIfFileInFilesystemExists(const std::filesystem::path& p);
+
+ void ensureFileInFilesystemExists(const std::filesystem::path& p);
+
+ void writeDataInFilesystemFile(const std::filesystem::path& p, const std::vector<unsigned char>& data);
+
+ std::vector<unsigned char> readDataFromFilesystemFile(const std::filesystem::path path);
+
+ std::vector<std::string> getAllFilesystemDirectories(const std::filesystem::path path);
+
+ std::vector<std::string> getAllFilesystemFiles(const std::filesystem::path path);
+ }
+} // namespace armarx::armem::server::ltm::disk
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/detail/util/minizip_util.cpp b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/util/minizip_util.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2995afc21cba2caa75e5dc093398b900bb81e819
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/util/minizip_util.cpp
@@ -0,0 +1,276 @@
+#include "minizip_util.h"
+
+namespace armarx::armem::server::ltm::disk
+{
+ namespace minizip::util
+ {
+ bool checkZipFile(const std::filesystem::path& pathToZip)
+ {
+ bool ret = false;
+ zipFile z = NULL;
+ if (std::filesystem::exists(pathToZip))
+ {
+ if (std::filesystem::is_regular_file(pathToZip) && pathToZip.extension() == MINIZIP_SUFFIX)
+ {
+ z = zipOpen64(pathToZip.string().c_str(), APPEND_STATUS_ADDINZIP);
+ ret = (bool) z;
+ zipClose(z, NULL);
+ }
+ }
+ return ret;
+ }
+
+ zipFile ensureZipFile(const std::filesystem::path& pathToZip, bool createIfNotExistent)
+ {
+ zipFile z = NULL;
+ if (!checkZipFile(pathToZip))
+ {
+ if (createIfNotExistent)
+ {
+ z = zipOpen64(pathToZip.string().c_str(), APPEND_STATUS_CREATE);
+ }
+ else
+ {
+ throw error::ArMemError("Could not find zip file: " + pathToZip.string());
+ }
+ }
+ else
+ {
+ z = zipOpen64(pathToZip.string().c_str(), APPEND_STATUS_ADDINZIP);
+ }
+
+ if (!z)
+ {
+ throw error::ArMemError("Unknown error occured during opening zip file: " + pathToZip.string());
+ }
+ return z;
+ }
+
+ unzFile getUnzFile(const std::filesystem::path& pathToZip)
+ {
+ unzFile z = NULL;
+ if (std::filesystem::exists(pathToZip))
+ {
+ if (std::filesystem::is_regular_file(pathToZip) && pathToZip.extension() == MINIZIP_SUFFIX)
+ {
+ z = unzOpen64(pathToZip.string().c_str());
+ }
+ else
+ {
+ throw error::ArMemError("Existing file is not a zip file: " + pathToZip.string());
+ }
+ }
+
+ // if z is NULL then the zip file might be empty
+
+ return z;
+ }
+
+ bool checkIfElementInZipExists(const std::filesystem::path& pathToZip, const std::filesystem::path& p)
+ {
+ if (!checkZipFile(pathToZip)) return false;
+ auto uzf = getUnzFile(pathToZip);
+
+ bool ret = (unzLocateFile(uzf, p.string().c_str(), MINIZIP_CASE_SENSITIVITY) == UNZ_OK);
+
+ unzClose(uzf);
+ return ret;
+ }
+
+ void ensureElementInZipExists(const std::filesystem::path& pathToZip, const std::filesystem::path& p, bool createIfNotExistent)
+ {
+ auto zf = ensureZipFile(pathToZip, createIfNotExistent);
+ auto uzf = getUnzFile(pathToZip);
+
+ if (auto r = unzLocateFile(uzf, p.string().c_str(), MINIZIP_CASE_SENSITIVITY); r != UNZ_OK)
+ {
+ if (createIfNotExistent)
+ {
+ zip_fileinfo zfi;
+ zipOpenNewFileInZip64(zf, p.string().c_str(), &zfi, NULL, 0, NULL, 0, NULL, Z_DEFLATED, Z_DEFAULT_COMPRESSION, 1);
+ zipCloseFileInZip(zf);
+ }
+ else
+ {
+ unzClose(uzf);
+ zipClose(zf, NULL);
+ throw error::ArMemError("Could not find element '" + p.string() + "' in zip file: " + pathToZip.string());
+ }
+ }
+ // else folder exists
+
+ unzClose(uzf);
+ zipClose(zf, NULL);
+ }
+
+ void ensureFolderInZipExists(const std::filesystem::path& pathToZip, const std::filesystem::path& p, bool createIfNotExistent)
+ {
+ if (!p.filename().empty())
+ {
+ throw error::ArMemError("The specified path is not a folder (it needs tailing /): " + p.string());
+ }
+
+ ensureElementInZipExists(pathToZip, p, createIfNotExistent);
+ }
+
+ void ensureFileInZipExists(const std::filesystem::path& pathToZip, const std::filesystem::path& p)
+ {
+ if (p.filename().empty())
+ {
+ throw error::ArMemError("The specified path is not a file (it needs a filename): " + p.string());
+ }
+
+ ensureElementInZipExists(pathToZip, p, true);
+ }
+
+ bool checkIfFolderInZipExists(const std::filesystem::path& pathToZip, const std::filesystem::path& p)
+ {
+ if (!p.filename().empty())
+ {
+ throw error::ArMemError("The specified path is not a folder (it needs tailing /): " + p.string());
+ }
+
+ return checkIfElementInZipExists(pathToZip, p);
+ }
+
+ bool checkIfFileInZipExists(const std::filesystem::path& pathToZip, const std::filesystem::path& p)
+ {
+ if (p.filename().empty())
+ {
+ throw error::ArMemError("The specified path is not a file (it needs a filename): " + p.string());
+ }
+
+ return checkIfElementInZipExists(pathToZip, p);
+ }
+
+ void writeDataInFileInZipFile(const std::filesystem::path& pathToZip, const std::filesystem::path& p, const std::vector<unsigned char>& data)
+ {
+ auto zf = ensureZipFile(pathToZip);
+
+ zip_fileinfo zfi;
+ if(zipOpenNewFileInZip64(zf, p.string().c_str(), &zfi, NULL, 0, NULL, 0, NULL, Z_DEFLATED, Z_DEFAULT_COMPRESSION, 1) == Z_OK)
+ {
+ if(zipWriteInFileInZip(zf, data.data(), data.size()) != Z_OK)
+ {
+ throw error::ArMemError("Could not write a file '"+p.string()+"' to zip '" + pathToZip.string() + "'.");
+ }
+ }
+ else
+ {
+ throw error::ArMemError("Could not add a new file '"+p.string()+"' to zip '" + pathToZip.string() + "'.");
+ }
+ zipCloseFileInZip(zf);
+ zipClose(zf, NULL);
+ }
+
+ std::vector<unsigned char> readDataFromFileInZipFile(const std::filesystem::path& pathToZip, const std::filesystem::path& p)
+ {
+ auto uzf = getUnzFile(pathToZip);
+ if (unzLocateFile(uzf, p.string().c_str(), MINIZIP_CASE_SENSITIVITY) == UNZ_OK) // set current file
+ {
+ // File located
+ unz_file_info uzfi;
+ if (unzGetCurrentFileInfo(uzf, &uzfi, NULL, 0, NULL, 0, NULL, 0) == UNZ_OK)
+ {
+ std::vector<unsigned char> data(uzfi.uncompressed_size);
+
+ if (unzOpenCurrentFile(uzf) == UNZ_OK) // open current file
+ {
+ if (unzReadCurrentFile(uzf, data.data(), uzfi.uncompressed_size) == UNZ_OK) // read file in buffer
+ {
+ unzCloseCurrentFile(uzf);
+ unzClose(uzf);
+ return data;
+ }
+ }
+ }
+ }
+ unzClose(uzf);
+ throw error::ArMemError("Could not read data from zip file '" + pathToZip.string() + "' and internal path '" + p.string() + "'.");
+ }
+
+ std::vector<std::string> getAllDirectoriesInZipFile(const std::filesystem::path& pathToZip, const std::filesystem::path p)
+ {
+ std::vector<std::string> ret;
+
+ unzFile uzf = getUnzFile(pathToZip);
+ if (unzLocateFile(uzf, p.string().c_str(), MINIZIP_CASE_SENSITIVITY) == UNZ_OK) // set current file
+ {
+ while(unzGoToNextFile(uzf) == UNZ_OK)
+ {
+ unz_file_info uzfi;
+ unzGetCurrentFileInfo(uzf, &uzfi, NULL, 0, NULL, 0, NULL, 0); // get file info
+ std::vector<char> filenameVec(uzfi.size_filename);
+ unzGetCurrentFileInfo(uzf, NULL, filenameVec.data(), uzfi.size_filename, NULL, 0, NULL, 0); // get file name
+ std::string filename(filenameVec.begin(), filenameVec.end());
+
+ auto pString = p.string();
+ if (!simox::alg::starts_with(filename, pString))
+ {
+ // we moved out of the folder. Abort
+ break;
+ }
+
+ std::string filenameWithoutPrefix = simox::alg::remove_prefix(filename, pString);
+
+ if (!simox::alg::ends_with(filenameWithoutPrefix, "/"))
+ {
+ // this is not a directory
+ continue;
+ }
+
+ if (simox::alg::count(filenameWithoutPrefix, "/") != 1)
+ {
+ // we are in a subfolder of the subfolder
+ continue;
+ }
+
+ ret.push_back(filenameWithoutPrefix);
+ }
+ }
+ return ret;
+ }
+
+ std::vector<std::string> getAllFilesInZipFile(const std::filesystem::path& pathToZip, const std::filesystem::path p)
+ {
+ std::vector<std::string> ret;
+
+ unzFile uzf = getUnzFile(pathToZip);
+ if (unzLocateFile(uzf, p.string().c_str(), MINIZIP_CASE_SENSITIVITY) == UNZ_OK) // set current file
+ {
+ while(unzGoToNextFile(uzf) == UNZ_OK)
+ {
+ unz_file_info uzfi;
+ unzGetCurrentFileInfo(uzf, &uzfi, NULL, 0, NULL, 0, NULL, 0); // get file info
+ std::vector<char> filenameVec(uzfi.size_filename);
+ unzGetCurrentFileInfo(uzf, NULL, filenameVec.data(), uzfi.size_filename, NULL, 0, NULL, 0); // get file name
+ std::string filename(filenameVec.begin(), filenameVec.end());
+
+ auto pString = p.string();
+ if (!simox::alg::starts_with(filename, pString))
+ {
+ // we moved out of the folder. Abort
+ break;
+ }
+
+ std::string filenameWithoutPrefix = simox::alg::remove_prefix(filename, pString);
+
+ if (simox::alg::ends_with(filenameWithoutPrefix, "/"))
+ {
+ // this is a directory
+ continue;
+ }
+
+ if (simox::alg::count(filenameWithoutPrefix, "/") != 1)
+ {
+ // we are in a subfolder of the subfolder
+ continue;
+ }
+
+ ret.push_back(filenameWithoutPrefix);
+ }
+ }
+ return ret;
+ }
+ }
+} // namespace armarx::armem::server::ltm::disk
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/detail/util/minizip_util.h b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/util/minizip_util.h
new file mode 100644
index 0000000000000000000000000000000000000000..99ebab93572afd50cccb194c1aa827635a6c585d
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/util/minizip_util.h
@@ -0,0 +1,47 @@
+#pragma once
+
+// STD / STL
+#include <filesystem>
+#include <iostream>
+
+#include <minizip/zip.h>
+#include <minizip/unzip.h>
+
+#include <SimoxUtility/algorithm/string.h>
+
+#include "../../../../../core/error.h"
+
+namespace armarx::armem::server::ltm::disk
+{
+ namespace minizip::util
+ {
+ const std::string MINIZIP_SUFFIX = ".zip";
+ const int MINIZIP_CASE_SENSITIVITY = 1;
+
+ bool checkZipFile(const std::filesystem::path& pathToZip);
+
+ zipFile ensureZipFile(const std::filesystem::path& pathToZip, bool createIfNotExistent = true);
+
+ unzFile getUnzFile(const std::filesystem::path& pathToZip);
+
+ bool checkIfElementInZipExists(const std::filesystem::path& pathToZip, const std::filesystem::path& p);
+
+ void ensureElementInZipExists(const std::filesystem::path& pathToZip, const std::filesystem::path& p, bool createIfNotExistent = true);
+
+ void ensureFolderInZipExists(const std::filesystem::path& pathToZip, const std::filesystem::path& p, bool createIfNotExistent = true);
+
+ void ensureFileInZipExists(const std::filesystem::path& pathToZip, const std::filesystem::path& p);
+
+ bool checkIfFolderInZipExists(const std::filesystem::path& pathToZip, const std::filesystem::path& p);
+
+ bool checkIfFileInZipExists(const std::filesystem::path& pathToZip, const std::filesystem::path& p);
+
+ void writeDataInFileInZipFile(const std::filesystem::path& pathToZip, const std::filesystem::path& p, const std::vector<unsigned char>& data);
+
+ std::vector<unsigned char> readDataFromFileInZipFile(const std::filesystem::path& pathToZip, const std::filesystem::path& p);
+
+ std::vector<std::string> getAllDirectoriesInZipFile(const std::filesystem::path& pathToZip, const std::filesystem::path p);
+
+ std::vector<std::string> getAllFilesInZipFile(const std::filesystem::path& pathToZip, const std::filesystem::path p);
+ }
+} // namespace armarx::armem::server::ltm::disk
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/detail/util/util.cpp b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/util/util.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8828dc595166c90f7a7217e247428ba0792bf0c4
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/util/util.cpp
@@ -0,0 +1,143 @@
+#include "util.h"
+
+#include <thread>
+#include <chrono>
+
+namespace armarx::armem::server::ltm::disk
+{
+ namespace util
+ {
+ // check whether a string is a number (e.g. timestamp folders)
+ bool isNumber(const std::string& s)
+ {
+ for (char const& ch : s)
+ {
+ if (std::isdigit(ch) == 0)
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ bool checkIfBasePathExists(const std::filesystem::path& mPath)
+ {
+ if (mPath.extension() == minizip::util::MINIZIP_SUFFIX)
+ {
+ return minizip::util::checkZipFile(mPath);
+ }
+ return filesystem::util::checkIfFolderInFilesystemExists(mPath);
+ }
+
+ void ensureBasePathExists(const std::filesystem::path& mPath, bool createIfNotExistent)
+ {
+ if (mPath.extension() == minizip::util::MINIZIP_SUFFIX)
+ {
+ auto z = minizip::util::ensureZipFile(mPath, createIfNotExistent);
+ zipClose(z, NULL);
+ return;
+ }
+ return filesystem::util::ensureFolderInFilesystemExists(mPath, createIfNotExistent);
+ }
+
+ bool checkIfFolderExists(const std::filesystem::path& mPath, const std::filesystem::path& p)
+ {
+ if (mPath.extension() == minizip::util::MINIZIP_SUFFIX)
+ {
+ return minizip::util::checkIfFolderInZipExists(mPath, p);
+ }
+
+ return filesystem::util::checkIfFolderInFilesystemExists(mPath / p);
+ }
+
+ void ensureFolderExists(const std::filesystem::path& mPath, const std::filesystem::path& p, bool createIfNotExistent)
+ {
+ if (mPath.extension() == minizip::util::MINIZIP_SUFFIX)
+ {
+ return minizip::util::ensureFolderInZipExists(mPath, p, createIfNotExistent);
+ }
+
+ return filesystem::util::ensureFolderInFilesystemExists(mPath / p, createIfNotExistent);
+ }
+
+ bool checkIfFileExists(const std::filesystem::path& mPath, const std::filesystem::path& p)
+ {
+ if (mPath.extension() == minizip::util::MINIZIP_SUFFIX)
+ {
+ return minizip::util::checkIfFileInZipExists(mPath, p);
+ }
+
+ return filesystem::util::checkIfFileInFilesystemExists(mPath / p);
+ }
+
+ void ensureFileExists(const std::filesystem::path& mPath, const std::filesystem::path& p)
+ {
+ if (mPath.extension() == minizip::util::MINIZIP_SUFFIX)
+ {
+ return minizip::util::ensureFileInZipExists(mPath, p);
+ }
+
+ return filesystem::util::ensureFileInFilesystemExists(mPath / p);
+ }
+
+ void writeDataToFile(const std::filesystem::path& mPath, const std::filesystem::path& p, const std::vector<unsigned char>& data)
+ {
+ if (mPath.extension() == minizip::util::MINIZIP_SUFFIX)
+ {
+ return minizip::util::writeDataInFileInZipFile(mPath, p, data);
+ }
+
+ return filesystem::util::writeDataInFilesystemFile(mPath / p, data);
+ }
+
+ void writeDataToFileRepeated(const std::filesystem::path& mPath, const std::filesystem::path& p, const std::vector<unsigned char>& data, const unsigned int maxTries, const unsigned int sleepTimeMs)
+ {
+ for (unsigned int i = 0; i < maxTries; ++i)
+ {
+ try
+ {
+ writeDataToFile(mPath, p, data);
+ return;
+ }
+ catch (const error::ArMemError&)
+ {
+ // wait a bit to give the filesystem enough time to manage the workload
+ std::this_thread::sleep_for(std::chrono::milliseconds(sleepTimeMs));
+ }
+ }
+
+ // even after all the tries we did not succeeded. This is very bad!
+ throw error::ArMemError("ATTENTION! Even after " + std::to_string(maxTries) + " tries, the memory was not able to store the instance at path '" + p.string() + "'. This means this instance will be lost!");
+ }
+
+ std::vector<unsigned char> readDataFromFile(const std::filesystem::path& mPath, const std::filesystem::path& p)
+ {
+ if (mPath.extension() == minizip::util::MINIZIP_SUFFIX)
+ {
+ return minizip::util::readDataFromFileInZipFile(mPath, p);
+ }
+
+ return filesystem::util::readDataFromFilesystemFile(mPath / p);
+ }
+
+ std::vector<std::string> getAllDirectories(const std::filesystem::path& mPath, const std::filesystem::path& p)
+ {
+ if (mPath.extension() == minizip::util::MINIZIP_SUFFIX)
+ {
+ return {};
+ }
+
+ return filesystem::util::getAllFilesystemDirectories(mPath / p);
+ }
+
+ std::vector<std::string> getAllFiles(const std::filesystem::path& mPath, const std::filesystem::path& p)
+ {
+ if (mPath.extension() == minizip::util::MINIZIP_SUFFIX)
+ {
+ return {};
+ }
+
+ return filesystem::util::getAllFilesystemFiles(mPath / p);
+ }
+ }
+} // namespace armarx::armem::server::ltm::disk
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/detail/util/util.h b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/util/util.h
new file mode 100644
index 0000000000000000000000000000000000000000..e260d70923c7eb36bf0a78ab6e0c3a7d3bf98c52
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/util/util.h
@@ -0,0 +1,44 @@
+#pragma once
+
+#include "../../../../../core/error.h"
+
+#include "filesystem_util.h"
+#include "minizip_util.h"
+
+namespace armarx::armem::server::ltm::disk
+{
+ namespace constantes
+ {
+ const std::string TYPE_FILENAME = "type.aron";
+ const std::string DATA_FILENAME = "data.aron";
+ const std::string METADATA_FILENAME = "metadata.aron";
+ }
+
+ namespace util
+ {
+ // check whether a string is a number (e.g. timestamp folders)
+ bool isNumber(const std::string& s);
+
+ bool checkIfBasePathExists(const std::filesystem::path& mPath);
+
+ void ensureBasePathExists(const std::filesystem::path& mPath, bool createIfNotExistent = true);
+
+ bool checkIfFolderExists(const std::filesystem::path& mPath, const std::filesystem::path& p);
+
+ void ensureFolderExists(const std::filesystem::path& mPath, const std::filesystem::path& p, bool createIfNotExistent = true);
+
+ bool checkIfFileExists(const std::filesystem::path& mPath, const std::filesystem::path& p);
+
+ void ensureFileExists(const std::filesystem::path& mPath, const std::filesystem::path& p);
+
+ void writeDataToFile(const std::filesystem::path& mPath, const std::filesystem::path& p, const std::vector<unsigned char>& data);
+
+ void writeDataToFileRepeated(const std::filesystem::path& mPath, const std::filesystem::path& p, const std::vector<unsigned char>& data, const unsigned int maxTries = 100, const unsigned int sleepTimeMs = 10);
+
+ std::vector<unsigned char> readDataFromFile(const std::filesystem::path& mPath, const std::filesystem::path& p);
+
+ std::vector<std::string> getAllDirectories(const std::filesystem::path& mPath, const std::filesystem::path& p);
+
+ std::vector<std::string> getAllFiles(const std::filesystem::path& mPath, const std::filesystem::path& p);
+ }
+} // namespace armarx::armem::server::ltm::disk
diff --git a/source/RobotAPI/libraries/armem/server/plugins/Plugin.cpp b/source/RobotAPI/libraries/armem/server/plugins/Plugin.cpp
index 7ae0bc4bac3f4e3166f47fd4777d44d68f41a86f..4356aed6f4591eb5d33baaa86b0f9d4388f0a628 100644
--- a/source/RobotAPI/libraries/armem/server/plugins/Plugin.cpp
+++ b/source/RobotAPI/libraries/armem/server/plugins/Plugin.cpp
@@ -47,6 +47,7 @@ namespace armarx::armem::server::plugins
{
Component& parent = this->parent<Component>();
+ // register to MNS
if (clientPlugin->isMemoryNameSystemEnabled() and clientPlugin->getMemoryNameSystemClient())
{
registerServer(parent);
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/ltm/detail/EntityQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/ltm/detail/EntityQueryProcessorBase.h
index a0be62d216464c0b04a167c9f71ae62543cf6086..71374c40f2792c7fd855acd39f581d2b18a750ec 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/ltm/detail/EntityQueryProcessorBase.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/ltm/detail/EntityQueryProcessorBase.h
@@ -30,7 +30,7 @@ namespace armarx::armem::server::query_proc::ltm::detail
void addResultSnapshot(ResultEntityT& result, const EntitySnapshotT& snapshot) const override
{
ResultSnapshotT s;
- snapshot.loadAll(s);
+ snapshot.loadAllReferences(s);
result.addSnapshot(s);
}
};
diff --git a/source/RobotAPI/libraries/armem/server/test/ArMemLTMBenchmark.cpp b/source/RobotAPI/libraries/armem/server/test/ArMemLTMBenchmark.cpp
index b4965373da2d7c3443b5748e133e62607b1a198c..8aded460e730d913f523c44a379c069bb70c41a6 100644
--- a/source/RobotAPI/libraries/armem/server/test/ArMemLTMBenchmark.cpp
+++ b/source/RobotAPI/libraries/armem/server/test/ArMemLTMBenchmark.cpp
@@ -68,9 +68,7 @@ namespace ArMemLTMBenchmark
void storeElementNTimes(const std::string& memoryName, const aron::data::DictPtr& dict, int waitingTimeMs, int n)
{
- armem::server::ltm::disk::Memory ltm;
- ltm.setPath(storagePath / memoryName);
- ltm.setMemoryID(ltm.id().withMemoryName(memoryName));
+ armem::server::ltm::disk::Memory ltm(storagePath, memoryName);
armem::wm::Memory wm(memoryName);
auto& core = wm.addCoreSegment("CoreS");
diff --git a/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.cpp b/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.cpp
index 21fde4574fb445d662474de5a3f56a9223df6dca..d8755bbfd2e640842b4342c55330a761ce8a362d 100644
--- a/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.cpp
+++ b/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.cpp
@@ -100,11 +100,8 @@ namespace armarx::armem::gui::disk
}
else
{
- std::filesystem::create_directories(path / name);
-
- armem::server::ltm::disk::Memory memory((path / name));
+ armem::server::ltm::disk::Memory memory(path, name);
memory.directlyStore(data);
- //memory.storeBuffer();
numStored++;
}
@@ -128,61 +125,86 @@ namespace armarx::armem::gui::disk
std::filesystem::path path(directory.toUtf8().constData());
std::map<std::string, wm::Memory> memoryData;
- std::stringstream status;
+
+ auto setStatus = [&](const std::string& s){
+ if (outStatus)
+ {
+ *outStatus = s;
+ }
+ };
if (not std::filesystem::is_directory(path))
{
- status << "Could not import a memory from " << path << ". Skipping import.";
+ setStatus("Could not import a memory from " + path.string() + ". It is not a directory. Skipping import.");
+ return memoryData;
}
- else
+
+ // Find out whether this is a single memory or a collection of memories by searching
+ // for a data.aron.* or metadata.aron.* file at depth 5 (if 6 then it is collection of memories)
+ bool isSingleMemory = false;
+ for (auto i = std::filesystem::recursive_directory_iterator(path); i != std::filesystem::recursive_directory_iterator(); ++i)
{
- // We use LTM as query target for the disk
- armem::client::QueryInput queryInput = _queryInput;
- queryInput.addQueryTargetToAll(armem::query::data::QueryTarget::LTM);
- const query::data::Input queryIce = queryInput.toIce();
+ if (i.depth() > armem::server::ltm::disk::Memory::DEPTH_TO_DATA_FILES + 2)
+ {
+ // After some depth we stop searching to not freeze GUI too long
+ setStatus("Could not import a memory from " + path.string() + ". Data files were not found until max-depth 7. Skipping import.");
+ return memoryData;
+ }
- int numLoaded = 0;
- for (const auto& dir : std::filesystem::directory_iterator(path))
+ auto& dir = *i;
+
+ // if one matches it is enough to check
+ if (std::filesystem::is_regular_file(dir.path()) && simox::alg::starts_with(dir.path().filename(), "data.aron"))
{
- if (dir.is_directory())
- {
- armem::server::ltm::disk::Memory ltm(dir.path());
+ isSingleMemory = (i.depth() == armem::server::ltm::disk::Memory::DEPTH_TO_DATA_FILES);
+ break;
+ }
+ }
- armem::wm::Memory memory;
- ltm.loadAll(memory); // load list of references
- ltm.load(memory); // resolve a list of references
+ // TODO: Only add data that matchs query?
+ // We use LTM as query target for the disk
+ // armem::client::QueryInput queryInput = _queryInput;
+ // queryInput.addQueryTargetToAll(armem::query::data::QueryTarget::LTM);
+ // const query::data::Input queryIce = queryInput.toIce();
- memoryData[memory.name()] = std::move(memory);
+ int numLoaded = 0;
+ auto loadMemory = [&](const std::filesystem::path& p) {
+ if (std::filesystem::is_directory(p))
+ {
+ armem::server::ltm::disk::Memory ltm(p.parent_path(), p.filename());
+ armem::wm::Memory memory = ltm.loadAllAndResolve(); // load list of references
+ memoryData[memory.name()] = std::move(memory);
- numLoaded++;
- }
+ numLoaded++;
}
- status << "Loaded " << numLoaded << " " << handleSingular(numLoaded, "memory", "memories") << " from " << path << ".";
- }
+ };
- if (outStatus)
+ if (isSingleMemory)
{
- *outStatus = status.str();
+ loadMemory(path);
+ }
+ else
+ {
+ // we have to load multiple memories (each subfolder)
+ for (const auto& dir : std::filesystem::directory_iterator(path))
+ {
+ loadMemory(dir.path());
+ }
}
+
+ setStatus("Loaded " + std::to_string(numLoaded) + " " + handleSingular(numLoaded, "memory", "memories") + " from " + path.string() + ".");
return memoryData;
}
QString ControlWidget::chooseDirectoryDialog()
{
- QFileDialog dialog;
- dialog.setFileMode(QFileDialog::DirectoryOnly);
- dialog.setOption(QFileDialog::ShowDirsOnly, false);
- dialog.setDirectory(_latestDirectory);
- if (dialog.exec())
- {
- _latestDirectory = dialog.directory().path();
- return _latestDirectory;
- }
- else
- {
- return QString::fromStdString("");
- }
+ _latestDirectory = QFileDialog::getExistingDirectory(this, "Open query result",
+ _latestDirectory,
+ QFileDialog::ShowDirsOnly
+ | QFileDialog::DontResolveSymlinks);
+
+ return _latestDirectory;
}
}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp b/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp
index 3bbb2ee55a87be3216386a0f847b40829b4bb785..61918cb20354d5a24a8ab0e8d1636a437966f2b9 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp
@@ -446,7 +446,7 @@ namespace armarx::armem::gui::instance
std::vector<QAction*> InstanceView::makeActionsCopyDataToClipboard()
{
- return makeCopyActions(currentInstance->data());
+ return makeCopyActions(currentInstance->data(), currentAronType);
}
std::vector<QAction*> InstanceView::makeActionsCopyDataToClipboard(const aron::Path& path)
@@ -454,7 +454,21 @@ namespace armarx::armem::gui::instance
try
{
aron::data::VariantPtr element = currentInstance->data()->navigateAbsolute(path);
- return makeCopyActions(element);
+ aron::type::VariantPtr elementType = nullptr;
+ if (currentAronType)
+ {
+ // There doesn't seem to be a way to check whether the path exists
+ // without potentially throwing an exception.
+ try
+ {
+ elementType = currentAronType->navigateAbsolute(path);
+ }
+ catch (const aron::error::AronException& e)
+ {
+ // No type available, elementType remains nullptr.
+ }
+ }
+ return makeCopyActions(element, elementType);
}
catch (const aron::error::AronException& e)
{
@@ -463,18 +477,17 @@ namespace armarx::armem::gui::instance
return {};
}
- std::vector<QAction*> InstanceView::makeCopyActions(const aron::data::VariantPtr& element)
+ std::vector<QAction*> InstanceView::makeCopyActions(
+ const aron::data::VariantPtr& element,
+ const aron::type::VariantPtr& elementType)
{
QAction* easyJsonAction = new QAction("Copy data to clipboard as easy JSON");
- connect(easyJsonAction, &QAction::triggered, [this, element]()
+ connect(easyJsonAction, &QAction::triggered, [this, element, elementType]()
{
try
{
TreeTypedJSONConverter conv;
- // TODO(phesch): Type hierarchy doesn't match data hierarchy
- armarx::aron::data::visitRecursive(conv, currentInstance->data(), currentAronType);
- //nlohmann::json json =
- // aron::converter::AronNlohmannJSONConverter::ConvertToNlohmannJSON(element);
+ armarx::aron::data::visitRecursive(conv, element, elementType);
QClipboard* clipboard = QApplication::clipboard();
clipboard->setText(QString::fromStdString(conv.getJSON().dump(2)));
QApplication::processEvents();
@@ -683,7 +696,10 @@ namespace armarx::armem::gui::instance
try
{
// TODO We cannot know what the str in the pixeltype belongs to (e.g. coming from java, python, c++ it may contain different values!
- //pixelType = aron::type::Image::pixelTypeFromName(imageData->getType());
+ // pixelType = aron::type::Image::pixelTypeFromName(imageData->getType());
+
+ // For now we assume it comes from c++ where '5' means CV_32FC1 (=5)
+ pixelType = (imageData->getType() == "5" ? PixelType::depth32 : PixelType::rgb24);
}
catch (const aron::error::AronException&)
{
diff --git a/source/RobotAPI/libraries/armem_gui/instance/InstanceView.h b/source/RobotAPI/libraries/armem_gui/instance/InstanceView.h
index 982d277db1febb68070290d3c86a3f06c602b990..970af2b6e45698cd342950a1ffe4f6612877d000 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/InstanceView.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/InstanceView.h
@@ -84,7 +84,9 @@ namespace armarx::armem::gui::instance
QAction* makeActionCopyMemoryID(const MemoryID& id);
std::vector<QAction*> makeActionsCopyDataToClipboard();
std::vector<QAction*> makeActionsCopyDataToClipboard(const aron::Path& path);
- std::vector<QAction*> makeCopyActions(const aron::data::VariantPtr& element);
+ std::vector<QAction*> makeCopyActions(
+ const aron::data::VariantPtr& element,
+ const aron::type::VariantPtr& elementType);
private:
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.cpp b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.cpp
index eae4cb934e7c878c2c2f7787f0ef0cd96a2f9775..de85c11c08080c88c5f5d72f5b00900547a66fbc 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.cpp
@@ -12,6 +12,7 @@
#include <RobotAPI/libraries/armem_gui/instance/sanitize_typename.h>
#include <RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.h>
#include <RobotAPI/libraries/armem_gui/instance/MemoryIDTreeWidgetItem.h>
+#include <RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.h>
namespace armarx::armem::gui::instance
@@ -28,21 +29,18 @@ namespace armarx::armem::gui::instance
const aron::data::Dict& data)
{
auto childType = type.getAcceptedType();
- if (childType)
+ DictBuilder builder = getDictBuilder();
+ builder.setUpdateItemFn([this, &childType, &data](const std::string & key, QTreeWidgetItem * item)
{
- DictBuilder builder = getDictBuilder();
- builder.setUpdateItemFn([this, &childType, &data](const std::string & key, QTreeWidgetItem * item)
+ auto childData = data.getElement(key);
+ if (childData)
{
- auto childData = data.getElement(key);
- if (childData)
- {
- this->update(item, key, childType, childData);
- }
- return true;
- });
-
- builder.updateTreeWithContainer(parent, data.getAllKeys());
- }
+ this->updateDispatch(item, key, childType, childData);
+ }
+ return true;
+ });
+
+ builder.updateTreeWithContainer(parent, data.getAllKeys());
}
@@ -54,7 +52,8 @@ namespace armarx::armem::gui::instance
DictBuilder builder = getDictBuilder();
builder.setMakeItemFn([this, &type](const std::string & key) -> QTreeWidgetItem*
{
- if (type.getMemberType(key)->getFullName() == instance::rawMemoryIDTypeName)
+ if (type.hasMemberType(key)
+ && type.getMemberType(key)->getFullName() == instance::rawMemoryIDTypeName)
{
MemoryIDTreeWidgetItem* item = new MemoryIDTreeWidgetItem({QString::fromStdString(key)});
item->addKeyChildren();
@@ -67,17 +66,22 @@ namespace armarx::armem::gui::instance
});
builder.setUpdateItemFn([this, &type, &data](const std::string & key, QTreeWidgetItem * item)
{
- auto childType = type.getMemberType(key);
auto childData = data.getElement(key);
- if (childType)
+ // We need this check here because getMemberType(key) throws
+ // instead of returning nullptr if the type doesn't have the key.
+ if (type.hasMemberType(key))
{
- this->update(item, key, childType, childData);
+ this->updateDispatch(item, key, type.getMemberType(key), childData);
+ }
+ else
+ {
+ this->updateDispatch(item, key, nullptr, childData);
}
return true;
});
- builder.updateTreeWithContainer(parent, type.getAllKeys());
+ builder.updateTreeWithContainer(parent, data.getAllKeys());
}
@@ -86,22 +90,19 @@ namespace armarx::armem::gui::instance
const aron::data::List& data)
{
auto childType = type.getAcceptedType();
- if (childType)
- {
- auto children = data.getChildren();
+ auto children = data.getChildren();
- ListBuilder builder = getListBuilder();
- builder.setUpdateItemFn([this, &children, &childType](size_t key, QTreeWidgetItem * item)
+ ListBuilder builder = getListBuilder();
+ builder.setUpdateItemFn([this, &children, &childType](size_t key, QTreeWidgetItem * item)
+ {
+ if (auto childData = children.at(key))
{
- if (auto childData = children.at(key))
- {
- this->update(item, std::to_string(key), childType, childData);
- }
- return true;
- });
-
- builder.updateTreeWithContainer(parent, getIndex(children.size()));
- }
+ this->updateDispatch(item, std::to_string(key), childType, childData);
+ }
+ return true;
+ });
+
+ builder.updateTreeWithContainer(parent, getIndex(children.size()));
}
@@ -119,10 +120,7 @@ namespace armarx::armem::gui::instance
auto childType = i == 0 ? childTypes.first : childTypes.second;
auto childData = data.getElement(static_cast<unsigned int>(i));
- if (childType)
- {
- this->update(item, std::to_string(i), childType, childData);
- }
+ this->updateDispatch(item, std::to_string(i), childType, childData);
return true;
});
@@ -135,22 +133,43 @@ namespace armarx::armem::gui::instance
const aron::type::Tuple& type,
const aron::data::List& data)
{
+ // Allows tuples where the data list is longer than the type tuple -
+ // is that desired behavior?
+
auto childTypes = type.getAcceptedTypes();
ListBuilder builder = getListBuilder();
builder.setUpdateItemFn([this, &data, &childTypes](size_t i, QTreeWidgetItem * item)
{
- auto childType = childTypes.at(i);
+ auto childType = (i < childTypes.size()) ? childTypes.at(i) : nullptr;
auto childData = data.getElement(static_cast<unsigned int>(i));
- if (childType)
- {
- this->update(item, std::to_string(i), childType, childData);
- }
+ this->updateDispatch(item, std::to_string(i), childType, childData);
return true;
});
- builder.updateTreeWithContainer(parent, getIndex(type.getAcceptedTypes().size()));
+ builder.updateTreeWithContainer(parent, getIndex(data.childrenSize()));
+ }
+
+
+ /*! Used so that elements in the data that don't appear in the type
+ * can still be shown in the GUI if type information is enabled
+ * (otherwise, they would be hidden).
+ */
+ void TypedDataTreeBuilder::updateDispatch(
+ QTreeWidgetItem* item,
+ const std::string& key,
+ const aron::type::VariantPtr& type,
+ const aron::data::VariantPtr& data)
+ {
+ if (type)
+ {
+ this->update(item, key, type, data);
+ }
+ else
+ {
+ this->update(item, key, data);
+ }
}
@@ -217,6 +236,36 @@ namespace armarx::armem::gui::instance
_updateTree(item, *t, *d);
}
// else???
+ // phesch: else we stop here, since there's no container to recurse into.
+ }
+ }
+
+
+ void TypedDataTreeBuilder::update(QTreeWidgetItem* item,
+ const std::string& key,
+ const aron::data::VariantPtr& data)
+ {
+ if (data)
+ {
+ this->setRowTexts(item, key, data);
+
+ item->setData(columnKey, Qt::UserRole,
+ data ? instance::serializePath(data->getPath()) : QStringList());
+
+ if (auto cast = aron::data::Dict::DynamicCast(data))
+ {
+ DataTreeBuilder builder;
+ builder.updateTree(item, cast);
+ }
+ else if (auto cast = aron::data::List::DynamicCast(data))
+ {
+ DataTreeBuilder builder;
+ builder.updateTree(item, cast);
+ }
+ }
+ else
+ {
+ this->setRowTexts(item, key, "(none)");
}
}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.h b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.h
index b4e6dc91bc963ac799e5230f95f5e46c7fb79ebb..b8aacc686df4dbf2119d963f5fc163aabe0c52bf 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.h
@@ -49,11 +49,20 @@ namespace armarx::armem::gui::instance
protected:
+ void updateDispatch(QTreeWidgetItem* item,
+ const std::string& key,
+ const aron::type::VariantPtr& type,
+ const aron::data::VariantPtr& data);
+
void update(QTreeWidgetItem* item,
const std::string& key,
const aron::type::VariantPtr& type,
const aron::data::VariantPtr& data);
+ void update(QTreeWidgetItem* item,
+ const std::string& key,
+ const aron::data::VariantPtr& data);
+
template <class DataT, class TypeT>
void _updateTree(QTreeWidgetItem* item, TypeT& type, DataT& data);
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedJSONConverter.cpp b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedJSONConverter.cpp
index d0f2f4c77863e680bc644a219370a41aac8f4a79..f80717342b3bc98dc1ebf9b7a72a4b274ede768a 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedJSONConverter.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedJSONConverter.cpp
@@ -4,10 +4,10 @@
#include <ArmarXCore/core/logging/Logging.h>
-#include <RobotAPI/libraries/aron/core/data/variant/All.h>
-#include <RobotAPI/libraries/aron/core/Exception.h>
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/aron/converter/eigen/EigenConverter.h>
+#include <RobotAPI/libraries/aron/core/Exception.h>
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
namespace armarx::armem::gui::instance
{
@@ -27,6 +27,34 @@ namespace armarx::armem::gui::instance
return obj;
}
+ /* We override this method because we need to handle untyped members in the hierarchy.
+ * The other get*Elements() methods will either not be called with a null type or can handle it.
+ */
+ TreeTypedJSONConverter::MapElements
+ TreeTypedJSONConverter::getObjectElements(DataInput& elementData, TypeInput& elementType)
+ {
+ std::map<std::string, std::pair<aron::data::VariantPtr, aron::type::VariantPtr>> ret;
+ auto data = aron::data::Dict::DynamicCastAndCheck(elementData);
+ auto type = aron::type::Object::DynamicCastAndCheck(elementType);
+
+ if (data)
+ {
+ for (const auto& [key, e] : data->getElements())
+ {
+ if (type && type->hasMemberType(key))
+ {
+ auto memberType = type->getMemberType(key);
+ ret.insert({key, {e, memberType}});
+ }
+ else
+ {
+ ret.insert({key, {e, nullptr}});
+ }
+ }
+ }
+ return ret;
+ }
+
void
TreeTypedJSONConverter::visitObjectOnEnter(DataInput& elementData, TypeInput& /*elementType*/)
{
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedJSONConverter.h b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedJSONConverter.h
index 7a112979e7baa9e01460cc98d00c3d74a0d846e5..901021de0b67a556cf5adcc62635ebee8704d9d9 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedJSONConverter.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedJSONConverter.h
@@ -17,6 +17,8 @@ namespace armarx::armem::gui::instance
const nlohmann::json& getJSON();
+ MapElements getObjectElements(DataInput& elementData, TypeInput& elementType) override;
+
void visitObjectOnEnter(DataInput& elementData, TypeInput& elementType) override;
void visitObjectOnExit(DataInput& elementData, TypeInput& elementType) override;
void visitDictOnEnter(DataInput& elementData, TypeInput& elementType) override;
diff --git a/source/RobotAPI/libraries/armem_mps/aron_conversions.h b/source/RobotAPI/libraries/armem_mps/aron_conversions.h
index 8de8754f9f656c61073aea2702910b50bf844cb6..343975886fdf6b6041189500ae40b5662701c5a4 100644
--- a/source/RobotAPI/libraries/armem_mps/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_mps/aron_conversions.h
@@ -3,7 +3,7 @@
#include <ArmarXCore/interface/core/Profiler.h>
#include <ArmarXCore/observers/ObserverObjectFactories.h>
-#include <RobotAPI/libraries/aron/common/aron/Trajectory.aron.generated.h>
+#include <RobotAPI/libraries/aron/common/aron/trajectory.aron.generated.h>
#include <dmp/representation/trajectory.h>
//#include <dmp
diff --git a/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/Segment.h b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/Segment.h
index ab14043ea70b511cacefba9409c8eb001d5f316d..f38c3c114b49e33b7f411450d11f0996f8ac5eb8 100644
--- a/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/Segment.h
+++ b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/Segment.h
@@ -8,7 +8,7 @@
#include <RobotAPI/libraries/armem/server/segment/SpecializedSegment.h>
// ArmarX
-#include <RobotAPI/libraries/aron/common/aron/Trajectory.aron.generated.h>
+#include <RobotAPI/libraries/aron/common/aron/trajectory.aron.generated.h>
namespace armarx::armem::server::motions::mps::segment
{
diff --git a/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.cpp b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.cpp
index 080766eafbaf6bc20e84a8fec0a0912ceac6cfc2..32f0302a2b33486f6c4336b278059c6db5cf9208 100644
--- a/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.cpp
+++ b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.cpp
@@ -8,8 +8,6 @@
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
-#include <RobotAPI/libraries/aron/common/aron/Trajectory.aron.generated.h>
-
#include <dmp/representation/trajectory.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
diff --git a/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.h b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.h
index 186074ce0dec8222017b0fb4a420beb227890739..72372ca2fa79a39e1a8ed022f4104627353f9727 100644
--- a/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.h
+++ b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.h
@@ -7,7 +7,7 @@
#include <optional>
// ArmarX
-#include <RobotAPI/libraries/aron/common/aron/Trajectory.aron.generated.h>
+#include <RobotAPI/libraries/aron/common/aron/trajectory.aron.generated.h>
namespace armarx::armem::server::motions::mps::segment::util
{
diff --git a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp
index 49dbdcf6140717e63ceec39e8cbf7d0f9b91dd77..861ff14b97addd9b87fee4a834bd605f20b2b681 100644
--- a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp
@@ -55,6 +55,7 @@ namespace armarx::armem::obj::instance
}
}
+ /// get the latest object from an memory and cast it to an ObjectInstance
std::optional<armarx::armem::arondto::ObjectInstance> Reader::queryObject(const armem::wm::Memory& memory, const armem::Time& timestamp)
{
// clang-format off
@@ -73,6 +74,7 @@ namespace armarx::armem::obj::instance
return armem::arondto::ObjectInstance::FromAron(instance->data());
}
+ /// Query an object with full name entityName (e.g. Kitchen/green-cup/0)
std::optional<armarx::armem::arondto::ObjectInstance> Reader::queryObjectByEntityID(const std::string& entityName, const armem::Time& timestamp)
{
// Query all entities from all provider.
@@ -98,6 +100,7 @@ namespace armarx::armem::obj::instance
return queryObject(qResult.memory, timestamp);
}
+ /// Query an object by objectId (e.g. Kitchen/green-cup in Entity Kitchen/green-cup/0)
std::optional<armarx::armem::arondto::ObjectInstance> Reader::queryObjectByObjectID(const std::string& objectId, const armem::Time& timestamp)
{
// Query all entities from all provider.
diff --git a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h
index 8db6a1cab642e103dd6c25105b607c47651ccb15..825687d5b6bf17acad4515841e9852c9f020dbed 100644
--- a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h
+++ b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h
@@ -44,15 +44,86 @@ namespace armarx::armem::obj::instance
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
void connect();
-
std::optional<armem::arondto::ObjectInstance> queryObject(const armem::wm::Memory& memory, const armem::Time&);
std::optional<armem::arondto::ObjectInstance> queryObjectByEntityID(const std::string& entityName, const armem::Time&);
std::optional<armem::arondto::ObjectInstance> queryObjectByObjectID(const std::string& objectId, const armem::Time&);
+ // return the class name, e.g. Kitchen/greencup in Kitchen/greencup/0
+ static std::string GetObjectId(const std::string& s)
+ {
+ auto split = simox::alg::split(s, "/");
+ if (simox::alg::starts_with(s, "/"))
+ {
+ split.insert(split.begin(), ""); // sanitize
+ }
+
+ for (auto& e : split)
+ {
+ e = simox::alg::replace_all(e, "/", "");
+ }
+
+ if (IsEntityId(s)) return (split[0] + "/" + split[1]);
+ if (IsObjectId(s)) return s;
+
+ ARMARX_ERROR << "Unknown structure for object id '" << s << "'.";
+ return "";
+ }
+
+ // return the class name, e.g. greencup in Kitchen/greencup/0
+ static std::string GetObjectClassName(const std::string& s)
+ {
+ auto split = simox::alg::split(s, "/");
+ if (simox::alg::starts_with(s, "/"))
+ {
+ split.insert(split.begin(), ""); // sanitize
+ }
+
+ for (auto& e : split)
+ {
+ e = simox::alg::replace_all(e, "/", "");
+ }
+
+ if (IsEntityId(s)) return split[1];
+ if (IsObjectId(s)) return split[1];
+
+ ARMARX_ERROR << "Unknown structure for object id '" << s << "'.";
+ return "";
+ }
+
+ // check if s matches ??/??/??
+ static bool IsEntityId(const std::string& s)
+ {
+ auto split = simox::alg::split(s, "/");
+ if (simox::alg::starts_with(s, "/"))
+ {
+ split.insert(split.begin(), ""); // sanitize
+ }
+
+ if (split.size() != 3)
+ {
+ return false;
+ }
+ return true;
+ }
+
+ // check if s matches ??/??
+ static bool IsObjectId(const std::string& s)
+ {
+ auto split = simox::alg::split(s, "/");
+ if (simox::alg::starts_with(s, "/"))
+ {
+ split.insert(split.begin(), ""); // sanitize
+ }
+
+ if (split.size() != 2)
+ {
+ return false;
+ }
+ return true;
+ }
private:
-
struct Properties
{
std::string memoryName = "Object";
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index 485715690f3fdbfe089f3557582bae86ef7210ad..351d0f5ac4ef71a4e2af10a460bb796e227370f5 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -32,6 +32,7 @@
#include <ArmarXCore/core/time/TimeUtil.h>
#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <SimoxUtility/algorithm/string.h>
#include <SimoxUtility/json.h>
#include <SimoxUtility/math/pose/pose.h>
@@ -100,8 +101,9 @@ namespace armarx::armem::server::obj::instance
defs->optional(p.sceneSnapshotsDirectory, prefix + "scene.11_Directory",
"Directory in Package/data/Package/ containing the scene snapshots.");
defs->optional(p.sceneSnapshotToLoad, prefix + "scene.12_SnapshotToLoad",
- "Scene snapshot to load on startup (e.g. 'Scene_2021-06-24_20-20-03').\n"
- "You can also specify paths relative to 'Package/scenes/'.");
+ "Scene to load on startup (e.g. 'Scene_2021-06-24_20-20-03').\n"
+ "You can also specify paths relative to 'Package/scenes/'. \n"
+ "You can also specify a ; separated list of scenes.");
decay.defineProperties(defs, prefix + "decay.");
}
@@ -113,8 +115,13 @@ namespace armarx::armem::server::obj::instance
if (not p.sceneSnapshotToLoad.empty())
{
- bool lockMemory = false;
- commitSceneSnapshotFromFilename(p.sceneSnapshotToLoad, lockMemory);
+ bool trim = true;
+ const std::vector<std::string> scenes = simox::alg::split(p.sceneSnapshotToLoad, ";", trim);
+ for (const std::string& scene : scenes)
+ {
+ const bool lockMemory = false;
+ commitSceneSnapshotFromFilename(scene, lockMemory);
+ }
}
}
@@ -808,13 +815,13 @@ namespace armarx::armem::server::obj::instance
{
if (const std::optional<std::filesystem::path> path = resolveSceneFilename(filename))
{
- const nlohmann::json j = scene;
- ARMARX_INFO << "Storing scene snapshot at: \n" << path.value() << "\n\n" << j.dump(2);
+
+ ARMARX_INFO << "Storing scene snapshot at: \n" << path.value();
try
{
- nlohmann::write_json(path.value(), j, 2);
+ simox::json::write(path.value(), scene, 2);
}
- catch (const std::ios_base::failure& e)
+ catch (const simox::json::error::JsonError& e)
{
ARMARX_WARNING << "Storing scene snapshot failed: \n" << e.what();
}
@@ -839,20 +846,15 @@ namespace armarx::armem::server::obj::instance
std::optional<armarx::objects::Scene>
Segment::loadScene(const std::filesystem::path& path)
{
- ARMARX_INFO << "Loading scene snapshot from: \n" << path;
- nlohmann::json j;
try
{
- j = nlohmann::read_json(path);
+ return simox::json::read<armarx::objects::Scene>(path);
}
- catch (const std::ios_base::failure& e)
+ catch (const simox::json::error::JsonError& e)
{
ARMARX_WARNING << "Loading scene snapshot failed: \n" << e.what();
return std::nullopt;
}
-
- armarx::objects::Scene snapshot = j;
- return snapshot;
}
@@ -936,7 +938,8 @@ namespace armarx::armem::server::obj::instance
pose.providerName = sceneName;
pose.objectType = objpose::ObjectType::KnownObject;
- pose.isStatic = true; // Objects loaded from prior knowledge are considerd static to exclude them from decay.
+ // If not specified, assume loaded objects are static.
+ pose.isStatic = object.isStatic.has_value() ? object.isStatic.value() : true;
pose.objectID = classID.withInstanceName(
object.instanceName.empty()
? std::to_string(idCounters[classID]++)
@@ -972,6 +975,7 @@ namespace armarx::armem::server::obj::instance
std::filesystem::path filename = path->filename();
filename.replace_extension(); // Removes extension
+ // The check seems useless?
if (lockMemory)
{
commitSceneSnapshot(snapshot.value(), filename.string());
diff --git a/source/RobotAPI/libraries/armem_reasoning/CMakeLists.txt b/source/RobotAPI/libraries/armem_reasoning/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..9da6213e75a9c53e041ab5060537dd9122d9a1c2
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_reasoning/CMakeLists.txt
@@ -0,0 +1,25 @@
+set(LIB_NAME armem_reasoning)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+
+armarx_add_library(
+ LIBS
+ ArmarXCoreInterfaces ArmarXCore
+ RobotAPICore aron armem
+
+ SOURCES
+ aron_conversions.cpp
+
+ HEADERS
+ aron_conversions.h
+
+ ARON_FILES
+ aron/Anticipation.xml
+)
+
+
+add_library(RobotAPI::armem_reasoning ALIAS ${LIB_NAME})
+
+add_subdirectory(server)
diff --git a/source/RobotAPI/libraries/armem_reasoning/aron/Anticipation.xml b/source/RobotAPI/libraries/armem_reasoning/aron/Anticipation.xml
new file mode 100644
index 0000000000000000000000000000000000000000..a3f481c630d573970ace675032a13becc9ca57fe
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_reasoning/aron/Anticipation.xml
@@ -0,0 +1,20 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+
+ <CodeIncludes>
+ <Include include="<RobotAPI/libraries/armem/aron/MemoryID.aron.generated.h>" />
+ </CodeIncludes>
+ <AronIncludes>
+ <Include include="<RobotAPI/libraries/armem/aron/MemoryID.xml>" autoinclude="true" />
+ </AronIncludes>
+
+ <GenerateTypes>
+ <Object name='armarx::reasoning::arondto::Anticipation'>
+ <ObjectChild key='predictions'>
+ <List optional='true'>
+ <string />
+ </List>
+ </ObjectChild>
+ </Object>
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/armem_reasoning/aron_conversions.cpp b/source/RobotAPI/libraries/armem_reasoning/aron_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e721f12a9349747d7d2b3c6e69a4bc223e5d76ad
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_reasoning/aron_conversions.cpp
@@ -0,0 +1,28 @@
+/*
+ * 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 VisionX::ArmarXObjects::armem_images_server
+ * @author Markus Grotz ( markus dot grotz at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "aron_conversions.h"
+
+namespace armarx::armem::server::reasoning
+{
+
+}
diff --git a/source/RobotAPI/libraries/armem_reasoning/aron_conversions.h b/source/RobotAPI/libraries/armem_reasoning/aron_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..2e4d7c8bbd2206ed184cfe7e01c2898d81523b04
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_reasoning/aron_conversions.h
@@ -0,0 +1,31 @@
+/*
+ * 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 VisionX::ArmarXObjects::armem_images_server
+ * @author Markus Grotz ( markus dot grotz at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/libraries/aron/core/data/variant/forward_declarations.h>
+#include <RobotAPI/libraries/armem_reasoning/aron/Anticipation.aron.generated.h>
+
+namespace armarx::armem::server::reasoning
+{
+
+}
diff --git a/source/RobotAPI/libraries/armem_reasoning/server/AnticipationSegment.cpp b/source/RobotAPI/libraries/armem_reasoning/server/AnticipationSegment.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bf2d89bb748ccf788288bc9632acf55937877125
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_reasoning/server/AnticipationSegment.cpp
@@ -0,0 +1,39 @@
+/*
+ * 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::ArmarXObjects::armem_images_server
+ * @author Markus Grotz ( markus dot grotz at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "AnticipationSegment.h"
+
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+
+namespace armarx::armem::server::reasoning
+{
+ AnticipationSegment::AnticipationSegment(armem::server::MemoryToIceAdapter& iceMemory) :
+ Base(iceMemory, CORE_SEGMENT_NAME)
+ {
+
+ }
+
+ void AnticipationSegment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ Base::defineProperties(defs, prefix);
+ }
+}
diff --git a/source/RobotAPI/libraries/armem_reasoning/server/AnticipationSegment.h b/source/RobotAPI/libraries/armem_reasoning/server/AnticipationSegment.h
new file mode 100644
index 0000000000000000000000000000000000000000..3a05c7adb5b9e7d96d51c2e07a6c18f016bc506a
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_reasoning/server/AnticipationSegment.h
@@ -0,0 +1,46 @@
+/*
+ * 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 VisionX::ArmarXObjects::armem_images_server
+ * @author Markus Grotz ( markus dot grotz at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+#include <string>
+
+#include <RobotAPI/libraries/armem/server/segment/SpecializedSegment.h>
+
+#include <RobotAPI/libraries/armem_reasoning/aron/Anticipation.aron.generated.h>
+
+namespace armarx::armem::server::reasoning
+{
+ class AnticipationSegment : public armem::server::segment::SpecializedCoreSegment
+ {
+ public:
+ using Base = armem::server::segment::SpecializedCoreSegment;
+
+ AnticipationSegment(armem::server::MemoryToIceAdapter& iceMemory);
+
+ virtual void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override;
+
+ public:
+ const std::string CORE_SEGMENT_NAME = "Anticipation";
+ };
+}
diff --git a/source/RobotAPI/libraries/armem_reasoning/server/CMakeLists.txt b/source/RobotAPI/libraries/armem_reasoning/server/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..21b041702717541f4def5b8b65705c4dd6463185
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_reasoning/server/CMakeLists.txt
@@ -0,0 +1,22 @@
+set(ARMARX_LIB_NAME "" )
+set(ARON_FILES "")
+
+set(LIB_NAME armem_reasoning_server)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+armarx_add_library(
+ LIBS
+ armem_reasoning
+ armem_server
+
+ SOURCES
+ AnticipationSegment.cpp
+
+ HEADERS
+ AnticipationSegment.h
+)
+
+
+add_library(RobotAPI::armem_reasoning_server ALIAS ${LIB_NAME})
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
index 6610085a42af19314fdd5f3a062da01dcd1f8cb2..b3dee3440967cf79ae9feee73403116345a23cbf 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
@@ -1,7 +1,9 @@
#include "RobotReader.h"
+#include <chrono>
#include <mutex>
#include <optional>
+#include <thread>
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/exceptions/LocalException.h>
@@ -91,19 +93,48 @@ namespace armarx::armem::robot_state
return robot;
}
+ void
+ RobotReader::setSyncTimeout(const armem::Time& duration)
+ {
+ syncTimeout = duration;
+ }
+
+ void
+ RobotReader::setSleepAfterSyncFailure(const armem::Time& duration)
+ {
+ ARMARX_CHECK_NONNEGATIVE(duration.toMicroSeconds());
+ sleepAfterFailure = duration;
+ }
+
bool
RobotReader::synchronize(robot::Robot& obj, const armem::Time& timestamp)
{
- auto state = queryState(obj.description, timestamp);
+ const auto tsStartFunctionInvokation = armem::Time::now();
- if (not state) /* c++20 [[unlikely]] */
+ while (true)
{
- ARMARX_WARNING << "Could not synchronize object " << obj.description.name;
- return false;
- }
+ auto state = queryState(obj.description, timestamp);
+
+ if (not state) /* c++20 [[unlikely]] */
+ {
+ ARMARX_VERBOSE << "Could not synchronize object " << obj.description.name;
+
+ // if the syncTime is non-positive there will be no retry
+ const auto elapsedTime = armem::Time::now() - tsStartFunctionInvokation;
+ if (elapsedTime > syncTimeout)
+ {
+ ARMARX_WARNING << "Could not synchronize object " << obj.description.name;
+ return false;
+ }
+
+ ARMARX_INFO << "Retrying to query robot state after failure";
+ std::this_thread::sleep_for(
+ std::chrono::microseconds(sleepAfterFailure.toMicroSeconds()));
+ }
- obj.config = std::move(*state);
- return true;
+ obj.config = std::move(*state);
+ return true;
+ }
}
std::optional<robot::RobotDescription>
@@ -117,7 +148,7 @@ namespace armarx::armem::robot_state
.coreSegments().all() // withName(properties.descriptionCoreSegment)
.providerSegments().all() // .withName(name)
.entities().all()
- .snapshots().latest(); // TODO(fabian.reister): atTime(timestamp);
+ .snapshots().beforeOrAtTime(timestamp);
// clang-format on
ARMARX_DEBUG << "Lookup query in reader";
@@ -188,7 +219,7 @@ namespace armarx::armem::robot_state
.coreSegments().withName(properties.proprioceptionCoreSegment)
.providerSegments().withName(description.name) // agent
.entities().all() // TODO
- .snapshots().latest();
+ .snapshots().beforeOrAtTime(timestamp);
// clang-format on
const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
@@ -252,7 +283,7 @@ namespace armarx::armem::robot_state
.coreSegments().withName(properties.proprioceptionCoreSegment)
.providerSegments().withName(description.name) // agent
.entities().all() // TODO
- .snapshots().latest();
+ .snapshots().beforeOrAtTime(timestamp);
// clang-format on
const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
@@ -322,8 +353,8 @@ namespace armarx::armem::robot_state
std::optional<AronClass>
tryCast(const wm::EntityInstance& item)
{
- static_assert(
- std::is_base_of<armarx::aron::codegenerator::cpp::AronGeneratedClass, AronClass>::value);
+ static_assert(std::is_base_of<armarx::aron::codegenerator::cpp::AronGeneratedClass,
+ AronClass>::value);
try
{
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
index 20cafbd2225160c7b9abacc2d73b981f60260e34..46b0924fc2cb06ebee34ff8b61174746939dd7dc 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
@@ -24,6 +24,7 @@
#include <mutex>
#include <optional>
+#include "RobotAPI/libraries/armem/core/Time.h"
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem_robot/client/interfaces.h>
@@ -67,10 +68,10 @@ namespace armarx::armem::robot_state
const armem::Time& timestamp) const;
using JointTrajectory = std::map<armem::Time, robot::RobotState::JointMap>;
-
- JointTrajectory
- queryJointStates(const robot::RobotDescription& description,
- const armem::Time& begin, const armem::Time& end) const;
+
+ JointTrajectory queryJointStates(const robot::RobotDescription& description,
+ const armem::Time& begin,
+ const armem::Time& end) const;
std::optional<robot::RobotState::Pose>
queryGlobalPose(const robot::RobotDescription& description,
@@ -80,6 +81,8 @@ namespace armarx::armem::robot_state
queryPlatformState(const robot::RobotDescription& description,
const armem::Time& timestamp) const;
+ void setSyncTimeout(const armem::Time& duration);
+ void setSleepAfterSyncFailure(const armem::Time& duration);
enum class Hand
{
@@ -96,6 +99,11 @@ namespace armarx::armem::robot_state
const armem::Time& start,
const armem::Time& end) const;
+ protected:
+ // by default, no timeout mechanism
+ armem::Time syncTimeout = armem::Time::microSeconds(0);
+ armem::Time sleepAfterFailure = armem::Time::microSeconds(0);
+
private:
std::optional<robot::RobotState> getRobotState(const armarx::armem::wm::Memory& memory,
const std::string& name) const;
@@ -106,9 +114,8 @@ namespace armarx::armem::robot_state
std::optional<robot::RobotState::JointMap>
getRobotJointState(const armarx::armem::wm::Memory& memory, const std::string& name) const;
- JointTrajectory
- getRobotJointStates(const armarx::armem::wm::Memory& memory,
- const std::string& name) const;
+ JointTrajectory getRobotJointStates(const armarx::armem::wm::Memory& memory,
+ const std::string& name) const;
std::optional<robot::PlatformState>
getRobotPlatformState(const armarx::armem::wm::Memory& memory,
@@ -117,7 +124,7 @@ namespace armarx::armem::robot_state
std::map<RobotReader::Hand, robot::ForceTorque>
getForceTorque(const armarx::armem::wm::Memory& memory, const std::string& name) const;
-
+
std::map<RobotReader::Hand, std::map<armem::Time, robot::ForceTorque>>
getForceTorques(const armarx::armem::wm::Memory& memory, const std::string& name) const;
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
index 4dea491c8e0a2eec9b8c04677433977263ffdda9..5ef7564f8a0e374060114d0391bfaeebf1df6198 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
@@ -1,8 +1,10 @@
#include "VirtualRobotReader.h"
#include <optional>
+#include <thread>
#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/XML/RobotIO.h>
#include <ArmarXCore/core/PackagePath.h>
@@ -19,30 +21,33 @@ namespace armarx::armem::robot_state
{
}
- void VirtualRobotReader::connect()
+ void
+ VirtualRobotReader::connect()
{
RobotReader::connect();
}
// TODO(fabian.reister): register property defs
- void VirtualRobotReader::registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def)
+ void
+ VirtualRobotReader::registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def)
{
RobotReader::registerPropertyDefinitions(def);
}
- bool VirtualRobotReader::synchronizeRobot(VirtualRobot::Robot& robot,
- const armem::Time& timestamp)
+ bool
+ VirtualRobotReader::synchronizeRobot(VirtualRobot::Robot& robot, const armem::Time& timestamp)
{
const auto packages = armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
const auto package = armarx::ArmarXDataPath::getProject(packages, robot.getFilename());
- const robot::RobotDescription robotDescription{.name = robot.getName(),
- .xml = PackagePath{package, robot.getFilename()}};
+ const robot::RobotDescription robotDescription{
+ .name = robot.getName(), .xml = PackagePath{package, robot.getFilename()}};
const auto robotState = queryState(robotDescription, timestamp);
if (not robotState)
{
- ARMARX_WARNING << "Querying robot state failed for robot `" << robot.getName() << " / " << robot.getType() << "`!";
+ ARMARX_WARNING << "Querying robot state failed for robot `" << robot.getName() << " / "
+ << robot.getType() << "`!";
return false;
}
@@ -52,9 +57,10 @@ namespace armarx::armem::robot_state
return true;
}
- VirtualRobot::RobotPtr VirtualRobotReader::getRobot(const std::string& name,
- const armem::Time& timestamp,
- const VirtualRobot::RobotIO::RobotDescription& loadMode)
+ VirtualRobot::RobotPtr
+ VirtualRobotReader::getRobot(const std::string& name,
+ const armem::Time& timestamp,
+ const VirtualRobot::RobotIO::RobotDescription& loadMode)
{
ARMARX_INFO << "Querying robot description for robot '" << name << "'";
const auto description = queryDescription(name, timestamp);
@@ -64,8 +70,10 @@ namespace armarx::armem::robot_state
return nullptr;
}
- const std::string xmlFilename = ArmarXDataPath::resolvePath(description->xml.serialize().path);
- ARMARX_INFO << "Loading (virtual) robot '" << description->name << "' from XML file '" << xmlFilename << "'";
+ const std::string xmlFilename =
+ ArmarXDataPath::resolvePath(description->xml.serialize().path);
+ ARMARX_INFO << "Loading (virtual) robot '" << description->name << "' from XML file '"
+ << xmlFilename << "'";
auto robot = VirtualRobot::RobotIO::loadRobot(xmlFilename, loadMode);
robot->setName(name);
@@ -76,15 +84,28 @@ namespace armarx::armem::robot_state
}
- VirtualRobot::RobotPtr VirtualRobotReader::getSynchronizedRobot(const std::string& name,
- const armem::Time& timestamp,
- const VirtualRobot::RobotIO::RobotDescription& loadMode)
+ VirtualRobot::RobotPtr
+ VirtualRobotReader::getSynchronizedRobot(
+ const std::string& name,
+ const armem::Time& timestamp,
+ const VirtualRobot::RobotIO::RobotDescription& loadMode,
+ const bool blocking)
{
- auto robot = getRobot(name, timestamp, loadMode);
-
- synchronizeRobot(*robot, timestamp);
+ while (blocking)
+ {
+ VirtualRobot::RobotPtr robot = getRobot(name, timestamp, loadMode);
+ if (robot and synchronizeRobot(*robot, timestamp))
+ {
+ return robot;
+ }
+
+ ARMARX_INFO << "Retrying to query robot after failure";
+ std::this_thread::sleep_for(
+ std::chrono::microseconds(sleepAfterFailure.toMicroSeconds()));
+ }
- return robot;
+ ARMARX_WARNING << "Failed to get synchronized robot `" << name << "`";
+ return nullptr;
}
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
index 68c0d5e7df911f23b06fcc582108bbf9f205e49c..c14e60aee78a490bd1ffb0937ca008dc56895499 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
@@ -21,12 +21,12 @@
#pragma once
-#include "RobotReader.h"
-
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/XML/RobotIO.h>
+#include "RobotReader.h"
+
namespace armarx::armem::robot_state
{
@@ -42,7 +42,7 @@ namespace armarx::armem::robot_state
{
public:
VirtualRobotReader(armem::client::MemoryNameSystem& memoryNameSystem);
- virtual ~VirtualRobotReader() = default;
+ ~VirtualRobotReader() override = default;
void connect();
void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def);
@@ -59,7 +59,8 @@ namespace armarx::armem::robot_state
getSynchronizedRobot(const std::string& name,
const armem::Time& timestamp,
const VirtualRobot::RobotIO::RobotDescription& loadMode =
- VirtualRobot::RobotIO::RobotDescription::eStructure);
+ VirtualRobot::RobotIO::RobotDescription::eStructure,
+ bool blocking = true);
};
} // namespace armarx::armem::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
index 2b2c942490e2b7527552881cbf1be5e8a4c11cff..21b8e55a9fa9c74d63f61dc67aea86bce381c01f 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
@@ -151,7 +151,7 @@ namespace armarx::armem::client::robot_state::localization
.coreSegments().withName(properties.localizationSegment)
.providerSegments().withName(query.header.agent) // agent
.entities().all() // parentFrame,frame
- .snapshots().latest();
+ .snapshots().beforeOrAtTime(query.header.timestamp);
// clang-format on
const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
diff --git a/source/RobotAPI/libraries/armem_skills/aron/Skill.xml b/source/RobotAPI/libraries/armem_skills/aron/Skill.xml
index 0e539ff08a436fee457a773a7dad2700ca19e8a2..b546afb89779ada2764d83faeb23684cd189ef50 100644
--- a/source/RobotAPI/libraries/armem_skills/aron/Skill.xml
+++ b/source/RobotAPI/libraries/armem_skills/aron/Skill.xml
@@ -34,7 +34,10 @@ The memory should look like the following:
<long />
</ObjectChild>
- <!-- accepted type as any type -->
+ <ObjectChild key='acceptedType'>
+ <string />
+ </ObjectChild>
+
</Object>
<Object name='armarx::skills::arondto::SkillExecutionRequest'>
@@ -50,7 +53,10 @@ The memory should look like the following:
<String />
</ObjectChild>
- <!-- ToDo: add params wih any type -->
+ <ObjectChild key='params'>
+ <AnyObject shared_ptr="1" />
+ </ObjectChild>
+
</Object>
<Object name='armarx::skills::arondto::SkillExecutionEvent'>
@@ -66,8 +72,13 @@ The memory should look like the following:
<String />
</ObjectChild>
- <!-- ToDo: add params with any type -->
- <!-- ToDo: add result with any type -->
+ <ObjectChild key='params'>
+ <AnyObject shared_ptr="1" />
+ </ObjectChild>
+
+ <ObjectChild key='data'>
+ <AnyObject shared_ptr="1" />
+ </ObjectChild>
</Object>
</GenerateTypes>
diff --git a/source/RobotAPI/libraries/armem_skills/server/segment/ExecutableSkillLibrarySegment.cpp b/source/RobotAPI/libraries/armem_skills/server/segment/ExecutableSkillLibrarySegment.cpp
index 1419ddf440ad718bc6c4252c7678b2c607534ab2..7a4b75514d9dde08b6fbd098f77547d626cb1b82 100644
--- a/source/RobotAPI/libraries/armem_skills/server/segment/ExecutableSkillLibrarySegment.cpp
+++ b/source/RobotAPI/libraries/armem_skills/server/segment/ExecutableSkillLibrarySegment.cpp
@@ -40,6 +40,12 @@ namespace armarx::skills::segment
skillDescription.targets = desc.targets;
skillDescription.timeoutMs = desc.timeoutMs;
+ if (desc.acceptedType)
+ {
+ auto t = aron::type::Object::FromAronObjectDTO(desc.acceptedType);
+ skillDescription.acceptedType = aron::converter::AronNlohmannJSONConverter::ConvertToNlohmannJSON(t).dump(2);
+ }
+
armem::Commit commit;
auto& entityUpdate = commit.add();
entityUpdate.confidence = 1.0;
@@ -55,5 +61,7 @@ namespace armarx::skills::segment
void ExecutableSkillLibraryCoreSegment::removeSkillProvider(const std::string& providerName)
{
skills.erase(providerName);
+
+ // TODO also add info about removed provider to memory?
}
}
diff --git a/source/RobotAPI/libraries/armem_skills/server/segment/SkillEventSegment.cpp b/source/RobotAPI/libraries/armem_skills/server/segment/SkillEventSegment.cpp
index 43e314fc827148c42a05e89d462964b84fdeed14..47270f33b3c20872b2d7a1d5c35058a3b618acd6 100644
--- a/source/RobotAPI/libraries/armem_skills/server/segment/SkillEventSegment.cpp
+++ b/source/RobotAPI/libraries/armem_skills/server/segment/SkillEventSegment.cpp
@@ -11,7 +11,7 @@ namespace armarx::skills::segment
{
SkillEventCoreSegment::SkillEventCoreSegment(armem::server::MemoryToIceAdapter& iceMemory):
- Base(iceMemory, CoreSegmentName)
+ Base(iceMemory, CoreSegmentName, armarx::skills::arondto::SkillExecutionRequest::ToAronType())
{
}
@@ -42,20 +42,14 @@ namespace armarx::skills::segment
event.providerName = update.providerName;
event.skillName = update.skillName;
event.status = ExecutionStatus2String.at(update.status);
-
- aron::data::DictPtr aron_params = nullptr;
- if (update.usedParams) aron_params = std::make_shared<aron::data::Dict>(update.usedParams);
-
- aron::data::DictPtr aron_data = nullptr;
- if (update.data) aron_data = std::make_shared<aron::data::Dict>(update.data);
+ event.params = aron::data::Dict::FromAronDictDTO(update.usedParams);
+ event.data = aron::data::Dict::FromAronDictDTO(update.data);
armem::MemoryID commitId = id();
commitId.providerSegmentName = event.providerName;
commitId.entityName = event.skillName;
auto aron = event.toAron();
- aron->addElement("return", aron_data); // how to name?!?
- aron->addElement("params", aron_params);
armem::Commit comm;
auto& entityUpdate = comm.add();
diff --git a/source/RobotAPI/libraries/armem_skills/server/segment/SkillExecutionRequestSegment.cpp b/source/RobotAPI/libraries/armem_skills/server/segment/SkillExecutionRequestSegment.cpp
index 5068cb04d910100a936b6934ef30fcaafb599f48..39b2ace4b719802477139cd917fff15a627e18e5 100644
--- a/source/RobotAPI/libraries/armem_skills/server/segment/SkillExecutionRequestSegment.cpp
+++ b/source/RobotAPI/libraries/armem_skills/server/segment/SkillExecutionRequestSegment.cpp
@@ -12,7 +12,7 @@ namespace armarx::skills::segment
{
SkillExecutionRequestCoreSegment::SkillExecutionRequestCoreSegment(armem::server::MemoryToIceAdapter& iceMemory):
- Base(iceMemory, CoreSegmentName/*, skills::arondto::SkillExecutionRequest::ToAronType()*/)
+ Base(iceMemory, CoreSegmentName, skills::arondto::SkillExecutionRequest::ToAronType())
{
}
@@ -35,12 +35,11 @@ namespace armarx::skills::segment
// we got a skill execution request
skills::arondto::SkillExecutionRequest request;
request.fromAron(commitDataAron);
- auto params = aron::data::Dict::DynamicCastAndCheck(commitDataAron->at("params")); // ToDo remov and add to request
skills::manager::dto::SkillExecutionInfo info;
info.providerName = request.providerName;
info.skillName = request.skillName;
- info.params = aron::data::Dict::ToAronDictDTO(params);
+ info.params = request.params->toAronDictDTO();
return info;
}
@@ -55,12 +54,10 @@ namespace armarx::skills::segment
request.clientId = "";
request.providerName = info.providerName;
request.skillName = info.skillName;
+ request.params = aron::data::Dict::FromAronDictDTO(info.params);
auto aron = request.toAron();
- aron::data::DictPtr aron_params = aron::data::Dict::FromAronDictDTO(info.params);
- aron->addElement("params", aron_params); // todo add as any type
-
armem::MemoryID skillExecutionMemID = id();
skillExecutionMemID.providerSegmentName = request.providerName;
skillExecutionMemID.entityName = request.skillName;
diff --git a/source/RobotAPI/libraries/aron/common/CMakeLists.txt b/source/RobotAPI/libraries/aron/common/CMakeLists.txt
index ea66ba9db549c0f48357b706dfa59f49ee1e8498..463f59dd1454c0f23fde9b304b5de1b29ae4345b 100644
--- a/source/RobotAPI/libraries/aron/common/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/common/CMakeLists.txt
@@ -32,11 +32,13 @@ armarx_enable_aron_file_generation_for_target(
TARGET_NAME
"${LIB_NAME}"
ARON_FILES
- aron/Trajectory.xml
- aron/Color.xml
+ aron/trajectory.xml
+ aron/color.xml
aron/PackagePath.xml
aron/AxisAlignedBoundingBox.xml
aron/OrientedBox.xml
+ aron/time.xml
+ aron/framed.xml
)
add_library(aron::common ALIAS aroncommon)
diff --git a/source/RobotAPI/libraries/aron/common/aron/FramedPose.xml b/source/RobotAPI/libraries/aron/common/aron/FramedPose.xml
new file mode 100644
index 0000000000000000000000000000000000000000..ec5aa0c961a11530ad1d6a08372fe2c42cbf10e8
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/aron/FramedPose.xml
@@ -0,0 +1,53 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+
+ <GenerateTypes>
+
+ <Object name="armarx::arondto::FrameHeader">
+ <ObjectChild key='frame'>
+ <string />
+ </ObjectChild>
+ <ObjectChild key='agent'>
+ <string />
+ </ObjectChild>
+ </Object>
+
+ <Object name="armarx::arondto::FramedPosition">
+ <ObjectChild key='header'>
+ <armarx::arondto::FrameHeader />
+ </ObjectChild>
+ <ObjectChild key='position'>
+ <Position />
+ </ObjectChild>
+ </Object>
+
+ <Object name="armarx::arondto::FramedDirection">
+ <ObjectChild key='header'>
+ <armarx::arondto::FrameHeader />
+ </ObjectChild>
+ <ObjectChild key='direction'>
+ <Position />
+ </ObjectChild>
+ </Object>
+
+ <Object name="armarx::arondto::FramedOrientation">
+ <ObjectChild key='header'>
+ <armarx::arondto::FrameHeader />
+ </ObjectChild>
+ <ObjectChild key='orientation'>
+ <Orientation />
+ </ObjectChild>
+ </Object>
+
+ <Object name="armarx::arondto::FramedPose">
+ <ObjectChild key='header'>
+ <armarx::arondto::FrameHeader />
+ </ObjectChild>
+ <ObjectChild key='pose'>
+ <Pose />
+ </ObjectChild>
+ </Object>
+
+ </GenerateTypes>
+
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/aron/common/aron/Color.xml b/source/RobotAPI/libraries/aron/common/aron/color.xml
similarity index 100%
rename from source/RobotAPI/libraries/aron/common/aron/Color.xml
rename to source/RobotAPI/libraries/aron/common/aron/color.xml
diff --git a/source/RobotAPI/libraries/aron/common/aron/framed.xml b/source/RobotAPI/libraries/aron/common/aron/framed.xml
new file mode 100644
index 0000000000000000000000000000000000000000..2e2e6d45f95defa21e39670d5957dba539d63972
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/aron/framed.xml
@@ -0,0 +1,48 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+
+ <CodeIncludes>
+ <Include include="<Eigen/Geometry>" />
+ </CodeIncludes>
+
+ <GenerateTypes>
+
+ <Object name="armarx::arondto::FrameID">
+ <ObjectChild key='frame'>
+ <string />
+ </ObjectChild>
+ <ObjectChild key='agent'>
+ <string />
+ </ObjectChild>
+ </Object>
+
+ <Object name="armarx::arondto::FramedPosition">
+ <ObjectChild key='header'>
+ <armarx::arondto::FrameID />
+ </ObjectChild>
+ <ObjectChild key='position'>
+ <Position />
+ </ObjectChild>
+ </Object>
+
+ <Object name="armarx::arondto::FramedOrientation">
+ <ObjectChild key='header'>
+ <armarx::arondto::FrameID />
+ </ObjectChild>
+ <ObjectChild key='orientation'>
+ <Orientation />
+ </ObjectChild>
+ </Object>
+
+ <Object name="armarx::arondto::FramedPose">
+ <ObjectChild key='header'>
+ <armarx::arondto::FrameID />
+ </ObjectChild>
+ <ObjectChild key='pose'>
+ <Pose />
+ </ObjectChild>
+ </Object>
+
+ </GenerateTypes>
+
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/aron/common/aron/time.xml b/source/RobotAPI/libraries/aron/common/aron/time.xml
new file mode 100644
index 0000000000000000000000000000000000000000..390e677cda4a528839abc613b1d147511511e9e9
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/aron/time.xml
@@ -0,0 +1,39 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+
+ <GenerateTypes>
+
+ <IntEnum name="armarx::arondto::ClockType">
+ <EnumValue key="Realtime" value="0" />
+ <EnumValue key="Monotonic" value="1" />
+ <EnumValue key="Virtual" value="2" />
+ <EnumValue key="Unknown" value="3" />
+ </IntEnum>
+
+ <Object name="armarx::arondto::Duration">
+ <ObjectChild key='microSeconds'>
+ <long />
+ </ObjectChild>
+ </Object>
+
+ <Object name="armarx::arondto::Frequency">
+ <ObjectChild key='cycleDuration'>
+ <armarx::arondto::Duration />
+ </ObjectChild>
+ </Object>
+
+ <Object name="armarx::arondto::DateTime">
+ <ObjectChild key='timeSinceEpoch'>
+ <armarx::arondto::Duration />
+ </ObjectChild>
+ <ObjectChild key='clockType'>
+ <armarx::arondto::ClockType />
+ </ObjectChild>
+ <ObjectChild key='hostname'>
+ <string />
+ </ObjectChild>
+ </Object>
+
+ </GenerateTypes>
+
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/aron/common/aron/Trajectory.xml b/source/RobotAPI/libraries/aron/common/aron/trajectory.xml
similarity index 100%
rename from source/RobotAPI/libraries/aron/common/aron/Trajectory.xml
rename to source/RobotAPI/libraries/aron/common/aron/trajectory.xml
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.cpp b/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.cpp
index d408f45ef12fe9ac8624469e6a25326e96498295..13535b2449250b9c3991accdee2d9289d16fbfaf 100644
--- a/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.cpp
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.cpp
@@ -31,4 +31,86 @@ namespace armarx
dto = bo.toMicroSeconds();
}
+ void fromAron(const arondto::ClockType& dto, ClockType& bo)
+ {
+ switch (dto.value)
+ {
+ case arondto::ClockType::Realtime:
+ bo = ClockType::Realtime;
+ break;
+ case arondto::ClockType::Monotonic:
+ bo = ClockType::Monotonic;
+ break;
+ case arondto::ClockType::Virtual:
+ bo = ClockType::Virtual;
+ break;
+ case arondto::ClockType::Unknown:
+ bo = ClockType::Unknown;
+ break;
+ }
+ }
+
+ void toAron(arondto::ClockType& dto, const ClockType& bo)
+ {
+ switch (bo)
+ {
+ case ClockType::Realtime:
+ dto = arondto::ClockType::Realtime;
+ break;
+ case ClockType::Monotonic:
+ dto = arondto::ClockType::Monotonic;
+ break;
+ case ClockType::Virtual:
+ dto = arondto::ClockType::Virtual;
+ break;
+ case ClockType::Unknown:
+ dto = arondto::ClockType::Unknown;
+ break;
+ }
+ }
+
+ void fromAron(const arondto::Duration& dto, Duration& bo)
+ {
+ bo = Duration::MicroSeconds(dto.microSeconds);
+ }
+
+ void toAron(arondto::Duration& dto, const Duration& bo)
+ {
+ dto.microSeconds = bo.toMicroSeconds();
+ }
+
+ void fromAron(const arondto::Frequency& dto, Frequency& bo)
+ {
+ Duration cycleDuration;
+ fromAron(dto.cycleDuration, cycleDuration);
+ bo = Frequency(cycleDuration);
+ }
+
+ void toAron(arondto::Frequency& dto, const Frequency& bo)
+ {
+ arondto::Duration cycleDuration;
+ toAron(cycleDuration, bo.toCycleDuration());
+ dto.cycleDuration = cycleDuration;
+ }
+
+ void fromAron(const arondto::DateTime& dto, DateTime& bo)
+ {
+ Duration timeSinceEpoch;
+ fromAron(dto.timeSinceEpoch, timeSinceEpoch);
+ ClockType clockType;
+ fromAron(dto.clockType, clockType);
+ bo = DateTime(timeSinceEpoch, clockType, dto.hostname);
+ }
+
+ void toAron(arondto::DateTime& dto, const DateTime& bo)
+ {
+ arondto::Duration timeSinceEpoch;
+ toAron(timeSinceEpoch, bo.toDurationSinceEpoch());
+ dto.timeSinceEpoch = timeSinceEpoch;
+ arondto::ClockType clockType;
+ toAron(clockType, bo.clockType());
+ dto.clockType = clockType;
+ dto.hostname = bo.hostname();
+ }
+
} // namespace armarx
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.h b/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.h
index 48c24690503cec91a17da37909a53c3228984e26..3f66752a865a0cfe2a0cb7e790e232f087751e2b 100644
--- a/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.h
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.h
@@ -1,17 +1,33 @@
#pragma once
#include <ArmarXCore/core/PackagePath.h>
+#include <ArmarXCore/core/time_minimal.h>
#include <IceUtil/Time.h>
#include <RobotAPI/libraries/aron/common/aron/PackagePath.aron.generated.h>
+#include <RobotAPI/libraries/aron/common/aron/time.aron.generated.h>
+#include <RobotAPI/libraries/aron/common/aron/framed.aron.generated.h>
namespace armarx
{
+
void fromAron(const arondto::PackagePath& dto, PackagePath& bo);
void toAron(arondto::PackagePath& dto, const PackagePath& bo);
void fromAron(const long& dto, IceUtil::Time& bo);
void toAron(long& dto, const IceUtil::Time& bo);
+ void fromAron(const arondto::ClockType& dto, ClockType& bo);
+ void toAron(arondto::ClockType& dto, const ClockType& bo);
+
+ void fromAron(const arondto::Duration& dto, Duration& bo);
+ void toAron(arondto::Duration& dto, const Duration& bo);
+
+ void fromAron(const arondto::Frequency& dto, Frequency& bo);
+ void toAron(arondto::Frequency& dto, const Frequency& bo);
+
+ void fromAron(const arondto::DateTime& dto, DateTime& bo);
+ void toAron(arondto::DateTime& dto, const DateTime& bo);
+
} // namespace armarx
diff --git a/source/RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.cpp b/source/RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.cpp
index 8eb3599faab31af90f0cf7666494f4eaf946d468..537b389e36191a4a59cd0da08d7ca26e3f889855 100644
--- a/source/RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.cpp
+++ b/source/RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.cpp
@@ -12,7 +12,7 @@ namespace armarx::aron::converter
void AronNlohmannJSONConverter::ConvertToNlohmannJSON(const aron::data::VariantPtr& aron, nlohmann::json& j)
{
aron::data::writer::NlohmannJSONWriter dataWriter;
- j = aron::data::readAndWrite<DataVariant2NlohmannJSONConverterHelper>(aron);
+ j = aron::data::readAndWrite<data::FromVariantConverter<data::writer::NlohmannJSONWriter>>(aron);
}
nlohmann::json AronNlohmannJSONConverter::ConvertToNlohmannJSON(const type::VariantPtr& aron)
@@ -25,7 +25,7 @@ namespace armarx::aron::converter
void AronNlohmannJSONConverter::ConvertToNlohmannJSON(const aron::type::VariantPtr& aron, nlohmann::json& j)
{
aron::type::writer::NlohmannJSONWriter typeWriter;
- j = aron::type::readAndWrite<TypeVariant2NlohmannJSONConverterHelper>(aron);
+ j = aron::type::readAndWrite<type::FromVariantConverter<type::writer::NlohmannJSONWriter>>(aron);
}
data::DictPtr AronNlohmannJSONConverter::ConvertFromNlohmannJSONObject(const nlohmann::json& j)
@@ -37,9 +37,21 @@ namespace armarx::aron::converter
return data::Dict::DynamicCastAndCheck(aron);
}
+ type::ObjectPtr AronNlohmannJSONConverter::ConvertFromNlohmannJSONTypeObject(const nlohmann::json& j)
+ {
+ type::VariantPtr aron = std::make_shared<aron::type::Object>("foo"); // foo is just a placeholder
+ ConvertFromNlohmannJSON(aron, j);
+ return type::Object::DynamicCastAndCheck(aron);
+ }
+
void AronNlohmannJSONConverter::ConvertFromNlohmannJSON(aron::data::VariantPtr& a, const nlohmann::json& e, const aron::type::VariantPtr& expectedStructure)
{
- aron::data::writer::NlohmannJSONWriter dataWriter;
- a = aron::data::readAndWrite<NlohmannJSON2DataVariantConverterHelper>(e);
+ a = aron::data::readAndWrite<data::FromNlohmannJSONConverter<data::writer::VariantWriter>>(e);
+ ARMARX_CHECK_EXPRESSION(a->fullfillsType(expectedStructure));
+ }
+
+ void AronNlohmannJSONConverter::ConvertFromNlohmannJSON(aron::type::VariantPtr& a, const nlohmann::json& e)
+ {
+ a = aron::type::readAndWrite<type::FromNlohmannJSONConverter<type::writer::VariantWriter>>(e);
}
}
diff --git a/source/RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h b/source/RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h
index 0a4b2a5c39c138c9be0f221f82b1f1c9e699638e..6cce4d7e363795f6a70184faf4fd60110516590f 100644
--- a/source/RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h
+++ b/source/RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h
@@ -29,27 +29,6 @@ namespace armarx::aron::converter
{
class AronNlohmannJSONConverter
{
- private:
- struct DataVariant2NlohmannJSONConverterHelper :
- public data::FromVariantConverter<data::writer::NlohmannJSONWriter>
- {
- };
-
- struct NlohmannJSON2DataVariantConverterHelper :
- public data::FromNlohmannJSONConverter<data::writer::VariantWriter>
- {
- };
-
- struct TypeVariant2NlohmannJSONConverterHelper :
- public type::FromVariantConverter<type::writer::NlohmannJSONWriter>
- {
- };
-
- struct NlohmannJSON2TypeVariantConverterHelper :
- public type::FromNlohmannJSONConverter<type::writer::VariantWriter>
- {
- };
-
public:
AronNlohmannJSONConverter() = delete;
@@ -61,5 +40,8 @@ namespace armarx::aron::converter
static data::DictPtr ConvertFromNlohmannJSONObject(const nlohmann::json&);
static void ConvertFromNlohmannJSON(data::VariantPtr&, const nlohmann::json&, const aron::type::VariantPtr& = nullptr);
+
+ static type::ObjectPtr ConvertFromNlohmannJSONTypeObject(const nlohmann::json& j);
+ static void ConvertFromNlohmannJSON(aron::type::VariantPtr& a, const nlohmann::json& e);
};
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/Writer.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/Writer.cpp
index 4ed774fb7f798efb5c5f3bf16cadaae5be22eb52..117a6cbf164c65a0e141ff7fd49bf5a3a11a0c96 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/Writer.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/Writer.cpp
@@ -52,6 +52,17 @@ namespace armarx::aron::codegenerator::cpp
dataWriters.push_back(toAron);
}
+ {
+ codegenerator::WriterInfo toAronDTO;
+ toAronDTO.methodName = "toAronDTO";
+ toAronDTO.returnType = "armarx::aron::data::dto::DictPtr";
+ toAronDTO.writerClassType = "armarx::aron::data::writer::VariantWriter";
+ toAronDTO.include = "<RobotAPI/libraries/aron/core/data/rw/writer/variant/VariantWriter.h>";
+ toAronDTO.enforceConversion = "armarx::aron::data::Dict::DynamicCastAndCheck";
+ toAronDTO.enforceMemberAccess = "->toAronDictDTO()";
+ dataWriters.push_back(toAronDTO);
+ }
+
{
// The toAron Serializer is visible by default
codegenerator::WriterInfo ToAronType;
@@ -63,6 +74,18 @@ namespace armarx::aron::codegenerator::cpp
initialTypeWriters.push_back(ToAronType);
}
+ {
+ // The toAron Serializer is visible by default
+ codegenerator::WriterInfo ToAronTypeDTO;
+ ToAronTypeDTO.methodName = "ToAronTypeDTO";
+ ToAronTypeDTO.returnType = "armarx::aron::type::dto::AronObjectPtr";
+ ToAronTypeDTO.writerClassType = "armarx::aron::type::writer::VariantWriter";
+ ToAronTypeDTO.include = "<RobotAPI/libraries/aron/core/type/rw/writer/variant/VariantWriter.h>";
+ ToAronTypeDTO.enforceConversion = "armarx::aron::type::Object::DynamicCastAndCheck";
+ ToAronTypeDTO.enforceMemberAccess = "->toAronObjectDTO()";
+ initialTypeWriters.push_back(ToAronTypeDTO);
+ }
+
// toJSON Method
/*
{
@@ -87,6 +110,15 @@ namespace armarx::aron::codegenerator::cpp
staticDataReaders.push_back(fromAron);
}
+ {
+ codegenerator::StaticReaderInfo fromAronDTO;
+ fromAronDTO.methodName = "FromAron";
+ fromAronDTO.argumentType = "armarx::aron::data::dto::DictPtr";
+ fromAronDTO.returnType = OWN_TYPE_NAME;
+ staticDataReaders.push_back(fromAronDTO);
+ }
+
+
// The fromAron Deserializer is visible by default
{
codegenerator::ReaderInfo fromAron;
@@ -97,6 +129,16 @@ namespace armarx::aron::codegenerator::cpp
dataReaders.push_back(fromAron);
}
+ {
+ codegenerator::ReaderInfo fromAronDTO;
+ fromAronDTO.methodName = "fromAron";
+ fromAronDTO.argumentType = "armarx::aron::data::dto::DictPtr";
+ fromAronDTO.readerClassType = "armarx::aron::data::reader::VariantReader";
+ fromAronDTO.include = "<RobotAPI/libraries/aron/core/data/rw/reader/variant/VariantReader.h>";
+ fromAronDTO.enforceConversion = "std::make_shared<armarx::aron::data::Dict>";
+ dataReaders.push_back(fromAronDTO);
+ }
+
// fromJSON Method
/*
{
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/Generator.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/Generator.cpp
index 4eee83a08e317bebd93eff20af01b543b65dc4b6..8e437dd62935c3de5fadf1c93daf10d89270c813 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/Generator.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/Generator.cpp
@@ -211,7 +211,7 @@ namespace armarx::aron::codegenerator::cpp
CppMethodPtr m = CppMethodPtr(new CppMethod("template<class WriterT>\nstatic typename WriterT::ReturnType writeType(WriterT& " + ARON_WRITER_ACCESSOR + ", std::vector<std::string> " + ARON_TEMPLATE_INSTANTIATIONS_ACCESSOR + " = {}, armarx::aron::type::Maybe "+ ARON_MAYBE_TYPE_ACCESSOR +" = armarx::aron::type::Maybe::eNone, const armarx::aron::Path& "+ARON_PATH_ACCESSOR+" = armarx::aron::Path())", doc.str()));
CppBlockPtr b = std::make_shared<CppBlock>();
- b->addLine("using T = typename WriterT::ReturnType;");
+ b->addLine("using T [[maybe_unused]] = typename WriterT::ReturnType;");
std::string dummy;
b->appendBlock(this->getWriteTypeBlock("", "", Path(), dummy));
@@ -228,7 +228,7 @@ namespace armarx::aron::codegenerator::cpp
CppMethodPtr m = CppMethodPtr(new CppMethod("template<class WriterT>\ntypename WriterT::ReturnType write(WriterT& " + ARON_WRITER_ACCESSOR + ", const armarx::aron::Path& "+ARON_PATH_ACCESSOR+" = armarx::aron::Path()) const", doc.str()));
CppBlockPtr b = std::make_shared<CppBlock>();
- b->addLine("using T = typename WriterT::ReturnType;");
+ b->addLine("using T [[maybe_unused]] = typename WriterT::ReturnType;");
std::string dummy;
b->appendBlock(this->getWriteBlock("", Path(), dummy));
@@ -245,20 +245,8 @@ namespace armarx::aron::codegenerator::cpp
CppMethodPtr m = CppMethodPtr(new CppMethod("template<class ReaderT>\nvoid read(ReaderT& " + ARON_READER_ACCESSOR + ", typename ReaderT::InputType& input)", doc.str()));
CppBlockPtr b = std::make_shared<CppBlock>();
- b->addLine("using T = typename ReaderT::InputType;");
- b->addLine("using TNonConst = typename ReaderT::InputTypeNonConst;");
-
- // TODO: Remove me and make nice
- auto makeSuppressUnusedBlock = []()
- {
- auto block = std::make_shared<CppBlock>();
- block->addLine("const T* _suppressUnusedWarning1;");
- block->addLine("const TNonConst* _suppressUnusedWarning2;");
- block->addLine("(void) _suppressUnusedWarning1;");
- block->addLine("(void) _suppressUnusedWarning2;");
- return block;
- };
- b->addBlock(makeSuppressUnusedBlock());
+ b->addLine("using T [[maybe_unused]] = typename ReaderT::InputType;");
+ b->addLine("using TNonConst [[maybe_unused]] = typename ReaderT::InputTypeNonConst;");
b->addLine("this->resetSoft();");
b->addLine("if (" + ARON_READER_ACCESSOR + ".readNull(input))");
@@ -276,7 +264,7 @@ namespace armarx::aron::codegenerator::cpp
CppMethodPtr m = CppMethodPtr(new CppMethod(info.returnType + " " + info.methodName + "() const", doc.str()));
m->addLine(info.writerClassType + " writer;");
- m->addLine("return " + info.enforceConversion + "(this->write(writer));");
+ m->addLine("return " + info.enforceConversion + "(this->write(writer))" + info.enforceMemberAccess + ";");
return m;
}
@@ -288,7 +276,7 @@ namespace armarx::aron::codegenerator::cpp
CppMethodPtr m = CppMethodPtr(new CppMethod("void " + info.methodName + "(const " + info.argumentType + "& input)", doc.str()));
m->addLine(info.readerClassType + " reader;");
- m->addLine("this->read(reader, " + info.enforceConversion + "(input));");
+ m->addLine("this->read(reader, " + info.enforceConversion + "(input)" + info.enforceMemberAccess + ");");
return m;
}
@@ -313,7 +301,7 @@ namespace armarx::aron::codegenerator::cpp
CppMethodPtr m = CppMethodPtr(new CppMethod("static " + info.returnType + " " + info.methodName + "()", doc.str()));
m->addLine(info.writerClassType + " writer;");
- m->addLine("return " + info.enforceConversion + "(writeType(writer));");
+ m->addLine("return " + info.enforceConversion + "(writeType(writer))" + info.enforceMemberAccess + ";");
return m;
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/enum/IntEnum.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/enum/IntEnum.cpp
index f27c0cd24ef33eaa2dc92ca3871363b205118fed..3c9fa87f1c6eb86a2184f46d2729094510f01ab6 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/enum/IntEnum.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/generator/enum/IntEnum.cpp
@@ -43,7 +43,7 @@ namespace armarx::aron::codegenerator::cpp::generator
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
block_if_data->addLine(accessor + nextEl() + "resetSoft();");
- return block_if_data;
+ return this->resolveMaybeResetSoftBlock(block_if_data, accessor);
}
CppBlockPtr IntEnum::getResetHardBlock(const std::string& accessor) const
@@ -57,7 +57,7 @@ namespace armarx::aron::codegenerator::cpp::generator
{
b->addLine(accessor + nextEl() + "resetHard();");
}
- return b;
+ return this->resolveMaybeResetHardBlock(b, accessor);
}
CppBlockPtr IntEnum::getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, const Path& p, std::string& variantAccessor) const
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/helper/ReaderInfo.h b/source/RobotAPI/libraries/aron/core/codegenerator/helper/ReaderInfo.h
index a025da99a26a9538f53306ce9ba96d1fb8396ad5..3bda10de7b14849fe6e18da11f7201704c2f3670 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/helper/ReaderInfo.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/helper/ReaderInfo.h
@@ -36,6 +36,7 @@ namespace armarx::aron::codegenerator
std::string readerClassType;
std::string include;
std::string enforceConversion = "";
+ std::string enforceMemberAccess = "";
};
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/helper/WriterInfo.h b/source/RobotAPI/libraries/aron/core/codegenerator/helper/WriterInfo.h
index 33090bcbe0a07d6e9c8f5f124905e13424945ec3..1989ac5de9e93ee5ddda8ac66cfa1e65edeab98b 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/helper/WriterInfo.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/helper/WriterInfo.h
@@ -36,5 +36,6 @@ namespace armarx::aron::codegenerator
std::string writerClassType;
std::string include;
std::string enforceConversion = "";
+ std::string enforceMemberAccess = "";
};
}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/Factory.cpp b/source/RobotAPI/libraries/aron/core/data/variant/Factory.cpp
index 94ec4a97f6e8159fa0a68f4a16a83330c55441a9..c228a449d39e10ff712942311f4181f6281461f9 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/Factory.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/Factory.cpp
@@ -53,7 +53,8 @@ namespace armarx::aron::data
case data::Descriptor::eDouble: return std::make_unique<data::Double>(data::dto::AronDoublePtr::dynamicCast(aron), path);
case data::Descriptor::eString: return std::make_unique<data::String>(data::dto::AronStringPtr::dynamicCast(aron), path);
case data::Descriptor::eBool: return std::make_unique<data::Bool>(data::dto::AronBoolPtr::dynamicCast(aron), path);
- default: throw error::ValueNotValidException(__PRETTY_FUNCTION__, "", std::to_string((int) descriptor), path);
+ case data::Descriptor::eUnknown: break;
}
+ throw error::ValueNotValidException(__PRETTY_FUNCTION__, "", std::to_string((int) descriptor), path);
}
}
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 7de89a06a92c1da3e84046c3af98c2d10d602515..fd840a80dfb192ad017fa481a2a167fc9cd872a2 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.cpp
@@ -177,10 +177,7 @@ namespace armarx::aron::data
bool NDArray::fullfillsType(const type::VariantPtr& type) const
{
- if (!type)
- {
- return false;
- }
+ if (!type) return true;
type::Descriptor typeDesc = type->getDescriptor();
switch (typeDesc)
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h b/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h
index 31677060acad93fe7fbbbe949b4439d434392931..593298a7df80e167b9525ce6280cebdbb132cd59 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h
@@ -97,3 +97,12 @@ namespace armarx::aron::data
virtual bool fullfillsType(const type::VariantPtr&) const override;
};
}
+
+namespace armarx::aron
+{
+ template<typename... _Args>
+ aron::data::NDArrayPtr make_ndarray(_Args&&... args)
+ {
+ return std::make_shared<aron::data::NDArray>(args...);
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.cpp b/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.cpp
index 1745474f386a2e7e05a078c5486a5322067b3dba..12b402fd0cb2d33cb3b3314ee7c9fa373f1128ff 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.cpp
@@ -208,10 +208,7 @@ namespace armarx::aron::data
bool Dict::fullfillsType(const type::VariantPtr& type) const
{
- if(!type)
- {
- return false;
- }
+ if (!type) return true;
type::Descriptor typeDesc = type->getDescriptor();
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.h b/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.h
index f022d02645d2b4b9b255de2ba43e982e150fc383..6087ddfac5c8f91123376afa5fe7eb2c60010cf8 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.h
@@ -26,6 +26,7 @@
// STD/STL
#include <memory>
#include <map>
+#include <utility>
// Base class
#include "../detail/ContainerVariant.h"
@@ -98,3 +99,12 @@ namespace armarx::aron::data
std::map<std::string, VariantPtr> childrenNavigators;
};
}
+
+namespace armarx::aron
+{
+ template<typename... _Args>
+ aron::data::DictPtr make_dict(_Args&&... args)
+ {
+ return std::make_shared<aron::data::Dict>(args...);
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/container/List.cpp b/source/RobotAPI/libraries/aron/core/data/variant/container/List.cpp
index bb86d4f1baff4013c62c788a7f6ddf0f99f80a1c..74eb78a75c6a38bcee5008025596126ba1a2ce99 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/container/List.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/container/List.cpp
@@ -211,10 +211,7 @@ namespace armarx::aron::data
bool List::fullfillsType(const type::VariantPtr& type) const
{
- if (!type)
- {
- return false;
- }
+ if (!type) return true;
type::Descriptor typeDesc = type->getDescriptor();
switch (typeDesc)
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/container/List.h b/source/RobotAPI/libraries/aron/core/data/variant/container/List.h
index b766ce5aae83416ae632b7e0ff27c30ade72c0e0..4ab656bfa2e11461ffb4914a2fc444f16c5ce6f8 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/container/List.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/container/List.h
@@ -97,3 +97,12 @@ namespace armarx::aron::data
std::vector<VariantPtr> childrenNavigators;
};
}
+
+namespace armarx::aron
+{
+ template<typename... _Args>
+ aron::data::ListPtr make_list(_Args&&... args)
+ {
+ return std::make_shared<aron::data::List>(args...);
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.cpp b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.cpp
index 7c5e526822769e56b61e3b0099509274f6ae9224..5b5cb2b0ed8cb7c3f5eb6f276723cbfadbc181da 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.cpp
@@ -110,6 +110,7 @@ namespace armarx::aron::data
bool Bool::fullfillsType(const type::VariantPtr& type) const
{
+ if (!type) return true;
return type->getDescriptor() == type::Descriptor::eBool;
}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.h b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.h
index 65cfc31a7bec67f63e8f9bf5101f697c7d61e2c7..5e07b8a8e33577b268ea8ae8366291664b3012e6 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.h
@@ -68,3 +68,12 @@ namespace armarx::aron::data
bool fullfillsType(const type::VariantPtr&) const override;
};
}
+
+namespace armarx::aron
+{
+ template<typename... _Args>
+ aron::data::BoolPtr make_bool(_Args&&... args)
+ {
+ return std::make_shared<aron::data::Bool>(args...);
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.cpp b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.cpp
index 729e5bda480e8a21d14e16be1f454a02d1f7cf9b..906afeeea30f14270097d70b1868517dac91acb3 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.cpp
@@ -103,6 +103,7 @@ namespace armarx::aron::data
bool Double::fullfillsType(const type::VariantPtr& type) const
{
+ if (!type) return true;
return type->getDescriptor() == type::Descriptor::eDouble;
}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.h b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.h
index b02571f1dd7cef61635849b21edddf943469c7cd..fb7a6880a9782397239243d38939d1ac22688fd4 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.h
@@ -68,3 +68,12 @@ namespace armarx::aron::data
bool fullfillsType(const type::VariantPtr&) const override;
};
}
+
+namespace armarx::aron
+{
+ template<typename... _Args>
+ aron::data::DoublePtr make_double(_Args&&... args)
+ {
+ return std::make_shared<aron::data::Double>(args...);
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.cpp b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.cpp
index f638882eaf80af2a6a4cba0f9d1fffe43cd6e304..154591f68942d63e02139fd866e7013eaea3f69e 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.cpp
@@ -100,6 +100,7 @@ namespace armarx::aron::data
bool Float::fullfillsType(const type::VariantPtr& type) const
{
+ if (!type) return true;
return type->getDescriptor() == type::Descriptor::eFloat;
}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.h b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.h
index d7b44e56784ef2a6a1c8b35ebb3b7b3841448426..2d0cecb7e609df5496da3e616ffcd0fe6e249800 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.h
@@ -68,3 +68,12 @@ namespace armarx::aron::data
bool fullfillsType(const type::VariantPtr&) const override;
};
}
+
+namespace armarx::aron
+{
+ template<typename... _Args>
+ aron::data::FloatPtr make_float(_Args&&... args)
+ {
+ return std::make_shared<aron::data::Float>(args...);
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.cpp b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.cpp
index b5f0d39196b750084876ad85a49f9f77cdf23112..14299d05a3f687d58916c4498dc4f50d167dad3a 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.cpp
@@ -103,6 +103,7 @@ namespace armarx::aron::data
bool Int::fullfillsType(const type::VariantPtr& type) const
{
+ if (!type) return true;
return type->getDescriptor() == type::Descriptor::eInt || type->getDescriptor() == type::Descriptor::eIntEnum;
}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.h b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.h
index 9a803a035d0be45cd7f38241a937f67179c2bfa2..5ee4a270f60bd2b21d8e2257c814f5dab8a0b759 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.h
@@ -69,3 +69,12 @@ namespace armarx::aron::data
bool fullfillsType(const type::VariantPtr&) const override;
};
}
+
+namespace armarx::aron
+{
+ template<typename... _Args>
+ aron::data::IntPtr make_int(_Args&&... args)
+ {
+ return std::make_shared<aron::data::Int>(args...);
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.cpp b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.cpp
index 1885fc750b5cee8a7b477627167972c37e514db9..c81aaa05054f3734c342a7c174d8c7f31407b9e4 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.cpp
@@ -104,6 +104,7 @@ namespace armarx::aron::data
bool Long::fullfillsType(const type::VariantPtr& type) const
{
+ if (!type) return true;
return type->getDescriptor() == type::Descriptor::eLong || type->getDescriptor() == type::Descriptor::eTime;
}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.h b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.h
index 82b9481be9f957271735d55686659767b60c53ec..0cbcbdb66efa8f6446831ac62617bbfd57119f9b 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.h
@@ -69,3 +69,12 @@ namespace armarx::aron::data
bool fullfillsType(const type::VariantPtr&) const override;
};
}
+
+namespace armarx::aron
+{
+ template<typename... _Args>
+ aron::data::LongPtr make_long(_Args&&... args)
+ {
+ return std::make_shared<aron::data::Long>(args...);
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.cpp b/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.cpp
index b896221b06d79f3e9cee12b2d84ec9156216dc7f..0c4ec5bd375fcb0e74e7564bd5a9caecbb3b5109 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.cpp
@@ -100,6 +100,7 @@ namespace armarx::aron::data
bool String::fullfillsType(const type::VariantPtr& type) const
{
+ if (!type) return true;
return type->getDescriptor() == type::Descriptor::eString;
}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.h b/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.h
index 33e9ef6ea7d160bc5dab7e64cc043cc7948c1e34..21c23698c114262b3d71658288cef82e12e4747f 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.h
@@ -68,3 +68,12 @@ namespace armarx::aron::data
bool fullfillsType(const type::VariantPtr&) const override;
};
}
+
+namespace armarx::aron
+{
+ template<typename... _Args>
+ aron::data::StringPtr make_string(_Args&&... args)
+ {
+ return std::make_shared<aron::data::String>(args...);
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/core/type/rw/reader/nlohmannJSON/NlohmannJSONReader.cpp b/source/RobotAPI/libraries/aron/core/type/rw/reader/nlohmannJSON/NlohmannJSONReader.cpp
index 246a3698a0b6bd05256643d7086ebdf4641220e5..23bbfa738571caffde68365cf5e62d52fbf2cddf 100644
--- a/source/RobotAPI/libraries/aron/core/type/rw/reader/nlohmannJSON/NlohmannJSONReader.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/rw/reader/nlohmannJSON/NlohmannJSONReader.cpp
@@ -111,7 +111,9 @@ namespace armarx::aron::type::reader
maybe = rw::json::conversion::String2Maybe.at(input[rw::json::constantes::MAYBE_SLUG]);
ndim = input[rw::json::constantes::DIMENSIONS_SLUG];
- type = input[rw::json::constantes::USED_TYPE_SLUG];
+
+ std::string t = input[rw::json::constantes::USED_TYPE_SLUG];
+ type = armarx::aron::type::rw::json::conversion::String2NDArrayType.at(t);
}
void NlohmannJSONReader::readMatrix(const nlohmann::json& input, int& rows, int& cols, type::matrix::ElementType& type, type::Maybe& maybe, Path& p)
@@ -122,7 +124,9 @@ namespace armarx::aron::type::reader
auto list = input[rw::json::constantes::DIMENSIONS_SLUG].get<std::vector<nlohmann::json>>();
rows = list[0];
cols = list[1];
- type = input[rw::json::constantes::USED_TYPE_SLUG];
+
+ std::string t = input[rw::json::constantes::USED_TYPE_SLUG];
+ type = armarx::aron::type::rw::json::conversion::String2MatrixType.at(t);
}
void NlohmannJSONReader::readQuaternion(const nlohmann::json& input, type::quaternion::ElementType& type, type::Maybe& maybe, Path& p)
@@ -130,7 +134,9 @@ namespace armarx::aron::type::reader
getAronMetaInformationForType(input, rw::json::constantes::QUATERNION_TYPENAME_SLUG, p);
maybe = rw::json::conversion::String2Maybe.at(input[rw::json::constantes::MAYBE_SLUG]);
- type = input[rw::json::constantes::USED_TYPE_SLUG];
+
+ std::string t = input[rw::json::constantes::USED_TYPE_SLUG];
+ type = armarx::aron::type::rw::json::conversion::String2QuaternionType.at(t);
}
void NlohmannJSONReader::readPointCloud(const nlohmann::json& input, type::pointcloud::VoxelType& type, type::Maybe& maybe, Path& p)
@@ -138,7 +144,9 @@ namespace armarx::aron::type::reader
getAronMetaInformationForType(input, rw::json::constantes::POINT_CLOUD_TYPENAME_SLUG, p);
maybe = rw::json::conversion::String2Maybe.at(input[rw::json::constantes::MAYBE_SLUG]);
- type = input[rw::json::constantes::USED_TYPE_SLUG];
+
+ std::string t = input[rw::json::constantes::USED_TYPE_SLUG];
+ type = armarx::aron::type::rw::json::conversion::String2VoxelType.at(t);
}
void NlohmannJSONReader::readImage(const nlohmann::json& input, type::image::PixelType& type, type::Maybe& maybe, Path& p)
@@ -146,7 +154,9 @@ namespace armarx::aron::type::reader
getAronMetaInformationForType(input, rw::json::constantes::IMAGE_TYPENAME_SLUG, p);
maybe = rw::json::conversion::String2Maybe.at(input[rw::json::constantes::MAYBE_SLUG]);
- type = input[rw::json::constantes::USED_TYPE_SLUG];
+
+ std::string t = input[rw::json::constantes::USED_TYPE_SLUG];
+ type = armarx::aron::type::rw::json::conversion::String2PixelType.at(t);
}
void NlohmannJSONReader::readPosition(const nlohmann::json& input, type::Maybe& maybe, Path& p)
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/Factory.cpp b/source/RobotAPI/libraries/aron/core/type/variant/Factory.cpp
index 16fb41506f61ff66a1d7d3770c869858e7f9370d..3d7ca9846e890fe4cd14020b70490788bde2bea4 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/Factory.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/Factory.cpp
@@ -58,8 +58,10 @@ namespace armarx::aron::type
case type::Descriptor::eString: return std::make_unique<type::String>(dynamic_cast<const type::dto::AronString&>(aron), path);
case type::Descriptor::eBool: return std::make_unique<type::Bool>(dynamic_cast<const type::dto::AronBool&>(aron), path);
case type::Descriptor::eTime: return std::make_unique<type::Time>(dynamic_cast<const type::dto::AronTime&>(aron), path);
- default: throw error::ValueNotValidException(__PRETTY_FUNCTION__, "Got an unknown descriptor", std::to_string((int) descriptor) + " aka " + defaultconversion::string::Descriptor2String.at(descriptor), path);
+ case type::Descriptor::eAnyObject: return std::make_unique<type::AnyObject>(dynamic_cast<const type::dto::AnyObject&>(aron), path);
+ case type::Descriptor::eUnknown: break;
}
+ throw error::ValueNotValidException(__PRETTY_FUNCTION__, "Got an unknown descriptor", std::to_string((int) descriptor) + " aka " + defaultconversion::string::Descriptor2String.at(descriptor), path);
}
}
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/container/Object.cpp b/source/RobotAPI/libraries/aron/core/type/variant/container/Object.cpp
index 56dd6f22a526a60c65bcd992b48ae214b7af2241..eb27eedcdb74c99f04003f0b0d143add11527bc7 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/container/Object.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/container/Object.cpp
@@ -105,7 +105,7 @@ namespace armarx::aron::type
VariantPtr Object::getMemberType(const std::string& s) const
{
- if (memberTypes.find(s) == memberTypes.end() and !extends->hasMemberType(s))
+ if (memberTypes.find(s) == memberTypes.end() and not (extends and extends->hasMemberType(s)))
{
throw error::ValueNotValidException("ObjectNavigator", "getMemberType", "Member not set. The list of all members is: " + simox::alg::to_string(simox::alg::get_keys(memberTypes)), s);
}
diff --git a/source/RobotAPI/libraries/core/CartesianPositionController.cpp b/source/RobotAPI/libraries/core/CartesianPositionController.cpp
index 9681636da9a820a53711ed50f327205216a0fcc8..c34d94957311c775b378bcda7e0dda17f38fe1c3 100644
--- a/source/RobotAPI/libraries/core/CartesianPositionController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianPositionController.cpp
@@ -24,6 +24,7 @@
#include "CartesianPositionController.h"
#include <RobotAPI/libraries/core/math/MathUtils.h>
+#include <ArmarXCore/util/CPPUtility/trace.h>
namespace armarx
{
@@ -35,12 +36,14 @@ namespace armarx
Eigen::VectorXf CartesianPositionController::calculate(const Eigen::Matrix4f& targetPose, VirtualRobot::IKSolver::CartesianSelection mode) const
{
+ ARMARX_TRACE;
int posLen = mode & VirtualRobot::IKSolver::Position ? 3 : 0;
int oriLen = mode & VirtualRobot::IKSolver::Orientation ? 3 : 0;
Eigen::VectorXf cartesianVel(posLen + oriLen);
if (posLen)
{
+ ARMARX_TRACE;
Eigen::Vector3f targetPos = targetPose.block<3, 1>(0, 3);
Eigen::Vector3f currentPos = tcp->getPositionInFrame(referenceFrame);
Eigen::Vector3f errPos = targetPos - currentPos;
@@ -54,6 +57,7 @@ namespace armarx
if (oriLen)
{
+ ARMARX_TRACE;
Eigen::Matrix3f targetOri = targetPose.block<3, 3>(0, 0);
Eigen::Matrix3f tcpOri = tcp->getPoseInFrame(referenceFrame).block<3, 3>(0, 0);
Eigen::Matrix3f oriDir = targetOri * tcpOri.inverse();
@@ -72,12 +76,14 @@ namespace armarx
Eigen::VectorXf CartesianPositionController::calculatePos(const Eigen::Vector3f& targetPos) const
{
+ ARMARX_TRACE;
Eigen::VectorXf cartesianVel(3);
Eigen::Vector3f currentPos = tcp->getPositionInFrame(referenceFrame);
Eigen::Vector3f errPos = targetPos - currentPos;
Eigen::Vector3f posVel = errPos * KpPos;
if (maxPosVel > 0)
{
+ ARMARX_TRACE;
posVel = math::MathUtils::LimitTo(posVel, maxPosVel);
}
cartesianVel.block<3, 1>(0, 0) = posVel;
@@ -98,6 +104,7 @@ namespace armarx
const VirtualRobot::RobotNodePtr& tcp,
const VirtualRobot::RobotNodePtr& referenceFrame)
{
+ ARMARX_TRACE;
Eigen::Vector3f targetPos = targetPose.block<3, 1>(0, 3);
Eigen::Vector3f tcpPos = referenceFrame ? tcp->getPositionInFrame(referenceFrame) : tcp->getPositionInRootFrame();
Eigen::Vector3f errPos = targetPos - tcpPos;
@@ -108,6 +115,7 @@ namespace armarx
const VirtualRobot::RobotNodePtr& tcp,
const VirtualRobot::RobotNodePtr& referenceFrame)
{
+ ARMARX_TRACE;
Eigen::Matrix3f targetOri = targetPose.block<3, 3>(0, 0);
Eigen::Matrix4f tcpPose = referenceFrame ? tcp->getPoseInFrame(referenceFrame) : tcp->getPoseInRootFrame();
Eigen::Matrix3f tcpOri = tcpPose.block<3, 3>(0, 0);
@@ -129,6 +137,7 @@ namespace armarx
bool CartesianPositionController::reached(const Eigen::Matrix4f& targetPose, VirtualRobot::IKSolver::CartesianSelection mode, float thresholdPosReached, float thresholdOriReached)
{
+ ARMARX_TRACE;
if (mode & VirtualRobot::IKSolver::Position)
{
if (GetPositionError(targetPose, tcp, referenceFrame) > thresholdPosReached)
@@ -148,6 +157,7 @@ namespace armarx
Eigen::Vector3f CartesianPositionController::getPositionDiff(const Eigen::Matrix4f& targetPose) const
{
+ ARMARX_TRACE;
Eigen::Vector3f targetPos = targetPose.block<3, 1>(0, 3);
return targetPos - tcp->getPositionInFrame(referenceFrame);
}
@@ -159,6 +169,7 @@ namespace armarx
Eigen::Vector3f CartesianPositionController::getOrientationDiff(const Eigen::Matrix4f& targetPose) const
{
+ ARMARX_TRACE;
Eigen::Matrix3f targetOri = targetPose.block<3, 3>(0, 0);
Eigen::Matrix3f tcpOri = tcp->getPoseInFrame(referenceFrame).block<3, 3>(0, 0);
Eigen::Matrix3f oriDir = targetOri * tcpOri.inverse();
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
index 9527835a6dd48107f5e11846ec882d30b056c54a..f7a7307bfb316db667d23ff59713d80551cd1e63 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
@@ -48,6 +48,7 @@ CartesianVelocityController::CartesianVelocityController(const VirtualRobot::Rob
void CartesianVelocityController::calculateJacobis(VirtualRobot::IKSolver::CartesianSelection mode)
{
+ ARMARX_TRACE;
jacobi = ik->getJacobianMatrix(_tcp, mode);
_jacobiWithCosts = Eigen::MatrixXf(jacobi.rows(), jacobi.cols());
for (int r = 0; r < jacobi.rows(); r++)
@@ -98,7 +99,7 @@ Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& ca
Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& cartesianVel, const Eigen::VectorXf& nullspaceVel, VirtualRobot::IKSolver::CartesianSelection mode)
{
-
+ ARMARX_TRACE;
calculateJacobis(mode);
@@ -106,6 +107,7 @@ Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& ca
if (nullspaceVel.rows() > 0)
{
+ ARMARX_TRACE;
// ARMARX_IMPORTANT << "nullspaceVel " << nullspaceVel.transpose();
Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(_jacobiWithCosts);
//ARMARX_IMPORTANT << "The rank of the _jacobiWithCosts is " << lu_decomp.rank();
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp
index a72fe97e50b3249595840b6fe18e1aca8be36542..ee06ab837f18e01764757e98e9aa372fc63e785f 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp
+++ b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp
@@ -77,6 +77,7 @@ namespace armarx
Eigen::VectorXf CartesianVelocityControllerWithRamp::calculate(const Eigen::VectorXf& cartesianVel, const Eigen::VectorXf& nullspaceVel, float dt)
{
+ ARMARX_TRACE;
return controller.calculate(cartesianVelocityRamp.update(cartesianVel, dt), nullSpaceVelocityRamp.update(nullspaceVel, dt), mode);
}
diff --git a/source/RobotAPI/libraries/core/CartesianWaypointController.cpp b/source/RobotAPI/libraries/core/CartesianWaypointController.cpp
index fe9477986cee0a9d9a9a55b3f94d40206c010e3e..917877b7a5ce1556ece46a9d8353b274dba87465 100644
--- a/source/RobotAPI/libraries/core/CartesianWaypointController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianWaypointController.cpp
@@ -32,6 +32,7 @@
#include <VirtualRobot/math/Helpers.h>
#include <VirtualRobot/MathTools.h>
#include <cfloat>
+#include <ArmarXCore/util/CPPUtility/trace.h>
namespace armarx
{
@@ -64,21 +65,25 @@ namespace armarx
const Eigen::VectorXf& CartesianWaypointController::calculate(float dt)
{
+ ARMARX_TRACE;
//calculate cartesian velocity + some management stuff
if (!isLastWaypoint() && isCurrentTargetNear())
{
+ ARMARX_TRACE;
currentWaypointIndex++;
}
cartesianVelocity = ctrlCartesianPos2Vel.calculate(getCurrentTarget(), VirtualRobot::IKSolver::All) + feedForwardVelocity;
if (autoClearFeedForward)
{
+ ARMARX_TRACE;
clearFeedForwardVelocity();
}
//calculate joint velocity
if (nullSpaceControlEnabled)
{
+ ARMARX_TRACE;
//avoid joint limits
_jnv = KpJointLimitAvoidance * ctrlCartesianVelWithRamps.controller.calculateJointLimitAvoidance() +
ctrlCartesianVelWithRamps.controller.calculateNullspaceVelocity(
@@ -89,9 +94,11 @@ namespace armarx
}
else
{
+ ARMARX_TRACE;
//don't avoid joint limits
_jnv *= 0;
}
+ ARMARX_TRACE;
_out = ctrlCartesianVelWithRamps.calculate(cartesianVelocity, _jnv, dt);
return _out;
}
@@ -178,11 +185,13 @@ namespace armarx
const Eigen::Vector3f CartesianWaypointController::getCurrentTargetPosition() const
{
+ ARMARX_TRACE;
return ::math::Helpers::GetPosition(waypoints.at(currentWaypointIndex));
}
size_t CartesianWaypointController::skipToClosestWaypoint(float rad2mmFactor)
{
+ ARMARX_TRACE;
float dist = FLT_MAX;
size_t minIndex = 0;
for (size_t i = 0; i < waypoints.size(); i++)
diff --git a/source/RobotAPI/libraries/core/EigenHelpers.h b/source/RobotAPI/libraries/core/EigenHelpers.h
index 742bcca37d57df9541eebf9eb508197a46bed251..ce933cc8e0f6ae706efbd821ff2d6a6f1ae2a196 100644
--- a/source/RobotAPI/libraries/core/EigenHelpers.h
+++ b/source/RobotAPI/libraries/core/EigenHelpers.h
@@ -23,7 +23,7 @@
#pragma once
-#include <eigen3/Eigen/Core>
+#include <Eigen/Core>
namespace armarx
{
diff --git a/source/RobotAPI/libraries/core/VectorHelpers.h b/source/RobotAPI/libraries/core/VectorHelpers.h
index 4d99a3158f2f2ebc91331d6615da3baefbf02b07..8ebb0f359a71770532d2627146a2b7fb014f732a 100644
--- a/source/RobotAPI/libraries/core/VectorHelpers.h
+++ b/source/RobotAPI/libraries/core/VectorHelpers.h
@@ -25,7 +25,7 @@
#include <ArmarXCore/core/exceptions/Exception.h>
-#include <eigen3/Eigen/Geometry>
+#include <Eigen/Geometry>
#include <cmath>
#include <map>
#include <vector>
diff --git a/source/RobotAPI/libraries/core/observerfilters/OffsetFilter.cpp b/source/RobotAPI/libraries/core/observerfilters/OffsetFilter.cpp
index d8a3246de16fa15282559204e96f45f516567d7c..6830dbc16972143111bdacecaf52907bf99d8427 100644
--- a/source/RobotAPI/libraries/core/observerfilters/OffsetFilter.cpp
+++ b/source/RobotAPI/libraries/core/observerfilters/OffsetFilter.cpp
@@ -38,6 +38,11 @@ namespace armarx::filters
int newValue = dataHistory.rbegin()->second->getInt() - initialValue->getInt();
newVariant = new Variant(newValue);
}
+ else if (type == VariantType::Long)
+ {
+ long newValue = dataHistory.rbegin()->second->getLong() - initialValue->getLong();
+ newVariant = new Variant(newValue);
+ }
else if (type == VariantType::Float)
{
float newValue = dataHistory.rbegin()->second->getFloat() - initialValue->getFloat();
@@ -80,6 +85,7 @@ namespace armarx::filters
{
ParameterTypeList result;
result.push_back(VariantType::Int);
+ result.push_back(VariantType::Long);
result.push_back(VariantType::Float);
result.push_back(VariantType::Double);
result.push_back(VariantType::FramedDirection);
diff --git a/source/RobotAPI/libraries/skills/CMakeLists.txt b/source/RobotAPI/libraries/skills/CMakeLists.txt
index 416121d847e6cf9dab5cbb280fa0d764fbf263c0..bbf9d1d35d12fff2ceb8b51361e09573986c94a3 100644
--- a/source/RobotAPI/libraries/skills/CMakeLists.txt
+++ b/source/RobotAPI/libraries/skills/CMakeLists.txt
@@ -1,4 +1,4 @@
-set(LIB_NAME skills)
+set(LIB_NAME RobotAPISkills)
armarx_component_set_name("${LIB_NAME}")
armarx_set_target("Library: ${LIB_NAME}")
@@ -15,20 +15,26 @@ armarx_add_library(
./manager/SkillManagerComponentPlugin.cpp
./provider/SkillProviderComponentPlugin.cpp
./provider/Skill.cpp
+ ./provider/PeriodicSkill.cpp
./provider/SpecializedSkill.cpp
+ ./provider/PeriodicSpecializedSkill.cpp
./provider/SkillDescription.cpp
./provider/SkillStatusUpdate.cpp
+ ./provider/SkillParameterization.cpp
./provider/helper/LambdaSkillImplementation.cpp
./provider/detail/SkillImplementationWrapper.cpp
HEADERS
./manager/SkillManagerComponentPlugin.h
./provider/SkillProviderComponentPlugin.h
./provider/Skill.h
+ ./provider/PeriodicSkill.h
./provider/SpecializedSkill.h
+ ./provider/PeriodicSpecializedSkill.h
./provider/SkillDescription.h
./provider/SkillStatusUpdate.h
+ ./provider/SkillParameterization.h
./provider/helper/LambdaSkillImplementation.h
./provider/detail/SkillImplementationWrapper.h
)
-add_library(RobotAPI::skills ALIAS skills)
+add_library(RobotAPI::skills ALIAS RobotAPISkills)
diff --git a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp
index 514f11ad7712776cc01ac33c71f3122510d78c01..1b97951eb8e7b6c72e5aa7bcc092c966da497a8c 100644
--- a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp
@@ -30,6 +30,10 @@ namespace armarx
ARMARX_INFO << "Adding a provider with name '" << info.providerName << "'.";
skillProviderMap.insert({info.providerName, info.provider});
}
+ else
+ {
+ ARMARX_INFO << "Trying to add a provider with name '" << info.providerName << "' but the provider already exists.";
+ }
}
void SkillManagerComponentPluginUser::removeProvider(const std::string& providerName, const Ice::Current&)
@@ -37,8 +41,13 @@ namespace armarx
std::lock_guard l(skillProviderMapMutex);
if (auto it = skillProviderMap.find(providerName); it != skillProviderMap.end())
{
+ ARMARX_INFO << "Removing a provider with name '" << providerName << "'.";
skillProviderMap.erase(it);
}
+ else
+ {
+ ARMARX_INFO << "Trying to remove a provider with name '" << providerName << "' but it couldn't found.";
+ }
}
skills::provider::dti::SkillProviderMap SkillManagerComponentPluginUser::getSkillProviders(const Ice::Current&)
@@ -57,6 +66,8 @@ namespace armarx
exInfo.skillName = info.skillName;
exInfo.callbackInterface = myPrx;
exInfo.params = info.params;
+ exInfo.waitUntilSkillFinished = info.waitUntilSkillFinished;
+
it->second->executeSkill(exInfo);
}
else
@@ -65,6 +76,14 @@ namespace armarx
}
}
+ void SkillManagerComponentPluginUser::abortSkill(const std::string& providerName, const std::string& skillName, const Ice::Current ¤t)
+ {
+ if (auto it = skillProviderMap.find(providerName); it != skillProviderMap.end())
+ {
+ it->second->abortSkill(skillName, false);
+ }
+ }
+
void SkillManagerComponentPluginUser::updateStatusForSkill(const skills::provider::dto::SkillStatusUpdate& statusUpdate, const Ice::Current&)
{
(void) statusUpdate;
diff --git a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.h b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.h
index 6fd40f3eb99345a837161c5ffced3161889c28b6..5c59564f6fc16eca8b6e79afafb1d5ba0ef32a4b 100644
--- a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.h
+++ b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.h
@@ -36,7 +36,7 @@ namespace armarx
skills::provider::dti::SkillProviderMap getSkillProviders(const Ice::Current ¤t) override;
void executeSkill(const skills::manager::dto::SkillExecutionInfo& info, const Ice::Current ¤t) override;
void updateStatusForSkill(const skills::provider::dto::SkillStatusUpdate& update, const Ice::Current ¤t) override;
-
+ void abortSkill(const std::string& providerName, const std::string& skillName, const Ice::Current ¤t) override;
private:
armarx::plugins::SkillManagerComponentPlugin* plugin = nullptr;
diff --git a/source/RobotAPI/libraries/skills/provider/PeriodicSkill.cpp b/source/RobotAPI/libraries/skills/provider/PeriodicSkill.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6175015154ea95ecd2229a0b81d16a0035b61265
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/PeriodicSkill.cpp
@@ -0,0 +1,90 @@
+/**
+ * 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 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "PeriodicSkill.h"
+
+#include "ArmarXCore/core/exceptions/LocalException.h"
+#include "ArmarXCore/core/logging/Logging.h"
+#include "ArmarXCore/core/time/Frequency.h"
+#include <ArmarXCore/core/time/Metronome.h>
+
+#include "RobotAPI/libraries/skills/provider/Skill.h"
+
+namespace armarx::skills
+{
+ PeriodicSkill::PeriodicSkill(const SkillDescription& skillDescription,
+ const armarx::Frequency& frequency) :
+ Skill(skillDescription), frequency(frequency)
+ {
+ }
+
+ Skill::Status
+ PeriodicSkill::execute(const aron::data::DictPtr& params, const CallbackT& callback)
+ {
+ if(not initialize(params))
+ {
+ onFailed();
+ return Skill::Status::Failed;
+ }
+
+ core::time::Metronome metronome(frequency);
+
+ while (not stopped and not timeoutReached)
+ {
+ const auto status = executeOnce(params);
+ switch (status)
+ {
+ case Status::Running:
+ // nothing to do here
+ break;
+ case Status::Succeeded:
+ onSucceeded();
+ return Skill::Status::Succeeded;
+ case Status::Failed:
+ onFailed();
+ return Skill::Status::Failed;
+ }
+
+ const auto sleepDuration = metronome.waitForNextTick();
+ if (not sleepDuration.isPositive())
+ {
+ ARMARX_INFO << deactivateSpam() << "PeriodicSkill: execution took too long ("
+ << -sleepDuration << " vs " << frequency.toCycleDuration() << ")";
+ }
+ }
+
+ if (stopped)
+ {
+ onStopped();
+ return Skill::Status::Stopped;
+ }
+
+ if (timeoutReached)
+ {
+ onTimeoutReached();
+ return Skill::Status::TimeoutReached;
+ }
+
+ throw armarx::LocalException("should not happen.");
+ }
+
+
+} // namespace armarx::skills
diff --git a/source/RobotAPI/libraries/skills/provider/PeriodicSkill.h b/source/RobotAPI/libraries/skills/provider/PeriodicSkill.h
new file mode 100644
index 0000000000000000000000000000000000000000..8c8f6c81c4d5c1e1718f330803af6d30c61dea7c
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/PeriodicSkill.h
@@ -0,0 +1,62 @@
+/**
+ * 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 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <ArmarXCore/core/time/Frequency.h>
+
+#include "Skill.h"
+#include "SpecializedSkill.h"
+
+namespace armarx::skills
+{
+
+ class PeriodicSkill : virtual public Skill
+ {
+ public:
+ PeriodicSkill(const SkillDescription& skillDescription, const armarx::Frequency& frequency);
+
+ protected:
+ typename Skill::Status execute(const aron::data::DictPtr& params,
+ const Skill::CallbackT& callback) final;
+
+ enum class Status
+ {
+ Running,
+ Failed,
+ Succeeded
+ };
+
+ virtual bool initialize(const aron::data::DictPtr& params) = 0;
+ virtual Status executeOnce(const aron::data::DictPtr& params) = 0;
+
+ virtual void onSucceeded() = 0;
+ virtual void onStopped() = 0;
+ virtual void onFailed() = 0;
+ virtual void onTimeoutReached() = 0;
+
+ private:
+ const armarx::Frequency frequency;
+ };
+
+
+
+} // namespace armarx::skills
diff --git a/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.cpp b/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4c70437ab37e8fd79fcff3cc373d5b349e1cf9f5
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.cpp
@@ -0,0 +1,28 @@
+/**
+ * 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 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+ #include "PeriodicSpecializedSkill.h"
+
+ namespace armarx::skills
+ {
+
+
+ } // namespace armarx::skills
diff --git a/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.h b/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.h
new file mode 100644
index 0000000000000000000000000000000000000000..01809566186465638e8601e91e56ce7190ce5d39
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.h
@@ -0,0 +1,116 @@
+/**
+ * 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 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <ArmarXCore/core/time/Frequency.h>
+#include <ArmarXCore/core/time/Metronome.h>
+
+#include "Skill.h"
+#include "SpecializedSkill.h"
+
+namespace armarx::skills
+{
+ template <class AronT>
+ class PeriodicSpecializedSkill : public SpecializedSkill<AronT>
+ {
+ public:
+ using Base = SpecializedSkill<AronT>;
+
+ PeriodicSpecializedSkill() = delete;
+
+ PeriodicSpecializedSkill(const SkillDescription& skillDescription,
+ const armarx::Frequency& frequency) :
+ Base(skillDescription), frequency(frequency)
+ {
+ }
+
+ protected:
+ typename Skill::Status
+ execute(const AronT& params, const Skill::CallbackT& callback) final
+ {
+ if (not initialize(params))
+ {
+ onFailed();
+ return Skill::Status::Failed;
+ }
+
+ core::time::Metronome metronome(frequency);
+
+ while (not Skill::stopped and not Skill::timeoutReached)
+ {
+ const auto status = executeOnce(params);
+ switch (status)
+ {
+ case Status::Running:
+ // nothing to do here
+ break;
+ case Status::Succeeded:
+ onSucceeded();
+ return Skill::Status::Succeeded;
+ case Status::Failed:
+ onFailed();
+ return Skill::Status::Failed;
+ }
+
+ const auto sleepDuration = metronome.waitForNextTick();
+ if (not sleepDuration.isPositive())
+ {
+ ARMARX_INFO << deactivateSpam() << "PeriodicSkill: execution took too long ("
+ << -sleepDuration << " vs " << frequency.toCycleDuration() << ")";
+ }
+ }
+
+ if (Skill::stopped)
+ {
+ onStopped();
+ return Skill::Status::Stopped;
+ }
+
+ if (Skill::timeoutReached)
+ {
+ onTimeoutReached();
+ return Skill::Status::TimeoutReached;
+ }
+
+ throw armarx::LocalException("should not happen.");
+ }
+
+ enum class Status
+ {
+ Running,
+ Failed,
+ Succeeded
+ };
+
+ virtual bool initialize(const AronT& params) = 0;
+ virtual Status executeOnce(const AronT& params) = 0;
+
+ virtual void onSucceeded() = 0;
+ virtual void onStopped() = 0;
+ virtual void onFailed() = 0;
+ virtual void onTimeoutReached() = 0;
+
+ private:
+ const armarx::Frequency frequency;
+ };
+
+} // namespace armarx::skills
diff --git a/source/RobotAPI/libraries/skills/provider/Skill.cpp b/source/RobotAPI/libraries/skills/provider/Skill.cpp
index 7a26a5bf92c70d6c3efe0b3b88ba060573207387..c74910e43c3d4b4578c67ad1af06d1cc9fc86f3b 100644
--- a/source/RobotAPI/libraries/skills/provider/Skill.cpp
+++ b/source/RobotAPI/libraries/skills/provider/Skill.cpp
@@ -17,7 +17,7 @@ namespace armarx
return std::thread{ [&](){
if (description.timeoutMs <= 0) return;
long skillStartedAt = IceUtil::Time::now().toMilliSeconds();
- while(running)
+ while(running and !stopRequested())
{
auto now = IceUtil::Time::now().toMilliSeconds();
if ((now - skillStartedAt) >= description.timeoutMs) notifyTimeoutReached();
@@ -30,52 +30,70 @@ namespace armarx
void Skill::notifyTimeoutReached()
{
timeoutReached = true;
- _notifyTimeoutReached();
}
void Skill::notifyStopped()
{
stopped = true;
- _notifyStopped();
}
void Skill::reset()
{
+ started = IceUtil::Time::milliSeconds(-1);
+ exited = IceUtil::Time::milliSeconds(-1);
+
+ running = false;
stopped = false;
timeoutReached = false;
-
- //provider = nullptr;
- //manager = nullptr;
-
- _reset();
}
- void Skill::_notifyTimeoutReached() { }
- void Skill::_notifyStopped() { }
- void Skill::_reset() { }
-
- Skill::Status Skill::execute(const aron::data::DictPtr& params, const CallbackT& callback)
+ void Skill::init(const aron::data::DictPtr& params)
{
+ (void) params;
+
+ ARMARX_IMPORTANT << "Initializing Skill '" << description.skillName << "'";
+
+ // always called before execute (should not take longer than 100ms)
running = true;
- started = IceUtil::Time::now().toMilliSeconds();
+ started = IceUtil::Time::now();
+ timeoutCheck = installTimeoutCondition();
+ }
- auto timeoutCheck = installTimeoutCondition();
+ void Skill::exit(const aron::data::DictPtr& params)
+ {
+ (void) params;
- auto ret = _execute(params, callback);
+ ARMARX_IMPORTANT << "Exiting Skill '" << description.skillName << "'";
- started = 0;
+ // always called after execute (should not take longer than 100ms)
running = false;
- timeoutCheck.join(); // safely wait for the timeoutcheck to return
- return ret;
+ if (timeoutCheck.joinable())
+ {
+ timeoutCheck.join();
+ }
+ exited = IceUtil::Time::now();
}
- Skill::Status Skill::_execute(const aron::data::DictPtr& params, const CallbackT& callback)
+ Skill::Status Skill::execute(const aron::data::DictPtr& params, const CallbackT& callback)
{
(void) params;
- ARMARX_WARNING_S << "You have to override this method!";
+ ARMARX_IMPORTANT << "Executing Skill '" << description.skillName << "'";
return Status::Succeeded;
}
+ Skill::Status Skill::initExecuteExit(const aron::data::DictPtr& params, const CallbackT& callback)
+ {
+ this->reset();
+ this->init(params);
+ auto ret = this->execute(params);
+ this->exit(params);
+ return ret;
+ }
+
+ bool Skill::stopRequested() const
+ {
+ return (stopped || timeoutReached);
+ }
}
}
diff --git a/source/RobotAPI/libraries/skills/provider/Skill.h b/source/RobotAPI/libraries/skills/provider/Skill.h
index 324ccb24e3ea657b5ae879358fa4cb5e62ecc4f7..d4fe0e5cf4fc892cf032f99a7576dfe8d0ab33ab 100644
--- a/source/RobotAPI/libraries/skills/provider/Skill.h
+++ b/source/RobotAPI/libraries/skills/provider/Skill.h
@@ -36,25 +36,26 @@ namespace armarx
Skill(const SkillDescription&);
virtual ~Skill() = default;
- /// Execute Skill
- Status execute(const aron::data::DictPtr& params, const CallbackT& callback = [](const aron::data::DictPtr& returnValue) { (void) returnValue; });
+ /// Override this method with the actual implementation.
+ virtual void init(const aron::data::DictPtr& params);
- /// Reset all parameters before starting a skill.
- void reset();
-
- /// Set the notification bools
- void notifyTimeoutReached();
- void notifyStopped();
+ /// Override this method with the actual implementation.
+ virtual void exit(const aron::data::DictPtr& params);
/// Override this method with the actual implementation. The callback is for status updates to the calling instance
- virtual Status _execute(const aron::data::DictPtr& params, const CallbackT& callback = [](const aron::data::DictPtr& returnValue) { (void) returnValue; });
+ virtual Status execute(const aron::data::DictPtr& params, const CallbackT& callback = [](const aron::data::DictPtr& returnValue) { (void) returnValue; });
+
+ /// Do all methods at once.
+ Status initExecuteExit(const aron::data::DictPtr& params, const CallbackT& callback = [](const aron::data::DictPtr& returnValue) { (void) returnValue; });
- /// Override if you have special members that needs to be resetted
- virtual void _reset();
+ /// Override if you have special members that needs to be resetted. It is called before the skill ititializes
+ virtual void reset();
/// Override these methods if you want to do something special when notification comes
- virtual void _notifyTimeoutReached();
- virtual void _notifyStopped();
+ virtual void notifyTimeoutReached();
+ virtual void notifyStopped();
+
+ virtual bool stopRequested() const;
private:
/// install a condition as a seperate thread
@@ -64,17 +65,21 @@ namespace armarx
const SkillDescription description;
/// running params
- std::atomic_bool running = false;
- long started = 0;
+ IceUtil::Time started = IceUtil::Time::milliSeconds(-1);
+ IceUtil::Time exited = IceUtil::Time::milliSeconds(-1);
- /// proxies that called the skills
- //provider::dti::SkillProviderInterfacePrx provider = nullptr;
- //manager::dti::SkillManagerInterfacePrx manager = nullptr;
+ /// proxies that called the skills. Will be set from provider and is const afterwards
+ provider::dti::SkillProviderInterfacePrx ownerProvider = nullptr;
+ manager::dti::SkillManagerInterfacePrx ownerManager = nullptr;
protected:
/// Use conditions during runtime this way
+ std::atomic_bool running = true;
std::atomic_bool stopped = false;
std::atomic_bool timeoutReached = false;
+
+ private:
+ std::thread timeoutCheck;
};
}
}
diff --git a/source/RobotAPI/libraries/skills/provider/SkillParameterization.cpp b/source/RobotAPI/libraries/skills/provider/SkillParameterization.cpp
index 222c860e47082fc8ea812ac90979738dcf7feb10..2dc791fe3e77495717c0b6fd9f44da19f1ae41f1 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillParameterization.cpp
+++ b/source/RobotAPI/libraries/skills/provider/SkillParameterization.cpp
@@ -4,14 +4,6 @@ namespace armarx
{
namespace skills
{
- provider::dto::SkillParameterization SkillParameterization::toIce() const
- {
- provider::dto::SkillParameterization ret;
- if (params)
- {
- ret.params = params->toAronDictPtr();
- }
- return ret;
- }
+
}
}
diff --git a/source/RobotAPI/libraries/skills/provider/SkillParameterization.h b/source/RobotAPI/libraries/skills/provider/SkillParameterization.h
index b42c0e927e2bddf1db91cfc0c54c7f3383ce452a..a6ce6ed4a95c0c090daa698f3cde8c2ebe30af39 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillParameterization.h
+++ b/source/RobotAPI/libraries/skills/provider/SkillParameterization.h
@@ -12,9 +12,8 @@ namespace armarx
{
struct SkillParameterization
{
- aron::data::DictPtr params;
-
- provider::dto::SkillParameterization toIce() const;
+ aron::data::DictPtr usedInputParams = nullptr;
+ callback::dti::SkillProviderCallbackInterfacePrx usedCallbackInterface = nullptr;
};
}
}
diff --git a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp
index c471f140cc312952e960a922ea26be50996eb260..638de458ae31b7abaa7e2cee610a6505b0d0ed63 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp
@@ -22,11 +22,24 @@ namespace armarx::plugins
auto& p = parent<SkillProviderComponentPluginUser>();
std::string providerName = p.getName();
+ // update skill ownership
+ for (auto& [skillName, impl] : p.skillImplementations)
+ {
+ impl.skill->ownerProvider = myPrx;
+ impl.skill->ownerManager = ownedBySkillManager;
+
+ impl.statusUpdate.providerName = p.getName();
+ impl.statusUpdate.skillName = skillName;
+ }
+
+ // register to manager
skills::manager::dto::ProviderInfo i;
i.provider = myPrx;
i.providerName = providerName;
i.providedSkills = p.getSkills();
- skillManager->addProvider(i);
+ ownedBySkillManager->addProvider(i);
+
+ p.connected = true;
}
void SkillProviderComponentPlugin::preOnDisconnectComponent()
@@ -34,13 +47,13 @@ namespace armarx::plugins
auto& p = parent<SkillProviderComponentPluginUser>();
std::string providerName = p.getName();
- skillManager->removeProvider(providerName);
+ ownedBySkillManager->removeProvider(providerName);
}
void SkillProviderComponentPlugin::postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties)
{
std::string prefix = "skill.";
- properties->component(skillManager, "SkillMemory", prefix + "SkillManager", "The name of the SkillManager (or SkillMemory) proxy.");
+ properties->component(ownedBySkillManager, "SkillMemory", prefix + "SkillManager", "The name of the SkillManager (or SkillMemory) proxy this provider belongs to.");
}
}
@@ -52,14 +65,20 @@ namespace armarx
addPlugin(plugin);
}
- void SkillProviderComponentPluginUser::addSkill(const std::shared_ptr<skills::Skill>& skill)
+ void SkillProviderComponentPluginUser::addSkill(std::unique_ptr<skills::Skill>&& skill)
{
if (!skill)
{
return;
}
- std::lock_guard l(skillsMutex);
+ if (connected)
+ {
+ ARMARX_WARNING << "The SkillProvider already registered to a manager. The skill '" + skill->description.skillName + "' therefore cannot be added anymore.";
+ return;
+ }
+
+ std::unique_lock l(skillsMutex);
std::string skillName = skill->description.skillName;
if (skillImplementations.find(skillName) != skillImplementations.end())
@@ -69,19 +88,18 @@ namespace armarx
}
ARMARX_INFO << "Adding skill " << skillName;
- skills::detail::SkillImplementationWrapper s(skill);
- skillImplementations.insert({skillName, s});
+ skillImplementations.emplace(skillName, std::move(skill));
}
void SkillProviderComponentPluginUser::addSkill(const skills::helper::LambdaSkill::FunT& f, const skills::SkillDescription& desc)
{
- auto lambda = std::make_shared<skills::helper::LambdaSkill>(f, desc);
- addSkill(lambda);
+ auto lambda = std::make_unique<skills::helper::LambdaSkill>(f, desc);
+ addSkill(std::move(lambda));
}
skills::provider::dto::SkillDescriptionMap SkillProviderComponentPluginUser::getSkills(const Ice::Current ¤t)
{
- std::lock_guard l(skillsMutex);
+ std::shared_lock l(skillsMutex);
skills::provider::dto::SkillDescriptionMap skillDesciptions;
for (const auto& [key, skillWrapper] : skillImplementations)
{
@@ -92,51 +110,54 @@ namespace armarx
skills::provider::dto::SkillStatusUpdate SkillProviderComponentPluginUser::getSkillExecutionStatus(const std::string& skill, const Ice::Current ¤t)
{
- std::lock_guard l(skillsMutex);
+ std::shared_lock l(skillsMutex);
auto& skillWrapper = skillImplementations.at(skill);
- std::lock_guard l2(skillWrapper.skillStatusMutex);
+ std::shared_lock l2(skillWrapper.skillStatusMutex);
return skillWrapper.statusUpdate.toIce();
}
+ // Please not that this method waits until the skill can be scheduled!
void SkillProviderComponentPluginUser::executeSkill(const skills::provider::dto::SkillExecutionInfo& info, const Ice::Current ¤t)
{
- std::lock_guard l(skillsMutex);
+ std::shared_lock l(skillsMutex);
std::string skillName = info.skillName;
ARMARX_CHECK_EXPRESSION(skillImplementations.count(skillName) > 0);
auto& wrapper = skillImplementations.at(skillName);
+
+ skills::SkillParameterization usedParameterization;
+ usedParameterization.usedCallbackInterface = info.callbackInterface;
+ usedParameterization.usedInputParams = aron::data::Dict::FromAronDictDTO(info.params);
+
+ // We have to wait until the last task is finished
if (wrapper.task.joinable())
{
wrapper.task.join();
}
- // update input params
- wrapper.reset();
- wrapper.statusUpdate.usedCallbackInterface = info.callbackInterface;
- wrapper.statusUpdate.skillName = info.skillName;
- wrapper.statusUpdate.providerName = getName();
- wrapper.statusUpdate.usedParameterization = aron::data::Dict::FromAronDictDTO(info.params);
+ // recreate thread and execute skill. A skill can only be executed once
+ wrapper.task = std::thread{ [&] { wrapper.execute(usedParameterization);}};
+ std::this_thread::sleep_for(std::chrono::milliseconds(50)); // somehow we need to wait, otherwise it chrashes
+
- // recreate thread and execute skill.
- wrapper.task = std::thread{ [&] { wrapper.execute();}};
+ if (info.waitUntilSkillFinished && wrapper.task.joinable())
+ {
+ // wait until task is finished. We hold the shared lock for the whole time.
+ wrapper.task.join();
+ }
}
- skills::provider::dto::SkillStatusUpdate SkillProviderComponentPluginUser::abortSkill(const std::string& skillName, const Ice::Current ¤t)
+ void SkillProviderComponentPluginUser::abortSkill(const std::string& skillName, bool waitUntilSkillFinished, const Ice::Current ¤t)
{
- std::lock_guard l(skillsMutex);
+ std::shared_lock l(skillsMutex);
ARMARX_CHECK_EXPRESSION(skillImplementations.count(skillName) > 0);
auto& wrapper = skillImplementations.at(skillName);
-
- std::lock_guard l2(wrapper.skillStatusMutex);
-
- if (wrapper.skill->running)
+ if (waitUntilSkillFinished && !wrapper.skill->stopRequested() && wrapper.task.joinable())
{
wrapper.skill->notifyStopped();
wrapper.task.join();
}
-
- return wrapper.statusUpdate.toIce();
}
}
diff --git a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h
index 7e370346cbd8d49f4ec2ac96e638ab85ea23fcea..58895250b3de95d5b3433c92b69fd04c1e201a2b 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h
+++ b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h
@@ -1,6 +1,6 @@
#pragma once
-#include <mutex>
+#include <shared_mutex>
#include <queue>
#include <thread>
#include <functional>
@@ -33,7 +33,7 @@ namespace armarx::plugins
void preOnDisconnectComponent() override;
private:
- skills::manager::dti::SkillManagerInterfacePrx skillManager;
+ skills::manager::dti::SkillManagerInterfacePrx ownedBySkillManager;
skills::provider::dti::SkillProviderInterfacePrx myPrx;
};
}
@@ -50,17 +50,20 @@ namespace armarx
skills::provider::dto::SkillDescriptionMap getSkills(const Ice::Current ¤t = Ice::Current()) override;
skills::provider::dto::SkillStatusUpdate getSkillExecutionStatus(const std::string& skill, const Ice::Current ¤t = Ice::Current()) override;
void executeSkill(const skills::provider::dto::SkillExecutionInfo& executionInfo, const Ice::Current ¤t = Ice::Current()) override;
- skills::provider::dto::SkillStatusUpdate abortSkill(const std::string &skill, const Ice::Current ¤t = Ice::Current()) override;
+ void abortSkill(const std::string &skill, bool waitUntilSkillFinished, const Ice::Current ¤t = Ice::Current()) override;
protected:
void addSkill(const skills::helper::LambdaSkill::FunT&, const skills::SkillDescription&);
- void addSkill(const std::shared_ptr<skills::Skill>&);
+ void addSkill(std::unique_ptr<skills::Skill>&&);
private:
armarx::plugins::SkillProviderComponentPlugin* plugin = nullptr;
protected:
- mutable std::mutex skillsMutex;
+ mutable std::shared_mutex skillsMutex;
+
+ public:
+ bool connected = false;
std::map<std::string, skills::detail::SkillImplementationWrapper> skillImplementations;
};
}
diff --git a/source/RobotAPI/libraries/skills/provider/SkillStatusUpdate.cpp b/source/RobotAPI/libraries/skills/provider/SkillStatusUpdate.cpp
index cfc9f6fd218acc22e618e5ab264078af053f71da..eb98bb0b01d0eee695b0ff98a5754b3038f881a7 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillStatusUpdate.cpp
+++ b/source/RobotAPI/libraries/skills/provider/SkillStatusUpdate.cpp
@@ -11,8 +11,8 @@ namespace armarx
ret.skillName = skillName;
ret.data = aron::data::Dict::ToAronDictDTO(data);
ret.status = status;
- ret.usedCallbackInterface = usedCallbackInterface;
- ret.usedParams = aron::data::Dict::ToAronDictDTO(usedParameterization);
+ ret.usedCallbackInterface = usedParameterization.usedCallbackInterface;
+ ret.usedParams = aron::data::Dict::ToAronDictDTO(usedParameterization.usedInputParams);
return ret;
}
}
diff --git a/source/RobotAPI/libraries/skills/provider/SkillStatusUpdate.h b/source/RobotAPI/libraries/skills/provider/SkillStatusUpdate.h
index 5385c6f0da4d2d1cc86ac664607903652c8a57b2..2b69c7e5d707da474d60285e04a5b01a5fc9a067 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillStatusUpdate.h
+++ b/source/RobotAPI/libraries/skills/provider/SkillStatusUpdate.h
@@ -6,18 +6,19 @@
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
#include <RobotAPI/interface/skills/SkillProviderInterface.h>
+#include "SkillParameterization.h"
+
namespace armarx
{
namespace skills
{
struct SkillStatusUpdate
{
- std::string providerName;
- std::string skillName;
- aron::data::DictPtr usedParameterization;
- callback::dti::SkillProviderCallbackInterfacePrx usedCallbackInterface;
- provider::dto::Execution::Status status;
- aron::data::DictPtr data;
+ std::string providerName = "";
+ std::string skillName = "";
+ provider::dto::Execution::Status status = provider::dto::Execution::Status::Idle;
+ aron::data::DictPtr data = nullptr;
+ SkillParameterization usedParameterization;
provider::dto::SkillStatusUpdate toIce() const;
};
diff --git a/source/RobotAPI/libraries/skills/provider/SpecializedSkill.h b/source/RobotAPI/libraries/skills/provider/SpecializedSkill.h
index fec98231ad74cf58da95c3bfd9324bc342e99c47..68711db013a4a463af03429befcee867d924cbdd 100644
--- a/source/RobotAPI/libraries/skills/provider/SpecializedSkill.h
+++ b/source/RobotAPI/libraries/skills/provider/SpecializedSkill.h
@@ -15,6 +15,8 @@ namespace armarx
class SpecializedSkill : public Skill
{
public:
+ using ParamType = AronT;
+
using Skill::Skill;
virtual ~SpecializedSkill() = default;
@@ -25,22 +27,56 @@ namespace armarx
}
/// Override this method with the actual implementation. The callback is for status updates to the calling instance
- virtual Status _execute(const AronT& params, const CallbackT& callback = [](const aron::data::DictPtr& returnValue) { (void) returnValue; })
+ virtual Status execute(const AronT& params, const CallbackT& callback = [](const aron::data::DictPtr& returnValue) { (void) returnValue; })
{
(void) params;
-
- ARMARX_WARNING_S << "You have to override this method!";
return Status::Succeeded;
}
+ virtual void init(const AronT& params)
+ {
+ (void) params;
+ }
+
+ virtual void exit(const AronT& params)
+ {
+ (void) params;
+ }
+
+ Status initExecuteExit(const AronT& params, const CallbackT& callback = [](const aron::data::DictPtr& returnValue) { (void) returnValue; })
+ {
+ return Skill::initExecuteExit(params.toAron(), callback);
+ }
+
/// Do not use anymore
- Status _execute(const aron::data::DictPtr& params, const CallbackT& callback = [](const aron::data::DictPtr& returnValue) { (void) returnValue; }) final
+ Status execute(const aron::data::DictPtr& params, const CallbackT& callback = [](const aron::data::DictPtr& returnValue) { (void) returnValue; }) final
{
+ Skill::execute(params, callback);
+ AronT p;
+ p.fromAron(params);
+
+ return execute(p, callback);
+ }
+
+ /// Do not use anymore
+ void init(const aron::data::DictPtr& params) final
+ {
+ Skill::init(params);
AronT p;
//ARMARX_IMPORTANT << aron::converter::AronNlohmannJSONConverter::ConvertToNlohmannJSON(params).dump(2);
p.fromAron(params);
- return _execute(p, callback);
+ return init(p);
+ }
+
+ /// Do not use anymore
+ void exit(const aron::data::DictPtr& params) final
+ {
+ Skill::exit(params);
+ AronT p;
+ p.fromAron(params);
+
+ exit(p);
}
};
}
diff --git a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
index 9f9c573b80df17e055baa37737f3f1dc86e4997a..c6add657030fbe8763936fdf28c6ef81bfebeb03 100644
--- a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
+++ b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
@@ -4,25 +4,40 @@ namespace armarx
{
namespace skills::detail
{
- void SkillImplementationWrapper::reset()
+ void SkillImplementationWrapper::execute(const skills::SkillParameterization parameterization)
{
- ARMARX_CHECK_NOT_NULL(skill);
- std::lock_guard l(skillStatusMutex);
- statusUpdate.status = skills::provider::dto::Execution::Status::Idle;
- statusUpdate.data = nullptr;
- skill->reset();
- }
-
- void SkillImplementationWrapper::execute()
- {
- ARMARX_CHECK_NOT_NULL(skill);
- std::lock_guard l(executingMutex);
+ std::unique_lock l(executingMutex);
const std::string skillName = skill->description.skillName;
ARMARX_INFO_S << "Executing skill: " << skillName;
- // get params and setup variables
- auto& aron_params = statusUpdate.usedParameterization;
+ // reset Skill
+ {
+ std::unique_lock l2(skillStatusMutex); // skill is not updating
+ statusUpdate.status = skills::provider::dto::Execution::Status::Idle;
+ statusUpdate.data = nullptr;
+ skill->reset();
+ }
+
+ // set params and setup variables
+ {
+ std::lock_guard l(skillStatusMutex);
+ statusUpdate.usedParameterization = parameterization;
+ }
+
+ auto& aron_params = parameterization.usedInputParams;
+ auto updateStatus = [&](const skills::provider::dto::Execution::Status status, const aron::data::DictPtr& data = nullptr){
+ std::lock_guard l(skillStatusMutex);
+ statusUpdate.status = status;
+ statusUpdate.data = data;
+
+ auto& callbackInterface = statusUpdate.usedParameterization.usedCallbackInterface;
+
+ if (callbackInterface)
+ {
+ callbackInterface->updateStatusForSkill(statusUpdate.toIce());
+ }
+ };
try
{
@@ -33,28 +48,16 @@ namespace armarx
}
// set scheduled
- {
- std::lock_guard l(skillStatusMutex);
- statusUpdate.status = skills::provider::dto::Execution::Status::Scheduled;
- updateStatus();
- }
-
+ updateStatus(skills::provider::dto::Execution::Status::Scheduled);
// execute
- {
- std::lock_guard l(skillStatusMutex);
- statusUpdate.status = skills::provider::dto::Execution::Status::Running;
- updateStatus();
- }
+ updateStatus(skills::provider::dto::Execution::Status::Running);
- auto execution_callback = [&](const aron::data::DictPtr& update)
- {
- statusUpdate.data = update;
- updateStatus();
- };
- auto result = skill->execute(aron_params, execution_callback);
+ skill->init(aron_params);
+ auto ret = skill->execute(aron_params, [&](const aron::data::DictPtr& update) { updateStatus(statusUpdate.status, update); });
+ skill->exit(aron_params);
- switch (result)
+ switch (ret)
{
case skills::Skill::Status::Failed:
throw armarx::LocalException("The Skill '" + skillName + "' failed during execution.");
@@ -64,36 +67,20 @@ namespace armarx
break;
case skills::Skill::Status::Stopped:
{
- std::lock_guard l(skillStatusMutex);
- statusUpdate.status = skills::provider::dto::Execution::Status::Aborted;
- updateStatus();
+ updateStatus(skills::provider::dto::Execution::Status::Aborted);
break;
}
- default:
+ case skills::Skill::Status::Succeeded:
{
- std::lock_guard l(skillStatusMutex);
- statusUpdate.status = skills::provider::dto::Execution::Status::Succeeded;
- updateStatus();
+ updateStatus(skills::provider::dto::Execution::Status::Succeeded);
+ break;
}
}
}
catch (const std::exception& ex)
{
ARMARX_WARNING_S << "Skill " << skillName << " died with exception:\n" << ex.what();
-
- std::lock_guard l(skillStatusMutex);
- statusUpdate.status = skills::provider::dto::Execution::Status::Failed;
- updateStatus();
- }
- }
-
- void SkillImplementationWrapper::updateStatus() const
- {
- auto& callbackInterface = statusUpdate.usedCallbackInterface;
-
- if (callbackInterface)
- {
- callbackInterface->updateStatusForSkill(statusUpdate.toIce());
+ updateStatus(skills::provider::dto::Execution::Status::Failed);
}
}
}
diff --git a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.h b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.h
index 35910a968a66026b5eff1399b133484adef1437a..1c70a14e5353bef38401a929783ed8c92b940054 100644
--- a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.h
+++ b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.h
@@ -1,5 +1,7 @@
#pragma once
+#include <shared_mutex>
+
#include "../SkillDescription.h"
#include "../SkillStatusUpdate.h"
#include "../Skill.h"
@@ -16,32 +18,26 @@ namespace armarx
{
public:
// fixed values. Do not change after skill instantiation
- const std::shared_ptr<Skill> skill;
+ const std::unique_ptr<Skill> skill;
// Current execution status. Changes during execution
// The status also holds the used parameterization
- mutable std::mutex skillStatusMutex;
+ // skillName and providerName are const after registering the skill in a provider
+ mutable std::shared_mutex skillStatusMutex;
SkillStatusUpdate statusUpdate;
// Task information. task is recreated every time the skill restarts
- mutable std::mutex executingMutex;
+ mutable std::shared_mutex executingMutex;
std::thread task;
- SkillImplementationWrapper(const std::shared_ptr<skills::Skill> skill) :
- skill(skill)
+ SkillImplementationWrapper(std::unique_ptr<skills::Skill>&& skill) :
+ skill(std::move(skill))
{
- reset();
+ ARMARX_CHECK_NOT_NULL(this->skill);
}
- SkillImplementationWrapper(const SkillImplementationWrapper& s) :
- SkillImplementationWrapper(s.skill)
- {}
-
- void execute();
- void reset();
-
- protected:
- void updateStatus() const;
+ // execute a skill. The parameterization is copied
+ void execute(const skills::SkillParameterization);
};
}
}
diff --git a/source/RobotAPI/libraries/skills/provider/helper/LambdaSkillImplementation.cpp b/source/RobotAPI/libraries/skills/provider/helper/LambdaSkillImplementation.cpp
index 59d51cd16b66a4799f25e0503d2c987bea7ab4cc..9b761b403f51552235b690d601137ff1c11a002b 100644
--- a/source/RobotAPI/libraries/skills/provider/helper/LambdaSkillImplementation.cpp
+++ b/source/RobotAPI/libraries/skills/provider/helper/LambdaSkillImplementation.cpp
@@ -5,7 +5,7 @@ namespace armarx
namespace skills::helper
{
- Skill::Status LambdaSkill::_execute(const aron::data::DictPtr& data, const CallbackT& callback)
+ Skill::Status LambdaSkill::execute(const aron::data::DictPtr& data, const CallbackT& callback)
{
(void) callback;
bool res = fun(data);
diff --git a/source/RobotAPI/libraries/skills/provider/helper/LambdaSkillImplementation.h b/source/RobotAPI/libraries/skills/provider/helper/LambdaSkillImplementation.h
index 27e1ab0aea43127441d7b4798207433bd7549783..63f537d4cf4c4cfd94633f45fb22ea9223449f5a 100644
--- a/source/RobotAPI/libraries/skills/provider/helper/LambdaSkillImplementation.h
+++ b/source/RobotAPI/libraries/skills/provider/helper/LambdaSkillImplementation.h
@@ -18,7 +18,7 @@ namespace armarx
{};
protected:
- Skill::Status _execute(const aron::data::DictPtr& data, const CallbackT& callback = [](const aron::data::DictPtr& returnValue) { (void) returnValue; }) override;
+ Skill::Status execute(const aron::data::DictPtr& data, const CallbackT& callback = [](const aron::data::DictPtr& returnValue) { (void) returnValue; }) override;
private:
FunT fun;