diff --git a/scenarios/ArMemObjectMemory/ArMemObjectMemory.scx b/scenarios/ArMemObjectMemory/ArMemObjectMemory.scx
index 6224cdd79034bfa4565a6e5d20c4ce4c87a020d1..e6f93e12cda04bf7a80e6a41daada5189f2ef787 100644
--- a/scenarios/ArMemObjectMemory/ArMemObjectMemory.scx
+++ b/scenarios/ArMemObjectMemory/ArMemObjectMemory.scx
@@ -9,6 +9,6 @@
<application name="RobotStateComponent" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="RobotToArVizApp" instance="" package="RobotAPI" nodeName="" enabled="false" iceAutoRestart="false"/>
<application name="ObjectPoseClientExample" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="ArticulatedObjectExampleMemoryWriterClient" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="ArticulatedObjectLocalizerExample" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
</scenario>
diff --git a/scenarios/ArMemObjectMemory/config/ArticulatedObjectLocalizerExample.cfg b/scenarios/ArMemObjectMemory/config/ArticulatedObjectLocalizerExample.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..50dce2f4a5313f280f8f06bab180de49a4ce8098
--- /dev/null
+++ b/scenarios/ArMemObjectMemory/config/ArticulatedObjectLocalizerExample.cfg
@@ -0,0 +1,246 @@
+# ==================================================================
+# ArticulatedObjectLocalizerExample 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.ArticulatedObjectLocalizerExample.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.ArticulatedObjectLocalizerExample.EnableProfiling = false
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.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.ArticulatedObjectLocalizerExample.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectLocalizerExample.ObjectName = ""
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.CoreSegment: Name of the memory core segment to use for object classes.
+# Attributes:
+# - Default: ArticulatedObjectClass
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.CoreSegment = ArticulatedObjectClass
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.MemoryName:
+# Attributes:
+# - Default: Object
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.MemoryName = Object
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.ProviderName:
+# Attributes:
+# - Default: ArmarXObjects
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.ProviderName = ExampleProvider
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
+# Set to false to use this memory as a stand-alone.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ArticulatedObjectLocalizerExample.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectLocalizerExample.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.tpc.pub.DebugObserver: Name of the `DebugObserver` topic to publish data to.
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectLocalizerExample.tpc.pub.DebugObserver = DebugObserver
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.tpc.sub.MemoryListener: Name of the `MemoryListener` topic to subscribe to.
+# Attributes:
+# - Default: MemoryUpdates
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectLocalizerExample.tpc.sub.MemoryListener = MemoryUpdates
+
+
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_USER_CONFIG_DIR is set, the cache path will be made relative to ARMARX_USER_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
+# 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.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoadLibraries = ""
+
+
+# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoggingGroup = ""
+
+
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
+
+
+# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+# Attributes:
+# - Default: 3000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SecondsStartupDelay = 0
+
+
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.StartDebuggerOnCrash = false
+
+
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ThreadPoolSize = 1
+
+
+# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
+
+
+# ArmarX.Verbosity: Global logging level for whole application
+# Attributes:
+# - Default: Info
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Info
+
+
diff --git a/scenarios/ArMemObjectMemory/config/ObjectMemory.cfg b/scenarios/ArMemObjectMemory/config/ObjectMemory.cfg
index 67e86da4ae2afbacca3e22a5eb4bc878f801145f..77b3f690f0906f31315d4f51d744bc02ab0bfe8c 100644
--- a/scenarios/ArMemObjectMemory/config/ObjectMemory.cfg
+++ b/scenarios/ArMemObjectMemory/config/ObjectMemory.cfg
@@ -7,7 +7,7 @@
# - Default: Default value not mapped.
# - Case sensitivity: yes
# - Required: no
-# ArmarX.AdditionalPackages = Default value not mapped.
+ArmarX.AdditionalPackages = ArmarXObjects
# ArmarX.ApplicationName: Application name
@@ -174,14 +174,13 @@
# ArmarX.ObjectMemory.mem.articulated.cls.CoreSegmentName = ArticulatedObjectClass
-# ArmarX.ObjectMemory.mem.articulated.cls.DiscardSnapshotsWhileAttached: If true, no new snapshots are stored while an object is attached to a robot node.
-# If false, new snapshots are stored, but the attachment is kept in the new snapshots.
+# ArmarX.ObjectMemory.mem.articulated.cls.LoadFromObjectsPackage: If true, load the objects from the objects package on startup.
# Attributes:
# - Default: true
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ObjectMemory.mem.articulated.cls.DiscardSnapshotsWhileAttached = true
+# ArmarX.ObjectMemory.mem.articulated.cls.LoadFromObjectsPackage = true
# ArmarX.ObjectMemory.mem.articulated.cls.MaxHistorySize: Maximal size of object poses history (-1 for infinite).
@@ -192,22 +191,20 @@
# ArmarX.ObjectMemory.mem.articulated.cls.MaxHistorySize = -1
-# ArmarX.ObjectMemory.mem.articulated.inst.CoreSegmentName: Name of the object instance core segment.
+# ArmarX.ObjectMemory.mem.articulated.cls.ObjectsPackage: Name of the objects package to load from.
# Attributes:
-# - Default: ArticulatedObjectInstance
+# - Default: ArmarXObjects
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectMemory.mem.articulated.inst.CoreSegmentName = ArticulatedObjectInstance
+# ArmarX.ObjectMemory.mem.articulated.cls.ObjectsPackage = ArmarXObjects
-# ArmarX.ObjectMemory.mem.articulated.inst.DiscardSnapshotsWhileAttached: If true, no new snapshots are stored while an object is attached to a robot node.
-# If false, new snapshots are stored, but the attachment is kept in the new snapshots.
+# ArmarX.ObjectMemory.mem.articulated.inst.CoreSegmentName: Name of the object instance core segment.
# Attributes:
-# - Default: true
+# - Default: ArticulatedObjectInstance
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ObjectMemory.mem.articulated.inst.DiscardSnapshotsWhileAttached = true
+# ArmarX.ObjectMemory.mem.articulated.inst.CoreSegmentName = ArticulatedObjectInstance
# ArmarX.ObjectMemory.mem.articulated.inst.MaxHistorySize: Maximal size of object poses history (-1 for infinite).
@@ -278,12 +275,12 @@
# ArmarX.ObjectMemory.mem.cls.MaxHistorySize = -1
-# ArmarX.ObjectMemory.mem.cls.ObjectsPackgage: Name of the objects package to load from.
+# ArmarX.ObjectMemory.mem.cls.ObjectsPackage: Name of the objects package to load from.
# Attributes:
# - Default: ArmarXObjects
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectMemory.mem.cls.ObjectsPackgage = ArmarXObjects
+# ArmarX.ObjectMemory.mem.cls.ObjectsPackage = ArmarXObjects
# ArmarX.ObjectMemory.mem.inst.CoreSegmentName: Name of the object instance core segment.
diff --git a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d1e48cdea6aa941033225c291e50a3da312f9475
--- /dev/null
+++ b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
@@ -0,0 +1,172 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "ArticulatedObjectLocalizerExample.h"
+#include "ArmarXCore/core/services/tasks/PeriodicTask.h"
+
+
+#include <memory>
+
+#include <Eigen/Geometry>
+
+#include <IceUtil/Time.h>
+
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/XML/RobotIO.h>
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/PackagePath.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/time/CycleUtil.h>
+
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/query/query_fns.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
+#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem_objects/types.h>
+
+
+namespace armarx::articulated_object
+{
+ ArticulatedObjectLocalizerExample::ArticulatedObjectLocalizerExample() :
+ articulatedObjectWriter(new ::armarx::armem::articulated_object::Writer(*this)),
+ articulatedObjectReader(new ::armarx::armem::articulated_object::Reader(*this)) {}
+
+ armarx::PropertyDefinitionsPtr ArticulatedObjectLocalizerExample::createPropertyDefinitions()
+ {
+ armarx::PropertyDefinitionsPtr defs =
+ new ComponentPropertyDefinitions(getConfigIdentifier());
+
+ defs->topic(debugObserver);
+
+ defs->optional(p.updateFrequency, "updateFrequency", "Memory update frequency (write).");
+
+ articulatedObjectWriter->registerPropertyDefinitions(defs);
+ articulatedObjectReader->registerPropertyDefinitions(defs);
+
+ return defs;
+ }
+
+ std::string ArticulatedObjectLocalizerExample::getDefaultName() const
+ {
+ return "ArticulatedObjectLocalizerExample";
+ }
+
+ void ArticulatedObjectLocalizerExample::onInitComponent() {}
+
+ void ArticulatedObjectLocalizerExample::onConnectComponent()
+ {
+ articulatedObjectWriter->connect();
+ articulatedObjectReader->connect();
+
+ ARMARX_IMPORTANT << "Running example.";
+ start = armem::Time::now();
+
+ task = new PeriodicTask<ArticulatedObjectLocalizerExample>(this, &ArticulatedObjectLocalizerExample::run, 1000 / p.updateFrequency);
+ task->start();
+ }
+
+ void ArticulatedObjectLocalizerExample::onDisconnectComponent()
+ {
+ task->stop();
+ }
+
+ void ArticulatedObjectLocalizerExample::onExitComponent() {}
+
+ VirtualRobot::RobotPtr ArticulatedObjectLocalizerExample::createDishwasher()
+ {
+ const std::string dishwasherName = "CupboardWithDishwasher";
+
+ const auto descriptions = articulatedObjectReader->queryDescriptions(IceUtil::Time::now());
+
+ ARMARX_INFO << "Found " << descriptions.size() << " articulated object descriptions";
+
+ const auto it = std::find_if(descriptions.begin(), descriptions.end(), [&](const armem::articulated_object::ArticulatedObjectDescription & desc) -> bool
+ {
+ return desc.name == dishwasherName;
+ });
+
+ if (it == descriptions.end())
+ {
+ ARMARX_WARNING << "Articulated object " << dishwasherName << " not (yet) available";
+ return nullptr;
+ }
+
+ return VirtualRobot::RobotIO::loadRobot(ArmarXDataPath::resolvePath(it->xml.serialize().path), VirtualRobot::RobotIO::eStructure);
+ }
+
+ armem::articulated_object::ArticulatedObject convert(const VirtualRobot::Robot& obj, const armem::Time& timestamp)
+ {
+ ARMARX_DEBUG << "Filename is " << obj.getFilename();
+
+ return
+ armem::articulated_object::ArticulatedObject
+ {
+ .description = {
+ .name = obj.getName(),
+ .xml = PackagePath(armarx::ArmarXDataPath::getProject({"ArmarXObjects"}, obj.getFilename()), obj.getFilename())
+ },
+ .instance = "", // TODO(fabian.reister):
+ .config = {
+ .timestamp = timestamp,
+ .globalPose = Eigen::Affine3f(obj.getRootNode()->getGlobalPose()),
+ .jointMap = obj.getJointValues()
+ },
+ .timestamp = timestamp
+ };
+ }
+
+ void ArticulatedObjectLocalizerExample::run()
+ {
+ if (dishwasher == nullptr)
+ {
+ dishwasher = createDishwasher();
+
+ if (dishwasher == nullptr) // still
+ {
+ return;
+ }
+ }
+
+ ARMARX_DEBUG << "Reporting articulated objects";
+
+ const IceUtil::Time now = TimeUtil::GetTime();
+ const float t = float((now - start).toSecondsDouble());
+
+ // move joints at certain frequency
+ const float k = (1 + std::sin(t / (M_2_PIf32))) / 2; // in [0,1]
+
+ const std::map<std::string, float> jointValues
+ {
+ {"dishwasher_door_joint", M_PIf32 / 2 * k},
+ {"drawer_joint", 350 * k}
+ };
+
+ dishwasher->setJointValues(jointValues);
+
+ armarx::armem::articulated_object::ArticulatedObject armemDishwasher = convert(*dishwasher, IceUtil::Time::now());
+ articulatedObjectWriter->store(armemDishwasher);
+ }
+
+} // namespace armarx::articulated_object
diff --git a/source/RobotAPI/components/armem/client/ArticulatedObjectExampleMemoryWriterClient/ArticulatedObjectExampleMemoryWriterClient.h b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h
similarity index 65%
rename from source/RobotAPI/components/armem/client/ArticulatedObjectExampleMemoryWriterClient/ArticulatedObjectExampleMemoryWriterClient.h
rename to source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h
index e7d60e9c1904c40c21018b773b7f3ce55831c407..75c0764cfb9c6103a82c68c0faba2b984ffabb86 100644
--- a/source/RobotAPI/components/armem/client/ArticulatedObjectExampleMemoryWriterClient/ArticulatedObjectExampleMemoryWriterClient.h
+++ b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h
@@ -3,6 +3,8 @@
// ArmarX
+#include "ArmarXCore/core/services/tasks/PeriodicTask.h"
+#include "RobotAPI/libraries/armem/core/Time.h"
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXCore/util/tasks.h>
@@ -15,6 +17,8 @@
#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
#include <RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h>
+#include <RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h>
+#include <VirtualRobot/VirtualRobot.h>
namespace armarx::articulated_object
{
@@ -30,12 +34,12 @@ namespace armarx::articulated_object
*
* Detailed description of class ExampleClient.
*/
- class ArticulatedObjectExampleMemoryWriterClient :
- virtual public armarx::Component
- // virtual public armarx::armem::client::WriterComponentPluginUser
+ class ArticulatedObjectLocalizerExample :
+ virtual public armarx::Component,
+ virtual public armarx::armem::client::ComponentPluginUser
{
public:
- ArticulatedObjectExampleMemoryWriterClient();
+ ArticulatedObjectLocalizerExample();
/// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override;
@@ -54,11 +58,23 @@ namespace armarx::articulated_object
private:
- armarx::RunningTask<ArticulatedObjectExampleMemoryWriterClient>::pointer_type task;
+ VirtualRobot::RobotPtr createDishwasher();
+ std::shared_ptr<VirtualRobot::Robot> dishwasher;
+
+ /// Reference timestamp for object movement
+ armem::Time start;
+
+ armarx::PeriodicTask<ArticulatedObjectLocalizerExample>::pointer_type task;
armarx::DebugObserverInterfacePrx debugObserver;
std::unique_ptr<::armarx::armem::articulated_object::Writer> articulatedObjectWriter;
+ std::unique_ptr<::armarx::armem::articulated_object::Reader> articulatedObjectReader;
+
+ struct Properties
+ {
+ float updateFrequency{25.F};
+ } p;
};
diff --git a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/CMakeLists.txt b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..e92a5b368905d9046bb439cf435c9ae4276aed3e
--- /dev/null
+++ b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/CMakeLists.txt
@@ -0,0 +1,25 @@
+set(LIB_NAME ArticulatedObjectLocalizerExample)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+# Add the component
+armarx_add_component(
+ COMPONENT_LIBS
+ ArmarXCore
+ armem_objects
+ SOURCES
+ ArticulatedObjectLocalizerExample.cpp
+
+ HEADERS
+ ArticulatedObjectLocalizerExample.h
+)
+
+# Add unit tests
+add_subdirectory(test)
+
+# Generate the application
+armarx_generate_and_add_component_executable(
+ # If your component is not defined in ::armarx, specify its namespace here:
+ COMPONENT_NAMESPACE "armarx::articulated_object"
+)
diff --git a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/test/ArticulatedObjectLocalizerExampleTest.cpp b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/test/ArticulatedObjectLocalizerExampleTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..551ba1265648103ef7d94282bc320ea486e3e3a6
--- /dev/null
+++ b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/test/ArticulatedObjectLocalizerExampleTest.cpp
@@ -0,0 +1,37 @@
+/*
+ * 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::ArticulatedObjectLocalizerExample
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#define BOOST_TEST_MODULE RobotAPI::ArmarXObjects::ArticulatedObjectLocalizerExample
+
+#define ARMARX_BOOST_TEST
+
+#include <RobotAPI/Test.h>
+#include "../ArticulatedObjectLocalizerExample.h"
+
+#include <iostream>
+
+BOOST_AUTO_TEST_CASE(testExample)
+{
+ armarx::articulated_object::ArticulatedObjectLocalizerExample instance;
+
+ BOOST_CHECK_EQUAL(true, true);
+}
diff --git a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/test/CMakeLists.txt b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/test/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..e67df2dc9f93e582d59ce368103bcc022cf7843b
--- /dev/null
+++ b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/test/CMakeLists.txt
@@ -0,0 +1,5 @@
+
+# Libs required for the tests
+SET(LIBS ${LIBS} ArmarXCore ArticulatedObjectLocalizerExample)
+
+armarx_add_test(ArticulatedObjectLocalizerExampleTest ArticulatedObjectLocalizerExampleTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/components/CMakeLists.txt b/source/RobotAPI/components/CMakeLists.txt
index d19343cee661810483206528bfccaa62c614bff4..97d8648b4fde12137ab393ed95bcae6ae2a8648c 100644
--- a/source/RobotAPI/components/CMakeLists.txt
+++ b/source/RobotAPI/components/CMakeLists.txt
@@ -30,3 +30,5 @@ add_subdirectory(ViewSelection)
add_subdirectory(SkillObserver)
+
+add_subdirectory(ArticulatedObjectLocalizerExample)
\ No newline at end of file
diff --git a/source/RobotAPI/components/armem/client/ArticulatedObjectExampleMemoryWriterClient/ArticulatedObjectExampleMemoryWriterClient.cpp b/source/RobotAPI/components/armem/client/ArticulatedObjectExampleMemoryWriterClient/ArticulatedObjectExampleMemoryWriterClient.cpp
deleted file mode 100644
index efe2de65b74e5f4a06b76983b1b1ae3f2e5097c4..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/armem/client/ArticulatedObjectExampleMemoryWriterClient/ArticulatedObjectExampleMemoryWriterClient.cpp
+++ /dev/null
@@ -1,133 +0,0 @@
-
-
-#include "ArticulatedObjectExampleMemoryWriterClient.h"
-
-#include <memory>
-
-#include <Eigen/Geometry>
-
-#include <IceUtil/Time.h>
-
-#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/XML/RobotIO.h>
-#include <VirtualRobot/VirtualRobot.h>
-
-#include <ArmarXCore/core/PackagePath.h>
-#include <ArmarXCore/core/system/ArmarXDataPath.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <ArmarXCore/core/time/CycleUtil.h>
-
-#include <RobotAPI/libraries/armem/client/query/Builder.h>
-#include <RobotAPI/libraries/armem/client/query/query_fns.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
-#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
-#include <RobotAPI/libraries/armem/core/Time.h>
-
-
-namespace armarx::articulated_object
-{
- ArticulatedObjectExampleMemoryWriterClient::ArticulatedObjectExampleMemoryWriterClient() :
- articulatedObjectWriter(new ::armarx::armem::articulated_object::Writer(*this)) {}
-
- armarx::PropertyDefinitionsPtr ArticulatedObjectExampleMemoryWriterClient::createPropertyDefinitions()
- {
- armarx::PropertyDefinitionsPtr defs =
- new ComponentPropertyDefinitions(getConfigIdentifier());
-
- defs->topic(debugObserver);
-
- // defs->optional(memoryName, "mem.MemoryName", "Name of the memory to use.");
-
- articulatedObjectWriter->registerPropertyDefinitions(defs);
-
- return defs;
- }
-
- std::string ArticulatedObjectExampleMemoryWriterClient::getDefaultName() const
- {
- return "ArticulatedObjectExampleMemoryWriterClient";
- }
-
- void ArticulatedObjectExampleMemoryWriterClient::onInitComponent() {}
-
- void ArticulatedObjectExampleMemoryWriterClient::onConnectComponent()
- {
- articulatedObjectWriter->connect();
-
- task = new RunningTask<ArticulatedObjectExampleMemoryWriterClient>(this, &ArticulatedObjectExampleMemoryWriterClient::run);
- task->start();
- }
-
- void ArticulatedObjectExampleMemoryWriterClient::onDisconnectComponent()
- {
- task->stop();
- }
-
- void ArticulatedObjectExampleMemoryWriterClient::onExitComponent() {}
-
- VirtualRobot::RobotPtr createDishwasher()
- {
- const std::string xml =
- "./ArmarXObjects/Environment/mobile-kitchen/dishwasher-only/dishwasher.xml";
-
- return VirtualRobot::RobotIO::loadRobot(ArmarXDataPath::resolvePath(xml), VirtualRobot::RobotIO::eStructure);
- }
-
- armem::articulated_object::ArticulatedObject convert(const VirtualRobot::Robot& obj, const armem::Time& timestamp)
- {
- ARMARX_INFO << "Filename is " << obj.getFilename();
-
- return
- armem::articulated_object::ArticulatedObject
- {
- .description = {
- .name = obj.getName(),
- .xml = PackagePath(armarx::ArmarXDataPath::getProject({"ArmarXObjects"}, obj.getFilename()), obj.getFilename())
- },
- .instance = "", // TODO(fabian.reister):
- .config = {
- .timestamp = timestamp,
- .globalPose = Eigen::Affine3f(obj.getRootNode()->getGlobalPose()),
- .jointMap = obj.getJointValues()
- },
- .timestamp = timestamp
- };
- }
-
- void ArticulatedObjectExampleMemoryWriterClient::run()
- {
- ARMARX_IMPORTANT << "Running example.";
-
- std::shared_ptr<VirtualRobot::Robot> dishwasher = createDishwasher();
-
- CycleUtil cycle(IceUtil::Time::milliSeconds(100));
- IceUtil::Time start = TimeUtil::GetTime();
-
- CycleUtil c(100);
-
- while (not task->isStopped())
- {
- ARMARX_INFO << "Reporting articulated objects";
-
- const IceUtil::Time now = TimeUtil::GetTime();
- const float t = float((now - start).toSecondsDouble());
-
- // move joints at certain frequency
- const float k = (1 + std::sin(t / (M_2_PIf32))) / 2; // in [0,1]
-
- const std::map<std::string, float> jointValues
- {
- {"dishwasher_door_joint", M_PIf32 / 2 * k},
- {"drawer_joint", 350 * k}
- };
-
- dishwasher->setJointValues(jointValues);
-
- armarx::armem::articulated_object::ArticulatedObject armemDishwasher = convert(*dishwasher, IceUtil::Time::now());
- articulatedObjectWriter->store(armemDishwasher);
-
- c.waitForCycleDuration();
- }
- }
-
-} // namespace armarx::articulated_object
diff --git a/source/RobotAPI/components/armem/client/CMakeLists.txt b/source/RobotAPI/components/armem/client/CMakeLists.txt
index a7594d23e1668080fc0006afc44b1a09abfa8587..de02c9f4e7ef45534c2675db6173205436b4475c 100644
--- a/source/RobotAPI/components/armem/client/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/client/CMakeLists.txt
@@ -1,2 +1,2 @@
add_subdirectory(ExampleMemoryClient)
-add_subdirectory(ArticulatedObjectExampleMemoryWriterClient)
+add_subdirectory(VirtualRobotReaderExampleClient)
diff --git a/source/RobotAPI/components/armem/client/ArticulatedObjectExampleMemoryWriterClient/CMakeLists.txt b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/CMakeLists.txt
similarity index 66%
rename from source/RobotAPI/components/armem/client/ArticulatedObjectExampleMemoryWriterClient/CMakeLists.txt
rename to source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/CMakeLists.txt
index b87d357561b4ce3583f450826a173c86034a40a4..cd132bb87336bf793e86e79b7e048ea044921491 100644
--- a/source/RobotAPI/components/armem/client/ArticulatedObjectExampleMemoryWriterClient/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/CMakeLists.txt
@@ -1,4 +1,4 @@
-armarx_component_set_name("ArticulatedObjectExampleMemoryWriterClient")
+armarx_component_set_name("VirtualRobotReaderExampleClient")
find_package(IVT QUIET)
armarx_build_if(IVT_FOUND "IVT not available")
@@ -9,16 +9,15 @@ set(COMPONENT_LIBS
ArmarXGuiComponentPlugins
RobotAPICore
RobotAPIInterfaces
- armem
- armem_objects
+ armem_robot_state
)
set(SOURCES
- ArticulatedObjectExampleMemoryWriterClient.cpp
+ VirtualRobotReaderExampleClient.cpp
)
set(HEADERS
- ArticulatedObjectExampleMemoryWriterClient.h
+ VirtualRobotReaderExampleClient.h
)
armarx_add_component("${SOURCES}" "${HEADERS}")
@@ -27,4 +26,4 @@ armarx_add_component("${SOURCES}" "${HEADERS}")
# add_subdirectory(test)
#generate the application
-armarx_generate_and_add_component_executable(COMPONENT_NAMESPACE "armarx::articulated_object")
+armarx_generate_and_add_component_executable(COMPONENT_NAMESPACE "armarx::robot_state")
diff --git a/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.cpp b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..85ca85f48ca62c7bc686bd9286d52fae5480ebc7
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.cpp
@@ -0,0 +1,105 @@
+
+
+#include "VirtualRobotReaderExampleClient.h"
+
+
+#include <memory>
+
+#include <Eigen/Geometry>
+
+#include <IceUtil/Time.h>
+
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/XML/RobotIO.h>
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <ArmarXCore/core/PackagePath.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/query/query_fns.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
+#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
+
+
+namespace armarx::robot_state
+{
+ VirtualRobotReaderExampleClient::VirtualRobotReaderExampleClient() :
+ virtualRobotReader(*this) {}
+
+ armarx::PropertyDefinitionsPtr VirtualRobotReaderExampleClient::createPropertyDefinitions()
+ {
+ armarx::PropertyDefinitionsPtr defs =
+ new ComponentPropertyDefinitions(getConfigIdentifier());
+
+ defs->topic(debugObserver);
+
+ defs->optional(p.robotName, "robotName");
+ defs->optional(p.updateFrequency, "updateFrequency");
+
+ virtualRobotReader.registerPropertyDefinitions(defs);
+
+ return defs;
+ }
+
+ std::string VirtualRobotReaderExampleClient::getDefaultName() const
+ {
+ return "VirtualRobotReaderExampleClient";
+ }
+
+ void VirtualRobotReaderExampleClient::onInitComponent() {}
+
+ void VirtualRobotReaderExampleClient::onConnectComponent()
+ {
+ virtualRobotReader.connect();
+
+ ARMARX_IMPORTANT << "Running virtual robot synchronization example.";
+
+ task = new PeriodicTask<VirtualRobotReaderExampleClient>(this, &VirtualRobotReaderExampleClient::run, 1000 / p.updateFrequency);
+ task->start();
+ }
+
+ void VirtualRobotReaderExampleClient::onDisconnectComponent()
+ {
+ task->stop();
+ }
+
+ void VirtualRobotReaderExampleClient::onExitComponent() {}
+
+ void VirtualRobotReaderExampleClient::run()
+ {
+
+ // initialize if needed
+ if (virtualRobot == nullptr)
+ {
+ TIMING_START(getRobot);
+
+ virtualRobot = virtualRobotReader.getRobot(p.robotName, IceUtil::Time::now());
+
+ if (virtualRobot == nullptr)
+ {
+ ARMARX_WARNING << deactivateSpam(1) << "Could not create virtual robot.";
+ return;
+ }
+ // only print timing once the robot is loadable & loaded
+ TIMING_END_STREAM(getRobot, ARMARX_INFO);
+ }
+
+ ARMARX_INFO << deactivateSpam(10) << "Synchronizing robot";
+
+ const IceUtil::Time now = TimeUtil::GetTime();
+
+ TIMING_START(synchronizeRobot);
+ virtualRobotReader.synchronizeRobot(*virtualRobot, now);
+ TIMING_END_STREAM(synchronizeRobot, ARMARX_INFO);
+
+
+ }
+
+} // namespace armarx::robot_state
diff --git a/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.h b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.h
new file mode 100644
index 0000000000000000000000000000000000000000..4e1d01f5564e96c7e46ad2db33081835873dd7a6
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.h
@@ -0,0 +1,96 @@
+
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+
+// ArmarX
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/interface/observers/ObserverInterface.h>
+#include <ArmarXCore/util/tasks.h>
+#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
+
+// RobotAPI
+#include <RobotAPI/interface/armem/server/MemoryInterface.h>
+#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+#include <RobotAPI/libraries/armem/client/ComponentPlugin.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
+
+#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
+
+namespace armarx::robot_state
+{
+
+ /**
+ * @defgroup Component-ExampleClient ExampleClient
+ * @ingroup RobotAPI-Components
+ * A description of the component ExampleClient.
+ *
+ * @class ExampleClient
+ * @ingroup Component-ExampleClient
+ * @brief Brief description of class ExampleClient.
+ *
+ * Detailed description of class ExampleClient.
+ */
+ class VirtualRobotReaderExampleClient :
+ virtual public armarx::Component,
+ virtual public armarx::armem::client::ComponentPluginUser
+ {
+ public:
+ VirtualRobotReaderExampleClient();
+
+ /// @see armarx::ManagedIceObject::getDefaultName()
+ std::string getDefaultName() const override;
+
+ protected:
+
+ armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+
+ void onInitComponent() override;
+ void onConnectComponent() override;
+ void onDisconnectComponent() override;
+ void onExitComponent() override;
+
+ void run();
+
+
+ private:
+
+ struct Properties
+ {
+ std::string robotName{"Armar6"};
+ float updateFrequency{10.F};
+ } p;
+
+ armarx::PeriodicTask<VirtualRobotReaderExampleClient>::pointer_type task;
+
+ armarx::DebugObserverInterfacePrx debugObserver;
+
+ // std::unique_ptr<::armarx::armem::articulated_object::Writer> articulatedObjectWriter;
+
+ armem::robot_state::VirtualRobotReader virtualRobotReader;
+
+ std::shared_ptr<VirtualRobot::Robot> virtualRobot{nullptr};
+
+ };
+
+} // namespace armarx::robot_state
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/CMakeLists.txt b/source/RobotAPI/components/armem/server/RobotStateMemory/CMakeLists.txt
index 4667365b1a6187694ca7bdbd06003275828ead55..137453970c1a97f0f3bd9653c067e959db21b4a5 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/CMakeLists.txt
@@ -4,8 +4,11 @@ armarx_component_set_name("RobotStateMemory")
set(COMPONENT_LIBS
ArmarXCore ArmarXCoreInterfaces # for DebugObserverInterface
ArmarXGuiComponentPlugins
- RobotAPICore RobotAPIInterfaces RobotAPIComponentPlugins armem
- # RobotAPIComponentPlugins # for ArViz and other plugins
+ RobotAPICore
+ RobotAPIInterfaces
+ RobotAPIComponentPlugins
+ armem
+ armem_robot_state
)
set(SOURCES
@@ -18,7 +21,7 @@ set(HEADERS
armarx_add_component("${SOURCES}" "${HEADERS}")
#generate the application
-armarx_generate_and_add_component_executable()
+armarx_generate_and_add_component_executable(COMPONENT_NAMESPACE armarx::armem::server::robot_state)
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
index 1624f899ca6dbae34d3ef210d84559c3a1390b2b..5aba1210686f0cc0a821d83f1053e65a41c348e6 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
@@ -20,13 +20,20 @@
* GNU General Public License
*/
+#include "RobotStateMemory.h"
+
// STD
+#include <Eigen/src/Geometry/Transform.h>
+#include <SimoxUtility/math/convert/rpy_to_mat3f.h>
#include <algorithm>
#include <iostream>
#include <fstream>
// Header
-#include "RobotStateMemory.h"
+#include "ArmarXCore/core/logging/Logging.h"
+#include "RobotAPI/libraries/armem/core/Time.h"
+#include "RobotAPI/libraries/aron/core/navigator/data/primitive/Primitive.h"
+#include "RobotAPI/libraries/core/Pose.h"
// Simox
#include <SimoxUtility/algorithm/string.h>
@@ -40,19 +47,44 @@
#include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
#include <RobotAPI/libraries/aron/core/navigator/type/AllNavigators.h>
-namespace armarx
+#include <RobotAPI/libraries/armem_robot_state/server/common/Visu.h>
+
+#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
+
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include <memory>
+
+namespace armarx::armem::server::robot_state
{
RobotStateMemory::RobotStateMemory()
+ : descriptionSegment(server::ComponentPluginUser::iceMemory,
+ server::ComponentPluginUser::workingMemoryMutex),
+ proprioceptionSegment(server::ComponentPluginUser::iceMemory,
+ server::ComponentPluginUser::workingMemoryMutex),
+ localizationSegment(server::ComponentPluginUser::iceMemory,
+ server::ComponentPluginUser::workingMemoryMutex),
+ commonVisu(descriptionSegment, proprioceptionSegment, localizationSegment)
{
+
}
+ RobotStateMemory::~RobotStateMemory() = default;
+
+
armarx::PropertyDefinitionsPtr RobotStateMemory::createPropertyDefinitions()
{
+ const std::string robotUnitPrefix{sensorValuePrefix};
+
armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
- defs->optional(robotUnitSensorPrefix, "SensorValuePrefix", "Prefix of all sensor values");
- defs->optional(robotUnitMemoryBatchSize, "RobotUnitMemoryBatchSize", "The size of the entity snapshot to send to the memory. Min is 1");
- defs->optional(robotUnitPollFrequency, "RobotUnitUpdateFrequency", "The frequency in Hz to store values. All other values get discarded. Min is 1, max is " + std::to_string(ROBOT_UNIT_MAXIMUM_FREQUENCY));
- defs->optional(robotUnitConfigPath, "robotUnitConfigPath", "Specify a configuration file to group the sensor values specifically.");
+ defs->optional(robotUnitSensorPrefix, robotUnitPrefix + "SensorValuePrefix", "Prefix of all sensor values");
+ defs->optional(robotUnitMemoryBatchSize, robotUnitPrefix + "MemoryBatchSize", "The size of the entity snapshot to send to the memory. Min is 1");
+ defs->optional(robotUnitPollFrequency, robotUnitPrefix + "UpdateFrequency", "The frequency in Hz to store values. All other values get discarded. Min is 1, max is " + std::to_string(ROBOT_UNIT_MAXIMUM_FREQUENCY));
+ defs->optional(robotUnitConfigPath, robotUnitPrefix + "ConfigPath", "Specify a configuration file to group the sensor values specifically.");
+
+ descriptionSegment.defineProperties(defs);
+ proprioceptionSegment.defineProperties(defs);
+ localizationSegment.defineProperties(defs);
+ commonVisu.defineProperties(defs);
return defs;
}
@@ -66,10 +98,15 @@ namespace armarx
void RobotStateMemory::onInitComponent()
{
+
+ descriptionSegment.init();
+ proprioceptionSegment.init();
+ localizationSegment.init();
+ commonVisu.init();
+
robotUnitMemoryBatchSize = std::max((unsigned int) 1, robotUnitMemoryBatchSize);
robotUnitPollFrequency = std::clamp(robotUnitPollFrequency, 1, ROBOT_UNIT_MAXIMUM_FREQUENCY);
-
Ice::StringSeq includePaths;
auto packages = armarx::Application::GetProjectDependencies();
packages.push_back(Application::GetProjectName());
@@ -95,9 +132,16 @@ namespace armarx
void RobotStateMemory::onConnectComponent()
{
- setupRobotUnitSegment();
+ descriptionSegment.connect(getArvizClient(), getRobotUnit());
- setupLocalizationCoreSegment();
+ proprioceptionSegment.connect(getArvizClient(), getRobotUnit());
+ toIce(robotUnitProviderID, proprioceptionSegment.getRobotUnitProviderID());
+
+ localizationSegment.connect(getArvizClient());
+
+ commonVisu.connect(
+ getArvizClient()
+ );
cfg.loggingNames.emplace_back(robotUnitSensorPrefix);
handler = make_shared<RobotUnitDataStreamingReceiver>(this, getRobotUnit(), cfg);
@@ -124,45 +168,6 @@ namespace armarx
/*************************************************************/
// RobotUnit Streaming functions
/*************************************************************/
- void RobotStateMemory::setupLocalizationCoreSegment()
- {
- ARMARX_INFO << "Adding core segment " << localizationCoreSegmentName;
- workingMemory.addCoreSegments({localizationCoreSegmentName});
- }
-
- /*************************************************************/
- // RobotUnit Streaming functions
- /*************************************************************/
- void RobotStateMemory::setupRobotUnitSegment()
- {
- ARMARX_INFO << "Adding core segment " << proprioceptionCoreSegmentName;
- workingMemory.addCoreSegments({proprioceptionCoreSegmentName});
-
- ARMARX_INFO << "Adding provider segment " << proprioceptionCoreSegmentName << "/" << robotUnitProviderSegmentName;
- armem::data::AddSegmentInput input;
- input.coreSegmentName = proprioceptionCoreSegmentName;
- input.providerSegmentName = robotUnitProviderSegmentName;
-
- auto encoderEntryType = std::make_shared<aron::typenavigator::ObjectNavigator>("RobotUnitEncoderEntry");
- auto encoderNameType = std::make_shared<aron::typenavigator::StringNavigator>();
- auto encoderIterationIDType = std::make_shared<aron::typenavigator::LongNavigator>();
- encoderEntryType->addMemberType("EncoderGroupName", encoderNameType);
- encoderEntryType->addMemberType("IterationId", encoderIterationIDType);
- //auto encoderValueType = std::make_shared<aron::typenavigator::AnyType>();
- //encoderEntryType->addMemberType("value", encoderValueType);
-
- auto result = addSegments({input})[0];
-
- if (!result.success)
- {
- ARMARX_ERROR << "Could not add segment " << proprioceptionCoreSegmentName << "/" << robotUnitProviderSegmentName << ". The error message is: " << result.errorMessage;
- }
-
- robotUnitProviderID.memoryName = workingMemoryName;
- robotUnitProviderID.coreSegmentName = proprioceptionCoreSegmentName;
- robotUnitProviderID.providerSegmentName = robotUnitProviderSegmentName;
- }
-
void RobotStateMemory::convertRobotUnitStreamingDataToAron()
{
auto& data = handler->getDataBuffer();
@@ -176,8 +181,11 @@ namespace armarx
auto start = std::chrono::high_resolution_clock::now();
RobotUnitData convertedAndGroupedData;
- for (const auto& [name, dataEntry] : keys.entries)
+ for (const auto& [nameEntry, dataEntry] : keys.entries)
{
+ std::string name = nameEntry;
+ std::string jointOrWhateverName;
+
std::string groupName = "";
if (configSensorMapping.find(name) != configSensorMapping.end())
{
@@ -185,13 +193,18 @@ namespace armarx
}
else
{
- size_t dot_pos = name.find(".", name.find(".") + 1); // find second occurence of "."
- if (dot_pos == std::string::npos)
+ const size_t first_dot_pos = name.find(".");
+ const size_t second_dot_pos = name.find(".", first_dot_pos + 1); // find second occurence of "."
+ if (second_dot_pos == std::string::npos)
{
ARMARX_WARNING << "Could not find a groupname for the sensor with name " << name << ". All sensors must be called sens.X.Y where X is the name of the group and Y is the actual sensor. Ignoring this sensor.";
continue;
}
- groupName = name.substr(0, dot_pos);
+ groupName = name.substr(0, second_dot_pos);
+
+ jointOrWhateverName = name.substr(first_dot_pos + 1, second_dot_pos - first_dot_pos - 1);
+
+ name = name.substr(second_dot_pos + 1); // remove the groupName, TODO check if +1 is valid
}
RobotUnitData::RobotUnitDataGroup e;
@@ -205,6 +218,9 @@ namespace armarx
auto iterId = std::make_shared<aron::datanavigator::LongNavigator>(currentTimestep.iterationId);
dict->addElement("IterationId", iterId);
+ const auto nameId = std::make_shared<aron::datanavigator::StringNavigator>(jointOrWhateverName);
+ dict->addElement("name", nameId);
+
e.timestamp = currentTimestep.timestampUSec;
e.name = groupName;
e.data = dict;
@@ -294,7 +310,9 @@ namespace armarx
auto start = std::chrono::high_resolution_clock::now();
// send batch to memory
- armem::data::Commit c;
+ armem::data::Commit proprioceptionCommit;
+ armem::data::Commit odometryCommit;
+
for (unsigned int i = 0; i < robotUnitMemoryBatchSize; ++i)
{
std::lock_guard g{robotUnitDataMutex};
@@ -309,15 +327,55 @@ namespace armarx
const auto& timeUSec = encTimestep.timestamp;
const auto& aron = encTimestep.data;
- armem::data::EntityUpdate& update = c.updates.emplace_back();
+ armem::data::EntityUpdate& update = proprioceptionCommit.updates.emplace_back();
update.entityID = entityID;
update.timeCreatedMicroSeconds = timeUSec;
update.instancesData = {aron->toAronDictPtr()};
}
+
+ // odometry pose -> localization segment
+ auto it = convertedAndGroupedData.groups.find("sens.Platform");
+ if (it != convertedAndGroupedData.groups.end())
+ {
+ ARMARX_DEBUG << "Found odometry data.";
+
+ const RobotUnitData::RobotUnitDataGroup& timestep = it->second;
+ const float relPosX = aron::datanavigator::FloatNavigator::DynamicCast(timestep.data->getElement("relativePositionX"))->getValue();
+ const float relPosY = aron::datanavigator::FloatNavigator::DynamicCast(timestep.data->getElement("relativePositionY"))->getValue();
+ const float relOrientation = aron::datanavigator::FloatNavigator::DynamicCast(timestep.data->getElement("relativePositionRotation"))->getValue();
+
+ Eigen::Affine3f odometryPose = Eigen::Affine3f::Identity();
+ odometryPose.translation() << relPosX, relPosY, 0; // TODO set height
+ odometryPose.linear() = simox::math::rpy_to_mat3f(0.F, 0.F, relOrientation);
+
+ // const auto timestamp = armem::Time::microSeconds(it->second.timestamp);
+ const auto timestamp = armem::Time::now();
+
+ const ::armarx::armem::robot_state::Transform transform
+ {
+ .header =
+ {
+ .parentFrame = OdometryFrame,
+ .frame = "root", // TODO: robot root node
+ .agent = robotUnitProviderID.providerSegmentName,
+ .timestamp = timestamp
+ },
+ .transform = odometryPose
+ };
+
+ localizationSegment.storeTransform(transform);
+
+ }
+ else
+ {
+ ARMARX_INFO << deactivateSpam(1000) << "No odometry data! If you are using a mobile platform this should not have happened";
+ }
+
+
robotUnitDataQueue.pop();
}
- auto results = commit(c);
+ auto results = commit(proprioceptionCommit);
auto stop = std::chrono::high_resolution_clock::now();
ARMARX_DEBUG << "RobotUnitData: The total runtime of sending a batch to the memory is: " << std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
@@ -442,4 +500,5 @@ namespace armarx
robotUnitConversionTask->start();
robotUnitStoringTask->start();
}
+
}
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
index 490b04109a1608b6c9de61a56e44f6c92b2b9d38..e8174416e9b547f70af5e5c6b578242a0234743d 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
@@ -30,18 +30,25 @@
// Simox
#include <SimoxUtility/meta/enum/adapt_enum.h>
-// BaseClass
+// ArmarX
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+
+// BaseClass
#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h>
+#include "RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h"
+
#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
#include <RobotAPI/libraries/armem/server/ComponentPlugin.h>
-// ArmarX
-#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
-
+#include <RobotAPI/libraries/armem_robot_state/server/description/Segment.h>
+#include <RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h>
+#include <RobotAPI/libraries/armem_robot_state/server/localization/Segment.h>
+#include <RobotAPI/libraries/armem_robot_state/server/common/Visu.h>
-namespace armarx
+namespace armarx::armem::server::robot_state
{
+
/**
* @defgroup Component-RobotSensorMemory RobotSensorMemory
* @ingroup RobotAPI-Components
@@ -56,12 +63,14 @@ namespace armarx
class RobotStateMemory :
virtual public armarx::Component,
virtual public armem::server::ComponentPluginUser,
- virtual public RobotUnitComponentPluginUser
-
+ virtual public RobotUnitComponentPluginUser,
+ virtual public armarx::ArVizComponentPluginUser
{
public:
RobotStateMemory();
+ virtual ~RobotStateMemory();
+
/// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override;
@@ -83,32 +92,31 @@ namespace armarx
private:
// RobotUnit Streaming
- void setupRobotUnitSegment();
-
void convertRobotUnitStreamingDataToAron();
void storeConvertedRobotUnitDataInMemory();
void startRobotUnitStream();
void stopRobotUnitStream();
- // localization segment
- void setupLocalizationCoreSegment();
-
private:
std::string workingMemoryName = "RobotStateMemory";
bool addCoreSegmentOnUsage = false;
mutable std::recursive_mutex startStopMutex;
- // Memory IDs
+ // Core segments
+ // - description
+ description::Segment descriptionSegment;
// - proprioception
- std::string proprioceptionCoreSegmentName = "Proprioception";
- std::string robotUnitProviderSegmentName = "RobotUnit"; // get robot name?
+ proprioception::Segment proprioceptionSegment;
armem::data::MemoryID robotUnitProviderID;
// - localization
- std::string localizationCoreSegmentName = "Localization";
+ localization::Segment localizationSegment;
+
+ // Joint visu for all segments => robot pose and configuration
+ Visu commonVisu;
// RobotUnit stuff
RobotUnitDataStreaming::DataStreamingDescription keys;
@@ -131,6 +139,8 @@ namespace armarx
// params
static constexpr int ROBOT_UNIT_MAXIMUM_FREQUENCY = 100;
+ static constexpr const char* sensorValuePrefix = "RobotUnit.";
+
int robotUnitPollFrequency = 50;
std::string robotUnitSensorPrefix = "sens.*";
unsigned int robotUnitMemoryBatchSize = 50;
@@ -144,4 +154,5 @@ namespace armarx
armarx::PeriodicTask<RobotStateMemory>::pointer_type robotUnitConversionTask;
armarx::PeriodicTask<RobotStateMemory>::pointer_type robotUnitStoringTask;
};
-}
+
+} // namespace armarx::armem::server::robot_state
diff --git a/source/RobotAPI/interface/armem/query.ice b/source/RobotAPI/interface/armem/query.ice
index 3833b4444736bc1f78fdf9c34d1c5d6ea4acfc8c..bfe0764272dbee12270de46cfe7cf0b90f851bb0 100644
--- a/source/RobotAPI/interface/armem/query.ice
+++ b/source/RobotAPI/interface/armem/query.ice
@@ -49,6 +49,57 @@ module armarx
long first = 0; ///< First index to get.
long last = -1; ///< Last index to get (inclusive).
};
+
+
+ /**
+ * @brief Get snapshot(s) around timestamp.
+ *
+ * If there is a snapshot which exactly matches the query timestamp,
+ * the behavior is as for the @see Single query.
+ *
+ * However, if there is no matching timestamp, the snapshots before
+ * and after the query timestamp will be returned.
+ *
+ * If #eps is positive, the snapshots must be within the time
+ * range [timestamp - eps, timestamp + eps].
+ * Consequently, the query result can be empty.
+ *
+ * Hint:
+ * This query can be quite useful when interpolating snapshots
+ * is possible.
+ */
+ class TimeApprox extends EntityQuery
+ {
+ long timestamp = -1;
+ long eps = -1;
+ };
+
+ /**
+ * @brief Get the last snapshot before or at the timestamp.
+ *
+ * This query is kind of similar to latest() but with a reference timestamp.
+ *
+ */
+ class BeforeOrAtTime extends EntityQuery
+ {
+ long timestamp;
+ }
+
+ /**
+ * @brief Get the last snapshot before the timestamp or a sequence of
+ * n last elements before the timestamp depending on #maxEntries
+ *
+ * Depending on #maxEntries, the behavior is as follows:
+ * - #maxEntries == 1 => last element before timestamp
+ * - #maxEntries > 1 => n last elements before timestamp
+ * - #maxEntries < 0 => all elements before timestamp
+ */
+ class BeforeTime extends EntityQuery
+ {
+ long timestamp;
+ long maxEntries = 1;
+ }
+
}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
index ebcf8c9766a5ca1d8204a1aac7c0f317c2173371..8e86d96dc398d707ab4e8629e4b971519db8dc65 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
@@ -1,8 +1,12 @@
#include <VirtualRobot/XML/ObjectIO.h>
+#include <boost/algorithm/string.hpp>
+
#include <SimoxUtility/algorithm/string.h>
#include <SimoxUtility/filesystem/list_directory.h>
+#include <VirtualRobot/XML/RobotIO.h>
+
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
@@ -43,6 +47,9 @@ namespace armarx
else
{
ARMARX_VERBOSE << "Objects root directory: " << _rootDirAbs();
+
+ // make sure this data path is available => e.g. for findArticulatedObjects
+ armarx::ArmarXDataPath::addDataPaths(std::vector<std::string> {packageDataDir});
}
}
}
@@ -149,6 +156,28 @@ namespace armarx
return objects;
}
+ std::vector<armem::articulated_object::ArticulatedObjectDescription> ObjectFinder::findAllArticulatedObjects(bool checkPaths) const
+ {
+ init();
+ if (!_ready())
+ {
+ return {};
+ }
+
+ const bool local = true;
+
+ std::vector<armem::articulated_object::ArticulatedObjectDescription> objects;
+ for (const path& datasetDir : simox::fs::list_directory(_rootDirAbs(), local))
+ {
+ if (fs::is_directory(_rootDirAbs() / datasetDir))
+ {
+ const auto dataset = findAllArticulatedObjectsOfDataset(datasetDir, checkPaths);
+ objects.insert(objects.end(), dataset.begin(), dataset.end());
+ }
+ }
+ return objects;
+ }
+
std::map<std::string, std::vector<ObjectInfo>>
ObjectFinder::findAllObjectsByDataset(bool checkPaths) const
{
@@ -192,6 +221,95 @@ namespace armarx
return objects;
}
+ std::unordered_map<std::string, std::vector<armem::articulated_object::ArticulatedObjectDescription>> ObjectFinder::findAllArticulatedObjectsByDataset(bool checkPaths) const
+ {
+ init();
+ if (!_ready())
+ {
+ return {};
+ }
+
+ const bool local = true;
+
+ std::unordered_map<std::string, std::vector<armem::articulated_object::ArticulatedObjectDescription>> datasets;
+ for (const path& datasetDir : simox::fs::list_directory(_rootDirAbs(), local))
+ {
+ if (fs::is_directory(_rootDirAbs() / datasetDir))
+ {
+ const auto dataset = findAllArticulatedObjectsOfDataset(datasetDir, checkPaths);
+ datasets[datasetDir] = dataset;
+ }
+ }
+ return datasets;
+ }
+
+
+ std::vector<armem::articulated_object::ArticulatedObjectDescription> ObjectFinder::findAllArticulatedObjectsOfDataset(const std::string& dataset, bool checkPaths) const
+ {
+ init();
+ if (!_ready())
+ {
+ return {};
+ }
+ path datasetDir = _rootDirAbs() / dataset;
+ if (!fs::is_directory(datasetDir))
+ {
+ ARMARX_WARNING << "Expected dataset directory for dataset '" << dataset << "': \n"
+ << datasetDir;
+ return {};
+ }
+
+ const std::vector<std::string> validExtensions{".urdf", ".xml"};
+
+ const auto hasValidExtension = [&](const std::string & file) -> bool
+ {
+ return std::find(validExtensions.begin(), validExtensions.end(), boost::algorithm::to_lower_copy(std::filesystem::path(file).extension().string())) != validExtensions.end();
+ };
+
+ std::vector<armem::articulated_object::ArticulatedObjectDescription> objects;
+ const bool local = true;
+ for (const path& dir : simox::fs::list_directory(datasetDir, local))
+ {
+ if (not fs::is_directory(datasetDir / dir))
+ {
+ continue;
+ }
+
+ for (const auto& file : std::filesystem::directory_iterator(datasetDir / dir))
+ {
+ const std::string xml = std::filesystem::path(file).string();
+
+ if (hasValidExtension(xml))
+ {
+ try
+ {
+ const auto robot = VirtualRobot::RobotIO::loadRobot(xml, VirtualRobot::RobotIO::RobotDescription::eStructure);
+ if (robot != nullptr && robot->isPassive())
+ {
+ const std::string relativeXMLPath = armarx::ArmarXDataPath::getRelativeArmarXPath(xml);
+
+ objects.emplace_back(armem::articulated_object::ArticulatedObjectDescription
+ {
+ .name = robot->getName(),
+ .xml = {packageName, relativeXMLPath}
+ // .dataset = dataset
+ });
+ }
+ }
+ catch (const armarx::LocalException& ex)
+ {
+ ARMARX_WARNING << ex.what();
+ }
+ catch (...)
+ {
+
+ }
+ }
+ }
+ }
+ return objects;
+ }
+
VirtualRobot::ManipulationObjectPtr
ObjectFinder::loadManipulationObject(const std::optional<ObjectInfo>& ts)
{
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h
index 6e50a9d9cbeb1c82b7b928b2289f4b74799d70c7..2b342f3ac1151d0589bbc4b0212c3aed2fde798d 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h
@@ -1,6 +1,7 @@
#pragma once
#include <filesystem>
+#include <unordered_map>
#include <VirtualRobot/VirtualRobot.h>
@@ -9,6 +10,8 @@
#include "ObjectInfo.h"
#include "ObjectPose.h"
+#include <RobotAPI/libraries/armem_objects/types.h>
+
namespace armarx
{
/**
@@ -46,6 +49,10 @@ namespace armarx
std::map<std::string, std::vector<ObjectInfo>> findAllObjectsByDataset(bool checkPaths = true) const;
std::vector<ObjectInfo> findAllObjectsOfDataset(const std::string& dataset, bool checkPaths = true) const;
+ std::vector<armem::articulated_object::ArticulatedObjectDescription> findAllArticulatedObjects(bool checkPaths) const;
+ std::vector<armem::articulated_object::ArticulatedObjectDescription> findAllArticulatedObjectsOfDataset(const std::string& dataset, bool checkPaths) const;
+ std::unordered_map<std::string, std::vector<armem::articulated_object::ArticulatedObjectDescription>> findAllArticulatedObjectsByDataset(bool checkPaths = true) const;
+
VirtualRobot::ManipulationObjectPtr
static loadManipulationObject(const std::optional<ObjectInfo>& ts);
VirtualRobot::ManipulationObjectPtr
diff --git a/source/RobotAPI/libraries/CMakeLists.txt b/source/RobotAPI/libraries/CMakeLists.txt
index 1609bb72783e787546c9ef7608d99d4d3374197f..5df6b028f0272c51a0538392375c27421d19dc06 100644
--- a/source/RobotAPI/libraries/CMakeLists.txt
+++ b/source/RobotAPI/libraries/CMakeLists.txt
@@ -20,7 +20,8 @@ add_subdirectory(natik)
add_subdirectory(armem)
add_subdirectory(armem_gui)
add_subdirectory(armem_objects)
-add_subdirectory(armem_robot_localization)
+add_subdirectory(armem_robot)
+add_subdirectory(armem_robot_state)
add_subdirectory(armem_robot_mapping)
add_subdirectory(armem_skills)
add_subdirectory(aron)
diff --git a/source/RobotAPI/libraries/armem/CMakeLists.txt b/source/RobotAPI/libraries/armem/CMakeLists.txt
index 03f509ce8d6cd2ddcce5c2800e14a8cafb25dfa1..eb83670b3cf4633d3fe2ce6bf52e61256a8d7931 100644
--- a/source/RobotAPI/libraries/armem/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem/CMakeLists.txt
@@ -65,7 +65,6 @@ set(LIB_FILES
client/ReaderComponentPlugin.cpp
client/Writer.cpp
client/WriterComponentPlugin.cpp
- client/MemoryConnector.cpp
client/Query.cpp
client/query/Builder.cpp
@@ -163,7 +162,6 @@ set(LIB_HEADERS
client/ReaderComponentPlugin.h
client/Writer.h
client/WriterComponentPlugin.h
- client/MemoryConnector.h
client/Query.h
client/query/Builder.h
@@ -217,7 +215,7 @@ armarx_enable_aron_file_generation_for_target(
)
-add_library(RobotAPI::libraries::armem ALIAS "${LIB_NAME}")
+add_library(RobotAPI::armem ALIAS "${LIB_NAME}")
# add unit tests
diff --git a/source/RobotAPI/libraries/armem/client/ComponentPlugin.h b/source/RobotAPI/libraries/armem/client/ComponentPlugin.h
index e35f9e08d2f6f13eaab6fbbeee010179e02dbf45..7fc432536b62411a912d7829bc40fd82efa5a855 100644
--- a/source/RobotAPI/libraries/armem/client/ComponentPlugin.h
+++ b/source/RobotAPI/libraries/armem/client/ComponentPlugin.h
@@ -27,9 +27,6 @@ namespace armarx::armem::client
ComponentPluginUser();
~ComponentPluginUser() override;
- protected:
-
- void setMemory(server::MemoryInterfacePrx memory);
/**
* @brief Resolve the given memory name, add it as dependency and update the readers and writers.
@@ -40,6 +37,10 @@ namespace armarx::armem::client
virtual armem::data::WaitForMemoryResult useMemory(const std::string& memoryName) override;
using mns::plugins::ClientPluginUserBase::useMemory;
+ protected:
+
+ void setMemory(server::MemoryInterfacePrx memory);
+
};
}
diff --git a/source/RobotAPI/libraries/armem/client/MemoryConnector.cpp b/source/RobotAPI/libraries/armem/client/MemoryConnector.cpp
deleted file mode 100644
index 4fadf6f65ebb0ade9b91d49d36860bf6e9b0ff64..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem/client/MemoryConnector.cpp
+++ /dev/null
@@ -1,53 +0,0 @@
-#include "MemoryConnector.h"
-
-#include <ArmarXCore/core/ManagedIceObject.h>
-
-#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-namespace armarx::armem
-{
-
- MemoryConnector::MemoryConnector(ManagedIceObject& component) : component(component) {}
-
- MemoryConnector::~MemoryConnector() = default;
-
- void MemoryConnector::registerPropertyDefinitions(PropertyDefinitionsPtr& def)
- {
- const std::string prefix = getPropertyPrefix();
-
- ARMARX_INFO << "Memory connector: registerPropertyDefinitions with prefix " << prefix;
-
-
- def->component(memoryNameSystem, "ArMemMemoryNameSystem", prefix + "ArMemMemoryNameSystem");
- }
-
- armem::data::WaitForMemoryResult MemoryConnector::useMemory(const std::string& memoryName)
- {
- armem::data::WaitForMemoryInput input;
- input.name = memoryName;
-
- ARMARX_CHECK_NOT_NULL(memoryNameSystem);
-
- ARMARX_INFO << "Waiting for memory ...";
- armem::data::WaitForMemoryResult result = memoryNameSystem->waitForMemory(input);
-
- if (result.success)
- {
- // Add dependency.
- component.usingProxy(result.proxy->ice_getIdentity().name);
- }
- else
- {
- ARMARX_WARNING << "Use memory: Failure";
- }
- return result;
- }
-
- const std::string& MemoryConnector::getPropertyPrefix() const
- {
- return propertyPrefix;
- }
-
-
-} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.h b/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.h
index 44f276ac95cc79d867287be68ebdb1b734056849..8c7858bfc6e5c14ca5f756512d000928f14b8502 100644
--- a/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.h
+++ b/source/RobotAPI/libraries/armem/client/ReaderComponentPlugin.h
@@ -50,16 +50,15 @@ namespace armarx::armem::client
ReaderComponentPluginUser();
~ReaderComponentPluginUser() override;
+ virtual armem::data::WaitForMemoryResult useMemory(const std::string& memoryName) override;
+ using mns::plugins::ClientPluginUserBase::useMemory;
virtual void memoryUpdated(const std::vector<data::MemoryID>& updatedSnapshotIDs, const Ice::Current& current) override;
-
protected:
void setReadingMemory(server::ReadingMemoryInterfacePrx memory);
- virtual armem::data::WaitForMemoryResult useMemory(const std::string& memoryName) override;
- using mns::plugins::ClientPluginUserBase::useMemory;
protected:
diff --git a/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.h b/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.h
index 6df32feea8e85f4b2edf2eb43c3ba131a3238f78..0f5f21f808fb646fe8af27192d04c82a5739426e 100644
--- a/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.h
+++ b/source/RobotAPI/libraries/armem/client/WriterComponentPlugin.h
@@ -40,16 +40,13 @@ namespace armarx::armem::client
WriterComponentPluginUser();
~WriterComponentPluginUser() override;
- protected:
-
- void setWritingMemory(server::WritingMemoryInterfacePrx memory);
-
virtual armem::data::WaitForMemoryResult useMemory(const std::string& memoryName) override;
using mns::plugins::ClientPluginUserBase::useMemory;
-
protected:
+ void setWritingMemory(server::WritingMemoryInterfacePrx memory);
+
Writer memoryWriter;
private:
diff --git a/source/RobotAPI/libraries/armem/client/query/query_fns.h b/source/RobotAPI/libraries/armem/client/query/query_fns.h
index 8dcb88991450b95583c7228b2349fdfdf05cba95..35203a014fa423cec4f8ef0f4d09759bc54385a9 100644
--- a/source/RobotAPI/libraries/armem/client/query/query_fns.h
+++ b/source/RobotAPI/libraries/armem/client/query/query_fns.h
@@ -134,4 +134,34 @@ namespace armarx::armem::client::query_fns
};
}
-}
+ inline
+ std::function<void(query::SnapshotSelector&)>
+ atTimeApprox(Time time, Duration eps)
+ {
+ return [ = ](query::SnapshotSelector & selector)
+ {
+ selector.atTimeApprox(time, eps);
+ };
+ }
+
+ inline
+ std::function<void(query::SnapshotSelector&)>
+ beforeOrAtTime(Time time)
+ {
+ return [ = ](query::SnapshotSelector & selector)
+ {
+ selector.beforeOrAtTime(time);
+ };
+ }
+
+ inline
+ std::function<void(query::SnapshotSelector&)>
+ beforeTime(Time time, long nElements = 1)
+ {
+ return [ = ](query::SnapshotSelector & selector)
+ {
+ selector.beforeTime(time, nElements);
+ };
+ }
+
+} // namespace armarx::armem::client::query_fns
diff --git a/source/RobotAPI/libraries/armem/client/query/selectors.cpp b/source/RobotAPI/libraries/armem/client/query/selectors.cpp
index f2bae712818b566ef51444aafb84af6031d338d0..cc3a73829ab7c79eabc96c30b55c4ca350ac33ad 100644
--- a/source/RobotAPI/libraries/armem/client/query/selectors.cpp
+++ b/source/RobotAPI/libraries/armem/client/query/selectors.cpp
@@ -1,4 +1,5 @@
#include "selectors.h"
+#include "RobotAPI/libraries/armem/core/ice_conversions.h"
#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
@@ -42,6 +43,15 @@ namespace armarx::armem::client::query
return *this;
}
+ SnapshotSelector& SnapshotSelector::atTimeApprox(Time timestamp, Duration eps)
+ {
+ auto& q = _addQuery<dq::entity::TimeApprox>();
+ toIce(q.timestamp, timestamp);
+ toIce(q.eps, eps);
+ return *this;
+ }
+
+
SnapshotSelector& SnapshotSelector::indexRange(long first, long last)
{
auto& q = _addQuery<dq::entity::IndexRange>();
@@ -51,6 +61,22 @@ namespace armarx::armem::client::query
}
+ SnapshotSelector& SnapshotSelector::beforeOrAtTime(Time timestamp)
+ {
+ auto& q = _addQuery<dq::entity::BeforeOrAtTime>();
+ toIce(q.timestamp, timestamp);
+ return *this;
+ }
+
+
+ SnapshotSelector& SnapshotSelector::beforeTime(Time timestamp, long maxEntries)
+ {
+ auto& q = _addQuery<dq::entity::BeforeTime>();
+ toIce(q.timestamp, timestamp);
+ toIce(q.maxEntries, maxEntries);
+ return *this;
+ }
+
SnapshotSelector& EntitySelector::snapshots()
{
diff --git a/source/RobotAPI/libraries/armem/client/query/selectors.h b/source/RobotAPI/libraries/armem/client/query/selectors.h
index 8bc2850b390b394a5d2b5fc07753bb846f598416..df642ded9a70ea2688eaca5865f730e8e99ee6de 100644
--- a/source/RobotAPI/libraries/armem/client/query/selectors.h
+++ b/source/RobotAPI/libraries/armem/client/query/selectors.h
@@ -26,6 +26,10 @@ namespace armarx::armem::client::query
SnapshotSelector& latest();
SnapshotSelector& atTime(Time timestamp);
+ SnapshotSelector& atTimeApprox(Time timestamp, Duration eps);
+
+ SnapshotSelector& beforeTime(Time timestamp, long maxEntries = 1);
+ SnapshotSelector& beforeOrAtTime(Time timestamp);
SnapshotSelector& timeRange(Time min, Time max);
SnapshotSelector& indexRange(long first, long last);
diff --git a/source/RobotAPI/libraries/armem/core/Time.h b/source/RobotAPI/libraries/armem/core/Time.h
index 328d1f59d793cbcc9a7d0febc0951775d200013a..f76f7657dda6e14a9c35bb4ad5edf58471680144 100644
--- a/source/RobotAPI/libraries/armem/core/Time.h
+++ b/source/RobotAPI/libraries/armem/core/Time.h
@@ -7,8 +7,8 @@
namespace armarx::armem
{
-
using Time = IceUtil::Time;
+ using Duration = IceUtil::Time;
/**
* @brief Returns `time` as e.g. "123456789.012 ms".
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h
index 447bd259a2c24a3ca80c91c59a518daed56c18f1..2a4308ebdc837255a9f6a816dc62880dc09e8c21 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h
@@ -1,15 +1,20 @@
#pragma once
-#include <RobotAPI/interface/armem/query.h>
+#include <cstdint>
+#include <iterator>
-#include "BaseQueryProcessorBase.h"
+#include <RobotAPI/interface/armem/query.h>
+#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/ice_conversions.h>
+#include "BaseQueryProcessorBase.h"
+#include "RobotAPI/libraries/armem/core/Time.h"
+
namespace armarx::armem::base::query_proc
{
@@ -48,6 +53,18 @@ namespace armarx::armem::base::query_proc
{
process(result, *q, entity);
}
+ else if (auto q = dynamic_cast<const armem::query::data::entity::TimeApprox*>(&query))
+ {
+ process(result, *q, entity);
+ }
+ else if (auto q = dynamic_cast<const armem::query::data::entity::BeforeOrAtTime*>(&query))
+ {
+ process(result, *q, entity);
+ }
+ else if (auto q = dynamic_cast<const armem::query::data::entity::BeforeTime*>(&query))
+ {
+ process(result, *q, entity);
+ }
else
{
throw armem::error::UnknownQueryType("entity snapshot", query);
@@ -153,6 +170,197 @@ namespace armarx::armem::base::query_proc
}
+ inline auto lastElementBeforeOrAt(const auto& history, const auto timestamp) const
+ {
+ // first element equal or greater
+ typename std::map<Time, EntitySnapshotT>::const_iterator refItFwd = history.upper_bound(timestamp);
+
+ // last element less than
+ typename std::map<Time, EntitySnapshotT>::const_iterator refItFwdLt = std::prev(refItFwd);
+
+ // last element not greater than => if this is invalid, we have no suitable elements
+ typename std::map<Time, EntitySnapshotT>::const_reverse_iterator refIt(refItFwd);
+ if (refIt == history.rend())
+ {
+ return history.end();
+ }
+
+ // now either refItFwd is a perfect match ...
+ if (refItFwd->first == timestamp)
+ {
+ return refItFwd;
+ }
+
+ // ... or we return the element before if possible
+ if (refItFwdLt != history.begin())
+ {
+ return refItFwdLt;
+ }
+
+ return history.end();
+ }
+
+ inline auto lastElementBefore(const auto& history, const auto& timestamp) const
+ {
+ // first element equal or greater
+ typename std::map<Time, EntitySnapshotT>::const_iterator refItFwd = history.upper_bound(timestamp);
+
+ // last element less than
+ typename std::map<Time, EntitySnapshotT>::const_iterator refItFwdLt = std::prev(refItFwd);
+
+ // last element not greater than => if this is invalid, we have no suitable elements
+ typename std::map<Time, EntitySnapshotT>::const_reverse_iterator refIt(refItFwd);
+ if (refIt == history.rend())
+ {
+ return history.end();
+ }
+
+ // we return the element before if possible
+ if (refItFwdLt != history.begin())
+ {
+ return refItFwdLt;
+ }
+
+ return history.end();
+ }
+
+
+ void process(_EntityT& result,
+ const armem::query::data::entity::BeforeOrAtTime& query,
+ const _EntityT& entity) const
+ {
+ const auto referenceTimestamp = fromIce<Time>(query.timestamp);
+ ARMARX_CHECK(referenceTimestamp.toMicroSeconds() >= 0) << "Reference timestamp is negative!";
+
+ const auto beforeOrAt = lastElementBeforeOrAt(entity.history(), referenceTimestamp);
+
+ if (beforeOrAt != entity.history().end())
+ {
+ addResultSnapshot(result, beforeOrAt);
+ }
+ }
+
+
+ void process(_EntityT& result,
+ const armem::query::data::entity::BeforeTime& query,
+ const _EntityT& entity) const
+ {
+ const auto referenceTimestamp = fromIce<Time>(query.timestamp);
+ ARMARX_CHECK(referenceTimestamp.toMicroSeconds() >= 0) << "Reference timestamp is negative!";
+
+ const auto maxEntries = fromIce<std::int64_t>(query.maxEntries);
+
+ const auto itBefore = lastElementBefore(entity.history(), referenceTimestamp);
+ if (itBefore == entity.history().end())
+ {
+ ARMARX_WARNING << "No suitable element found";
+ return;
+ }
+
+ const auto nEntriesBefore = std::distance(entity.history().begin(), itBefore);
+
+ const int nEntries = [&]()
+ {
+ // see query.ice
+ if (maxEntries > 0)
+ {
+ return std::min(nEntriesBefore, maxEntries);
+ }
+
+ return nEntriesBefore; // all elements before timestamp
+ }
+ ();
+
+ auto it = itBefore;
+ for (std::int64_t i = 0; i < nEntries; i++, --it)
+ {
+ addResultSnapshot(result, it);
+ }
+ }
+
+ void process(_EntityT& result,
+ const armem::query::data::entity::TimeApprox& query,
+ const _EntityT& entity) const
+ {
+
+ const auto referenceTimestamp = fromIce<Time>(query.timestamp);
+ ARMARX_CHECK(referenceTimestamp.toMicroSeconds() >= 0) << "Reference timestamp is negative!";
+
+ const auto referenceTimestampMicroSeconds = referenceTimestamp.toMicroSeconds();
+ const auto epsDuration = fromIce<Time>(query.eps).toMicroSeconds();
+
+ // elements have to be in range [t_ref - eps, t_ref + eps] if eps is positive
+ const auto isInRange = [&](const Time & t) -> bool
+ {
+ if (epsDuration <= 0)
+ {
+ return true;
+ }
+
+ return std::abs(t.toMicroSeconds() - referenceTimestampMicroSeconds) <= epsDuration;
+ };
+
+ // first element before or at timestamp
+ const auto beforeOrAt = lastElementBeforeOrAt(entity.history(), referenceTimestamp);
+ const bool isBeforeOrAtValid = beforeOrAt != entity.history().end();
+ const auto isPerfectMatch = isBeforeOrAtValid && beforeOrAt->first == referenceTimestamp;
+
+ // first element greater
+ const auto after = entity.history().upper_bound(referenceTimestamp);
+ const bool isAfterValid = after != entity.history().end();
+
+ // if both are invalid, there is nothing to be gained here.
+ if ((not isBeforeOrAtValid) and (not isAfterValid))
+ {
+ const armem::Time dt = referenceTimestamp - entity.getLatestTimestamp();
+
+ ARMARX_WARNING << "Lookup " << dt.toMilliSeconds() << " ms into the future.";
+ return;
+ }
+ // -> now one or both are valid ...
+
+ // is 'before' a perfect match?
+ if(isPerfectMatch)
+ {
+ addResultSnapshot(result, beforeOrAt);
+ return;
+ }
+
+ // only 'before' valid?
+ if (not isAfterValid)
+ {
+ if (isInRange(beforeOrAt->first))
+ {
+ addResultSnapshot(result, beforeOrAt);
+ }
+ return;
+ }
+
+ // only 'after' valid?
+ if (not isBeforeOrAtValid)
+ {
+ if (isInRange(after->first))
+ {
+ addResultSnapshot(result, after);
+ }
+ return;
+ }
+ // -> now both are valid
+
+ // return both (if in range) => user can interpolate
+ if (isInRange(beforeOrAt->first))
+ {
+ addResultSnapshot(result, beforeOrAt);
+ }
+
+ if (isInRange(after->first))
+ {
+ addResultSnapshot(result, after);
+ }
+
+ }
+
+
static size_t negativeIndexSemantics(long index, size_t size)
{
const size_t max = size > 0 ? size - 1 : 0;
diff --git a/source/RobotAPI/libraries/armem/test/ArMemQueryTest.cpp b/source/RobotAPI/libraries/armem/test/ArMemQueryTest.cpp
index 42d357b005ae9c4c4663bc72fa91ed4fc459b78a..789600c2fe82857b865c845d6892fc2da44d3d6a 100644
--- a/source/RobotAPI/libraries/armem/test/ArMemQueryTest.cpp
+++ b/source/RobotAPI/libraries/armem/test/ArMemQueryTest.cpp
@@ -20,6 +20,9 @@
* GNU General Public License
*/
+#include "ArmarXCore/core/exceptions/LocalException.h"
+#include <RobotAPI/interface/armem/query.h>
+#include <boost/test/tools/old/interface.hpp>
#define BOOST_TEST_MODULE RobotAPI::ArmarXLibraries::armem
#define ARMARX_BOOST_TEST
@@ -294,6 +297,308 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_to_end)
}
+/* BeforeTime */
+
+BOOST_AUTO_TEST_CASE(test_entity_BeforeTime_1)
+{
+ BOOST_REQUIRE_EQUAL(entity.size(), 5);
+ addResults(query::entity::BeforeTime{ 3500, 1 });
+ BOOST_REQUIRE_EQUAL(results.size(), 2);
+
+ for (const auto& result : results)
+ {
+ std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ BOOST_REQUIRE_EQUAL(times.size(), 1);
+
+ BOOST_REQUIRE_EQUAL(times.front(), armem::Time::microSeconds(3000));
+ }
+
+}
+
+BOOST_AUTO_TEST_CASE(test_entity_BeforeTime_2)
+{
+ BOOST_REQUIRE_EQUAL(entity.size(), 5);
+ addResults(query::entity::BeforeTime{ 3500, 2});
+ BOOST_REQUIRE_EQUAL(results.size(), 2);
+
+ for (const auto& result : results)
+ {
+ std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ BOOST_REQUIRE_EQUAL(times.size(), 2);
+
+ std::vector<armem::Time> expected
+ {
+ armem::Time::microSeconds(2000), armem::Time::microSeconds(3000)
+ };
+
+ BOOST_CHECK_EQUAL_COLLECTIONS(times.begin(), times.end(), expected.begin(), expected.end());
+ }
+
+}
+
+
+
+/* BeforeOrAtTime */
+
+BOOST_AUTO_TEST_CASE(test_entity_BeforeOrAtTime_before)
+{
+ BOOST_REQUIRE_EQUAL(entity.size(), 5);
+ addResults(query::entity::BeforeOrAtTime{ 3500 });
+ BOOST_REQUIRE_EQUAL(results.size(), 2);
+
+ for (const auto& result : results)
+ {
+ std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ BOOST_REQUIRE_EQUAL(times.size(), 1);
+
+ BOOST_REQUIRE_EQUAL(times.front(), armem::Time::microSeconds(3000));
+ }
+}
+
+BOOST_AUTO_TEST_CASE(test_entity_BeforeOrAtTime_at)
+{
+ BOOST_REQUIRE_EQUAL(entity.size(), 5);
+ addResults(query::entity::BeforeOrAtTime{ 3000 });
+ BOOST_REQUIRE_EQUAL(results.size(), 2);
+
+ for (const auto& result : results)
+ {
+ std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ BOOST_REQUIRE_EQUAL(times.size(), 1);
+
+ BOOST_REQUIRE_EQUAL(times.front(), armem::Time::microSeconds(3000));
+ }
+}
+
+BOOST_AUTO_TEST_CASE(test_entity_BeforeOrAtTime_lookup_past)
+{
+ BOOST_REQUIRE_EQUAL(entity.size(), 5);
+ addResults(query::entity::BeforeOrAtTime{ 1 });
+ BOOST_REQUIRE_EQUAL(results.size(), 2);
+
+ for (const auto& result : results)
+ {
+ std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ BOOST_REQUIRE(times.empty());
+ }
+}
+
+/* TimeApprox */
+
+/**
+ * @brief Lookup between elements. No range specified.
+ *
+ * Desired behavior: Return elements before and after timestamp.
+ */
+BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_no_limit)
+{
+ BOOST_REQUIRE_EQUAL(entity.size(), 5);
+ addResults(query::entity::TimeApprox{ 3500, -1});
+ BOOST_REQUIRE_EQUAL(results.size(), 2);
+
+ for (const auto& result : results)
+ {
+ std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ BOOST_REQUIRE_EQUAL(times.size(), 2);
+
+ std::vector<armem::Time> expected
+ {
+ armem::Time::microSeconds(3000), armem::Time::microSeconds(4000)
+ };
+
+ BOOST_CHECK_EQUAL_COLLECTIONS(times.begin(), times.end(), expected.begin(), expected.end());
+ }
+
+}
+
+/**
+ * @brief Lookup between elements. Range is OK.
+ *
+ * Desired behavior: Return elements before and after timestamp.
+ */
+BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_600)
+{
+ BOOST_REQUIRE_EQUAL(entity.size(), 5);
+ addResults(query::entity::TimeApprox{ 3500, 600});
+ BOOST_REQUIRE_EQUAL(results.size(), 2);
+
+ for (const auto& result : results)
+ {
+ std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ BOOST_REQUIRE_EQUAL(times.size(), 2);
+
+ std::vector<armem::Time> expected
+ {
+ armem::Time::microSeconds(3000), armem::Time::microSeconds(4000)
+ };
+
+ BOOST_CHECK_EQUAL_COLLECTIONS(times.begin(), times.end(), expected.begin(), expected.end());
+ }
+
+}
+
+/**
+ * @brief Lookup between elements. Range is too small.
+ *
+ * Desired behavior: Return empty list.
+ */
+BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_too_small)
+{
+ BOOST_REQUIRE_EQUAL(entity.size(), 5);
+ addResults(query::entity::TimeApprox{ 3500, 100});
+ BOOST_REQUIRE_EQUAL(results.size(), 2);
+
+ for (const auto& result : results)
+ {
+ std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ BOOST_REQUIRE_EQUAL(times.size(), 0);
+ }
+
+}
+
+/**
+ * @brief Lookup between elements. Only next element is in range.
+ *
+ * Desired behavior: Return only element after query timestamp.
+ */
+BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_only_next)
+{
+ BOOST_REQUIRE_EQUAL(entity.size(), 5);
+ addResults(query::entity::TimeApprox{ 3700, 400});
+ BOOST_REQUIRE_EQUAL(results.size(), 2);
+
+ for (const auto& result : results)
+ {
+ std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ BOOST_REQUIRE_EQUAL(times.size(), 1);
+
+ std::vector<armem::Time> expected
+ {
+ armem::Time::microSeconds(4000)
+ };
+
+ BOOST_CHECK_EQUAL_COLLECTIONS(times.begin(), times.end(), expected.begin(), expected.end());
+ }
+}
+
+/**
+ * @brief Lookup between elements. Only previous element is in range.
+ *
+ * Desired behavior: Return only element before query timestamp.
+ */
+BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_only_previous)
+{
+ BOOST_REQUIRE_EQUAL(entity.size(), 5);
+ addResults(query::entity::TimeApprox{ 3300, 400});
+ BOOST_REQUIRE_EQUAL(results.size(), 2);
+
+ for (const auto& result : results)
+ {
+ std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ BOOST_REQUIRE_EQUAL(times.size(), 1);
+
+ std::vector<armem::Time> expected
+ {
+ armem::Time::microSeconds(3000)
+ };
+
+ BOOST_CHECK_EQUAL_COLLECTIONS(times.begin(), times.end(), expected.begin(), expected.end());
+ }
+}
+
+/**
+ * @brief Lookup with perfect match.
+ *
+ * Desired behavior: Return only element matching timestamp exactly.
+ */
+BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_perfect_match)
+{
+ BOOST_REQUIRE_EQUAL(entity.size(), 5);
+ addResults(query::entity::TimeApprox{ 3000, -1});
+ BOOST_REQUIRE_EQUAL(results.size(), 2);
+
+ for (const auto& result : results)
+ {
+ std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ BOOST_REQUIRE_EQUAL(times.size(), 1);
+
+ std::vector<armem::Time> expected
+ {
+ armem::Time::microSeconds(3000)
+ };
+
+ BOOST_CHECK_EQUAL_COLLECTIONS(times.begin(), times.end(), expected.begin(), expected.end());
+ }
+}
+
+/**
+ * @brief Invalid lookup into the past.
+ *
+ * Desired behavior: Return empty list.
+ */
+BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_lookup_past)
+{
+ BOOST_REQUIRE_EQUAL(entity.size(), 5);
+ addResults(query::entity::TimeApprox{ 1, 1});
+ BOOST_REQUIRE_EQUAL(results.size(), 2);
+
+ for (const auto& result : results)
+ {
+ std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ BOOST_REQUIRE(times.empty());
+ }
+}
+
+/**
+ * @brief Invalid lookup into the future.
+ *
+ * Desired behavior: Return empty list.
+ */
+BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_lookup_future)
+{
+ BOOST_REQUIRE_EQUAL(entity.size(), 5);
+ addResults(query::entity::TimeApprox{ 10'000, 1});
+ BOOST_REQUIRE_EQUAL(results.size(), 2);
+
+ for (const auto& result : results)
+ {
+ std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ BOOST_REQUIRE(times.empty());
+ }
+}
+
+/**
+ * @brief Lookup into the future, but still considered valid as time range is not set.
+ *
+ * Desired behavior: Return most recent element in history.
+ */
+BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_lookup_future_valid)
+{
+ BOOST_REQUIRE_EQUAL(entity.size(), 5);
+ addResults(query::entity::TimeApprox{ 10'000, -1});
+ BOOST_REQUIRE_EQUAL(results.size(), 2);
+
+ for (const auto& result : results)
+ {
+ std::vector<armem::Time> times = simox::alg::get_keys(result.history());
+ BOOST_REQUIRE_EQUAL(times.size(), 1);
+
+ std::vector<armem::Time> expected
+ {
+ armem::Time::microSeconds(5'000)
+ };
+
+ BOOST_CHECK_EQUAL_COLLECTIONS(times.begin(), times.end(), expected.begin(), expected.end());
+ }
+}
+
+BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_lookup_invalid_timestamp)
+{
+ BOOST_REQUIRE_THROW(addResults(query::entity::TimeApprox{ -1, 1}), ::armarx::LocalException);
+}
+
+
+
BOOST_AUTO_TEST_CASE(test_negative_index_semantics)
{
BOOST_CHECK_EQUAL(EntityQueryProcessor::negativeIndexSemantics(0, 0), 0);
diff --git a/source/RobotAPI/libraries/armem/util/util.h b/source/RobotAPI/libraries/armem/util/util.h
index 91a31651cf055572dd6c3e8fcbb27163e4d30918..44a32b8317022bbcee867413de19b6f6688a853b 100644
--- a/source/RobotAPI/libraries/armem/util/util.h
+++ b/source/RobotAPI/libraries/armem/util/util.h
@@ -13,7 +13,6 @@
* 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 RobotComponents::ArmarXObjects::
* @author Fabian Reister ( fabian dot reister at kit dot edu )
* @date 2021
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
@@ -134,7 +133,7 @@ namespace armarx::armem
ARMARX_WARNING << "Empty history for " << s;
}
- ARMARX_INFO << "History size: " << entity.history().size();
+ ARMARX_DEBUG << "History size: " << entity.history().size();
for (const auto &[ss, entitySnapshot] : entity.history())
{
diff --git a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
index b491f16ce361bb4c5ca46b1f46104b6cb302711e..ed02a9adf315ef755af2c8da64d4a37c3d4817b6 100644
--- a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
@@ -14,7 +14,8 @@ armarx_add_library(
RobotAPI::ArViz
RobotAPI::ComponentPlugins
RobotAPI::Core
- RobotAPI::libraries::armem
+ RobotAPI::armem
+ RobotAPI::armem_robot
HEADERS
aron_conversions.h
aron_forward_declarations.h
@@ -36,9 +37,6 @@ armarx_add_library(
client/articulated_object/Reader.cpp
client/articulated_object/Writer.cpp
- articulated_object_conversions.cpp
-
-
SOURCES
aron_conversions.cpp
@@ -61,8 +59,6 @@ armarx_add_library(
client/articulated_object/Writer.h
client/articulated_object/interfaces.h
- articulated_object_conversions.h
-
)
@@ -75,9 +71,6 @@ armarx_enable_aron_file_generation_for_target(
aron/ObjectInstance.xml
# aron/Attachment.xml
- aron/RobotDescription.xml
- aron/RobotState.xml
- aron/Robot.xml
aron/Constraint.xml
)
diff --git a/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp b/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp
index f7db3dc7f5a34bcdacad33cf85ec3c2b2aa29b19..967060360c3bff360150f1c8988b5d40db4dbd5c 100644
--- a/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp
@@ -30,6 +30,7 @@ namespace armarx::armem
objpose::toAron(dto.pose, bo);
}
+} // namespace armarx::armem
armarx::armem::MemoryID
armarx::armem::obj::makeObjectInstanceMemoryID(const objpose::ObjectPose& objectPose)
@@ -39,60 +40,3 @@ armarx::armem::obj::makeObjectInstanceMemoryID(const objpose::ObjectPose& object
.withEntityName(objectPose.objectID.str())
.withTimestamp(objectPose.timestamp);
}
- /* to be moved */
- void fromAron(const long& dto, IceUtil::Time& time)
- {
- time = IceUtil::Time::microSeconds(dto);
- }
-
- void toAron(long& dto, const IceUtil::Time& time)
- {
- dto = time.toMicroSeconds();
- }
-
-
- /* Robot */
-
- void fromAron(const arondto::Robot& dto, Robot& bo)
- {
- // fromAron(dto.description, bo.description);
- fromAron(dto.state, bo.config);
- }
-
- void toAron(arondto::Robot& dto, const Robot& bo)
- {
- // toAron(dto.description, bo.description);
- toAron(dto.state, bo.config);
- }
-
- /* RobotDescription */
-
- void fromAron(const arondto::RobotDescription& dto, RobotDescription& bo)
- {
- aron::fromAron(dto.name, bo.name);
- fromAron(dto.xml, bo.xml);
- }
-
- void toAron(arondto::RobotDescription& dto, const RobotDescription& bo)
- {
- aron::toAron(dto.name, bo.name);
- toAron(dto.xml, bo.xml);
- }
-
- /* RobotState */
-
- void fromAron(const arondto::RobotState& dto, RobotState& bo)
- {
- fromAron(dto.timestamp, bo.timestamp);
- bo.globalPose.matrix() = dto.globalPose;
- bo.jointMap = dto.jointMap;
- }
-
- void toAron(arondto::RobotState& dto, const RobotState& bo)
- {
- toAron(dto.timestamp, bo.timestamp);
- dto.globalPose = bo.globalPose.matrix();
- dto.jointMap = bo.jointMap;
- }
-
-} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_objects/aron_conversions.h b/source/RobotAPI/libraries/armem_objects/aron_conversions.h
index 3f17d584747a2aa60f14f42b032dd5740e07bb99..1d74e95317521e27467753e46320914192558908 100644
--- a/source/RobotAPI/libraries/armem_objects/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_objects/aron_conversions.h
@@ -3,14 +3,8 @@
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
-
-
#include <RobotAPI/libraries/armem_objects/types.h>
-#include <RobotAPI/libraries/armem_objects/aron/RobotDescription.aron.generated.h>
-#include <RobotAPI/libraries/armem_objects/aron/RobotState.aron.generated.h>
-#include <RobotAPI/libraries/armem_objects/aron/Robot.aron.generated.h>
-
namespace armarx::armem
{
void fromAron(const arondto::ObjectInstance& dto, objpose::arondto::ObjectPose& bo);
@@ -19,22 +13,6 @@ namespace armarx::armem
void fromAron(const arondto::ObjectInstance& dto, objpose::ObjectPose& bo);
void toAron(arondto::ObjectInstance& dto, const objpose::ObjectPose& bo);
-
- // TODO move the following
- void fromAron(const long& dto, IceUtil::Time& time);
- void toAron(long& dto, const IceUtil::Time& time);
- // end TODO
-
-
- void fromAron(const arondto::Robot& dto, Robot& bo);
- void toAron(arondto::Robot& dto, const Robot& bo);
-
- void fromAron(const arondto::RobotDescription& dto, RobotDescription& bo);
- void toAron(arondto::RobotDescription& dto, const RobotDescription& bo);
-
- void fromAron(const arondto::RobotState& dto, RobotState& bo);
- void toAron(arondto::RobotState& dto, const RobotState& bo);
-
} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
index 930cbb34405a80fde48eb61241233ce852dbbdb2..b80a0a07c82ac6ee452b7cab489cee1f12e11b3b 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
@@ -8,17 +8,94 @@
#include "RobotAPI/libraries/armem/core/Time.h"
#include "RobotAPI/libraries/armem/client/query/Builder.h"
-#include "RobotAPI/libraries/armem_objects/aron_conversions.h"
-#include <RobotAPI/libraries/armem_objects/aron/Robot.aron.generated.h>
+#include "RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h"
+#include "RobotAPI/libraries/armem_robot/robot_conversions.h"
+#include "RobotAPI/libraries/armem_robot/aron_conversions.h"
+#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
-#include "RobotAPI/libraries/armem_objects/articulated_object_conversions.h"
namespace fs = ::std::filesystem;
namespace armarx::armem::articulated_object
{
- Reader::Reader(ManagedIceObject& component) : MemoryConnector(component) {}
+ Reader::Reader(armem::ClientReaderComponentPluginUser& component) : component(component) {}
+
+ void Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ {
+ ARMARX_DEBUG << "Reader: registerPropertyDefinitions";
+
+ const std::string prefix = propertyPrefix;
+
+ def->optional(properties.memoryName, prefix + "MemoryName");
+
+ def->optional(properties.coreInstanceSegmentName,
+ prefix + "CoreSegment",
+ "Name of the memory core segment to use for object instances.");
+ def->optional(properties.coreClassSegmentName,
+ prefix + "CoreSegment",
+ "Name of the memory core segment to use for object classes.");
+ def->optional(properties.providerName, prefix + "ProviderName");
+ }
+
+
+ void Reader::connect()
+ {
+ // Wait for the memory to become available and add it as dependency.
+ ARMARX_IMPORTANT << "Reader: Waiting for memory '" << properties.memoryName << "' ...";
+ auto result = component.useMemory(properties.memoryName);
+ if (not result.success)
+ {
+ ARMARX_ERROR << result.errorMessage;
+ return;
+ }
+
+ ARMARX_IMPORTANT << "Reader: Connected to memory '" << properties.memoryName;
+
+ memoryReader.setReadingMemory(result.proxy);
+
+ armem::MemoryID id = armem::MemoryID();
+ id.memoryName = properties.memoryName;
+ id.coreSegmentName = properties.coreClassSegmentName;
+ // listen to all provider segments!
+
+ memoryReader.subscribe(id, this, &Reader::updateKnownObjects);
+ }
+
+ void Reader::updateKnownObject(const armem::MemoryID& snapshotId)
+ {
+ // const std::string& nameWithDataset = snapshotId.providerSegmentName;
+
+ // arondto::RobotDescription aronArticulatedObjectDescription;
+ // aronArticulatedObjectDescription.fromAron(snapshotId.);
+
+ // TODO(fabian.reister): implement
+ }
+
+ void Reader::updateKnownObjects(const armem::MemoryID& subscriptionID, const std::vector<armem::MemoryID>& snapshotIDs)
+ {
+ ARMARX_INFO << "New objects available!";
+
+ // // Query all entities from provider.
+ // armem::client::query::Builder qb;
+
+ // // clang-format off
+ // qb
+ // .coreSegments().withName(properties.coreClassSegmentName)
+ // .providerSegments().all() // TODO(fabian.reister): think about this: which authority is trustworthy?
+ // .entities().withName(name)
+ // .snapshots().atTime(timestamp);
+ // // clang-format on
+
+ // const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+
+ // std::for_each(snapshotIDs.begin(), snapshotIDs.end(), [&](const auto & snapshotID)
+ // {
+ // updateKnownObject(snapshotID);
+ // });
+ }
+
std::optional<ArticulatedObject> Reader::get(const std::string& name, const armem::Time& timestamp)
{
@@ -63,7 +140,32 @@ namespace armarx::armem::articulated_object
obj.config = std::move(*state);
}
- std::optional<RobotDescription> Reader::queryDescription(const std::string& name, const armem::Time& timestamp)
+ std::vector<robot::RobotDescription> Reader::queryDescriptions(const armem::Time& timestamp)
+ {
+ // Query all entities from provider.
+ armem::client::query::Builder qb;
+
+ // clang-format off
+ qb
+ .coreSegments().withName(properties.coreClassSegmentName)
+ .providerSegments().all()
+ .entities().all()
+ .snapshots().latest(); // TODO beforeTime(timestamp);
+ // clang-format on
+
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+
+ if (not qResult.success) /* c++20 [[unlikely]] */
+ {
+ return {};
+ }
+
+ return getRobotDescriptions(qResult.memory);
+ }
+
+ std::optional<robot::RobotDescription> Reader::queryDescription(const std::string& name, const armem::Time& timestamp)
{
// Query all entities from provider.
armem::client::query::Builder qb;
@@ -88,7 +190,7 @@ namespace armarx::armem::articulated_object
return getRobotDescription(qResult.memory);
}
- std::optional<RobotState> Reader::queryState(const RobotDescription& description, const armem::Time& timestamp)
+ std::optional<robot::RobotState> Reader::queryState(const robot::RobotDescription& description, const armem::Time& timestamp)
{
// TODO(fabian.reister): how to deal with multiple providers?
@@ -116,7 +218,7 @@ namespace armarx::armem::articulated_object
}
- std::optional<RobotState> Reader::getRobotState(const armarx::armem::wm::Memory& memory) const
+ std::optional<robot::RobotState> Reader::getRobotState(const armarx::armem::wm::Memory& memory) const
{
// clang-format off
const armem::wm::ProviderSegment& providerSegment = memory
@@ -142,12 +244,12 @@ namespace armarx::armem::articulated_object
// TODO(fabian.reister): check if 0 available
const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
- return convertRobotState(instance);
+ return robot::convertRobotState(instance);
}
- std::optional<RobotDescription> Reader::getRobotDescription(const armarx::armem::wm::Memory& memory) const
+ std::optional<robot::RobotDescription> Reader::getRobotDescription(const armarx::armem::wm::Memory& memory) const
{
// clang-format off
const armem::wm::ProviderSegment& providerSegment = memory
@@ -173,7 +275,39 @@ namespace armarx::armem::articulated_object
// TODO(fabian.reister): check if 0 available
const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
- return convertRobotDescription(instance);
+ return robot::convertRobotDescription(instance);
+ }
+
+ std::vector<robot::RobotDescription> Reader::getRobotDescriptions(const armarx::armem::wm::Memory& memory) const
+ {
+ std::vector<robot::RobotDescription> descriptions;
+
+ const armem::wm::CoreSegment& coreSegment = memory.getCoreSegment(properties.coreClassSegmentName);
+
+ for (const auto& [providerName, providerSegment] : coreSegment.providerSegments())
+ {
+ for (const auto& [name, entity] : providerSegment.entities())
+ {
+ if (entity.empty())
+ {
+ ARMARX_WARNING << "No entity found";
+ continue;
+ }
+
+ const auto entitySnapshots = simox::alg::get_values(entity.history());
+ const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
+
+ const auto robotDescription = robot::convertRobotDescription(instance);
+
+ if (robotDescription)
+ {
+ descriptions.push_back(*robotDescription);
+ }
+ }
+ }
+
+ return descriptions;
}
+
} // namespace armarx::armem::articulated_object
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
index cd047cf6cc81dca29061f679537d787935ebfa75..21a4591cd2f1f5c9a125177e50285b3802f561b9 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
@@ -24,7 +24,7 @@
#include <mutex>
#include <optional>
-#include "RobotAPI/libraries/armem/client/MemoryConnector.h"
+#include "RobotAPI/libraries/armem/client.h"
#include "RobotAPI/libraries/armem/client/Reader.h"
#include "interfaces.h"
@@ -32,36 +32,42 @@
namespace armarx::armem::articulated_object
{
class Reader:
- virtual public ReaderInterface,
- virtual public ::armarx::armem::MemoryConnector
+ virtual public ReaderInterface
{
public:
- Reader(ManagedIceObject& component);
+ Reader(armem::ClientReaderComponentPluginUser& component);
virtual ~Reader() = default;
+ void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
+ void connect();
+
void synchronize(ArticulatedObject& obj, const armem::Time& timestamp) override;
std::optional<ArticulatedObject> get(const std::string& name, const armem::Time& timestamp) override;
ArticulatedObject get(const ArticulatedObjectDescription& description, const armem::Time& timestamp) override;
- std::optional<RobotState> queryState(const RobotDescription& description, const armem::Time& timestamp);
- std::optional<RobotDescription> queryDescription(const std::string& name, const armem::Time& timestamp);
+ std::optional<robot::RobotState> queryState(const robot::RobotDescription& description, const armem::Time& timestamp);
+ std::optional<robot::RobotDescription> queryDescription(const std::string& name, const armem::Time& timestamp);
+ std::vector<robot::RobotDescription> queryDescriptions(const armem::Time& timestamp);
// TODO(fabian.reister): register property defs
protected:
- std::optional<RobotState> getRobotState(const armarx::armem::wm::Memory& memory) const;
- std::optional<RobotDescription> getRobotDescription(const armarx::armem::wm::Memory& memory) const;
+ std::optional<robot::RobotState> getRobotState(const armarx::armem::wm::Memory& memory) const;
+ std::optional<robot::RobotDescription> getRobotDescription(const armarx::armem::wm::Memory& memory) const;
+ std::vector<robot::RobotDescription> getRobotDescriptions(const armarx::armem::wm::Memory& memory) const;
private:
+ void updateKnownObjects(const armem::MemoryID& subscriptionID, const std::vector<armem::MemoryID>& snapshotIDs);
+ void updateKnownObject(const armem::MemoryID& snapshotId);
struct Properties
{
std::string memoryName = "Object";
std::string coreInstanceSegmentName = "ArticulatedObjectInstance";
std::string coreClassSegmentName = "ArticulatedObjectClass";
- std::string providerName;
+ std::string providerName = "ArmarXObjects";
} properties;
const std::string propertyPrefix = "mem.obj.articulated.";
@@ -69,6 +75,7 @@ namespace armarx::armem::articulated_object
armem::client::Reader memoryReader;
std::mutex memoryWriterMutex;
+ armem::ClientReaderComponentPluginUser& component;
};
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
index cf057be299a48eb3e289a03886bd39c9a16046a3..703ef91aa2eea9d00ea09b88255b68ffacba68bc 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
@@ -1,5 +1,7 @@
#include "Writer.h"
+#include <IceUtil/Time.h>
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <mutex>
#include <optional>
@@ -7,22 +9,22 @@
#include "RobotAPI/libraries/armem/core/MemoryID.h"
#include "RobotAPI/libraries/armem_objects/aron_conversions.h"
-#include <RobotAPI/libraries/armem_objects/aron/RobotDescription.aron.generated.h>
-#include <RobotAPI/libraries/armem_objects/aron/Robot.aron.generated.h>
+#include "RobotAPI/libraries/armem_robot/aron_conversions.h"
+#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include "RobotAPI/libraries/armem_robot/robot_conversions.h"
namespace armarx::armem::articulated_object
{
- Writer::Writer(ManagedIceObject& component): MemoryConnector(component) {}
+ Writer::Writer(armem::ClientComponentPluginUser& component): component(component) {}
void Writer::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
ARMARX_DEBUG << "Writer: registerPropertyDefinitions";
- MemoryConnector::registerPropertyDefinitions(def);
-
- const std::string prefix = getPropertyPrefix();
+ const std::string prefix = propertyPrefix;
def->optional(properties.memoryName, prefix + "MemoryName");
@@ -32,14 +34,14 @@ namespace armarx::armem::articulated_object
def->optional(properties.coreClassSegmentName,
prefix + "CoreSegment",
"Name of the memory core segment to use for object classes.");
- def->required(properties.providerName, prefix + "ProviderName");
+ def->optional(properties.providerName, prefix + "ProviderName");
}
void Writer::connect()
{
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "Writer: Waiting for memory '" << properties.memoryName << "' ...";
- auto result = useMemory(properties.memoryName);
+ auto result = component.useMemory(properties.memoryName);
if (not result.success)
{
ARMARX_ERROR << result.errorMessage;
@@ -61,6 +63,7 @@ namespace armarx::armem::articulated_object
armem::MemoryID id;
id.setCoreSegmentID(refId); // listen to all provider segments!
+ updateKnownObjects();
memoryReader.subscribe(id, this, &Writer::updateKnownObjects);
}
@@ -76,11 +79,12 @@ namespace armarx::armem::articulated_object
void Writer::updateKnownObjects(const armem::MemoryID& subscriptionID, const std::vector<armem::MemoryID>& snapshotIDs)
{
ARMARX_INFO << "New objects available!";
+ updateKnownObjects();
+ }
- std::for_each(snapshotIDs.begin(), snapshotIDs.end(), [&](const auto & snapshotID)
- {
- updateKnownObject(snapshotID);
- });
+ void Writer::updateKnownObjects()
+ {
+ knownObjects = queryDescriptions(IceUtil::Time::now());
}
std::optional<armem::MemoryID> Writer::storeOrGetClass(const ArticulatedObject& obj)
@@ -94,7 +98,12 @@ namespace armarx::armem::articulated_object
}
// otherwise create
- return storeClass(obj);
+ if (properties.allowClassCreation)
+ {
+ return storeClass(obj);
+ }
+
+ return std::nullopt;
}
std::optional<armem::MemoryID> Writer::storeClass(const ArticulatedObject& obj)
@@ -173,13 +182,13 @@ namespace armarx::armem::articulated_object
update.entityID = entityID;
arondto::Robot aronArticulatedObject;
- toAron(aronArticulatedObject, obj);
+ robot::toAron(aronArticulatedObject, obj);
const auto descriptionId = storeOrGetClass(obj);
if (not descriptionId)
{
- ARMARX_ERROR << "Could not get class for object " << obj.description.name;
+ ARMARX_WARNING << "Could not get class for object " << obj.description.name;
return false;
}
@@ -196,7 +205,7 @@ namespace armarx::armem::articulated_object
if (not updateResult.success)
{
- ARMARX_ERROR << updateResult.errorMessage;
+ ARMARX_WARNING << updateResult.errorMessage;
}
return updateResult.success;
@@ -208,7 +217,8 @@ namespace armarx::armem::articulated_object
if (not classId)
{
- ARMARX_WARNING << "Could not get class id!";
+ ARMARX_WARNING << "Could not get class id for object " << obj.description.name << "! "
+ << "Known classes are " << simox::alg::get_keys(knownObjects);
return false;
}
@@ -216,9 +226,101 @@ namespace armarx::armem::articulated_object
return storeInstance(obj);
}
- const std::string& Writer::getPropertyPrefix() const
+ // const std::string& Writer::getPropertyPrefix() const
+ // {
+ // return propertyPrefix;
+ // }
+
+
+ // TODO this is a duplicate
+ std::optional<robot::RobotDescription> Writer::getRobotDescription(const armarx::armem::wm::Memory& memory) const
{
- return propertyPrefix;
+ // clang-format off
+ const armem::wm::ProviderSegment& providerSegment = memory
+ .getCoreSegment(properties.coreClassSegmentName)
+ .getProviderSegment(properties.providerName); // TODO(fabian.reister): all
+ // clang-format on
+ const auto entities = simox::alg::get_values(providerSegment.entities());
+
+ if (entities.empty())
+ {
+ ARMARX_WARNING << "No entity found";
+ return std::nullopt;
+ }
+
+ const auto entitySnapshots = simox::alg::get_values(entities.front().history());
+
+ if (entitySnapshots.empty())
+ {
+ ARMARX_WARNING << "No entity snapshots found";
+ return std::nullopt;
+ }
+
+ // TODO(fabian.reister): check if 0 available
+ const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
+
+ return robot::convertRobotDescription(instance);
}
+ std::unordered_map<std::string, armem::MemoryID>Writer::getRobotDescriptions(const armarx::armem::wm::Memory& memory) const
+ {
+ std::unordered_map<std::string, armem::MemoryID> descriptions;
+
+ const armem::wm::CoreSegment& coreSegment = memory.getCoreSegment(properties.coreClassSegmentName);
+
+ for (const auto& [providerName, providerSegment] : coreSegment.providerSegments())
+ {
+ for (const auto& [name, entity] : providerSegment.entities())
+ {
+ if (entity.empty())
+ {
+ ARMARX_WARNING << "No entity found";
+ continue;
+ }
+
+ const auto entitySnapshots = simox::alg::get_values(entity.history());
+ const armem::wm::EntitySnapshot& sn = entitySnapshots.front();
+ const armem::wm::EntityInstance& instance = sn.getInstance(0);
+
+ const auto robotDescription = robot::convertRobotDescription(instance);
+
+ if (robotDescription)
+ {
+ const armem::MemoryID snapshotID(sn.id());
+ descriptions.insert({robotDescription->name, snapshotID});
+ }
+ }
+
+ }
+
+ return descriptions;
+ }
+
+
+ std::unordered_map<std::string, armem::MemoryID> Writer::queryDescriptions(const armem::Time& timestamp)
+ {
+ // Query all entities from provider.
+ armem::client::query::Builder qb;
+
+ // clang-format off
+ qb
+ .coreSegments().withName(properties.coreClassSegmentName)
+ .providerSegments().all()
+ .entities().all()
+ .snapshots().latest(); // TODO beforeTime(timestamp);
+ // clang-format on
+
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+
+ if (not qResult.success) /* c++20 [[unlikely]] */
+ {
+ return {};
+ }
+
+ return getRobotDescriptions(qResult.memory);
+ }
+
+
} // namespace armarx::armem::articulated_object
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
index 9e9b2075846219ca9390f56f3474c839d169dc52..7e450b0464d1b571791392beb4622afc77444f5b 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
@@ -23,9 +23,9 @@
#include <mutex>
-#include "RobotAPI/libraries/armem/client/MemoryConnector.h"
#include "RobotAPI/libraries/armem/client/Reader.h"
#include "RobotAPI/libraries/armem/client/Writer.h"
+#include "RobotAPI/libraries/armem/client.h"
#include "interfaces.h"
@@ -34,11 +34,10 @@ namespace armarx::armem::articulated_object
{
class Writer:
- virtual public WriterInterface,
- virtual public MemoryConnector
+ virtual public WriterInterface
{
public:
- Writer(ManagedIceObject& component);
+ Writer(armem::ClientComponentPluginUser& component);
virtual ~Writer() = default;
@@ -51,20 +50,29 @@ namespace armarx::armem::articulated_object
bool storeInstance(const ArticulatedObject& obj);
std::optional<armem::MemoryID> storeClass(const ArticulatedObject& obj);
- const std::string& getPropertyPrefix() const override;
+ // const std::string& getPropertyPrefix() const override;
private:
std::optional<armem::MemoryID> storeOrGetClass(const ArticulatedObject& obj);
void updateKnownObjects(const armem::MemoryID& subscriptionID, const std::vector<armem::MemoryID>& snapshotIDs);
+ void updateKnownObjects();
void updateKnownObject(const armem::MemoryID& snapshotId);
+ // TODO duplicate
+ std::unordered_map<std::string, armem::MemoryID> queryDescriptions(const armem::Time& timestamp);
+ std::optional<robot::RobotDescription> getRobotDescription(const armarx::armem::wm::Memory& memory) const;
+ std::unordered_map<std::string, armem::MemoryID> getRobotDescriptions(const armarx::armem::wm::Memory& memory) const;
+
+
struct Properties
{
std::string memoryName = "Object";
std::string coreInstanceSegmentName = "ArticulatedObjectInstance";
std::string coreClassSegmentName = "ArticulatedObjectClass";
- std::string providerName;
+ std::string providerName = "ArmarXObjects";
+
+ bool allowClassCreation = false;
} properties;
const std::string propertyPrefix = "mem.obj.articulated.";
@@ -76,7 +84,9 @@ namespace armarx::armem::articulated_object
std::mutex memoryReaderMutex;
// key: name of object: RobotDescription::name
- std::map<std::string, MemoryID> knownObjects;
+ std::unordered_map<std::string, MemoryID> knownObjects;
+
+ armem::ClientComponentPluginUser& component;
};
diff --git a/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.cpp
index e2babe5079ff5a0e00d07317d163c089644db77b..422b8e6be1fa3b50fe0ffc154ce2612528c3a578 100644
--- a/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.cpp
@@ -1,5 +1,6 @@
#include "Segment.h"
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <sstream>
#include <ArmarXCore/core/time/TimeUtil.h>
@@ -15,9 +16,9 @@
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
-#include <RobotAPI/libraries/armem_objects/aron/Robot.aron.generated.h>
-#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
-#include "RobotAPI/libraries/armem_objects/articulated_object_conversions.h"
+#include "RobotAPI/libraries/armem_robot/robot_conversions.h"
+#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
namespace armarx::armem::server::obj::articulated_object_class
@@ -36,14 +37,24 @@ namespace armarx::armem::server::obj::articulated_object_class
{
defs->optional(p.coreClassSegmentName, prefix + "CoreSegmentName", "Name of the object instance core segment.");
defs->optional(p.maxHistorySize, prefix + "MaxHistorySize", "Maximal size of object poses history (-1 for infinite).");
+
+ defs->optional(p.objectsPackage, prefix + "ObjectsPackage", "Name of the objects package to load from.");
+ defs->optional(p.loadFromObjectsPackage, prefix + "LoadFromObjectsPackage",
+ "If true, load the objects from the objects package on startup.");
+
}
void Segment::init()
{
ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
- coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreClassSegmentName, arondto::Robot::toInitialAronType());
+ coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreClassSegmentName, arondto::RobotDescription::toInitialAronType());
coreSegment->setMaxHistorySize(p.maxHistorySize);
+
+ if (p.loadFromObjectsPackage)
+ {
+ loadByObjectFinder(p.objectsPackage);
+ }
}
void Segment::connect(viz::Client arviz)
@@ -51,6 +62,50 @@ namespace armarx::armem::server::obj::articulated_object_class
// this->visu = std::make_unique<Visu>(arviz, *this);
}
+ void Segment::loadByObjectFinder(const std::string& package)
+ {
+ ObjectFinder finder(package);
+
+ const auto knownArticulatedObjectDescriptions = finder.findAllArticulatedObjectsByDataset(true);
+ ARMARX_DEBUG << "Found " << knownArticulatedObjectDescriptions.size() << " articulated objects";
+
+ loadObjectsIntoMemory(knownArticulatedObjectDescriptions, package);
+ }
+
+ void Segment::loadObjectsIntoMemory(const std::unordered_map<std::string, std::vector<armem::articulated_object::ArticulatedObjectDescription>>& datasets, const std::string& package)
+ {
+ const Time now = TimeUtil::GetTime();
+
+ const MemoryID providerID = coreSegment->id().withProviderSegmentName(package);
+ coreSegment->addProviderSegment(providerID.providerSegmentName);
+
+ // ARMARX_INFO << "Loading up to " << infos.size() << " object classes from '"
+ // << objectFinder.getPackageName() << "' ...";
+ Commit commit;
+
+ for (const auto& [datasetName, descriptions] : datasets)
+ {
+
+ for (const armem::articulated_object::ArticulatedObjectDescription& desc : descriptions)
+ {
+ EntityUpdate& update = commit.updates.emplace_back();
+ update.entityID = providerID.withEntityName(datasetName + "/" + desc.name);
+ update.timeArrived = update.timeCreated = update.timeSent = now;
+
+ arondto::RobotDescription aronRobotDescription;
+ toAron(aronRobotDescription, desc);
+ // TODO toAron(aronRobotDescription.timestamp, now);
+
+ update.instancesData = { aronRobotDescription.toAron()};
+ }
+
+ ARMARX_INFO << "Loaded " << commit.updates.size() << " articulated object classes from '"
+ << package << "' in dataset '" << datasetName << "'.";
+ }
+ iceMemory.commit(commit);
+ }
+
+
std::unordered_map<armem::MemoryID, ::armarx::armem::articulated_object::ArticulatedObjectDescription> Segment::getKnownObjectClasses() const
{
@@ -60,22 +115,25 @@ namespace armarx::armem::server::obj::articulated_object_class
{
for (const auto& [name, entity] : provSeg.entities())
{
- const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
- const auto description = articulated_object::convertRobotDescription(entityInstance);
-
- if (not description)
+ for (const auto& snapshot : simox::alg::get_values(entity.history()))
{
- ARMARX_WARNING << "Could not convert entity instance to 'RobotDescription'";
- continue;
- }
+ const auto& entityInstance = snapshot.getInstance(0);
+ const auto description = robot::convertRobotDescription(entityInstance);
+
+ if (not description)
+ {
+ ARMARX_WARNING << "Could not convert entity instance to 'RobotDescription'";
+ continue;
+ }
- ARMARX_INFO << "Key is " << armem::MemoryID(entity.id());
+ ARMARX_DEBUG << "Key is " << armem::MemoryID(snapshot.id());
- objects.emplace(armem::MemoryID(entity.id()), *description);
+ objects.emplace(armem::MemoryID(snapshot.id()), *description);
+ }
}
}
- ARMARX_IMPORTANT << "Number of known articulated object classes: " << objects.size();
+ ARMARX_INFO << deactivateSpam(10) << "Number of known articulated object classes: " << objects.size();
return objects;
}
diff --git a/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.h b/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.h
index 3c17b51f3ac00db12123e3bd0b0dcb61e77c11a7..e2a7bbc6fbf2c875e11fb25427f5a7a8809aee4c 100644
--- a/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.h
+++ b/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.h
@@ -36,6 +36,8 @@
#include "RobotAPI/libraries/armem/core/MemoryID.h"
#include "RobotAPI/libraries/armem_objects/types.h"
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+
namespace armarx::armem
{
namespace server
@@ -72,6 +74,8 @@ namespace armarx::armem::server::obj::articulated_object_class
private:
+ void loadByObjectFinder(const std::string& package);
+ void loadObjectsIntoMemory(const std::unordered_map<std::string, std::vector<armem::articulated_object::ArticulatedObjectDescription>>& datasets, const std::string& package);
server::MemoryToIceAdapter& iceMemory;
wm::CoreSegment* coreSegment = nullptr;
@@ -81,6 +85,9 @@ namespace armarx::armem::server::obj::articulated_object_class
{
std::string coreClassSegmentName = "ArticulatedObjectClass";
int64_t maxHistorySize = -1;
+
+ std::string objectsPackage = ObjectFinder::DefaultObjectsPackageName;
+ bool loadFromObjectsPackage = true;
};
Properties p;
diff --git a/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Segment.cpp
index 9418254fff97b79fdca0c5abc55b53378c33ae8b..c7fa562a61f81e8a09889182efe136a963da1a2a 100644
--- a/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Segment.cpp
@@ -17,34 +17,16 @@
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
#include <RobotAPI/libraries/armem/aron/MemoryID.aron.generated.h>
-#include <RobotAPI/libraries/armem_objects/aron/Robot.aron.generated.h>
-#include <RobotAPI/libraries/armem_objects/aron/RobotDescription.aron.generated.h>
-#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
-#include <RobotAPI/libraries/armem_objects/articulated_object_conversions.h>
+#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
+
#include <RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.h>
#include "Visu.h"
-namespace simox::alg
-{
- /// Get the keys of `map` in a vector.
- template <class K, class V, template<class...> class MapT = std::unordered_map, class...Ts>
- std::vector<K> get_keys2(const MapT<K, V, Ts...>& map)
- {
- std::vector<K> keys;
- if constexpr(std::is_same_v<std::unordered_map<K, V, Ts...>, MapT<K, V, Ts...>>)
- {
- keys.reserve(map.size());
- }
- for (const auto& [k, v] : map)
- {
- keys.emplace_back(k);
- }
- return keys;
- }
-}
-
namespace armarx::armem::server::obj::articulated_object_instance
{
@@ -88,20 +70,30 @@ namespace armarx::armem::server::obj::articulated_object_instance
ARMARX_CHECK_NOT_NULL(classSegment);
const auto knownObjectClasses = classSegment->getKnownObjectClasses();
+ ARMARX_DEBUG << "Class segment has " << knownObjectClasses.size() << " known articulated objects";
+
+ const auto escape = [](std::string & v)
+ {
+ v = simox::alg::replace_all(v, "/", "\\/");
+ };
+
const auto resolveDescriptionLink = [&](auto & articulatedObject, const auto & aronDescriptionLink) -> bool
{
armem::MemoryID descriptionLink;
fromAron(aronDescriptionLink, descriptionLink);
+ escape(descriptionLink.providerSegmentName);
+
ARMARX_DEBUG << "Lookup key is " << descriptionLink;
-
+
// const auto keys = simox::alg::get_keys(knownObjectClasses);
// ARMARX_DEBUG << "Known keys " << keys;
const auto it = knownObjectClasses.find(descriptionLink);
if (it == knownObjectClasses.end())
{
- ARMARX_WARNING << "Unknown object class " ; //<< aronArticulatedObject.description;
+ ARMARX_WARNING << "Unknown object class " << descriptionLink
+ << "Known classes are " << simox::alg::get_keys(knownObjectClasses);
return false;
}
diff --git a/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.cpp b/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.cpp
index 385866ce9453fd8db576da3f3b26965c7f36549a..d1ee962acf267c3263676772816ba7d79f8e46cb 100644
--- a/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.cpp
@@ -1,15 +1,16 @@
#include "Visu.h"
-#include "ArmarXCore/core/logging/Logging.h"
-#include "ArmarXCore/core/time/CycleUtil.h"
#include <algorithm>
-#include <SimoxUtility/math/pose.h>
-
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/time/CycleUtil.h>
#include <ArmarXCore/core/time/TimeUtil.h>
+#include <SimoxUtility/math/pose.h>
+
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+#include "ArmarXCore/core/services/tasks/PeriodicTask.h"
#include "Segment.h"
namespace armarx::armem::server::obj::articulated_object_instance
@@ -38,7 +39,6 @@ namespace armarx::armem::server::obj::articulated_object_instance
void Visu::visualizeObjects(viz::Layer& layer, const armarx::armem::articulated_object::ArticulatedObjects& objects) const
{
- ARMARX_INFO << "Entering visualizeObjects";
const auto visualizeObject = [&](const armarx::armem::articulated_object::ArticulatedObject & obj)
{
@@ -47,31 +47,64 @@ namespace armarx::armem::server::obj::articulated_object_instance
// clang-format off
auto robot = viz::Robot(obj.description.name)
// .file(xmlPath.package, xmlPath.path)
- .file("ArmarXObjects", "./data/ArmarXObjects/Environment/mobile-kitchen/dishwasher-only/dishwasher.xml")
+ .file(xmlPath.path, xmlPath.path)
.joints(obj.config.jointMap)
.pose(obj.config.globalPose);
+ // clang-format on
robot.useFullModel();
- // clang-format on
layer.add(robot);
};
std::for_each(objects.begin(), objects.end(), visualizeObject);
- ARMARX_INFO << "Done visualizeObjects";
}
void Visu::init()
{
- updateTask = new SimpleRunningTask<>([this]()
- {
- this->visualizeRun();
- });
+ updateTask = new PeriodicTask<Visu>(this, &Visu::visualizeRun, 1000 / p.frequencyHz);
updateTask->start();
}
+ void Visu::visualizeRun()
+ {
+ // std::scoped_lock lock(visuMutex);
+ ARMARX_DEBUG << "Update task";
+
+ if (not p.enabled)
+ {
+ return;
+ }
+
+ // TIMING_START(Visu);
+
+ const auto articulatedObjects = segment.getArticulatedObjects();
+ ARMARX_DEBUG << "Found " << articulatedObjects.size() << " articulated objects";
+
+ viz::Layer layer = arviz.layer("ArticulatedObjectInstances");
+
+ ARMARX_DEBUG << "visualizing objects";
+ visualizeObjects(layer, articulatedObjects);
+
+ ARMARX_DEBUG << "Committing objects";
+ arviz.commit({layer});
+
+ ARMARX_DEBUG << "Done committing";
+
+ // TIMING_END_STREAM(Visu, ARMARX_VERBOSE);
+
+ // if (debugObserver)
+ // {
+ // debugObserver->setDebugChannel(getName(),
+ // {
+ // { "t Visualize [ms]", new Variant(Visu.toMilliSecondsDouble()) },
+ // });
+ // }
+
+ }
+
// void Visu::RemoteGui::setup(const Visu& visu)
// {
// using namespace armarx::RemoteGui::Client;
@@ -123,50 +156,5 @@ namespace armarx::armem::server::obj::articulated_object_instance
// }
- void Visu::visualizeRun()
- {
- CycleUtil cycle(static_cast<int>(1000 / p.frequencyHz));
- while (updateTask && not updateTask->isStopped())
- {
- {
- // std::scoped_lock lock(visuMutex);
- ARMARX_IMPORTANT << "Update task";
-
- if (p.enabled)
- {
- // TIMING_START(Visu);
-
- const auto articulatedObjects = segment.getArticulatedObjects();
-
- ARMARX_INFO << "Found " << articulatedObjects.size() << " articulated objects";
-
- viz::Layer layer = arviz.layer("ArticulatedObjectInstances");
-
- ARMARX_INFO << "visualizing objects";
- visualizeObjects(layer, articulatedObjects);
-
- ARMARX_INFO << "Committing objects";
-
- arviz.commit({layer});
-
- ARMARX_INFO << "Done committing";
-
-
- // TIMING_END_STREAM(Visu, ARMARX_VERBOSE);
-
- // if (debugObserver)
- // {
- // debugObserver->setDebugChannel(getName(),
- // {
- // { "t Visualize [ms]", new Variant(Visu.toMilliSecondsDouble()) },
- // });
- // }
- }
- }
- cycle.waitForCycleDuration();
- }
- }
-
-
} // namespace armarx::armem::server::obj::articulated_object_instance
diff --git a/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.h b/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.h
index 54aea5e09f0d3000596c83d2eb8ffbafcbf8b81e..3590270fe545c830b4907082dd127d8a44598587 100644
--- a/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.h
+++ b/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.h
@@ -22,7 +22,7 @@
#pragma once
#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/services/tasks/TaskUtil.h>
+#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
// #include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
@@ -35,13 +35,13 @@ namespace armarx
{
class ObjectFinder;
}
+
namespace armarx::armem::server::obj::articulated_object_instance
{
class Segment;
/**
- * @brief Models decay of object localizations by decreasing the confidence
- * the longer the object was not localized.
+ * @brief Visualizes articulated objects
*/
class Visu : public armarx::Logging
{
@@ -76,7 +76,7 @@ namespace armarx::armem::server::obj::articulated_object_instance
} p;
- SimpleRunningTask<>::pointer_type updateTask;
+ PeriodicTask<Visu>::pointer_type updateTask;
void visualizeRun();
// struct RemoteGui
diff --git a/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
index 38b143f2baded43c3a4f028174380abaef288dcf..93989e18d6c5cd39d0f878cd2caea652fadb41b3 100644
--- a/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
@@ -31,7 +31,7 @@ namespace armarx::armem::server::obj::clazz
defs->optional(p.coreSegmentName, prefix + "CoreSegmentName", "Name of the object clazz core segment.");
defs->optional(p.maxHistorySize, prefix + "MaxHistorySize", "Maximal size of object poses history (-1 for infinite).");
- defs->optional(p.objectsPackage, prefix + "ObjectsPackgage", "Name of the objects package to load from.");
+ defs->optional(p.objectsPackage, prefix + "ObjectsPackage", "Name of the objects package to load from.");
defs->optional(p.loadFromObjectsPackage, prefix + "LoadFromObjectsPackage",
"If true, load the objects from the objects package on startup.");
diff --git a/source/RobotAPI/libraries/armem_objects/types.h b/source/RobotAPI/libraries/armem_objects/types.h
index 5d2b4f76c83fa5ed3e823cc2d1528d941dcad6f5..855c855c4d36d5cf02785b6e34d652b1f82b1272 100644
--- a/source/RobotAPI/libraries/armem_objects/types.h
+++ b/source/RobotAPI/libraries/armem_objects/types.h
@@ -1,54 +1,34 @@
-#pragma once
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
-// these types should be core types
+#pragma once
-#include <map>
#include <vector>
-#include <filesystem>
-
-#include <Eigen/Geometry>
-
-#include <IceUtil/Time.h>
-#include <ArmarXCore/core/PackagePath.h>
+#include <RobotAPI/libraries/armem_robot/types.h>
-namespace armarx::armem
+namespace armarx::armem::articulated_object
{
+ using ArticulatedObjectDescription = armarx::armem::robot::RobotDescription;
- struct RobotDescription
- {
- // IceUtil::Time timestamp;
-
- std::string name;
- PackagePath xml{"", std::filesystem::path("")};
-
- };
-
-
- struct RobotState
- {
- IceUtil::Time timestamp;
-
- Eigen::Affine3f globalPose;
- std::map<std::string, float> jointMap;
- };
-
- struct Robot
- {
- RobotDescription description;
- std::string instance;
-
- RobotState config;
-
- IceUtil::Time timestamp;
- };
-
- namespace articulated_object
- {
- using ArticulatedObjectDescription = RobotDescription;
-
- using ArticulatedObject = Robot;
- using ArticulatedObjects = std::vector<ArticulatedObject>;
- }
-
-} // namespace armarx::armem
\ No newline at end of file
+ using ArticulatedObject = armarx::armem::robot::Robot;
+ using ArticulatedObjects = armarx::armem::robot::Robots;
+} // namespace armarx::armem::articulated_object
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot/CMakeLists.txt b/source/RobotAPI/libraries/armem_robot/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..eb0c4fb7d37a4bb43830f224d3a81714f1175f39
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot/CMakeLists.txt
@@ -0,0 +1,43 @@
+set(LIB_NAME armem_robot)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+
+armarx_add_library(
+ LIBS
+ # ArmarXCore
+ ArmarXCore
+ # ArmarXGui
+ ArmarXGuiComponentPlugins
+ # RobotAPI
+ RobotAPI::ArViz
+ RobotAPI::ComponentPlugins
+ RobotAPI::Core
+ RobotAPI::armem
+ HEADERS
+ types.h
+
+ aron_conversions.h
+ robot_conversions.h
+
+ SOURCES
+
+ aron_conversions.cpp
+ robot_conversions.cpp
+)
+
+
+armarx_enable_aron_file_generation_for_target(
+ TARGET_NAME
+ "${LIB_NAME}"
+ ARON_FILES
+ aron/RobotDescription.xml
+ aron/RobotState.xml
+ aron/Robot.xml
+)
+
+add_library(${PROJECT_NAME}::armem_robot ALIAS armem_robot)
+
+# add unit tests
+# add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/armem_objects/aron/Robot.xml b/source/RobotAPI/libraries/armem_robot/aron/Robot.xml
similarity index 78%
rename from source/RobotAPI/libraries/armem_objects/aron/Robot.xml
rename to source/RobotAPI/libraries/armem_robot/aron/Robot.xml
index cc005ec0521c2b6f9dcb5334a14e61337ece5cf1..63e87ab3c4ba7193a8b7800ed4c24881c09e1b45 100644
--- a/source/RobotAPI/libraries/armem_objects/aron/Robot.xml
+++ b/source/RobotAPI/libraries/armem_robot/aron/Robot.xml
@@ -2,13 +2,13 @@
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
<CodeIncludes>
- <Include include="<RobotAPI/libraries/armem_objects/aron/RobotDescription.aron.generated.h>" />
- <Include include="<RobotAPI/libraries/armem_objects/aron/RobotState.aron.generated.h>" />
+ <Include include="<RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>" />
+ <Include include="<RobotAPI/libraries/armem_robot/aron/RobotState.aron.generated.h>" />
<Include include="<RobotAPI/libraries/armem/aron/MemoryID.aron.generated.h>" />
</CodeIncludes>
<AronIncludes>
<!-- <Include include="<RobotAPI/libraries/armem_objects/aron/RobotDescription.xml>" /> -->
- <Include include="<RobotAPI/libraries/armem_objects/aron/RobotState.xml>" />
+ <Include include="<RobotAPI/libraries/armem_robot/aron/RobotState.xml>" />
<Include include="<RobotAPI/libraries/armem/aron/MemoryID.xml>" />
</AronIncludes>
<GenerateTypes>
diff --git a/source/RobotAPI/libraries/armem_objects/aron/RobotDescription.xml b/source/RobotAPI/libraries/armem_robot/aron/RobotDescription.xml
similarity index 100%
rename from source/RobotAPI/libraries/armem_objects/aron/RobotDescription.xml
rename to source/RobotAPI/libraries/armem_robot/aron/RobotDescription.xml
diff --git a/source/RobotAPI/libraries/armem_objects/aron/RobotState.xml b/source/RobotAPI/libraries/armem_robot/aron/RobotState.xml
similarity index 100%
rename from source/RobotAPI/libraries/armem_objects/aron/RobotState.xml
rename to source/RobotAPI/libraries/armem_robot/aron/RobotState.xml
diff --git a/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6a56d19f385af667974269c72c0de37af406da9f
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp
@@ -0,0 +1,66 @@
+#include "aron_conversions.h"
+
+#include <RobotAPI/libraries/aron/common/aron_conversions/core.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions/stl.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions/armarx.h>
+
+namespace armarx::armem::robot
+{
+
+ /* to be moved */
+ void fromAron(const long& dto, IceUtil::Time& time)
+ {
+ time = IceUtil::Time::microSeconds(dto);
+ }
+
+ void toAron(long& dto, const IceUtil::Time& time)
+ {
+ dto = time.toMicroSeconds();
+ }
+
+
+ /* Robot */
+
+ void fromAron(const arondto::Robot& dto, Robot& bo)
+ {
+ // fromAron(dto.description, bo.description);
+ fromAron(dto.state, bo.config);
+ }
+
+ void toAron(arondto::Robot& dto, const Robot& bo)
+ {
+ // toAron(dto.description, bo.description);
+ toAron(dto.state, bo.config);
+ }
+
+ /* RobotDescription */
+
+ void fromAron(const arondto::RobotDescription& dto, RobotDescription& bo)
+ {
+ aron::fromAron(dto.name, bo.name);
+ fromAron(dto.xml, bo.xml);
+ }
+
+ void toAron(arondto::RobotDescription& dto, const RobotDescription& bo)
+ {
+ aron::toAron(dto.name, bo.name);
+ toAron(dto.xml, bo.xml);
+ }
+
+ /* RobotState */
+
+ void fromAron(const arondto::RobotState& dto, RobotState& bo)
+ {
+ fromAron(dto.timestamp, bo.timestamp);
+ bo.globalPose.matrix() = dto.globalPose;
+ bo.jointMap = dto.jointMap;
+ }
+
+ void toAron(arondto::RobotState& dto, const RobotState& bo)
+ {
+ toAron(dto.timestamp, bo.timestamp);
+ dto.globalPose = bo.globalPose.matrix();
+ dto.jointMap = bo.jointMap;
+ }
+
+} // namespace armarx::armem::robot
diff --git a/source/RobotAPI/libraries/armem_robot/aron_conversions.h b/source/RobotAPI/libraries/armem_robot/aron_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..70e06ce8eedb7de9538f5249b3eedf797aec6060
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot/aron_conversions.h
@@ -0,0 +1,25 @@
+#pragma once
+
+#include <RobotAPI/libraries/armem_robot/types.h>
+
+#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot/aron/RobotState.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+
+namespace armarx::armem::robot
+{
+ // TODO move the following
+ void fromAron(const long& dto, IceUtil::Time& time);
+ void toAron(long& dto, const IceUtil::Time& time);
+ // end TODO
+
+ void fromAron(const arondto::Robot& dto, Robot& bo);
+ void toAron(arondto::Robot& dto, const Robot& bo);
+
+ void fromAron(const arondto::RobotDescription& dto, RobotDescription& bo);
+ void toAron(arondto::RobotDescription& dto, const RobotDescription& bo);
+
+ void fromAron(const arondto::RobotState& dto, RobotState& bo);
+ void toAron(arondto::RobotState& dto, const RobotState& bo);
+
+} // namespace armarx::armem::robot
diff --git a/source/RobotAPI/libraries/armem_robot/client/interfaces.h b/source/RobotAPI/libraries/armem_robot/client/interfaces.h
new file mode 100644
index 0000000000000000000000000000000000000000..d127fbf51eee8d842ee259211fd63a467d0a2f9b
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot/client/interfaces.h
@@ -0,0 +1,29 @@
+#pragma once
+
+
+#include <RobotAPI/libraries/armem/core/Time.h>
+
+#include <RobotAPI/libraries/armem_robot/types.h>
+
+namespace armarx::armem::robot
+{
+ class ReaderInterface
+ {
+ public:
+ virtual ~ReaderInterface() = default;
+
+ virtual bool synchronize(Robot& obj, const armem::Time& timestamp) = 0;
+
+ virtual Robot get(const RobotDescription& description, const armem::Time& timestamp) = 0;
+ virtual std::optional<Robot> get(const std::string& name, const armem::Time& timestamp) = 0;
+ };
+
+ class WriterInterface
+ {
+ public:
+ virtual ~WriterInterface() = default;
+
+ virtual bool store(const Robot& obj) = 0;
+ };
+
+} // namespace armarx::armem::robot
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_objects/articulated_object_conversions.cpp b/source/RobotAPI/libraries/armem_robot/robot_conversions.cpp
similarity index 85%
rename from source/RobotAPI/libraries/armem_objects/articulated_object_conversions.cpp
rename to source/RobotAPI/libraries/armem_robot/robot_conversions.cpp
index 03cac43717941e8da1a3a738d6c15aad1aa8987d..6c05e89e038c6f80f61e3e9c47f25bf8b2f1335f 100644
--- a/source/RobotAPI/libraries/armem_objects/articulated_object_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_robot/robot_conversions.cpp
@@ -1,4 +1,4 @@
-#include "articulated_object_conversions.h"
+#include "robot_conversions.h"
#include <filesystem>
@@ -7,14 +7,13 @@
#include "RobotAPI/libraries/armem/core/workingmemory/EntityInstance.h"
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
-
-#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
namespace fs = ::std::filesystem;
-namespace armarx::armem::articulated_object
+namespace armarx::armem::robot
{
std::optional<RobotDescription> convertRobotDescription(const armem::wm::EntityInstance& instance)
@@ -30,7 +29,6 @@ namespace armarx::armem::articulated_object
return std::nullopt;
}
-
RobotDescription robotDescription
{
.name = "",
@@ -62,4 +60,4 @@ namespace armarx::armem::articulated_object
return robotState;
}
-}
+} // namespace armarx::armem::robot
diff --git a/source/RobotAPI/libraries/armem_objects/articulated_object_conversions.h b/source/RobotAPI/libraries/armem_robot/robot_conversions.h
similarity index 89%
rename from source/RobotAPI/libraries/armem_objects/articulated_object_conversions.h
rename to source/RobotAPI/libraries/armem_robot/robot_conversions.h
index ab29163c347bca6cfa78aa1b10581955140a3fc1..3692ae048dacb94d2f24469f5265e998ab23fb34 100644
--- a/source/RobotAPI/libraries/armem_objects/articulated_object_conversions.h
+++ b/source/RobotAPI/libraries/armem_robot/robot_conversions.h
@@ -9,7 +9,7 @@ namespace armarx::armem::wm
class EntityInstance;
}
-namespace armarx::armem::articulated_object
+namespace armarx::armem::robot
{
std::optional<RobotDescription> convertRobotDescription(const armem::wm::EntityInstance& instance);
std::optional<RobotState> convertRobotState(const armem::wm::EntityInstance& instance);
diff --git a/source/RobotAPI/libraries/armem_robot/types.h b/source/RobotAPI/libraries/armem_robot/types.h
new file mode 100644
index 0000000000000000000000000000000000000000..4fed4d1cb277ab316a318cbc13b4f32222fd8939
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot/types.h
@@ -0,0 +1,48 @@
+#pragma once
+
+#include <map>
+#include <vector>
+#include <filesystem>
+
+#include <Eigen/Geometry>
+
+#include <IceUtil/Time.h>
+
+#include <ArmarXCore/core/PackagePath.h>
+
+namespace armarx::armem::robot
+{
+ struct RobotDescription
+ {
+ // IceUtil::Time timestamp;
+
+ std::string name;
+ PackagePath xml{"", std::filesystem::path("")};
+ };
+
+ struct RobotState
+ {
+ using JointMap = std::map<std::string, float>;
+ using Pose = Eigen::Affine3f;
+
+ IceUtil::Time timestamp;
+
+ Pose globalPose;
+ JointMap jointMap;
+ };
+
+ struct Robot
+ {
+ RobotDescription description;
+ std::string instance;
+
+ RobotState config;
+
+ IceUtil::Time timestamp;
+ };
+
+ using Robots = std::vector<Robot>;
+ using RobotDescriptions = std::vector<RobotDescription>;
+ using RobotStates = std::vector<RobotState>;
+
+} // namespace armarx::armem::robot
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_localization/CMakeLists.txt b/source/RobotAPI/libraries/armem_robot_localization/CMakeLists.txt
deleted file mode 100644
index 24caa9df3f797dc1e259f2709a995d50e1a584a7..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_robot_localization/CMakeLists.txt
+++ /dev/null
@@ -1,42 +0,0 @@
-set(LIB_NAME armem_robot_localization)
-
-armarx_component_set_name("${LIB_NAME}")
-armarx_set_target("Library: ${LIB_NAME}")
-
-find_package(Eigen3 QUIET)
-armarx_build_if(Eigen3_FOUND "Eigen3 not available")
-
-armarx_add_library(
- LIBS
- # ArmarX
- ArmarXCore
- ArmarXCoreInterfaces
- ArmarXGuiComponentPlugins
- # This package
- RobotAPICore
- RobotAPIInterfaces
- RobotAPI::libraries::armem
- # System / External
- Eigen3::Eigen
- HEADERS
- ./TransformInterfaces.h
- ./TransformReader.h
- ./TransformWriter.h
- ./aron_conversions.h
- SOURCES
- ./TransformReader.cpp
- ./TransformWriter.cpp
- ./aron_conversions.cpp
-)
-
-
-armarx_enable_aron_file_generation_for_target(
- TARGET_NAME
- "${LIB_NAME}"
- ARON_FILES
- aron/TransformHeader.xml
- aron/Transform.xml
-)
-
-
-add_library(RobotAPI::armem_robot_localization ALIAS armem_robot_localization)
diff --git a/source/RobotAPI/libraries/armem_robot_localization/TransformInterfaces.h b/source/RobotAPI/libraries/armem_robot_localization/TransformInterfaces.h
deleted file mode 100644
index 6962593814414267635efc425637266c7fbcd34a..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_robot_localization/TransformInterfaces.h
+++ /dev/null
@@ -1,111 +0,0 @@
-/*
- * 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::
- * @author Fabian Reister ( fabian dot reister at kit dot edu )
- * @date 2021
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include <Eigen/Geometry>
-
-#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
-
-namespace armarx
-{
-
- struct TransformHeader
- {
- std::string parentFrame;
- std::string frame;
-
- std::string agent;
-
- std::int64_t timestamp; // [µs]
- };
-
- struct Transform
- {
- TransformHeader header;
-
- Eigen::Affine3f transform = Eigen::Affine3f::Identity();
- };
-
- namespace armem
- {
-
- struct TransformResult
- {
- Transform transform;
-
- enum class Status
- {
- Success,
- ErrorLookupIntoFuture,
- ErrorFrameNotAvailable
- } status;
-
- explicit operator bool() const
- {
- return status == Status::Success;
- }
-
- std::string errorMessage = "";
- };
-
- struct TransformQuery
- {
- TransformHeader header;
-
- // bool exact;
- };
-
- class TransformInterface
- {
- public:
- virtual ~TransformInterface() = default;
-
- virtual void registerPropertyDefinitions(PropertyDefinitionsPtr& def) = 0;
- virtual void connect() = 0;
- };
-
- class TransformReaderInterface : virtual public TransformInterface
- {
- public:
- virtual ~TransformReaderInterface() = default;
-
- virtual TransformResult
- getGlobalPose(const std::string& agentName, const std::string& robotRootFrame,
- const std::int64_t& timestamp) const = 0;
-
- virtual TransformResult
- lookupTransform(const TransformQuery& query) const = 0;
- // waitForTransform()
- };
-
- class TransformWriterInterface : virtual public TransformInterface
- {
- public:
- ~TransformWriterInterface() override = default;
-
- virtual bool commitTransform(const Transform& transform) = 0;
- };
-
- } // namespace armem
-
-} // namespace armarx
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_localization/TransformReader.cpp b/source/RobotAPI/libraries/armem_robot_localization/TransformReader.cpp
deleted file mode 100644
index c3b24c4895328e951bc4950339639200206a9daf..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_robot_localization/TransformReader.cpp
+++ /dev/null
@@ -1,449 +0,0 @@
-/*
- * 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::
- * @author Fabian Reister ( fabian dot reister at kit dot edu )
- * @date 2021
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include "TransformReader.h"
-
-#include <Eigen/src/Geometry/Transform.h>
-#include <algorithm>
-#include <iterator>
-#include <numeric>
-
-// Ice
-#include <IceUtil/Time.h>
-
-// Eigen
-#include <Eigen/Geometry>
-
-// Simox
-#include <SimoxUtility/algorithm/get_map_keys_values.h>
-#include <SimoxUtility/algorithm/string/string_tools.h>
-#include <SimoxUtility/color/cmaps.h>
-#include <SimoxUtility/math/pose/interpolate.h>
-
-// ArmarX
-#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <ArmarXCore/core/time/CycleUtil.h>
-
-// this package
-#include <RobotAPI/libraries/armem/client/query/Builder.h>
-#include <RobotAPI/libraries/armem/client/query/query_fns.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
-#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
-#include <RobotAPI/libraries/aron/core/navigator/type/NavigatorFactory.h>
-#include <RobotAPI/libraries/core/FramedPose.h>
-#include <RobotAPI/libraries/armem_robot_localization/aron_conversions.h>
-#include <RobotAPI/libraries/armem_robot_localization/aron/Transform.aron.generated.h>
-
-namespace armarx::armem
-{
-
- TransformReader::TransformReader(ManagedIceObject& component) : MemoryConnector(component) {}
-
- TransformReader::~TransformReader() = default;
-
- void TransformReader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
- {
- ARMARX_DEBUG << "TransformReader: registerPropertyDefinitions";
- MemoryConnector::registerPropertyDefinitions(def);
-
- const std::string prefix = getPropertyPrefix();
-
- def->optional(properties.localizationMemoryName,
- prefix + "LocalizationMemoryName",
- "Name of the localization memory core segment to use.");
-
- def->optional(properties.memoryName, prefix + "MemoryName");
- }
-
- void TransformReader::connect()
- {
- // Wait for the memory to become available and add it as dependency.
- ARMARX_IMPORTANT << "TransformReader: Waiting for memory '" << properties.memoryName
- << "' ...";
- auto result = useMemory(properties.memoryName);
- if (not result.success)
- {
- ARMARX_ERROR << result.errorMessage;
- return;
- }
-
- ARMARX_IMPORTANT << "TransformReader: Connected to memory '" << properties.memoryName;
-
- memoryReader.setReadingMemory(result.proxy);
- }
-
- TransformResult TransformReader::getGlobalPose(const std::string& agentName,
- const std::string& robotRootFrame,
- const std::int64_t& timestamp) const
- {
- const TransformQuery query{.header = {.parentFrame = GlobalFrame,
- .frame = robotRootFrame,
- .agent = agentName,
- .timestamp = timestamp
- }};
-
- return lookupTransform(query);
- }
-
- // std::vector<std::string> buildTransformChainArbitrary(const
- // std::map<std::string, armem::ProviderSegment>& providerSegments, const
- // std::string& parentFrame, const std::string& frame){
-
- // std::map<std::string, std::string> tf;
-
- // // build string representation of transforms
- // for(const auto& [name, segment]: providerSegments){
- // const auto frames = simox::alg::split(name, ",");
- // if (frames.size() != 2){
- // ARMARX_WARNING << "Segment name " << name << " not understood";
- // continue;
- // }
-
- // tf.insert(frame[0], frame[1]);
- // }
-
- // // find
-
- // }
-
- std::vector<std::string> TransformReader::buildTransformChain(const armem::wm::Memory& memory,
- const TransformQuery& query) const
- {
- ARMARX_DEBUG << "Building transform chain";
-
- auto join = [](const std::string & parentFrame, const std::string & frame)
- {
- return parentFrame + "," + frame;
- };
-
- std::vector<std::string> chain;
-
- const auto& agentProviderSegment = memory.getCoreSegment(properties.localizationMemoryName)
- .getProviderSegment(query.header.agent);
-
- auto addToChain = [&](const std::string & parentFrame, const std::string & frame)
- {
- const auto entityName = join(parentFrame, frame);
-
- if (agentProviderSegment.hasEntity(entityName))
- {
- chain.push_back(entityName);
- }
- else
- {
- ARMARX_WARNING << "Cannot perform tf lookup '" << parentFrame << " -> " << frame
- << "'";
- }
- };
-
- std::array<std::string, 3> knownChain
- {
- GlobalFrame, MapFrame, OdometryFrame}; // Robot comes next
-
- auto* frameBeginIt =
- std::find(knownChain.begin(), knownChain.end(), query.header.parentFrame);
- auto* const frameEndIt =
- std::find(knownChain.begin(), knownChain.end(), query.header.frame);
-
- if (frameBeginIt == knownChain.end())
- {
- ARMARX_WARNING << "Parent frame '" << query.header.parentFrame << "' unknown";
- return {};
- }
-
- if (frameEndIt == knownChain.end())
- {
- ARMARX_DEBUG << "Frame '" << query.header.frame << "' must be robot frame";
- }
-
- const size_t nFrames = std::distance(frameBeginIt, frameEndIt);
- ARMARX_DEBUG << "Lookup '" << query.header.parentFrame << " -> " << query.header.frame
- << "' across " << nFrames << " frames";
-
- for (; frameBeginIt != knownChain.end() - 1; frameBeginIt++)
- {
- addToChain(*frameBeginIt, *(frameBeginIt + 1));
- }
-
- if (frameEndIt == knownChain.end())
- {
- addToChain(knownChain.back(), query.header.frame);
- }
-
- if (chain.empty())
- {
- ARMARX_WARNING << "Cannot create tf lookup chain '" << query.header.parentFrame
- << " -> " << query.header.frame << "'";
- return {};
- }
-
- return chain;
- }
-
- inline Transform convertEntityToTransform(const armem::wm::EntityInstance& item)
- {
- aron::Transform aronTransform;
- aronTransform.fromAron(item.data());
-
- Transform transform;
- fromAron(aronTransform, transform);
-
- return transform;
- }
-
- decltype(auto) findFirstElementAfter(const std::vector<Transform>& transforms,
- const int64_t timestamp)
- {
- auto timestampBeyond = [timestamp](const Transform & transform)
- {
- return transform.header.timestamp > timestamp;
- };
-
- const auto poseNextIt = std::find_if(transforms.begin(), transforms.end(), timestampBeyond);
- return poseNextIt;
- }
-
- Eigen::Affine3f interpolateTransform(const std::vector<Transform>& queue, int64_t timestamp)
- {
- ARMARX_TRACE;
-
- ARMARX_DEBUG << "Entering";
-
-
- ARMARX_CHECK(not queue.empty())
- << "The queue has to contain at least two items to perform a lookup";
-
-
- ARMARX_DEBUG << "Entering ... "
- << "Q front " << queue.front().header.timestamp << " "
- << "Q back " << queue.back().header.timestamp << " "
- << "query timestamp " << timestamp;
-
- // TODO(fabian.reister): sort queue.
-
-
- ARMARX_CHECK(queue.back().header.timestamp > timestamp)
- << "Cannot perform lookup into the future!";
-
- // ARMARX_DEBUG << "Entering 1.5 " << queue.front().timestamp << " " << timestamp;
- ARMARX_CHECK(queue.front().header.timestamp < timestamp)
- << "Cannot perform lookup. Timestamp too old";
- // => now we know that there is an element right after and before the timestamp within our queue
-
- ARMARX_DEBUG << "Entering 2";
-
- const auto poseNextIt = findFirstElementAfter(queue, timestamp);
-
- ARMARX_DEBUG << "it ari";
-
- const auto posePreIt = poseNextIt - 1;
-
- ARMARX_DEBUG << "deref";
-
- // the time fraction [0..1] of the lookup wrt to posePre and poseNext
- const float t = static_cast<float>(timestamp - posePreIt->header.timestamp) /
- (poseNextIt->header.timestamp - posePreIt->header.timestamp);
-
- ARMARX_DEBUG << "interpolate";
-
- return simox::math::interpolatePose(posePreIt->transform, poseNextIt->transform, t);
- }
-
- Eigen::Affine3f TransformReader::obtainTransform(const std::string& entityName, const armem::wm::ProviderSegment& agentProviderSegment, const int64_t timestamp) const
- {
-
- ARMARX_DEBUG << "getEntity:" + entityName;
- const auto& entity = agentProviderSegment.getEntity(entityName);
-
- ARMARX_DEBUG << "History (size: " << entity.history().size() << ")"
- << simox::alg::get_keys(entity.history());
-
- // if (entity.history.empty())
- // {
- // // TODO(fabian.reister): fixme boom
- // ARMARX_ERROR << "No snapshots received.";
- // return Eigen::Affine3f::Identity();
- // }
-
-
- std::vector<Transform> transforms;
- transforms.reserve(entity.history().size());
-
- const auto entitySnapshots = simox::alg::get_values(entity.history());
- std::transform(entitySnapshots.begin(),
- entitySnapshots.end(),
- std::back_inserter(transforms),
- [](const auto & entity)
- {
- return convertEntityToTransform(entity.getInstance(0));
- });
-
- ARMARX_DEBUG << "obtaining transform";
-
- if (transforms.size() > 1)
- {
- // TODO(fabian.reister): remove
- return transforms.front().transform;
-
- ARMARX_DEBUG << "More than one snapshots received: " << transforms.size();
- const auto p = interpolateTransform(transforms, timestamp);
- ARMARX_DEBUG << "Done interpolating transform";
- return p;
- }
-
-
- // accept this to fail (will raise armem::error::MissingEntry)
- if (transforms.empty())
- {
- ARMARX_DEBUG << "empty transform";
-
- throw armem::error::MissingEntry("foo", "bar", "foo2", "bar2");
- }
-
- ARMARX_DEBUG << "single transform";
-
-
- return transforms.front().transform;
- }
-
- std::vector<Eigen::Affine3f>
- TransformReader::obtainTransforms(const armem::wm::Memory& memory,
- const std::vector<std::string>& tfChain,
- const std::string& agent,
- const std::int64_t& timestamp) const
- {
-
- ARMARX_DEBUG << "Core segments" << simox::alg::get_keys(memory.coreSegments());
-
- const auto& agentProviderSegment =
- memory.getCoreSegment(properties.localizationMemoryName).getProviderSegment(agent);
-
- ARMARX_DEBUG << "Provider segments"
- << simox::alg::get_keys(
- memory.getCoreSegment(properties.localizationMemoryName)
- .providerSegments());
-
- ARMARX_DEBUG << "Entities: " << simox::alg::get_keys(agentProviderSegment.entities());
-
- try
- {
- std::vector<Eigen::Affine3f> transforms;
- transforms.reserve(tfChain.size());
- std::transform(tfChain.begin(),
- tfChain.end(),
- std::back_inserter(transforms),
- [&](const std::string & entityName)
- {
- return obtainTransform(entityName, agentProviderSegment, timestamp);
- });
- return transforms;
- }
- catch (const armem::error::MissingEntry& missingEntryError)
- {
- ARMARX_WARNING << missingEntryError.what();
- }
- catch (const ::armarx::exceptions::local::ExpressionException& ex)
- {
- ARMARX_WARNING << "local exception: " << ex.what();
- }
-
- return {};
-
- }
-
- TransformResult TransformReader::lookupTransform(const TransformQuery& query) const
- {
-
- const auto timestamp = IceUtil::Time::microSeconds(query.header.timestamp);
-
- const auto durationEpsilon = IceUtil::Time::milliSeconds(100);
-
- ARMARX_DEBUG << "Looking up transform at timestamp " << timestamp;
-
- // Query all entities from provider.
- armem::client::query::Builder qb;
-
- // clang-format off
- qb
- .coreSegments().withName(properties.localizationMemoryName)
- .providerSegments().withName(query.header.agent) // agent
- .entities().all() // parentFrame,frame
- .snapshots().timeRange(timestamp - durationEpsilon, timestamp + durationEpsilon);
- // clang-format on
-
- // TODO(fabian.reister): remove latest() and add atTime
- // .atTime(timestamp); // transformation
-
- const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
-
- ARMARX_DEBUG << "Lookup result in reader: " << qResult;
-
- if (not qResult.success)
- {
- return {.transform =
- {
- .header = query.header,
- },
- .status = TransformResult::Status::ErrorFrameNotAvailable,
- .errorMessage = "Error in tf lookup '" + query.header.parentFrame + " -> " +
- query.header.frame + "' : " + qResult.errorMessage};
- }
-
- const std::vector<std::string> tfChain = buildTransformChain(qResult.memory, query);
-
- if (tfChain.empty())
- {
- return {.transform = {.header = query.header},
- .status = TransformResult::Status::ErrorFrameNotAvailable,
- .errorMessage = "Cannot create tf lookup chain '" +
- query.header.parentFrame + " -> " + query.header.frame +
- "'"};
- }
-
- const std::vector<Eigen::Affine3f> transforms = obtainTransforms(
- qResult.memory, tfChain, query.header.agent, timestamp.toMicroSeconds());
-
- if (transforms.empty())
- {
- ARMARX_WARNING << "No transform available.";
- return {.transform = {.header = query.header},
- .status = TransformResult::Status::ErrorFrameNotAvailable,
- .errorMessage = "Error in TF loookup: '" +
- query.header.parentFrame + " -> " + query.header.frame +
- "'. No memory data in time range."};
- }
-
- const Eigen::Affine3f transform = std::accumulate(transforms.begin(),
- transforms.end(),
- Eigen::Affine3f::Identity(),
- std::multiplies<>());
-
- ARMARX_DEBUG << "Found valid transform";
-
- return {.transform = {.header = query.header, .transform = transform},
- .status = TransformResult::Status::Success};
- }
-
-} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_robot_localization/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot_localization/aron_conversions.cpp
deleted file mode 100644
index 40832ae90e2686d76a1d2256b8e427c2cafc88a2..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_robot_localization/aron_conversions.cpp
+++ /dev/null
@@ -1,55 +0,0 @@
-#include "aron_conversions.h"
-
-// STL
-#include <string>
-
-// Ice
-#include <IceUtil/Time.h>
-
-// RobotAPI
-#include <RobotAPI/libraries/armem_robot_localization/TransformInterfaces.h>
-#include <RobotAPI/libraries/armem_robot_localization/aron/Transform.aron.generated.h>
-
-
-namespace armarx
-{
-
- TransformHeader fromAron(const aron::TransformHeader& aronHeader)
- {
- TransformHeader header;
-
- header.parentFrame = aronHeader.parentFrame;
- header.frame = aronHeader.frame;
- header.agent = aronHeader.agent;
- header.timestamp = aronHeader.timestamp;
-
- return header;
- }
-
-
- void fromAron(const aron::Transform& aronTransform, Transform& transform)
- {
- transform.header = fromAron(aronTransform.header);
- transform.transform = aronTransform.transform;
- }
-
-
- aron::TransformHeader toAron(const TransformHeader& header)
- {
- aron::TransformHeader aronHeader;
-
- aronHeader.parentFrame = header.parentFrame;
- aronHeader.frame = header.frame;
- aronHeader.agent = header.agent;
- aronHeader.timestamp = header.timestamp;
-
- return aronHeader;
- }
-
- void toAron(const Transform& transform, aron::Transform& aronTransform)
- {
- aronTransform.header = toAron(transform.header);
- aronTransform.transform = transform.transform.matrix();
- }
-
-} // namespace armarx
diff --git a/source/RobotAPI/libraries/armem_robot_localization/aron_conversions.h b/source/RobotAPI/libraries/armem_robot_localization/aron_conversions.h
deleted file mode 100644
index 6a5379fe759f73fd03ad7e274a9a2211ba89d846..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_robot_localization/aron_conversions.h
+++ /dev/null
@@ -1,16 +0,0 @@
-#pragma once
-
-
-namespace armarx
-{
- struct Transform;
-
- namespace aron
- {
- struct Transform;
- } // namespace aron
-
- void fromAron(const aron::Transform& aronTransform, Transform& transform);
- void toAron(const Transform& transform, aron::Transform& aronTransform);
-
-} // namespace armarx
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/CMakeLists.txt b/source/RobotAPI/libraries/armem_robot_mapping/CMakeLists.txt
index 28e32f85f987b88246265ae4fc165c7bd68968de..145ced819b68db659a92e232747c645c24037ce5 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_robot_mapping/CMakeLists.txt
@@ -9,7 +9,7 @@ armarx_add_library(
ArmarXCore
# This package
RobotAPICore
- RobotAPI::libraries::armem
+ RobotAPI::armem
# System / External
Eigen3::Eigen
HEADERS
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp
index 9f7317320a13d5598c9457fe2fa5fcf4bc14fda7..fd8250c5b2b086f3869729e2bf6836e5e3c5dc32 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp
@@ -44,7 +44,7 @@
#include <RobotAPI/libraries/armem/core/workingmemory/EntityInstance.h>
#include <RobotAPI/libraries/armem/util/util.h>
-#include <RobotAPI/libraries/armem/client/MemoryConnector.h>
+
#include <RobotAPI/libraries/armem_robot_mapping/aron/LaserScan.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_mapping/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot_mapping/types.h>
@@ -52,8 +52,8 @@
namespace armarx::armem
{
- MappingDataReader::MappingDataReader(ManagedIceObject& component)
- : armarx::armem::MemoryConnector(component) {}
+ MappingDataReader::MappingDataReader(armem::ClientReaderComponentPluginUser& memoryClient)
+ : memoryClient(memoryClient) {}
MappingDataReader::~MappingDataReader() = default;
@@ -61,9 +61,9 @@ namespace armarx::armem
armarx::PropertyDefinitionsPtr& def)
{
ARMARX_DEBUG << "TransformReader: registerPropertyDefinitions";
- MemoryConnector::registerPropertyDefinitions(def);
+ registerPropertyDefinitions(def);
- const std::string prefix = getPropertyPrefix();
+ const std::string prefix = propertyPrefix;
def->optional(properties.mappingMemoryName, prefix + "MappingMemoryName",
"Name of the mapping memory core segment to use.");
@@ -76,7 +76,7 @@ namespace armarx::armem
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "TransformReader: Waiting for memory '"
<< properties.memoryName << "' ...";
- auto result = useMemory(properties.memoryName);
+ auto result = memoryClient.useMemory(properties.memoryName);
if (not result.success)
{
ARMARX_ERROR << result.errorMessage;
@@ -156,7 +156,7 @@ namespace armarx::armem
ARMARX_WARNING << "Empty history for " << s;
}
- ARMARX_INFO << "History size: " << entity.size();
+ ARMARX_DEBUG << "History size: " << entity.size();
for (const auto &[ss, entitySnapshot] : entity)
{
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.h b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.h
index 5135d41b89c14380d85b5af4f1e90323cf9feb70..aee783764f4b5720e4ae0d8d69fbf6faed297e98 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.h
+++ b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.h
@@ -27,8 +27,8 @@
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+#include <RobotAPI/libraries/armem/client.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
-#include <RobotAPI/libraries/armem/client/MemoryConnector.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem_robot_mapping/types.h>
@@ -59,11 +59,10 @@ namespace armarx::armem
*
* Detailed description of class ExampleClient.
*/
- class MappingDataReader :
- virtual public armarx::armem::MemoryConnector
+ class MappingDataReader
{
public:
- MappingDataReader(ManagedIceObject& component);
+ MappingDataReader(armem::ClientReaderComponentPluginUser& memoryClient);
virtual ~MappingDataReader();
@@ -102,10 +101,6 @@ namespace armarx::armem
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
- const std::string& getPropertyPrefix() const override
- {
- return propertyPrefix;
- }
client::query::Builder buildQuery(const Query& query) const ;
@@ -122,7 +117,9 @@ namespace armarx::armem
} properties;
- const std::string propertyPrefix = "mem.mapping.read.";
+ const std::string propertyPrefix = "mem.mapping.";
+
+ armem::ClientReaderComponentPluginUser& memoryClient;
};
} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.cpp b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.cpp
index 2216b159f0f18d336834e80f16b9c58c7d05948c..351d71398a6959f346aadf45c6c604edcd865a53 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.cpp
@@ -7,8 +7,8 @@
namespace armarx::armem
{
- MappingDataWriter::MappingDataWriter(ManagedIceObject& component)
- : MemoryConnector(component) {}
+ MappingDataWriter::MappingDataWriter(armem::ClientWriterComponentPluginUser& component)
+ : component(component) {}
MappingDataWriter::~MappingDataWriter() = default;
@@ -17,9 +17,7 @@ namespace armarx::armem
{
ARMARX_DEBUG << "TransformWriter: registerPropertyDefinitions";
- MemoryConnector::registerPropertyDefinitions(def);
-
- const std::string prefix = getPropertyPrefix();
+ const std::string prefix = propertyPrefix;
def->optional(properties.mappingMemoryName, prefix + "MappingMemoryName",
"Name of the mapping memory core segment to use.");
@@ -32,7 +30,7 @@ namespace armarx::armem
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "MappingDataWriter: Waiting for memory '" << properties.memoryName
<< "' ...";
- auto result = useMemory(properties.memoryName);
+ auto result = component.useMemory(properties.memoryName);
if (not result.success)
{
ARMARX_ERROR << result.errorMessage;
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.h b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.h
index 4eddff56c3dfa7b314fc844fff6a18c60bd45003..e5de996746b7d54b46faa891cf2cd26007fbaf02 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.h
+++ b/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.h
@@ -27,7 +27,7 @@
#include <RobotAPI/interface/units/LaserScannerUnit.h>
#include <RobotAPI/libraries/armem/client/Writer.h>
-#include <RobotAPI/libraries/armem/client/MemoryConnector.h>
+#include <RobotAPI/libraries/armem/client.h>
namespace armarx::armem
@@ -44,11 +44,10 @@ namespace armarx::armem
*
* Detailed description of class ExampleClient.
*/
- class MappingDataWriter :
- virtual public ::armarx::armem::MemoryConnector
+ class MappingDataWriter
{
public:
- MappingDataWriter(ManagedIceObject& component);
+ MappingDataWriter(armem::ClientWriterComponentPluginUser& component);
virtual ~MappingDataWriter();
void connect();
@@ -62,11 +61,6 @@ namespace armarx::armem
bool storeSensorData(const LaserScan& laserScan, const std::string& frame, const std::string& agentName, const std::int64_t& timestamp);
- const std::string& getPropertyPrefix() const override
- {
- return propertyPrefix;
- }
-
private:
armem::client::Writer memoryWriter;
@@ -80,7 +74,9 @@ namespace armarx::armem
std::mutex memoryWriterMutex;
- const std::string propertyPrefix = "mem.mapping.write.";
+ const std::string propertyPrefix = "mem.mapping.";
+
+ armem::ClientWriterComponentPluginUser& component;
};
diff --git a/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt b/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..2eead70ac6845aedad5d4f1e5c69e9c0d8522347
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
@@ -0,0 +1,80 @@
+set(LIB_NAME armem_robot_state)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+find_package(Eigen3 QUIET)
+armarx_build_if(Eigen3_FOUND "Eigen3 not available")
+
+armarx_add_library(
+ LIBS
+ # ArmarX
+ ArmarXCore
+ ArmarXCoreInterfaces
+ ArmarXGuiComponentPlugins
+ # This package
+ RobotAPICore
+ RobotAPIInterfaces
+ RobotAPI::armem
+ RobotAPI::armem_robot
+ # System / External
+ Eigen3::Eigen
+ aroncommon
+ HEADERS
+ ./common/localization/types.h
+ ./common/localization/TransformHelper.h
+
+ ./client/common/RobotReader.h
+ ./client/common/VirtualRobotReader.h
+
+ ./client/localization/interfaces.h
+ ./client/localization/TransformReader.h
+ ./client/localization/TransformWriter.h
+
+ ./server/common/Visu.h
+
+ ./server/localization/Segment.h
+ # ./server/localization/Visu.h
+
+ ./server/proprioception/Segment.h
+ # ./server/proprioception/Visu.h
+
+ ./server/description/Segment.h
+
+
+ ./aron_conversions.h
+ SOURCES
+ ./common/localization/TransformHelper.cpp
+
+ ./client/common/RobotReader.cpp
+ ./client/common/VirtualRobotReader.cpp
+
+
+ ./client/localization/TransformReader.cpp
+ ./client/localization/TransformWriter.cpp
+
+ ./server/common/Visu.cpp
+
+ ./server/localization/Segment.cpp
+ # ./server/localization/Visu.cpp
+
+ ./server/proprioception/Segment.cpp
+ # ./server/proprioception/Visu.cpp
+
+ ./server/description/Segment.cpp
+
+ ./aron_conversions.cpp
+)
+
+
+armarx_enable_aron_file_generation_for_target(
+ TARGET_NAME
+ "${LIB_NAME}"
+ ARON_FILES
+ aron/TransformHeader.xml
+ aron/Transform.xml
+ aron/JointState.xml
+)
+
+
+add_library(RobotAPI::armem_robot_state ALIAS armem_robot_state)
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron/JointState.xml b/source/RobotAPI/libraries/armem_robot_state/aron/JointState.xml
new file mode 100644
index 0000000000000000000000000000000000000000..c7b78a90cfa452579dfea76227833b66c5befbe4
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/aron/JointState.xml
@@ -0,0 +1,14 @@
+<!--This class contains the data structure for ObjectPose -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <GenerateTypes>
+ <Object name="armarx::armem::arondto::JointState">
+ <ObjectChild key='name'>
+ <String/>
+ </ObjectChild>
+ <ObjectChild key='position'>
+ <Float />
+ </ObjectChild>
+ </Object>
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/armem_robot_localization/aron/Transform.xml b/source/RobotAPI/libraries/armem_robot_state/aron/Transform.xml
similarity index 60%
rename from source/RobotAPI/libraries/armem_robot_localization/aron/Transform.xml
rename to source/RobotAPI/libraries/armem_robot_state/aron/Transform.xml
index afdd4fafdbd74b08c19bbb98cbc1dd326ee6244e..02ebe91b35094809953de6cfd0252c8a2048cf98 100644
--- a/source/RobotAPI/libraries/armem_robot_localization/aron/Transform.xml
+++ b/source/RobotAPI/libraries/armem_robot_state/aron/Transform.xml
@@ -3,15 +3,15 @@
<AronTypeDefinition>
<CodeIncludes>
<Include include="<Eigen/Core>" />
- <Include include="<RobotAPI/libraries/armem_robot_localization/aron/TransformHeader.aron.generated.h>" />
+ <Include include="<RobotAPI/libraries/armem_robot_state/aron/TransformHeader.aron.generated.h>" />
</CodeIncludes>
<AronIncludes>
- <Include include="<RobotAPI/libraries/armem_robot_localization/aron/TransformHeader.xml>" />
+ <Include include="<RobotAPI/libraries/armem_robot_state/aron/TransformHeader.xml>" />
</AronIncludes>
<GenerateTypes>
- <Object name="armarx::aron::Transform">
+ <Object name="armarx::armem::arondto::Transform">
<ObjectChild key='header'>
- <armarx::aron::TransformHeader />
+ <armarx::armem::arondto::TransformHeader />
</ObjectChild>
<ObjectChild key='transform'>
<Pose />
diff --git a/source/RobotAPI/libraries/armem_robot_localization/aron/TransformHeader.xml b/source/RobotAPI/libraries/armem_robot_state/aron/TransformHeader.xml
similarity index 91%
rename from source/RobotAPI/libraries/armem_robot_localization/aron/TransformHeader.xml
rename to source/RobotAPI/libraries/armem_robot_state/aron/TransformHeader.xml
index 85add64bec4441b3e203a2d798322629521ea4fe..c6bd5b94f93fa10e0638c75d17e8dbed0ad5bc60 100644
--- a/source/RobotAPI/libraries/armem_robot_localization/aron/TransformHeader.xml
+++ b/source/RobotAPI/libraries/armem_robot_state/aron/TransformHeader.xml
@@ -5,7 +5,7 @@
<Include include="<Eigen/Core>" />
</CodeIncludes>
<GenerateTypes>
- <Object name="armarx::aron::TransformHeader">
+ <Object name="armarx::armem::arondto::TransformHeader">
<ObjectChild key='parentFrame'>
<string />
</ObjectChild>
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..27fecee2e4f4ae9137a4e7f6281bef679d308da4
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
@@ -0,0 +1,66 @@
+#include "aron_conversions.h"
+
+// STL
+#include <string>
+
+// Ice
+#include <IceUtil/Time.h>
+
+// RobotAPI
+#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/JointState.aron.generated.h>
+
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+
+#include "RobotAPI/libraries/armem_robot_state/types.h"
+
+namespace armarx::armem
+{
+
+ /* Transform */
+
+ void fromAron(const arondto::Transform& dto, robot_state::Transform& bo)
+ {
+ fromAron(dto.header, bo.header);
+ aron::fromAron(dto.transform, bo.transform);
+ }
+
+ void toAron(arondto::Transform& dto, const robot_state::Transform& bo)
+ {
+ toAron(dto.header, bo.header);
+ aron::toAron(dto.transform, bo.transform);
+ }
+
+ /* TransformHeader */
+
+ void toAron(arondto::TransformHeader& dto, const robot_state::TransformHeader& bo)
+ {
+ aron::toAron(dto.parentFrame, bo.parentFrame);
+ aron::toAron(dto.frame, bo.frame);
+ aron::toAron(dto.agent, bo.agent);
+ armarx::toAron(dto.timestamp, bo.timestamp);
+ }
+
+ void fromAron(const arondto::TransformHeader& dto, robot_state::TransformHeader& bo)
+ {
+ aron::fromAron(dto.parentFrame, bo.parentFrame);
+ aron::fromAron(dto.frame, bo.frame);
+ aron::fromAron(dto.agent, bo.agent);
+ armarx::fromAron(dto.timestamp, bo.timestamp);
+ }
+
+ /* JointState */
+
+ void fromAron(const arondto::JointState& dto, robot_state::JointState& bo)
+ {
+ aron::fromAron(dto.name, bo.name);
+ aron::fromAron(dto.position, bo.position);
+ }
+
+ void toAron(arondto::JointState& dto, const robot_state::JointState& bo)
+ {
+ aron::toAron(dto.name, bo.name);
+ aron::toAron(dto.position, bo.position);
+ }
+
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..202a81f089702e07155232200911fa0e0afa197d
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
@@ -0,0 +1,53 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+namespace armarx::armem
+{
+ namespace robot_state
+ {
+ struct Transform;
+ struct TransformHeader;
+
+ struct JointState;
+
+ } // namespace robot_state
+
+ namespace arondto
+ {
+ struct Transform;
+ struct TransformHeader;
+
+ struct JointState;
+
+ } // namespace arondto
+
+ void fromAron(const arondto::Transform& dto, robot_state::Transform& bo);
+ void toAron(arondto::Transform& dto, const robot_state::Transform& bo);
+
+ void fromAron(const arondto::TransformHeader& dto, robot_state::TransformHeader& bo);
+ void toAron(arondto::TransformHeader& dto, const robot_state::TransformHeader& bo);
+
+ void fromAron(const arondto::JointState& dto, robot_state::JointState& bo);
+ void toAron(arondto::JointState& dto, const robot_state::JointState& bo);
+
+} // namespace armarx::armem
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5253659423cb5b30d57a18ad8738ae8b5ba79597
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
@@ -0,0 +1,300 @@
+#include "RobotReader.h"
+
+#include <mutex>
+#include <optional>
+
+#include "ArmarXCore/core/exceptions/LocalException.h"
+#include "ArmarXCore/core/logging/Logging.h"
+#include <ArmarXCore/core/PackagePath.h>
+
+#include "RobotAPI/libraries/armem/core/Time.h"
+#include "RobotAPI/libraries/armem/client/query/Builder.h"
+#include "RobotAPI/libraries/armem_robot/robot_conversions.h"
+#include "RobotAPI/libraries/armem_robot/aron_conversions.h"
+#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+
+
+namespace fs = ::std::filesystem;
+
+namespace armarx::armem::robot_state
+{
+
+ RobotReader::RobotReader(armem::ClientReaderComponentPluginUser& component) : memoryClient(component), transformReader(component) {}
+
+ void RobotReader::registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def)
+ {
+ transformReader.registerPropertyDefinitions(def);
+
+ def->optional(properties.memoryName, propertyPrefix + "Memory");
+ def->optional(properties.descriptionCoreSegment, propertyPrefix + "descriptionSegment");
+ def->optional(properties.localizationCoreSegment, propertyPrefix + "localizationSegment");
+ def->optional(properties.proprioceptionCoreSegment, propertyPrefix + "proprioceptionSegment");
+ }
+
+ void RobotReader::connect()
+ {
+ transformReader.connect();
+
+ // Wait for the memory to become available and add it as dependency.
+ ARMARX_IMPORTANT << "RobotReader: Waiting for memory '" << properties.memoryName
+ << "' ...";
+ auto result = memoryClient.useMemory(properties.memoryName);
+ if (not result.success)
+ {
+ ARMARX_ERROR << result.errorMessage;
+ return;
+ }
+
+ ARMARX_IMPORTANT << "RobotReader: Connected to memory '" << properties.memoryName;
+
+ memoryReader.setReadingMemory(result.proxy);
+ }
+
+ std::optional<robot::Robot> RobotReader::get(const std::string& name, const armem::Time& timestamp)
+ {
+ const auto description = queryDescription(name, timestamp);
+
+ if (not description)
+ {
+ ARMARX_ERROR << "Unknown object " << name;
+ return std::nullopt;
+ }
+
+ return get(*description, timestamp);
+ }
+
+
+ robot::Robot RobotReader::get(const robot::RobotDescription& description,
+ const armem::Time& timestamp)
+ {
+ robot::Robot robot
+ {
+ .description = description,
+ .instance = "", // TODO(fabian.reister):
+ .config = {}, // will be populated by synchronize
+ .timestamp = timestamp
+ };
+
+ synchronize(robot, timestamp);
+
+ return robot;
+ }
+
+ bool RobotReader::synchronize(robot::Robot& obj, const armem::Time& timestamp)
+ {
+ auto state = queryState(obj.description, timestamp);
+
+ if (not state) /* c++20 [[unlikely]] */
+ {
+ ARMARX_WARNING << "Could not synchronize object " << obj.description.name;
+ return false;
+ }
+
+ obj.config = std::move(*state);
+ return true;
+ }
+
+ std::optional<robot::RobotDescription> RobotReader::queryDescription(const std::string& name, const armem::Time& timestamp)
+ {
+ // Query all entities from provider.
+ armem::client::query::Builder qb;
+
+ // clang-format off
+ qb
+ .coreSegments().all() // withName(properties.descriptionCoreSegment)
+ .providerSegments().all() // .withName(name)
+ .entities().all()
+ .snapshots().latest(); // TODO(fabian.reister): atTime(timestamp);
+ // clang-format on
+
+ ARMARX_INFO << "Lookup query in reader";
+
+ if (not memoryReader)
+ {
+ ARMARX_WARNING << "Memory reader is null";
+ return std::nullopt;
+ }
+
+ try
+ {
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+ ARMARX_INFO << "Lookup result in reader: " << qResult;
+
+ if (not qResult.success) /* c++20 [[unlikely]] */
+ {
+ return std::nullopt;
+ }
+
+ return getRobotDescription(qResult.memory, name);
+
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << "query description failure" << GetHandledExceptionString();
+ }
+
+ return std::nullopt;
+
+ }
+
+ std::optional<robot::RobotState> RobotReader::queryState(const robot::RobotDescription& description, const armem::Time& timestamp)
+ {
+ const auto jointMap = queryJointState(description, timestamp);
+ if (not jointMap)
+ {
+ return std::nullopt;
+ }
+
+ const auto globalPose = queryGlobalPose(description, timestamp);
+ if (not globalPose)
+ {
+ return std::nullopt;
+ }
+
+ return robot::RobotState
+ {
+ .timestamp = timestamp,
+ .globalPose = *globalPose,
+ .jointMap = *jointMap
+ };
+ }
+
+ std::optional<robot::RobotState::JointMap> RobotReader::queryJointState(const robot::RobotDescription& description, const armem::Time& timestamp) const
+ {
+ // TODO(fabian.reister): how to deal with multiple providers?
+
+ // Query all entities from provider.
+ armem::client::query::Builder qb;
+
+ // clang-format off
+ qb
+ .coreSegments().withName(properties.proprioceptionCoreSegment)
+ .providerSegments().withName(description.name) // agent
+ .entities().all() // TODO
+ .snapshots().latest();
+ // clang-format on
+
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+
+ if (not qResult.success) /* c++20 [[unlikely]] */
+ {
+ return std::nullopt;
+ }
+
+ return getRobotJointState(qResult.memory, description.name);
+ }
+
+ std::optional<robot::RobotState::Pose> RobotReader::queryGlobalPose(const robot::RobotDescription& description, const armem::Time& timestamp) const
+ {
+ const auto result = transformReader.getGlobalPose(description.name, "root", timestamp);
+ if (not result)
+ {
+ return std::nullopt;
+ }
+
+ return result.transform.transform;
+ }
+
+
+ std::optional<robot::RobotState> RobotReader::getRobotState(const armarx::armem::wm::Memory& memory, const std::string& name) const
+ {
+ // clang-format off
+ const armem::wm::ProviderSegment& providerSegment = memory
+ .getCoreSegment(properties.proprioceptionCoreSegment)
+ .getProviderSegment(name);
+ // clang-format on
+ const auto entities = simox::alg::get_values(providerSegment.entities());
+
+ // TODO entitiesToRobotState()
+
+ if (entities.empty())
+ {
+ ARMARX_WARNING << "No entity found";
+ return std::nullopt;
+ }
+
+ const auto entitySnapshots = simox::alg::get_values(entities.front().history());
+
+ if (entitySnapshots.empty())
+ {
+ ARMARX_WARNING << "No entity snapshots found";
+ return std::nullopt;
+ }
+
+ // TODO(fabian.reister): check if 0 available
+ const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
+
+ // Here, we access the RobotUnit streaming data stored in the proprioception segment.
+ return robot::convertRobotState(instance);
+ }
+
+
+ std::optional<robot::RobotState::JointMap> RobotReader::getRobotJointState(const armarx::armem::wm::Memory& memory, const std::string& name) const
+ {
+ // // clang-format off
+ // const armem::wm::ProviderSegment& providerSegment = memory
+ // .getCoreSegment(properties.proprioceptionCoreSegment)
+ // .getProviderSegment(name);
+ // // clang-format on
+ // const auto entities = simox::alg::get_values(providerSegment.entities());
+
+ // // TODO entitiesToRobotState()
+
+ // if (entities.empty())
+ // {
+ // ARMARX_WARNING << "No entity found";
+ // return std::nullopt;
+ // }
+
+ // const auto entitySnapshots = simox::alg::get_values(entities.front().history());
+
+ // if (entitySnapshots.empty())
+ // {
+ // ARMARX_WARNING << "No entity snapshots found";
+ // return std::nullopt;
+ // }
+
+ // // TODO(fabian.reister): check if 0 available
+ // const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
+
+ // // Here, we access the RobotUnit streaming data stored in the proprioception segment.
+ // return robot::convertRobotState(instance);
+
+ return std::nullopt; // TODO implement
+ }
+
+
+
+ std::optional<robot::RobotDescription> RobotReader::getRobotDescription(const armarx::armem::wm::Memory& memory, const std::string& name) const
+ {
+ // clang-format off
+ const armem::wm::ProviderSegment& providerSegment = memory
+ .getCoreSegment(properties.descriptionCoreSegment)
+ .getProviderSegment(name); // TODO(fabian.reister): all
+ // clang-format on
+ const auto entities = simox::alg::get_values(providerSegment.entities());
+
+ if (entities.empty())
+ {
+ ARMARX_WARNING << "No entity found";
+ return std::nullopt;
+ }
+
+ const auto entitySnapshots = simox::alg::get_values(entities.front().history());
+
+ if (entitySnapshots.empty())
+ {
+ ARMARX_WARNING << "No entity snapshots found";
+ return std::nullopt;
+ }
+
+ // TODO(fabian.reister): check if 0 available
+ const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
+
+ return robot::convertRobotDescription(instance);
+ }
+
+} // namespace armarx::armem::robot_state
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
new file mode 100644
index 0000000000000000000000000000000000000000..b5bd01e5d182a88cc6d36e8c8112e1683c017d16
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
@@ -0,0 +1,100 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+#include <optional>
+
+#include "RobotAPI/libraries/armem/client.h"
+#include "RobotAPI/libraries/armem/client/Reader.h"
+
+#include "RobotAPI/libraries/armem_robot/client/interfaces.h"
+#include "RobotAPI/libraries/armem_robot/types.h"
+
+#include "RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h"
+
+namespace armarx::armem::robot_state
+{
+ /**
+ * @brief The RobotReader class.
+ *
+ * The purpose of this class is to synchronize the armem data structure armem::Robot
+ * with the memory.
+ */
+ class RobotReader : virtual public robot::ReaderInterface
+ {
+ public:
+ RobotReader(armem::ClientReaderComponentPluginUser& component);
+ virtual ~RobotReader() = default;
+
+ void connect();
+
+ void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def);
+
+ bool synchronize(robot::Robot& obj, const armem::Time& timestamp) override;
+
+ std::optional<robot::Robot> get(const std::string& name,
+ const armem::Time& timestamp) override;
+ robot::Robot get(const robot::RobotDescription& description,
+ const armem::Time& timestamp) override;
+
+ std::optional<robot::RobotDescription> queryDescription(const std::string& name,
+ const armem::Time& timestamp);
+
+ std::optional<robot::RobotState> queryState(const robot::RobotDescription& description,
+ const armem::Time& timestamp);
+
+ std::optional<robot::RobotState::JointMap>
+ queryJointState(const robot::RobotDescription& description,
+ const armem::Time& timestamp) const;
+
+ std::optional<robot::RobotState::Pose>
+ queryGlobalPose(const robot::RobotDescription& description,
+ const armem::Time& timestamp) const;
+
+ private:
+ std::optional<robot::RobotState> getRobotState(const armarx::armem::wm::Memory& memory,
+ const std::string& name) const;
+ std::optional<robot::RobotDescription>
+ getRobotDescription(const armarx::armem::wm::Memory& memory, const std::string& name) const;
+ std::optional<robot::RobotState::JointMap>
+ getRobotJointState(const armarx::armem::wm::Memory& memory, const std::string& name) const;
+
+ struct Properties
+ {
+ std::string memoryName = "RobotStateMemory";
+ std::string descriptionCoreSegment = "Description";
+ std::string localizationCoreSegment = "Localization";
+ std::string proprioceptionCoreSegment = "Proprioception";
+ } properties;
+
+ const std::string propertyPrefix = "mem.robot_state.";
+
+ armem::client::Reader memoryReader;
+ std::mutex memoryWriterMutex;
+
+ armem::ClientReaderComponentPluginUser& memoryClient;
+
+ client::robot_state::localization::TransformReader transformReader;
+ };
+
+} // namespace armarx::armem::robot_state
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..97633b788dcfae4cd93d6a1fd7a72850a9a009b1
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
@@ -0,0 +1,73 @@
+#include "VirtualRobotReader.h"
+
+#include <optional>
+
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/XML/RobotIO.h>
+
+#include "ArmarXCore/core/PackagePath.h"
+#include "ArmarXCore/core/system/ArmarXDataPath.h"
+#include "ArmarXCore/core/system/cmake/CMakePackageFinder.h"
+
+
+namespace armarx::armem::robot_state
+{
+
+ VirtualRobotReader::VirtualRobotReader(armem::ClientReaderComponentPluginUser& component) :
+ RobotReader(component)
+ {
+ }
+
+ void VirtualRobotReader::connect()
+ {
+ RobotReader::connect();
+ }
+
+ // TODO(fabian.reister): register property defs
+ void VirtualRobotReader::registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def)
+ {
+ RobotReader::registerPropertyDefinitions(def);
+ }
+
+ 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 auto robotState = queryState(robotDescription, timestamp);
+ if (not robotState)
+ {
+ return false;
+ }
+
+ robot.setJointValues(robotState->jointMap);
+ robot.setGlobalPose(robotState->globalPose.matrix());
+
+ return true;
+ }
+
+ 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);
+
+ if (not description)
+ {
+ return nullptr;
+ }
+
+ const std::string xmlFilename = ArmarXDataPath::resolvePath(description->xml.serialize().path);
+ ARMARX_INFO << "Loading (virtual) robot '" << description->name << "' from XML file '" << xmlFilename << "'";
+
+ return VirtualRobot::RobotIO::loadRobot(xmlFilename, loadMode);
+
+ }
+
+
+} // namespace armarx::armem::robot_state
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
new file mode 100644
index 0000000000000000000000000000000000000000..5958bbca18cb618210b67b72b970bf0435d24bd9
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
@@ -0,0 +1,58 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include "RobotReader.h"
+
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/VirtualRobot.h>
+#include <VirtualRobot/XML/RobotIO.h>
+
+namespace armarx::armem::robot_state
+{
+ /**
+ * @brief The VirtualRobotReader class.
+ *
+ * The aim of this class is to obtain a virtual robot instance and synchronize it
+ * with the data (joint positions, global pose, ...) stored in the working memory.
+ *
+ * This is only a lightweight wrapper of @see RobotReader for Simox's VirtualRobot class.
+ */
+ class VirtualRobotReader : virtual public RobotReader
+ {
+ public:
+ VirtualRobotReader(armem::ClientReaderComponentPluginUser& component);
+ virtual ~VirtualRobotReader() = default;
+
+ void connect();
+ void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def);
+
+ bool synchronizeRobot(VirtualRobot::Robot& robot, const armem::Time& timestamp);
+
+ VirtualRobot::RobotPtr
+ getRobot(const std::string& name,
+ const armem::Time& timestamp,
+ const VirtualRobot::RobotIO::RobotDescription& loadMode =
+ VirtualRobot::RobotIO::RobotDescription::eStructure);
+ };
+
+} // namespace armarx::armem::robot_state
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8a37b2dec14cfc6721b7458fce984dac59bdfb21
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
@@ -0,0 +1,173 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "TransformReader.h"
+#include "RobotAPI/libraries/armem/core/Time.h"
+#include "RobotAPI/libraries/armem_robot_state/common/localization/types.h"
+
+#include <algorithm>
+#include <iterator>
+#include <numeric>
+
+// Ice
+#include <IceUtil/Time.h>
+
+// Eigen
+#include <Eigen/Geometry>
+
+// Simox
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <SimoxUtility/algorithm/string/string_tools.h>
+#include <SimoxUtility/color/cmaps.h>
+#include <SimoxUtility/math/pose/interpolate.h>
+
+// ArmarX
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/time/CycleUtil.h>
+
+// this package
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/query/query_fns.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
+#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
+#include <RobotAPI/libraries/aron/core/navigator/type/NavigatorFactory.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
+
+#include <RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h>
+
+namespace armarx::armem::client::robot_state::localization
+{
+
+ TransformReader::TransformReader(armem::ClientReaderComponentPluginUser& memoryClient) : memoryClient(memoryClient) {}
+
+ TransformReader::~TransformReader() = default;
+
+ void TransformReader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ {
+ ARMARX_DEBUG << "TransformReader: registerPropertyDefinitions";
+
+ const std::string prefix = propertyPrefix;
+
+ def->optional(properties.localizationSegment,
+ prefix + "localizationSegment",
+ "Name of the localization memory core segment to use.");
+
+ def->optional(properties.memoryName, prefix + "Memory");
+ }
+
+ void TransformReader::connect()
+ {
+ // Wait for the memory to become available and add it as dependency.
+ ARMARX_IMPORTANT << "TransformReader: Waiting for memory '" << properties.memoryName
+ << "' ...";
+ auto result = memoryClient.useMemory(properties.memoryName);
+ if (not result.success)
+ {
+ ARMARX_ERROR << result.errorMessage;
+ return;
+ }
+
+ ARMARX_IMPORTANT << "TransformReader: Connected to memory '" << properties.memoryName;
+
+ memoryReader.setReadingMemory(result.proxy);
+ }
+
+ TransformResult TransformReader::getGlobalPose(const std::string& agentName,
+ const std::string& robotRootFrame,
+ const armem::Time& timestamp) const
+ {
+ const TransformQuery query{.header = {.parentFrame = GlobalFrame,
+ .frame = robotRootFrame,
+ .agent = agentName,
+ .timestamp = timestamp
+ }};
+
+ return lookupTransform(query);
+ }
+
+ // std::vector<std::string> buildTransformChainArbitrary(const
+ // std::map<std::string, armem::ProviderSegment>& providerSegments, const
+ // std::string& parentFrame, const std::string& frame){
+
+ // std::map<std::string, std::string> tf;
+
+ // // build string representation of transforms
+ // for(const auto& [name, segment]: providerSegments){
+ // const auto frames = simox::alg::split(name, ",");
+ // if (frames.size() != 2){
+ // ARMARX_WARNING << "Segment name " << name << " not understood";
+ // continue;
+ // }
+
+ // tf.insert(frame[0], frame[1]);
+ // }
+
+ // // find
+
+ // }
+
+
+ TransformResult TransformReader::lookupTransform(const TransformQuery& query) const
+ {
+ const auto& timestamp = query.header.timestamp;
+
+ const auto durationEpsilon = IceUtil::Time::milliSeconds(-1);
+
+ ARMARX_DEBUG << "Looking up transform at timestamp " << timestamp;
+
+ // Query all entities from provider.
+ armem::client::query::Builder qb;
+
+ // clang-format off
+ qb
+ .coreSegments().withName(properties.localizationSegment)
+ .providerSegments().withName(query.header.agent) // agent
+ .entities().all() // parentFrame,frame
+ .snapshots().latest();
+ // clang-format on
+
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+
+ if (not qResult.success)
+ {
+ return {.transform =
+ {
+ .header = query.header,
+ },
+ .status = TransformResult::Status::ErrorFrameNotAvailable,
+ .errorMessage = "Error in tf lookup '" + query.header.parentFrame + " -> " +
+ query.header.frame + "' : " + qResult.errorMessage};
+ }
+
+ const auto& localizationCoreSegment = qResult.memory.getCoreSegment(properties.localizationSegment);
+
+ using armarx::armem::common::robot_state::localization::TransformHelper;
+ return TransformHelper::lookupTransform(localizationCoreSegment, query);
+ }
+
+} // namespace armarx::armem::client::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_robot_localization/TransformReader.h b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
similarity index 58%
rename from source/RobotAPI/libraries/armem_robot_localization/TransformReader.h
rename to source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
index 4107ed61fcc108737049f6fb063929012575a898..2d1d98ddf82c5c137b934b3be91079bb87298503 100644
--- a/source/RobotAPI/libraries/armem_robot_localization/TransformReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
@@ -23,29 +23,29 @@
#pragma once
#include <RobotAPI/libraries/armem/client/Reader.h>
-#include <RobotAPI/libraries/armem/client/MemoryConnector.h>
-#include "TransformInterfaces.h"
+#include <RobotAPI/libraries/armem/client.h>
-namespace armarx::armem
+#include "interfaces.h"
+
+namespace armarx::armem::client::robot_state::localization
{
/**
* @defgroup Component-ExampleClient ExampleClient
* @ingroup RobotAPI-Components
* A description of the component ExampleClient.
*
- * @class ExampleClient
+ * @class TransformReader
* @ingroup Component-ExampleClient
* @brief Brief description of class ExampleClient.
*
* Detailed description of class ExampleClient.
*/
class TransformReader :
- virtual public ::armarx::armem::TransformReaderInterface,
- virtual public ::armarx::armem::MemoryConnector
+ virtual public TransformReaderInterface
{
public:
- TransformReader(ManagedIceObject& component);
+ TransformReader(armem::ClientReaderComponentPluginUser& memoryClient);
~TransformReader() override;
@@ -53,37 +53,27 @@ namespace armarx::armem
TransformResult getGlobalPose(const std::string& agentName,
const std::string& robotRootFrame,
- const std::int64_t& timestamp) const override;
+ const armem::Time& timestamp) const override;
+
TransformResult lookupTransform(const TransformQuery& query) const override;
void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def) override;
- const std::string& getPropertyPrefix() const override
- {
- return propertyPrefix;
- }
-
private:
- std::vector<std::string> buildTransformChain(const armem::wm::Memory& memory,
- const TransformQuery& query) const;
-
- std::vector<Eigen::Affine3f> obtainTransforms(const armem::wm::Memory& memory,
- const std::vector<std::string>& tfChain,
- const std::string& agent, const std::int64_t& timestamp) const;
-
- Eigen::Affine3f obtainTransform(const std::string& entityName, const armem::wm::ProviderSegment& agentProviderSegment, int64_t timestamp) const;
-
armem::client::Reader memoryReader;
// Properties
struct Properties
{
- std::string memoryName = "RobotState";
- std::string localizationMemoryName = "Localization";
+ std::string memoryName = "RobotStateMemory";
+ std::string localizationSegment = "Localization";
} properties;
- const std::string propertyPrefix = "mem.localization.read.";
+ const std::string propertyPrefix = "mem.robot_state.";
+
+ armem::ClientReaderComponentPluginUser& memoryClient;
+
};
-} // namespace armarx::armem
+} // namespace armarx::armem::client::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_robot_localization/TransformWriter.cpp b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp
similarity index 61%
rename from source/RobotAPI/libraries/armem_robot_localization/TransformWriter.cpp
rename to source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp
index 7a9bcd1f4825c6d775201fb862e5a21f0af70cbf..2327c141840bc74f8b43ab14df03b03f2b98ccdf 100644
--- a/source/RobotAPI/libraries/armem_robot_localization/TransformWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp
@@ -13,7 +13,6 @@
* 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::
* @author Fabian Reister ( fabian dot reister at kit dot edu )
* @date 2021
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
@@ -22,6 +21,7 @@
#include "TransformWriter.h"
+#include <optional>
#include <algorithm>
#include <iterator>
#include <numeric>
@@ -44,15 +44,16 @@
#include <RobotAPI/libraries/aron/core/navigator/type/NavigatorFactory.h>
#include <RobotAPI/libraries/core/FramedPose.h>
-#include <RobotAPI/libraries/armem_robot_localization/aron/Transform.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
+#include "RobotAPI/libraries/armem/core/MemoryID.h"
-#include "aron_conversions.h"
-namespace armarx::armem
+namespace armarx::armem::client::robot_state::localization
{
- TransformWriter::TransformWriter(ManagedIceObject& component) : MemoryConnector(component) {}
+ TransformWriter::TransformWriter(armem::ClientWriterComponentPluginUser& memoryClient) : memoryClient(memoryClient) {}
TransformWriter::~TransformWriter() = default;
@@ -60,15 +61,11 @@ namespace armarx::armem
{
ARMARX_DEBUG << "TransformWriter: registerPropertyDefinitions";
- MemoryConnector::registerPropertyDefinitions(def);
-
- const std::string prefix = getPropertyPrefix();
-
- def->optional(properties.localizationMemoryName,
- prefix + "LocalizationMemoryName",
+ def->optional(properties.localizationSegment,
+ propertyPrefix + "localizationSegment",
"Name of the localization memory core segment to use.");
- def->optional(properties.memoryName, prefix + "MemoryName");
+ def->optional(properties.memoryName, propertyPrefix + "Memory");
}
void TransformWriter::connect()
@@ -76,7 +73,7 @@ namespace armarx::armem
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "TransformWriter: Waiting for memory '" << properties.memoryName
<< "' ...";
- auto result = useMemory(properties.memoryName);
+ auto result = memoryClient.useMemory(properties.memoryName);
if (not result.success)
{
ARMARX_ERROR << result.errorMessage;
@@ -88,39 +85,56 @@ namespace armarx::armem
memoryWriter.setWritingMemory(result.proxy);
}
- bool TransformWriter::commitTransform(const Transform& transform)
+ bool TransformWriter::commitTransform(const ::armarx::armem::robot_state::Transform& transform)
{
std::lock_guard g{memoryWriterMutex};
ARMARX_DEBUG << "Trying to create core segment + provider segment";
- const auto result =
- memoryWriter.addSegment(properties.localizationMemoryName, transform.header.agent);
-
- if (not result.success)
+ const auto providerId = [&]() -> std::optional<armem::MemoryID>
+ {
+ try
+ {
+ const auto result =
+ memoryWriter.addSegment(properties.localizationSegment, transform.header.agent);
+
+ if (not result.success)
+ {
+ ARMARX_WARNING << "Could not obtain provider id! Reason: " << result.errorMessage;
+ return std::nullopt;
+ }
+
+ return armem::MemoryID(result.segmentID);
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << "Could not obtain provider id!";
+ return std::nullopt;
+ }
+ }();
+
+ if (not providerId)
{
- ARMARX_ERROR << result.errorMessage;
-
- // TODO(fabian.reister): message
return false;
}
- const auto timestamp = IceUtil::Time::microSeconds(transform.header.timestamp);
+ // const auto& timestamp = transform.header.timestamp;
+ const auto timestamp = IceUtil::Time::now(); // FIXME remove
+
- const auto providerId = armem::MemoryID(result.segmentID);
- const auto entityID = providerId.withEntityName(transform.header.parentFrame + "," +
+ const auto entityID = providerId->withEntityName(transform.header.parentFrame + "," +
transform.header.frame).withTimestamp(timestamp);
armem::EntityUpdate update;
update.entityID = entityID;
- aron::Transform aronTransform;
- toAron(transform, aronTransform);
+ arondto::Transform aronTransform;
+ toAron(aronTransform, transform);
update.instancesData = {aronTransform.toAron()};
- update.timeCreated = IceUtil::Time::microSeconds(aronTransform.header.timestamp);
+ update.timeCreated = timestamp;
- ARMARX_DEBUG << "Committing " << update << " at time " << IceUtil::Time::microSeconds(transform.header.timestamp);
+ ARMARX_DEBUG << "Committing " << update << " at time " << transform.header.timestamp;
armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
ARMARX_DEBUG << updateResult;
@@ -133,10 +147,10 @@ namespace armarx::armem
return updateResult.success;
}
- const std::string& TransformWriter::getPropertyPrefix() const
- {
- return propertyPrefix;
- }
+ // const std::string& TransformWriter::getPropertyPrefix() const
+ // {
+ // return propertyPrefix;
+ // }
-} // namespace armarx::armem
+} // namespace armarx::armem::client::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_robot_localization/TransformWriter.h b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h
similarity index 69%
rename from source/RobotAPI/libraries/armem_robot_localization/TransformWriter.h
rename to source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h
index 7d1ee2acbac7739ea73f92ca2a5205068cf0e5fa..aa9e295bf19b34229697ad43725884469c9d1172 100644
--- a/source/RobotAPI/libraries/armem_robot_localization/TransformWriter.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h
@@ -25,11 +25,13 @@
#include <mutex>
#include <RobotAPI/libraries/armem/client/Writer.h>
-#include <RobotAPI/libraries/armem/client/MemoryConnector.h>
+// #include <RobotAPI/libraries/armem/client/MemoryConnector.h>
-#include "TransformInterfaces.h"
+#include <RobotAPI/libraries/armem/client.h>
-namespace armarx::armem
+#include "interfaces.h"
+
+namespace armarx::armem::client::robot_state::localization
{
/**
@@ -44,11 +46,11 @@ namespace armarx::armem
* Detailed description of class ExampleClient.
*/
class TransformWriter :
- virtual public ::armarx::armem::TransformWriterInterface,
- virtual public ::armarx::armem::MemoryConnector
+ virtual public TransformWriterInterface
+ // virtual public ::armarx::armem::MemoryConnector
{
public:
- TransformWriter(ManagedIceObject& component);
+ TransformWriter(armem::ClientWriterComponentPluginUser& memoryClient);
~TransformWriter() override;
@@ -59,22 +61,22 @@ namespace armarx::armem
/// to be called in Component::addPropertyDefinitions
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def) override;
- bool commitTransform(const Transform& transform) override;
-
- const std::string& getPropertyPrefix() const override;
+ bool commitTransform(const ::armarx::armem::robot_state::Transform& transform) override;
private:
armem::client::Writer memoryWriter;
+ std::mutex memoryWriterMutex;
// Properties
struct Properties
{
- std::string memoryName = "RobotState";
- std::string localizationMemoryName = "Localization";
+ std::string memoryName = "RobotStateMemory";
+ std::string localizationSegment = "Localization";
} properties;
- std::mutex memoryWriterMutex;
+ const std::string propertyPrefix = "mem.robot_state.";
- const std::string propertyPrefix = "mem.localization.write.";
+ armem::ClientWriterComponentPluginUser& memoryClient;
};
-} // namespace armarx::armem
+
+} // namespace armarx::armem::client::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/interfaces.h b/source/RobotAPI/libraries/armem_robot_state/client/localization/interfaces.h
new file mode 100644
index 0000000000000000000000000000000000000000..ae35822c6080701195c0e6d7d8b6c8c4ad282684
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/interfaces.h
@@ -0,0 +1,68 @@
+/*
+ * 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::
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <Eigen/Geometry>
+
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+
+#include <RobotAPI/libraries/armem_robot_state/common/localization/types.h>
+#include <RobotAPI/libraries/armem_robot_state/types.h>
+
+namespace armarx::armem::client::robot_state::localization
+{
+
+ using armarx::armem::common::robot_state::localization::TransformQuery;
+ using armarx::armem::common::robot_state::localization::TransformResult;
+
+ class TransformInterface
+ {
+ public:
+ virtual ~TransformInterface() = default;
+
+ virtual void registerPropertyDefinitions(PropertyDefinitionsPtr& def) = 0;
+ virtual void connect() = 0;
+ };
+
+ class TransformReaderInterface : virtual public TransformInterface
+ {
+ public:
+ virtual ~TransformReaderInterface() = default;
+
+ virtual TransformResult getGlobalPose(const std::string& agentName,
+ const std::string& robotRootFrame,
+ const armem::Time& timestamp) const = 0;
+
+ virtual TransformResult lookupTransform(const TransformQuery& query) const = 0;
+ // waitForTransform()
+ };
+
+ class TransformWriterInterface : virtual public TransformInterface
+ {
+ public:
+ ~TransformWriterInterface() override = default;
+
+ virtual bool commitTransform(const ::armarx::armem::robot_state::Transform& transform) = 0;
+ };
+
+} // namespace armarx::armem::client::robot_state::localization
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c8db83835ccc53f21e416b723745892a1455f801
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
@@ -0,0 +1,322 @@
+#include "TransformHelper.h"
+
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <SimoxUtility/algorithm/string/string_tools.h>
+#include <SimoxUtility/color/cmaps.h>
+#include <SimoxUtility/math/pose/interpolate.h>
+
+#include "ArmarXCore/core/exceptions/LocalException.h"
+#include "RobotAPI/libraries/core/FramedPose.h"
+
+#include "RobotAPI/libraries/aron/common/aron_conversions.h"
+
+#include "RobotAPI/libraries/armem/core/error/ArMemError.h"
+#include "RobotAPI/libraries/armem/core/workingmemory/EntityInstance.h"
+#include "RobotAPI/libraries/armem/core/workingmemory/Memory.h"
+
+#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
+
+namespace armarx::armem::common::robot_state::localization
+{
+
+ TransformResult TransformHelper::lookupTransform(const armem::wm::CoreSegment& localizationCoreSegment,
+ const TransformQuery& query)
+ {
+ const std::vector<std::string> tfChain =
+ buildTransformChain(localizationCoreSegment, query);
+
+ if (tfChain.empty())
+ {
+ return {.transform = {.header = query.header},
+ .status = TransformResult::Status::ErrorFrameNotAvailable,
+ .errorMessage = "Cannot create tf lookup chain '" +
+ query.header.parentFrame + " -> " + query.header.frame +
+ "'"};
+ }
+
+ const std::vector<Eigen::Affine3f> transforms = obtainTransforms(
+ localizationCoreSegment, tfChain, query.header.agent, query.header.timestamp);
+
+ if (transforms.empty())
+ {
+ ARMARX_WARNING << deactivateSpam(1) << "No transform available.";
+ return {.transform = {.header = query.header},
+ .status = TransformResult::Status::ErrorFrameNotAvailable,
+ .errorMessage = "Error in TF loookup: '" + query.header.parentFrame +
+ " -> " + query.header.frame +
+ "'. No memory data in time range."};
+ }
+
+ const Eigen::Affine3f transform = std::accumulate(transforms.begin(),
+ transforms.end(),
+ Eigen::Affine3f::Identity(),
+ std::multiplies<>());
+
+ ARMARX_DEBUG << "Found valid transform";
+
+ return {.transform = {.header = query.header, .transform = transform},
+ .status = TransformResult::Status::Success};
+ }
+
+ std::vector<std::string>
+ TransformHelper::buildTransformChain(const armem::wm::CoreSegment& localizationCoreSegment,
+ const TransformQuery& query)
+ {
+ ARMARX_DEBUG << "Building transform chain";
+
+ auto join = [](const std::string & parentFrame, const std::string & frame)
+ {
+ return parentFrame + "," + frame;
+ };
+
+ std::vector<std::string> chain;
+
+ const auto& agentProviderSegment =
+ localizationCoreSegment.getProviderSegment(query.header.agent);
+
+ auto addToChain = [&](const std::string & parentFrame, const std::string & frame)
+ {
+ const auto entityName = join(parentFrame, frame);
+
+ if (agentProviderSegment.hasEntity(entityName))
+ {
+ chain.push_back(entityName);
+ }
+ else
+ {
+ ARMARX_WARNING << deactivateSpam(1) << "Cannot perform tf lookup '" << parentFrame << " -> " << frame
+ << "'";
+ }
+ };
+
+ std::array<std::string, 3> knownChain
+ {
+ GlobalFrame, MapFrame, OdometryFrame}; // Robot comes next
+
+ auto* frameBeginIt =
+ std::find(knownChain.begin(), knownChain.end(), query.header.parentFrame);
+ auto* const frameEndIt =
+ std::find(knownChain.begin(), knownChain.end(), query.header.frame);
+
+ if (frameBeginIt == knownChain.end())
+ {
+ ARMARX_WARNING << "Parent frame '" << query.header.parentFrame << "' unknown";
+ return {};
+ }
+
+ if (frameEndIt == knownChain.end())
+ {
+ ARMARX_DEBUG << "Frame '" << query.header.frame << "' must be robot frame";
+ }
+
+ const size_t nFrames = std::distance(frameBeginIt, frameEndIt);
+ ARMARX_DEBUG << "Lookup '" << query.header.parentFrame << " -> " << query.header.frame
+ << "' across " << nFrames << " frames";
+
+ for (; frameBeginIt != knownChain.end() - 1; frameBeginIt++)
+ {
+ addToChain(*frameBeginIt, *(frameBeginIt + 1));
+ }
+
+ if (frameEndIt == knownChain.end())
+ {
+ addToChain(knownChain.back(), query.header.frame);
+ }
+
+ if (chain.empty())
+ {
+ ARMARX_WARNING << deactivateSpam(1) << "Cannot create tf lookup chain '" << query.header.parentFrame
+ << " -> " << query.header.frame << "'";
+ return {};
+ }
+
+ return chain;
+ }
+
+ inline ::armarx::armem::robot_state::Transform
+ convertEntityToTransform(const armem::wm::EntityInstance& item)
+ {
+ arondto::Transform aronTransform;
+ aronTransform.fromAron(item.data());
+
+ ::armarx::armem::robot_state::Transform transform;
+ fromAron(aronTransform, transform);
+
+ return transform;
+ }
+
+ auto
+ findFirstElementAfter(const std::vector<::armarx::armem::robot_state::Transform>& transforms,
+ const armem::Time& timestamp)
+ {
+ const auto comp = [](const armem::Time & timestamp, const auto & transform)
+ {
+ return transform.header.timestamp < timestamp;
+ };
+
+ const auto it = std::upper_bound(transforms.begin(), transforms.end(), timestamp, comp);
+
+ auto timestampBeyond =
+ [timestamp](const ::armarx::armem::robot_state::Transform & transform)
+ {
+ return transform.header.timestamp > timestamp;
+ };
+
+ const auto poseNextIt = std::find_if(transforms.begin(), transforms.end(), timestampBeyond);
+
+ ARMARX_WARNING << "Fooasdfasdjfh";
+
+ ARMARX_CHECK(it == poseNextIt);
+
+ return poseNextIt;
+ }
+
+ Eigen::Affine3f
+ interpolateTransform(const std::vector<::armarx::armem::robot_state::Transform>& queue,
+ armem::Time timestamp)
+ {
+ ARMARX_TRACE;
+
+ ARMARX_DEBUG << "Entering";
+
+ ARMARX_CHECK(not queue.empty())
+ << "The queue has to contain at least two items to perform a lookup";
+
+ ARMARX_DEBUG << "Entering ... "
+ << "Q front " << queue.front().header.timestamp << " "
+ << "Q back " << queue.back().header.timestamp << " "
+ << "query timestamp " << timestamp;
+
+ // TODO(fabian.reister): sort queue.
+
+ ARMARX_CHECK(queue.back().header.timestamp > timestamp)
+ << "Cannot perform lookup into the future!";
+
+ // ARMARX_DEBUG << "Entering 1.5 " << queue.front().timestamp << " " << timestamp;
+ ARMARX_CHECK(queue.front().header.timestamp < timestamp)
+ << "Cannot perform lookup. Timestamp too old";
+ // => now we know that there is an element right after and before the timestamp within our queue
+
+ ARMARX_DEBUG << "Entering 2";
+
+ const auto poseNextIt = findFirstElementAfter(queue, timestamp);
+
+ ARMARX_DEBUG << "it ari";
+
+ const auto posePreIt = poseNextIt - 1;
+
+ ARMARX_DEBUG << "deref";
+
+ // the time fraction [0..1] of the lookup wrt to posePre and poseNext
+ const double t = (timestamp - posePreIt->header.timestamp).toMicroSecondsDouble() /
+ (poseNextIt->header.timestamp - posePreIt->header.timestamp).toMicroSecondsDouble();
+
+ ARMARX_DEBUG << "interpolate";
+
+ return simox::math::interpolatePose(posePreIt->transform, poseNextIt->transform, static_cast<float>(t));
+ }
+
+ std::vector<Eigen::Affine3f>
+ TransformHelper::obtainTransforms(const armem::wm::CoreSegment& localizationCoreSegment,
+ const std::vector<std::string>& tfChain,
+ const std::string& agent,
+ const armem::Time& timestamp)
+ {
+ const auto& agentProviderSegment = localizationCoreSegment.getProviderSegment(agent);
+
+ ARMARX_DEBUG << "Provider segments"
+ << simox::alg::get_keys(localizationCoreSegment.providerSegments());
+
+ ARMARX_DEBUG << "Entities: " << simox::alg::get_keys(agentProviderSegment.entities());
+
+ try
+ {
+ std::vector<Eigen::Affine3f> transforms;
+ transforms.reserve(tfChain.size());
+ std::transform(tfChain.begin(),
+ tfChain.end(),
+ std::back_inserter(transforms),
+ [&](const std::string & entityName)
+ {
+ return obtainTransform(
+ entityName, agentProviderSegment, timestamp);
+ });
+ return transforms;
+ }
+ catch (const armem::error::MissingEntry& missingEntryError)
+ {
+ ARMARX_WARNING << missingEntryError.what();
+ }
+ catch (const ::armarx::exceptions::local::ExpressionException& ex)
+ {
+ ARMARX_WARNING << "local exception: " << ex.what();
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << "Error: " << GetHandledExceptionString();
+ }
+
+
+
+ return {};
+ }
+
+ Eigen::Affine3f
+ TransformHelper::obtainTransform(const std::string& entityName,
+ const armem::wm::ProviderSegment& agentProviderSegment,
+ const armem::Time& timestamp)
+ {
+
+ ARMARX_DEBUG << "getEntity:" + entityName;
+ const auto& entity = agentProviderSegment.getEntity(entityName);
+
+ ARMARX_DEBUG << "History (size: " << entity.history().size() << ")"
+ << simox::alg::get_keys(entity.history());
+
+ // if (entity.history.empty())
+ // {
+ // // TODO(fabian.reister): fixme boom
+ // ARMARX_ERROR << "No snapshots received.";
+ // return Eigen::Affine3f::Identity();
+ // }
+
+ std::vector<::armarx::armem::robot_state::Transform> transforms;
+ transforms.reserve(entity.history().size());
+
+ const auto entitySnapshots = simox::alg::get_values(entity.history());
+ std::transform(
+ entitySnapshots.begin(),
+ entitySnapshots.end(),
+ std::back_inserter(transforms),
+ [](const auto & entity)
+ {
+ return convertEntityToTransform(entity.getInstance(0));
+ });
+
+ ARMARX_DEBUG << "obtaining transform";
+
+ if (transforms.size() > 1)
+ {
+ // TODO(fabian.reister): remove
+ return transforms.front().transform;
+
+ ARMARX_DEBUG << "More than one snapshots received: " << transforms.size();
+ const auto p = interpolateTransform(transforms, timestamp);
+ ARMARX_DEBUG << "Done interpolating transform";
+ return p;
+ }
+
+ // accept this to fail (will raise armem::error::MissingEntry)
+ if (transforms.empty())
+ {
+ ARMARX_DEBUG << "empty transform";
+
+ throw armem::error::MissingEntry("foo", "bar", "foo2", "bar2");
+ }
+
+ ARMARX_DEBUG << "single transform";
+
+ return transforms.front().transform;
+ }
+} // namespace armarx::armem::common::robot_state::localization
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h
new file mode 100644
index 0000000000000000000000000000000000000000..8045c552e839b657c47dca5aabe6f76e0c384d0c
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h
@@ -0,0 +1,65 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <vector>
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include <RobotAPI/libraries/armem_robot_state/common/localization/types.h>
+
+namespace armarx::armem::wm
+{
+ class CoreSegment;
+ class ProviderSegment;
+} // namespace armarx::armem::wm
+
+namespace armarx::armem::common::robot_state::localization
+{
+ using armarx::armem::common::robot_state::localization::TransformQuery;
+ using armarx::armem::common::robot_state::localization::TransformResult;
+
+ class TransformHelper
+ {
+ public:
+ static TransformResult
+ lookupTransform(const armem::wm::CoreSegment& localizationCoreSegment,
+ const TransformQuery& query);
+
+ private:
+ static std::vector<std::string>
+ buildTransformChain(const armem::wm::CoreSegment& localizationCoreSegment,
+ const TransformQuery& query);
+
+ static std::vector<Eigen::Affine3f>
+ obtainTransforms(const armem::wm::CoreSegment& localizationCoreSegment,
+ const std::vector<std::string>& tfChain,
+ const std::string& agent,
+ const armem::Time& timestamp);
+
+ static Eigen::Affine3f
+ obtainTransform(const std::string& entityName,
+ const armem::wm::ProviderSegment& agentProviderSegment,
+ const armem::Time& timestamp);
+ };
+} // namespace armarx::armem::common::robot_state::localization
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem/client/MemoryConnector.h b/source/RobotAPI/libraries/armem_robot_state/common/localization/types.h
similarity index 50%
rename from source/RobotAPI/libraries/armem/client/MemoryConnector.h
rename to source/RobotAPI/libraries/armem_robot_state/common/localization/types.h
index d3a9f288da15cefb71eb8bf5ecabf9bf7e250c4b..7f0c7c3396c7688ebe50b854579b107e9cd55b6f 100644
--- a/source/RobotAPI/libraries/armem/client/MemoryConnector.h
+++ b/source/RobotAPI/libraries/armem_robot_state/common/localization/types.h
@@ -22,48 +22,39 @@
#pragma once
+#include <Eigen/Geometry>
-// TODO(fabian.reister): remove
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
-#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+#include <RobotAPI/libraries/armem_robot_state/types.h>
-namespace armarx
+namespace armarx::armem::common::robot_state::localization
{
- class ManagedIceObject;
-}
-
-namespace armarx::armem
-{
-
- /**
- * @brief The MemoryConnector class simplifies connecting to the ArMem memory.
- *
- * Use this as the base class of any class that needs to connect to the memory.
- *
- *
- */
- class MemoryConnector
+ struct TransformResult
{
+ ::armarx::armem::robot_state::Transform transform;
+
+ enum class Status
+ {
+ Success,
+ Error,
+ ErrorLookupIntoFuture,
+ ErrorFrameNotAvailable
+ } status;
+
+ explicit operator bool() const
+ {
+ return status == Status::Success;
+ }
+
+ std::string errorMessage = "";
+ };
- public:
- MemoryConnector(ManagedIceObject& component);
- virtual ~MemoryConnector();
-
- protected:
- armem::data::WaitForMemoryResult useMemory(const std::string& memoryName);
- void registerPropertyDefinitions(PropertyDefinitionsPtr& def);
-
- void waitForMemory();
-
- virtual const std::string& getPropertyPrefix() const;
-
- private:
- ManagedIceObject& component;
-
- armem::mns::MemoryNameSystemInterfacePrx memoryNameSystem;
+ struct TransformQuery
+ {
+ ::armarx::armem::robot_state::TransformHeader header;
- const std::string propertyPrefix;
+ // bool exact;
};
-} // namespace armarx::armem
\ No newline at end of file
+} // namespace armarx::armem::common::robot_state::localization
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..73f17915af1c3497fb8b5627d629fd7b2373a90c
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
@@ -0,0 +1,251 @@
+#include "Visu.h"
+
+#include <algorithm>
+
+#include <Eigen/Geometry>
+
+#include <IceUtil/Time.h>
+
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <SimoxUtility/math/pose.h>
+
+#include "ArmarXCore/core/logging/Logging.h"
+#include "ArmarXCore/core/time/CycleUtil.h"
+#include <ArmarXCore/core/time/TimeUtil.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+#include <exception>
+#include "RobotAPI/libraries/armem/core/Time.h"
+
+#include "../localization/Segment.h"
+#include "../proprioception/Segment.h"
+#include "../description/Segment.h"
+
+namespace armarx::armem::server::robot_state
+{
+
+ void Visu::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ defs->optional(p.enabled, prefix + "enabled",
+ "Enable or disable visualization of objects.");
+ defs->optional(p.frequencyHz, prefix + "frequenzyHz",
+ "Frequency of visualization.");
+ }
+
+ void Visu::visualizeRobots(
+ viz::Layer& layer,
+ const robot::Robots& robots
+ )
+ {
+
+ const auto visualizeRobot = [&](const robot::Robot & robot)
+ {
+
+ const auto xmlPath = robot.description.xml.serialize();
+
+ // clang-format off
+ auto robotVisu = viz::Robot(robot.description.name)
+ // .file(xmlPath.package, xmlPath.path)
+ .file(xmlPath.package, xmlPath.path)
+ .joints(robot.config.jointMap)
+ .pose(robot.config.globalPose);
+
+ robotVisu.useFullModel();
+ // clang-format on
+
+ layer.add(robotVisu);
+ };
+
+ std::for_each(robots.begin(), robots.end(), visualizeRobot);
+
+ }
+
+ void Visu::init()
+ {
+
+ }
+
+ void Visu::connect(const viz::Client& arviz)
+ {
+ this->arviz = arviz;
+
+ updateTask = new SimpleRunningTask<>([this]()
+ {
+ this->visualizeRun();
+ });
+ updateTask->start();
+ }
+
+ // void Visu::RemoteGui::setup(const Visu& visu)
+ // {
+ // using namespace armarx::RemoteGui::Client;
+
+ // enabled.setValue(visu.enabled);
+ // inGlobalFrame.setValue(visu.inGlobalFrame);
+ // alpha.setRange(0, 1.0);
+ // alpha.setValue(visu.alpha);
+ // alphaByConfidence.setValue(visu.alphaByConfidence);
+ // oobbs.setValue(visu.oobbs);
+ // objectFrames.setValue(visu.objectFrames);
+ // {
+ // float max = 10000;
+ // objectFramesScale.setRange(0, max);
+ // objectFramesScale.setDecimals(2);
+ // objectFramesScale.setSteps(int(10 * max));
+ // objectFramesScale.setValue(visu.objectFramesScale);
+ // }
+
+ // GridLayout grid;
+ // int row = 0;
+ // grid.add(Label("Enabled"), {row, 0}).add(enabled, {row, 1});
+ // row++;
+ // grid.add(Label("Global Frame"), {row, 0}).add(inGlobalFrame, {row, 1});
+ // row++;
+ // grid.add(Label("Alpha"), {row, 0}).add(alpha, {row, 1}, {1, 3});
+ // row++;
+ // grid.add(Label("Alpha by Confidence"), {row, 0}).add(alphaByConfidence, {row, 1});
+ // row++;
+ // grid.add(Label("OOBB"), {row, 0}).add(oobbs, {row, 1});
+ // row++;
+ // grid.add(Label("Object Frames"), {row, 0}).add(objectFrames, {row, 1});
+ // grid.add(Label("Scale:"), {row, 2}).add(objectFramesScale, {row, 3});
+ // row++;
+
+ // group.setLabel("Visualization");
+ // group.addChild(grid);
+ // }
+
+ // void Visu::RemoteGui::update(Visu& visu)
+ // {
+ // visu.enabled = enabled.getValue();
+ // visu.inGlobalFrame = inGlobalFrame.getValue();
+ // visu.alpha = alpha.getValue();
+ // visu.alphaByConfidence = alphaByConfidence.getValue();
+ // visu.oobbs = oobbs.getValue();
+ // visu.objectFrames = objectFrames.getValue();
+ // visu.objectFramesScale = objectFramesScale.getValue();
+ // }
+
+ robot::Robots combine(const std::unordered_map<std::string, robot::RobotDescription>& robotDescriptions,
+ const std::unordered_map<std::string, Eigen::Affine3f>& globalRobotPoseMap,
+ const std::unordered_map<std::string, std::map<std::string, float>>& robotJointPositionMap)
+ {
+
+ robot::Robots robots;
+
+ for (const auto& [robotName, robotDescription] : robotDescriptions)
+ {
+ const auto& globalPose = globalRobotPoseMap.find(robotName);
+ if (globalPose == globalRobotPoseMap.end())
+ {
+ ARMARX_WARNING << deactivateSpam(10) << "No global pose for robot '" << robotName << "'";
+ continue;
+ }
+
+ const auto& jointMap = robotJointPositionMap.find(robotName);
+ if (jointMap == robotJointPositionMap.end())
+ {
+ ARMARX_WARNING << deactivateSpam(10) << "No joint positions for robot '" << robotName << "'";
+ continue;
+ }
+
+ ARMARX_DEBUG << "Found the following joints for robot " << robotName << ": " << simox::alg::get_keys(jointMap->second);
+
+ // TODO(fabian.reister): based on data
+ const armem::Time timestamp = IceUtil::Time::now();
+
+ robots.emplace_back(
+ robot::Robot
+ {
+ .description = robotDescription,
+ .instance = "", // TODO(fabian.reister): set this properly
+ .config = {
+ .timestamp = timestamp,
+ .globalPose = globalPose->second,
+ .jointMap = jointMap->second
+ },
+ .timestamp = timestamp,
+ }
+ );
+ }
+
+ return robots;
+
+ }
+
+
+ void Visu::visualizeRun()
+ {
+
+ CycleUtil cycle(static_cast<int>(1000 / p.frequencyHz));
+ while (updateTask && not updateTask->isStopped())
+ {
+ {
+ // std::scoped_lock lock(visuMutex);
+ ARMARX_DEBUG << "Update task";
+
+ if (p.enabled)
+ {
+ TIMING_START(Visu);
+
+ // TODO(fabian.reister): use timestamp
+
+ const auto timestamp = IceUtil::Time::now();
+
+ try
+ {
+ const auto robotDescriptions = descriptionSegment.getRobotDescriptions(timestamp);
+ const auto globalRobotPoseMap = localizationSegment.getRobotGlobalPoses(timestamp);
+ const auto robotJointPositionMap = proprioceptionSegment.getRobotJointPositions(timestamp);
+
+
+ // we need all 3 informations:
+ // - robot description
+ // - global pose
+ // - joint positions
+ // => this is nothing but a armem::Robot
+
+ const robot::Robots robots = combine(robotDescriptions, globalRobotPoseMap, robotJointPositionMap);
+ viz::Layer layer = arviz.layer("Robots");
+
+ ARMARX_DEBUG << "visualizing robots";
+ visualizeRobots(layer, robots);
+
+ ARMARX_DEBUG << "Committing robots";
+
+ arviz.commit({layer});
+
+ ARMARX_DEBUG << "Done committing";
+
+
+ TIMING_END_STREAM(Visu, ARMARX_VERBOSE);
+
+ // if (debugObserver)
+ // {
+ // debugObserver->setDebugChannel(getName(),
+ // {
+ // { "t Visualize [ms]", new Variant(Visu.toMilliSecondsDouble()) },
+ // });
+ // }
+
+ }
+ catch (const std::exception& ex)
+ {
+ ARMARX_WARNING << ex.what();
+ continue;
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << "Unknown exception";
+ continue;
+ }
+
+ }
+ }
+ cycle.waitForCycleDuration();
+ }
+ }
+
+
+
+} // namespace armarx::armem::server::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
new file mode 100644
index 0000000000000000000000000000000000000000..dad4e49feb530109596a57fab7b52742b421e15c
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
@@ -0,0 +1,123 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/services/tasks/TaskUtil.h>
+
+// #include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
+
+#include <RobotAPI/components/ArViz/Client/Client.h>
+
+#include <RobotAPI/libraries/armem_objects/types.h>
+
+
+namespace armarx
+{
+ class ObjectFinder;
+}
+
+namespace armarx::armem::server::robot_state
+{
+ namespace localization
+ {
+ class Segment;
+ }
+
+ namespace proprioception
+ {
+ class Segment;
+ }
+
+ namespace description
+ {
+ class Segment;
+ }
+
+
+ /**
+ * @brief Models decay of object localizations by decreasing the confidence
+ * the longer the object was not localized.
+ */
+ class Visu : public armarx::Logging
+ {
+ public:
+
+ Visu(const description::Segment& descriptionSegment,
+ const proprioception::Segment& proprioceptionSegment,
+ const localization::Segment& localizationSegment)
+ : descriptionSegment(descriptionSegment),
+ proprioceptionSegment(proprioceptionSegment),
+ localizationSegment(localizationSegment)
+ {}
+
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "visu.");
+
+ void init();
+
+ void connect(const viz::Client& arviz);
+
+ protected:
+
+ static void visualizeRobots(
+ viz::Layer& layer,
+ const robot::Robots& robots
+ );
+
+
+ private:
+ viz::Client arviz;
+
+ const description::Segment& descriptionSegment;
+ const proprioception::Segment& proprioceptionSegment;
+ const localization::Segment& localizationSegment;
+
+ struct Properties
+ {
+ bool enabled = true;
+ float frequencyHz = 25;
+ } p;
+
+
+ SimpleRunningTask<>::pointer_type updateTask;
+ void visualizeRun();
+
+ // struct RemoteGui
+ // {
+ // armarx::RemoteGui::Client::GroupBox group;
+
+ // armarx::RemoteGui::Client::CheckBox enabled;
+
+ // armarx::RemoteGui::Client::CheckBox inGlobalFrame;
+ // armarx::RemoteGui::Client::FloatSlider alpha;
+ // armarx::RemoteGui::Client::CheckBox alphaByConfidence;
+ // armarx::RemoteGui::Client::CheckBox oobbs;
+ // armarx::RemoteGui::Client::CheckBox objectFrames;
+ // armarx::RemoteGui::Client::FloatSpinBox objectFramesScale;
+
+ // // void setup(const Visu& visu);
+ // // void update(Visu& visu);
+ // };
+
+ };
+
+} // namespace armarx::armem::server::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..826c5b484768686e2edb8cee409df4136bcc85ab
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
@@ -0,0 +1,243 @@
+#include "Segment.h"
+
+#include <IceUtil/Time.h>
+#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
+#include <sstream>
+
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+#include "ArmarXCore/core/logging/Logging.h"
+#include <ArmarXCore/core/time/TimeUtil.h>
+
+#include "ArmarXCore/core/system/ArmarXDataPath.h"
+#include "ArmarXCore/core/system/cmake/CMakePackageFinder.h"
+#include "RobotAPI/libraries/armem_robot/types.h"
+#include "RobotAPI/libraries/aron/common/aron_conversions.h"
+
+#include "RobotAPI/libraries/armem/core/MemoryID.h"
+#include <RobotAPI/libraries/armem/client/Writer.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/query/query_fns.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/Visitor.h>
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+
+#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
+#include <thread>
+
+namespace armarx::armem::server::robot_state::description
+{
+
+ Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter,
+ std::mutex& memoryMutex) :
+ iceMemory(memoryToIceAdapter), memoryMutex(memoryMutex)
+ {
+ Logging::setTag("DescriptionSegment");
+ }
+
+ Segment::~Segment() = default;
+
+ void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ defs->optional(p.coreSegment,
+ prefix + "seg.description.CoreSegment",
+ "Name of the robot description core segment.");
+ defs->optional(p.maxHistorySize,
+ prefix + "seg.description.MaxHistorySize",
+ "Maximal size of object poses history (-1 for infinite).");
+ }
+
+ void Segment::init()
+ {
+ ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
+
+ ARMARX_INFO << "Adding core segment '" << p.coreSegment << "'";
+
+ coreSegment = &iceMemory.workingMemory->addCoreSegment(
+ p.coreSegment, arondto::RobotDescription::toInitialAronType());
+ coreSegment->setMaxHistorySize(p.maxHistorySize);
+ }
+
+ void Segment::connect(const viz::Client& arviz, const RobotUnitInterfacePrx& robotUnitPrx)
+ {
+ // this->visu = std::make_unique<Visu>(arviz, *this);
+ robotUnit = robotUnitPrx;
+
+ // store the robot description linked to the robot unit in this segment
+ updateRobotDescription();
+ }
+
+ void Segment::storeRobotDescription(const robot::RobotDescription& robotDescription)
+ {
+ const Time now = TimeUtil::GetTime();
+
+ const MemoryID providerID = coreSegment->id().withProviderSegmentName(robotDescription.name);
+ coreSegment->addProviderSegment(providerID.providerSegmentName, arondto::RobotDescription::toInitialAronType());
+
+ EntityUpdate update;
+ update.entityID = providerID.withEntityName("description");
+ update.timeArrived = update.timeCreated = update.timeSent = now;
+
+ arondto::RobotDescription dto;
+ robot::toAron(dto, robotDescription);
+ update.instancesData =
+ {
+ dto.toAron()
+ };
+
+ Commit commit;
+ commit.updates.push_back(update);
+
+ {
+ // std::lock_guard g{memoryMutex};
+ iceMemory.commit(commit);
+ }
+
+ }
+
+ void Segment::updateRobotDescription()
+ {
+ ARMARX_CHECK_NOT_NULL(robotUnit);
+
+ const auto waitForKinematicUnit = [&]()
+ {
+ while (true)
+ {
+ auto kinematicUnit = robotUnit->getKinematicUnit();
+
+ if (kinematicUnit)
+ {
+ ARMARX_INFO << "Kinematic unit is now available.";
+ return kinematicUnit;
+ }
+
+ ARMARX_INFO << "Waiting for kinematic unit ...";
+ std::this_thread::sleep_for(std::chrono::seconds(1));
+ }
+ };
+
+ auto kinematicUnit = waitForKinematicUnit();
+ ARMARX_CHECK_NOT_NULL(kinematicUnit);
+
+ const auto robotName = kinematicUnit->getRobotName();
+ const auto robotFilename = kinematicUnit->getRobotFilename();
+
+ const auto packages = armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
+ const auto package = armarx::ArmarXDataPath::getProject(packages, robotFilename);
+
+ ARMARX_INFO << "Robot description '" << robotFilename << "' found in package " << package;
+
+ const robot::RobotDescription robotDescription
+ {
+ .name = kinematicUnit->getRobotName(),
+ .xml = {package, kinematicUnit->getRobotFilename()}}; // FIXME
+
+ storeRobotDescription(robotDescription);
+ }
+
+ Segment::RobotDescriptionMap Segment::getRobotDescriptions(const armem::Time& timestamp) const
+ {
+
+ RobotDescriptionMap robotDescriptions;
+
+ {
+ // std::lock_guard g{memoryMutex};
+
+ for (const auto &[_, provSeg] : iceMemory.workingMemory->getCoreSegment(p.coreSegment))
+ {
+ for (const auto &[name, entity] : provSeg.entities())
+ {
+ const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
+ const auto description = robot::convertRobotDescription(entityInstance);
+
+ if (not description)
+ {
+ ARMARX_WARNING << "Could not convert entity instance to "
+ "'RobotDescription'";
+ continue;
+ }
+
+ ARMARX_DEBUG << "Key is " << armem::MemoryID(entity.id());
+
+ robotDescriptions.emplace(description->name, *description);
+ }
+ }
+ }
+
+ ARMARX_INFO << deactivateSpam(10) << "Number of known robot descriptions: " << robotDescriptions.size();
+
+ return robotDescriptions;
+ }
+
+ // std::unordered_map<armem::MemoryID, ::armarx::armem::articulated_object::ArticulatedObjectDescription> Segment::getKnownObjectClasses() const
+ // {
+ // std::unordered_map<armem::MemoryID, ::armarx::armem::articulated_object::ArticulatedObjectDescription> objects;
+
+ // for (const auto& [_, provSeg] : iceMemory.workingMemory->getCoreSegment(p.coreClassSegmentName))
+ // {
+ // for (const auto& [name, entity] : provSeg.entities())
+ // {
+ // const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
+ // const auto description = articulated_object::convertRobotDescription(entityInstance);
+
+ // if (not description)
+ // {
+ // ARMARX_WARNING << "Could not convert entity instance to 'RobotDescription'";
+ // continue;
+ // }
+
+ // ARMARX_INFO << "Key is " << armem::MemoryID(entity.id());
+
+ // objects.emplace(armem::MemoryID(entity.id()), *description);
+ // }
+ // }
+
+ // ARMARX_IMPORTANT << "Number of known articulated object classes: " << objects.size();
+
+ // return objects;
+ // }
+
+ // void Segment::RemoteGui::setup(const Segment& data)
+ // {
+ // using namespace armarx::RemoteGui::Client;
+
+ // maxHistorySize.setValue(std::max(1, int(data.p.maxHistorySize)));
+ // maxHistorySize.setRange(1, 1e6);
+ // infiniteHistory.setValue(data.p.maxHistorySize == -1);
+ // discardSnapshotsWhileAttached.setValue(data.p.discardSnapshotsWhileAttached);
+
+ // GridLayout grid;
+ // int row = 0;
+ // grid.add(Label("Max History Size"), {row, 0}).add(maxHistorySize, {row, 1});
+ // row++;
+ // grid.add(Label("Infinite History Size"), {row, 0}).add(infiniteHistory, {row, 1});
+ // row++;
+ // grid.add(Label("Discard Snapshots while Attached"), {row, 0}).add(discardSnapshotsWhileAttached, {row, 1});
+ // row++;
+
+ // group.setLabel("Data");
+ // group.addChild(grid);
+ // }
+
+ // void Segment::RemoteGui::update(Segment& data)
+ // {
+ // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged()
+ // || discardSnapshotsWhileAttached.hasValueChanged())
+ // {
+ // std::scoped_lock lock(data.memoryMutex);
+
+ // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
+ // {
+ // data.p.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
+ // if (data.coreSegment)
+ // {
+ // data.coreSegment->setMaxHistorySize(long(data.p.maxHistorySize));
+ // }
+ // }
+
+ // data.p.discardSnapshotsWhileAttached = discardSnapshotsWhileAttached.getValue();
+ // }
+ // }
+
+} // namespace armarx::armem::server::robot_state::description
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.h b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.h
new file mode 100644
index 0000000000000000000000000000000000000000..2f603cf4961d90342fbd74770a0ef4d0d181c033
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.h
@@ -0,0 +1,114 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
+#include <string>
+#include <optional>
+#include <mutex>
+#include <unordered_map>
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include "ArmarXCore/core/application/properties/PropertyDefinitionContainer.h"
+
+// #include "ArmarXGui/libraries/RemoteGui/Client/Widgets.h"
+
+#include "RobotAPI/components/ArViz/Client/Client.h"
+
+#include "RobotAPI/libraries/armem/core/MemoryID.h"
+#include "RobotAPI/libraries/armem_objects/types.h"
+
+namespace armarx::armem
+{
+ namespace server
+ {
+ class MemoryToIceAdapter;
+ }
+
+ namespace wm
+ {
+ class CoreSegment;
+ }
+} // namespace armarx::armem
+
+
+namespace armarx::armem::server::robot_state::description
+{
+ class Visu;
+
+ class Segment : public armarx::Logging
+ {
+ public:
+ Segment(server::MemoryToIceAdapter& iceMemory,
+ std::mutex& memoryMutex);
+
+ virtual ~Segment();
+
+ void connect(const viz::Client& arviz, const RobotUnitInterfacePrx& robotUnitPrx);
+
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
+
+ void init();
+
+ /// mapping "robot name" -> "robot description"
+ using RobotDescriptionMap = std::unordered_map<std::string, robot::RobotDescription>;
+
+ RobotDescriptionMap getRobotDescriptions(const armem::Time& timestamp) const;
+
+ private:
+
+ void storeRobotDescription(const robot::RobotDescription& robotDescription);
+ void updateRobotDescription();
+
+
+ server::MemoryToIceAdapter& iceMemory;
+ wm::CoreSegment* coreSegment = nullptr;
+ std::mutex& memoryMutex;
+
+ RobotUnitInterfacePrx robotUnit;
+
+ struct Properties
+ {
+ std::string coreSegment = "Description";
+ int64_t maxHistorySize = -1;
+ };
+ Properties p;
+
+ // std::unique_ptr<Visu> visu;
+
+ public:
+
+ // struct RemoteGui
+ // {
+ // armarx::RemoteGui::Client::GroupBox group;
+
+ // armarx::RemoteGui::Client::IntSpinBox maxHistorySize;
+ // armarx::RemoteGui::Client::CheckBox infiniteHistory;
+ // armarx::RemoteGui::Client::CheckBox discardSnapshotsWhileAttached;
+
+ // void setup(const Segment& data);
+ // void update(Segment& data);
+ // };
+
+ };
+
+} // namespace armarx::armem::server::robot_state::description
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6858f0c346dabebf96b9c2337d85760651abbbc8
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
@@ -0,0 +1,209 @@
+#include "Segment.h"
+
+#include <Eigen/src/Geometry/Transform.h>
+#include <IceUtil/Time.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
+#include <iterator>
+#include <sstream>
+
+#include "ArmarXCore/core/logging/Logging.h"
+#include <ArmarXCore/core/time/TimeUtil.h>
+
+#include "RobotAPI/libraries/aron/common/aron_conversions.h"
+
+#include "RobotAPI/libraries/armem/core/MemoryID.h"
+#include "RobotAPI/libraries/armem/core/Time.h"
+#include "RobotAPI/libraries/armem_robot/robot_conversions.h"
+#include "RobotAPI/libraries/core/FramedPose.h"
+#include <RobotAPI/libraries/armem/client/Writer.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/query/query_fns.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/Visitor.h>
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+
+#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
+
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+
+#include <RobotAPI/libraries/armem_robot_state/common/localization/types.h>
+#include <RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h>
+
+
+namespace armarx::armem::server::robot_state::localization
+{
+
+ Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter,
+ std::mutex& memoryMutex) :
+ iceMemory(memoryToIceAdapter), memoryMutex(memoryMutex)
+ {
+ Logging::setTag("LocalizationSegment");
+ }
+
+ Segment::~Segment() = default;
+
+ void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ defs->optional(p.coreSegment,
+ prefix + "seg.localization.CoreSegment",
+ "Name of the object instance core segment.");
+ defs->optional(p.maxHistorySize,
+ prefix + "seg.localization.MaxHistorySize",
+ "Maximal size of object poses history (-1 for infinite).");
+ }
+
+ void Segment::init()
+ {
+ ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
+
+ ARMARX_INFO << "Adding core segment '" << p.coreSegment << "'";
+
+ coreSegment = &iceMemory.workingMemory->addCoreSegment(
+ p.coreSegment, arondto::Transform::toInitialAronType());
+ coreSegment->setMaxHistorySize(p.maxHistorySize);
+ }
+
+ void Segment::connect(viz::Client arviz)
+ {
+ // this->visu = std::make_unique<Visu>(arviz, *this);
+ }
+
+ std::unordered_map<std::string, Eigen::Affine3f> Segment::getRobotGlobalPoses(const armem::Time& timestamp) const
+ {
+
+ using common::robot_state::localization::TransformHelper;
+ using common::robot_state::localization::TransformQuery;
+
+ std::unordered_map<std::string, Eigen::Affine3f> robotGlobalPoses;
+
+ const auto& localizationCoreSegment =
+ iceMemory.workingMemory->getCoreSegment(p.coreSegment);
+ const auto knownRobots = simox::alg::get_keys(localizationCoreSegment);
+
+ for (const auto& robotName : knownRobots)
+ {
+
+ TransformQuery query{.header{
+ .parentFrame = GlobalFrame,
+ .frame = "root", // TODO, FIXME
+ .agent = robotName,
+ .timestamp = timestamp
+ }};
+
+ const auto result =
+ TransformHelper::lookupTransform(localizationCoreSegment, query);
+
+ if (not result)
+ {
+ // TODO
+ continue;
+ }
+
+ robotGlobalPoses.emplace(robotName, result.transform.transform);
+ }
+
+ ARMARX_INFO << deactivateSpam(10)
+ << "Number of known robot poses: " << robotGlobalPoses.size();
+
+ return robotGlobalPoses;
+ }
+
+ bool Segment::storeTransform(const armarx::armem::robot_state::Transform& transform)
+ {
+ const auto& timestamp = transform.header.timestamp;
+
+ const MemoryID providerID =
+ coreSegment->id().withProviderSegmentName(transform.header.agent);
+ if (not coreSegment->hasProviderSegment(providerID.providerSegmentName))
+ {
+ coreSegment->addProviderSegment(providerID.providerSegmentName);
+ }
+
+ Commit commit;
+
+ EntityUpdate& update = commit.updates.emplace_back();
+ update.entityID = providerID.withEntityName(transform.header.parentFrame + "," +
+ transform.header.frame);
+ update.timeArrived = update.timeCreated = update.timeSent = timestamp;
+
+ arondto::Transform aronTransform;
+ toAron(aronTransform, transform);
+ update.instancesData = {aronTransform.toAron()};
+
+ const armem::CommitResult result = iceMemory.commit(commit);
+ return result.allSuccess();
+ }
+
+ // std::unordered_map<armem::MemoryID, ::armarx::armem::articulated_object::ArticulatedObjectDescription> Segment::getKnownObjectClasses() const
+ // {
+ // std::unordered_map<armem::MemoryID, ::armarx::armem::articulated_object::ArticulatedObjectDescription> objects;
+
+ // for (const auto& [_, provSeg] : iceMemory.workingMemory->getCoreSegment(p.coreClassSegmentName))
+ // {
+ // for (const auto& [name, entity] : provSeg.entities())
+ // {
+ // const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
+ // const auto description = articulated_object::convertRobotDescription(entityInstance);
+
+ // if (not description)
+ // {
+ // ARMARX_WARNING << "Could not convert entity instance to 'RobotDescription'";
+ // continue;
+ // }
+
+ // ARMARX_INFO << "Key is " << armem::MemoryID(entity.id());
+
+ // objects.emplace(armem::MemoryID(entity.id()), *description);
+ // }
+ // }
+
+ // ARMARX_IMPORTANT << "Number of known articulated object classes: " << objects.size();
+
+ // return objects;
+ // }
+
+ // void Segment::RemoteGui::setup(const Segment& data)
+ // {
+ // using namespace armarx::RemoteGui::Client;
+
+ // maxHistorySize.setValue(std::max(1, int(data.p.maxHistorySize)));
+ // maxHistorySize.setRange(1, 1e6);
+ // infiniteHistory.setValue(data.p.maxHistorySize == -1);
+ // discardSnapshotsWhileAttached.setValue(data.p.discardSnapshotsWhileAttached);
+
+ // GridLayout grid;
+ // int row = 0;
+ // grid.add(Label("Max History Size"), {row, 0}).add(maxHistorySize, {row, 1});
+ // row++;
+ // grid.add(Label("Infinite History Size"), {row, 0}).add(infiniteHistory, {row, 1});
+ // row++;
+ // grid.add(Label("Discard Snapshots while Attached"), {row, 0}).add(discardSnapshotsWhileAttached, {row, 1});
+ // row++;
+
+ // group.setLabel("Data");
+ // group.addChild(grid);
+ // }
+
+ // void Segment::RemoteGui::update(Segment& data)
+ // {
+ // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged()
+ // || discardSnapshotsWhileAttached.hasValueChanged())
+ // {
+ // std::scoped_lock lock(data.memoryMutex);
+
+ // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
+ // {
+ // data.p.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
+ // if (data.coreSegment)
+ // {
+ // data.coreSegment->setMaxHistorySize(long(data.p.maxHistorySize));
+ // }
+ // }
+
+ // data.p.discardSnapshotsWhileAttached = discardSnapshotsWhileAttached.getValue();
+ // }
+ // }
+
+} // namespace armarx::armem::server::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h
new file mode 100644
index 0000000000000000000000000000000000000000..c23e9bcdce71f64288cec1a53c5c82d816327f92
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h
@@ -0,0 +1,108 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <string>
+#include <optional>
+#include <mutex>
+#include <unordered_map>
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include "ArmarXCore/core/application/properties/PropertyDefinitionContainer.h"
+
+// #include "ArmarXGui/libraries/RemoteGui/Client/Widgets.h"
+
+#include "RobotAPI/components/ArViz/Client/Client.h"
+
+#include "RobotAPI/libraries/armem/core/MemoryID.h"
+#include "RobotAPI/libraries/armem_objects/types.h"
+
+#include <RobotAPI/libraries/armem_robot_state/types.h>
+
+namespace armarx::armem
+{
+ namespace server
+ {
+ class MemoryToIceAdapter;
+ }
+
+ namespace wm
+ {
+ class CoreSegment;
+ }
+} // namespace armarx::armem
+
+
+namespace armarx::armem::server::robot_state::localization
+{
+ class Visu;
+
+ class Segment : public armarx::Logging
+ {
+ public:
+ Segment(server::MemoryToIceAdapter& iceMemory,
+ std::mutex& memoryMutex);
+
+ virtual ~Segment();
+
+ void connect(viz::Client arviz);
+
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
+
+ void init();
+
+ std::unordered_map<std::string, Eigen::Affine3f> getRobotGlobalPoses(const armem::Time& timestamp) const;
+
+ bool storeTransform(const armarx::armem::robot_state::Transform& transform);
+
+ private:
+
+ server::MemoryToIceAdapter& iceMemory;
+ wm::CoreSegment* coreSegment = nullptr;
+ std::mutex& memoryMutex;
+
+ struct Properties
+ {
+ std::string coreSegment = "Localization";
+ int64_t maxHistorySize = -1;
+ };
+ Properties p;
+
+ // std::unique_ptr<Visu> visu;
+
+ public:
+
+ // struct RemoteGui
+ // {
+ // armarx::RemoteGui::Client::GroupBox group;
+
+ // armarx::RemoteGui::Client::IntSpinBox maxHistorySize;
+ // armarx::RemoteGui::Client::CheckBox infiniteHistory;
+ // armarx::RemoteGui::Client::CheckBox discardSnapshotsWhileAttached;
+
+ // void setup(const Segment& data);
+ // void update(Segment& data);
+ // };
+
+ };
+
+} // namespace armarx::armem::server::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5ec63c12d9c3f6485839ff138b1ebe409dd1adc8
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
@@ -0,0 +1,168 @@
+#include "Segment.h"
+
+#include <mutex>
+#include <sstream>
+
+#include <ArmarXCore/core/time/TimeUtil.h>
+#include "ArmarXCore/core/logging/Logging.h"
+
+#include "RobotAPI/libraries/armem_robot_state/types.h"
+#include "RobotAPI/libraries/aron/common/aron_conversions.h"
+
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/Visitor.h>
+#include "RobotAPI/libraries/armem/core/MemoryID.h"
+#include <RobotAPI/libraries/armem/client/Writer.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/query/query_fns.h>
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+
+#include "RobotAPI/libraries/armem_robot/robot_conversions.h"
+#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/JointState.aron.generated.h>
+
+#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+
+#include <RobotAPI/libraries/armem/util/util.h>
+
+
+namespace armarx::armem::server::robot_state::proprioception
+{
+
+ Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter, std::mutex& memoryMutex) :
+ iceMemory(memoryToIceAdapter),
+ memoryMutex(memoryMutex)
+ {
+ Logging::setTag("ProprioceptionSegment");
+ }
+
+ Segment::~Segment() = default;
+
+ void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ defs->optional(p.coreSegment, prefix + "seg.proprioception.CoreSegment", "Name of the object instance core segment.");
+ defs->optional(p.maxHistorySize, prefix + "seg.proprioception.MaxHistorySize", "Maximal size of object poses history (-1 for infinite).");
+ }
+
+ void Segment::init()
+ {
+ ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
+
+ ARMARX_INFO << "Adding core segment '" << p.coreSegment << "'";
+ coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreSegment);
+ coreSegment->setMaxHistorySize(p.maxHistorySize);
+ }
+
+ void Segment::connect(viz::Client arviz, RobotUnitInterfacePrx robotUnitPrx)
+ {
+ robotUnit = robotUnitPrx;
+
+ auto kinematicUnit = robotUnit->getKinematicUnit();
+ const auto providerSegmentName = kinematicUnit->getRobotName();
+
+ // TODO what is the purpose?
+ auto encoderEntryType = std::make_shared<aron::typenavigator::ObjectNavigator>("RobotUnitEncoderEntry");
+ auto encoderNameType = std::make_shared<aron::typenavigator::StringNavigator>();
+ auto encoderIterationIDType = std::make_shared<aron::typenavigator::LongNavigator>();
+ encoderEntryType->addMemberType("EncoderGroupName", encoderNameType);
+ encoderEntryType->addMemberType("IterationId", encoderIterationIDType);
+ //auto encoderValueType = std::make_shared<aron::typenavigator::AnyType>();
+ //encoderEntryType->addMemberType("value", encoderValueType);
+
+ ARMARX_INFO << "Adding provider segment " << p.coreSegment << "/" << providerSegmentName;
+ armem::data::AddSegmentInput input;
+ input.coreSegmentName = p.coreSegment;
+ input.providerSegmentName = providerSegmentName;
+
+ {
+ std::lock_guard g{memoryMutex};
+ auto result = iceMemory.addSegments({input})[0];
+
+ if (!result.success)
+ {
+ ARMARX_ERROR << "Could not add segment " << p.coreSegment << "/" << providerSegmentName << ". The error message is: " << result.errorMessage;
+ }
+ }
+
+ robotUnitProviderID.memoryName = iceMemory.workingMemory->id().memoryName;
+ robotUnitProviderID.coreSegmentName = p.coreSegment;
+ robotUnitProviderID.providerSegmentName = providerSegmentName;
+ }
+
+ std::unordered_map<std::string, std::map<std::string, float>> Segment::getRobotJointPositions(const armem::Time& timestamp) const
+ {
+ std::unordered_map<std::string, std::map<std::string, float>> jointMap;
+
+ for (const auto& [robotName, provSeg] : iceMemory.workingMemory->getCoreSegment(p.coreSegment))
+ {
+ for (const auto& [name, entity] : provSeg.entities())
+ {
+ const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
+
+ const auto jointState = tryCast<armarx::armem::arondto::JointState>(entityInstance);
+
+ if (not jointState)
+ {
+ // ARMARX_WARNING << "Could not convert entity instance to 'JointState'";
+ continue;
+ }
+
+ jointMap[robotName].emplace(jointState->name, jointState->position);
+ }
+ }
+
+ ARMARX_INFO << deactivateSpam(10) << "Number of known robot joint maps: " << jointMap.size();
+
+ return jointMap;
+ }
+
+ const armem::MemoryID& Segment::getRobotUnitProviderID() const
+ {
+ return robotUnitProviderID;
+ }
+
+
+
+ // void Segment::RemoteGui::setup(const Segment& data)
+ // {
+ // using namespace armarx::RemoteGui::Client;
+
+ // maxHistorySize.setValue(std::max(1, int(data.p.maxHistorySize)));
+ // maxHistorySize.setRange(1, 1e6);
+ // infiniteHistory.setValue(data.p.maxHistorySize == -1);
+ // discardSnapshotsWhileAttached.setValue(data.p.discardSnapshotsWhileAttached);
+
+ // GridLayout grid;
+ // int row = 0;
+ // grid.add(Label("Max History Size"), {row, 0}).add(maxHistorySize, {row, 1});
+ // row++;
+ // grid.add(Label("Infinite History Size"), {row, 0}).add(infiniteHistory, {row, 1});
+ // row++;
+ // grid.add(Label("Discard Snapshots while Attached"), {row, 0}).add(discardSnapshotsWhileAttached, {row, 1});
+ // row++;
+
+ // group.setLabel("Data");
+ // group.addChild(grid);
+ // }
+
+ // void Segment::RemoteGui::update(Segment& data)
+ // {
+ // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged()
+ // || discardSnapshotsWhileAttached.hasValueChanged())
+ // {
+ // std::scoped_lock lock(data.memoryMutex);
+
+ // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
+ // {
+ // data.p.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
+ // if (data.coreSegment)
+ // {
+ // data.coreSegment->setMaxHistorySize(long(data.p.maxHistorySize));
+ // }
+ // }
+
+ // data.p.discardSnapshotsWhileAttached = discardSnapshotsWhileAttached.getValue();
+ // }
+ // }
+
+} // namespace armarx::armem::server::robot_state::proprioception
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h
new file mode 100644
index 0000000000000000000000000000000000000000..bc83a946bcadafbc85a15d55cff79c3a18af970f
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h
@@ -0,0 +1,110 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
+#include <string>
+#include <optional>
+#include <mutex>
+#include <unordered_map>
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include "ArmarXCore/core/application/properties/PropertyDefinitionContainer.h"
+
+// #include "ArmarXGui/libraries/RemoteGui/Client/Widgets.h"
+
+#include "RobotAPI/components/ArViz/Client/Client.h"
+
+#include "RobotAPI/libraries/armem/core/MemoryID.h"
+#include "RobotAPI/libraries/armem_objects/types.h"
+
+namespace armarx::armem
+{
+ namespace server
+ {
+ class MemoryToIceAdapter;
+ }
+
+ namespace wm
+ {
+ class CoreSegment;
+ }
+} // namespace armarx::armem
+
+
+namespace armarx::armem::server::robot_state::proprioception
+{
+ class Visu;
+
+ class Segment : public armarx::Logging
+ {
+ public:
+ Segment(server::MemoryToIceAdapter& iceMemory, std::mutex& memoryMutex);
+
+ virtual ~Segment();
+
+ void connect(viz::Client arviz, RobotUnitInterfacePrx robotUnitPrx);
+
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
+
+ void init();
+
+ std::unordered_map<std::string, std::map<std::string, float>> getRobotJointPositions(const armem::Time& timestamp) const;
+
+ const armem::MemoryID& getRobotUnitProviderID() const;
+
+ private:
+
+ server::MemoryToIceAdapter& iceMemory;
+ wm::CoreSegment* coreSegment = nullptr;
+ std::mutex& memoryMutex;
+
+ RobotUnitInterfacePrx robotUnit;
+
+ armem::MemoryID robotUnitProviderID;
+
+ struct Properties
+ {
+ std::string coreSegment = "Proprioception";
+ int64_t maxHistorySize = -1;
+ };
+ Properties p;
+
+ // std::unique_ptr<Visu> visu;
+
+ public:
+
+ // struct RemoteGui
+ // {
+ // armarx::RemoteGui::Client::GroupBox group;
+
+ // armarx::RemoteGui::Client::IntSpinBox maxHistorySize;
+ // armarx::RemoteGui::Client::CheckBox infiniteHistory;
+ // armarx::RemoteGui::Client::CheckBox discardSnapshotsWhileAttached;
+
+ // void setup(const Segment& data);
+ // void update(Segment& data);
+ // };
+
+ };
+
+} // namespace armarx::armem::server::robot_state::proprioception
diff --git a/source/RobotAPI/libraries/armem_robot_state/types.h b/source/RobotAPI/libraries/armem_robot_state/types.h
new file mode 100644
index 0000000000000000000000000000000000000000..39daba5d2d7db0148cb9df264d8da2acac54d036
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/types.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/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <Eigen/Geometry>
+
+#include "RobotAPI/libraries/armem/core/Time.h"
+
+namespace armarx::armem::robot_state
+{
+ struct TransformHeader
+ {
+ std::string parentFrame;
+ std::string frame;
+
+ std::string agent;
+
+ armem::Time timestamp;
+ };
+
+ struct Transform
+ {
+ TransformHeader header;
+
+ Eigen::Affine3f transform = Eigen::Affine3f::Identity();
+
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ };
+
+ struct JointState
+ {
+ std::string name;
+ float position;
+ };
+
+} // namespace armarx::armem::robot_state
diff --git a/source/RobotAPI/libraries/aron/common/CMakeLists.txt b/source/RobotAPI/libraries/aron/common/CMakeLists.txt
index 6fc9fa463f3f037c38f6e0142c934678775d9aaa..ec6c74d9e669f9effb5825afda1cc1645aadf547 100644
--- a/source/RobotAPI/libraries/aron/common/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/common/CMakeLists.txt
@@ -17,12 +17,14 @@ armarx_add_library(
aron_conversions/armarx.h
aron_conversions/simox.h
aron_conversions/stl.h
+ aron_conversions/eigen.h
SOURCES
aron_conversions/core.cpp
aron_conversions/armarx.cpp
aron_conversions/simox.cpp
aron_conversions/stl.cpp
+ aron_conversions/eigen.cpp
)
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions.h b/source/RobotAPI/libraries/aron/common/aron_conversions.h
index 2f96d0285b9fb4dbd96abdb92c12fe9518ee88b6..b771ef362aaf0a886aaabcc19027e74a9dfbeb9b 100644
--- a/source/RobotAPI/libraries/aron/common/aron_conversions.h
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions.h
@@ -5,3 +5,4 @@
#include "aron_conversions/armarx.h"
#include "aron_conversions/simox.h"
#include "aron_conversions/stl.h"
+#include "aron_conversions/eigen.h"
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.cpp b/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.cpp
index ea25e01e3517602df7cf7871cd393d5639de8d50..2cd6b7eb906d4b85cf6527979eb9573f4d9568ea 100644
--- a/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.cpp
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.cpp
@@ -21,5 +21,14 @@ namespace armarx
dto.path = icedto.path;
}
+ void fromAron(const long& dto, IceUtil::Time& bo)
+ {
+ bo = IceUtil::Time::microSeconds(dto);
+ }
+
+ void toAron(long& dto, const IceUtil::Time& bo)
+ {
+ dto = bo.toMicroSeconds();
+ }
} // 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 985409745af2576b980de11b169809efd91118b2..48c24690503cec91a17da37909a53c3228984e26 100644
--- a/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.h
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.h
@@ -11,4 +11,7 @@ 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);
+
} // namespace armarx
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/eigen.cpp b/source/RobotAPI/libraries/aron/common/aron_conversions/eigen.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0bf14ec90439e50a4869234b81449adaf068d173
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/eigen.cpp
@@ -0,0 +1,17 @@
+#include "eigen.h"
+
+namespace armarx::aron
+{
+ using AronPose = Eigen::Matrix<float, 4, 4>;
+
+ void fromAron(const AronPose& dto, Eigen::Affine3f& bo)
+ {
+ bo.matrix() = dto;
+ }
+
+ void toAron(AronPose& dto, const Eigen::Affine3f& bo)
+ {
+ dto = bo.matrix();
+ }
+
+} // namespace armarx::aron
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/eigen.h b/source/RobotAPI/libraries/aron/common/aron_conversions/eigen.h
new file mode 100644
index 0000000000000000000000000000000000000000..3f091dbfc40ee8e91b8d2922687273269ce790a8
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/eigen.h
@@ -0,0 +1,12 @@
+#pragma once
+
+#include <Eigen/Geometry>
+
+namespace armarx::aron
+{
+ using AronPose = Eigen::Matrix<float, 4, 4>;
+
+ void fromAron(const AronPose& dto, Eigen::Affine3f& bo);
+ void toAron(AronPose& dto, const Eigen::Affine3f& bo);
+
+} // namespace armarx