diff --git a/scenarios/ArMemObjectMemory/ArMemObjectMemory.scx b/scenarios/ArMemObjectMemory/ArMemObjectMemory.scx
index 186377708d49981f0a052fcfc33d8c8286acb2fd..9e30d4e3984f80d64e40d72fdb21b7a071c8031a 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="false" iceAutoRestart="false"/>
- <application name="ArticulatedObjectLocalizerExample" instance="" package="RobotAPI" nodeName="" enabled="false" 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
index 82769e0f22bfa6de0ca678be094d396df07c89e0..9d225df2f4efadbb731a79fd45dbf852189ec80b 100644
--- a/scenarios/ArMemObjectMemory/config/ArticulatedObjectLocalizerExample.cfg
+++ b/scenarios/ArMemObjectMemory/config/ArticulatedObjectLocalizerExample.cfg
@@ -46,10 +46,10 @@
# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.CoreSegment: Name of the memory core segment to use for object classes.
# Attributes:
-# - Default: ArticulatedObjectClass
+# - Default: Class
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.CoreSegment = ArticulatedObjectClass
+# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.CoreSegment = Class
# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.MemoryName:
@@ -62,10 +62,10 @@
# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.ProviderName:
# Attributes:
-# - Default: ArmarXObjects
+# - Default: PriorKnowledgeData
# - Case sensitivity: yes
# - Required: no
-ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.ProviderName = ExampleProvider
+# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.ProviderName = PriorKnowledgeData
# ArmarX.ArticulatedObjectLocalizerExample.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
diff --git a/scenarios/ArMemObjectMemory/config/ArticulatedObjectLocalizerExample.instance1.cfg b/scenarios/ArMemObjectMemory/config/ArticulatedObjectLocalizerExample.instance1.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..9d225df2f4efadbb731a79fd45dbf852189ec80b
--- /dev/null
+++ b/scenarios/ArMemObjectMemory/config/ArticulatedObjectLocalizerExample.instance1.cfg
@@ -0,0 +1,254 @@
+# ==================================================================
+# 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: Class
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.CoreSegment = Class
+
+
+# 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: PriorKnowledgeData
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.ProviderName = PriorKnowledgeData
+
+
+# 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.ArticulatedObjectLocalizerExample.updateFrequency: Memory update frequency (write).
+# Attributes:
+# - Default: 25
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectLocalizerExample.updateFrequency = 25
+
+
+# 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 3a94ad28db4d65ce3873003c36276ea6aac0485b..e67a4c41162632026046dc8402466d2deb7ee233 100644
--- a/scenarios/ArMemObjectMemory/config/ObjectMemory.cfg
+++ b/scenarios/ArMemObjectMemory/config/ObjectMemory.cfg
@@ -550,6 +550,15 @@
# ArmarX.ObjectMemory.mem.inst.visu.oobbs = false
+# ArmarX.ObjectMemory.mem.inst.visu.useArticulatedModels: Prefer articulated object models if available.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.useArticulatedModels = true
+
+
# ArmarX.ObjectMemory.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:
diff --git a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
index 7009a9267a2828a4cd6ca5cc2238d9e153a7062f..487c9c6fc4c732fc1d4fe2f126724cb74c8dd810 100644
--- a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
+++ b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
@@ -20,8 +20,6 @@
*/
#include "ArticulatedObjectLocalizerExample.h"
-#include "ArmarXCore/core/services/tasks/PeriodicTask.h"
-
#include <memory>
@@ -30,30 +28,29 @@
#include <IceUtil/Time.h>
#include <SimoxUtility/math/pose/pose.h>
-
#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/XML/RobotIO.h>
#include <VirtualRobot/VirtualRobot.h>
+#include <VirtualRobot/XML/RobotIO.h>
-#include <ArmarXCore/core/logging/Logging.h>
+#include "ArmarXCore/core/services/tasks/PeriodicTask.h"
#include <ArmarXCore/core/PackagePath.h>
-#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.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/Time.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(memoryNameSystem)),
- articulatedObjectReader(new ::armarx::armem::articulated_object::Reader(memoryNameSystem))
+ articulatedObjectWriter(new ::armarx::armem::articulated_object::ArticulatedObjectWriter(memoryNameSystem)),
+ articulatedObjectReader(new ::armarx::armem::articulated_object::ArticulatedObjectReader(memoryNameSystem))
{
}
@@ -92,7 +89,8 @@ namespace armarx::articulated_object
ARMARX_IMPORTANT << "Running example.";
start = armem::Time::now();
- task = new PeriodicTask<ArticulatedObjectLocalizerExample>(this, &ArticulatedObjectLocalizerExample::run, 1000.f / p.updateFrequency);
+ task = new PeriodicTask<ArticulatedObjectLocalizerExample>(
+ this, &ArticulatedObjectLocalizerExample::run, 1000.f / p.updateFrequency);
task->start();
}
@@ -101,20 +99,23 @@ namespace armarx::articulated_object
task->stop();
}
- void ArticulatedObjectLocalizerExample::onExitComponent() {}
+ void ArticulatedObjectLocalizerExample::onExitComponent()
+ {
+ }
VirtualRobot::RobotPtr ArticulatedObjectLocalizerExample::createDishwasher()
{
- const std::string dishwasherName = "CupboardWithDishwasher";
+ const std::string dishwasherName = "Kitchen/mobile-dishwasher";
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;
- });
+ 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())
{
@@ -122,30 +123,23 @@ namespace armarx::articulated_object
return nullptr;
}
- return VirtualRobot::RobotIO::loadRobot(ArmarXDataPath::resolvePath(it->xml.serialize().path), VirtualRobot::RobotIO::eStructure);
- }
+ auto obj =
+ 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
+ if (not obj)
{
- .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
- };
+ return nullptr;
+ }
+
+ obj->setName("MobileDishwasher0");
+ obj->setType(it->name);
+
+ return obj;
}
+
+
void ArticulatedObjectLocalizerExample::run()
{
if (dishwasher == nullptr)
@@ -161,23 +155,19 @@ namespace armarx::articulated_object
ARMARX_DEBUG << "Reporting articulated objects";
const IceUtil::Time now = TimeUtil::GetTime();
- const float t = float((now - start).toSecondsDouble());
+ 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}
- };
+ const std::map<std::string, float> jointValues{{"dishwasher_door_joint", M_PIf32 / 2 * k},
+ {"drawer_joint", 350 * k}};
dishwasher->setGlobalPose(simox::math::pose(Eigen::Vector3f(1000, 0, 0)));
dishwasher->setJointValues(jointValues);
- armarx::armem::articulated_object::ArticulatedObject armemDishwasher =
- convert(*dishwasher, IceUtil::Time::now());
- articulatedObjectWriter->store(armemDishwasher);
+
+ articulatedObjectWriter->storeArticulatedObject(dishwasher, IceUtil::Time::now());
}
} // namespace armarx::articulated_object
diff --git a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h
index 75c0764cfb9c6103a82c68c0faba2b984ffabb86..f85a804e97337e1193ca1f38048beb103e65ce59 100644
--- a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h
+++ b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h
@@ -1,24 +1,22 @@
#pragma once
+#include <VirtualRobot/VirtualRobot.h>
-// 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>
+
#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
-// RobotAPI
-#include <RobotAPI/interface/armem/server/MemoryInterface.h>
+#include "RobotAPI/libraries/armem/core/Time.h"
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+#include <RobotAPI/interface/armem/server/MemoryInterface.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>
-#include <RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h>
-#include <VirtualRobot/VirtualRobot.h>
+#include <RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.h>
+#include <RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.h>
namespace armarx::articulated_object
{
@@ -45,7 +43,6 @@ namespace armarx::articulated_object
std::string getDefaultName() const override;
protected:
-
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
void onInitComponent() override;
@@ -55,9 +52,7 @@ namespace armarx::articulated_object
void run();
-
private:
-
VirtualRobot::RobotPtr createDishwasher();
std::shared_ptr<VirtualRobot::Robot> dishwasher;
@@ -68,14 +63,14 @@ namespace armarx::articulated_object
armarx::DebugObserverInterfacePrx debugObserver;
- std::unique_ptr<::armarx::armem::articulated_object::Writer> articulatedObjectWriter;
- std::unique_ptr<::armarx::armem::articulated_object::Reader> articulatedObjectReader;
+ std::unique_ptr<::armarx::armem::articulated_object::ArticulatedObjectWriter> articulatedObjectWriter;
+ std::unique_ptr<::armarx::armem::articulated_object::ArticulatedObjectReader>
+ articulatedObjectReader;
struct Properties
{
float updateFrequency{25.F};
} p;
-
};
-} // namespace armarx::articulated_object
+} // namespace armarx::articulated_object
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
index 422419b7821762e9b808a23e5946dc49bea14918..91ea5b23400f568891326f6dd6c79b5e603d229c 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
@@ -54,9 +54,6 @@ namespace armarx::armem::server::obj
classSegment.defineProperties(defs, prefix + "cls.");
instance::SegmentAdapter::defineProperties(defs, prefix + "inst.");
- articulatedObjectInstanceSegment.defineProperties(defs, prefix + "articulated.inst.");
- articulatedObjectClassSegment.defineProperties(defs, prefix + "articulated.cls.");
-
attachmentSegment.defineProperties(defs, prefix + "attachments.");
return defs;
@@ -68,10 +65,6 @@ namespace armarx::armem::server::obj
server::ComponentPluginUser::workingMemoryMutex),
classSegment(server::ComponentPluginUser::iceMemory,
server::ComponentPluginUser::workingMemoryMutex),
- articulatedObjectClassSegment(server::ComponentPluginUser::iceMemory,
- server::ComponentPluginUser::workingMemoryMutex),
- articulatedObjectInstanceSegment(server::ComponentPluginUser::iceMemory,
- server::ComponentPluginUser::workingMemoryMutex),
attachmentSegment(server::ComponentPluginUser::iceMemory,
server::ComponentPluginUser::workingMemoryMutex)
{
@@ -115,17 +108,6 @@ namespace armarx::armem::server::obj
instance::SegmentAdapter::init();
- initSegmentWithCatch("articulated object class", [&]()
- {
- articulatedObjectClassSegment.init();
- });
-
- initSegmentWithCatch("articulated object instance", [&]()
- {
- articulatedObjectInstanceSegment.setArticulatedObjectClassSegment(articulatedObjectClassSegment);
- articulatedObjectInstanceSegment.init();
- });
-
initSegmentWithCatch("attachment", [&]()
{
attachmentSegment.init();
@@ -159,14 +141,6 @@ namespace armarx::armem::server::obj
ArVizComponentPluginUser::getArvizClient()
);
- articulatedObjectClassSegment.connect(
- ArVizComponentPluginUser::getArvizClient()
- );
-
- articulatedObjectInstanceSegment.connect(
- ArVizComponentPluginUser::getArvizClient()
- );
-
attachmentSegment.connect(
ArVizComponentPluginUser::getArvizClient()
);
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
index a513016a3da9f0376a092768e4a977974493ca32..f29b2cef93efb2aa35a7983f4dce87d3db854b80 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
@@ -111,8 +111,6 @@ namespace armarx::armem::server::obj
clazz::Segment classSegment;
- articulated_object_class::Segment articulatedObjectClassSegment;
- articulated_object_instance::Segment articulatedObjectInstanceSegment;
attachments::Segment attachmentSegment;
// associations::Segment associationsSegment;
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
index 8c41ace39ea53283c22b3a191ae9cd9ebba2ce3c..111cced66ba9f678ba852d1005a10ded834d0946 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
@@ -89,6 +89,8 @@ namespace armarx::armem::server::robot_state
localizationSegment.defineProperties(defs);
commonVisu.defineProperties(defs);
+ setRobotUnitAsOptionalDependency();
+
return defs;
}
@@ -135,7 +137,11 @@ namespace armarx::armem::server::robot_state
void RobotStateMemory::onConnectComponent()
{
- waitUntilRobotUnitIsRunning();
+
+ if (getRobotUnit())
+ {
+ waitUntilRobotUnitIsRunning();
+ }
ARMARX_CHECK_NOT_NULL(getRobotUnit()->getKinematicUnit());
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
index ce653e0923ae8fc3bb1f1283f9e905624ce61171..23d54901fbf7fa5db2c222a09c268da111a22512 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
@@ -9,6 +9,7 @@
#include <SimoxUtility/shapes/OrientedBox.h>
#include <VirtualRobot/VirtualRobot.h>
+#include "ArmarXCore/core/PackagePath.h"
#include <IceUtil/Time.h>
@@ -96,6 +97,10 @@ namespace armarx::objpose
* @param agent The agent/robot.
*/
void updateAttached(VirtualRobot::RobotPtr agent);
+
+
+ std::optional<PackagePath> articulatedSimoxXmlPath;
+
};
using ObjectPoseSeq = std::vector<ObjectPose>;
using ObjectPoseMap = std::map<ObjectID, ObjectPose>;
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp
index 7d32ae794b427f07dc27e38edd8178460ec9fbd4..3259d2011790dad13984c6e2e442a3202519b411 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp
@@ -63,27 +63,25 @@ namespace armarx::plugins
void RobotUnitComponentPlugin::preOnInitComponent()
{
- if (!_deactivated)
+ if (_robotUnitName.empty())
+ {
+ parent<Component>().getProperty(_robotUnitName, PROPERTY_NAME);
+ }
+
+ if (not _isRobotUnitOptionalDependency)
{
- if (_robotUnitName.empty())
- {
- parent<Component>().getProperty(_robotUnitName, PROPERTY_NAME);
- }
parent<Component>().usingProxy(_robotUnitName);
}
}
void RobotUnitComponentPlugin::preOnConnectComponent()
{
- if (!_deactivated)
- {
- parent<Component>().getProxy(_robotUnit, _robotUnitName);
- }
+ parent<Component>().getProxy(_robotUnit, _robotUnitName);
}
void RobotUnitComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
{
- if (!_deactivated && !properties->hasDefinition(PROPERTY_NAME))
+ if (!properties->hasDefinition(PROPERTY_NAME))
{
properties->defineRequiredProperty<std::string>(
PROPERTY_NAME,
@@ -98,13 +96,11 @@ namespace armarx::plugins
void RobotUnitComponentPlugin::setRobotUnitName(const std::string& name)
{
- if (!_deactivated)
- {
- ARMARX_CHECK_NOT_EMPTY(name);
- ARMARX_CHECK_EMPTY(_robotUnitName);
- _robotUnitName = name;
- }
+ ARMARX_CHECK_NOT_EMPTY(name);
+ ARMARX_CHECK_EMPTY(_robotUnitName);
+ _robotUnitName = name;
}
+
const std::string& RobotUnitComponentPlugin::getRobotUnitName() const
{
return _robotUnitName;
@@ -112,7 +108,7 @@ namespace armarx::plugins
void RobotUnitComponentPlugin::deactivate()
{
- _deactivated = true;
+ _isRobotUnitOptionalDependency = true;
}
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h
index 63d78de7da9033399b92497a7bccf89161337588..80deaf3e55177a2d38385b5a272c1969f584b61f 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h
@@ -29,6 +29,12 @@ namespace armarx
void deactivate();
+ void setRobotUnitAsOptionalDependency(bool isOptional = true)
+ {
+
+ }
+
+
//controllers
public:
template<class PrxT>
@@ -63,7 +69,7 @@ namespace armarx
static constexpr const char* PROPERTY_NAME = "RobotUnitName";
RobotUnitInterfacePrx _robotUnit;
std::string _robotUnitName;
- bool _deactivated = false;
+ bool _isRobotUnitOptionalDependency = false;
std::set<std::string> _ctrls;
};
}
@@ -81,13 +87,18 @@ namespace armarx
RobotUnitInterfacePrx getRobotUnit() const;
+ void setRobotUnitAsOptionalDependency(bool isOptional = true)
+ {
+ plugin->setRobotUnitAsOptionalDependency(isOptional);
+ }
+
/**
* @brief Waits until the robot unit is running.
- *
+ *
* Although the robot unit proxy might be initialized (\see getRobotUnit()), the robot unit might
- * not be fully initialized.
- *
- * @param termCond Termination condition. If it evaluates to true, waitUntilRobotUnitIsRunning returns without waiting
+ * not be fully initialized.
+ *
+ * @param termCond Termination condition. If it evaluates to true, waitUntilRobotUnitIsRunning returns without waiting
* for the robot unit to become available.
*/
void waitUntilRobotUnitIsRunning(const std::function<bool()>& termCond = [] {return false;}) const;
diff --git a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
index 51296814ff03036d197a96683b15827ef9af5fe5..54eb919e1804ce099efe5ade4ea14294a202e0bb 100644
--- a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
@@ -30,17 +30,21 @@ armarx_add_library(
server/instance/Decay.h
server/instance/RobotHeadMovement.h
server/instance/Visu.h
+ server/instance/ArticulatedObjectVisu.h
- server/articulated_object_class/Segment.h
- server/articulated_object_instance/Segment.h
+ # server/articulated_object_class/Segment.h
+ # server/articulated_object_instance/Segment.h
# server/articulated_object/SegmentAdapter.h
- server/articulated_object_instance/Visu.h
+ # server/articulated_object_instance/Visu.h
server/attachments/Segment.h
client/articulated_object/Reader.h
client/articulated_object/Writer.h
+ client/articulated_object/ArticulatedObjectReader.h
+ client/articulated_object/ArticulatedObjectWriter.h
client/articulated_object/interfaces.h
+ client/articulated_object/utils.h
client/attachment/Reader.h
client/attachment/Writer.h
@@ -58,17 +62,21 @@ armarx_add_library(
server/instance/Decay.cpp
server/instance/RobotHeadMovement.cpp
server/instance/Visu.cpp
+ server/instance/ArticulatedObjectVisu.cpp
server/articulated_object_class/Segment.cpp
- server/articulated_object_instance/Segment.cpp
+ # server/articulated_object_instance/Segment.cpp
# server/articulated_object/SegmentAdapter.cpp
- server/articulated_object_instance/Visu.cpp
+ # server/articulated_object_instance/Visu.cpp
server/attachments/Segment.cpp
client/articulated_object/Reader.cpp
client/articulated_object/Writer.cpp
+ client/articulated_object/ArticulatedObjectReader.cpp
+ client/articulated_object/ArticulatedObjectWriter.cpp
+ client/articulated_object/utils.cpp
client/attachment/Reader.cpp
client/attachment/Writer.cpp
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c973cf24801967244589677f9b044be099f5c306
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp
@@ -0,0 +1,61 @@
+#include "ArticulatedObjectReader.h"
+
+#include <mutex>
+#include <optional>
+
+#include <VirtualRobot/XML/RobotIO.h>
+
+#include <ArmarXCore/core/PackagePath.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+
+#include "RobotAPI/libraries/ArmarXObjects/ObjectInfo.h"
+#include "RobotAPI/libraries/ArmarXObjects/ObjectPose.h"
+#include "RobotAPI/libraries/armem_objects/aron_conversions.h"
+#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/CoreSegment.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>
+
+namespace armarx::armem::articulated_object
+{
+
+ VirtualRobot::RobotPtr ArticulatedObjectReader::getArticulatedObject(const std::string& name,
+ const armem::Time& timestamp)
+ {
+ const auto descriptions = 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 == name; });
+
+ if (it == descriptions.end())
+ {
+ ARMARX_WARNING << "Articulated object " << name << " not (yet) available";
+ return nullptr;
+ }
+
+ auto obj =
+ VirtualRobot::RobotIO::loadRobot(ArmarXDataPath::resolvePath(it->xml.serialize().path),
+ VirtualRobot::RobotIO::eStructure);
+
+ if (not obj)
+ {
+ return nullptr;
+ }
+
+ obj->setName("");
+ obj->setType(it->name);
+
+ return obj;
+ }
+
+} // namespace armarx::armem::articulated_object
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.h
new file mode 100644
index 0000000000000000000000000000000000000000..5d918ddf474bfb110afaede2c4ce765ae446e931
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.h
@@ -0,0 +1,16 @@
+#pragma once
+
+#include "Reader.h"
+
+namespace armarx::armem::articulated_object
+{
+
+ class ArticulatedObjectReader : virtual public Reader
+ {
+ public:
+ using Reader::Reader;
+
+ VirtualRobot::RobotPtr getArticulatedObject(const std::string& name,
+ const armem::Time& timestamp);
+ };
+} // namespace armarx::armem::articulated_object
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f86f5f90d7787078572c9efa4b20cd5142100273
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
@@ -0,0 +1,49 @@
+#include "ArticulatedObjectWriter.h"
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+
+#include <VirtualRobot/Robot.h>
+
+namespace armarx::armem::articulated_object
+{
+ armem::articulated_object::ArticulatedObject convert(const VirtualRobot::Robot& obj,
+ const armem::Time& timestamp)
+ {
+ ARMARX_DEBUG << "Filename is " << obj.getFilename();
+
+ // TODO(fabian.reister): remove "PriorKnowledgeData" below
+
+ return armem::articulated_object::ArticulatedObject
+ {
+ .description = {
+ .name = obj.getType(),
+ .xml = PackagePath(armarx::ArmarXDataPath::getProject(
+ {"PriorKnowledgeData"}, obj.getFilename()),
+ obj.getFilename())
+ },
+ .instance = "", // TODO(fabian.reister):
+ .config = {
+ .timestamp = timestamp,
+ .globalPose = Eigen::Affine3f(obj.getRootNode()->getGlobalPose()),
+ .jointMap = obj.getJointValues()
+ },
+ .timestamp = timestamp};
+ }
+
+ bool
+ ArticulatedObjectWriter::storeArticulatedObject(const VirtualRobot::RobotPtr& articulatedObject,
+ const armem::Time& timestamp)
+ {
+
+ ARMARX_CHECK_NOT_NULL(articulatedObject);
+
+ armarx::armem::articulated_object::ArticulatedObject armemArticulatedObject =
+ convert(*articulatedObject, IceUtil::Time::now());
+
+ return store(armemArticulatedObject);
+ }
+} // namespace armarx::armem::articulated_object
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.h
new file mode 100644
index 0000000000000000000000000000000000000000..ff39d70f049fa670e41157dbb79608b16d98b66c
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.h
@@ -0,0 +1,21 @@
+
+
+#pragma once
+
+#include <VirtualRobot/VirtualRobot.h>
+
+#include "Writer.h"
+
+namespace armarx::armem::articulated_object
+{
+
+ class ArticulatedObjectWriter : virtual public Writer
+ {
+ public:
+ using Writer::Writer;
+
+ bool
+ storeArticulatedObject(const VirtualRobot::RobotPtr& articulatedObject,
+ const armem::Time& timestamp);
+ };
+} // namespace armarx::armem::articulated_object
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 8824b1e7f8b172b0c837a3441b7db97480a7e3e1..d0bfb35708104712d2bebc33b4d7676b999ddac5 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
@@ -3,16 +3,22 @@
#include <mutex>
#include <optional>
-#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/PackagePath.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include "RobotAPI/libraries/ArmarXObjects/ObjectInfo.h"
+#include "RobotAPI/libraries/ArmarXObjects/ObjectPose.h"
+#include "RobotAPI/libraries/armem_objects/aron_conversions.h"
+#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
-#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/workingmemory/CoreSegment.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 <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+
+#include "utils.h"
namespace fs = ::std::filesystem;
@@ -21,7 +27,8 @@ namespace armarx::armem::articulated_object
Reader::Reader(armem::client::MemoryNameSystem& memoryNameSystem) :
memoryNameSystem(memoryNameSystem)
- {}
+ {
+ }
void Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
@@ -235,7 +242,7 @@ namespace armarx::armem::articulated_object
.getCoreSegment(properties.coreInstanceSegmentName);
// clang-format on
- for (const auto &[_, providerSegment] : coreSegment.providerSegments())
+ for (const auto& [_, providerSegment] : coreSegment.providerSegments())
{
const auto entities = simox::alg::get_values(providerSegment.entities());
@@ -263,16 +270,16 @@ namespace armarx::armem::articulated_object
return std::nullopt;
}
+
std::optional<robot::RobotDescription>
Reader::getRobotDescription(const armarx::armem::wm::Memory& memory) const
{
// clang-format off
const armem::wm::CoreSegment& coreSegment = memory
.getCoreSegment(properties.coreClassSegmentName);
- // .getProviderSegment(properties.providerName); // TODO(fabian.reister): all
// clang-format on
- for (const auto &[_, providerSegment] : coreSegment.providerSegments())
+ for (const auto& [_, providerSegment] : coreSegment.providerSegments())
{
const auto entities = simox::alg::get_values(providerSegment.entities());
@@ -280,7 +287,6 @@ namespace armarx::armem::articulated_object
{
ARMARX_WARNING << "No entity found";
continue;
- // return std::nullopt;
}
const auto entitySnapshots = simox::alg::get_values(entities.front().history());
@@ -289,13 +295,13 @@ namespace armarx::armem::articulated_object
{
ARMARX_WARNING << "No entity snapshots found";
continue;
- // return std::nullopt;
}
- // TODO(fabian.reister): check if 0 available
- const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
-
- return robot::convertRobotDescription(instance);
+ if (entitySnapshots.front().hasInstance(0))
+ {
+ const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
+ return convertRobotDescription(instance);
+ }
}
return std::nullopt;
@@ -309,9 +315,9 @@ namespace armarx::armem::articulated_object
const armem::wm::CoreSegment& coreSegment =
memory.getCoreSegment(properties.coreClassSegmentName);
- for (const auto &[providerName, providerSegment] : coreSegment.providerSegments())
+ for (const auto& [providerName, providerSegment] : coreSegment.providerSegments())
{
- for (const auto &[name, entity] : providerSegment.entities())
+ for (const auto& [name, entity] : providerSegment.entities())
{
if (entity.empty())
{
@@ -322,7 +328,7 @@ namespace armarx::armem::articulated_object
const auto entitySnapshots = simox::alg::get_values(entity.history());
const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
- const auto robotDescription = robot::convertRobotDescription(instance);
+ const auto robotDescription = convertRobotDescription(instance);
if (robotDescription)
{
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 5bc5efc3e3fe2a66716467444bd4ee54c86ac92c..f5dac4035dfb4c8ccab3e82e78af035d64fd981f 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
@@ -24,6 +24,8 @@
#include <mutex>
#include <optional>
+#include <VirtualRobot/VirtualRobot.h>
+
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
@@ -31,11 +33,10 @@
#include "interfaces.h"
-
namespace armarx::armem::articulated_object
{
- class Reader:
- virtual public ReaderInterface
+
+ class Reader : virtual public ReaderInterface
{
public:
Reader(armem::client::MemoryNameSystem& memoryNameSystem);
@@ -46,11 +47,15 @@ namespace armarx::armem::articulated_object
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<ArticulatedObject> get(const std::string& name,
+ const armem::Time& timestamp) override;
+ ArticulatedObject get(const ArticulatedObjectDescription& description,
+ const armem::Time& timestamp) override;
- 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::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);
@@ -60,20 +65,24 @@ namespace armarx::armem::articulated_object
// TODO(fabian.reister): register property defs
protected:
- 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;
+ 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 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 = "ArmarXObjects";
+ std::string memoryName = "Object";
+ std::string coreInstanceSegmentName = "Instance";
+ std::string coreClassSegmentName = "Class";
+ std::string providerName = "PriorKnowledgeData";
} properties;
const std::string propertyPrefix = "mem.obj.articulated.";
@@ -85,4 +94,4 @@ namespace armarx::armem::articulated_object
};
-} // namespace armarx::armem::articulated_object
+} // namespace armarx::armem::articulated_object
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 f351382f36d94372154116327dc3e65e38eb0dfc..237bc31cc8827f0fb9f80284bf0c639c23d093ec 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
@@ -1,28 +1,38 @@
#include "Writer.h"
-#include <IceUtil/Time.h>
-#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <mutex>
#include <optional>
+#include <IceUtil/Time.h>
+
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+
+#include "ArmarXCore/core/exceptions/LocalException.h"
#include <ArmarXCore/core/logging/Logging.h>
+#include "RobotAPI/libraries/ArmarXObjects/ObjectID.h"
+#include "RobotAPI/libraries/ArmarXObjects/ObjectInfo.h"
+#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectID.aron.generated.h>
+#include <RobotAPI/libraries/armem/client/query.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
#include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem/client/query.h>
+#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
-#include <RobotAPI/libraries/armem_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/aron/RobotDescription.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
+#include "utils.h"
namespace armarx::armem::articulated_object
{
Writer::Writer(armem::client::MemoryNameSystem& memoryNameSystem) :
memoryNameSystem(memoryNameSystem)
- {}
+ {
+ }
void Writer::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
@@ -59,7 +69,8 @@ namespace armarx::armem::articulated_object
return;
}
- const auto resultCoreClassSegment = memoryWriter.addSegment(properties.coreClassSegmentName, properties.providerName);
+ const auto resultCoreClassSegment =
+ memoryWriter.addSegment(properties.coreClassSegmentName, properties.providerName);
const auto resultCoreInstanceSegmentName =
memoryWriter.addSegment(properties.coreInstanceSegmentName, properties.providerName);
@@ -82,7 +93,8 @@ namespace armarx::armem::articulated_object
// TODO(fabian.reister): implement
}
- void Writer::updateKnownObjects(const armem::MemoryID& subscriptionID, const std::vector<armem::MemoryID>& snapshotIDs)
+ void Writer::updateKnownObjects(const armem::MemoryID& subscriptionID,
+ const std::vector<armem::MemoryID>& snapshotIDs)
{
ARMARX_INFO << "New objects available!";
updateKnownObjects();
@@ -91,6 +103,8 @@ namespace armarx::armem::articulated_object
void Writer::updateKnownObjects()
{
knownObjects = queryDescriptions(IceUtil::Time::now());
+
+ ARMARX_INFO << "Known articulated objects " << simox::alg::get_keys(knownObjects);
}
std::optional<armem::MemoryID> Writer::storeOrGetClass(const ArticulatedObject& obj)
@@ -103,6 +117,8 @@ namespace armarx::armem::articulated_object
return objectId->second;
}
+ throw LocalException("articulated object class " + obj.description.name + " not found");
+
// otherwise create
if (properties.allowClassCreation)
{
@@ -119,7 +135,8 @@ namespace armarx::armem::articulated_object
ARMARX_DEBUG << "Trying to create core segment + provider segment";
// TODO(fabian.reister): variable provider segment
- const auto result = memoryWriter.addSegment(properties.coreClassSegmentName, properties.providerName);
+ const auto result =
+ memoryWriter.addSegment(properties.coreClassSegmentName, properties.providerName);
if (not result.success)
{
@@ -131,9 +148,7 @@ namespace armarx::armem::articulated_object
const auto providerId = armem::MemoryID(result.segmentID);
const auto entityID =
- providerId
- .withEntityName(obj.description.name)
- .withTimestamp(timestamp);
+ providerId.withEntityName(obj.description.name).withTimestamp(timestamp);
armem::EntityUpdate update;
update.entityID = entityID;
@@ -175,26 +190,6 @@ namespace armarx::armem::articulated_object
{
std::lock_guard g{memoryWriterMutex};
- ARMARX_DEBUG << "Trying to create core segment + provider segment";
-
- // Provider segments are now added when necessary.
- // Adding them explicitly is only needed to set a deriving provider segment type.
- if (false)
- {
- const auto result =
- memoryWriter.addSegment(properties.coreInstanceSegmentName, properties.providerName);
-
- if (not result.success)
- {
- ARMARX_ERROR << "Creating core segment failed. Reason: " << result.errorMessage;
- return false;
- }
- else
- {
- ARMARX_IMPORTANT << VAROUT(result.segmentID);
- }
- }
-
const auto& timestamp = obj.timestamp;
const auto providerId = armem::MemoryID()
@@ -206,21 +201,36 @@ namespace armarx::armem::articulated_object
update.entityID = providerId.withEntityName(obj.description.name);
// .withTimestamp(timestamp); // You only need to specify the entity ID, not the snapshot ID
- arondto::Robot aronArticulatedObject;
- robot::toAron(aronArticulatedObject, obj);
+ // arondto::Robot aronArticulatedObject;
+ // robot::toAron(aronArticulatedObject, obj);
- const auto descriptionId = storeOrGetClass(obj);
+ arondto::ObjectInstance objectInstance;
+ toAron(objectInstance, obj.config);
- if (not descriptionId)
+ const auto classId = storeOrGetClass(obj);
+
+ if (not classId)
{
ARMARX_WARNING << "Could not get class for object " << obj.description.name;
return false;
}
// install memory link
- toAron(aronArticulatedObject.description, *descriptionId);
+ toAron(objectInstance.classID, *classId);
- update.instancesData = {aronArticulatedObject.toAron()};
+ armem::MemoryID id;
+ id.setEntityID(classId->getEntityID());
+
+ armarx::ObjectID objectId(id.entityName);
+
+ armarx::arondto::ObjectID cs;
+ cs.className = objectId.className();
+ cs.instanceName = objectId.instanceName();
+ cs.dataset = objectId.dataset();
+
+ objectInstance.pose.objectID = cs;
+
+ update.instancesData = {objectInstance.toAron()};
update.timeCreated = timestamp;
ARMARX_DEBUG << "Committing " << update << " at time " << timestamp;
@@ -247,18 +257,12 @@ namespace armarx::armem::articulated_object
return false;
}
- // TODO(fabian.reister): integrate memory link
return storeInstance(obj);
}
- // 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
+ std::optional<robot::RobotDescription>
+ Writer::getRobotDescription(const armarx::armem::wm::Memory& memory) const
{
// clang-format off
const armem::wm::ProviderSegment& providerSegment = memory
@@ -284,14 +288,16 @@ namespace armarx::armem::articulated_object
// TODO(fabian.reister): check if 0 available
const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
- return robot::convertRobotDescription(instance);
+ return 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>
+ 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);
+ const armem::wm::CoreSegment& coreSegment =
+ memory.getCoreSegment(properties.coreClassSegmentName);
for (const auto& [providerName, providerSegment] : coreSegment.providerSegments())
{
@@ -303,11 +309,11 @@ namespace armarx::armem::articulated_object
continue;
}
- const auto entitySnapshots = simox::alg::get_values(entity.history());
+ 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);
+ const auto robotDescription = convertRobotDescription(instance);
if (robotDescription)
{
@@ -315,14 +321,13 @@ namespace armarx::armem::articulated_object
descriptions.insert({robotDescription->name, snapshotID});
}
}
-
}
return descriptions;
}
-
- std::unordered_map<std::string, armem::MemoryID> Writer::queryDescriptions(const armem::Time& timestamp)
+ std::unordered_map<std::string, armem::MemoryID>
+ Writer::queryDescriptions(const armem::Time& timestamp)
{
// Query all entities from provider.
armem::client::query::Builder qb;
@@ -347,5 +352,4 @@ namespace armarx::armem::articulated_object
return getRobotDescriptions(qResult.memory);
}
-
} // namespace armarx::armem::articulated_object
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 c5fa97dd6e5d37402b1a2a7d0626131348f1e25c..95495220780d95278685c8c349462708aa822a3e 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
@@ -74,8 +74,8 @@ namespace armarx::armem::articulated_object
struct Properties
{
std::string memoryName = "Object";
- std::string coreInstanceSegmentName = "ArticulatedObjectInstance";
- std::string coreClassSegmentName = "ArticulatedObjectClass";
+ std::string coreInstanceSegmentName = "Instance";
+ std::string coreClassSegmentName = "Class";
std::string providerName = "";
bool allowClassCreation = false;
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/utils.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/utils.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c6da5e109ff356ecbe4a4f2b569c332a5e34cfcb
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/utils.cpp
@@ -0,0 +1,34 @@
+#include "utils.h"
+
+#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
+
+namespace armarx::armem::articulated_object
+{
+ std::optional<robot::RobotDescription>
+ convertRobotDescription(const armem::wm::EntityInstance& instance)
+ {
+
+ arondto::ObjectClass aronObjectInfo;
+ try
+ {
+ aronObjectInfo.fromAron(instance.data());
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << "Conversion to ObjectPose failed!";
+ return std::nullopt;
+ }
+
+ robot::RobotDescription robotDescription;
+ fromAron(aronObjectInfo, robotDescription);
+
+ // check if robot description is valid
+ const auto xml = robotDescription.xml.serialize();
+ if (robotDescription.name.empty() or xml.package.empty() or xml.path.empty())
+ {
+ return std::nullopt;
+ }
+
+ return robotDescription;
+ }
+} // namespace armarx::armem::articulated_object
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/utils.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/utils.h
new file mode 100644
index 0000000000000000000000000000000000000000..2e51a9e4b336bd4550027227d30a0236efa0f217
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/utils.h
@@ -0,0 +1,16 @@
+#pragma once
+
+#include <optional>
+
+#include "RobotAPI/libraries/armem/core/workingmemory/EntityInstance.h"
+#include "RobotAPI/libraries/armem_robot/types.h"
+#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
+
+
+namespace armarx::armem::articulated_object
+{
+
+ std::optional<robot::RobotDescription>
+ convertRobotDescription(const armem::wm::EntityInstance& instance);
+
+} // namespace armarx::armem::articulated_object
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/ArticulatedObjectVisu.cpp
similarity index 73%
rename from source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.cpp
rename to source/RobotAPI/libraries/armem_objects/server/instance/ArticulatedObjectVisu.cpp
index d1ee962acf267c3263676772816ba7d79f8e46cb..f1b90e5daec4edf42590ce0d8aad7a4aa9d83838 100644
--- a/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/ArticulatedObjectVisu.cpp
@@ -1,31 +1,30 @@
-#include "Visu.h"
+#include "ArticulatedObjectVisu.h"
#include <algorithm>
+#include <SimoxUtility/math/pose.h>
+
+#include "ArmarXCore/core/services/tasks/PeriodicTask.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
+namespace armarx::armem::server::obj::instance
{
- void Visu::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ void ArticulatedObjectVisu::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.");
+ defs->optional(
+ p.enabled, prefix + "enabled", "Enable or disable visualization of objects.");
+ defs->optional(p.frequencyHz, prefix + "frequenzyHz", "Frequency of visualization.");
}
-
- viz::Layer Visu::visualizeProvider(
+ viz::Layer ArticulatedObjectVisu::visualizeProvider(
const std::string& providerName,
const armarx::armem::articulated_object::ArticulatedObjects& objects) const
{
@@ -36,12 +35,13 @@ namespace armarx::armem::server::obj::articulated_object_instance
return layer;
}
-
- void Visu::visualizeObjects(viz::Layer& layer, const armarx::armem::articulated_object::ArticulatedObjects& objects) const
+ void ArticulatedObjectVisu::visualizeObjects(
+ viz::Layer& layer,
+ const armarx::armem::articulated_object::ArticulatedObjects& objects) const
{
- const auto visualizeObject = [&](const armarx::armem::articulated_object::ArticulatedObject & obj)
+ const auto visualizeObject =
+ [&](const armarx::armem::articulated_object::ArticulatedObject & obj)
{
-
const auto xmlPath = obj.description.xml.serialize();
// clang-format off
@@ -58,40 +58,40 @@ namespace armarx::armem::server::obj::articulated_object_instance
};
std::for_each(objects.begin(), objects.end(), visualizeObject);
-
}
- void Visu::init()
+ void ArticulatedObjectVisu::init()
{
- updateTask = new PeriodicTask<Visu>(this, &Visu::visualizeRun, 1000 / p.frequencyHz);
+ updateTask = new PeriodicTask<ArticulatedObjectVisu>(this, &ArticulatedObjectVisu::visualizeRun, 1000 / p.frequencyHz);
+
+ ARMARX_INFO << "ArticulatedObjectVisu: init";
updateTask->start();
}
-
- void Visu::visualizeRun()
+ void ArticulatedObjectVisu::visualizeRun()
{
// std::scoped_lock lock(visuMutex);
- ARMARX_DEBUG << "Update task";
+ ARMARX_INFO << "Update task";
- if (not p.enabled)
- {
- return;
- }
+ // if (not p.enabled)
+ // {
+ // return;
+ // }
// TIMING_START(Visu);
const auto articulatedObjects = segment.getArticulatedObjects();
- ARMARX_DEBUG << "Found " << articulatedObjects.size() << " articulated objects";
+ ARMARX_INFO << "Found " << articulatedObjects.size() << " articulated objects";
viz::Layer layer = arviz.layer("ArticulatedObjectInstances");
- ARMARX_DEBUG << "visualizing objects";
+ ARMARX_INFO << "visualizing objects";
visualizeObjects(layer, articulatedObjects);
- ARMARX_DEBUG << "Committing objects";
+ ARMARX_INFO << "Committing objects";
arviz.commit({layer});
- ARMARX_DEBUG << "Done committing";
+ ARMARX_INFO << "Done committing";
// TIMING_END_STREAM(Visu, ARMARX_VERBOSE);
@@ -102,7 +102,6 @@ namespace armarx::armem::server::obj::articulated_object_instance
// { "t Visualize [ms]", new Variant(Visu.toMilliSecondsDouble()) },
// });
// }
-
}
// void Visu::RemoteGui::setup(const Visu& visu)
@@ -155,6 +154,4 @@ namespace armarx::armem::server::obj::articulated_object_instance
// visu.objectFramesScale = objectFramesScale.getValue();
// }
-
-
-} // namespace armarx::armem::server::obj::articulated_object_instance
+} // namespace armarx::armem::server::obj::instance
diff --git a/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.h b/source/RobotAPI/libraries/armem_objects/server/instance/ArticulatedObjectVisu.h
similarity index 81%
rename from source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.h
rename to source/RobotAPI/libraries/armem_objects/server/instance/ArticulatedObjectVisu.h
index 3590270fe545c830b4907082dd127d8a44598587..9c66fd7fb613d41e36f65fe1d8273dc8bd8b0e13 100644
--- a/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/ArticulatedObjectVisu.h
@@ -27,56 +27,53 @@
// #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::obj::articulated_object_instance
+namespace armarx::armem::server::obj::instance
{
class Segment;
/**
* @brief Visualizes articulated objects
*/
- class Visu : public armarx::Logging
+ class ArticulatedObjectVisu : public armarx::Logging
{
public:
+ ArticulatedObjectVisu(const viz::Client& arviz, Segment& segment) :
+ arviz(arviz), segment(segment)
+ {
+ }
- Visu(const viz::Client& arviz, const Segment& segment): arviz(arviz), segment(segment) {}
-
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "visu.");
+ void defineProperties(armarx::PropertyDefinitionsPtr defs,
+ const std::string& prefix = "visu.");
void init();
protected:
viz::Layer visualizeProvider(
const std::string& providerName,
- const armarx::armem::articulated_object::ArticulatedObjects& objects
- ) const;
+ const armarx::armem::articulated_object::ArticulatedObjects& objects) const;
void visualizeObjects(
viz::Layer& layer,
- const armarx::armem::articulated_object::ArticulatedObjects& objects
- ) const;
-
+ const armarx::armem::articulated_object::ArticulatedObjects& objects) const;
private:
viz::Client arviz;
- const Segment& segment;
+ Segment& segment;
struct Properties
{
- bool enabled = true;
+ bool enabled = true;
float frequencyHz = 25;
} p;
-
- PeriodicTask<Visu>::pointer_type updateTask;
+ PeriodicTask<ArticulatedObjectVisu>::pointer_type updateTask;
void visualizeRun();
// struct RemoteGui
@@ -95,7 +92,6 @@ namespace armarx::armem::server::obj::articulated_object_instance
// // void setup(const Visu& visu);
// // void update(Visu& visu);
// };
-
};
-} // namespace armarx::armem::server::obj::articulated_object_instance
+} // namespace armarx::armem::server::obj::instance
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index 31f25a68ad359505e52ea3928c16bde8fb10385b..db706075c9bd5431fefd2b04b7634cf5e8ff6747 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -1,5 +1,6 @@
#include "Segment.h"
+#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
#include <RobotAPI/libraries/armem_objects/SceneSnapshot.h>
@@ -18,6 +19,8 @@
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
+
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
@@ -30,6 +33,7 @@
#include <Eigen/Geometry>
+#include <IceUtil/Time.h>
#include <sstream>
@@ -111,6 +115,13 @@ namespace armarx::armem::server::obj::instance
}
}
+ void Segment::connect(viz::Client arviz)
+ {
+ // ARMARX_INFO << "ArticulatedObjectVisu";
+ // this->visu = std::make_unique<ArticulatedObjectVisu>(arviz, *this);
+ // visu->init();
+ }
+
Segment::CommitStats Segment::commitObjectPoses(
const std::string& providerName,
@@ -228,6 +239,7 @@ namespace armarx::armem::server::obj::instance
}
toAron(dto.sourceID, MemoryID());
update.instancesData.push_back(dto.toAron());
+
}
iceMemory.commit(commit);
}
@@ -459,6 +471,64 @@ namespace armarx::armem::server::obj::instance
}
+ ::armarx::armem::articulated_object::ArticulatedObjects Segment::getArticulatedObjects()
+ {
+ objpose::ObjectPoseMap objectPoses = getObjectPoses(IceUtil::Time::now());
+
+ ARMARX_INFO << "Found " << objectPoses.size() << " object poses";
+
+ ::armarx::armem::articulated_object::ArticulatedObjects objects;
+ for (const auto&[objectId, objectPose] : objectPoses)
+ {
+ armem::articulated_object::ArticulatedObject articulatedObject;
+ articulatedObject.config.jointMap = objectPose.objectJointValues;
+ articulatedObject.config.globalPose = objectPose.objectPoseGlobal;
+ articulatedObject.config.timestamp = objectPose.timestamp;
+ articulatedObject.instance = objectPose.objectID.instanceName();
+ articulatedObject.timestamp = objectPose.timestamp;
+
+
+ ARMARX_INFO << "Object id is " << objectId.str();
+ ARMARX_INFO << "Object id for objectPose is " << objectPose.objectID.str();
+
+ // Search for object class.
+ if (auto classInstance = findClassInstance(objectPose.objectID))
+ {
+ arondto::ObjectClass dto;
+
+ try
+ {
+ dto.fromAron(classInstance->data());
+ robot::RobotDescription description;
+
+ fromAron(dto, description);
+ articulatedObject.description = description;
+
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << "Conversion failed!";
+ continue;
+ }
+ }
+ else
+ {
+ ARMARX_WARNING << "Class instance not found!";
+ continue;
+ }
+
+
+
+ if (not articulatedObject.config.jointMap.empty())
+ {
+ objects.push_back(articulatedObject);
+ }
+ }
+
+ return objects;
+ }
+
+
std::optional<simox::OrientedBoxf> Segment::getObjectOOBB(const ObjectID& id)
{
return oobbCache.get(id);
@@ -719,11 +789,14 @@ namespace armarx::armem::server::obj::instance
return provSeg.getEntity(classID.str()).getLatestSnapshot().getInstance(0);
}
}
+
+ ARMARX_WARNING << "No provider segment for classID " << classID.str() << " found";
return std::nullopt;
}
- catch (const armem::error::ArMemError&)
+ catch (const armem::error::ArMemError& e)
{
// Some segment or entity did not exist.
+ ARMARX_WARNING << e.what();
return std::nullopt;
}
}
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
index 3d7af768b9ec04457050342d6b83480386d9d579..3ec6a2f22f41569945cc0c0ed3f5fd0a4021241e 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
@@ -10,6 +10,7 @@
#include <ArmarXCore/core/logging/Logging.h>
+#include "RobotAPI/components/ArViz/Client/Client.h"
#include <RobotAPI/interface/core/RobotState.h>
#include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.h>
@@ -23,6 +24,8 @@
#include "Decay.h"
+#include "ArticulatedObjectVisu.h"
+
namespace armarx::armem::obj
{
@@ -55,6 +58,9 @@ namespace armarx::armem::server::obj::instance
void init();
+ void connect(viz::Client arviz);
+
+
CommitStats commitObjectPoses(
const std::string& providerName,
@@ -104,6 +110,9 @@ namespace armarx::armem::server::obj::instance
static arondto::ObjectInstance getLatestInstanceData(const wm::Entity& entity);
+ ::armarx::armem::articulated_object::ArticulatedObjects getArticulatedObjects();
+
+
private:
@@ -218,6 +227,9 @@ namespace armarx::armem::server::obj::instance
void update(Segment& data);
};
+ private:
+ std::unique_ptr<ArticulatedObjectVisu> visu;
+
};
}
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
index 54b298b6c1f98e160d5803659ca078de1158c9cd..e0577877f2ac9e9c7e97e8aaa50d99c438fe4c9f 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
@@ -93,6 +93,8 @@ namespace armarx::armem::server::obj::instance
});
visu.updateTask->start();
}
+
+ segment.connect(arviz);
}
void SegmentAdapter::reportProviderAvailable(const std::string& providerName, const objpose::ProviderInfo& info, const Ice::Current&)
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
index 7f95e43878aee62619f8c4997db25460e11ed8cd..ac5e5e1c36e66d47251700ec6565a9fb61ce5430 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
@@ -93,21 +93,29 @@ namespace armarx::armem::server::robot_state::proprioception
{
std::unordered_map<std::string, std::map<std::string, float>> jointMap;
+ std::lock_guard g{memoryMutex};
+
for (const auto& [robotName, provSeg] : iceMemory.workingMemory->getCoreSegment(p.coreSegment))
{
for (const auto& [name, entity] : provSeg.entities())
{
- const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
+ try
+ {
+ const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
+ const auto jointState = tryCast<armarx::armem::arondto::JointState>(entityInstance);
- 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);
- if (not jointState)
- {
- // ARMARX_WARNING << "Could not convert entity instance to 'JointState'";
- continue;
}
+ catch (...) // empty history etc
+ {
- jointMap[robotName].emplace(jointState->name, jointState->position);
+ }
}
}