From f76f836f287783e82ec908e03cf9b3017e6c1087 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 4 May 2021 14:59:26 +0200
Subject: [PATCH] articulated object example client
---
.../ArMemObjectMemory/ArMemObjectMemory.scx | 2 +-
...culatedObjectExampleMemoryWriterClient.cfg | 227 ++++++++++++++++++
...culatedObjectExampleMemoryWriterClient.cpp | 133 ++++++++++
...ticulatedObjectExampleMemoryWriterClient.h | 65 +++++
.../CMakeLists.txt | 30 +++
.../components/armem/client/CMakeLists.txt | 1 +
.../client/articulated_object/Writer.cpp | 1 +
.../client/articulated_object/Writer.h | 1 +
8 files changed, 459 insertions(+), 1 deletion(-)
create mode 100644 scenarios/ArMemObjectMemory/config/ArticulatedObjectExampleMemoryWriterClient.cfg
create mode 100644 source/RobotAPI/components/armem/client/ArticulatedObjectExampleMemoryWriterClient/ArticulatedObjectExampleMemoryWriterClient.cpp
create mode 100644 source/RobotAPI/components/armem/client/ArticulatedObjectExampleMemoryWriterClient/ArticulatedObjectExampleMemoryWriterClient.h
create mode 100644 source/RobotAPI/components/armem/client/ArticulatedObjectExampleMemoryWriterClient/CMakeLists.txt
diff --git a/scenarios/ArMemObjectMemory/ArMemObjectMemory.scx b/scenarios/ArMemObjectMemory/ArMemObjectMemory.scx
index 6ecac65d7..6224cdd79 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="ArticulatedObjectStateProviderExample" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="ArticulatedObjectExampleMemoryWriterClient" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
</scenario>
diff --git a/scenarios/ArMemObjectMemory/config/ArticulatedObjectExampleMemoryWriterClient.cfg b/scenarios/ArMemObjectMemory/config/ArticulatedObjectExampleMemoryWriterClient.cfg
new file mode 100644
index 000000000..340a7f1e6
--- /dev/null
+++ b/scenarios/ArMemObjectMemory/config/ArticulatedObjectExampleMemoryWriterClient.cfg
@@ -0,0 +1,227 @@
+# ==================================================================
+# ArticulatedObjectExampleMemoryWriterClient 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.ArticulatedObjectExampleMemoryWriterClient.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.ArticulatedObjectExampleMemoryWriterClient.EnableProfiling = false
+
+
+# ArmarX.ArticulatedObjectExampleMemoryWriterClient.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.ArticulatedObjectExampleMemoryWriterClient.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.ArticulatedObjectExampleMemoryWriterClient.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectExampleMemoryWriterClient.ObjectName = ""
+
+
+# ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.write.ArMemMemoryNameSystem: Ice object name of the `MemoryNameSystem` component.
+# Attributes:
+# - Default: ArMemMemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.write.ArMemMemoryNameSystem = MemoryNameSystem
+
+
+# ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.write.CoreSegment: Name of the memory core segment to use.
+# Attributes:
+# - Default: ArticulatedObjectInstance
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.write.CoreSegment = ArticulatedObjectInstance
+
+
+# ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.write.MemoryName:
+# Attributes:
+# - Default: ObjectMemory
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.write.MemoryName = Object
+
+
+# ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.write.ProviderName:
+# Attributes:
+# - Case sensitivity: yes
+# - Required: yes
+ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.write.ProviderName = ExampleProvider
+
+
+# ArmarX.ArticulatedObjectExampleMemoryWriterClient.tpc.pub.DebugObserver: Name of the `DebugObserver` topic to publish data to.
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectExampleMemoryWriterClient.tpc.pub.DebugObserver = DebugObserver
+
+
+# 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/source/RobotAPI/components/armem/client/ArticulatedObjectExampleMemoryWriterClient/ArticulatedObjectExampleMemoryWriterClient.cpp b/source/RobotAPI/components/armem/client/ArticulatedObjectExampleMemoryWriterClient/ArticulatedObjectExampleMemoryWriterClient.cpp
new file mode 100644
index 000000000..3432b49f4
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/ArticulatedObjectExampleMemoryWriterClient/ArticulatedObjectExampleMemoryWriterClient.cpp
@@ -0,0 +1,133 @@
+
+
+#include "ArticulatedObjectExampleMemoryWriterClient.h"
+#include "ArmarXCore/core/PackagePath.h"
+#include "ArmarXCore/core/system/ArmarXDataPath.h"
+#include "RobotAPI/libraries/armem/core/Time.h"
+
+#include <Eigen/src/Geometry/Transform.h>
+#include <IceUtil/Time.h>
+#include <VirtualRobot/VirtualRobot.h>
+#include <memory>
+
+#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 <VirtualRobot/Robot.h>
+#include <VirtualRobot/XML/RobotIO.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 =
+ "Environment/mobile-kitchen/dishwasher-only/dishwasher.xml";
+ const std::string name = "dishwasher";
+
+ const PackagePath packagePath(armarx::ArmarXDataPath::getProject({"ArmarXObjects"}, xml), xml);
+
+ auto robot = VirtualRobot::RobotIO::loadRobot(packagePath.toSystemPath(), VirtualRobot::RobotIO::eStructure);
+
+ return robot;
+ }
+
+ 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())
+ },
+ .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(1000);
+
+ 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 * 0.2))) / 2; // in [0,1]
+
+ const std::map<std::string, float> jointValues
+ {
+ {"dishwasher_door_joint", 0.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/ArticulatedObjectExampleMemoryWriterClient/ArticulatedObjectExampleMemoryWriterClient.h b/source/RobotAPI/components/armem/client/ArticulatedObjectExampleMemoryWriterClient/ArticulatedObjectExampleMemoryWriterClient.h
new file mode 100644
index 000000000..e7d60e9c1
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/ArticulatedObjectExampleMemoryWriterClient/ArticulatedObjectExampleMemoryWriterClient.h
@@ -0,0 +1,65 @@
+
+#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_objects/client/articulated_object/Writer.h>
+
+namespace armarx::articulated_object
+{
+
+ /**
+ * @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 ArticulatedObjectExampleMemoryWriterClient :
+ virtual public armarx::Component
+ // virtual public armarx::armem::client::WriterComponentPluginUser
+ {
+ public:
+ ArticulatedObjectExampleMemoryWriterClient();
+
+ /// @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:
+
+ armarx::RunningTask<ArticulatedObjectExampleMemoryWriterClient>::pointer_type task;
+
+ armarx::DebugObserverInterfacePrx debugObserver;
+
+ std::unique_ptr<::armarx::armem::articulated_object::Writer> articulatedObjectWriter;
+
+ };
+
+} // namespace armarx::articulated_object
diff --git a/source/RobotAPI/components/armem/client/ArticulatedObjectExampleMemoryWriterClient/CMakeLists.txt b/source/RobotAPI/components/armem/client/ArticulatedObjectExampleMemoryWriterClient/CMakeLists.txt
new file mode 100644
index 000000000..b87d35756
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/ArticulatedObjectExampleMemoryWriterClient/CMakeLists.txt
@@ -0,0 +1,30 @@
+armarx_component_set_name("ArticulatedObjectExampleMemoryWriterClient")
+
+find_package(IVT QUIET)
+armarx_build_if(IVT_FOUND "IVT not available")
+
+set(COMPONENT_LIBS
+ ArmarXCore
+ ArmarXCoreInterfaces # for DebugObserverInterface
+ ArmarXGuiComponentPlugins
+ RobotAPICore
+ RobotAPIInterfaces
+ armem
+ armem_objects
+)
+
+set(SOURCES
+ ArticulatedObjectExampleMemoryWriterClient.cpp
+)
+
+set(HEADERS
+ ArticulatedObjectExampleMemoryWriterClient.h
+)
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
+
+# add unit tests
+# add_subdirectory(test)
+
+#generate the application
+armarx_generate_and_add_component_executable(COMPONENT_NAMESPACE "armarx::articulated_object")
diff --git a/source/RobotAPI/components/armem/client/CMakeLists.txt b/source/RobotAPI/components/armem/client/CMakeLists.txt
index 4eda25283..a7594d23e 100644
--- a/source/RobotAPI/components/armem/client/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/client/CMakeLists.txt
@@ -1 +1,2 @@
add_subdirectory(ExampleMemoryClient)
+add_subdirectory(ArticulatedObjectExampleMemoryWriterClient)
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 b445f3ae6..c47215d13 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
@@ -7,6 +7,7 @@
namespace armarx::armem::articulated_object
{
+ Writer::Writer(ManagedIceObject& component): MemoryConnector(component) {}
void Writer::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
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 fdcac2eaf..e396b1c4c 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
@@ -16,6 +16,7 @@ namespace armarx::armem::articulated_object
virtual public MemoryConnector
{
public:
+ Writer(ManagedIceObject& component);
virtual ~Writer() = default;
bool store(const ArticulatedObject& obj) override;
--
GitLab