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/ArViz/Client/drawer/ArVizDrawerBase.cpp b/source/RobotAPI/components/ArViz/Client/drawer/ArVizDrawerBase.cpp
index a19032cd088ccdf49e16af5ea394d73a92b30878..9d2c8e7ed36b6a494556ecdd55e652aea93b7e77 100644
--- a/source/RobotAPI/components/ArViz/Client/drawer/ArVizDrawerBase.cpp
+++ b/source/RobotAPI/components/ArViz/Client/drawer/ArVizDrawerBase.cpp
@@ -6,21 +6,27 @@
namespace armarx
{
- ArVizDrawerBase::ArVizDrawerBase(armarx::viz::Client &arviz) : arvizClient(arviz) {}
+ ArVizDrawerBase::ArVizDrawerBase(armarx::viz::Client& arviz) : arvizClient(arviz) {}
ArVizDrawerBase::~ArVizDrawerBase() = default;
- viz::Client &ArVizDrawerBase::arviz() { return arvizClient; }
+ viz::ScopedClient& ArVizDrawerBase::arviz()
+ {
+ return arvizClient;
+ }
- const viz::Client &ArVizDrawerBase::arviz() const { return arvizClient; }
+ const viz::ScopedClient& ArVizDrawerBase::arviz() const
+ {
+ return arvizClient;
+ }
- void ArVizDrawerBase::commit(const viz::Layer &layer)
+ void ArVizDrawerBase::commit(const viz::Layer& layer)
{
std::lock_guard guard{layerMtx};
arvizClient.commit(layer);
}
- void ArVizDrawerBase::commit(const std::vector<viz::Layer> &layers)
+ void ArVizDrawerBase::commit(const std::vector<viz::Layer>& layers)
{
std::lock_guard guard{layerMtx};
arvizClient.commit(layers);
diff --git a/source/RobotAPI/components/ArViz/Client/drawer/ArVizDrawerBase.h b/source/RobotAPI/components/ArViz/Client/drawer/ArVizDrawerBase.h
index f7c813be72283ec4899f402a1133c41ab5a381e2..e302acc5c27e025aac9c103793f7a5ed726d76d9 100644
--- a/source/RobotAPI/components/ArViz/Client/drawer/ArVizDrawerBase.h
+++ b/source/RobotAPI/components/ArViz/Client/drawer/ArVizDrawerBase.h
@@ -24,6 +24,8 @@
#include <mutex>
#include <vector>
+#include "RobotAPI/components/ArViz/Client/ScopedClient.h"
+
namespace armarx
{
// forward declaration
@@ -42,19 +44,19 @@ namespace armarx
*/
class ArVizDrawerBase
{
- public:
- ArVizDrawerBase(armarx::viz::Client &arviz);
+ public:
+ ArVizDrawerBase(armarx::viz::Client& arviz);
virtual ~ArVizDrawerBase();
- protected:
- viz::Client &arviz();
- const viz::Client &arviz() const;
+ protected:
+ viz::ScopedClient& arviz();
+ const viz::ScopedClient& arviz() const;
- void commit(const viz::Layer &layer);
- void commit(const std::vector<viz::Layer> &layers);
+ void commit(const viz::Layer& layer);
+ void commit(const std::vector<viz::Layer>& layers);
- private:
- viz::Client &arvizClient;
+ private:
+ viz::ScopedClient arvizClient;
std::mutex layerMtx;
};
diff --git a/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp b/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
index 96ad7fed33e9be2813679f6ab20405f85300fc57..5f99047a649e80be511efd37c1cfbc8ac438031a 100644
--- a/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
@@ -51,11 +51,11 @@ namespace armarx::viz
return "UNKNOWN";
}
- static void updateVisualizationCB(void* data, SoSensor* sensor)
- {
- auto* visu = static_cast<CoinVisualizer*>(data);
- visu->update();
- }
+ // static void updateVisualizationCB(void* data, SoSensor* sensor)
+ // {
+ // auto* visu = static_cast<CoinVisualizer*>(data);
+ // visu->update();
+ // }
struct TimedBlock
{
@@ -89,10 +89,10 @@ namespace armarx::viz
&CoinVisualizerWrapper::onUpdateFailure);
root = new SoSeparator;
- timerSensor = new SoTimerSensor(updateVisualizationCB, this);
+ //timerSensor = new SoTimerSensor(updateVisualizationCB, this);
- float cycleTimeMS = 33.0f;
- timerSensor->setInterval(SbTime(cycleTimeMS / 1000.0f));
+ //float cycleTimeMS = 33.0f;
+ //timerSensor->setInterval(SbTime(cycleTimeMS / 1000.0f));
}
CoinVisualizer::~CoinVisualizer()
@@ -115,8 +115,8 @@ namespace armarx::viz
state = CoinVisualizerState::STARTING;
stateStorage = storage;
- SoSensorManager* sensor_mgr = SoDB::getSensorManager();
- sensor_mgr->insertTimerSensor(timerSensor);
+ //SoSensorManager* sensor_mgr = SoDB::getSensorManager();
+ //sensor_mgr->insertTimerSensor(timerSensor);
}
void CoinVisualizer::stop()
@@ -272,6 +272,10 @@ namespace armarx::viz
void CoinVisualizer::update()
{
+ {
+ std::lock_guard lock(timingMutex);
+ lastTiming.updateToggle = (lastTiming.updateToggle + 1) % 10;
+ }
switch (state)
{
case CoinVisualizerState::STARTING:
@@ -312,6 +316,7 @@ namespace armarx::viz
// We should restart the pull for updates so it can run in parallel
data::LayerUpdates currentUpdates = pulledUpdates;
updateResult = CoinVisualizerUpdateResult::WAITING;
+ timing.waitStart = time_start;
storage->begin_pullUpdatesSince(currentUpdates.revision, callback);
auto layerIDsBefore = getLayerIDs();
@@ -343,6 +348,7 @@ namespace armarx::viz
// Copy the timing result
std::lock_guard lock(timingMutex);
timing.counter = lastTiming.counter + 1;
+ timing.updateToggle = lastTiming.updateToggle;
lastTiming = std::move(timing);
}
}
@@ -350,7 +356,13 @@ namespace armarx::viz
case CoinVisualizerUpdateResult::WAITING:
{
// Still waiting for result
- } break;
+ {
+ // Copy the timing result
+ std::lock_guard lock(timingMutex);
+ lastTiming.waitDuration = IceUtil::Time::now() - lastTiming.waitStart;
+ }
+ }
+ break;
case CoinVisualizerUpdateResult::FAILURE:
{
std::unique_lock<std::mutex> lock(stateMutex);
diff --git a/source/RobotAPI/components/ArViz/Coin/Visualizer.h b/source/RobotAPI/components/ArViz/Coin/Visualizer.h
index 5cb71b0fecc568e06b42ae49d8f8e9525104feea..49ad2a51bd6f70dfddac1785296b8abc07ba08c9 100644
--- a/source/RobotAPI/components/ArViz/Coin/Visualizer.h
+++ b/source/RobotAPI/components/ArViz/Coin/Visualizer.h
@@ -174,6 +174,11 @@ namespace armarx::viz
IceUtil::Time layersChanged = IceUtil::Time::seconds(0);
IceUtil::Time total = IceUtil::Time::seconds(0);
+ IceUtil::Time waitStart = IceUtil::Time::seconds(0);
+ IceUtil::Time waitDuration = IceUtil::Time::seconds(0);
+
+ int updateToggle = 0;
+
int counter = 0;
};
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/SkillObserver/SkillObserver.h b/source/RobotAPI/components/SkillObserver/SkillObserver.h
index b3b0e4243429919a8805b55188edfabadb724a45..b9f927312888048a01bedcfdc53dadce6b6902ba 100644
--- a/source/RobotAPI/components/SkillObserver/SkillObserver.h
+++ b/source/RobotAPI/components/SkillObserver/SkillObserver.h
@@ -22,6 +22,7 @@
#pragma once
+#include <mutex>
#include <thread>
#include <string>
#include <vector>
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/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.cpp b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.cpp
index 2acb4a23b423df17ca7f80ffc76251634bcb933f..fc0274a7eb932135018cf356ca39105ea70890c6 100644
--- a/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.cpp
+++ b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.cpp
@@ -171,6 +171,8 @@ armarx::ObstacleAwarePlatformUnit::moveTo(
invalidate(m_control_data.target_vel);
invalidate(m_control_data.target_rot_vel);
+ m_rot_pid_controller.reset();
+
schedule_high_level_control_loop(control_mode::position);
}
@@ -230,10 +232,14 @@ armarx::ObstacleAwarePlatformUnit::moveRelative(
void
armarx::ObstacleAwarePlatformUnit::setMaxVelocities(
- const float max_pos_vel,
- const float max_rot_vel,
+ float max_pos_vel,
+ float max_rot_vel,
const Ice::Current&)
{
+ // ensure positiveness
+ max_pos_vel = std::max(max_pos_vel, 0.F);
+ max_rot_vel = std::max(max_rot_vel, 0.F);
+
std::scoped_lock l{m_control_data.mutex};
m_control_data.max_vel = max_pos_vel;
m_control_data.max_rot_vel = max_rot_vel;
@@ -320,7 +326,19 @@ armarx::ObstacleAwarePlatformUnit::high_level_control_loop()
m["modulated_local_y"] = new Variant{vels.modulated_local.y()};
m["modulated_local_abs"] = new Variant{vels.modulated_local.norm()};
- setDebugObserverChannel("ObstacleAvoidingPlatformCtrl", m);
+// m["max_rot_vel"] = new Variant{m_control_data.max_rot_vel};
+// m["max_lin_vel"] = new Variant{m_control_data.max_vel};
+
+// m["rot_desired"] = new Variant{m_control_data.target_ori};
+// m["rot_measured"] = new Variant{m_control_data.agent_ori};
+
+// m["pos_x_desired"] = new Variant{m_control_data.target_pos.x()};
+// m["pos_x_measured"] = new Variant{m_control_data.agent_pos.y()};
+
+// m["pos_y_desired"] = new Variant{m_control_data.target_pos.x()};
+// m["pos_y_measured"] = new Variant{m_control_data.agent_pos.y()};
+
+ setDebugObserverChannel("ObstacleAwarePlatformCtrl", m);
m_platform->move(vels.modulated_local.x(), vels.modulated_local.y(), vels.target_rot);
visualize(vels);
@@ -434,8 +452,8 @@ const
if (m_control_loop.mode == control_mode::position /*and not target_orientation_reached()*/)
{
- m_rot_pid_controller.update(m_control_data.target_angular_dist, 0);
- uncapped_target_rot_vel = -m_rot_pid_controller.getControlValue();
+ m_rot_pid_controller.update(m_control_data.agent_ori, m_control_data.target_ori);
+ uncapped_target_rot_vel = m_rot_pid_controller.getControlValue();
}
else if (m_control_loop.mode == control_mode::velocity)
{
@@ -444,8 +462,10 @@ const
ARMARX_CHECK(std::isfinite(uncapped_target_rot_vel));
- return std::copysign(std::min(std::fabs(uncapped_target_rot_vel), m_control_data.max_rot_vel),
- uncapped_target_rot_vel);
+ return std::clamp(uncapped_target_rot_vel, -m_control_data.max_rot_vel, m_control_data.max_rot_vel);
+
+ // return std::copysign(std::min(std::fabs(uncapped_target_rot_vel), m_control_data.max_rot_vel),
+ // uncapped_target_rot_vel);
}
@@ -591,7 +611,7 @@ armarx::ObstacleAwarePlatformUnit::visualize(const velocities& vels)
l_prog.add(Cylinder{"boundingCylinder"}
.position(agent_pos)
.color(Color::cyan(255, 64))
- .pose(simox::math::pos_rpy_to_mat4f(agent_pos, -M_PI_2, 0, 0))
+ .pose(simox::math::pos_rpy_to_mat4f(agent_pos, -M_PI_2, 0, 0) * Eigen::Isometry3f(Eigen::Translation3f(0, -1000, 0)).matrix())
.radius(m_viz.boundingCircle.radius)
.height(2000));
}
@@ -671,6 +691,8 @@ armarx::ObstacleAwarePlatformUnit::visualize(const velocities& vels)
.color(Color::cyan(255, 128))
.width(25));
}
+
+ // TODO rotation arrow
}
// Agent.
diff --git a/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.h b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.h
index 274b67ea153eec20473b67fa4f45b8425c7b174d..1bc78b92b4ba4edd0811d7324d2a2f811aa2ee15 100644
--- a/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.h
+++ b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.h
@@ -250,7 +250,7 @@ namespace armarx
ObstacleAwarePlatformUnit::control_loop m_control_loop;
ObstacleAwarePlatformUnit::control_data m_control_data;
- mutable PIDController m_rot_pid_controller{1.0, 0.00009, 0.0};
+ mutable PIDController m_rot_pid_controller{1.0, 0.00009, 0.0, std::numeric_limits<double>::max(), std::numeric_limits<double>::max(), true};
visualization m_viz;
diff --git a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
index 44d9c0aa5a71bee4a4095077a3aca2525cb0437a..4f7372a5e178ed8ec895049ef094af354d297ee2 100644
--- a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
@@ -65,6 +65,9 @@ namespace armarx
widget.setupUi(getWidget());
+ updateTimer = new QTimer(this);
+ connect(updateTimer, &QTimer::timeout, this, QOverload<>::of(&This::onUpdate));
+
timingObserverTimer = new QTimer(this);
connect(timingObserverTimer, &QTimer::timeout, this, QOverload<>::of(&This::onTimingObserverUpdate));
@@ -186,6 +189,7 @@ namespace armarx
changeMode(ArVizWidgetMode::Live);
timingObserverTimer->start(33);
+ updateTimer->start(33);
}
void ArVizWidgetController::onDisconnectGui()
@@ -440,10 +444,15 @@ namespace armarx
layerTreeChanged(nullptr, 0);
}
+ void ArVizWidgetController::onUpdate()
+ {
+ visualizer.update();
+ }
+
void ArVizWidgetController::onTimingObserverUpdate()
{
viz::CoinVisualizer_UpdateTiming timing = visualizer.getTiming();
- if (timing.counter > lastTiming.counter)
+ //if (timing.counter > lastTiming.counter)
{
if (debugObserver)
{
@@ -453,6 +462,8 @@ namespace armarx
timingMap["1.2 apply, updateElements (ms)"] = new Variant(timing.applyTotal.updateElements.toMilliSecondsDouble());
timingMap["1.3 apply, removeElements (ms)"] = new Variant(timing.applyTotal.removeElements.toMilliSecondsDouble());
timingMap["2. layers (ms)"] = new Variant(timing.layersChanged.toMilliSecondsDouble());
+ timingMap["3. wait duration (ms)"] = new Variant(timing.waitDuration.toMilliSecondsDouble());
+ timingMap["4. update toggle"] = new Variant(timing.updateToggle);
timingMap["total (ms)"] = new Variant(timing.total.toMilliSecondsDouble());
timings.push_back(timing.total.toMilliSecondsDouble());
diff --git a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.h b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.h
index 7a3a0bfa34330ecf32509b24d2e1d420e4016d0f..31383a63ea78c510ec781951ff56b5fdbcafee06 100644
--- a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.h
+++ b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.h
@@ -140,6 +140,7 @@ namespace armarx
void showAllLayers(bool visible);
void showFilteredLayers(bool visible);
+ void onUpdate();
void onTimingObserverUpdate();
@@ -174,6 +175,7 @@ namespace armarx
QPointer<SimpleConfigDialog> configDialog;
+ QTimer* updateTimer;
QTimer* timingObserverTimer;
viz::CoinVisualizer_UpdateTiming lastTiming;
StringVariantBaseMap timingMap;
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.cpp b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.cpp
index ac5fe6828a68ed2cb600a0bbef9d8cb2ac7df674..bb239e656673d84e2849ae1485de41f744889ccf 100644
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.cpp
+++ b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCAT.cpp
@@ -785,7 +785,7 @@ void EtherCAT::readErrorCounters()
//not used, only confusing info...
//uint16_t configAddr = ecx_APRDw(ecx_context.port, ADPh, ECT_REG_STADR, 100000);
- for (int i = 0; i < 4; i++)
+ for (int i = 0; i < 2; i++)
{
// Error handling taken from
@@ -809,14 +809,14 @@ void EtherCAT::readErrorCounters()
}
else if (ret1 > 0 && ret2 > 0 && ret3 > 0 && ret4 > 0)
{
- ARMARX_INFO << "no errors for Slavenumber " << slaveIndex << " port:" << i << "\tname: " << name;
+ ARMARX_DEBUG << "no errors for Slavenumber " << slaveIndex << " port:" << i << "\tname: " << name;
}
}
IceUtil::Time elapsed = (IceUtil::Time::now(IceUtil::Time::Monotonic) - start);
if (elapsed.toMilliSeconds() > 10)
{
updatePDO();
- ARMARX_INFO << "Updated BUS to prevent timeout, " << elapsed << " has passed since last bus update.";
+ ARMARX_DEBUG << "Updated BUS to prevent timeout, " << elapsed << " has passed since last bus update.";
start = IceUtil::Time::now(IceUtil::Time::Monotonic);
}
}
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/ArmarXObjects/plugins/ObjectPoseClientPlugin.cpp b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.cpp
index b12572855775c7c9f3f11f4f2d571da7ff36a14f..9bf87346d6014d3add6edad66bbda0bfa84019ad 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.cpp
@@ -1,5 +1,8 @@
#include "ObjectPoseClientPlugin.h"
+#include <ArmarXCore/core/Component.h>
+
+
namespace armarx::plugins
{
void ObjectPoseClientPlugin::postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties)
diff --git a/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h
index 996f699acd35dd50afa095f3bdaa66d9d2a7e061..769d1176dd1fc8db6063fe867a3aec1ff3e1b90d 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h
@@ -1,11 +1,12 @@
#pragma once
-#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/ComponentPlugin.h>
#include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
+
namespace armarx::plugins
{
class ObjectPoseClientPlugin : public ComponentPlugin
@@ -51,6 +52,9 @@ namespace armarx::plugins
};
}
+
+#include <ArmarXCore/core/ManagedIceObject.h>
+
namespace armarx
{
/**
diff --git a/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.cpp b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.cpp
index 63ccab6e140afa4bb8fdd174d0523e7c3b0cd78d..6aef209e68f06d66eae2a5a9a94fde6d084e04bd 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.cpp
@@ -1,5 +1,8 @@
#include "ObjectPoseProviderPlugin.h"
+#include <ArmarXCore/core/Component.h>
+
+
namespace armarx::plugins
{
diff --git a/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.h b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.h
index 2f7c87ef560cad2e4a0b20257387f52b8bd6835d..1d0191ed6c32cc912efcb683155d465bdbfa0ff2 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.h
@@ -1,6 +1,6 @@
#pragma once
-#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/ComponentPlugin.h>
#include <RobotAPI/interface/objectpose/ObjectPoseProvider.h>
@@ -32,6 +32,8 @@ namespace armarx::plugins
}
+#include <ArmarXCore/core/ManagedIceObject.h>
+
namespace armarx
{
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.cpp
index b0079227d85264a7aafea4cbf5a8795d1d606cc0..859389a12506d1a73734a9111dcae884164ea4d0 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.cpp
@@ -1,5 +1,8 @@
#include "ArVizComponentPlugin.h"
+#include <ArmarXCore/core/Component.h>
+
+
namespace armarx::plugins
{
std::string ArVizComponentPlugin::getTopicName()
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h
index a4388cb604ea784494091be344b89320a317ad5b..6ce746eddb21cf676150d615cb2d9d57f2211d13 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h
@@ -1,6 +1,6 @@
#pragma once
-#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/ComponentPlugin.h>
#include <RobotAPI/components/ArViz/Client/Client.h>
@@ -27,6 +27,9 @@ namespace armarx::plugins
};
}
+
+#include <ArmarXCore/core/ManagedIceObject.h>
+
namespace armarx
{
/**
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/CartesianPositionControlComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CartesianPositionControlComponentPlugin.cpp
index 6b403e45ccfea923d1ead7b8e73fa5e7dbcf0de2..9d241c0c94832026b41a328aa215b7008e583db8 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/CartesianPositionControlComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CartesianPositionControlComponentPlugin.cpp
@@ -1,5 +1,8 @@
#include "CartesianPositionControlComponentPlugin.h"
+#include <ArmarXCore/core/Component.h>
+
+
namespace armarx
{
namespace plugins
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/CartesianPositionControlComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CartesianPositionControlComponentPlugin.h
index 0d53885cc44d3d0d42f4357c6c7b7cf0cfa7db5f..3a6dfebd06e39743a612ed9cfa3480fbd453d492 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/CartesianPositionControlComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CartesianPositionControlComponentPlugin.h
@@ -1,6 +1,6 @@
#pragma once
-#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/ComponentPlugin.h>
#include <RobotAPI/interface/components/CartesianPositionControlInterface.h>
@@ -30,6 +30,9 @@ namespace armarx
}
}
+
+#include <ArmarXCore/core/ManagedIceObject.h>
+
namespace armarx
{
/**
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/DebugDrawerHelperComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/DebugDrawerHelperComponentPlugin.cpp
index 5fd0cd798b0e16567b57b443bff1381467a88a89..bd995800595c1715f71723f42a5c59dfee4dc063 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/DebugDrawerHelperComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/DebugDrawerHelperComponentPlugin.cpp
@@ -21,9 +21,11 @@
*/
#include <ArmarXCore/util/CPPUtility/trace.h>
+#include <ArmarXCore/core/Component.h>
#include "DebugDrawerHelperComponentPlugin.h"
+
namespace armarx::plugins
{
DebugDrawerHelperComponentPlugin::DebugDrawerHelperComponentPlugin(ManagedIceObject& parent, std::string pre) :
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/DebugDrawerHelperComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/DebugDrawerHelperComponentPlugin.h
index 1cf37ba70ec4000b61db4a3d8872e6871d6c7c2a..59ed86bb95d3c66aa02c6ad935066e8d97fc53e3 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/DebugDrawerHelperComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/DebugDrawerHelperComponentPlugin.h
@@ -22,7 +22,7 @@
#pragma once
-#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/ComponentPlugin.h>
#include <RobotAPI/components/DebugDrawer/DebugDrawerHelper.h>
@@ -82,6 +82,9 @@ namespace armarx::plugins
};
}
+
+#include <ArmarXCore/core/ManagedIceObject.h>
+
namespace armarx
{
class DebugDrawerHelperComponentPluginUser : virtual public ManagedIceObject
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/FrameTrackingComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/FrameTrackingComponentPlugin.cpp
index f5ba6417ad1ee3c5a683a33209678c3465c76277..1ca9b39590f7f49df239495cce72356223b70514 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/FrameTrackingComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/FrameTrackingComponentPlugin.cpp
@@ -1,5 +1,8 @@
#include "FrameTrackingComponentPlugin.h"
+#include <ArmarXCore/core/Component.h>
+
+
namespace armarx
{
namespace plugins
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/FrameTrackingComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/FrameTrackingComponentPlugin.h
index 82b1b593d1c457b1b2c636a292ee0ce8aebce477..8364375ea697ba9129d3699edc00c0dede740810 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/FrameTrackingComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/FrameTrackingComponentPlugin.h
@@ -1,6 +1,6 @@
#pragma once
-#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/ComponentPlugin.h>
#include <RobotAPI/interface/components/FrameTrackingInterface.h>
@@ -30,6 +30,9 @@ namespace armarx
}
}
+
+#include <ArmarXCore/core/ManagedIceObject.h>
+
namespace armarx
{
/**
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/GraspCandidateObserverComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/GraspCandidateObserverComponentPlugin.cpp
index 47494bcad7cba1f390206cbb513a12f8fe1fa838..6ab08acaee80d8896636f3c16c81bb38822365a5 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/GraspCandidateObserverComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/GraspCandidateObserverComponentPlugin.cpp
@@ -1,5 +1,7 @@
#include <SimoxUtility/algorithm/string/string_tools.h>
+#include <ArmarXCore/core/Component.h>
+
#include "GraspCandidateObserverComponentPlugin.h"
namespace armarx
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/GraspCandidateObserverComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/GraspCandidateObserverComponentPlugin.h
index e2f134ef78f79da7ea1e7298a969d984cd3a4123..a47f7877514702b1d4c719cb87947fea09ed5c90 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/GraspCandidateObserverComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/GraspCandidateObserverComponentPlugin.h
@@ -1,6 +1,6 @@
#pragma once
-#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/ComponentPlugin.h>
#include <RobotAPI/interface/observers/GraspCandidateObserverInterface.h>
namespace armarx
@@ -36,6 +36,9 @@ namespace armarx
}
}
+
+#include <ArmarXCore/core/ManagedIceObject.h>
+
namespace armarx
{
/**
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/HandUnitComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HandUnitComponentPlugin.cpp
index c99b6a9c2dafb560e00e86c28eee0108b650c888..df3ed14e186ef4bced826f2e4cc9e581529ae5af 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/HandUnitComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HandUnitComponentPlugin.cpp
@@ -1,5 +1,8 @@
#include "HandUnitComponentPlugin.h"
+#include <ArmarXCore/core/Component.h>
+
+
namespace armarx
{
namespace plugins
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/HandUnitComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HandUnitComponentPlugin.h
index 4cd3593cb95abd83e023e8aa5642b68e23bea3fb..6cfe35ef754764c1c657ddefd7ec5bee811339c0 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/HandUnitComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HandUnitComponentPlugin.h
@@ -1,6 +1,6 @@
#pragma once
-#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/ComponentPlugin.h>
#include <RobotAPI/interface/units/HandUnitInterface.h>
@@ -30,6 +30,9 @@ namespace armarx
}
}
+
+#include <ArmarXCore/core/ManagedIceObject.h>
+
namespace armarx
{
/**
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/KinematicUnitComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/KinematicUnitComponentPlugin.cpp
index ba8ba16d24d40bbb2312690ef87b4dd9f5619cd5..59dc918a1be3688d70795a5175c90ca28c5da099 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/KinematicUnitComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/KinematicUnitComponentPlugin.cpp
@@ -1,5 +1,8 @@
#include "KinematicUnitComponentPlugin.h"
+#include <ArmarXCore/core/Component.h>
+
+
namespace armarx
{
namespace plugins
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/KinematicUnitComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/KinematicUnitComponentPlugin.h
index e3b4062b59500f26314a6ccdb6e5480dd7d34a2b..2b3486826b6b84883e28c86598f3e05ea04c2674 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/KinematicUnitComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/KinematicUnitComponentPlugin.h
@@ -1,6 +1,6 @@
#pragma once
-#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/ComponentPlugin.h>
#include <RobotAPI/interface/units/KinematicUnitInterface.h>
@@ -30,6 +30,9 @@ namespace armarx
}
}
+
+#include <ArmarXCore/core/ManagedIceObject.h>
+
namespace armarx
{
/**
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/NaturalIKComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/NaturalIKComponentPlugin.cpp
index 008d497652fbd0888ef82eb1e5b99650bda11a25..fd592a9250cb0bb99caef834243ff4cf5bbaab23 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/NaturalIKComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/NaturalIKComponentPlugin.cpp
@@ -1,5 +1,8 @@
#include "NaturalIKComponentPlugin.h"
+#include <ArmarXCore/core/Component.h>
+
+
namespace armarx
{
namespace plugins
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/NaturalIKComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/NaturalIKComponentPlugin.h
index 0532416d50ae164baf5645d0cbc30ff50f3e9f48..c7e1c9cf61c21bfa986b3a0c827a6b371aba3048 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/NaturalIKComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/NaturalIKComponentPlugin.h
@@ -1,6 +1,6 @@
#pragma once
-#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/ComponentPlugin.h>
#include <RobotAPI/interface/components/NaturalIKInterface.h>
@@ -30,6 +30,9 @@ namespace armarx
}
}
+
+#include <ArmarXCore/core/ManagedIceObject.h>
+
namespace armarx
{
/**
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/PlatformUnitComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/PlatformUnitComponentPlugin.cpp
index f40b6cd34a5f5a8f028b714cc7a1517d590f2b3c..affec3b906414a2616c3e786aa0f88e6fa7b4d44 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/PlatformUnitComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/PlatformUnitComponentPlugin.cpp
@@ -1,5 +1,8 @@
#include "PlatformUnitComponentPlugin.h"
+#include <ArmarXCore/core/Component.h>
+
+
namespace armarx
{
namespace plugins
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/PlatformUnitComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/PlatformUnitComponentPlugin.h
index d2cec0f45b698f813d12a10920a122efbb63d00b..67b0edbc320bf724e321e2c32631b37ac5ebc92f 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/PlatformUnitComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/PlatformUnitComponentPlugin.h
@@ -1,6 +1,6 @@
#pragma once
-#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/ComponentPlugin.h>
#include <RobotAPI/interface/units/PlatformUnitInterface.h>
@@ -30,6 +30,9 @@ namespace armarx
}
}
+
+#include <ArmarXCore/core/ManagedIceObject.h>
+
namespace armarx
{
/**
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp
index 026024a9fb704181271f09f69ec4b94306a3d295..b9ad4f67a8e71d645fc5c868912a0ea9e271c1c9 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp
@@ -19,11 +19,15 @@
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
+
+#include <ArmarXCore/core/Component.h>
+
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include "RobotStateComponentPlugin.h"
+
namespace armarx::plugins
{
const RobotNameHelper& RobotStateComponentPlugin::getRobotNameHelper() const
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h
index d60ed34f0964b4dc8592c88b055fb32e7bab4608..d3a1ba42bd9a21279268f24780572957b1d80aa4 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h
@@ -27,12 +27,13 @@
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/XML/RobotIO.h>
-#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/ComponentPlugin.h>
#include <RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h>
#include <RobotAPI/interface/core/RobotState.h>
#include <RobotAPI/libraries/diffik/SimpleDiffIK.h>
+
namespace armarx::plugins
{
/**
@@ -158,6 +159,9 @@ namespace armarx::plugins
};
}
+
+#include <ArmarXCore/core/ManagedIceObject.h>
+
namespace armarx
{
class RobotStateComponentPluginUser : virtual public ManagedIceObject
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp
index 7d32ae794b427f07dc27e38edd8178460ec9fbd4..4e6e7966cc4c71e819da47cd9a7ee06ccd536775 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp
@@ -1,7 +1,11 @@
#include "RobotUnitComponentPlugin.h"
-#include "ArmarXCore/util/CPPUtility/Pointer.h"
+
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/util/CPPUtility/Pointer.h>
+
#include <thread>
+
namespace armarx::plugins
{
RobotUnitDataStreamingReceiverPtr
@@ -63,27 +67,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 +100,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 +112,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..80357a96e9a3bb6c889d7b73435baef90a385b54 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h
@@ -1,6 +1,6 @@
#pragma once
-#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/ComponentPlugin.h>
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
@@ -29,6 +29,12 @@ namespace armarx
void deactivate();
+ void setRobotUnitAsOptionalDependency(bool isOptional = true)
+ {
+
+ }
+
+
//controllers
public:
template<class PrxT>
@@ -63,12 +69,15 @@ 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;
};
}
}
+
+#include <ArmarXCore/core/ManagedIceObject.h>
+
namespace armarx
{
/**
@@ -81,13 +90,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/RobotAPIComponentPlugins/RobotUnitObserverComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitObserverComponentPlugin.cpp
index 284533cf16e62834d85ff33e73d36b158943acef..0b5805c209fe40ab96eaafbeb2f573ca91a9ba9e 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitObserverComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitObserverComponentPlugin.cpp
@@ -1,5 +1,8 @@
#include "RobotUnitObserverComponentPlugin.h"
+#include <ArmarXCore/core/Component.h>
+
+
namespace armarx
{
namespace plugins
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitObserverComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitObserverComponentPlugin.h
index 0ebeff910700102871aa6229259e05f2efec866d..27b70a5064ad2e6be651e04b2cffe5f7d2d0360f 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitObserverComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitObserverComponentPlugin.h
@@ -1,6 +1,6 @@
#pragma once
-#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/ComponentPlugin.h>
#include <ArmarXCore/interface/observers/ObserverInterface.h>
@@ -30,6 +30,9 @@ namespace armarx
}
}
+
+#include <ArmarXCore/core/ManagedIceObject.h>
+
namespace armarx
{
/**
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/TrajectoryPlayerComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/TrajectoryPlayerComponentPlugin.cpp
index 1de30ae07c9f8218666121c28350f32c1ca1f5dc..28a6479b0ab5f844e5a2e68c14fdf84df66e3c67 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/TrajectoryPlayerComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/TrajectoryPlayerComponentPlugin.cpp
@@ -1,5 +1,8 @@
#include "TrajectoryPlayerComponentPlugin.h"
+#include <ArmarXCore/core/Component.h>
+
+
namespace armarx
{
namespace plugins
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/TrajectoryPlayerComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/TrajectoryPlayerComponentPlugin.h
index 3fc52df857f7a7225eacb25103dbd7fa16de4af5..28d57ca0488a8318ca154add6ec749b5380f8e50 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/TrajectoryPlayerComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/TrajectoryPlayerComponentPlugin.h
@@ -1,6 +1,6 @@
#pragma once
-#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/ComponentPlugin.h>
#include <RobotAPI/interface/components/TrajectoryPlayerInterface.h>
@@ -30,6 +30,9 @@ namespace armarx
}
}
+
+#include <ArmarXCore/core/ManagedIceObject.h>
+
namespace armarx
{
/**
diff --git a/source/RobotAPI/libraries/armem/client/MemoryNameSystem.cpp b/source/RobotAPI/libraries/armem/client/MemoryNameSystem.cpp
index 2478cba21ad3b73730ed8aa9c11902c3d2038689..4395b55fd45e6bdb019ee1285fbc5cc3dac648f9 100644
--- a/source/RobotAPI/libraries/armem/client/MemoryNameSystem.cpp
+++ b/source/RobotAPI/libraries/armem/client/MemoryNameSystem.cpp
@@ -7,6 +7,7 @@
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem/client/Writer.h>
+#include <SimoxUtility/algorithm/string/string_tools.h>
namespace armarx::armem::client
{
diff --git a/source/RobotAPI/libraries/armem/client/MemoryNameSystemComponentPlugin.cpp b/source/RobotAPI/libraries/armem/client/MemoryNameSystemComponentPlugin.cpp
index 1d24d974e97932be9a56dd8693b1f74ef8a604f1..60fae2cf9b37e450fcfbaee806af8115f496c678 100644
--- a/source/RobotAPI/libraries/armem/client/MemoryNameSystemComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/armem/client/MemoryNameSystemComponentPlugin.cpp
@@ -1,5 +1,6 @@
#include "MemoryNameSystemComponentPlugin.h"
+#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <RobotAPI/libraries/armem/core/error.h>
diff --git a/source/RobotAPI/libraries/armem/client/MemoryNameSystemComponentPlugin.h b/source/RobotAPI/libraries/armem/client/MemoryNameSystemComponentPlugin.h
index 2e71cd33c126a572e74441fdcb42cc219bee10fe..a6a2434a68a033d3e540729c08e83c90babda847 100644
--- a/source/RobotAPI/libraries/armem/client/MemoryNameSystemComponentPlugin.h
+++ b/source/RobotAPI/libraries/armem/client/MemoryNameSystemComponentPlugin.h
@@ -1,6 +1,6 @@
#pragma once
-#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/ComponentPlugin.h>
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
#include <RobotAPI/interface/armem/client/MemoryListenerInterface.h>
@@ -55,6 +55,8 @@ namespace armarx::armem::client::plugins
}
+#include <ArmarXCore/core/ManagedIceObject.h>
+
namespace armarx::armem::client
{
diff --git a/source/RobotAPI/libraries/armem/client/util/MemoryListener.cpp b/source/RobotAPI/libraries/armem/client/util/MemoryListener.cpp
index 2c491c11f4cc4f223d940a60ef79ab577a428b6f..3318b9ed385ff337b13871fb017fcb521af7fd74 100644
--- a/source/RobotAPI/libraries/armem/client/util/MemoryListener.cpp
+++ b/source/RobotAPI/libraries/armem/client/util/MemoryListener.cpp
@@ -29,7 +29,7 @@ namespace armarx::armem::client::util
{
std::stringstream error;
- for (const auto& [subscription, callbacks] : this->callbacks)
+ for (const auto& [subscription, subCallbacks] : this->callbacks)
{
std::vector<MemoryID> matchingSnapshotIDs;
@@ -53,7 +53,10 @@ namespace armarx::armem::client::util
if (not matchingSnapshotIDs.empty())
{
- for (auto& callback : callbacks)
+ ARMARX_DEBUG << "Calling " << subCallbacks.size() << " callbacks"
+ << " subscribing " << subscription
+ << " with " << matchingSnapshotIDs.size() << " snapshot IDs ...";
+ for (auto& callback : subCallbacks)
{
callback(subscription, matchingSnapshotIDs);
}
diff --git a/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h b/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h
index eefb1b5ce73fdc3f83b5f15101ad3e10e9f0f989..6dc77c57625b0a8ebd33200885f06045eef49bbf 100644
--- a/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h
@@ -85,13 +85,13 @@ namespace armarx::armem::base
}
- inline const std::map<std::string, ProviderSegmentT>& providerSegments() const
+ inline const auto& providerSegments() const
{
return this->_container;
}
- inline std::map<std::string, ProviderSegmentT>& providerSegments()
+ inline auto& providerSegments()
{
- return const_cast<std::map<std::string, ProviderSegmentT>&>(const_cast<const CoreSegmentBase*>(this)->providerSegments());
+ return this->_container;
}
@@ -197,7 +197,7 @@ namespace armarx::armem::base
}
else
{
- auto wms = this->_container.emplace(std::make_pair(k, this->id().withProviderSegmentName(k)));
+ auto wms = this->_container.emplace(k, this->id().withProviderSegmentName(k));
wms.first->second.append(s);
}
}
diff --git a/source/RobotAPI/libraries/armem/core/base/MemoryBase.h b/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
index 69cd01fc128338e3d251a2ca99e54849c962dc9e..d7c8b8cd203fc8b140cbed0b959110f0868a4b83 100644
--- a/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
@@ -82,13 +82,13 @@ namespace armarx::armem::base
}
- inline const std::map<std::string, CoreSegmentT>& coreSegments() const
+ inline auto& coreSegments() const
{
return this->_container;
}
- inline std::map<std::string, CoreSegmentT>& coreSegments()
+ inline auto& coreSegments()
{
- return const_cast<std::map<std::string, CoreSegmentT>&>(const_cast<const MemoryBase*>(this)->coreSegments());
+ return this->_container;
}
@@ -272,7 +272,7 @@ namespace armarx::armem::base
}
else
{
- auto wms = this->_container.emplace(std::make_pair(k, this->id().withCoreSegmentName(k)));
+ auto wms = this->_container.emplace(k, this->id().withCoreSegmentName(k));
wms.first->second.append(s);
}
}
diff --git a/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h b/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h
index a53595c8784ea64cc4994f3f77b4ac22a1510cb1..57ff3210244fa516551f4906a2ea07c2cc81f09d 100644
--- a/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h
@@ -83,13 +83,13 @@ namespace armarx::armem::base
}
- inline const std::map<std::string, EntityT>& entities() const
+ inline const auto& entities() const
{
return this->_container;
}
- inline std::map<std::string, EntityT>& entities()
+ inline auto& entities()
{
- return const_cast<std::map<std::string, EntityT>&>(const_cast<const ProviderSegmentBase*>(this)->entities());
+ return this->_container;
}
@@ -181,7 +181,7 @@ namespace armarx::armem::base
}
else
{
- auto wms = this->_container.emplace(std::make_pair(k, this->id().withEntityName(k)));
+ auto wms = this->_container.emplace(k, this->id().withEntityName(k));
wms.first->second.append(s);
}
}
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/CoreSegment.cpp b/source/RobotAPI/libraries/armem/core/diskmemory/CoreSegment.cpp
index 2dd6fd615432b9245e308ba285b4be62f99578ab..0f1bd0be881f477908464bcda5a4a858afb54488 100644
--- a/source/RobotAPI/libraries/armem/core/diskmemory/CoreSegment.cpp
+++ b/source/RobotAPI/libraries/armem/core/diskmemory/CoreSegment.cpp
@@ -58,7 +58,7 @@ namespace armarx::armem::d_ltm
if (d.is_directory())
{
std::string k = d.path().filename();
- auto wms = _container.emplace(std::make_pair(k, id().withProviderSegmentName(k)));
+ auto wms = _container.emplace(k, id().withProviderSegmentName(k));
wms.first->second.reload(p_ptr);
}
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/Memory.cpp b/source/RobotAPI/libraries/armem/core/diskmemory/Memory.cpp
index 4a913f2b497fce236c3cb8f9343fa97179b2d757..0fb6fba92b2ebc116787bffba2d7ed5bc743227b 100644
--- a/source/RobotAPI/libraries/armem/core/diskmemory/Memory.cpp
+++ b/source/RobotAPI/libraries/armem/core/diskmemory/Memory.cpp
@@ -55,7 +55,7 @@ namespace armarx::armem::d_ltm
if (d.is_directory())
{
std::string k = d.path().filename();
- auto wms = _container.emplace(std::make_pair(k, id().withCoreSegmentName(k)));
+ auto wms = _container.emplace(k, id().withCoreSegmentName(k));
wms.first->second.reload(path);
}
}
@@ -83,7 +83,7 @@ namespace armarx::armem::d_ltm
return;
}
- auto wms = _container.emplace(std::make_pair(k, id().withCoreSegmentName(k)));
+ auto wms = _container.emplace(k, id().withCoreSegmentName(k));
wms.first->second.path = path;
wms.first->second.append(s);
}
diff --git a/source/RobotAPI/libraries/armem/core/diskmemory/ProviderSegment.cpp b/source/RobotAPI/libraries/armem/core/diskmemory/ProviderSegment.cpp
index f46e165381bf0d9cbdcc12b4cdfa31e286f4a545..620e819f6b1fc05d4aeadc285edb087f5ea0e119 100644
--- a/source/RobotAPI/libraries/armem/core/diskmemory/ProviderSegment.cpp
+++ b/source/RobotAPI/libraries/armem/core/diskmemory/ProviderSegment.cpp
@@ -64,7 +64,7 @@ namespace armarx::armem::d_ltm
if (d.is_directory())
{
std::string k = d.path().filename();
- auto wms = _container.emplace(std::make_pair(k, id().withEntityName(k)));
+ auto wms = _container.emplace(k, id().withEntityName(k));
wms.first->second.reload(p_ptr);
}
@@ -114,7 +114,7 @@ namespace armarx::armem::d_ltm
return;
}
- auto wms = _container.emplace(std::make_pair(k, id().withEntityName(k)));
+ auto wms = _container.emplace(k, id().withEntityName(k));
wms.first->second.path = path;
wms.first->second.append(s);
}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.cpp b/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.cpp
index 7c4307da5395a58a6bdfce57c18705cefe535f7f..a64a5982e143e2627718f5039d3559b2d3479348 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.cpp
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.cpp
@@ -51,7 +51,7 @@ namespace armarx::armem::ltm
}
else
{
- auto wms = _container.emplace(std::make_pair(k, id().withProviderSegmentName(k)));
+ auto wms = _container.emplace(k, id().withProviderSegmentName(k));
wms.first->second.dbsettings = dbsettings;
wms.first->second.reload();
}
@@ -78,7 +78,7 @@ namespace armarx::armem::ltm
<< bsoncxx::builder::stream::finalize;
coll.insert_one(foreign_key.view());
- auto wms = _container.emplace(std::make_pair(k, id().withProviderSegmentName(k)));
+ auto wms = _container.emplace(k, id().withProviderSegmentName(k));
wms.first->second.dbsettings = dbsettings;
wms.first->second.append(s);
}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.cpp b/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.cpp
index 7edf8946abe9bbf5967454a99ad73fefee535686..fbea444e9d44008407703fe7b7f8b007179726b6 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.cpp
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.cpp
@@ -82,7 +82,7 @@ namespace armarx::armem::ltm
}
else
{
- auto wms = _container.emplace(std::make_pair(k, id().withCoreSegmentName(k)));
+ auto wms = _container.emplace(k, id().withCoreSegmentName(k));
wms.first->second.dbsettings = dbsettings;
wms.first->second.reload();
}
@@ -119,7 +119,7 @@ namespace armarx::armem::ltm
<< bsoncxx::builder::stream::finalize;
coll.insert_one(foreign_key.view());
- auto wms = _container.emplace(std::make_pair(k, id().withCoreSegmentName(k)));
+ auto wms = _container.emplace(k, id().withCoreSegmentName(k));
wms.first->second.dbsettings = dbsettings;
wms.first->second.append(s);
}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.cpp b/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.cpp
index 7bf439bfec2282d6919d69af1f94d9d7384cc4f8..aac80459904cae59550ecdf33f7057f92ed03aa4 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.cpp
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.cpp
@@ -50,7 +50,7 @@ namespace armarx::armem::ltm
}
else
{
- auto wms = _container.emplace(std::make_pair(k, id().withEntityName(k)));
+ auto wms = _container.emplace(k, id().withEntityName(k));
wms.first->second.dbsettings = dbsettings;
wms.first->second.reload();
}
@@ -77,7 +77,7 @@ namespace armarx::armem::ltm
<< bsoncxx::builder::stream::finalize;
coll.insert_one(foreign_key.view());
- auto wms = _container.emplace(std::make_pair(k, id().withEntityName(k)));
+ auto wms = _container.emplace(k, id().withEntityName(k));
wms.first->second.dbsettings = dbsettings;
wms.first->second.append(s);
}
diff --git a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
index f35e9ecb4c330edf520cadbdf4fbc163fc431781..a846cffac89c6118b6c476d3531e47cd49f96f8a 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
+++ b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
@@ -1,5 +1,6 @@
#include "MemoryToIceAdapter.h"
+#include "ArmarXCore/core/logging/Logging.h"
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include "../error.h"
@@ -158,7 +159,7 @@ namespace armarx::armem::server
// TODO: Add if and param here (fabian.peller)
for (const auto& snapshot : updateResult.removedSnapshots)
{
- ARMARX_IMPORTANT << "The id " << snapshot.id() << " was removed from wm";
+ ARMARX_DEBUG << "The id " << snapshot.id() << " was removed from wm";
}
if (publishUpdates)
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedDataVisitor.h b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedDataVisitor.h
index fb1c1baa557167432ffaa19f46e71d223a5c080d..49885f7eb3d66e561e0a83c496524907afd5d049 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedDataVisitor.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedDataVisitor.h
@@ -13,6 +13,8 @@
#include <RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitorBase.h>
+#include <SimoxUtility/algorithm/string/string_tools.h>
+
namespace armarx::armem::gui::instance
{
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/Decay.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Decay.cpp
index 2d3942250cf43a294a5e599aa2f5e28e454a0960..df3728fb3bbc6149fb329e60a3a94083caa6a441 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Decay.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Decay.cpp
@@ -26,22 +26,21 @@ namespace armarx::armem::server::obj::instance
void Decay::updateConfidence(objpose::ObjectPose& pose, IceUtil::Time now) const
{
- float confidence = calculateConfidence(pose.timestamp, now);
- pose.confidence = confidence;
+ if (pose.attachment or pose.isStatic)
+ {
+ pose.confidence = 1.0;
+ }
+ else
+ {
+ pose.confidence = calculateConfidence(pose.timestamp, now);
+ }
}
void Decay::updateConfidences(objpose::ObjectPoseSeq& objectPoses, IceUtil::Time now) const
{
for (objpose::ObjectPose& pose : objectPoses)
{
- if (pose.attachment or pose.isStatic)
- {
- pose.confidence = 1.0;
- }
- else
- {
- updateConfidence(pose, now);
- }
+ updateConfidence(pose, now);
}
}
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);
+ }
}
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Data.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Data.cpp
index 9a2b8a18ad8032fdf32fca9c0166d515a0356063..a593bad8cf3d2b9b2a08af3adc10ae478f56cf87 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Data.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Data.cpp
@@ -22,3 +22,97 @@
*/
#include "Data.h"
+
+#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
+
+#include <SimoxUtility/algorithm/string/string_tools.h>
+
+namespace armarx::aron::xmltypereader
+{
+
+ void Data::EnforceAttribute(const RapidXmlReaderNode& node, const std::string& att)
+ {
+ if (!HasAttribute(node, att))
+ {
+ throw error::StringNotValidException("XMLReaderData", "EnforceAttribute", "A <" + node.name() + ">-tag does not have the correct attribute", att);
+ }
+ }
+
+ bool Data::HasAttribute(const RapidXmlReaderNode& node, const std::string& att)
+ {
+ return node.has_attribute(att.c_str());
+ }
+
+ std::string Data::GetAttribute(const RapidXmlReaderNode& node, const std::string& att)
+ {
+ EnforceAttribute(node, att);
+ return node.attribute_value(att.c_str());
+ }
+
+ std::string Data::GetAttributeWithDefault(const armarx::RapidXmlReaderNode& node, const std::string& att, const std::string def)
+ {
+ if (!(HasAttribute(node, att)))
+ {
+ return def;
+ }
+ return node.attribute_value(att.c_str());
+ }
+
+ bool Data::AttributeIsTrue(const armarx::RapidXmlReaderNode& node, const std::string& att)
+ {
+ if (HasAttribute(node, att))
+ {
+ std::string v = simox::alg::to_lower(node.attribute_value(att.c_str()));
+ if (v == "1" or v == "true" or v == "wahr" or v == "yes" or v == "ja" or v == "")
+ {
+ return true;
+ }
+ }
+ return false;
+ }
+
+ bool Data::HasTagName(const armarx::RapidXmlReaderNode& node, const std::string& name)
+ {
+ return (simox::alg::to_lower(name) == simox::alg::to_lower(node.name()));
+ }
+
+ void Data::EnforceTagName(const armarx::RapidXmlReaderNode& node, const std::string& name)
+ {
+ if (!(HasTagName(node, name)))
+ {
+ throw error::StringNotValidException("XMLReaderData", "EnforceTagName", "The node <" + node.name() + "> has the wrong tag", name);
+ }
+ }
+
+ std::string Data::GetTagName(const armarx::RapidXmlReaderNode& node)
+ {
+ return simox::alg::to_lower(node.name());
+ }
+
+ bool Data::CheckMinChildSize(const armarx::RapidXmlReaderNode& node, const size_t size)
+ {
+ std::vector<RapidXmlReaderNode> children = node.nodes();
+ return children.size() >= size;
+ }
+
+ bool Data::CheckMaxChildSize(const armarx::RapidXmlReaderNode& node, const size_t size)
+ {
+ std::vector<RapidXmlReaderNode> children = node.nodes();
+ return children.size() <= size;
+ }
+
+ bool Data::CheckExactChildSize(const armarx::RapidXmlReaderNode& node, const size_t size)
+ {
+ std::vector<RapidXmlReaderNode> children = node.nodes();
+ return children.size() == size;
+ }
+
+ void Data::EnforceChildSize(const armarx::RapidXmlReaderNode& node, const size_t size)
+ {
+ if (!Data::CheckExactChildSize(node, size))
+ {
+ throw error::SizeNotValidException("XMLReaderData", "EnforceChildSize", "The node <" + node.name() + "> has the wrong number of children", node.nodes().size(), size);
+ }
+ }
+
+}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Data.h b/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Data.h
index 35ac224587ed6fe83afcd7b3dca4f3f014375f65..695050d69d765585b3192e9a2874c721afe8693c 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Data.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Data.h
@@ -28,12 +28,13 @@
#include <map>
// ArmarX
-#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
-
#include <RobotAPI/libraries/aron/core/Concepts.h>
-
#include <RobotAPI/libraries/aron/core/navigator/type/AllNavigators.h>
+namespace armarx
+{
+ class RapidXmlReaderNode;
+}
namespace armarx::aron::xmltypereader
{
@@ -93,89 +94,28 @@ namespace armarx::aron::xmltypereader
HANDLE_ALL_ARON_TYPES
#undef RUN_ARON_MACRO
- static void EnforceAttribute(const RapidXmlReaderNode& node, const std::string& att)
- {
- if (!HasAttribute(node, att))
- {
- throw error::StringNotValidException("XMLReaderData", "EnforceAttribute", "A <" + node.name() + ">-tag does not have the correct attribute", att);
- }
- }
-
- static bool HasAttribute(const RapidXmlReaderNode& node, const std::string& att)
- {
- return node.has_attribute(att.c_str());
- }
-
- static std::string GetAttribute(const RapidXmlReaderNode& node, const std::string& att)
- {
- EnforceAttribute(node, att);
- return node.attribute_value(att.c_str());
- }
-
- static std::string GetAttributeWithDefault(const RapidXmlReaderNode& node, const std::string& att, const std::string def)
- {
- if (!(HasAttribute(node, att)))
- {
- return def;
- }
- return node.attribute_value(att.c_str());
- }
-
- static bool AttributeIsTrue(const RapidXmlReaderNode& node, const std::string& att)
- {
- if (HasAttribute(node, att))
- {
- std::string v = simox::alg::to_lower(node.attribute_value(att.c_str()));
- if (v == "1" or v == "true" or v == "wahr" or v == "yes" or v == "ja" or v == "")
- {
- return true;
- }
- }
- return false;
- }
-
- static bool HasTagName(const RapidXmlReaderNode& node, const std::string& name)
- {
- return (simox::alg::to_lower(name) == simox::alg::to_lower(node.name()));
- }
-
- static void EnforceTagName(const RapidXmlReaderNode& node, const std::string& name)
- {
- if (!(HasTagName(node, name)))
- {
- throw error::StringNotValidException("XMLReaderData", "EnforceTagName", "The node <" + node.name() + "> has the wrong tag", name);
- }
- }
-
- static std::string GetTagName(const RapidXmlReaderNode& node)
- {
- return simox::alg::to_lower(node.name());
- }
-
- static bool CheckMinChildSize(const RapidXmlReaderNode& node, const size_t size)
- {
- std::vector<RapidXmlReaderNode> children = node.nodes();
- return children.size() >= size;
- }
-
- static bool CheckMaxChildSize(const RapidXmlReaderNode& node, const size_t size)
- {
- std::vector<RapidXmlReaderNode> children = node.nodes();
- return children.size() <= size;
- }
-
- static bool CheckExactChildSize(const RapidXmlReaderNode& node, const size_t size)
- {
- std::vector<RapidXmlReaderNode> children = node.nodes();
- return children.size() == size;
- }
-
- static void EnforceChildSize(const RapidXmlReaderNode& node, const size_t size)
- {
- if (!Data::CheckExactChildSize(node, size))
- {
- throw error::SizeNotValidException("XMLReaderData", "EnforceChildSize", "The node <" + node.name() + "> has the wrong number of children", node.nodes().size(), size);
- }
- }
+ static void EnforceAttribute(const RapidXmlReaderNode& node, const std::string& att);
+
+ static bool HasAttribute(const RapidXmlReaderNode& node, const std::string& att);
+
+ static std::string GetAttribute(const RapidXmlReaderNode& node, const std::string& att);
+
+ static std::string GetAttributeWithDefault(const RapidXmlReaderNode& node, const std::string& att, const std::string def);
+
+ static bool AttributeIsTrue(const RapidXmlReaderNode& node, const std::string& att);
+
+ static bool HasTagName(const RapidXmlReaderNode& node, const std::string& name);
+
+ static void EnforceTagName(const RapidXmlReaderNode& node, const std::string& name);
+
+ static std::string GetTagName(const RapidXmlReaderNode& node);
+
+ static bool CheckMinChildSize(const RapidXmlReaderNode& node, const size_t size);
+
+ static bool CheckMaxChildSize(const RapidXmlReaderNode& node, const size_t size);
+
+ static bool CheckExactChildSize(const RapidXmlReaderNode& node, const size_t size);
+
+ static void EnforceChildSize(const RapidXmlReaderNode& node, const size_t size);
};
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/NavigatorFwd.h b/source/RobotAPI/libraries/aron/core/navigator/data/NavigatorFwd.h
new file mode 100644
index 0000000000000000000000000000000000000000..64ac3a8c95258d5c9585570935660252bbec58fb
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/navigator/data/NavigatorFwd.h
@@ -0,0 +1,41 @@
+/**
+ * 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 <memory>
+
+// TODO add forward declarations for those as well
+// #include "complex/NDArray.h"
+// #include "container/List.h"
+// #include "primitive/Bool.h"
+// #include "primitive/Double.h"
+// #include "primitive/Float.h"
+// #include "primitive/Int.h"
+// #include "primitive/Long.h"
+// #include "primitive/String.h"
+
+namespace armarx::aron::datanavigator
+{
+ class DictNavigator;
+ using DictNavigatorPtr = std::shared_ptr<DictNavigator>;
+
+} // namespace armarx::aron::datanavigator
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/container/Dict.cpp b/source/RobotAPI/libraries/aron/core/navigator/data/container/Dict.cpp
index ef42b1cca0b28496e2b7e7d06f80822608b87e52..db8b7a02f37b948c0030cac5090281b10f400aec 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/data/container/Dict.cpp
+++ b/source/RobotAPI/libraries/aron/core/navigator/data/container/Dict.cpp
@@ -29,6 +29,8 @@
#include <RobotAPI/libraries/aron/core/navigator/type/container/Dict.h>
#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
+#include <SimoxUtility/algorithm/string/string_conversion.h>
+
namespace armarx::aron::datanavigator
{
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/Navigator.h b/source/RobotAPI/libraries/aron/core/navigator/type/Navigator.h
index 3a6cc964a75c65d44440fa4790ae481542a72e8f..1ca45844db2cedd4cba004e94a57dde857ac491f 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/Navigator.h
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/Navigator.h
@@ -30,7 +30,7 @@
#include <string>
// Simox
-#include <SimoxUtility/algorithm/string.h>
+// #include <SimoxUtility/algorithm/string.h>
// ArmarX
#include <RobotAPI/libraries/aron/core/Exception.h>
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/NavigatorFwd.h b/source/RobotAPI/libraries/aron/core/navigator/type/NavigatorFwd.h
new file mode 100644
index 0000000000000000000000000000000000000000..b5a9da2b9d05df4a0744a1d6074abcfff32aaafd
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/NavigatorFwd.h
@@ -0,0 +1,59 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <memory>
+
+/// implemented
+// #include "container/List.h"
+// #include "container/Object.h"
+
+/// TODO add forward decl for those as well
+// #include "container/Pair.h"
+// #include "container/Tuple.h"
+// #include "enum/IntEnum.h"
+// #include "ndarray/EigenMatrix.h"
+// #include "ndarray/EigenQuaternion.h"
+// #include "ndarray/IVTCByteImage.h"
+// #include "ndarray/NDArray.h"
+// #include "ndarray/OpenCVMat.h"
+// #include "ndarray/Orientation.h"
+// #include "ndarray/PCLPointCloud.h"
+// #include "ndarray/Pose.h"
+// #include "ndarray/Position.h"
+// #include "primitive/Bool.h"
+// #include "primitive/Double.h"
+// #include "primitive/Float.h"
+// #include "primitive/Int.h"
+// #include "primitive/Long.h"
+// #include "primitive/String.h"
+// #include "primitive/Time.h"
+
+namespace armarx::aron::typenavigator
+{
+ class DictNavigator;
+ using DictNavigatorPtr = std::shared_ptr<DictNavigator>;
+
+ class ListNavigator;
+ using ListNavigatorPtr = std::shared_ptr<ListNavigator>;
+
+} // namespace armarx::aron::typenavigator
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/container/Object.cpp b/source/RobotAPI/libraries/aron/core/navigator/type/container/Object.cpp
index 05a47b98a4fd2b406ed59a058b30635a1bd29abd..d5e028477cb93837e70cedf95d321652e8a14db6 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/container/Object.cpp
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/container/Object.cpp
@@ -24,8 +24,9 @@
// Header
#include "Object.h"
-// ArmarX
-#include "SimoxUtility/algorithm/get_map_keys_values.h"
+
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <SimoxUtility/algorithm/string/string_conversion.h>
namespace armarx::aron::typenavigator
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/container/Tuple.cpp b/source/RobotAPI/libraries/aron/core/navigator/type/container/Tuple.cpp
index 4fed86ff9bd413550c381fb109363ac1c05e1ff6..bded16377622e363c07fd5007de86349d2248a11 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/container/Tuple.cpp
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/container/Tuple.cpp
@@ -24,6 +24,7 @@
// Header
#include "Tuple.h"
+#include <SimoxUtility/algorithm/string/string_conversion.h>
namespace armarx::aron::typenavigator
{
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/EigenQuaternion.cpp b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/EigenQuaternion.cpp
index 96ec0a152eefa4743701f7ed219d3ee557a0ceae..fbea54cbfe068e63adff2a16ef2fdb9dd168959e 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/EigenQuaternion.cpp
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/EigenQuaternion.cpp
@@ -24,6 +24,8 @@
// Header
#include "EigenQuaternion.h"
+#include <SimoxUtility/algorithm/string/string_conversion.h>
+
namespace armarx::aron::typenavigator
{
// constructors