diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
index 31c31934b335d79b7eb426e6606179c21d910546..a29e434694593cdb0f87f4e524b1fbafbbc25cf3 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
@@ -34,6 +34,7 @@
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
+
namespace armarx
{
@@ -47,6 +48,7 @@ namespace armarx
armarx::PropertyDefinitionsPtr defs(new ObjectPoseObserverPropertyDefinitions(getConfigIdentifier()));
defs->defineOptionalProperty<std::string>("ObjectPoseTopicName", "ObjectPoseTopic", "Name of the Object Pose Topic.");
+ defs->defineOptionalProperty<std::string>("KinematicUnitObserverName", "KinematicUnitObserver", "Name of the kinematic unit observer.");
defs->optional(visu.enabled, "visu.enabled", "Enable or disable visualization of objects.");
defs->optional(visu.inGlobalFrame, "visu.inGlobalFrame", "If true, show global poses. If false, show poses in robot frame.");
@@ -54,8 +56,11 @@ namespace armarx
defs->optional(visu.oobbs, "visu.oobbs", "Enable showing oriented bounding boxes.");
defs->optional(visu.objectFrames, "visu.objectFrames", "Enable showing object frames.");
defs->optional(visu.objectFramesScale, "visu.objectFramesScale", "Scaling of object frames.");
- defs->optional(calibration.robotNode, "calibration.robotNode", "Robot node which can be calibrated");
- defs->optional(calibration.offset, "calibration.offset", "Offset for the node to be calibrated");
+ defs->optional(calibration.robotNode, "calibration.robotNode", "Robot node which can be calibrated.");
+ defs->optional(calibration.offset, "calibration.offset", "Offset for the node to be calibrated.");
+ defs->optional(robotHead.checkHeadVelocity, "head.checkHeadVelocity", "If true, check whether the head is moving.");
+ defs->optional(robotHead.maxJointVelocity, "head.maxJointVelocity", "If a head joint's velocity is higher, the head is considered moving.");
+ defs->optional(robotHead.discardIntervalAfterMoveMS, "head.discardIntervalAfterMoveMS", "For how long new updates are ignored after moving the head.");
return defs;
}
@@ -79,6 +84,9 @@ namespace armarx
robot = RobotState::addRobot("robot", VirtualRobot::RobotIO::RobotDescription::eStructure);
}
+ getProxyFromProperty(robotHead.kinematicUnitObserver, "KinematicUnitObserverName", false, "", false);
+ robotHead.fetchDatafields();
+
createRemoteGuiTab();
RemoteGui_startRunningTask();
}
@@ -159,7 +167,25 @@ namespace armarx
{
ARMARX_VERBOSE << "Received object poses from '" << providerName << "'.";
- objpose::ObjectPoseSeq objectPoses;
+ IceUtil::Time discardUpdatesUntil;
+ if (robotHead.checkHeadVelocity)
+ {
+ std::scoped_lock lock(robotHeadMutex);
+ if (robotHead.isMoving())
+ {
+ robotHead.discardUpdatesUntil = TimeUtil::GetTime() + IceUtil::Time::milliSeconds(robotHead.discardIntervalAfterMoveMS);
+ // ARMARX_IMPORTANT << "Ignoring update because robot head is moving! until " << robotHead.discardUpdatesUntil;
+ return;
+ }
+ if (TimeUtil::GetTime() < robotHead.discardUpdatesUntil)
+ {
+ // ARMARX_IMPORTANT << "Ignoring update because robot head is moved until: " << robotHead.discardUpdatesUntil;
+ return;
+ }
+ discardUpdatesUntil = robotHead.discardUpdatesUntil;
+ }
+
+ objpose::ObjectPoseSeq newObjectPoses;
{
std::scoped_lock lock(dataMutex);
RobotState::synchronizeLocalClone(robot);
@@ -171,28 +197,68 @@ namespace armarx
robotNode->setJointValue(value + calibration.offset);
}
+ // This stays empty on the first report.
+ objpose::ObjectPoseSeq previousPoses;
+ if (auto it = this->objectPoses.find(providerName); it != this->objectPoses.end())
+ {
+ previousPoses = it->second;
+ }
+
for (const objpose::data::ProvidedObjectPose& provided : providedPoses)
{
- objpose::ObjectPose& pose = objectPoses.emplace_back();
- pose.fromProvidedPose(provided, robot);
- if (pose.objectID.dataset().empty())
+ IceUtil::Time timestamp = IceUtil::Time::microSeconds(provided.timestampMicroSeconds);
+
+ std::optional<objpose::ObjectPose> previousPose;
+ for (const objpose::ObjectPose& prev : previousPoses)
{
- // Try to find the data set. (It might be good to cache this.)
- if (std::optional<ObjectInfo> objectInfo = objectFinder.findObject(pose.objectID))
+ if (prev.objectID == fromIce(provided.objectID))
{
- pose.objectID = { objectInfo->dataset(), pose.objectID.className(), pose.objectID.instanceName() };
+ previousPose = prev;
}
}
- if (!provided.localOOBB)
+
+ if (robotHead.checkHeadVelocity && timestamp < discardUpdatesUntil)
{
- pose.localOOBB = getObjectOOBB(pose.objectID);
+ if (previousPose)
+ {
+ // Keep the old one
+ newObjectPoses.push_back(*previousPose);
+ }
+ else
+ {
+ // Discard the new pose.
+ // ARMARX_IMPORTANT << "Ignoring update of object " << provided.objectID << " because robot head is moved.\n"
+ // << "timestamp " << timestamp << " < " << robotHead.discardUpdatesUntil;
+ }
+ }
+ else if (previousPose && timestamp == previousPose->timestamp)
+ {
+ // Keep the old one.
+ newObjectPoses.push_back(*previousPose);
+ }
+ else
+ {
+ objpose::ObjectPose& newPose = newObjectPoses.emplace_back();
+ newPose.fromProvidedPose(provided, robot);
+ if (newPose.objectID.dataset().empty())
+ {
+ // Try to find the data set. (It might be good to cache this.)
+ if (std::optional<ObjectInfo> objectInfo = objectFinder.findObject(newPose.objectID))
+ {
+ newPose.objectID = { objectInfo->dataset(), newPose.objectID.className(), newPose.objectID.instanceName() };
+ }
+ }
+ if (!provided.localOOBB)
+ {
+ newPose.localOOBB = getObjectOOBB(newPose.objectID);
+ }
}
}
}
{
std::scoped_lock lock(dataMutex);
- this->objectPoses[providerName] = objectPoses;
+ this->objectPoses[providerName] = newObjectPoses;
handleProviderUpdate(providerName);
}
}
@@ -760,5 +826,55 @@ namespace armarx
}
}
+ void ObjectPoseObserver::RobotHead::fetchDatafields()
+ {
+ if (kinematicUnitObserver)
+ {
+ for (const std::string& jointName : jointNames)
+ {
+ std::string error = "";
+ try
+ {
+ DatafieldRefBasePtr datafield = kinematicUnitObserver->getDatafieldRefByName(jointVelocitiesChannelName, jointName);
+ DatafieldRefPtr casted = DatafieldRefPtr::dynamicCast(datafield);
+ if (casted)
+ {
+ jointVelocitiesDatafields.push_back(casted);
+ }
+ }
+ catch (const InvalidDatafieldException& e)
+ {
+ error = e.what();
+ }
+ catch (const InvalidChannelException& e)
+ {
+ error = e.what();
+ }
+ if (error.size() > 0)
+ {
+ ARMARX_WARNING_S << "Could not get datafield for joint '" << jointName << "' in channel '" << jointVelocitiesChannelName << "': \n "
+ << error;
+ }
+ }
+ }
+ }
+
+ bool ObjectPoseObserver::RobotHead::isMoving() const
+ {
+ for (DatafieldRefPtr df : jointVelocitiesDatafields)
+ {
+ if (df)
+ {
+ float jointVelocity = df->getFloat();
+ // ARMARX_IMPORTANT_S << "Is " << df->datafieldName << " " << VAROUT(std::abs(jointVelocity)) << " > " << VAROUT(maxJointVelocity) << "?";
+ if (std::abs(jointVelocity) > maxJointVelocity)
+ {
+ return true;
+ }
+ }
+ }
+ return false;
+ }
+
}
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
index 70a0e80abdb6983e58205c44d3a3c3ccc2ea3de3..1d5809452645dd1b7952a87ced9d8d05898fc44d 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
@@ -27,10 +27,12 @@
#include <SimoxUtility/caching/CacheMap.h>
#include <ArmarXCore/observers/Observer.h>
+#include <ArmarXCore/observers/variant/DatafieldRef.h>
#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
#include <RobotAPI/interface/objectpose/ObjectPoseObserver.h>
+#include <RobotAPI/interface/observers/KinematicUnitObserverInterface.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
@@ -158,6 +160,25 @@ namespace armarx
VirtualRobot::RobotPtr robot;
+ struct RobotHead
+ {
+ bool checkHeadVelocity = true;
+
+ std::string jointVelocitiesChannelName = "jointvelocities";
+ std::vector<std::string> jointNames = {"Neck_1_Yaw", "Neck_2_Pitch"};
+ float maxJointVelocity = 0.05;
+ int discardIntervalAfterMoveMS = 100;
+
+ KinematicUnitObserverInterfacePrx kinematicUnitObserver;
+ std::vector<DatafieldRefPtr> jointVelocitiesDatafields;
+ IceUtil::Time discardUpdatesUntil = IceUtil::Time::seconds(-1);
+
+ void fetchDatafields();
+ bool isMoving() const;
+ };
+ RobotHead robotHead;
+ std::mutex robotHeadMutex;
+
objpose::ProviderInfoMap providers;
@@ -190,6 +211,8 @@ namespace armarx
Calibration calibration;
+
+
struct RemoteGuiTab : RemoteGui::Client::Tab
{
RemoteGui::Client::CheckBox visuEnabled;
diff --git a/source/RobotAPI/components/armem/ArMemDebugMemory/ArMemDebugMemory.cpp b/source/RobotAPI/components/armem/ArMemDebugMemory/ArMemDebugMemory.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..859bbd4a23a6dd324ae884bd1aa0cf4cb6dcb804
--- /dev/null
+++ b/source/RobotAPI/components/armem/ArMemDebugMemory/ArMemDebugMemory.cpp
@@ -0,0 +1,130 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::ArMemDebugMemory
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "ArMemDebugMemory.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <SimoxUtility/algorithm/string.h>
+
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
+
+namespace armarx
+{
+ ArMemDebugMemoryPropertyDefinitions::ArMemDebugMemoryPropertyDefinitions(std::string prefix) :
+ armarx::ComponentPropertyDefinitions(prefix)
+ {
+ }
+
+ ArMemDebugMemory::ArMemDebugMemory()
+ {
+ }
+
+ armarx::PropertyDefinitionsPtr ArMemDebugMemory::createPropertyDefinitions()
+ {
+ armarx::PropertyDefinitionsPtr defs = new ArMemDebugMemoryPropertyDefinitions(getConfigIdentifier());
+
+ defs->topic(debugObserver);
+ return defs;
+ }
+
+
+ std::string ArMemDebugMemory::getDefaultName() const
+ {
+ return "ArMemDebugMemory";
+ }
+
+
+ void ArMemDebugMemory::onInitComponent()
+ {
+ memory.name() = memoryName;
+ }
+
+
+ void ArMemDebugMemory::onConnectComponent()
+ {
+ RemoteGui__createTab();
+ RemoteGui_startRunningTask();
+ }
+
+
+ void ArMemDebugMemory::onDisconnectComponent()
+ {
+ }
+
+
+ void ArMemDebugMemory::onExitComponent()
+ {
+ }
+
+
+
+ // WRITING
+ armem::data::AddSegmentsResult ArMemDebugMemory::addSegments(const armem::data::AddSegmentsInput& input, const Ice::Current&)
+ {
+ armem::data::AddSegmentsResult result = ComponentPluginUser::addSegments(input, addCoreSegmentOnUsage);
+ tab.rebuild = true;
+ return result;
+ }
+
+
+ armem::data::CommitResult ArMemDebugMemory::commit(const armem::data::Commit& commit, const Ice::Current&)
+ {
+ armem::data::CommitResult result = ComponentPluginUser::commit(commit);
+ tab.rebuild = true;
+ return result;
+ }
+
+
+ // READING
+
+ // Inherited from Plugin
+
+
+
+ // REMOTE GUI
+
+ void ArMemDebugMemory::RemoteGui__createTab()
+ {
+ using namespace armarx::RemoteGui::Client;
+
+ armem::server::MemoryRemoteGui mrg;
+ {
+ std::scoped_lock lock(memoryMutex);
+ tab.memoryGroup = mrg.makeGroupBox(memory);
+ }
+
+ VBoxLayout root = {tab.memoryGroup, VSpacer()};
+ RemoteGui_createTab(getName(), root, &tab);
+ }
+
+
+ void ArMemDebugMemory::RemoteGui_update()
+ {
+ if (tab.rebuild.exchange(false))
+ {
+ RemoteGui__createTab();
+ }
+ }
+
+}
diff --git a/source/RobotAPI/components/armem/ArMemDebugMemory/ArMemDebugMemory.h b/source/RobotAPI/components/armem/ArMemDebugMemory/ArMemDebugMemory.h
new file mode 100644
index 0000000000000000000000000000000000000000..568f22c1674e319cdfc0d4eca37618ee314a9835
--- /dev/null
+++ b/source/RobotAPI/components/armem/ArMemDebugMemory/ArMemDebugMemory.h
@@ -0,0 +1,115 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::ArMemDebugMemory
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+
+#include <ArmarXCore/core/Component.h>
+
+#include <ArmarXCore/interface/observers/ObserverInterface.h>
+#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+
+#include <RobotAPI/libraries/armem/server/ComponentPlugin.h>
+
+
+namespace armarx
+{
+ /**
+ * @class ArMemDebugMemoryPropertyDefinitions
+ * @brief Property definitions of `ArMemDebugMemory`.
+ */
+ class ArMemDebugMemoryPropertyDefinitions :
+ public armarx::ComponentPropertyDefinitions
+ {
+ public:
+ ArMemDebugMemoryPropertyDefinitions(std::string prefix);
+ };
+
+
+ /**
+ * @defgroup Component-ArMemDebugMemory ArMemDebugMemory
+ * @ingroup RobotAPI-Components
+ * A description of the component ArMemDebugMemory.
+ *
+ * @class ArMemDebugMemory
+ * @ingroup Component-ArMemDebugMemory
+ * @brief Brief description of class ArMemDebugMemory.
+ *
+ * Detailed description of class ArMemDebugMemory.
+ */
+ class ArMemDebugMemory :
+ virtual public armarx::Component
+ , virtual public armem::server::ComponentPluginUser
+ , virtual public LightweightRemoteGuiComponentPluginUser
+ // , virtual public armarx::ArVizComponentPluginUser
+ {
+ public:
+ ArMemDebugMemory();
+
+ /// @see armarx::ManagedIceObject::getDefaultName()
+ std::string getDefaultName() const override;
+
+ // WritingInterface interface
+ public:
+ armem::data::AddSegmentsResult addSegments(const armem::data::AddSegmentsInput& input, const Ice::Current&) override;
+ armem::data::CommitResult commit(const armem::data::Commit& commit, const Ice::Current&) override;
+
+ // LightweightRemoteGuiComponentPluginUser interface
+ public:
+ void RemoteGui__createTab();
+ void RemoteGui_update() override;
+
+
+ protected:
+ /// @see armarx::ManagedIceObject::onInitComponent()
+ void onInitComponent() override;
+
+ /// @see armarx::ManagedIceObject::onConnectComponent()
+ void onConnectComponent() override;
+
+ /// @see armarx::ManagedIceObject::onDisconnectComponent()
+ void onDisconnectComponent() override;
+
+ /// @see armarx::ManagedIceObject::onExitComponent()
+ void onExitComponent() override;
+
+ /// @see PropertyUser::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+
+
+ private:
+ /// Debug observer. Used to visualize e.g. time series.
+ armarx::DebugObserverInterfacePrx debugObserver;
+
+ std::string memoryName = "DebugMemory";
+ bool addCoreSegmentOnUsage = true;
+
+ struct RemoteGuiTab : RemoteGui::Client::Tab
+ {
+ std::atomic_bool rebuild = false;
+ RemoteGui::Client::GroupBox memoryGroup;
+ };
+ RemoteGuiTab tab;
+
+ };
+}
diff --git a/source/RobotAPI/components/armem/ArMemDebugMemory/CMakeLists.txt b/source/RobotAPI/components/armem/ArMemDebugMemory/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..017e1709df01b4becba5b6aef90fa3dc031399a0
--- /dev/null
+++ b/source/RobotAPI/components/armem/ArMemDebugMemory/CMakeLists.txt
@@ -0,0 +1,37 @@
+armarx_component_set_name("ArMemDebugMemory")
+
+find_package(IVT QUIET)
+armarx_build_if(IVT_FOUND "IVT not available")
+
+
+set(COMPONENT_LIBS
+ ArmarXCore ArmarXCoreInterfaces # for DebugObserverInterface
+ ArmarXGuiComponentPlugins
+ RobotAPICore RobotAPIInterfaces armem
+ # RobotAPIComponentPlugins # for ArViz and other plugins
+
+ ${IVT_LIBRARIES}
+)
+
+set(SOURCES
+ ArMemDebugMemory.cpp
+)
+set(HEADERS
+ ArMemDebugMemory.h
+)
+
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
+if (IVT_FOUND)
+ target_include_directories(${ARMARX_COMPONENT_NAME} PUBLIC ${IVT_INCLUDE_DIRS})
+endif()
+
+
+# add unit tests
+# add_subdirectory(test)
+
+#generate the application
+armarx_generate_and_add_component_executable()
+
+
+
diff --git a/source/RobotAPI/components/armem/ArMemExampleMemory/xmls/ExampleData.xml b/source/RobotAPI/components/armem/ArMemDebugMemory/xmls/ExampleData.xml
similarity index 100%
rename from source/RobotAPI/components/armem/ArMemExampleMemory/xmls/ExampleData.xml
rename to source/RobotAPI/components/armem/ArMemDebugMemory/xmls/ExampleData.xml
diff --git a/source/RobotAPI/components/armem/ArMemExampleMemory/CMakeLists.txt b/source/RobotAPI/components/armem/ArMemExampleMemory/CMakeLists.txt
index 6cbc35ed322c3c1a941b7ef1c1cecc3e8d9ead81..c02d06dd2fc8aae51f745685e8fcb29a2b3fc696 100644
--- a/source/RobotAPI/components/armem/ArMemExampleMemory/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/ArMemExampleMemory/CMakeLists.txt
@@ -31,12 +31,12 @@ armarx_enable_aron_file_generation_for_target(
TARGET_NAME
${ARMARX_COMPONENT_NAME}
ARON_FILES
- xmls/ExampleData.xml
+ aron/ExampleData.xml
)
# add unit tests
-# add_subdirectory(test)
+add_subdirectory(test)
#generate the application
armarx_generate_and_add_component_executable()
diff --git a/source/RobotAPI/components/armem/ArMemExampleMemory/aron/ExampleData.xml b/source/RobotAPI/components/armem/ArMemExampleMemory/aron/ExampleData.xml
new file mode 100644
index 0000000000000000000000000000000000000000..0cd92acebcb91a793a58a7ce09896ac8c2f91214
--- /dev/null
+++ b/source/RobotAPI/components/armem/ArMemExampleMemory/aron/ExampleData.xml
@@ -0,0 +1,86 @@
+<!--Some fancy comment -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ <Include include="<Eigen/Core>" />
+ <Include include="<Image/ByteImage.h>" />
+ </CodeIncludes>
+ <GenerateTypes>
+ <GenerateType name='armarx::armem::aron::example::ExampleData'>
+ <ObjectChild key='the_int'>
+ <Int />
+ </ObjectChild>
+ <ObjectChild key='the_long'>
+ <Long />
+ </ObjectChild>
+ <ObjectChild key='the_float'>
+ <Float />
+ </ObjectChild>
+ <ObjectChild key='the_double'>
+ <Double />
+ </ObjectChild>
+ <ObjectChild key='the_string'>
+ <String />
+ </ObjectChild>
+ <ObjectChild key='the_bool'>
+ <Bool />
+ </ObjectChild>
+
+
+ <ObjectChild key='the_eigen_position'>
+ <EigenMatrix rows="3" cols="1" type="float" />
+ </ObjectChild>
+ <ObjectChild key='the_eigen_pose'>
+ <EigenMatrix rows="4" cols="4" type="float" />
+ </ObjectChild>
+ <ObjectChild key='the_ivt_image'>
+ <IVTCByteImage type="GrayScale" />
+ </ObjectChild>
+
+
+ <ObjectChild key='the_float_list'>
+ <List>
+ <Float />
+ </List>
+ </ObjectChild>
+ <ObjectChild key='the_int_list'>
+ <List>
+ <Int />
+ </List>
+ </ObjectChild>
+ <ObjectChild key='the_string_list'>
+ <List>
+ <String />
+ </List>
+ </ObjectChild>
+
+ <ObjectChild key='the_object_list'>
+ <List>
+ <Object name='ListClass'>
+ <ObjectChild key='element_int'>
+ <Int />
+ </ObjectChild>
+ <ObjectChild key='element_float'>
+ <Float />
+ </ObjectChild>
+ <ObjectChild key='element_string'>
+ <String />
+ </ObjectChild>
+ </Object>
+ </List>
+ </ObjectChild>
+
+ <ObjectChild key='the_float_dict'>
+ <Dict>
+ <Float />
+ </Dict>
+ </ObjectChild>
+ <ObjectChild key='the_int_dict'>
+ <Dict>
+ <Int />
+ </Dict>
+ </ObjectChild>
+
+ </GenerateType>
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/RobotAPI/components/armem/ArMemExampleMemory/test/ArMemExampleMemoryTest.cpp b/source/RobotAPI/components/armem/ArMemExampleMemory/test/ArMemExampleMemoryTest.cpp
index e8a7cc5fc9bfd76db190055f788e78c4d1212f55..1665316aa33d755c2dcd06e88e0a7bc37aebf864 100644
--- a/source/RobotAPI/components/armem/ArMemExampleMemory/test/ArMemExampleMemoryTest.cpp
+++ b/source/RobotAPI/components/armem/ArMemExampleMemory/test/ArMemExampleMemoryTest.cpp
@@ -27,11 +27,33 @@
#include <RobotAPI/Test.h>
#include "../ArMemExampleMemory.h"
+#include <RobotAPI/components/armem/ArMemExampleMemory/aron/ExampleData.aron.generated.h>
+#include <RobotAPI/libraries/armem/core.h>
+
#include <iostream>
-BOOST_AUTO_TEST_CASE(testExample)
+
+using armarx::armem::aron::example::ExampleData;
+namespace armem = armarx::armem;
+
+/*
+BOOST_AUTO_TEST_CASE(test_ExampleData)
+{
+ ExampleData data;
+ BOOST_CHECK(true);
+}
+*/
+
+
+BOOST_AUTO_TEST_CASE(test_ExampleData_type)
{
- armarx::ArMemExampleMemory instance;
+ armarx::aron::typenavigator::AronObjectTypeNavigatorPtr type = ExampleData::toInitialAronType();
+
+ BOOST_CHECK_EQUAL(type->getAcceptedTypes().size(), 15);
+
+ armem::Memory memory;
+ armem::CoreSegment& core = memory.addCoreSegment("ExampleData", type);
+ armem::ProviderSegment& prov = core.addProviderSegment("Provider");
- BOOST_CHECK_EQUAL(true, true);
+ BOOST_CHECK_EQUAL(core.aronType(), prov.aronType());
}
diff --git a/source/RobotAPI/components/armem/CMakeLists.txt b/source/RobotAPI/components/armem/CMakeLists.txt
index dfd5208e5a20c95daf7ffa45d6966e98f2afc9ff..6d4b4b0bf0e91cff4278a646479140f9afd27b59 100644
--- a/source/RobotAPI/components/armem/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/CMakeLists.txt
@@ -1,3 +1,4 @@
add_subdirectory(ArMemExampleMemory)
+add_subdirectory(ArMemDebugMemory)
add_subdirectory(ArMemExampleClient)
add_subdirectory(ArMemMemoryNameSystem)
diff --git a/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp b/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp
index 60672d5da0b446160b6e0a075a52303a44d10c79..950edbf74b1bdc1516614b7dfc8ac5c9a9598845 100644
--- a/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp
+++ b/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp
@@ -359,8 +359,8 @@ armarx::ObstacleAvoidingPlatformUnit::get_velocities()
ARMARX_CHECK(modulated_vel.allFinite()) << "Velocity contains non-finite values.";
ARMARX_CHECK(std::isfinite(target_rot_vel)) << "Rotation velocity is non-finite.";
- ARMARX_DEBUG << "Target velocity: " << target_vel.transpose() << "; " << target_rot_vel;
- ARMARX_DEBUG << "Modulated velocity: " << modulated_vel.transpose();
+ ARMARX_DEBUG << "Target velocity: " << target_vel.transpose() << "; norm: " << target_vel.norm() << "; " << target_rot_vel;
+ ARMARX_DEBUG << "Modulated velocity: " << modulated_vel.transpose() << modulated_vel.norm();
const auto r = Eigen::Rotation2D(m_control_data.agent_ori).toRotationMatrix().inverse();
@@ -802,14 +802,16 @@ armarx::ObstacleAvoidingPlatformUnit::createPropertyDefinitions()
// Settings.
def->optional(m_control_data.adaptive_max_vel_exp, "adaptive_max_vel_exponent",
- "Adaptive max vel exponent. 0 = disable.");
+ "Adaptive max vel exponent. This throttles the max_vel adaptively "
+ "depending on the angle between target velocity and modulated velocity. "
+ "0 = disable.");
// Control parameters.
def->optional(m_control_data.kp, "control.pos.kp");
def->optional(m_rot_pid_controller.Kp, "control.rot.kp");
def->optional(m_rot_pid_controller.Ki, "control.rot.ki");
def->optional(m_rot_pid_controller.Kd, "control.rot.kd");
- def->optional(m_control_loop.cycle_time, "control.pose.cycle_time");
+ def->optional(m_control_loop.cycle_time, "control.pose.cycle_time", "Control loop cycle time.");
// Obstacle avoidance parameter.
def->optional(m_control_data.agent_safety_margin, "doa.agent_safety_margin");
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
index fb07832483bfea3d9ae69d9aece2bb20fbe90289..6ad411f67b743fecaf8ed8fa82a31f0d3e0d0b8c 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
@@ -182,7 +182,10 @@ namespace armarx
_publishWpsNum = r.wps.size();
_rtHasWps = !r.wps.empty();
_rtWpController->swapWaypoints(r.wps);
- _rtWpController->skipToClosestWaypoint(r.skipRad2mmFactor);
+ if (r.cfg.skipToClosestWaypoint)
+ {
+ _rtWpController->skipToClosestWaypoint(r.skipRad2mmFactor);
+ }
}
_publishIsAtForceLimit = false;
}
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h
index b09e6f605d46dd1be00301765a974fd6b084f7a7..9ed0b775c33ca9bc3a32c239d6635fd9d021044e 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h
@@ -77,6 +77,7 @@ namespace armarx
float skipRad2mmFactor = 500;
bool wpsUpdated = false;
bool cfgUpdated = false;
+ bool skipToClosestWaypoint = true;
};
struct RtToNonRtData
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp
index 3ccb1f9be38f93ae8d7a3ee96ac6cfb58957f631..f03a527d7a2b87c5c5752e2f16cdaa0baaee13c4 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp
@@ -209,6 +209,17 @@ namespace armarx::RobotUnitModule
ARMARX_TRACE;
return nullptr;
}
+ ARMARX_INFO << "wiating until controller '" << instanceName << "' is deleted";
+ }
+ while (getArmarXManager()->getIceManager()->isObjectReachable(instanceName))
+ {
+ ARMARX_TRACE;
+ if (isShuttingDown())
+ {
+ ARMARX_TRACE;
+ return nullptr;
+ }
+ ARMARX_INFO << "wiating until controller '" << instanceName << "' is removed from ice";
}
return NJointControllerInterfacePrx::uncheckedCast(
createNJointController(className, instanceName, config, true, false)->getProxy(-1, true));
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
index 0ed1a250ab31b69f64afd236ad20462aab05fc95..26fe846c4c2b6165d0a4f5f57d95f47dd874f6ab 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
@@ -37,7 +37,47 @@
#include "../util/ControlThreadOutputBuffer.h"
#include <boost/algorithm/string/predicate.hpp>
-
+namespace armarx::RobotUnitModule::details
+{
+ struct DoLoggingDurations
+ {
+ struct DurationsEntry
+ {
+ DurationsEntry()
+ {
+ t = IceUtil::Time::seconds(0);
+ }
+ IceUtil::Time t;
+ void start()
+ {
+ t -= IceUtil::Time::now();
+ ++n;
+ }
+ void stop()
+ {
+ t += IceUtil::Time::now();
+ }
+ double ms() const
+ {
+ return t.toMilliSecondsDouble();
+ }
+ std::size_t n = 0;
+ };
+ DurationsEntry header;
+ DurationsEntry header_csv;
+ DurationsEntry header_stream;
+ DurationsEntry sens;
+ DurationsEntry sens_csv;
+ DurationsEntry sens_stream;
+ DurationsEntry sens_stream_elem;
+ DurationsEntry ctrl;
+ DurationsEntry ctrl_csv;
+ DurationsEntry ctrl_stream;
+ DurationsEntry ctrl_stream_elem;
+ DurationsEntry backlog;
+ DurationsEntry msg;
+ };
+}
namespace armarx::RobotUnitModule
{
void Logging::addMarkerToRtLog(const RemoteReferenceCounterBasePtr& token, const std::string& marker, const Ice::Current&)
@@ -373,6 +413,8 @@ namespace armarx::RobotUnitModule
else if make_case(Ice::Long, Long)
else if make_case(Ice::Float, Float)
else if make_case(Ice::Double, Double)
+ else if make_case(std::uint16_t, Long)
+ else if make_case(std::uint32_t, Long)
else
{
ARMARX_CHECK_EXPRESSION(false)
@@ -435,15 +477,11 @@ namespace armarx::RobotUnitModule
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
defaultLogHandle = nullptr;
- if (rtLoggingTask)
+ if (rtLoggingTask.joinable())
{
ARMARX_DEBUG << "shutting down rt logging task";
- rtLoggingTask->stop();
- while (rtLoggingTask->isFunctionExecuting() || rtLoggingTask->isRunning())
- {
- ARMARX_FATAL << deactivateSpam(0.1) << "RT LOGGING TASK IS RUNNING AFTER IT WAS STOPPED!";
- }
- rtLoggingTask = nullptr;
+ stopRtLoggingTask = true;
+ rtLoggingTask.join();
ARMARX_DEBUG << "shutting down rt logging task done";
}
}
@@ -455,8 +493,25 @@ namespace armarx::RobotUnitModule
controlThreadId = LogSender::getThreadId();
ControlThreadOutputBuffer::RtLoggingInstance = &(_module<ControlThreadDataBuffer>().getControlThreadOutputBuffer());
- ARMARX_INFO << "starting rt logging with timestep " << rtLoggingTimestep;
- rtLoggingTask->start();
+ ARMARX_INFO << "starting rt logging with timestep " << rtLoggingTimestepMs;
+ stopRtLoggingTask = false;
+ rtLoggingTask = std::thread{[&]{
+ using clock_t = std::chrono::steady_clock;
+ const auto now = []{return clock_t::now();};
+ while (!stopRtLoggingTask)
+ {
+ const auto start = now();
+ doLogging();
+ const std::uint64_t ms = std::chrono::duration_cast<std::chrono::milliseconds>(now() - start).count();
+ if (ms < rtLoggingTimestepMs)
+ {
+ std::this_thread::sleep_for(std::chrono::milliseconds{rtLoggingTimestepMs - ms});
+ continue;
+ }
+ ARMARX_WARNING << deactivateSpam(10) << "logging thread took " << ms
+ << " ms > " << rtLoggingTimestepMs << " ms (message printed every 10 seconds)";
+ }
+ }};
}
void Logging::doLogging()
@@ -468,6 +523,7 @@ namespace armarx::RobotUnitModule
// entries are removed last
//remove backlog entries
+ const auto start_time_remove_backlog_entries = IceUtil::Time::now();
{
while (!backlog.empty() && backlog.front().writeTimestamp + rtLoggingBacklogRetentionTime < now)
{
@@ -475,6 +531,8 @@ namespace armarx::RobotUnitModule
}
}
//log all
+ const auto start_time_log_all = IceUtil::Time::now();
+ details::DoLoggingDurations dlogdurs;
{
ARMARX_TRACE;
if (!rtLoggingEntries.empty() || !rtDataStreamingEntry.empty())
@@ -486,12 +544,14 @@ namespace armarx::RobotUnitModule
_module<ControlThreadDataBuffer>().getControlThreadOutputBuffer().foreachNewLoggingEntry(
[&](const auto & data, auto i, auto num)
{
- doLogging(now, data, i, num);
+ ARMARX_TRACE;
+ doLogging(dlogdurs, now, data, i, num);
}
);
}
ARMARX_DEBUG_S << ::deactivateSpam() << "the last " << backlog.size() << " iterations are stored";
//flush all files
+ const auto start_time_flush_all_files = IceUtil::Time::now();
{
for (auto& pair : rtLoggingEntries)
{
@@ -500,6 +560,7 @@ namespace armarx::RobotUnitModule
}
//remove entries
+ const auto start_time_remove_entries = IceUtil::Time::now();
{
ARMARX_TRACE;
std::vector<std::string> toRemove;
@@ -519,6 +580,7 @@ namespace armarx::RobotUnitModule
}
}
//deal with data streaming
+ const auto start_time_data_streaming = IceUtil::Time::now();
{
ARMARX_TRACE;
std::vector<RobotUnitDataStreaming::ReceiverPrx> toRemove;
@@ -531,45 +593,82 @@ namespace armarx::RobotUnitModule
}
else
{
- data.send(prx);
+ data.send(prx, rtDataStreamingMsgID);
}
}
+ ++rtDataStreamingMsgID;
for (const auto& prx : toRemove)
{
rtDataStreamingEntry.erase(prx);
}
}
+ const auto end_time = IceUtil::Time::now();
+ const auto time_total = (end_time - now).toMilliSecondsDouble();
+ ARMARX_DEBUG_S << "rtlogging time required: " << time_total << "ms\n"
+ << " time_remove_backlog_entries " << (start_time_log_all - start_time_remove_backlog_entries).toMilliSecondsDouble() << "ms\n"
+ << " time_log_all " << (start_time_flush_all_files - start_time_log_all).toMilliSecondsDouble() << "ms\n"
+ << " header " << dlogdurs.header.ms() << "ms\t(" << dlogdurs.header.n << " calls)\n"
+ << " csv " << dlogdurs.header_csv.ms() << "ms\t(" << dlogdurs.header_csv.n << " calls)\n"
+ << " stream " << dlogdurs.header_stream.ms() << "ms\t(" << dlogdurs.header_stream.n << " calls)\n"
+ << " sens " << dlogdurs.sens.ms() << "ms\t(" << dlogdurs.sens.n << " calls)\n"
+ << " csv " << dlogdurs.sens_csv.ms() << "ms\t(" << dlogdurs.sens_csv.n << " calls)\n"
+ << " stream " << dlogdurs.sens_stream.ms() << "ms\t(" << dlogdurs.sens_stream.n << " calls)\n"
+ << " per elem " << dlogdurs.sens_stream_elem.ms() << "ms\t(" << dlogdurs.sens_stream_elem.n << " calls)\n"
+ << " ctrl " << dlogdurs.ctrl.ms() << "ms\t(" << dlogdurs.ctrl.n << " calls)\n"
+ << " csv " << dlogdurs.ctrl_csv.ms() << "ms\t(" << dlogdurs.ctrl_csv.n << " calls)\n"
+ << " stream " << dlogdurs.ctrl_stream.ms() << "ms\t(" << dlogdurs.ctrl_stream.n << " calls)\n"
+ << " per elem " << dlogdurs.ctrl_stream_elem.ms() << "ms\t(" << dlogdurs.ctrl_stream_elem.n << " calls)\n"
+ << " backlog " << dlogdurs.backlog.ms() << "ms\t(" << dlogdurs.backlog.n << " calls)\n"
+ << " msg " << dlogdurs.msg.ms() << "ms\t(" << dlogdurs.msg.n << " calls)\n"
+ << " time_flush_all_files " << (start_time_remove_entries - start_time_flush_all_files).toMilliSecondsDouble() << "ms\n"
+ << " time_remove_entries " << (start_time_data_streaming - start_time_remove_entries).toMilliSecondsDouble() << "ms\n"
+ << " time_data_streaming " << (end_time - start_time_data_streaming).toMilliSecondsDouble() << "ms\n";
}
- void Logging::doLogging(const IceUtil::Time& now, const
+ void Logging::doLogging(details::DoLoggingDurations& durations,
+ const IceUtil::Time& now, const
ControlThreadOutputBuffer::Entry& data,
std::size_t i, std::size_t num)
{
+
ARMARX_TRACE;
//header
{
+ durations.header.start();
ARMARX_TRACE;
//base (marker;iteration;timestamp)
- for (auto& [_, e] : rtLoggingEntries)
+ if (!rtLoggingEntries.empty())
{
- e->stream << "\n"
- << e->marker << ";"
- << data.iteration << ";"
- << data.sensorValuesTimestamp.toMicroSeconds() << ";"
- << data.timeSinceLastIteration.toMicroSeconds();
- e->marker.clear();
+ durations.header_csv.start();
+ for (auto& [_, e] : rtLoggingEntries)
+ {
+ e->stream << "\n"
+ << e->marker << ";"
+ << data.iteration << ";"
+ << data.sensorValuesTimestamp.toMicroSeconds() << ";"
+ << data.timeSinceLastIteration.toMicroSeconds();
+ e->marker.clear();
+ }
+ durations.header_csv.stop();
}
//streaming
- for (auto& [_, e] : rtDataStreamingEntry)
+ if (!rtDataStreamingEntry.empty())
{
- e.processHeader(data);
+ durations.header_stream.start();
+ for (auto& [_, e] : rtDataStreamingEntry)
+ {
+ e.processHeader(data);
+ }
+ durations.header_stream.stop();
}
+ durations.header.stop();
}
//process devices
{
//sens
{
ARMARX_TRACE;
+ durations.sens.start();
//sensors
for (std::size_t idxDev = 0; idxDev < data.sensors.size(); ++ idxDev)
{
@@ -577,23 +676,37 @@ namespace armarx::RobotUnitModule
//dimensions of sensor value (e.g. vel, tor, f_x, f_y, ...)
for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields(); ++ idxField)
{
- const auto str = val->getDataFieldAs<std::string>(idxField);
- for (auto& [_, entry] : rtLoggingEntries)
+ if (!rtLoggingEntries.empty())
{
- if (entry->loggedSensorDeviceValues.at(idxDev).at(idxField))
+ durations.sens_csv.start();
+ const auto str = val->getDataFieldAs<std::string>(idxField);
+ for (auto& [_, entry] : rtLoggingEntries)
{
- entry->stream << ";" << str;
+ if (entry->loggedSensorDeviceValues.at(idxDev).at(idxField))
+ {
+ entry->stream << ";" << str;
+ }
}
+ durations.sens_csv.stop();
}
- for (auto& [_, data] : rtDataStreamingEntry)
+ if (!rtDataStreamingEntry.empty())
{
- data.processSens(*val, idxDev, idxField);
+ durations.sens_stream.start();
+ for (auto& [_, data] : rtDataStreamingEntry)
+ {
+ durations.sens_stream_elem.start();
+ data.processSens(*val, idxDev, idxField);
+ durations.sens_stream_elem.stop();
+ }
+ durations.sens_stream.stop();
}
}
}
+ durations.sens.stop();
}
//ctrl
{
+ durations.ctrl.start();
ARMARX_TRACE;
//joint controllers
for (std::size_t idxDev = 0; idxDev < data.control.size(); ++ idxDev)
@@ -606,36 +719,52 @@ namespace armarx::RobotUnitModule
//dimensions of control value (e.g. v_platform_x, v_platform_y, v_platform_rotate)
for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields(); ++ idxField)
{
- std::string str;
- val->getDataFieldAs(idxField, str);
- for (auto& [_, entry] : rtLoggingEntries)
+ if (!rtLoggingEntries.empty())
{
- if (entry->loggedControlDeviceValues.at(idxDev).at(idxVal).at(idxField))
+ durations.ctrl_csv.start();
+ std::string str;
+ val->getDataFieldAs(idxField, str); // expensive function
+ for (auto& [_, entry] : rtLoggingEntries)
{
- entry->stream << ";" << str;
+ if (entry->loggedControlDeviceValues.at(idxDev).at(idxVal).at(idxField))
+ {
+ entry->stream << ";" << str;
+ }
}
+ durations.ctrl_csv.stop();
}
- for (auto& [_, data] : rtDataStreamingEntry)
+ if (!rtDataStreamingEntry.empty())
{
- data.processCtrl(*val, idxDev, idxVal, idxField);
+ durations.ctrl_stream.start();
+ for (auto& [_, data] : rtDataStreamingEntry)
+ {
+ durations.ctrl_stream_elem.start();
+ data.processCtrl(*val, idxDev, idxVal, idxField);
+ durations.ctrl_stream_elem.stop();
+ }
+ durations.ctrl_stream.stop();
}
}
}
}
+ durations.ctrl.stop();
}
}
//finish processing
{
//store data to backlog
{
+ durations.backlog.start();
ARMARX_TRACE;
if (data.writeTimestamp + rtLoggingBacklogRetentionTime >= now)
{
backlog.emplace_back(data, true); //true for minimal copy
}
+ durations.backlog.stop();
}
//print + reset messages
{
+ durations.msg.start();
ARMARX_TRACE;
for (const ::armarx::detail::RtMessageLogEntryBase* ptr : data.messages.getEntries())
{
@@ -645,6 +774,7 @@ namespace armarx::RobotUnitModule
}
ptr->print(controlThreadId);
}
+ durations.msg.stop();
}
}
}
@@ -673,8 +803,8 @@ namespace armarx::RobotUnitModule
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- rtLoggingTimestep = getProperty<std::size_t>("RTLogging_PeriodMs");
- ARMARX_CHECK_LESS(0, rtLoggingTimestep) << "The property RTLoggingPeriodMs must not be 0";
+ rtLoggingTimestepMs = getProperty<std::size_t>("RTLogging_PeriodMs");
+ ARMARX_CHECK_LESS(0, rtLoggingTimestepMs) << "The property RTLoggingPeriodMs must not be 0";
messageBufferSize = getProperty<std::size_t>("RTLogging_MessageBufferSize");
messageBufferMaxSize = getProperty<std::size_t>("RTLogging_MaxMessageBufferSize");
@@ -697,7 +827,7 @@ namespace armarx::RobotUnitModule
{
ARMARX_TRACE;
std::size_t ctrlThreadPeriodUs = static_cast<std::size_t>(getControlThreadTargetPeriod().toMicroSeconds());
- std::size_t logThreadPeriodUs = rtLoggingTimestep * 1000;
+ std::size_t logThreadPeriodUs = rtLoggingTimestepMs * 1000;
std::size_t nBuffers = (logThreadPeriodUs / ctrlThreadPeriodUs + 1) * 100;
const auto bufferSize = _module<ControlThreadDataBuffer>().getControlThreadOutputBuffer().initialize(
@@ -762,24 +892,20 @@ namespace armarx::RobotUnitModule
defaultLogHandle = startRtLogging(loggingpath, getLoggingNames());
}
}
- //setup thread (don't start it
- rtLoggingTask = new RTLoggingTaskT([&] {doLogging();}, rtLoggingTimestep, false, getName() + "_RTLoggingTask");
- rtLoggingTask->setDelayWarningTolerance(rtLoggingTimestep * 10);
}
- void Logging::DataStreamingEntry::processHeader(const ControlThreadOutputBuffer::Entry& e)
+ void Logging::DataStreamingEntry::clearResult()
{
- ARMARX_TRACE;
- if (stopStreaming)
+ for (auto& e : result)
{
- return;
+ entryBuffer.emplace_back(std::move(e));
}
- result.emplace_back();
- auto& data = result.back();
- data.iterationId = e.iteration;
- data.timestampUSec = e.sensorValuesTimestamp.toMicroSeconds();
- data.timesSinceLastIterationUSec = e.timeSinceLastIteration.toMicroSeconds();
+ result.clear();
+ }
+ RobotUnitDataStreaming::TimeStep Logging::DataStreamingEntry::allocateResultElement() const
+ {
+ RobotUnitDataStreaming::TimeStep data;
data.bools .resize(numBools);
data.bytes .resize(numBytes);
data.shorts .resize(numShorts);
@@ -787,43 +913,87 @@ namespace armarx::RobotUnitModule
data.longs .resize(numLongs);
data.floats .resize(numFloats);
data.doubles.resize(numDoubles);
+ return data;
+ }
+
+ RobotUnitDataStreaming::TimeStep Logging::DataStreamingEntry::getResultElement()
+ {
+ if (entryBuffer.empty())
+ {
+ return allocateResultElement();
+ }
+ auto e = std::move(entryBuffer.back());
+ entryBuffer.pop_back();
+ return e;
+ }
+
+ void Logging::DataStreamingEntry::processHeader(const ControlThreadOutputBuffer::Entry& e)
+ {
+ ARMARX_TRACE;
+ if (stopStreaming)
+ {
+ return;
+ }
+ result.emplace_back(getResultElement());
+ auto& data = result.back();
+ data.iterationId = e.iteration;
+ data.timestampUSec = e.sensorValuesTimestamp.toMicroSeconds();
+ data.timesSinceLastIterationUSec = e.timeSinceLastIteration.toMicroSeconds();
}
- void WriteTo(const Logging::DataStreamingEntry::OutVal& out,
+ void WriteTo(const auto& dentr,
+ const Logging::DataStreamingEntry::OutVal& out,
const auto& val,
std::size_t fidx,
auto& data)
{
ARMARX_TRACE;
using enum_t = Logging::DataStreamingEntry::ValueT;
- switch (out.value)
- {
- case enum_t::Bool :
- bool b;
- val.getDataFieldAs(fidx, b);
- data.bools.at(out.idx) = b;
- return;
- case enum_t::Byte :
- val.getDataFieldAs(fidx, data.bytes.at(out.idx));
- return;
- case enum_t::Short :
- val.getDataFieldAs(fidx, data.shorts.at(out.idx));
- return;
- case enum_t::Int :
- val.getDataFieldAs(fidx, data.ints.at(out.idx));
- return;
- case enum_t::Long :
- val.getDataFieldAs(fidx, data.longs.at(out.idx));
- return;
- case enum_t::Float :
- val.getDataFieldAs(fidx, data.floats.at(out.idx));
- return;
- case enum_t::Double :
- val.getDataFieldAs(fidx, data.doubles.at(out.idx));
- return;
- case enum_t::Skipped:
- return;
+ try
+ {
+ ARMARX_TRACE;
+ switch (out.value)
+ {
+ case enum_t::Bool :
+ bool b;
+ val.getDataFieldAs(fidx, b);
+ data.bools.at(out.idx) = b;
+ return;
+ case enum_t::Byte :
+ val.getDataFieldAs(fidx, data.bytes.at(out.idx));
+ return;
+ case enum_t::Short :
+ val.getDataFieldAs(fidx, data.shorts.at(out.idx));
+ return;
+ case enum_t::Int :
+ val.getDataFieldAs(fidx, data.ints.at(out.idx));
+ return;
+ case enum_t::Long :
+ val.getDataFieldAs(fidx, data.longs.at(out.idx));
+ return;
+ case enum_t::Float :
+ val.getDataFieldAs(fidx, data.floats.at(out.idx));
+ return;
+ case enum_t::Double :
+ val.getDataFieldAs(fidx, data.doubles.at(out.idx));
+ return;
+ case enum_t::Skipped:
+ return;
+ }
+ }
+ catch (...)
+ {
+ ARMARX_ERROR << GetHandledExceptionString()
+ << "\ntype " << static_cast<int>(out.value)
+ << "\n" << VAROUT(data.bools.size()) << " " << VAROUT(dentr.numBools)
+ << "\n" << VAROUT(data.bytes.size()) << " " << VAROUT(dentr.numBytes)
+ << "\n" << VAROUT(data.shorts.size()) << " " << VAROUT(dentr.numShorts)
+ << "\n" << VAROUT(data.ints.size()) << " " << VAROUT(dentr.numInts)
+ << "\n" << VAROUT(data.longs.size()) << " " << VAROUT(dentr.numLongs)
+ << "\n" << VAROUT(data.floats.size()) << " " << VAROUT(dentr.numFloats)
+ << "\n" << VAROUT(data.doubles.size()) << " " << VAROUT(dentr.numDoubles);
+ throw;
}
}
@@ -840,7 +1010,7 @@ namespace armarx::RobotUnitModule
}
auto& data = result.back();
const OutVal& o = ctrlDevs.at(didx).at(vidx).at(fidx);
- WriteTo(o, val, fidx, data);
+ WriteTo(*this, o, val, fidx, data);
}
void Logging::DataStreamingEntry::processSens(
@@ -855,28 +1025,49 @@ namespace armarx::RobotUnitModule
}
auto& data = result.back();
const OutVal& o = sensDevs.at(didx).at(fidx);
- WriteTo(o, val, fidx, data);
+ WriteTo(*this, o, val, fidx, data);
}
- void Logging::DataStreamingEntry::send(const RobotUnitDataStreaming::ReceiverPrx& r)
+ void Logging::DataStreamingEntry::send(const RobotUnitDataStreaming::ReceiverPrx& r,
+ std::uint64_t msgId)
{
ARMARX_TRACE;
- try
- {
- r->update(result);
- connectionFailures = 0;
- result.clear();
- }
- catch (...)
+ const auto start_send = IceUtil::Time::now();
+ updateCalls.emplace_back(r->begin_update(result, static_cast<Ice::Long>(msgId)));
+ const auto start_clear = IceUtil::Time::now();
+ clearResult();
+ const auto end = IceUtil::Time::now();
+ ARMARX_DEBUG_S << "Logging::DataStreamingEntry::send"
+ << "\n update " << (start_clear - start_send).toMilliSecondsDouble() << "ms (" << result.size() << " timesteps)"
+ << "\n clear " << (end - start_clear).toMilliSecondsDouble() << "ms";
+
+ //now execute all ready callbacks
+ std::size_t i = 0;
+ for (; i < updateCalls.size(); ++i)
{
- ARMARX_TRACE;
- ++connectionFailures;
- if (connectionFailures > rtStreamMaxClientErrors)
+ try
+ {
+ if (!updateCalls.at(i)->isCompleted())
+ {
+ break;
+ }
+ r->end_update(updateCalls.at(i));
+ connectionFailures = 0;
+ }
+ catch (...)
{
- stopStreaming = true;
- ARMARX_WARNING_S << "DataStreaming Receiver was not reachable for "
- << connectionFailures << " iterations! Removing receiver";
+ ARMARX_TRACE;
+ ++connectionFailures;
+ if (connectionFailures > rtStreamMaxClientErrors)
+ {
+ stopStreaming = true;
+ ARMARX_WARNING_S << "DataStreaming Receiver was not reachable for "
+ << connectionFailures << " iterations! Removing receiver";
+ updateCalls.clear();
+ break;
+ }
}
}
+ updateCalls.erase(updateCalls.begin(), updateCalls.begin() + i);
}
}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.h
index 6271ef1784a4cd79e8e3b37addcb4e679e946bc2..975979177d45d71530317ea6f282929d72f6924c 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.h
@@ -25,6 +25,7 @@
#include <atomic>
#include <map>
#include <vector>
+#include <deque>
#include <fstream>
#include <ArmarXCore/core/services/tasks/TaskUtil.h>
@@ -34,6 +35,11 @@
#include "RobotUnitModuleBase.h"
#include "../util/ControlThreadOutputBuffer.h"
+namespace armarx::RobotUnitModule::details
+{
+ struct DoLoggingDurations;
+}
+
namespace armarx::RobotUnitModule
{
class LoggingPropertyDefinitions: public ModuleBasePropertyDefinitions
@@ -168,7 +174,8 @@ namespace armarx::RobotUnitModule
private:
/// @brief Executes the logging loop.
void doLogging();
- void doLogging(const IceUtil::Time& now,
+ void doLogging(details::DoLoggingDurations& durations,
+ const IceUtil::Time& now,
const ControlThreadOutputBuffer::Entry& data,
std::size_t i, std::size_t num);
static bool MatchName(const std::string& pattern, const std::string& name);
@@ -177,8 +184,6 @@ namespace armarx::RobotUnitModule
// ///////////////////////////////////////// Data ///////////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
private:
- using RTLoggingTaskT = SimplePeriodicTask<std::function<void(void)>>;
-
/// @brief Data structure holding information about an logging entry.
struct CSVLoggingEntry
{
@@ -233,6 +238,14 @@ namespace armarx::RobotUnitModule
std::vector<std::vector<std::vector<OutVal>>> ctrlDevs;
RobotUnitDataStreaming::TimeStepSeq result;
+ std::deque<RobotUnitDataStreaming::TimeStep> entryBuffer;
+ std::deque<Ice::AsyncResultPtr> updateCalls;
+ void clearResult();
+ RobotUnitDataStreaming::TimeStep getResultElement();
+ RobotUnitDataStreaming::TimeStep allocateResultElement() const;
+
+
+
void processHeader(const ControlThreadOutputBuffer::Entry& e);
void processCtrl(const ControlTargetBase& val,
std::size_t didx,
@@ -241,12 +254,14 @@ namespace armarx::RobotUnitModule
void processSens(const SensorValueBase& val,
std::size_t didx,
std::size_t fidx);
- void send(const RobotUnitDataStreaming::ReceiverPrx& r);
+ void send(const RobotUnitDataStreaming::ReceiverPrx& r, uint64_t msgId);
};
std::map<RobotUnitDataStreaming::ReceiverPrx, DataStreamingEntry> rtDataStreamingEntry;
+ std::uint64_t rtDataStreamingMsgID = 0;
/// @brief The thread executing the logging functions.
- RTLoggingTaskT::pointer_type rtLoggingTask;
+ std::thread rtLoggingTask;
+ std::atomic_bool stopRtLoggingTask{false};
/// @brief All entries for logging.
std::map<std::string, std::shared_ptr<CSVLoggingEntry>> rtLoggingEntries;
/// @brief Handle for the default log. (This log can be enabled via a property and logs everything)
@@ -282,7 +297,7 @@ namespace armarx::RobotUnitModule
/// @brief The maximal number of Message entries
std::size_t messageBufferMaxNumberEntries {0};
/// @brief The logging thread's period
- std::size_t rtLoggingTimestep {0};
+ std::size_t rtLoggingTimestepMs {0};
/// @brief The time an entry shold remain in the backlog.
IceUtil::Time rtLoggingBacklogRetentionTime;
};
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
index 763bab62f2773dbb26e45efbc3e38ae7b4839561..6f24dde1d0f5cf513b1c8520a5deabe1ad6ca91e 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
@@ -277,8 +277,7 @@ namespace armarx::RobotUnitModule
{
if (robotUnitObserver->getState() >= eManagedIceObjectStarted)
{
- robotUnitObserver->offerOrUpdateDataFieldsFlatCopy(robotUnitObserver->controlDevicesChannel, ctrlDevMap);
- robotUnitObserver->updateChannel(robotUnitObserver->controlDevicesChannel);
+ robotUnitObserver->offerOrUpdateDataFieldsFlatCopy_async(robotUnitObserver->controlDevicesChannel, std::move(ctrlDevMap));
}
}
@@ -338,8 +337,7 @@ namespace armarx::RobotUnitModule
out << pair.first << ": " << (pair.second ? pair.second->ice_id() + Variant::typeToString(pair.second->getType()) : "NULL") << "\n";
}
};
- robotUnitObserver->offerOrUpdateDataFieldsFlatCopy(robotUnitObserver->sensorDevicesChannel, sensorDevMap);
- robotUnitObserver->updateChannel(robotUnitObserver->sensorDevicesChannel);
+ robotUnitObserver->offerOrUpdateDataFieldsFlatCopy_async(robotUnitObserver->sensorDevicesChannel, std::move(sensorDevMap));
}
}
@@ -583,13 +581,11 @@ namespace armarx::RobotUnitModule
{
if (observerPublishTimingInformation)
{
- robotUnitObserver->offerOrUpdateDataFieldsFlatCopy(robotUnitObserver->additionalChannel, additionalMap);
- robotUnitObserver->updateChannel(robotUnitObserver->additionalChannel);
+ robotUnitObserver->offerOrUpdateDataFieldsFlatCopy_async(robotUnitObserver->additionalChannel, std::move(additionalMap));
}
if (observerPublishAdditionalInformation)
{
- robotUnitObserver->offerOrUpdateDataFieldsFlatCopy(robotUnitObserver->timingChannel, timingMap);
- robotUnitObserver->updateChannel(robotUnitObserver->timingChannel);
+ robotUnitObserver->offerOrUpdateDataFieldsFlatCopy_async(robotUnitObserver->timingChannel, std::move(timingMap));
}
}
}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitObserver.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitObserver.cpp
index bb04d0b1538db48e35e43745dd8020c3418ab0ac..004d3361faf19b3acfdfd99e0278d0246c004449 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitObserver.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitObserver.cpp
@@ -4,52 +4,12 @@
namespace armarx
{
-
- RobotUnitObserver::RobotUnitObserver()
- {
-
- }
-
- void RobotUnitObserver::offerOrUpdateDataFieldsFlatCopy(const std::string& channelName, const StringVariantBaseMap& valueMap)
+ void RobotUnitObserver::offerOrUpdateDataFieldsFlatCopy_async(const std::string& channelName, StringVariantBaseMap&& valueMap)
{
- if (!existsChannel(channelName))
- {
- offerChannel(channelName, "");
- }
- try
+ addWorkerJob("RobotUnitObserver::offerOrUpdateDataFieldsFlatCopy_async", [this, channelName, vmap = std::move(valueMap)]
{
- // fastest way is to just try to set the datafields
- setDataFieldsFlatCopy(channelName, valueMap, true);
- }
- catch (exceptions::user::InvalidDataFieldException& e)
- {
- ARMARX_INFO << deactivateSpam() << "Creating datafields for channel " << channelName;
- // on failure: do it the slow way
- for (const auto& value : valueMap)
- {
- const std::string& datafieldName = value.first;
-
- if (!existsDataField(channelName, datafieldName))
- {
- try
- {
- offerDataFieldWithDefault(channelName, datafieldName, *VariantPtr::dynamicCast(value.second), "");
- }
- catch (exceptions::user::DatafieldExistsAlreadyException& e)
- {
- setDataFieldFlatCopy(channelName, datafieldName, VariantPtr::dynamicCast(value.second));
- }
- }
- else
- {
- setDataFieldFlatCopy(channelName, datafieldName, VariantPtr::dynamicCast(value.second));
- }
- }
- }
- }
-
- void RobotUnitObserver::onInitObserver()
- {
+ offerOrUpdateDataFieldsFlatCopy(channelName, vmap);
+ });
}
void RobotUnitObserver::onConnectObserver()
@@ -59,5 +19,4 @@ namespace armarx
offerChannel(timingChannel, "Channel for timings of the different phases of the robot unit");
offerChannel(additionalChannel, "Additional custom datafields");
}
-
-} // namespace armarx
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitObserver.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitObserver.h
index 728d1682cba7f127b003c070179dc1ef0a52ada9..2fdfb990e824b343cb88337c8316363ceb964a3e 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitObserver.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitObserver.h
@@ -20,12 +20,13 @@ namespace armarx
const std::string timingChannel = "TimingInformation";
const std::string controlDevicesChannel = "ControlDevices";
const std::string sensorDevicesChannel = "SensorDevices";
- RobotUnitObserver();
- void offerOrUpdateDataFieldsFlatCopy(const std::string& channelName, StringVariantBaseMap const& valueMap);
+ RobotUnitObserver() = default;
+
+ void offerOrUpdateDataFieldsFlatCopy_async(const std::string& channelName, StringVariantBaseMap&& valueMap);
// Observer interface
protected:
- void onInitObserver() override;
+ void onInitObserver() override {}
void onConnectObserver() override;
friend class RobotUnitModule::Publisher;
diff --git a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp
index 8f038519bc03b014aef5219bb939e1acf03d55af..2dd62115bcf3ff0895cb3ecaa04fb2d2614d2294 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp
@@ -73,6 +73,7 @@ namespace armarx
void ControlThreadOutputBuffer::foreachNewLoggingEntry(ConsumerFunctor consumer)
{
+ ARMARX_TRACE;
ARMARX_CHECK_EXPRESSION(isInitialized);
if (writePosition - onePastLoggingReadPosition >= numEntries)
{
@@ -85,7 +86,11 @@ namespace armarx
}
//the number of new entries
auto numNewEntries = writePosition - onePastLoggingReadPosition;
- ARMARX_CHECK_EXPRESSION(numNewEntries < numEntries);
+ if (numNewEntries >= numEntries)
+ {
+ ARMARX_ERROR << " more new entries (" << numNewEntries << ") than space (" << numEntries << ") -> Skipping everything else";
+ return;
+ }
//consume all
const std::size_t num = writePosition - onePastLoggingReadPosition;
for (
@@ -94,6 +99,7 @@ namespace armarx
++onePastLoggingReadPosition, ++offset
)
{
+ ARMARX_TRACE;
auto& entry = entries.at(toBounds(onePastLoggingReadPosition));
consumer(entry, offset, num);
entry.messages.reset(messageBufferSize, messageBufferEntries, entry.iteration);
@@ -109,8 +115,13 @@ namespace armarx
<< numNewEntries << " /" << newNumNewEntries << " / " << numEntries;
}
numNewEntries = newNumNewEntries;
- ARMARX_CHECK_EXPRESSION(numNewEntries < numEntries);
+ if (numNewEntries >= numEntries)
+ {
+ ARMARX_ERROR << " more new entries (" << numNewEntries << ") than space (" << numEntries << ") -> Skipping everything else";
+ return;
+ }
}
+
}
std::size_t ControlThreadOutputBuffer::initialize(std::size_t numEntries,
diff --git a/source/RobotAPI/gui-plugins/ArMemMemoryViewer/ArMemMemoryViewerWidget.ui b/source/RobotAPI/gui-plugins/ArMemMemoryViewer/ArMemMemoryViewerWidget.ui
index a92856396a36ed42467a1f0d11c37838a7aece7e..0eb55660336c5ec4d72f869ff0faa3cadb8ff2ca 100644
--- a/source/RobotAPI/gui-plugins/ArMemMemoryViewer/ArMemMemoryViewerWidget.ui
+++ b/source/RobotAPI/gui-plugins/ArMemMemoryViewer/ArMemMemoryViewerWidget.ui
@@ -460,7 +460,7 @@
<property name="title">
<string>Entity Instance (click on an instance on the left)</string>
</property>
- <layout class="QHBoxLayout" name="horizontalLayout_4">
+ <layout class="QVBoxLayout" name="verticalLayout_5">
<property name="leftMargin">
<number>0</number>
</property>
@@ -490,8 +490,50 @@
<string>Value</string>
</property>
</column>
+ <item>
+ <property name="text">
+ <string>id</string>
+ </property>
+ </item>
+ <item>
+ <property name="text">
+ <string>data</string>
+ </property>
+ </item>
+ <item>
+ <property name="text">
+ <string>metadata</string>
+ </property>
+ </item>
</widget>
</item>
+ <item>
+ <layout class="QHBoxLayout" name="horizontalLayout_2">
+ <item>
+ <spacer name="horizontalSpacer_2">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>40</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item>
+ <widget class="QCheckBox" name="instanceUseTypeInfoCheckBox">
+ <property name="text">
+ <string>Use Type Info</string>
+ </property>
+ <property name="checked">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
</layout>
</widget>
</item>
diff --git a/source/RobotAPI/gui-plugins/ArMemMemoryViewer/ArMemMemoryViewerWidgetController.cpp b/source/RobotAPI/gui-plugins/ArMemMemoryViewer/ArMemMemoryViewerWidgetController.cpp
index 28ddc277f4cc817f8c22cbf51bca8e5d7d250ba8..ddef1da66e7c6cd6195b8437d2c932a26250f9cc 100644
--- a/source/RobotAPI/gui-plugins/ArMemMemoryViewer/ArMemMemoryViewerWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/ArMemMemoryViewer/ArMemMemoryViewerWidgetController.cpp
@@ -28,10 +28,9 @@
#include <QTimer>
#include <ArmarXCore/observers/variant/Variant.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
#include <RobotAPI/libraries/armem/core/ice_conversions.h>
-#include <RobotAPI/libraries/armem_gui/QTreeWidgetAronDataVisitor.h>
-
#include <RobotAPI/libraries/armem_gui/gui_utils.h>
@@ -50,6 +49,38 @@ namespace armarx
}
}
+ static QSplitter* useSplitter(QLayout* layout)
+ {
+ ARMARX_CHECK(layout);
+
+ QSplitter* splitter;
+ if (dynamic_cast<QHBoxLayout*>(layout))
+ {
+ splitter = new QSplitter(Qt::Orientation::Horizontal);
+ }
+ else if (dynamic_cast<QVBoxLayout*>(layout))
+ {
+ splitter = new QSplitter(Qt::Orientation::Vertical);
+ }
+ else
+ {
+ splitter = new QSplitter();
+ }
+
+ while (layout->count() > 0)
+ {
+ int index = 0;
+ if (layout->itemAt(index)->widget())
+ {
+ QLayoutItem* item = layout->takeAt(index);
+ splitter->addWidget(item->widget());
+ }
+ }
+ layout->addWidget(splitter);
+
+ return splitter;
+ }
+
QString ArMemMemoryViewerWidgetController::GetWidgetName()
{
@@ -62,20 +93,6 @@ namespace armarx
widget.setupUi(getWidget());
// Setup widgets
- {
- QSplitter* splitter = new QSplitter();
- while (widget.treesLayout->count() > 0)
- {
- int index = 0; //widget.treesLayout->count() - 1;
- if (widget.treesLayout->itemAt(index)->widget())
- {
- QLayoutItem* item = widget.treesLayout->takeAt(index);
- splitter->addWidget(item->widget());
- }
- }
- widget.treesLayout->addWidget(splitter);
- }
-
widget.statusLabel->clear();
@@ -89,7 +106,6 @@ namespace armarx
replaceWidget(widget._memoryTreeWidget, memoryTree, widget.memoryGroupBox->layout());
ARMARX_CHECK_NULL(widget._memoryTreeWidget);
-
queryTabs = new armem::gui::QueryWidget();
widget._queryTabWidget = nullptr;
@@ -103,6 +119,10 @@ namespace armarx
ARMARX_CHECK_NULL(widget._instanceTree);
+ useSplitter(widget.treesLayout);
+ useSplitter(widget.memoryGroupBox->layout());
+
+
// Connect signals
connect(this, &This::connected, this, &This::updateMemory);
connect(widget.updateButton, &QPushButton::pressed, this, &This::updateMemory);
@@ -113,7 +133,10 @@ namespace armarx
connect(widget.updateFrequencySpinBox, &QDoubleSpinBox::editingFinished, this, &This::updateTimerFrequency);
connect(memoryTree, &armem::gui::MemoryTreeWidget::updated, this, &This::memoryTreeUpdated);
- connect(memoryTree, &armem::gui::MemoryTreeWidget::instanceSelected, this, &This::updateInstanceTree);
+ connect(memoryTree, &armem::gui::MemoryTreeWidget::updated, this, &This::updateInstanceTree);
+ connect(memoryTree, &armem::gui::MemoryTreeWidget::itemSelected, this, &This::updateInstanceTree);
+
+ connect(widget.instanceUseTypeInfoCheckBox, &QCheckBox::toggled, instanceTree, &armem::gui::InstanceTreeWidget::setUseTypeInfo);
connect(instanceTree, &armem::gui::InstanceTreeWidget::updated, this, &This::instanceTreeUpdated);
}
@@ -188,7 +211,7 @@ namespace armarx
return;
}
- armem::Time start = armem::Time::now();
+ TIMING_START(MemoryQuery);
{
armem::client::QueryInput input = queryTabs->queryInput();
armem::client::QueryResult result = memoryReader.query(input);
@@ -201,11 +224,11 @@ namespace armarx
widget.statusLabel->setText(QString::fromStdString(result.errorMessage));
}
}
- ARMARX_VERBOSE << "Memory query took " << (armem::Time::now() - start).toMilliSecondsDouble() << " ms.";
+ TIMING_END_STREAM(MemoryQuery, ARMARX_VERBOSE);
if (debugObserver)
{
- debugObserver->setDebugDatafield(debugObserverChannelName, "Memory Query [ms]", new Variant((armem::Time::now() - start).toMilliSecondsDouble()));
+ debugObserver->setDebugDatafield(debugObserverChannelName, "Memory Query [ms]", new Variant(MemoryQuery.toMilliSecondsDouble()));
}
if (this->memoryData)
@@ -218,24 +241,49 @@ namespace armarx
}
}
- void ArMemMemoryViewerWidgetController::updateInstanceTree(const armem::MemoryID& id)
+ void ArMemMemoryViewerWidgetController::updateInstanceTree()
{
- if (memoryData)
+ if (memoryData && memoryTree->selectedID())
{
- instanceTree->update(id, *memoryData);
+ armem::MemoryID id = *memoryTree->selectedID();
+
+ if (!id.hasEntityName())
+ {
+ return;
+ }
+ const armem::EntitySnapshot* snapshot = nullptr;
+ if (!id.hasTimestamp())
+ {
+ snapshot = &memoryData->getEntity(id).getLatestSnapshot();
+ id.timestamp = snapshot->time();
+ }
+ if (!id.hasInstanceIndex())
+ {
+ if (!snapshot)
+ {
+ snapshot = &memoryData->getEntitySnapshot(id);
+ }
+ if (snapshot->size() > 0)
+ {
+ id.instanceIndex = 0;
+ }
+ }
+ if (id.hasInstanceIndex())
+ {
+ instanceTree->update(id, *memoryData);
+ }
}
}
void ArMemMemoryViewerWidgetController::updateMemoryTree(armem::Memory& memory)
{
- armem::Time start = armem::Time::now();
-
+ TIMING_START(GuiUpdate);
memoryTree->update(memory);
+ TIMING_END_STREAM(GuiUpdate, ARMARX_VERBOSE);
- ARMARX_VERBOSE << "GUI update took " << (armem::Time::now() - start).toMilliSecondsDouble() << " ms.";
if (debugObserver)
{
- debugObserver->setDebugDatafield(debugObserverChannelName, "GUI Update [ms]", new Variant((armem::Time::now() - start).toMilliSecondsDouble()));
+ debugObserver->setDebugDatafield(debugObserverChannelName, "GUI Update [ms]", new Variant(GuiUpdate.toMilliSecondsDouble()));
}
}
diff --git a/source/RobotAPI/gui-plugins/ArMemMemoryViewer/ArMemMemoryViewerWidgetController.h b/source/RobotAPI/gui-plugins/ArMemMemoryViewer/ArMemMemoryViewerWidgetController.h
index 48f0c0b6bf8ff00a99ca0e9adfadddea7ccbd9ef..973a3e24e18d5633fd32a008ea69fd681d06377a 100644
--- a/source/RobotAPI/gui-plugins/ArMemMemoryViewer/ArMemMemoryViewerWidgetController.h
+++ b/source/RobotAPI/gui-plugins/ArMemMemoryViewer/ArMemMemoryViewerWidgetController.h
@@ -99,7 +99,7 @@ namespace armarx
void updateTimerFrequency();
void toggleTimer(bool start);
void updateMemory();
- void updateInstanceTree(const armem::MemoryID& id);
+ void updateInstanceTree();
signals:
diff --git a/source/RobotAPI/gui-plugins/CMakeLists.txt b/source/RobotAPI/gui-plugins/CMakeLists.txt
index d215b7276e4d6eaf4c420493466e54630d1faff7..48c9062175974a656810cb82721bdca203b2769c 100644
--- a/source/RobotAPI/gui-plugins/CMakeLists.txt
+++ b/source/RobotAPI/gui-plugins/CMakeLists.txt
@@ -27,3 +27,5 @@ add_subdirectory(GraspCandidateViewer)
add_subdirectory(BoxToGraspCandidates)
+
+add_subdirectory(DebugRobotUnitDataStreaming)
\ No newline at end of file
diff --git a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidget.ui b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidget.ui
index 85819912107bc3006fb94cec8a71428f57e0481f..fef69f4366e204f0a27fa3857e0221d7d7dba2bb 100644
--- a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidget.ui
+++ b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidget.ui
@@ -135,6 +135,13 @@
</property>
</spacer>
</item>
+ <item>
+ <widget class="QPushButton" name="pushButtonCopyCurrentPose">
+ <property name="text">
+ <string>copy current pose</string>
+ </property>
+ </widget>
+ </item>
<item>
<widget class="QLabel" name="labelParsingSuccess">
<property name="text">
@@ -524,6 +531,16 @@
</item>
</layout>
</item>
+ <item row="7" column="2" colspan="6">
+ <widget class="QCheckBox" name="checkBoxSkipWaypoints">
+ <property name="text">
+ <string>Skip waypoints</string>
+ </property>
+ <property name="checked">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
</layout>
</widget>
</item>
diff --git a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.cpp b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.cpp
index 7f245db4be10b20612849486bb388071c1504875..303aa4ecdeabc7030e74b432858cb9c063b7f5bc 100644
--- a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.cpp
@@ -23,6 +23,8 @@
#include <random>
#include <string>
+#include <QClipboard>
+
#include <SimoxUtility/json.h>
#include <ArmarXCore/util/CPPUtility/container.h>
@@ -38,13 +40,14 @@ namespace armarx
using T = CartesianWaypointControlGuiWidgetController;
std::lock_guard g{_allMutex};
_ui.setupUi(getWidget());
- connect(_ui.radioButtonWPJson, &QPushButton::clicked, this, &T::triggerParsing);
- connect(_ui.radioButton4f, &QPushButton::clicked, this, &T::triggerParsing);
- connect(_ui.textEditWPs, &QTextEdit::textChanged, this, &T::triggerParsing);
+ connect(_ui.radioButtonWPJson, &QPushButton::clicked, this, &T::triggerParsing);
+ connect(_ui.radioButton4f, &QPushButton::clicked, this, &T::triggerParsing);
+ connect(_ui.textEditWPs, &QTextEdit::textChanged, this, &T::triggerParsing);
- connect(_ui.pushButtonExecute, &QPushButton::clicked, this, &T::on_pushButtonExecute_clicked);
- connect(_ui.pushButtonZeroFT, &QPushButton::clicked, this, &T::on_pushButtonZeroFT_clicked);
- connect(_ui.pushButtonSendSettings, &QPushButton::clicked, this, &T::on_pushButtonSendSettings_clicked);
+ connect(_ui.pushButtonExecute, &QPushButton::clicked, this, &T::on_pushButtonExecute_clicked);
+ connect(_ui.pushButtonZeroFT, &QPushButton::clicked, this, &T::on_pushButtonZeroFT_clicked);
+ connect(_ui.pushButtonSendSettings, &QPushButton::clicked, this, &T::on_pushButtonSendSettings_clicked);
+ connect(_ui.pushButtonCopyCurrentPose, &QPushButton::clicked, this, &T::copyCurrentPose);
connectCreateAcivateDeactivateDelete(
_ui.pushButtonCreateController,
@@ -88,6 +91,28 @@ namespace armarx
}
}
+ void CartesianWaypointControlGuiWidgetController::copyCurrentPose()
+ {
+ std::lock_guard g{_allMutex};
+ if (!_robot)
+ {
+ return;
+ }
+ synchronizeLocalClone(_robot);
+ const auto rns = _robot->getRobotNodeSet(_ui.comboBoxChain->currentText().toStdString());
+ if (!rns)
+ {
+ return;
+ }
+ const auto tcp = rns->getTCP();
+ if (!tcp)
+ {
+ return;
+ }
+ const auto str = simox::json::eigen4f2posquatJson(tcp->getPoseInRootFrame());
+ QApplication::clipboard()->setText(QString::fromStdString(str));
+ }
+
void CartesianWaypointControlGuiWidgetController::on_pushButtonZeroFT_clicked()
{
std::lock_guard g{_allMutex};
@@ -148,26 +173,27 @@ namespace armarx
std::lock_guard g{_allMutex};
NJointCartesianWaypointControllerRuntimeConfig cfg;
- cfg.wpCfg.maxPositionAcceleration = static_cast<float>(_ui.doubleSpinBoxMaxAccPos->value());
- cfg.wpCfg.maxOrientationAcceleration = static_cast<float>(_ui.doubleSpinBoxMaxAccOri->value());
- cfg.wpCfg.maxNullspaceAcceleration = static_cast<float>(_ui.doubleSpinBoxMaxAccNull->value());
+ cfg.wpCfg.maxPositionAcceleration = static_cast<float>(_ui.doubleSpinBoxMaxAccPos->value());
+ cfg.wpCfg.maxOrientationAcceleration = static_cast<float>(_ui.doubleSpinBoxMaxAccOri->value());
+ cfg.wpCfg.maxNullspaceAcceleration = static_cast<float>(_ui.doubleSpinBoxMaxAccNull->value());
- cfg.wpCfg.kpJointLimitAvoidance = static_cast<float>(_ui.doubleSpinBoxLimitAvoidKP->value());
- cfg.wpCfg.jointLimitAvoidanceScale = static_cast<float>(_ui.doubleSpinBoxLimitAvoidScale->value());
+ cfg.wpCfg.kpJointLimitAvoidance = static_cast<float>(_ui.doubleSpinBoxLimitAvoidKP->value());
+ cfg.wpCfg.jointLimitAvoidanceScale = static_cast<float>(_ui.doubleSpinBoxLimitAvoidScale->value());
- cfg.wpCfg.thresholdOrientationNear = static_cast<float>(_ui.doubleSpinBoxOriNear->value());
- cfg.wpCfg.thresholdOrientationReached = static_cast<float>(_ui.doubleSpinBoxOriReached->value());
- cfg.wpCfg.thresholdPositionNear = static_cast<float>(_ui.doubleSpinBoxPosNear->value());
- cfg.wpCfg.thresholdPositionReached = static_cast<float>(_ui.doubleSpinBoxPosReached->value());
+ cfg.wpCfg.thresholdOrientationNear = static_cast<float>(_ui.doubleSpinBoxOriNear->value());
+ cfg.wpCfg.thresholdOrientationReached = static_cast<float>(_ui.doubleSpinBoxOriReached->value());
+ cfg.wpCfg.thresholdPositionNear = static_cast<float>(_ui.doubleSpinBoxPosNear->value());
+ cfg.wpCfg.thresholdPositionReached = static_cast<float>(_ui.doubleSpinBoxPosReached->value());
- cfg.wpCfg.maxOriVel = static_cast<float>(_ui.doubleSpinBoxOriMaxVel->value());
- cfg.wpCfg.maxPosVel = static_cast<float>(_ui.doubleSpinBoxPosMaxVel->value());
- cfg.wpCfg.kpOri = static_cast<float>(_ui.doubleSpinBoxOriKP->value());
- cfg.wpCfg.kpPos = static_cast<float>(_ui.doubleSpinBoxPosKP->value());
+ cfg.wpCfg.maxOriVel = static_cast<float>(_ui.doubleSpinBoxOriMaxVel->value());
+ cfg.wpCfg.maxPosVel = static_cast<float>(_ui.doubleSpinBoxPosMaxVel->value());
+ cfg.wpCfg.kpOri = static_cast<float>(_ui.doubleSpinBoxOriKP->value());
+ cfg.wpCfg.kpPos = static_cast<float>(_ui.doubleSpinBoxPosKP->value());
- cfg.forceThreshold = static_cast<float>(_ui.doubleSpinBoxFTLimit->value());
+ cfg.forceThreshold = static_cast<float>(_ui.doubleSpinBoxFTLimit->value());
cfg.optimizeNullspaceIfTargetWasReached = _ui.checkBoxKeepOptimizing->isChecked();
cfg.forceThresholdInRobotRootZ = _ui.checkBoxLimitinZ->isChecked();
+ cfg.skipToClosestWaypoint = _ui.checkBoxSkipWaypoints->isChecked();
return cfg;
}
diff --git a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.h b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.h
index f5358b49ea1dfbb097abd2d9432b403ee823f46c..01c59683c8dd3081e50ac58aa180d9c1a1d7ced0 100644
--- a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.h
+++ b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.h
@@ -76,6 +76,7 @@ namespace armarx
void on_pushButtonZeroFT_clicked();
void on_pushButtonSendSettings_clicked();
void createController() override;
+ void copyCurrentPose();
void triggerParsing();
diff --git a/source/RobotAPI/gui-plugins/DebugRobotUnitDataStreaming/CMakeLists.txt b/source/RobotAPI/gui-plugins/DebugRobotUnitDataStreaming/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..04cb7202b49e8c7f215334bd07e001a50f5e819b
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/DebugRobotUnitDataStreaming/CMakeLists.txt
@@ -0,0 +1,17 @@
+set(LIB_NAME "DebugRobotUnitDataStreamingGuiPlugin")
+armarx_set_target("${LIB_NAME}")
+
+armarx_build_if(ArmarXGui_FOUND "ArmarXGui not available")
+
+# do not rename this variable, it is used in armarx_gui_library()...
+set(SOURCES DebugRobotUnitDataStreamingWidgetController.cpp)
+set(HEADERS DebugRobotUnitDataStreamingWidgetController.h)
+set(GUI_UIS DebugRobotUnitDataStreamingWidget.ui)
+
+set(COMPONENT_LIBS
+ RobotAPIComponentPlugins
+ SimpleConfigDialog)
+
+if(ArmarXGui_FOUND)
+ armarx_gui_plugin("${LIB_NAME}" "${SOURCES}" "" "${GUI_UIS}" "" "${COMPONENT_LIBS}")
+endif()
diff --git a/source/RobotAPI/gui-plugins/DebugRobotUnitDataStreaming/DebugRobotUnitDataStreamingWidget.ui b/source/RobotAPI/gui-plugins/DebugRobotUnitDataStreaming/DebugRobotUnitDataStreamingWidget.ui
new file mode 100644
index 0000000000000000000000000000000000000000..cbf9340e13c887af2e06fc39c6621698b5a09560
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/DebugRobotUnitDataStreaming/DebugRobotUnitDataStreamingWidget.ui
@@ -0,0 +1,78 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>DebugRobotUnitDataStreamingWidget</class>
+ <widget class="QWidget" name="DebugRobotUnitDataStreamingWidget">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>400</width>
+ <height>300</height>
+ </rect>
+ </property>
+ <property name="windowTitle">
+ <string>DebugRobotUnitDataStreamingWidget</string>
+ </property>
+ <layout class="QGridLayout" name="gridLayout">
+ <item row="0" column="1">
+ <widget class="QSpinBox" name="spinBoxNumberOfStreams">
+ <property name="maximum">
+ <number>100</number>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="0">
+ <spacer name="verticalSpacer">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>20</width>
+ <height>247</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item row="0" column="0">
+ <widget class="QLabel" name="label">
+ <property name="text">
+ <string>number of streams</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="4">
+ <spacer name="horizontalSpacer">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>185</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item row="0" column="2">
+ <widget class="QCheckBox" name="checkBoxStreamSens">
+ <property name="text">
+ <string>Sens</string>
+ </property>
+ <property name="checked">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="3">
+ <widget class="QCheckBox" name="checkBoxStreamCtrl">
+ <property name="text">
+ <string>Ctrl</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>
diff --git a/source/RobotAPI/gui-plugins/DebugRobotUnitDataStreaming/DebugRobotUnitDataStreamingWidgetController.cpp b/source/RobotAPI/gui-plugins/DebugRobotUnitDataStreaming/DebugRobotUnitDataStreamingWidgetController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..18c9dc655ed8af90cb5f557d0f2ac6fb09c12643
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/DebugRobotUnitDataStreaming/DebugRobotUnitDataStreamingWidgetController.cpp
@@ -0,0 +1,100 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * \package RobotAPI::gui-plugins::DebugRobotUnitDataStreamingWidgetController
+ * \author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * \date 2021
+ * \copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "DebugRobotUnitDataStreamingWidgetController.h"
+
+#include <string>
+
+namespace armarx
+{
+
+ void DebugRobotUnitDataStreamingWidgetController::loadSettings(QSettings* settings)
+ {
+ getRobotUnitComponentPlugin().setRobotUnitName(
+ settings->value("ru", "Armar6Unit").toString().toStdString());
+ }
+
+ void DebugRobotUnitDataStreamingWidgetController::saveSettings(QSettings* settings)
+ {
+ settings->setValue("ru", QString::fromStdString(getRobotUnitComponentPlugin().getRobotUnitName()));
+ }
+
+ QPointer<QDialog> DebugRobotUnitDataStreamingWidgetController::getConfigDialog(QWidget* parent)
+ {
+ if (!dialog)
+ {
+ dialog = new SimpleConfigDialog(parent);
+ dialog->addProxyFinder<RobotUnitInterfacePrx>("ru", "Robot Unit", "*Unit");
+ }
+ return qobject_cast<SimpleConfigDialog*>(dialog);
+ }
+
+ void DebugRobotUnitDataStreamingWidgetController::configured()
+ {
+ getRobotUnitComponentPlugin().setRobotUnitName(dialog->get("ru"));
+ }
+
+ DebugRobotUnitDataStreamingWidgetController::DebugRobotUnitDataStreamingWidgetController()
+ {
+ widget.setupUi(getWidget());
+ startTimer(10);
+ }
+
+ void DebugRobotUnitDataStreamingWidgetController::onDisconnectComponent()
+ {
+ std::lock_guard f{mutex};
+ rec.clear();
+ }
+
+ void DebugRobotUnitDataStreamingWidgetController::timerEvent(QTimerEvent* event)
+ {
+ const std::size_t n = widget.spinBoxNumberOfStreams->value();
+ if(!getRobotUnit())
+ {
+ return;
+ }
+ if (rec.size() > n)
+ {
+ rec.resize(n);
+ }
+ std::lock_guard f{mutex};
+ RobotUnitDataStreaming::Config cfg;
+ if(widget.checkBoxStreamSens->isChecked())
+ {
+ cfg.loggingNames.emplace_back("sens");
+ }
+ if(widget.checkBoxStreamCtrl->isChecked())
+ {
+ cfg.loggingNames.emplace_back("ctrl");
+ }
+ while (rec.size() < n)
+ {
+ rec.emplace_back(getRobotUnitComponentPlugin()
+ .startDataSatreming(cfg));
+ ARMARX_INFO << rec.back()->getDataDescriptionString();
+ }
+ for (auto& r : rec)
+ {
+ r->getDataBuffer().clear();
+ }
+ }
+}
diff --git a/source/RobotAPI/gui-plugins/DebugRobotUnitDataStreaming/DebugRobotUnitDataStreamingWidgetController.h b/source/RobotAPI/gui-plugins/DebugRobotUnitDataStreaming/DebugRobotUnitDataStreamingWidgetController.h
new file mode 100644
index 0000000000000000000000000000000000000000..436985349fdc03139f6ded9a0a0b6a5327c1fdab
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/DebugRobotUnitDataStreaming/DebugRobotUnitDataStreamingWidgetController.h
@@ -0,0 +1,100 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::gui-plugins::DebugRobotUnitDataStreamingWidgetController
+ * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#pragma once
+
+#include <ArmarXCore/core/system/ImportExportComponent.h>
+
+#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+
+#include <RobotAPI/gui-plugins/DebugRobotUnitDataStreaming/ui_DebugRobotUnitDataStreamingWidget.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h>
+
+namespace armarx
+{
+ /**
+ \page RobotAPI-GuiPlugins-DebugRobotUnitDataStreaming DebugRobotUnitDataStreaming
+ \brief The DebugRobotUnitDataStreaming allows visualizing ...
+
+ \image html DebugRobotUnitDataStreaming.png
+ The user can
+
+ API Documentation \ref DebugRobotUnitDataStreamingWidgetController
+
+ \see DebugRobotUnitDataStreamingGuiPlugin
+ */
+
+
+ /**
+ * \class DebugRobotUnitDataStreamingGuiPlugin
+ * \ingroup ArmarXGuiPlugins
+ * \brief DebugRobotUnitDataStreamingGuiPlugin brief description
+ *
+ * Detailed description
+ */
+
+ /**
+ * \class DebugRobotUnitDataStreamingWidgetController
+ * \brief DebugRobotUnitDataStreamingWidgetController brief one line description
+ *
+ * Detailed description
+ */
+ class ARMARXCOMPONENT_IMPORT_EXPORT
+ DebugRobotUnitDataStreamingWidgetController:
+ public armarx::ArmarXComponentWidgetControllerTemplate < DebugRobotUnitDataStreamingWidgetController >,
+ public virtual RobotUnitComponentPluginUser
+ {
+ Q_OBJECT
+ public:
+ explicit DebugRobotUnitDataStreamingWidgetController();
+
+ void loadSettings(QSettings* settings) override;
+ void saveSettings(QSettings* settings) override;
+ QPointer<QDialog> getConfigDialog(QWidget* parent) override;
+ void configured() override;
+
+ /**
+ * Returns the Widget name displayed in the ArmarXGui to create an
+ * instance of this class.
+ */
+ static QString GetWidgetName()
+ {
+ return "Debugging.DebugRobotUnitDataStreaming";
+ }
+
+ void onInitComponent() override {}
+ void onConnectComponent() override {}
+ void onDisconnectComponent() override;
+ void onExitComponent() override {}
+
+ protected:
+ void timerEvent(QTimerEvent* event) override;
+
+ private:
+ std::mutex mutex;
+ Ui::DebugRobotUnitDataStreamingWidget widget;
+ std::vector<RobotUnitDataStreamingReceiverPtr> rec;
+ QPointer<SimpleConfigDialog> dialog;};
+}
+
+
diff --git a/source/RobotAPI/gui-plugins/FTSensorCalibrationGui/FTSensorCalibrationGuiWidgetController.cpp b/source/RobotAPI/gui-plugins/FTSensorCalibrationGui/FTSensorCalibrationGuiWidgetController.cpp
index 6252b2b9a4af2d0d9a18e32d12d876099d6a80bd..288e480893d5f9865a83781fcde00ed799d7ccf7 100644
--- a/source/RobotAPI/gui-plugins/FTSensorCalibrationGui/FTSensorCalibrationGuiWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/FTSensorCalibrationGui/FTSensorCalibrationGuiWidgetController.cpp
@@ -222,9 +222,9 @@ namespace armarx
{
_logfile << s.iterationId << ';' << s.timestampUSec;
//logging
- RobotUnitDataStreamingReceiver::visitEntries(log, s, _adc);
- RobotUnitDataStreamingReceiver::visitEntries(log, s, _adc_temp);
- RobotUnitDataStreamingReceiver::visitEntries(log, s, _joints);
+ RobotUnitDataStreamingReceiver::VisitEntries(log, s, _adc);
+ RobotUnitDataStreamingReceiver::VisitEntries(log, s, _adc_temp);
+ RobotUnitDataStreamingReceiver::VisitEntries(log, s, _joints);
_logfile << '\n';
}
std::vector<float> vals;
@@ -234,8 +234,8 @@ namespace armarx
{
vals.emplace_back(n);
};
- RobotUnitDataStreamingReceiver::visitEntries(pback, last, _adc);
- RobotUnitDataStreamingReceiver::visitEntries(pback, last, _adc_temp);
+ RobotUnitDataStreamingReceiver::VisitEntries(pback, last, _adc);
+ RobotUnitDataStreamingReceiver::VisitEntries(pback, last, _adc_temp);
}
//calc + compensate
diff --git a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllerClassesWidget.cpp b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllerClassesWidget.cpp
index 7df85b9141364c93721785486e19de74da364655..27cf1dca60f0d527637c9c3901998caf4324eb54 100644
--- a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllerClassesWidget.cpp
+++ b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllerClassesWidget.cpp
@@ -316,7 +316,8 @@ namespace armarx
QPushButton* button = new QPushButton {"Create"};
headerW->layout()->addWidget(button);
button->setToolTip("Create a new controller instance of this class");
- connect(button, SIGNAL(clicked(bool)), this, SLOT(createCtrl()));
+ connect(button, &QPushButton::clicked, this, &NJointControllerClassesWidgetEntry::createCtrl);
+ connect(nameEdit, &QLineEdit::returnPressed, this, &NJointControllerClassesWidgetEntry::createCtrl);
}
//descr
{
diff --git a/source/RobotAPI/interface/core/RobotStateObserverInterface.ice b/source/RobotAPI/interface/core/RobotStateObserverInterface.ice
index 6f0abb5b36e73a1b5bda9550d45d5a3bad9ad5c8..1a6115fbe83d4c534876f27d54113156ba2c8d8f 100644
--- a/source/RobotAPI/interface/core/RobotStateObserverInterface.ice
+++ b/source/RobotAPI/interface/core/RobotStateObserverInterface.ice
@@ -29,7 +29,7 @@
module armarx
{
- interface RobotStateObserverInterface extends ObserverInterface
+ ["amd"] interface RobotStateObserverInterface extends ObserverInterface
{
["cpp:const"]
idempotent DatafieldRefBase getPoseDatafield(string nodeName);
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.ice
index 44cba9dad29c8b6d56d0dd1143f5aa724e8f23f6..712399c5f77ec1a7c116b3e77b0ad2e4df616a87 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.ice
@@ -36,6 +36,7 @@ module armarx
float forceThreshold = -1; // < 0 -> no limit
bool forceThresholdInRobotRootZ = true;
bool optimizeNullspaceIfTargetWasReached = false;
+ bool skipToClosestWaypoint = true;
};
class NJointCartesianWaypointControllerConfig extends NJointControllerConfig
diff --git a/source/RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice b/source/RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice
index c7871f0860fbe343a1704b2022f54a8ed6cd8674..dac07929c87809b5b51552cf79ab07ffe0b51e0d 100644
--- a/source/RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice
@@ -222,7 +222,7 @@ module armarx
interface Receiver
{
- void update(TimeStepSeq data);
+ ["amd"] void update(TimeStepSeq data, long msgSequenceNbr);
};
struct Config
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateProvider.cpp b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateProvider.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..99a80a53febe01e9f91cdedee69e4bbc91a35555
--- /dev/null
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateProvider.cpp
@@ -0,0 +1,103 @@
+
+
+//#include <ArmarXCore/core/application/properties/PluginAll.h>
+//#include <ArmarXGui/libraries/RemoteGui/ConfigIntrospection.h>
+
+
+#include "GraspCandidateProvider.h"
+
+////boilerplate (properties / cycle)
+//namespace arches
+//{
+// std::string GraspCandidateCollisionFilter::getDefaultName() const
+// {
+// return "GraspCandidateCollisionFilter";
+// }
+
+// armarx::PropertyDefinitionsPtr
+// GraspCandidateCollisionFilter::createPropertyDefinitions()
+// {
+// armarx::PropertyDefinitionsPtr ptr =
+// new visionx::PointCloudProcessorPropertyDefinitions(getConfigIdentifier());
+// ptr->optional(_cfg_buf.getWriteBuffer(), "", "");
+// ptr->optional(_provider.ref_frame_name_default,
+// "ReferenceFrame",
+// "ReferenceFrame (if not set, autodetection is performed)");
+// ptr->optional(_upstream_gc_provider_csv,
+// "UpstreamGraspCandidateProviderCSV",
+// "Upstream Provider(s) (if empty all)");
+// ptr->topic(_gc_topic,
+// "GraspCandidatesTopicName",
+// "GraspCandidatesTopic",
+// "Name of the Grasp Candidate Topic");
+// ptr->topic(_service_request_topic,
+// "ServiceRequestsTopicName",
+// "ServiceRequests",
+// "Name of the ServiceRequestsTopic");
+// return ptr;
+// }
+
+// void GraspCandidateCollisionFilter::onInitPointCloudProcessor()
+// {
+// ARMARX_TRACE;
+// ARMARX_INFO << "init...";
+// setDebugObserverBatchModeEnabled(true);
+// getProperty(_provider.name, "ProviderName");
+// ARMARX_INFO << "init...done!";
+// }
+// void GraspCandidateCollisionFilter::onConnectPointCloudProcessor()
+// {
+// ARMARX_TRACE;
+// ARMARX_INFO << "connect...";
+// ARMARX_INFO << "prov frame";
+// {
+// ARMARX_TRACE;
+// _provider.info = getPointCloudProvider(_provider.name, true);
+// _provider.proxy = _provider.info.proxy;
+// ARMARX_CHECK_NOT_NULL(_provider.proxy) << VAROUT(_provider.name);
+// _provider.ref_frame_name_used =
+// _provider.ref_frame_name_default.empty() ?
+// getPointCloudFrame(_provider.name, _provider.ref_frame_name_default) :
+// _provider.ref_frame_name_default;
+// ARMARX_CHECK_EXPRESSION(!_provider.ref_frame_name_used.empty())
+// << VAROUT(_provider.name) << " default = "
+// << VAROUT(_provider.ref_frame_name_default);
+// ARMARX_INFO << "Provider '" << _provider.name
+// << "' has ref frame '" << _provider.ref_frame_name_used << "'";
+// }
+// ARMARX_INFO << "robot";
+// _robot = addRobot("grounding", VirtualRobot::RobotIO::eStructure);
+// ARMARX_INFO << "gui";
+// createOrUpdateRemoteGuiTab(_cfg_buf);
+// ARMARX_INFO << "configure gc observer";
+// getGraspCandidateObserverComponentPlugin().setUpstreamGraspCandidateProvidersFromCSV(_upstream_gc_provider_csv);
+// ARMARX_INFO << "configure colision checker!";
+// // _gc_col_checker.reset(getRobotNameHelper(), _robot);
+// ARMARX_INFO << "start reporting gc config!";
+// startPeriodicTask("reportConfigTask", [&]()
+// {
+// _gc_topic->reportProviderInfo(getName(), getProviderInfo());
+// }, 1000, false);
+// ARMARX_INFO << "connect...done!";
+// }
+
+// void GraspCandidateCollisionFilter::requestService(
+// const std::string& providerName,
+// Ice::Int relativeTimeoutMS,
+// const Ice::Current&)
+// {
+// if (providerName == getName() || providerName == "AllGraspProviders")
+// {
+// ///TODO do not ignore this!
+// _service_request_topic->requestService(getProperty<std::string>("ServiceRequestsTopicName").getValue(), relativeTimeoutMS);
+// }
+// }
+// armarx::grasping::ProviderInfoPtr GraspCandidateCollisionFilter::getProviderInfo() const
+// {
+// armarx::grasping::ProviderInfoPtr info = new armarx::grasping::ProviderInfo();
+// info->objectType = armarx::grasping::AnyObject;
+// return info;
+// }
+//}
+
+
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateProvider.h b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateProvider.h
new file mode 100644
index 0000000000000000000000000000000000000000..7b2095c08364c4d0a5cd870f908ff6e45d38e6b9
--- /dev/null
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateProvider.h
@@ -0,0 +1,64 @@
+#pragma once
+
+////plugins
+//#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
+//#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/RemoteGuiComponentPlugin.h>
+//#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+//#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
+//#include <RobotAPI/libraries/RobotAPIComponentPlugins/GraspCandidateObserverComponentPlugin.h>
+
+////other
+//#include <ArmarXCore/util/CPPUtility/ConfigIntrospection/create_macro.h>
+//#include <RobotAPI/interface/observers/GraspCandidateObserverInterface.h>
+
+namespace armarx
+{
+ // class GraspCandidateCollisionFilter :
+ // virtual public visionx::PointCloudProcessor,
+ // virtual public armarx::RemoteGuiComponentPluginUser,
+ // virtual public armarx::DebugObserverComponentPluginUser,
+ // virtual public armarx::ArVizComponentPluginUser,
+ // virtual public armarx::RobotStateComponentPluginUser,
+ // virtual public armarx::GraspCandidateObserverComponentPluginUser,
+ // public arches::time_util<std::chrono::high_resolution_clock>
+ // {
+ // public:
+ // using clock_t = std::chrono::high_resolution_clock;
+ // using time_point_t = typename clock_t::time_point;
+ // using config_t = arches::cfg::GCColFilter::config;
+
+ // std::string getDefaultName() const override;
+
+ // void requestService(const std::string& providerName, Ice::Int relativeTimeoutMS, const Ice::Current&);
+ // armarx::grasping::ProviderInfoPtr getProviderInfo() const;
+ // protected:
+ // void onInitPointCloudProcessor() override;
+ // void onConnectPointCloudProcessor() override;
+ // void onDisconnectPointCloudProcessor() override {}
+ // void onExitPointCloudProcessor() override {}
+ // armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+ // private:
+ // void process() override;
+ // template<class PointT>
+ // void process();
+
+
+ // private:
+ // struct provider_data
+ // {
+ // std::string ref_frame_name_default;
+ // std::string ref_frame_name_used;
+ // std::string name;
+ // visionx::PointCloudProviderInfo info;
+ // visionx::PointCloudProviderInterfacePrx proxy;
+ // };
+ // provider_data _provider;
+ // armarx::WriteBufferedTripleBuffer<config_t> _cfg_buf;
+ // VirtualRobot::RobotPtr _robot;
+ // // grasp_candidate_collision_checker _gc_col_checker;
+ // std::string _upstream_gc_provider_csv;
+ // std::size_t _old_num_visualized;
+ // armarx::grasping::GraspCandidatesTopicInterfacePrx _gc_topic;
+ // armarx::RequestableServiceListenerInterfacePrx _service_request_topic;
+ // };
+}
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt
index 3c9bbe17dda9debf6b63cc753f70cc9685e49fb1..8e61ccd8332b8ef0c8cc4fc891dd2548a68a5682 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt
@@ -8,6 +8,7 @@ set(LIBS
DebugDrawer
diffik
RobotStatechartHelpers
+ RobotUnitDataStreamingReceiver
)
set(LIB_FILES
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp
index e711da48631a7562fbc748ff2ef5d0d26d6fd84c..026024a9fb704181271f09f69ec4b94306a3d295 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp
@@ -212,12 +212,12 @@ namespace armarx::plugins
void RobotStateComponentPlugin::preOnInitComponent()
{
- if (!_robotStateComponent && _robotStateComponentName.empty())
+ if (!_deactivated && !_robotStateComponent && _robotStateComponentName.empty())
{
parent<Component>().getProperty(_robotStateComponentName, makePropertyName(_propertyName));
}
- if (!_robotStateComponent)
+ if (!_deactivated && !_robotStateComponent)
{
parent<Component>().usingProxy(_robotStateComponentName);
}
@@ -225,11 +225,14 @@ namespace armarx::plugins
void RobotStateComponentPlugin::preOnConnectComponent()
{
- if (!_robotStateComponent)
+ if (!_deactivated && !_robotStateComponent)
{
parent<Component>().getProxy(_robotStateComponent, _robotStateComponentName);
}
- _nameHelper = std::make_shared<RobotNameHelper>(_robotStateComponent->getRobotInfo());
+ if (!_deactivated)
+ {
+ _nameHelper = std::make_shared<RobotNameHelper>(_robotStateComponent->getRobotInfo());
+ }
}
void RobotStateComponentPlugin::postOnDisconnectComponent()
@@ -274,6 +277,11 @@ namespace armarx::plugins
return _robotStateComponentName;
}
+ void RobotStateComponentPlugin::deactivate()
+ {
+ _deactivated = true;
+ }
+
Eigen::Matrix4f RobotStateComponentPlugin::transformFromTo(
const std::string& from,
const std::string& to,
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h
index 1f267bd715ac192c2152c2534e3a97ce9b8ff8d1..d60ed34f0964b4dc8592c88b055fb32e7bab4608 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h
@@ -57,6 +57,7 @@ namespace armarx::plugins
void setRobotStateComponentName(const std::string& name);
void setRobotStateComponent(const RobotStateComponentInterfacePrx& rsc);
const std::string& getRobotStateComponentName() const;
+ void deactivate();
//get / add
public:
bool hasRobot(const std::string& id) const;
@@ -153,6 +154,7 @@ namespace armarx::plugins
mutable std::recursive_mutex _robotsMutex;
std::map<std::string, RobotData> _robots;
RobotNameHelperPtr _nameHelper;
+ bool _deactivated = false;
};
}
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp
index 1e94a395814376eb08b20e45f5d043f21fa6dbb4..7c120b3cc4db3aabbf6b184e5d747d3014ae0a61 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp
@@ -2,23 +2,86 @@
namespace armarx::plugins
{
+ RobotUnitDataStreamingReceiverPtr
+ RobotUnitComponentPlugin::startDataSatreming(
+ const RobotUnitDataStreaming::Config& cfg)
+ {
+ //ok to create smart ptr from parent, since ice handels this
+ return make_shared<RobotUnitDataStreamingReceiver>(
+ &parent(), getRobotUnit(), cfg);
+ }
+
+ void RobotUnitComponentPlugin::postOnDisconnectComponent()
+ {
+ if (!_ctrls.empty())
+ {
+ ARMARX_CHECK_NOT_NULL(_robotUnit);
+ std::vector<std::string> cs(_ctrls.begin(), _ctrls.end());
+ _robotUnit->deactivateAndDeleteNJointControllers(cs);
+ }
+ _robotUnit = nullptr;
+ }
+
+ NJointControllerInterfacePrx RobotUnitComponentPlugin::createNJointController(
+ const std::string& className,
+ const std::string& instanceName,
+ const armarx::NJointControllerConfigPtr& config,
+ bool doManageController)
+ {
+ ARMARX_CHECK_NOT_NULL(_robotUnit);
+ ARMARX_INFO << ARMARX_STREAM_PRINTER
+ {
+ out << "creating ";
+ if (doManageController)
+ {
+ out << "and managing ";
+ }
+ out << " controller '" << instanceName
+ << "' of class '" << instanceName << "'";
+ };
+ const auto prx = _robotUnit->createOrReplaceNJointController(className, instanceName, config);
+ ARMARX_CHECK_NOT_NULL(prx);
+ if (doManageController)
+ {
+ manageController(instanceName);
+ }
+ return prx;
+ }
+
+ void RobotUnitComponentPlugin::manageController(const armarx::NJointControllerInterfacePrx& ctrl)
+ {
+ ARMARX_CHECK_NOT_NULL(ctrl);
+ manageController(ctrl->getInstanceName());
+ }
+
+ void RobotUnitComponentPlugin::manageController(const std::string& ctrl)
+ {
+ _ctrls.emplace(ctrl);
+ }
+
void RobotUnitComponentPlugin::preOnInitComponent()
{
- if (_robotUnitName.empty())
+ if (!_deactivated)
{
- parent<Component>().getProperty(_robotUnitName, PROPERTY_NAME);
+ if (_robotUnitName.empty())
+ {
+ parent<Component>().getProperty(_robotUnitName, PROPERTY_NAME);
+ }
+ parent<Component>().usingProxy(_robotUnitName);
}
- parent<Component>().usingProxy(_robotUnitName);
}
void RobotUnitComponentPlugin::preOnConnectComponent()
{
- parent<Component>().getProxy(_robotUnit, _robotUnitName);
+ if (!_deactivated)
+ {
+ parent<Component>().getProxy(_robotUnit, _robotUnitName);
+ }
}
void RobotUnitComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
{
- if (!properties->hasDefinition(PROPERTY_NAME))
+ if (!_deactivated && !properties->hasDefinition(PROPERTY_NAME))
{
properties->defineRequiredProperty<std::string>(
PROPERTY_NAME,
@@ -33,14 +96,22 @@ namespace armarx::plugins
void RobotUnitComponentPlugin::setRobotUnitName(const std::string& name)
{
- ARMARX_CHECK_NOT_EMPTY(name);
- ARMARX_CHECK_EMPTY(_robotUnitName);
- _robotUnitName = name;
+ if (!_deactivated)
+ {
+ ARMARX_CHECK_NOT_EMPTY(name);
+ ARMARX_CHECK_EMPTY(_robotUnitName);
+ _robotUnitName = name;
+ }
}
const std::string& RobotUnitComponentPlugin::getRobotUnitName() const
{
return _robotUnitName;
}
+
+ void RobotUnitComponentPlugin::deactivate()
+ {
+ _deactivated = true;
+ }
}
namespace armarx
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h
index b3cbf9a3b1e43d262370fc3b6082d07612a9c7cc..18616020e0683a70e6fcb0f95c7e7a09939db357 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h
@@ -2,6 +2,7 @@
#include <ArmarXCore/core/Component.h>
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
+#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
namespace armarx
@@ -17,6 +18,7 @@ namespace armarx
void preOnInitComponent() override;
void preOnConnectComponent() override;
+ void postOnDisconnectComponent() override;
void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
@@ -24,12 +26,46 @@ namespace armarx
void setRobotUnitName(const std::string& name);
const std::string& getRobotUnitName() const;
+
+ void deactivate();
+
+ //controllers
+ public:
+ template<class PrxT>
+ PrxT createNJointController(const std::string& className,
+ const std::string& instanceName,
+ const NJointControllerConfigPtr& config,
+ bool doManageController = true)
+ {
+ return PrxT::checkedCast(createNJointController(className, instanceName, config, doManageController));
+ }
+ template<class PrxT>
+ void createNJointController(PrxT& prx,
+ const std::string& className,
+ const std::string& instanceName,
+ const NJointControllerConfigPtr& config,
+ bool doManageController = true)
+ {
+ prx = PrxT::checkedCast(createNJointController(className, instanceName, config, doManageController));
+ }
+ NJointControllerInterfacePrx createNJointController(const std::string& className,
+ const std::string& instanceName,
+ const NJointControllerConfigPtr& config,
+ bool doManageController = true);
+ void manageController(const NJointControllerInterfacePrx& ctrl);
+ void manageController(const std::string& ctrl);
+
+ //datastreaming
+ public:
+ RobotUnitDataStreamingReceiverPtr startDataSatreming(const RobotUnitDataStreaming::Config& cfg);
+
private:
static constexpr const char* PROPERTY_NAME = "RobotUnitName";
RobotUnitInterfacePrx _robotUnit;
std::string _robotUnitName;
+ bool _deactivated = false;
+ std::set<std::string> _ctrls;
};
-
}
}
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.cpp
index 27525c42d19079f8fbd3db620f03ad1710f09897..03ed255120ccf8a5084ea824f7d8e2d790c1c485 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.cpp
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.cpp
@@ -89,3 +89,10 @@ const
return target_dist < m_pos_reached_threshold and
std::fabs(target_angular_dist) < m_ori_reached_threshold;
}
+
+
+void
+armarx::ObstacleAvoidingPlatformUnitHelper::setMaxVelocities(float max_vel, float max_angular_vel)
+{
+ m_platform_unit->setMaxVelocities(max_vel, max_angular_vel);
+}
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.h
index 96bd461e7f6194de38283839eaaff0d5ec135967..12d7100f997eeb818f6cf0aff989e42a92059df5 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.h
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.h
@@ -61,6 +61,9 @@ namespace armarx
isFinalTargetReached()
const;
+ void
+ setMaxVelocities(float max_vel, float max_angular_vel);
+
private:
struct target
diff --git a/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.cpp b/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.cpp
index 2846da675f26f263aa244715205b490d78d4803b..10a524cb71f64e12add7d65870139b1c44707ea5 100644
--- a/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.cpp
+++ b/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.cpp
@@ -37,21 +37,37 @@ namespace armarx::detail::RobotUnitDataStreamingReceiver
{
return "RobotUnitDataStreamingReceiver";
}
+ ~Receiver()
+ {
+ std::lock_guard g{_data_mutex};
+ }
void onInitComponent() override {}
void onConnectComponent() override {}
void onExitComponent() override {}
- void update(const RobotUnitDataStreaming::TimeStepSeq& data, const Ice::Current&) override
+ void update_async(
+ const RobotUnitDataStreaming::AMD_Receiver_updatePtr& ptr,
+ const RobotUnitDataStreaming::TimeStepSeq& data,
+ Ice::Long msgSequenceNbr,
+ const Ice::Current&) override
{
+ ptr->ice_response();
+ if (_discard_data)
+ {
+ return;
+ }
+ static_assert(sizeof(std::uint64_t) == sizeof(msgSequenceNbr));
+ const auto seq = static_cast<std::uint64_t>(msgSequenceNbr);
std::lock_guard g{_data_mutex};
ARMARX_INFO << deactivateSpam()
<< "received " << data.size() << " timesteps";
- _data.emplace_back(data);
+ _data[seq] = data;
}
- std::mutex _data_mutex;
- std::deque<RobotUnitDataStreaming::TimeStepSeq> _data;
- Ice::Identity _identity;
+ std::atomic_bool _discard_data = false;
+ std::mutex _data_mutex;
+ std::map<std::uint64_t, RobotUnitDataStreaming::TimeStepSeq> _data;
+ Ice::Identity _identity;
};
}
@@ -82,12 +98,26 @@ namespace armarx
{
if (_proxy)
{
+ _receiver->_discard_data = true;
if (!_description.entries.empty())
{
- _ru->stopDataStreaming(_proxy);
+ try
+ {
+ _ru->stopDataStreaming(_proxy);
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << "did not stop streaming since the network call failed";
+ }
}
+ auto icemanager = _obj->getArmarXManager()->getIceManager();
auto adapter = _obj->getArmarXManager()->getAdapter();
adapter->remove(_receiver->_identity);
+
+ while (icemanager->isObjectReachable(_receiver->_identity.name))
+ {
+ ARMARX_INFO << deactivateSpam() << "waiting until receiver is removed from ice";
+ }
}
_proxy = nullptr;
_receiver = nullptr;
@@ -96,14 +126,47 @@ namespace armarx
std::deque<RobotUnitDataStreaming::TimeStep>& RobotUnitDataStreamingReceiver::getDataBuffer()
{
ARMARX_CHECK_NOT_NULL(_receiver);
- std::deque<RobotUnitDataStreaming::TimeStepSeq> data;
+ std::map<std::uint64_t, RobotUnitDataStreaming::TimeStepSeq> data;
{
std::lock_guard g{_receiver->_data_mutex};
std::swap(data, _receiver->_data);
}
- for (auto& chunk : data)
+ _tmp_data_buffer.merge(data);
+ if (!data.empty())
{
- for (auto& step : chunk)
+ ARMARX_ERROR << "Double message sequence IDs! This should not happen!\nIDs:\n"
+ << ARMARX_STREAM_PRINTER
+ {
+ for (const auto& [key, _] : data)
+ {
+ out << " " << key << "\n";
+ }
+ };
+ }
+
+ auto it = _tmp_data_buffer.begin();
+ for (std::size_t idx = 0; it != _tmp_data_buffer.end(); ++it, ++idx)
+ {
+ if (_last_iteration_id == -1)
+ {
+ _tmp_data_buffer_seq_id = it->first - 1;
+ }
+ if (_tmp_data_buffer_seq_id + 1 != it->first)
+ {
+ if (_tmp_data_buffer.size() > 10 && idx < _tmp_data_buffer.size() - 10)
+ {
+ //there is a lot more data (10 updates) in the buffer!
+ //-> some message calls went missing!
+ ARMARX_ERROR << "some update messages went missing!";
+ }
+ else
+ {
+ //maybe one or two frames are missing (due to async calls) -> wait
+ break;
+ }
+ }
+ _tmp_data_buffer_seq_id = it->first;
+ for (auto& step : it->second)
{
if (
_last_iteration_id != -1 &&
@@ -117,6 +180,7 @@ namespace armarx
_data_buffer.emplace_back(std::move(step));
}
}
+ _tmp_data_buffer.erase(_tmp_data_buffer.begin(), it);
return _data_buffer;
}
@@ -124,4 +188,16 @@ namespace armarx
{
return _description;
}
+
+ std::string RobotUnitDataStreamingReceiver::getDataDescriptionString() const
+ {
+ std::stringstream str;
+ const auto& entr = getDataDescription().entries;
+ str << "Received data (" << entr.size() << " entries):\n";
+ for (const auto& [k, v] : entr)
+ {
+ str << " " << k << ": type " << v.type << " index " << v.index << "\n";
+ }
+ return str.str();
+ }
}
diff --git a/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h b/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h
index 6f52239991ae99fd1e82272f322d8455311e714b..60a03b5c98109b657adddcd48c55731c84cb2f63 100644
--- a/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h
+++ b/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h
@@ -23,6 +23,7 @@
#pragma once
#include <mutex>
+#include <type_traits>
#include <ArmarXCore/core/ManagedIceObject.h>
@@ -53,6 +54,26 @@ namespace armarx
using timestep_t = RobotUnitDataStreaming::TimeStep;
using entry_t = RobotUnitDataStreaming::DataEntry;
+ template<class T>
+ struct DataEntryReader
+ {
+ DataEntryReader() = default;
+ DataEntryReader(DataEntryReader&&) = default;
+ DataEntryReader(const DataEntryReader&) = default;
+ DataEntryReader& operator=(DataEntryReader&&) = default;
+ DataEntryReader& operator=(const DataEntryReader&) = default;
+
+ DataEntryReader(const RobotUnitDataStreaming::DataEntry& k);
+
+ DataEntryReader& operator=(const RobotUnitDataStreaming::DataEntry& k);
+
+ void set(const RobotUnitDataStreaming::DataEntry& k);
+ T get(const RobotUnitDataStreaming::TimeStep& t) const;
+ T operator()(const RobotUnitDataStreaming::TimeStep& t) const;
+ private:
+ RobotUnitDataStreaming::DataEntry _key;
+ };
+
RobotUnitDataStreamingReceiver(
const ManagedIceObjectPtr& obj,
const RobotUnitInterfacePrx& ru,
@@ -61,39 +82,198 @@ namespace armarx
std::deque<timestep_t>& getDataBuffer();
const RobotUnitDataStreaming::DataStreamingDescription& getDataDescription() const;
+ std::string getDataDescriptionString() const;
+
+ template<class T>
+ DataEntryReader<T> getDataEntryReader(const std::string& name) const;
+
+ template<class T>
+ void getDataEntryReader(DataEntryReader<T>& e, const std::string& name) const;
+ //statics
+ static void VisitEntry(auto&& f, const timestep_t& st, const entry_t& e);
+ template<class T>
+ static T GetAs(const timestep_t& st, const entry_t& e);
+
+ template<class T>
+ static RobotUnitDataStreaming::DataEntryType ExpectedDataEntryType();
+ static void VisitEntries(auto&& f, const timestep_t& st, const auto& cont);
+ private:
+ ManagedIceObjectPtr _obj;
+ RobotUnitInterfacePrx _ru;
+ detail::RobotUnitDataStreamingReceiver::ReceiverPtr _receiver;
+ RobotUnitDataStreaming::ReceiverPrx _proxy;
+ RobotUnitDataStreaming::DataStreamingDescription _description;
+ std::map<std::uint64_t, RobotUnitDataStreaming::TimeStepSeq> _tmp_data_buffer;
+ std::uint64_t _tmp_data_buffer_seq_id = 0;
+ std::deque<RobotUnitDataStreaming::TimeStep> _data_buffer;
+ long _last_iteration_id = -1;
+
+ };
+}
+//impl
+namespace armarx
+{
+ template<class T> inline
+ RobotUnitDataStreamingReceiver::DataEntryReader<T>::DataEntryReader(const RobotUnitDataStreaming::DataEntry& k)
+ {
+ set(k);
+ }
+
+ template<class T> inline
+ void
+ RobotUnitDataStreamingReceiver::DataEntryReader<T>::set(const RobotUnitDataStreaming::DataEntry& k)
+ {
+ ARMARX_CHECK_EQUAL(RobotUnitDataStreamingReceiver::ExpectedDataEntryType<T>(),
+ k.type) << "the key references a value of the wrong type!";
+ _key = k;
+ }
- static void visitEntry(auto&& f, const timestep_t& st, const entry_t& e)
+ template<class T> inline
+ RobotUnitDataStreamingReceiver::DataEntryReader<T>&
+ RobotUnitDataStreamingReceiver::DataEntryReader<T>::operator=(const RobotUnitDataStreaming::DataEntry& k)
+ {
+ set(k);
+ return *this;
+ }
+
+ template<class T> inline
+ T
+ RobotUnitDataStreamingReceiver::DataEntryReader<T>::get(const RobotUnitDataStreaming::TimeStep& t) const
+ {
+ return RobotUnitDataStreamingReceiver::GetAs<T>(t, _key);
+ }
+
+ template<class T> inline
+ T
+ RobotUnitDataStreamingReceiver::DataEntryReader<T>::operator()(const RobotUnitDataStreaming::TimeStep& t) const
+ {
+ return get(t);
+ }
+
+ template<class T> inline
+ void
+ RobotUnitDataStreamingReceiver::getDataEntryReader(DataEntryReader<T>& e, const std::string& name) const
+ {
+ e = _description.entries.at(name);
+ }
+ template<class T> inline
+ RobotUnitDataStreamingReceiver::DataEntryReader<T>
+ RobotUnitDataStreamingReceiver::getDataEntryReader(const std::string& name) const
+ {
+ DataEntryReader<T> r;
+ getDataEntryReader(r);
+ return r;
+ }
+
+ inline void
+ RobotUnitDataStreamingReceiver::VisitEntry(auto&& f, const timestep_t& st, const entry_t& e)
+ {
+ using enum_t = RobotUnitDataStreaming::DataEntryType;
+ // *INDENT-OFF*
+ switch (e.type)
+ {
+ case enum_t::NodeTypeBool : f(st.bools .at(e.index)); break;
+ case enum_t::NodeTypeByte : f(st.bytes .at(e.index)); break;
+ case enum_t::NodeTypeShort : f(st.shorts .at(e.index)); break;
+ case enum_t::NodeTypeInt : f(st.ints .at(e.index)); break;
+ case enum_t::NodeTypeLong : f(st.longs .at(e.index)); break;
+ case enum_t::NodeTypeFloat : f(st.floats .at(e.index)); break;
+ case enum_t::NodeTypeDouble: f(st.doubles.at(e.index)); break;
+ };
+ // *INDENT-ON*
+ }
+
+ template<class T> inline
+ T
+ RobotUnitDataStreamingReceiver::GetAs(const timestep_t& st, const entry_t& e)
+ {
+ using enum_t = RobotUnitDataStreaming::DataEntryType;
+ if constexpr(std::is_same_v<bool, T>)
+ {
+ ARMARX_CHECK_EQUAL(e.type, enum_t::NodeTypeBool);
+ return st.bools .at(e.index);
+ }
+ else if constexpr(std::is_same_v<Ice::Byte, T>)
{
- using enum_t = RobotUnitDataStreaming::DataEntryType;
- // *INDENT-OFF*
- switch (e.type)
- {
- case enum_t::NodeTypeBool : f(st.bools .at(e.index)); break;
- case enum_t::NodeTypeByte : f(st.bytes .at(e.index)); break;
- case enum_t::NodeTypeShort : f(st.shorts .at(e.index)); break;
- case enum_t::NodeTypeInt : f(st.ints .at(e.index)); break;
- case enum_t::NodeTypeLong : f(st.longs .at(e.index)); break;
- case enum_t::NodeTypeFloat : f(st.floats .at(e.index)); break;
- case enum_t::NodeTypeDouble: f(st.doubles.at(e.index)); break;
- };
- // *INDENT-ON*
+ ARMARX_CHECK_EQUAL(e.type, enum_t::NodeTypeByte);
+ return st.bytes .at(e.index);
}
+ else if constexpr(std::is_same_v<Ice::Short, T>)
+ {
+ ARMARX_CHECK_EQUAL(e.type, enum_t::NodeTypeShort);
+ return st.shorts .at(e.index);
+ }
+ else if constexpr(std::is_same_v<Ice::Int, T>)
+ {
+ ARMARX_CHECK_EQUAL(e.type, enum_t::NodeTypeInt);
+ return st.ints .at(e.index);
+ }
+ else if constexpr(std::is_same_v<Ice::Long, T>)
+ {
+ ARMARX_CHECK_EQUAL(e.type, enum_t::NodeTypeLong);
+ return st.longs .at(e.index);
+ }
+ else if constexpr(std::is_same_v<Ice::Float, T>)
+ {
+ ARMARX_CHECK_EQUAL(e.type, enum_t::NodeTypeFloat);
+ return st.floats .at(e.index);
+ }
+ else if constexpr(std::is_same_v<Ice::Double, T>)
+ {
+ ARMARX_CHECK_EQUAL(e.type, enum_t::NodeTypeDouble);
+ return st.doubles.at(e.index);
+ }
+ else
+ {
+ static_assert(!std::is_same_v<T, T>, "Type not supported!");
+ }
+ }
- static void visitEntries(auto&& f, const timestep_t& st, const auto& cont)
+ template<class T> inline
+ RobotUnitDataStreaming::DataEntryType
+ RobotUnitDataStreamingReceiver::ExpectedDataEntryType()
+ {
+ using enum_t = RobotUnitDataStreaming::DataEntryType;
+ if constexpr(std::is_same_v<bool, T>)
+ {
+ return enum_t::NodeTypeBool;
+ }
+ else if constexpr(std::is_same_v<Ice::Byte, T>)
+ {
+ return enum_t::NodeTypeByte;
+ }
+ else if constexpr(std::is_same_v<Ice::Short, T>)
+ {
+ return enum_t::NodeTypeShort;
+ }
+ else if constexpr(std::is_same_v<Ice::Int, T>)
+ {
+ return enum_t::NodeTypeInt;
+ }
+ else if constexpr(std::is_same_v<Ice::Long, T>)
+ {
+ return enum_t::NodeTypeLong;
+ }
+ else if constexpr(std::is_same_v<Ice::Float, T>)
+ {
+ return enum_t::NodeTypeFloat;
+ }
+ else if constexpr(std::is_same_v<Ice::Double, T>)
+ {
+ return enum_t::NodeTypeDouble;
+ }
+ else
{
- for (const entry_t& e : cont)
- {
- visitEntry(f, st, e);
- }
+ static_assert(!std::is_same_v<T, T>, "Type not supported!");
}
+ }
- private:
- ManagedIceObjectPtr _obj;
- RobotUnitInterfacePrx _ru;
- detail::RobotUnitDataStreamingReceiver::ReceiverPtr _receiver;
- RobotUnitDataStreaming::ReceiverPrx _proxy;
- RobotUnitDataStreaming::DataStreamingDescription _description;
- std::deque<RobotUnitDataStreaming::TimeStep> _data_buffer;
- long _last_iteration_id = -1;
- };
+ inline void
+ RobotUnitDataStreamingReceiver::VisitEntries(auto&& f, const timestep_t& st, const auto& cont)
+ {
+ for (const entry_t& e : cont)
+ {
+ VisitEntry(f, st, e);
+ }
+ }
}
diff --git a/source/RobotAPI/libraries/armem/CMakeLists.txt b/source/RobotAPI/libraries/armem/CMakeLists.txt
index 644c655fab44881d28429b7fdd266b1d41e9a8e8..5e85729b15a7afbf8855a6eca1059062635df405 100644
--- a/source/RobotAPI/libraries/armem/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem/CMakeLists.txt
@@ -28,7 +28,9 @@ set(LIB_FILES
core/ice_conversions.cpp
core/Memory.cpp
core/ProviderSegment.cpp
+
core/detail/MemoryItem.cpp
+ core/detail/MaxHistorySize.cpp
core/error/ArMemError.cpp
@@ -88,6 +90,7 @@ set(LIB_HEADERS
core/ProviderSegment.h
core/detail/EntityContainer.h
+ core/detail/MaxHistorySize.h
core/detail/MemoryContainer.h
core/detail/MemoryItem.h
core/detail/TypedEntityContainer.h
diff --git a/source/RobotAPI/libraries/armem/client/Query.cpp b/source/RobotAPI/libraries/armem/client/Query.cpp
index 87f8c7dfc678fbb065e526f7f6255e9c847ef50a..33a6b0a94db270cbf007e063436cdda47a041023 100644
--- a/source/RobotAPI/libraries/armem/client/Query.cpp
+++ b/source/RobotAPI/libraries/armem/client/Query.cpp
@@ -12,7 +12,7 @@ namespace armarx::armem::client
armem::query::data::Input QueryInput::toIce() const
{
- return armem::toIce<query::data::Input>(*this);
+ return armem::toIce<armem::query::data::Input>(*this);
}
QueryResult QueryResult::fromIce(const armem::query::data::Result& ice)
@@ -22,7 +22,7 @@ namespace armarx::armem::client
armem::query::data::Result QueryResult::toIce() const
{
- return armem::toIce<query::data::Result>(*this);
+ return armem::toIce<armem::query::data::Result>(*this);
}
}
diff --git a/source/RobotAPI/libraries/armem/client/Writer.cpp b/source/RobotAPI/libraries/armem/client/Writer.cpp
index 015d0773aded333bc6f528f744c20989717f052e..6286921f85ecdbbd123ea0e388353ff842d79d34 100644
--- a/source/RobotAPI/libraries/armem/client/Writer.cpp
+++ b/source/RobotAPI/libraries/armem/client/Writer.cpp
@@ -111,36 +111,18 @@ namespace armarx::armem::client
return result.results.at(0);
}
- armarx::armem::MemoryID
- Writer::commit(const armem::MemoryID& entityID, const armarx::aron::datanavigator::AronDictDataNavigatorPtr& instance)
+ EntityUpdateResult Writer::commit(
+ const MemoryID& entityID,
+ const std::vector<aron::datanavigator::AronDictDataNavigatorPtr>& instancesData,
+ Time timeCreated)
{
- std::vector<armarx::aron::datanavigator::AronDictDataNavigatorPtr> data({instance});
- return commit(entityID, data)[0];
- }
-
- std::vector<armarx::armem::MemoryID>
- Writer::commit(const armem::MemoryID& entityID, const std::vector<armarx::aron::datanavigator::AronDictDataNavigatorPtr>& data)
- {
- armem::Commit c;
- armem::EntityUpdate& update = c.updates.emplace_back();
-
+ EntityUpdate update;
update.entityID = entityID;
- update.timeCreated = armem::Time::now();
- update.instancesData = data;
-
- armem::CommitResult commitResult = commit(c);
-
- std::vector<armarx::armem::MemoryID> ret;
- for (const auto& res : commitResult.results)
- {
- ARMARX_INFO << res;
- ret.push_back(res.snapshotID);
- }
-
- return ret;
+ update.instancesData = instancesData;
+ update.timeCreated = timeCreated;
+ return commit(update);
}
-
void
Writer::setWritingMemory(server::WritingMemoryInterfacePrx memory)
{
diff --git a/source/RobotAPI/libraries/armem/client/Writer.h b/source/RobotAPI/libraries/armem/client/Writer.h
index 6f8fbe8769c15665b28959e2aaab21085214abd7..990a0f1c312eaf10fad24cf4cfc3012bb5138d67 100644
--- a/source/RobotAPI/libraries/armem/client/Writer.h
+++ b/source/RobotAPI/libraries/armem/client/Writer.h
@@ -9,7 +9,17 @@ namespace armarx::armem::client
{
/**
- * @brief Helps sending data to a memory.
+ * @brief Helps a memory client sending data to a memory.
+ *
+ * You can check whether the writer is ready using the bool operator:
+ * @code
+ * client::Writer writer(writingMemoryProxy);
+ * ...
+ * if (writer)
+ * {
+ * writer.commit(...);
+ * }
+ * @endcode
*/
class Writer
{
@@ -27,16 +37,18 @@ namespace armarx::armem::client
data::AddSegmentResult addSegment(const data::AddSegmentInput& input);
data::AddSegmentsResult addSegments(const data::AddSegmentsInput& input);
+
/**
* @brief Writes a `Commit` to the memory.
*/
CommitResult commit(const Commit& commit);
+ /// Commit a single entity update.
EntityUpdateResult commit(const EntityUpdate& update);
-
- armem::MemoryID commit(const armem::MemoryID& entityID, const armarx::aron::datanavigator::AronDictDataNavigatorPtr& instance);
- std::vector<armarx::armem::MemoryID> commit(const armem::MemoryID& entityID, const std::vector<armarx::aron::datanavigator::AronDictDataNavigatorPtr>& data);
-
-
+ /// Commit a single entity update.
+ EntityUpdateResult commit(
+ const MemoryID& entityID,
+ const std::vector<aron::datanavigator::AronDictDataNavigatorPtr>& instancesData,
+ Time timeCreated);
void setWritingMemory(server::WritingMemoryInterfacePrx memory);
diff --git a/source/RobotAPI/libraries/armem/core/CoreSegment.cpp b/source/RobotAPI/libraries/armem/core/CoreSegment.cpp
index 16e3b4291608c5970c074f7ea89e4d59c0e44dba..34f73cbdf4f7845a8fc2ad0ab14bc7d9ffcb8775 100644
--- a/source/RobotAPI/libraries/armem/core/CoreSegment.cpp
+++ b/source/RobotAPI/libraries/armem/core/CoreSegment.cpp
@@ -126,11 +126,22 @@ namespace armarx::armem
throw armem::error::ContainerEntryAlreadyExists(
providerSegment.getLevelName(), providerSegment.name(), getLevelName(), this->name());
}
+
auto it = providerSegments.emplace(providerSegment.name(), std::move(providerSegment)).first;
it->second.id().setCoreSegmentID(id());
+ it->second.setMaxHistorySize(maxHistorySize);
return it->second;
}
+ void CoreSegment::setMaxHistorySize(long maxSize)
+ {
+ MaxHistorySize::setMaxHistorySize(maxSize);
+ for (auto& [name, seg] : providerSegments)
+ {
+ seg.setMaxHistorySize(maxSize);
+ }
+ }
+
std::string CoreSegment::getLevelName() const
{
return "core segment";
diff --git a/source/RobotAPI/libraries/armem/core/CoreSegment.h b/source/RobotAPI/libraries/armem/core/CoreSegment.h
index 68bf7dc4ebf1d9a22d74728b6f4b4b2470dbe218..aed697fb00d5aaf4c3d1f8446be5b289fae97cb8 100644
--- a/source/RobotAPI/libraries/armem/core/CoreSegment.h
+++ b/source/RobotAPI/libraries/armem/core/CoreSegment.h
@@ -5,6 +5,7 @@
#include "ProviderSegment.h"
#include "detail/TypedEntityContainer.h"
+#include "detail/MaxHistorySize.h"
namespace armarx::armem
@@ -13,7 +14,9 @@ namespace armarx::armem
/**
* @brief Data of a core segment containing multiple provider segments.
*/
- class CoreSegment : public detail::TypedEntityContainer<ProviderSegment, CoreSegment>
+ class CoreSegment :
+ public detail::TypedEntityContainer<ProviderSegment, CoreSegment>
+ , public detail::MaxHistorySize
{
using Base = detail::TypedEntityContainer<ProviderSegment, CoreSegment>;
@@ -59,6 +62,14 @@ namespace armarx::armem
ProviderSegment& addProviderSegment(ProviderSegment&& providerSegment);
+ /**
+ * @brief Sets the maximum history size of entities in this segment.
+ * This affects all current entities as well as new ones.
+ * @see Entity::setMaxHistorySize()
+ */
+ void setMaxHistorySize(long maxSize) override;
+
+
bool equalsDeep(const CoreSegment& other) const;
diff --git a/source/RobotAPI/libraries/armem/core/Entity.h b/source/RobotAPI/libraries/armem/core/Entity.h
index 51d55aa4951af17e671417ab95c21bbbb3690e7b..b81ffe76199f7a978d822a4d91cc421b48537682 100644
--- a/source/RobotAPI/libraries/armem/core/Entity.h
+++ b/source/RobotAPI/libraries/armem/core/Entity.h
@@ -169,8 +169,6 @@ namespace armarx::armem
// ToDo: Add max age;
// ToDo in future: keep/remove predicate
-
-
// MemoryItem interface
public:
std::string getKeyString() const override
diff --git a/source/RobotAPI/libraries/armem/core/MemoryID.cpp b/source/RobotAPI/libraries/armem/core/MemoryID.cpp
index 73d92995e3a24d15bd3593ea8f0ec1314cefe2e2..664319410949b11c7563512a6ccc22dc0c2dd3e1 100644
--- a/source/RobotAPI/libraries/armem/core/MemoryID.cpp
+++ b/source/RobotAPI/libraries/armem/core/MemoryID.cpp
@@ -70,18 +70,37 @@ namespace armarx::armem
return simox::alg::join(items, delimeter, false, false);
}
- std::string MemoryID::getLastNameSet() const
+ std::string MemoryID::getLeafItem() const
{
std::vector<std::string> items = getAllItems();
- std::string latest = "";
- for (const std::string& name : items)
+ for (auto it = items.rbegin(); it != items.rend(); ++it)
{
- if (!name.empty())
+ if (!it->empty())
{
- latest = name;
+ return *it;
}
}
- return latest;
+ return "";
+ }
+
+ bool MemoryID::hasGap() const
+ {
+ bool emptyFound = false;
+ for (const std::string& name : getAllItems())
+ {
+ if (name.empty())
+ {
+ emptyFound = true;
+ }
+ else
+ {
+ if (emptyFound)
+ {
+ return true;
+ }
+ }
+ }
+ return false;
}
@@ -277,6 +296,16 @@ namespace armarx::armem
return int(parseInteger(string, "instance index"));
}
+ bool MemoryID::operator==(const MemoryID& other) const
+ {
+ return memoryName == other.memoryName
+ and coreSegmentName == other.coreSegmentName
+ and providerSegmentName == other.providerSegmentName
+ and entityName == other.entityName
+ and timestamp == other.timestamp
+ and instanceIndex == other.instanceIndex;
+ }
+
long long MemoryID::parseInteger(const std::string& string, const std::string& semanticName)
{
try
diff --git a/source/RobotAPI/libraries/armem/core/MemoryID.h b/source/RobotAPI/libraries/armem/core/MemoryID.h
index d92197c96bf08a0dfcabf22e4effe7f54d697762..1fc22f7288b51970f298a9309cbe33b087d48f5f 100644
--- a/source/RobotAPI/libraries/armem/core/MemoryID.h
+++ b/source/RobotAPI/libraries/armem/core/MemoryID.h
@@ -41,9 +41,14 @@ namespace armarx::armem
std::string str(const std::string& delimeter) const;
static MemoryID fromString(const std::string& string);
+ std::string timestampStr() const;
+ std::string instanceIndexStr() const;
- std::vector<std::string> getItems() const;
+
+ /// Get all levels as string.
std::vector<std::string> getAllItems() const;
+ /// Get the levels from root to first not defined level (excluding).
+ std::vector<std::string> getItems() const;
bool hasMemoryName() const
@@ -107,26 +112,26 @@ namespace armarx::armem
MemoryID withTimestamp(Time time) const;
MemoryID withInstanceIndex(int index) const;
- std::string getLastNameSet() const;
- std::string timestampStr() const;
- std::string instanceIndexStr() const;
+ /// Indicate whether this ID has a gap such as in 'Memory//MyProvider' (no core segment name).
+ bool hasGap() const;
+ /// Get the lowest defined level (or empty string if there is none).
+ std::string getLeafItem() const;
+
static Time timestampFromStr(const std::string& timestamp);
static int instanceIndexFromStr(const std::string& index);
- friend std::ostream& operator<<(std::ostream& os, const MemoryID id);
- bool operator== (const MemoryID& other) const
+ bool operator ==(const MemoryID& other) const;
+ inline bool operator !=(const MemoryID& other) const
{
- return memoryName == other.memoryName
- and coreSegmentName == other.coreSegmentName
- and providerSegmentName == other.providerSegmentName
- and entityName == other.entityName
- and timestamp == other.timestamp
- and instanceIndex == other.instanceIndex;
+ return !(*this == other);
}
+ friend std::ostream& operator<<(std::ostream& os, const MemoryID id);
+
+
private:
static long long parseInteger(const std::string& string, const std::string& semanticName);
diff --git a/source/RobotAPI/libraries/armem/core/ProviderSegment.cpp b/source/RobotAPI/libraries/armem/core/ProviderSegment.cpp
index 28010d765fd024f0b83442082d9daf636e7274e0..fb0ca8cc82a3cfcbf3f89251c87b2d79a7f4e357 100644
--- a/source/RobotAPI/libraries/armem/core/ProviderSegment.cpp
+++ b/source/RobotAPI/libraries/armem/core/ProviderSegment.cpp
@@ -154,7 +154,7 @@ namespace armarx::armem
void ProviderSegment::setMaxHistorySize(long maxSize)
{
- this->maxHistorySize = maxSize;
+ MaxHistorySize::setMaxHistorySize(maxSize);
for (auto& [name, entity] : entities)
{
entity.setMaxHistorySize(maxSize);
diff --git a/source/RobotAPI/libraries/armem/core/ProviderSegment.h b/source/RobotAPI/libraries/armem/core/ProviderSegment.h
index ecfa53be218d27cb9978009196aec93f68f475c7..384ee1b416c2dac48ae1dcccdc9fa3e984ff6700 100644
--- a/source/RobotAPI/libraries/armem/core/ProviderSegment.h
+++ b/source/RobotAPI/libraries/armem/core/ProviderSegment.h
@@ -5,6 +5,7 @@
#include "Entity.h"
#include "detail/TypedEntityContainer.h"
+#include "detail/MaxHistorySize.h"
namespace armarx::armem
@@ -13,7 +14,10 @@ namespace armarx::armem
/**
* @brief Data of a provider segment containing multiple entities.
*/
- class ProviderSegment : public detail::TypedEntityContainer<Entity, ProviderSegment>
+ class ProviderSegment :
+ public detail::TypedEntityContainer<Entity, ProviderSegment>
+ , public detail::MaxHistorySize
+
{
using Base = detail::TypedEntityContainer<Entity, ProviderSegment>;
@@ -66,7 +70,7 @@ namespace armarx::armem
* This affects all current entities as well as new ones.
* @see Entity::setMaxHistorySize()
*/
- void setMaxHistorySize(long maxSize);
+ void setMaxHistorySize(long maxSize) override;
bool equalsDeep(const ProviderSegment& other) const;
@@ -88,12 +92,6 @@ namespace armarx::armem
/// The entities.
std::map<std::string, Entity> entities;
- /**
- * @brief Maximum size of entity histories.
- * @see Entity::maxHstorySize
- */
- long maxHistorySize = -1;
-
};
}
diff --git a/source/RobotAPI/libraries/armem/core/detail/MaxHistorySize.cpp b/source/RobotAPI/libraries/armem/core/detail/MaxHistorySize.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..cdd34e3be26351bab89ad6b712026769057f8b0e
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/detail/MaxHistorySize.cpp
@@ -0,0 +1,19 @@
+#include "MaxHistorySize.h"
+
+
+namespace armarx::armem::detail
+{
+ MaxHistorySize::~MaxHistorySize()
+ {
+ }
+
+ void MaxHistorySize::setMaxHistorySize(long maxSize)
+ {
+ this->maxHistorySize = maxSize;
+ }
+
+ long MaxHistorySize::getMaxHistorySize() const
+ {
+ return maxHistorySize;
+ }
+}
diff --git a/source/RobotAPI/libraries/armem/core/detail/MaxHistorySize.h b/source/RobotAPI/libraries/armem/core/detail/MaxHistorySize.h
new file mode 100644
index 0000000000000000000000000000000000000000..d96e732964103c93987708e2b1d5dbaf9377640a
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/detail/MaxHistorySize.h
@@ -0,0 +1,31 @@
+#pragma once
+
+
+
+namespace armarx::armem::detail
+{
+ class MaxHistorySize
+ {
+ public:
+
+ virtual ~MaxHistorySize();
+
+ /**
+ * @brief Sets the maximum history size of entities in this segment.
+ * This affects all current entities as well as new ones.
+ * @see Entity::setMaxHistorySize()
+ */
+ virtual void setMaxHistorySize(long maxSize);
+ virtual long getMaxHistorySize() const;
+
+
+ protected:
+
+ /**
+ * @brief Maximum size of entity histories.
+ * @see Entity::maxHstorySize
+ */
+ long maxHistorySize = -1;
+
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/core/io/DiskReader/DiskReader.cpp b/source/RobotAPI/libraries/armem/core/io/DiskReader/DiskReader.cpp
index 491ea69873af11f55d8df97f428e562b9aafcc54..92f7d29a2363141f1018935140ba011e2865cdc9 100644
--- a/source/RobotAPI/libraries/armem/core/io/DiskReader/DiskReader.cpp
+++ b/source/RobotAPI/libraries/armem/core/io/DiskReader/DiskReader.cpp
@@ -155,7 +155,7 @@ namespace armarx::armem::io
aron::typenavigator::AronObjectTypeNavigatorPtr DiskReader::readSingleTypeInformationFromDisk(const MemoryID& id) const
{
- std::filesystem::path p = rootPath / id.str() / (id.getLastNameSet() + getTypeSuffix());
+ std::filesystem::path p = rootPath / id.str() / (id.getLeafItem() + getTypeSuffix());
return readSingleTypeInformationFromDisk(p);
}
diff --git a/source/RobotAPI/libraries/armem/core/io/DiskWriter/DiskWriter.cpp b/source/RobotAPI/libraries/armem/core/io/DiskWriter/DiskWriter.cpp
index d0a5d9d3fc002b759c385c61037eef0291436fc2..f996440e9f20d3892398033d9631999694ba1215 100644
--- a/source/RobotAPI/libraries/armem/core/io/DiskWriter/DiskWriter.cpp
+++ b/source/RobotAPI/libraries/armem/core/io/DiskWriter/DiskWriter.cpp
@@ -308,5 +308,4 @@ namespace armarx::armem::io
historyTimestampsError.insert(historyTimestampsError.end(), o.historyTimestampsError.begin(), o.historyTimestampsError.end());
entityInstancesError.insert(entityInstancesError.end(), o.entityInstancesError.begin(), o.entityInstancesError.end());
}
-
}
diff --git a/source/RobotAPI/libraries/armem/mns/ClientPlugin.cpp b/source/RobotAPI/libraries/armem/mns/ClientPlugin.cpp
index 3ecd25a856c018b75a007af78f57d41401fdc432..f7bba9445c73a7c13a3374969d631530145d9c14 100644
--- a/source/RobotAPI/libraries/armem/mns/ClientPlugin.cpp
+++ b/source/RobotAPI/libraries/armem/mns/ClientPlugin.cpp
@@ -19,20 +19,19 @@ namespace armarx::armem::mns::plugins
{
properties->defineOptionalProperty<std::string>(
makePropertyName(PROPERTY_MNS_NAME_NAME),
- PROPERTY_MNS_NAME_DEFAULT,
+ parent<ClientPluginUserBase>().memoryNameSystemName,
"Name of the Memory Name System (MNS) component.");
}
if (!properties->hasDefinition(makePropertyName(PROPERTY_MNS_ENABLED_NAME)))
{
properties->defineOptionalProperty<bool>(
makePropertyName(PROPERTY_MNS_ENABLED_NAME),
- PROPERTY_MNS_ENABLED_DEFAULT,
+ parent<ClientPluginUserBase>().memoryNameSystemEnabled,
"Whether to use (and depend on) the Memory Name System (MNS)."
"\nSet to false to use this memory as a stand-alone.");
}
}
-
void ClientPlugin::preOnInitComponent()
{
if (isMemoryNameSystemEnabled())
@@ -49,19 +48,18 @@ namespace armarx::armem::mns::plugins
}
}
-
bool ClientPlugin::isMemoryNameSystemEnabled()
{
return parentDerives<Component>() ?
parent<Component>().getProperty<bool>(makePropertyName(PROPERTY_MNS_ENABLED_NAME)) :
- PROPERTY_MNS_ENABLED_DEFAULT;
+ parent<ClientPluginUserBase>().memoryNameSystemEnabled;
}
std::string ClientPlugin::getMemoryNameSystemName()
{
return parentDerives<Component>() ?
parent<Component>().getProperty<std::string>(makePropertyName(PROPERTY_MNS_NAME_NAME)) :
- std::string{PROPERTY_MNS_NAME_DEFAULT};
+ std::string{parent<ClientPluginUserBase>().memoryNameSystemName};
}
mns::MemoryNameSystemInterfacePrx ClientPlugin::getMemoryNameSystem()
@@ -71,9 +69,38 @@ namespace armarx::armem::mns::plugins
: nullptr;
}
+ ClientPluginUserBase::~ClientPluginUserBase()
+ {
+ }
+
+ armem::data::WaitForMemoryResult ClientPluginUserBase::useMemory(const MemoryID& id)
+ {
+ return useMemory(id.memoryName);
+ }
+
+ armem::data::WaitForMemoryResult ClientPluginUserBase::useMemory(const std::string& memoryName)
+ {
+ armem::data::WaitForMemoryInput input;
+ input.name = memoryName;
+
+ armem::data::WaitForMemoryResult result = memoryNameSystem->waitForMemory(input);
+ if (result.success)
+ {
+ if (Component* comp = dynamic_cast<Component*>(this))
+ {
+ // Add dependency.
+ comp->usingProxy(result.proxy->ice_getIdentity().name);
+ }
+ }
+ return result;
+ }
armem::data::WaitForMemoryResult ClientPluginUserBase::waitForMemory(const std::string& memoryName)
{
+ if (!memoryNameSystem)
+ {
+ return {};
+ }
armem::data::WaitForMemoryInput input;
input.name = memoryName;
return memoryNameSystem->waitForMemory(input);
@@ -81,6 +108,10 @@ namespace armarx::armem::mns::plugins
armem::data::ResolveMemoryNameResult ClientPluginUserBase::resolveMemoryName(const std::string& memoryName)
{
+ if (!memoryNameSystem)
+ {
+ return {};
+ }
armem::data::ResolveMemoryNameInput input;
input.name = memoryName;
return memoryNameSystem->resolveMemoryName(input);
diff --git a/source/RobotAPI/libraries/armem/mns/ClientPlugin.h b/source/RobotAPI/libraries/armem/mns/ClientPlugin.h
index 074ec2ef023bc9286276dfe07229f822ea3f5b88..b941d214cf4fb3530d0e074cdc4e4b8e6e1e60b3 100644
--- a/source/RobotAPI/libraries/armem/mns/ClientPlugin.h
+++ b/source/RobotAPI/libraries/armem/mns/ClientPlugin.h
@@ -3,6 +3,7 @@
#include <ArmarXCore/core/Component.h>
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
namespace armarx::armem::mns
@@ -44,12 +45,9 @@ namespace armarx::armem::mns::plugins
*/
mns::MemoryNameSystemInterfacePrx getMemoryNameSystem();
-
- private:
+ public:
static constexpr const char* PROPERTY_MNS_ENABLED_NAME = "mns.MemoryNameSystemEnabled";
- static constexpr const bool PROPERTY_MNS_ENABLED_DEFAULT = true;
static constexpr const char* PROPERTY_MNS_NAME_NAME = "mns.MemoryNameSystemName";
- static constexpr const char* PROPERTY_MNS_NAME_DEFAULT = "ArMemMemoryNameSystem";
};
@@ -60,13 +58,25 @@ namespace armarx::armem::mns::plugins
class ClientPluginUserBase
{
protected:
+
+ virtual ~ClientPluginUserBase();
+
+
armem::data::WaitForMemoryResult waitForMemory(const std::string& memoryName);
armem::data::ResolveMemoryNameResult resolveMemoryName(const std::string& memoryName);
bool isMemoryAvailable(const std::string& memoryName);
+ armem::data::WaitForMemoryResult useMemory(const MemoryID& id);
+ armem::data::WaitForMemoryResult useMemory(const std::string& memoryName);
+
+
public:
+
/// Only set when enabled.
mns::MemoryNameSystemInterfacePrx memoryNameSystem = nullptr;
+ bool memoryNameSystemEnabled = true;
+ std::string memoryNameSystemName = "ArMemMemoryNameSystem";
+
};
}
diff --git a/source/RobotAPI/libraries/armem/server/MemoryRemoteGui.cpp b/source/RobotAPI/libraries/armem/server/MemoryRemoteGui.cpp
index e6c51d554b185570c0de0b3e658a363bc0abac1b..3c5e7b6ecb4172b075d6cca7b4a59b0d543c3283 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryRemoteGui.cpp
+++ b/source/RobotAPI/libraries/armem/server/MemoryRemoteGui.cpp
@@ -3,7 +3,7 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <RobotAPI/libraries/aron/aroncore/navigators/datanavigator/AronAllDataNavigators.h>
-#include <RobotAPI/libraries/aron/aroncore/navigators/visitors/AronDataVisitor.h>
+#include <RobotAPI/libraries/aron/aroncore/navigators/visitors/DataVisitor.h>
#include <SimoxUtility/meta/type_name.h>
diff --git a/source/RobotAPI/libraries/armem/server/RemoteGuiAronDataVisitor.cpp b/source/RobotAPI/libraries/armem/server/RemoteGuiAronDataVisitor.cpp
index 6f6369a4f23909b38362f823128a29284c4f5946..d2253746e591bcd18caabf997df25f9f0746fe5f 100644
--- a/source/RobotAPI/libraries/armem/server/RemoteGuiAronDataVisitor.cpp
+++ b/source/RobotAPI/libraries/armem/server/RemoteGuiAronDataVisitor.cpp
@@ -41,12 +41,12 @@ namespace armarx::armem::server
return true;
}
- void RemoteGuiAronDataVisitor::streamValueText(AronStringDataNavigator& n, std::stringstream& ss)
+ void RemoteGuiAronDataVisitor::streamValueText(StringDataNavigator& n, std::stringstream& ss)
{
ss << "'" << n.getValue() << "'";
}
- void RemoteGuiAronDataVisitor::streamValueText(AronNDArrayDataNavigator& n, std::stringstream& ss)
+ void RemoteGuiAronDataVisitor::streamValueText(NDArrayDataNavigator& n, std::stringstream& ss)
{
ss << "shape (" << simox::alg::join(simox::alg::multi_to_string(n.getDimensions()), ", ") << ")";
}
diff --git a/source/RobotAPI/libraries/armem/server/RemoteGuiAronDataVisitor.h b/source/RobotAPI/libraries/armem/server/RemoteGuiAronDataVisitor.h
index 945cd1de6a220d90735b24e09bbbabbf33490836..ee2c40570db1cca8a4e371e3d4bfd5f7347cf18f 100644
--- a/source/RobotAPI/libraries/armem/server/RemoteGuiAronDataVisitor.h
+++ b/source/RobotAPI/libraries/armem/server/RemoteGuiAronDataVisitor.h
@@ -5,13 +5,13 @@
#include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
-#include <RobotAPI/libraries/aron/aroncore/navigators/visitors/AronDataVisitor.h>
+#include <RobotAPI/libraries/aron/aroncore/navigators/visitors/DataVisitor.h>
namespace armarx::armem::server
{
- struct RemoteGuiAronDataVisitor : public aron::visitor::AronDataVisitor
+ struct RemoteGuiAronDataVisitor : public aron::visitor::DataVisitor
{
using GroupBox = armarx::RemoteGui::Client::GroupBox;
using GridLayout = armarx::RemoteGui::Client::GridLayout;
@@ -34,20 +34,20 @@ namespace armarx::armem::server
virtual ~RemoteGuiAronDataVisitor() override;
- bool visitEnter(const std::string& key, AronDictDataNavigator& n) override
+ bool visitEnter(const std::string& key, DictDataNavigator& n) override
{
return visitEnter(key, "dict", n.childrenSize());
}
- bool visitExit(AronDictDataNavigator&) override
+ bool visitExit(DictDataNavigator&) override
{
return visitExit();
}
- bool visitEnter(const std::string& key, AronListDataNavigator& n) override
+ bool visitEnter(const std::string& key, ListDataNavigator& n) override
{
return visitEnter(key, "list", n.childrenSize());
}
- bool visitExit(AronListDataNavigator&) override
+ bool visitExit(ListDataNavigator&) override
{
return visitExit();
}
@@ -57,42 +57,42 @@ namespace armarx::armem::server
bool visitExit();
// Do not hide base overloads.
- using AronDataVisitor::visitEnter;
- using AronDataVisitor::visitExit;
+ using DataVisitor::visitEnter;
+ using DataVisitor::visitExit;
- bool visit(const std::string& key, AronBoolDataNavigator& b) override
+ bool visit(const std::string& key, BoolDataNavigator& b) override
{
this->addValueLabel(key, b, "bool");
return true;
}
- bool visit(const std::string& key, AronDoubleDataNavigator& d) override
+ bool visit(const std::string& key, DoubleDataNavigator& d) override
{
this->addValueLabel(key, d, "double");
return true;
}
- bool visit(const std::string& key, AronFloatDataNavigator& f) override
+ bool visit(const std::string& key, FloatDataNavigator& f) override
{
this->addValueLabel(key, f, "float");
return true;
}
- bool visit(const std::string& key, AronIntDataNavigator& i) override
+ bool visit(const std::string& key, IntDataNavigator& i) override
{
this->addValueLabel(key, i, "int");
return true;
}
- bool visit(const std::string& key, AronLongDataNavigator& l) override
+ bool visit(const std::string& key, LongDataNavigator& l) override
{
this->addValueLabel(key, l, "long");
return true;
}
- bool visit(const std::string& key, AronStringDataNavigator& string) override
+ bool visit(const std::string& key, StringDataNavigator& string) override
{
this->addValueLabel(key, string, "string");
return true;
}
- bool visit(const std::string& key, AronNDArrayDataNavigator& array) override
+ bool visit(const std::string& key, NDArrayDataNavigator& array) override
{
this->addValueLabel(key, array, "ND Array");
return true;
@@ -127,8 +127,8 @@ namespace armarx::armem::server
{
ss << n.getValue();
}
- void streamValueText(AronStringDataNavigator& n, std::stringstream& ss);
- void streamValueText(AronNDArrayDataNavigator& n, std::stringstream& ss);
+ void streamValueText(StringDataNavigator& n, std::stringstream& ss);
+ void streamValueText(NDArrayDataNavigator& n, std::stringstream& ss);
void checkGroupsNotEmpty() const;
diff --git a/source/RobotAPI/libraries/armem_gui/CMakeLists.txt b/source/RobotAPI/libraries/armem_gui/CMakeLists.txt
index 677ac70bc947279f8632d60d5d9ef792b2eb1a07..b0f974d9effc324e80a108d1889f291122d4b61f 100644
--- a/source/RobotAPI/libraries/armem_gui/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_gui/CMakeLists.txt
@@ -12,9 +12,12 @@ set(LIBRARIES
set(SOURCES
InstanceTreeWidget.cpp
MemoryTreeWidget.cpp
- QTreeWidgetAronDataVisitor.cpp
gui_utils.cpp
+ instance/InstanceTreeDataVisitorBase.cpp
+ instance/InstanceTreeDataVisitor.cpp
+ instance/InstanceTreeTypedDataVisitor.cpp
+
query_widgets/QueryWidget.cpp
query_widgets/SnapshotForm.cpp
query_widgets/SnapshotSelectorWidget.cpp
@@ -23,9 +26,12 @@ set(HEADERS
InstanceTreeWidget.h
MemoryTreeWidget.h
TreeWidgetBuilder.h
- QTreeWidgetAronDataVisitor.h
gui_utils.h
+ instance/InstanceTreeDataVisitorBase.h
+ instance/InstanceTreeDataVisitor.h
+ instance/InstanceTreeTypedDataVisitor.h
+
query_widgets/QueryWidget.h
query_widgets/SnapshotForm.h
query_widgets/SnapshotSelectorWidget.h
diff --git a/source/RobotAPI/libraries/armem_gui/InstanceTreeWidget.cpp b/source/RobotAPI/libraries/armem_gui/InstanceTreeWidget.cpp
index d63f84fe55ddafddfc054bb94d287d392c21fc3f..f8c5ed61a3ee104c5463d7f17ef53064474fea0a 100644
--- a/source/RobotAPI/libraries/armem_gui/InstanceTreeWidget.cpp
+++ b/source/RobotAPI/libraries/armem_gui/InstanceTreeWidget.cpp
@@ -8,7 +8,9 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronObjectTypeNavigator.h>
-#include <RobotAPI/libraries/armem_gui/QTreeWidgetAronDataVisitor.h>
+#include <RobotAPI/libraries/armem_gui/gui_utils.h>
+#include <RobotAPI/libraries/armem_gui/instance/InstanceTreeDataVisitor.h>
+#include <RobotAPI/libraries/armem_gui/instance/InstanceTreeTypedDataVisitor.h>
namespace armarx::armem::gui
@@ -24,22 +26,63 @@ namespace armarx::armem::gui
clear();
QStringList columns;
columns.insert(int(Columns::KEY), "Key");
- columns.insert(int(Columns::TYPE), "Type");
columns.insert(int(Columns::VALUE), "Value");
+ columns.insert(int(Columns::TYPE), "Type");
setColumnCount(columns.size());
setHeaderLabels(columns);
// header()->setMinimumSectionSize(25);
- // header()->resizeSection(int(Columns::KEY), 250);
+ header()->resizeSection(int(Columns::KEY), 250);
+ header()->resizeSection(int(Columns::VALUE), 250);
// header()->setTextElideMode(Qt::TextElideMode::ElideRight);
+
+ itemInstanceID = new QTreeWidgetItem({"Instance ID"});
+ itemInstanceID->addChild(new QTreeWidgetItem({"Memory Name"}));
+ itemInstanceID->addChild(new QTreeWidgetItem({"Core Segment Name"}));
+ itemInstanceID->addChild(new QTreeWidgetItem({"Provider Segment Name"}));
+ itemInstanceID->addChild(new QTreeWidgetItem({"Entity Name"}));
+ itemInstanceID->addChild(new QTreeWidgetItem({"Timestamp"}));
+ itemInstanceID->addChild(new QTreeWidgetItem({"Instance Index"}));
+
+ itemMetadata = new QTreeWidgetItem({"Metadata"});
+ itemMetadata->addChild(new QTreeWidgetItem({"Confidence"}));
+ itemMetadata->addChild(new QTreeWidgetItem({"Time Created"}));
+ itemMetadata->addChild(new QTreeWidgetItem({"Time Sent"}));
+ itemMetadata->addChild(new QTreeWidgetItem({"Time Arrived"}));
+
+ itemData = new QTreeWidgetItem({"Data"});
+
+ QList<QTreeWidgetItem*> items = {itemInstanceID, itemMetadata, itemData};
+ addTopLevelItems(items);
+ for (auto* item : items)
+ {
+ item->setExpanded(true);
+ }
}
+ void InstanceTreeWidget::setStatusLabel(QLabel* statusLabel)
+ {
+ this->statusLabel = statusLabel;
+ }
+
+ void InstanceTreeWidget::setUseTypeInfo(bool enable)
+ {
+ this->useTypeInfo = enable;
+ update();
+ }
+
+
void InstanceTreeWidget::update(const MemoryID& id, const Memory& memory)
{
- const armem::EntityInstance* instance;
+ aron::typenavigator::AronObjectTypeNavigatorPtr aronType = nullptr;
+ const armem::EntityInstance* instance = nullptr;
try
{
instance = &memory.getEntityInstance(id);
+ if (useTypeInfo)
+ {
+ aronType = memory.getCoreSegment(id.coreSegmentName).getProviderSegment(id.providerSegmentName).aronType();
+ }
}
catch (const armem::error::ArMemError& e)
{
@@ -51,35 +94,80 @@ namespace armarx::armem::gui
};
if (instance)
{
- update(*instance);
+ update(*instance, aronType);
}
}
- void InstanceTreeWidget::update(const EntityInstance& instance)
+
+ void InstanceTreeWidget::update(const EntityInstance& instance, aron::typenavigator::AronObjectTypeNavigatorPtr aronType)
{
- QTreeWidgetItem* item = new QTreeWidgetItem();
- item->setText(0, QString::fromStdString(instance.id().str()));
+ currentInstance = instance;
+ currentAronType = aronType;
+ update();
+ }
- if (!instance.data())
+ void InstanceTreeWidget::update()
+ {
+ if (currentInstance)
{
- item->setText(0, "(No data.)");
+ updateInstanceID(currentInstance->id());
+ updateData(currentInstance->data(), currentAronType);
+ updateMetaData(currentInstance->metadata());
+
+ emit updated();
}
- else
+ }
+
+
+ void InstanceTreeWidget::updateInstanceID(const MemoryID& id)
+ {
+ const std::vector<std::string> items = id.getAllItems();
+ ARMARX_CHECK_EQUAL(itemInstanceID->childCount(), static_cast<int>(items.size()));
+ int i = 0;
+ for (const std::string& item : items)
{
- QTreeWidgetAronDataVisitor visitor(item);
- visitor.applyTo(*instance.data());
+ itemInstanceID->child(i++)->setText(int(Columns::VALUE), QString::fromStdString(item));
}
+ }
- clear();
- addTopLevelItem(item);
-
- item->setExpanded(true);
- emit updated();
+ void InstanceTreeWidget::updateData(const aron::datanavigator::AronDictDataNavigatorPtr& data, aron::typenavigator::AronObjectTypeNavigatorPtr aronType)
+ {
+ armarx::gui::clearItem(itemData);
+ if (!data)
+ {
+ QTreeWidgetItem* item = new QTreeWidgetItem({"(No data.)"});
+ itemData->addChild(item);
+ }
+ else if (useTypeInfo && aronType)
+ {
+ InstanceTreeTypedDataVisitor visitor(itemData);
+ visitor.setColumns(int(Columns::KEY), int(Columns::VALUE), int(Columns::TYPE));
+ visitor.applyTo(*aronType, *data);
+ }
+ else
+ {
+ InstanceTreeDataVisitor visitor(itemData);
+ visitor.setColumns(int(Columns::KEY), int(Columns::VALUE), int(Columns::TYPE));
+ visitor.applyTo(*data);
+ }
+ itemData->setExpanded(true);
}
- void InstanceTreeWidget::setStatusLabel(QLabel* statusLabel)
+ void InstanceTreeWidget::updateMetaData(const EntityInstanceMetadata& metadata)
{
- this->statusLabel = statusLabel;
+ std::vector<std::string> items =
+ {
+ std::to_string(metadata.confidence),
+ armem::toDateTimeMilliSeconds(metadata.timeCreated),
+ armem::toDateTimeMilliSeconds(metadata.timeSent),
+ armem::toDateTimeMilliSeconds(metadata.timeArrived)
+ };
+ ARMARX_CHECK_EQUAL(itemMetadata->childCount(), items.size());
+ int i = 0;
+ for (const std::string& item : items)
+ {
+ itemMetadata->child(i++)->setText(int(Columns::VALUE), QString::fromStdString(item));
+ }
}
}
diff --git a/source/RobotAPI/libraries/armem_gui/InstanceTreeWidget.h b/source/RobotAPI/libraries/armem_gui/InstanceTreeWidget.h
index 4c3c2fa300d4364ea8b968babf9a83ce64e714e8..9bf076d65c3868b52775eb31f02ca4e01e1ca284 100644
--- a/source/RobotAPI/libraries/armem_gui/InstanceTreeWidget.h
+++ b/source/RobotAPI/libraries/armem_gui/InstanceTreeWidget.h
@@ -1,9 +1,14 @@
#pragma once
+#include <optional>
+
#include <QTreeWidget>
+#include <RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronObjectTypeNavigator.h>
+
#include <RobotAPI/libraries/armem/core/Memory.h>
+
class QLabel;
@@ -17,10 +22,12 @@ namespace armarx::armem::gui
InstanceTreeWidget();
- void update(const MemoryID& id, const Memory& memory);
- void update(const EntityInstance& instance);
-
void setStatusLabel(QLabel* statusLabel);
+ void setUseTypeInfo(bool enable);
+
+ void update(const MemoryID& id, const Memory& memory);
+ void update(const EntityInstance& instance, aron::typenavigator::AronObjectTypeNavigatorPtr aronType = nullptr);
+ void update();
signals:
@@ -35,15 +42,28 @@ namespace armarx::armem::gui
private:
void initWidget();
+ void updateInstanceID(const MemoryID& id);
+ void updateData(const aron::datanavigator::AronDictDataNavigatorPtr& data, aron::typenavigator::AronObjectTypeNavigatorPtr aronType = nullptr);
+ void updateMetaData(const EntityInstanceMetadata& metadata);
+
+
+ private:
QLabel* statusLabel = nullptr;
+ bool useTypeInfo = true;
+
+ QTreeWidgetItem* itemInstanceID;
+ QTreeWidgetItem* itemMetadata;
+ QTreeWidgetItem* itemData;
+ std::optional<EntityInstance> currentInstance;
+ aron::typenavigator::AronObjectTypeNavigatorPtr currentAronType = nullptr;
enum class Columns
{
KEY = 0,
- TYPE = 1,
- VALUE = 2,
+ VALUE = 1,
+ TYPE = 2,
};
};
diff --git a/source/RobotAPI/libraries/armem_gui/MemoryTreeWidget.cpp b/source/RobotAPI/libraries/armem_gui/MemoryTreeWidget.cpp
index cbea351b288a5f766326dbe6857752d5d6aa093d..f17ff107046cbe7bc304bada3368c05bae7e406a 100644
--- a/source/RobotAPI/libraries/armem_gui/MemoryTreeWidget.cpp
+++ b/source/RobotAPI/libraries/armem_gui/MemoryTreeWidget.cpp
@@ -7,7 +7,6 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronObjectTypeNavigator.h>
-#include <RobotAPI/libraries/armem_gui/QTreeWidgetAronDataVisitor.h>
namespace armarx::armem::gui
@@ -37,7 +36,14 @@ namespace armarx::armem::gui
header()->setTextElideMode(Qt::TextElideMode::ElideRight);
- connect(this, &QTreeWidget::currentItemChanged, this, &MemoryTreeWidget::handleSelection);
+ connect(this, &QTreeWidget::currentItemChanged, this, &This::handleSelection);
+
+ connect(this, &This::memorySelected, this, &This::itemSelected);
+ connect(this, &This::coreSegmentSelected, this, &This::itemSelected);
+ connect(this, &This::providerSegmentSelected, this, &This::itemSelected);
+ connect(this, &This::entitySelected, this, &This::itemSelected);
+ connect(this, &This::snapshotSelected, this, &This::itemSelected);
+ connect(this, &This::instanceSelected, this, &This::itemSelected);
}
void MemoryTreeWidget::initBuilders()
@@ -116,7 +122,6 @@ namespace armarx::armem::gui
instanceBuilder.setMakeItemFn([this](const EntityInstance & instance)
{
QTreeWidgetItem* item = makeItem("", instance);
- item->setData(int(Columns::ID), Qt::UserRole, QString::fromStdString(instance.id().str()));
return item;
});
instanceBuilder.setCompareFn([](const EntityInstance & lhs, QTreeWidgetItem * rhsItem)
@@ -145,19 +150,46 @@ namespace armarx::armem::gui
emit updated();
}
+ std::optional<MemoryID> MemoryTreeWidget::selectedID() const
+ {
+ return _selectedID;
+ }
+
void MemoryTreeWidget::handleSelection()
{
QTreeWidgetItem* item = this->currentItem();
- if (!item
- || item->data(int(Columns::LEVEL), Qt::UserRole).toString().toStdString() != EntityInstance().getLevelName())
+ if (!item)
{
return;
}
- QString itemData = item->data(int(Columns::ID), Qt::UserRole).toString();
- MemoryID instanceID(itemData.toStdString());
+ const std::string levelName = item->data(int(Columns::LEVEL), Qt::UserRole).toString().toStdString();
+ _selectedID = MemoryID(item->data(int(Columns::ID), Qt::UserRole).toString().toStdString());
- emit instanceSelected(instanceID);
+ if (levelName == Memory().getLevelName())
+ {
+ emit memorySelected(*_selectedID);
+ }
+ else if (levelName == CoreSegment().getLevelName())
+ {
+ emit coreSegmentSelected(*_selectedID);
+ }
+ else if (levelName == ProviderSegment().getLevelName())
+ {
+ emit providerSegmentSelected(*_selectedID);
+ }
+ else if (levelName == Entity().getLevelName())
+ {
+ emit entitySelected(*_selectedID);
+ }
+ else if (levelName == EntitySnapshot().getLevelName())
+ {
+ emit snapshotSelected(*_selectedID);
+ }
+ else if (levelName == EntityInstance().getLevelName())
+ {
+ emit instanceSelected(*_selectedID);
+ }
}
@@ -199,18 +231,7 @@ namespace armarx::armem::gui
void MemoryTreeWidget::updateChildren(const armem::EntityInstance& data, QTreeWidgetItem* dataItem)
{
- if (instanceIntrospection)
- {
- if (data.data())
- {
- QTreeWidgetAronDataVisitor visitor(dataItem);
- visitor.applyTo(*data.data());
- }
- else
- {
- dataItem->addChild(new QTreeWidgetItem(QStringList{"(No introspection available.)"}));
- }
- }
+ (void) data, (void) dataItem;
}
QTreeWidgetItem* MemoryTreeWidget::makeItem(const std::string& key, const armem::detail::MemoryItem& memoryItem)
@@ -225,6 +246,7 @@ namespace armarx::armem::gui
QTreeWidgetItem* item = new QTreeWidgetItem(columns);
item->setData(int(Columns::LEVEL), Qt::UserRole, QString::fromStdString(memoryItem.getLevelName()));
+ item->setData(int(Columns::ID), Qt::UserRole, QString::fromStdString(memoryItem.id().str()));
item->setTextAlignment(int(Columns::SIZE), Qt::AlignRight);
return item;
}
@@ -250,7 +272,17 @@ namespace armarx::armem::gui
std::string typeName;
if (container.aronType())
{
- typeName = container.aronType()->getName();
+ typeName = container.aronType()->getObjectName();
+
+ std::string del = "::";
+ size_t find = typeName.rfind(del);
+ if (find != typeName.npos)
+ {
+ find += del.size(); // include del
+ std::stringstream ss;
+ ss << typeName.substr(find) << " (" << typeName.substr(0, find) << ")";
+ typeName = ss.str();
+ }
}
else
{
diff --git a/source/RobotAPI/libraries/armem_gui/MemoryTreeWidget.h b/source/RobotAPI/libraries/armem_gui/MemoryTreeWidget.h
index 0aff62014b444997d67d1ce67517f95952cda734..4310f3e30cbf79b7b257d3a060c30e0b135a028c 100644
--- a/source/RobotAPI/libraries/armem_gui/MemoryTreeWidget.h
+++ b/source/RobotAPI/libraries/armem_gui/MemoryTreeWidget.h
@@ -17,6 +17,8 @@ namespace armarx::armem::gui
class MemoryTreeWidget : public QTreeWidget
{
Q_OBJECT
+ using This = MemoryTreeWidget;
+
public:
MemoryTreeWidget();
@@ -24,10 +26,20 @@ namespace armarx::armem::gui
void update(const armem::Memory& memory);
void update(const std::map<std::string, const armem::Memory*>& memories);
+ std::optional<MemoryID> selectedID() const;
+
signals:
void updated();
+
+ void itemSelected(const MemoryID& id);
+
+ void memorySelected(const MemoryID& id);
+ void coreSegmentSelected(const MemoryID& id);
+ void providerSegmentSelected(const MemoryID& id);
+ void entitySelected(const MemoryID& id);
+ void snapshotSelected(const MemoryID& id);
void instanceSelected(const MemoryID& id);
@@ -63,8 +75,6 @@ namespace armarx::armem::gui
private:
- bool instanceIntrospection = false;
-
MapTreeWidgetBuilder<std::string, const Memory*> memoryBuilder;
MapTreeWidgetBuilder<std::string, CoreSegment> coreSegmentBuilder;
MapTreeWidgetBuilder<std::string, ProviderSegment> provSegmentBuilder;
@@ -72,6 +82,7 @@ namespace armarx::armem::gui
MapTreeWidgetBuilder<armem::Time, EntitySnapshot> snapshotBuilder;
TreeWidgetBuilder<std::vector<EntityInstance>> instanceBuilder;
+ std::optional<MemoryID> _selectedID;
enum class Columns
{
diff --git a/source/RobotAPI/libraries/armem_gui/QTreeWidgetAronDataVisitor.cpp b/source/RobotAPI/libraries/armem_gui/QTreeWidgetAronDataVisitor.cpp
deleted file mode 100644
index c3d169ff240ed50fd45fcbd5c9456fafb974ba68..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_gui/QTreeWidgetAronDataVisitor.cpp
+++ /dev/null
@@ -1,69 +0,0 @@
-#include "QTreeWidgetAronDataVisitor.h"
-
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-
-namespace armarx::armem::gui
-{
-
- QTreeWidgetAronDataVisitor::QTreeWidgetAronDataVisitor()
- {
- }
-
- QTreeWidgetAronDataVisitor::QTreeWidgetAronDataVisitor(QTreeWidgetItem* root)
- {
- rootItems.push(root);
- }
-
- QTreeWidgetAronDataVisitor::~QTreeWidgetAronDataVisitor()
- {}
-
- bool QTreeWidgetAronDataVisitor::_visitEnter(const std::string& key, const std::string& type, size_t numChildren)
- {
- QTreeWidgetItem* item = nullptr;
- if (rootItems.size() > 0)
- {
- item = rootItems.top();
- rootItems.pop();
- }
- else
- {
- QStringList cols;
- cols.insert(0, QString::fromStdString(key));
- cols.insert(1, QString::fromStdString(type));
- cols.insert(2, QString::number(numChildren) + " items");
- item = new QTreeWidgetItem(cols);
- }
- items.push(item);
- return true;
- }
-
- bool QTreeWidgetAronDataVisitor::_visitExit()
- {
- ARMARX_CHECK_POSITIVE(items.size());
- QTreeWidgetItem* item = items.top();
- items.pop();
- item->setExpanded(true);
- if (items.size() > 0)
- {
- items.top()->addChild(item);
- }
- else
- {
- rootItems.push(item);
- }
- return true;
- }
-
- void QTreeWidgetAronDataVisitor::streamValueText(AronStringDataNavigator& n, std::stringstream& ss)
- {
- ss << "'" << n.getValue() << "'";
- }
-
- void QTreeWidgetAronDataVisitor::streamValueText(AronNDArrayDataNavigator& n, std::stringstream& ss)
- {
- ss << "shape (" << simox::alg::join(simox::alg::multi_to_string(n.getDimensions()), ", ") << ")";
- }
-
-
-}
diff --git a/source/RobotAPI/libraries/armem_gui/QTreeWidgetAronDataVisitor.h b/source/RobotAPI/libraries/armem_gui/QTreeWidgetAronDataVisitor.h
deleted file mode 100644
index 5d6c7667e636745aa57d1076563eda1c19f01685..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_gui/QTreeWidgetAronDataVisitor.h
+++ /dev/null
@@ -1,124 +0,0 @@
-#pragma once
-
-#include <sstream>
-#include <stack>
-
-#include <QTreeWidget>
-#include <QLabel>
-
-#include <RobotAPI/libraries/aron/aroncore/navigators/visitors/AronDataVisitor.h>
-
-
-namespace armarx::armem::gui
-{
-
-
-
- struct QTreeWidgetAronDataVisitor : public aron::visitor::AronDataVisitor
- {
- std::stack<QTreeWidgetItem*> rootItems;
- std::stack<QTreeWidgetItem*> items;
-
-
- QTreeWidgetAronDataVisitor();
- QTreeWidgetAronDataVisitor(QTreeWidgetItem* root);
- virtual ~QTreeWidgetAronDataVisitor() override;
-
-
- bool visitEnter(const std::string& key, AronDictDataNavigator& n) override
- {
- return _visitEnter(key, "Dict", n.childrenSize());
- }
- bool visitExit(AronDictDataNavigator&) override
- {
- return _visitExit();
- }
-
- bool visitEnter(const std::string& key, AronListDataNavigator& n) override
- {
- return _visitEnter(key, "List", n.childrenSize());
- }
- bool visitExit(AronListDataNavigator&) override
- {
- return _visitExit();
- }
-
- // Same for Dict and List
- bool _visitEnter(const std::string& key, const std::string& type, size_t numChildren);
- bool _visitExit();
-
-
- bool visit(const std::string& key, AronBoolDataNavigator& b) override
- {
- this->addValueRow(key, b, "Bool");
- return true;
- }
- bool visit(const std::string& key, AronDoubleDataNavigator& d) override
- {
- this->addValueRow(key, d, "Double");
- return true;
- }
- bool visit(const std::string& key, AronFloatDataNavigator& f) override
- {
- this->addValueRow(key, f, "Float");
- return true;
- }
- bool visit(const std::string& key, AronIntDataNavigator& i) override
- {
- this->addValueRow(key, i, "Int");
- return true;
- }
- bool visit(const std::string& key, AronLongDataNavigator& l) override
- {
- this->addValueRow(key, l, "Long");
- return true;
- }
- bool visit(const std::string& key, AronStringDataNavigator& string) override
- {
- this->addValueRow(key, string, "String");
- return true;
- }
-
- bool visit(const std::string& key, AronNDArrayDataNavigator& array) override
- {
- this->addValueRow(key, array, "ND Array");
- return true;
- }
-
-
- private:
-
- template <class Navigator>
- void addValueRow(const std::string& key, Navigator& n, const std::string& typeName)
- {
- if (items.size() > 0)
- {
- items.top()->addChild(new QTreeWidgetItem(this->makeValueRowStrings(key, n, typeName)));
- }
- }
-
- template <class Navigator>
- QStringList makeValueRowStrings(const std::string& key, Navigator& n, const std::string& typeName)
- {
- std::stringstream value;
- streamValueText(n, value);
- return
- {
- QString::fromStdString(key),
- QString::fromStdString(typeName),
- QString::fromStdString(value.str()),
- };
- }
-
- template <class Navigator>
- void streamValueText(Navigator& n, std::stringstream& ss)
- {
- ss << n.getValue();
- }
- void streamValueText(AronStringDataNavigator& n, std::stringstream& ss);
- void streamValueText(AronNDArrayDataNavigator& n, std::stringstream& ss);
-
- };
-
-
-}
diff --git a/source/RobotAPI/libraries/armem_gui/gui_utils.cpp b/source/RobotAPI/libraries/armem_gui/gui_utils.cpp
index b914d624236852e05796d009d3077892eed48d43..1bbfe2dc065129bdc65d35dd09b27856b2a70742 100644
--- a/source/RobotAPI/libraries/armem_gui/gui_utils.cpp
+++ b/source/RobotAPI/libraries/armem_gui/gui_utils.cpp
@@ -3,6 +3,7 @@
#include <QLayout>
#include <QLayoutItem>
#include <QWidget>
+#include <QTreeWidgetItem>
namespace armarx
@@ -28,5 +29,12 @@ namespace armarx
}
}
-
+ void gui::clearItem(QTreeWidgetItem* item)
+ {
+ while (item->childCount() > 0)
+ {
+ delete item->takeChild(0);
+ }
+ }
}
+
diff --git a/source/RobotAPI/libraries/armem_gui/gui_utils.h b/source/RobotAPI/libraries/armem_gui/gui_utils.h
index eabcffb765798175b93a1376305f01e868d6ecf4..23824173121dda4dce002e4185a94601fa739beb 100644
--- a/source/RobotAPI/libraries/armem_gui/gui_utils.h
+++ b/source/RobotAPI/libraries/armem_gui/gui_utils.h
@@ -2,6 +2,7 @@
class QLayout;
+class QTreeWidgetItem;
namespace armarx::gui
@@ -15,4 +16,13 @@ namespace armarx::gui
*/
void clearLayout(QLayout* layout);
+ /**
+ * @brief Clear a tree widget item
+ *
+ * Take and delete all children of `item`.
+ *
+ * @param item The tree widget item.
+ */
+ void clearItem(QTreeWidgetItem* item);
+
}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/InstanceTreeDataVisitor.cpp b/source/RobotAPI/libraries/armem_gui/instance/InstanceTreeDataVisitor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d92450283599807b2b4d35531bccc113c0906f54
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/InstanceTreeDataVisitor.cpp
@@ -0,0 +1,9 @@
+#include "InstanceTreeDataVisitor.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+
+namespace armarx::armem::gui
+{
+
+}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/InstanceTreeDataVisitor.h b/source/RobotAPI/libraries/armem_gui/instance/InstanceTreeDataVisitor.h
new file mode 100644
index 0000000000000000000000000000000000000000..2ceb790caf10591bb0260a52f83445f4458fa84f
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/InstanceTreeDataVisitor.h
@@ -0,0 +1,78 @@
+#pragma once
+
+#include <sstream>
+#include <stack>
+
+#include <QTreeWidget>
+#include <QLabel>
+
+#include <RobotAPI/libraries/aron/aroncore/navigators/visitors/DataVisitor.h>
+
+#include "InstanceTreeDataVisitorBase.h"
+
+
+namespace armarx::armem::gui
+{
+
+ class InstanceTreeDataVisitor :
+ public aron::visitor::DataVisitor,
+ public InstanceTreeDataVisitorBase
+ {
+ public:
+
+ using InstanceTreeDataVisitorBase::InstanceTreeDataVisitorBase;
+
+
+ bool visitEnter(const std::string& key, DictDataNavigator& n) override
+ {
+ return _visitEnter(key, "Dict", n.childrenSize());
+ }
+ bool visitExit(DictDataNavigator&) override
+ {
+ return _visitExit();
+ }
+
+ bool visitEnter(const std::string& key, ListDataNavigator& n) override
+ {
+ return _visitEnter(key, "List", n.childrenSize());
+ }
+ bool visitExit(ListDataNavigator&) override
+ {
+ return _visitExit();
+ }
+
+
+ bool visit(const std::string& key, BoolDataNavigator& b) override
+ {
+ return this->addValueRow(key, b, "Bool");
+ }
+ bool visit(const std::string& key, DoubleDataNavigator& d) override
+ {
+ return this->addValueRow(key, d, "Double");
+ }
+ bool visit(const std::string& key, FloatDataNavigator& f) override
+ {
+ return this->addValueRow(key, f, "Float");
+ }
+ bool visit(const std::string& key, IntDataNavigator& i) override
+ {
+ return this->addValueRow(key, i, "Int");
+ }
+ bool visit(const std::string& key, LongDataNavigator& l) override
+ {
+ return this->addValueRow(key, l, "Long");
+ }
+ bool visit(const std::string& key, StringDataNavigator& string) override
+ {
+ return this->addValueRow(key, string, "String");
+ }
+
+ bool visit(const std::string& key, NDArrayDataNavigator& array) override
+ {
+ return this->addValueRow(key, array, "ND Array");
+ }
+
+ };
+
+
+}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/InstanceTreeDataVisitorBase.cpp b/source/RobotAPI/libraries/armem_gui/instance/InstanceTreeDataVisitorBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7784106311912e98e924745632a9fdfcc18c1cf8
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/InstanceTreeDataVisitorBase.cpp
@@ -0,0 +1,75 @@
+#include "InstanceTreeDataVisitorBase.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+
+namespace armarx::armem::gui
+{
+
+ InstanceTreeDataVisitorBase::InstanceTreeDataVisitorBase()
+ {
+ }
+
+ InstanceTreeDataVisitorBase::InstanceTreeDataVisitorBase(QTreeWidgetItem* root)
+ {
+ rootItems.push(root);
+ }
+
+ InstanceTreeDataVisitorBase::~InstanceTreeDataVisitorBase()
+ {}
+
+ void InstanceTreeDataVisitorBase::setColumns(int key, int value, int type)
+ {
+ this->columnKey = key;
+ this->columnType = type;
+ this->columnValue = value;
+ }
+
+ bool InstanceTreeDataVisitorBase::_visitEnter(const std::string& key, const std::string& type, size_t numChildren)
+ {
+ QTreeWidgetItem* item = nullptr;
+ if (rootItems.size() > 0)
+ {
+ item = rootItems.top();
+ rootItems.pop();
+ }
+ else
+ {
+ QStringList cols;
+ cols.insert(columnKey, QString::fromStdString(key));
+ cols.insert(columnValue, QString::number(numChildren) + " items");
+ cols.insert(columnType, QString::fromStdString(type));
+ item = new QTreeWidgetItem(cols);
+ }
+ items.push(item);
+ return true;
+ }
+
+ bool InstanceTreeDataVisitorBase::_visitExit()
+ {
+ ARMARX_CHECK_POSITIVE(items.size());
+ QTreeWidgetItem* item = items.top();
+ items.pop();
+ item->setExpanded(true);
+ if (items.size() > 0)
+ {
+ items.top()->addChild(item);
+ }
+ else
+ {
+ rootItems.push(item);
+ }
+ return true;
+ }
+
+ void InstanceTreeDataVisitorBase::streamValueText(aron::visitor::DataVisitor::StringDataNavigator& n, std::stringstream& ss)
+ {
+ ss << "'" << n.getValue() << "'";
+ }
+
+ void InstanceTreeDataVisitorBase::streamValueText(aron::visitor::DataVisitor::NDArrayDataNavigator& n, std::stringstream& ss)
+ {
+ ss << "shape (" << simox::alg::join(simox::alg::multi_to_string(n.getDimensions()), ", ") << ")";
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/InstanceTreeDataVisitorBase.h b/source/RobotAPI/libraries/armem_gui/instance/InstanceTreeDataVisitorBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..fd45d515e2bb4e71a165dd173a3d63fd8980d763
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/InstanceTreeDataVisitorBase.h
@@ -0,0 +1,75 @@
+#pragma once
+
+#include <sstream>
+#include <stack>
+
+#include <QTreeWidget>
+#include <QLabel>
+
+#include <RobotAPI/libraries/aron/aroncore/navigators/visitors/DataVisitor.h>
+
+
+namespace armarx::armem::gui
+{
+
+ class InstanceTreeDataVisitorBase
+ {
+ public:
+
+ InstanceTreeDataVisitorBase();
+ InstanceTreeDataVisitorBase(QTreeWidgetItem* root);
+ virtual ~InstanceTreeDataVisitorBase();
+
+ void setColumns(int key, int value, int type);
+
+
+ protected:
+
+ // Same for Dict and List
+ bool _visitEnter(const std::string& key, const std::string& type, size_t numChildren);
+ bool _visitExit();
+
+
+ template <class Navigator>
+ bool addValueRow(const std::string& key, Navigator& n, const std::string& typeName)
+ {
+ if (items.size() > 0)
+ {
+ items.top()->addChild(new QTreeWidgetItem(this->makeValueRowStrings(key, n, typeName)));
+ }
+ return true;
+ }
+
+ template <class Navigator>
+ QStringList makeValueRowStrings(const std::string& key, Navigator& n, const std::string& typeName)
+ {
+ std::stringstream value;
+ streamValueText(n, value);
+ QStringList cols;
+ cols.insert(columnKey, QString::fromStdString(key));
+ cols.insert(columnValue, QString::fromStdString(value.str()));
+ cols.insert(columnType, QString::fromStdString(typeName));
+ return cols;
+ }
+
+ template <class Navigator>
+ void streamValueText(Navigator& n, std::stringstream& ss)
+ {
+ ss << n.getValue();
+ }
+ void streamValueText(aron::visitor::DataVisitor::StringDataNavigator& n, std::stringstream& ss);
+ void streamValueText(aron::visitor::DataVisitor::NDArrayDataNavigator& n, std::stringstream& ss);
+
+
+ public:
+
+ std::stack<QTreeWidgetItem*> rootItems;
+ std::stack<QTreeWidgetItem*> items;
+
+ int columnKey = 0;
+ int columnValue = 1;
+ int columnType = 2;
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/InstanceTreeTypedDataVisitor.cpp b/source/RobotAPI/libraries/armem_gui/instance/InstanceTreeTypedDataVisitor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d7f8169b940499ac297bcd6e27e16d4c2f411333
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/InstanceTreeTypedDataVisitor.cpp
@@ -0,0 +1,10 @@
+#include "InstanceTreeTypedDataVisitor.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+
+namespace armarx::armem::gui
+{
+
+
+}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/InstanceTreeTypedDataVisitor.h b/source/RobotAPI/libraries/armem_gui/instance/InstanceTreeTypedDataVisitor.h
new file mode 100644
index 0000000000000000000000000000000000000000..e55b3eb5eaa359817526a943558daa20af6645b3
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/InstanceTreeTypedDataVisitor.h
@@ -0,0 +1,107 @@
+#pragma once
+
+#include <sstream>
+#include <stack>
+
+#include <QTreeWidget>
+#include <QLabel>
+
+#include <RobotAPI/libraries/aron/aroncore/navigators/visitors/TypedDataVisitor.h>
+#include "InstanceTreeDataVisitorBase.h"
+
+
+namespace armarx::armem::gui
+{
+
+ class InstanceTreeTypedDataVisitor :
+ public aron::visitor::TypedDataVisitor,
+ public InstanceTreeDataVisitorBase
+ {
+ public:
+
+ using InstanceTreeDataVisitorBase::InstanceTreeDataVisitorBase;
+
+
+ bool visitEnter(DictTypeNavigator& type, const std::string& key, DictDataNavigator& data) override
+ {
+ return _visitEnter(key, type.getName(), data.childrenSize());
+ }
+ bool visitExit(DictTypeNavigator&, DictDataNavigator&) override
+ {
+ return _visitExit();
+ }
+
+ bool visitEnter(ObjectTypeNavigator& type, const std::string& key, DictDataNavigator& data) override
+ {
+ return _visitEnter(key, type.getName(), data.childrenSize());
+ }
+ bool visitExit(ObjectTypeNavigator&, DictDataNavigator&) override
+ {
+ return _visitExit();
+ }
+
+ bool visitEnter(ListTypeNavigator& type, const std::string& key, ListDataNavigator& data) override
+ {
+ return _visitEnter(key, type.getName(), data.childrenSize());
+ }
+ bool visitExit(ListTypeNavigator&, ListDataNavigator&) override
+ {
+ return _visitExit();
+ }
+
+ bool visitEnter(TupleTypeNavigator& type, const std::string& key, ListDataNavigator& data) override
+ {
+ return _visitEnter(key, type.getName(), data.childrenSize());
+ }
+ bool visitExit(TupleTypeNavigator&, ListDataNavigator&) override
+ {
+ return _visitExit();
+ }
+
+
+ bool visit(BoolTypeNavigator& type, const std::string& key, BoolDataNavigator& data) override
+ {
+ return this->addValueRow(key, data, type.getName());
+ }
+ bool visit(DoubleTypeNavigator& type, const std::string& key, DoubleDataNavigator& data) override
+ {
+ return this->addValueRow(key, data, type.getName());
+ }
+ bool visit(FloatTypeNavigator& type, const std::string& key, FloatDataNavigator& data) override
+ {
+ return this->addValueRow(key, data, type.getName());
+ }
+ bool visit(IntTypeNavigator& type, const std::string& key, IntDataNavigator& data) override
+ {
+ return this->addValueRow(key, data, type.getName());
+ }
+ bool visit(LongTypeNavigator& type, const std::string& key, LongDataNavigator& data) override
+ {
+ return this->addValueRow(key, data, type.getName());
+ }
+ bool visit(StringTypeNavigator& type, const std::string& key, StringDataNavigator& data) override
+ {
+ return this->addValueRow(key, data, type.getName());
+ }
+
+
+ bool visit(EigenMatrixTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data) override
+ {
+ return this->addValueRow(key, data, type.getName());
+ }
+ bool visit(IVTCByteImageTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data) override
+ {
+ return this->addValueRow(key, data, type.getName());
+ }
+ bool visit(OpenCVMatTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data) override
+ {
+ return this->addValueRow(key, data, type.getName());
+ }
+ bool visit(PCLPointCloudTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data) override
+ {
+ return this->addValueRow(key, data, type.getName());
+ }
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/aron/aroncore/CMakeLists.txt b/source/RobotAPI/libraries/aron/aroncore/CMakeLists.txt
index ca62febd315774ce184005fdc018a5f1187329f0..fb75e9b7c50bbdd4d6769d3a371a3c47b24a7e9a 100644
--- a/source/RobotAPI/libraries/aron/aroncore/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/aroncore/CMakeLists.txt
@@ -47,7 +47,8 @@ set(LIB_FILES
navigators/typenavigator/AronPrimitiveTypeNavigator.cpp
navigators/typenavigator/AronTypeNavigatorFactory.cpp
- navigators/visitors/AronDataVisitor.cpp
+ navigators/visitors/DataVisitor.cpp
+ navigators/visitors/TypedDataVisitor.cpp
io/AronDataIO/classWriters/NavigatorWriter/AronDataNavigatorWriter.cpp
io/AronDataIO/classWriters/NlohmannJSONWriter/AronDataNlohmannJSONWriter.cpp
@@ -122,7 +123,8 @@ set(LIB_HEADERS
navigators/typenavigator/AronPrimitiveTypeNavigator.h
navigators/typenavigator/AronTypeNavigatorFactory.h
- navigators/visitors/AronDataVisitor.h
+ navigators/visitors/DataVisitor.h
+ navigators/visitors/TypedDataVisitor.h
io/AronReaderWriter.h
io/AronReaderWriterToken.h
diff --git a/source/RobotAPI/libraries/aron/aroncore/codegenerator/typeReaders/xmlReader/AronTypeXMLReaderFactory.cpp b/source/RobotAPI/libraries/aron/aroncore/codegenerator/typeReaders/xmlReader/AronTypeXMLReaderFactory.cpp
index 385f92212cce8e30735cdd3139addb674726d2e8..f2ac4d541a8dcf34d052bc9e8604f943abe87fbe 100644
--- a/source/RobotAPI/libraries/aron/aroncore/codegenerator/typeReaders/xmlReader/AronTypeXMLReaderFactory.cpp
+++ b/source/RobotAPI/libraries/aron/aroncore/codegenerator/typeReaders/xmlReader/AronTypeXMLReaderFactory.cpp
@@ -44,25 +44,24 @@ namespace armarx
std::stack<AronGenerateTypeInfoPtr> AronTypeXMLReaderFactory::TheObjectWeAreGoingToGenerateNowStack = {};
std::map<std::string, AronGenerateTypeInfoPtr> AronTypeXMLReaderFactory::AllKnownGeneratedPublicObjects = {};
- const std::map<std::string, AronTypeXMLReaderFactoryPtr> AronTypeXMLReaderFactory::Factories =
+
+ typenavigator::AronTypeNavigatorPtr AronTypeXMLReaderFactory::create(const RapidXmlReaderNode& node, const AronPath& path) const
{
#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
- {AronTypeXMLReaderData::ARON_GENERATE_##capsType##_TAG, AronTypeXMLReaderFactoryPtr(new Aron##upperType##TypeXMLReaderFactory())}, \
-
- HANDLE_CONTAINER_TYPES
- HANDLE_COMPLEX_TYPES
- HANDLE_PRIMITIVE_TYPES
+ {AronTypeXMLReaderData::ARON_GENERATE_##capsType##_TAG, AronTypeXMLReaderFactoryPtr(new Aron##upperType##TypeXMLReaderFactory())},
+ static const std::map<std::string, AronTypeXMLReaderFactoryPtr> Factories =
+ {
+ HANDLE_CONTAINER_TYPES
+ HANDLE_COMPLEX_TYPES
+ HANDLE_PRIMITIVE_TYPES
+ };
#undef RUN_ARON_MACRO
- };
- typenavigator::AronTypeNavigatorPtr AronTypeXMLReaderFactory::create(const RapidXmlReaderNode& node, const AronPath& path) const
- {
const std::string tag = simox::alg::to_lower(node.name());
if (tag == "")
{
throw exception::AronException("AronTypeXMLReaderFactory", "create", "An node is empty. Cannot find facory for empty nodes.");
}
-
auto factory_iterator = Factories.find(tag);
if (factory_iterator != Factories.end())
{
diff --git a/source/RobotAPI/libraries/aron/aroncore/codegenerator/typeReaders/xmlReader/AronTypeXMLReaderFactory.h b/source/RobotAPI/libraries/aron/aroncore/codegenerator/typeReaders/xmlReader/AronTypeXMLReaderFactory.h
index df781306e704a3cdef7e1a41e50bf3d6763d1f42..8347da4c914e1058b3c8be10f9b80417b31966a9 100644
--- a/source/RobotAPI/libraries/aron/aroncore/codegenerator/typeReaders/xmlReader/AronTypeXMLReaderFactory.h
+++ b/source/RobotAPI/libraries/aron/aroncore/codegenerator/typeReaders/xmlReader/AronTypeXMLReaderFactory.h
@@ -68,9 +68,6 @@ namespace armarx
protected:
static std::stack<AronGenerateTypeInfoPtr> TheObjectWeAreGoingToGenerateNowStack;
static std::map<std::string, AronGenerateTypeInfoPtr> AllKnownGeneratedPublicObjects;
-
- private:
- static const std::map<std::string, AronTypeXMLReaderFactoryPtr> Factories;
};
#define RUN_ARON_MACRO(upperType, lowerType, capsType) \
diff --git a/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronDictTypeNavigator.cpp b/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronDictTypeNavigator.cpp
index 997d1bf8cf73dda37bce8938d202e621188c659b..6cb665fd736af4fb3b6aa6e28541d4f796ed11fe 100644
--- a/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronDictTypeNavigator.cpp
+++ b/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronDictTypeNavigator.cpp
@@ -46,6 +46,8 @@ namespace armarx
type(o)
{
CheckAronPtrForNull("AronDictTypeNavigator", "AronDictTypeNavigator", getPath(), o);
+
+ acceptedTypeNavigator = FromAronType(o->acceptedType);
}
void AronDictTypeNavigator::setAcceptedType(const AronTypeNavigatorPtr& a)
diff --git a/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronListTypeNavigator.cpp b/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronListTypeNavigator.cpp
index d77033eccc99b96ea2c57db99f36179c2a7b9b75..98f6f7364a94546b21afebad5c3fc86c04189af0 100644
--- a/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronListTypeNavigator.cpp
+++ b/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronListTypeNavigator.cpp
@@ -44,6 +44,8 @@ namespace armarx
type(o)
{
CheckAronPtrForNull("AronListTypeNavigator", "AronListTypeNavigator", getPath(), o);
+
+ acceptedTypeNavigator = FromAronType(o->acceptedType);
}
// static methods
diff --git a/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronObjectTypeNavigator.cpp b/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronObjectTypeNavigator.cpp
index 63b13137ed73427745069b8032f8f03488a8ccff..9d483dd69ab02a49a1a8ea95bfe93a6010ee6807 100644
--- a/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronObjectTypeNavigator.cpp
+++ b/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronObjectTypeNavigator.cpp
@@ -46,6 +46,11 @@ namespace armarx
type(o)
{
CheckAronPtrForNull("AronObjectTypeNavigator", "AronObjectTypeNavigator", getPath(), o);
+
+ for(const auto& [key, t] : o->elementTypes)
+ {
+ acceptedTypeNavigators[key] = FromAronType(t);
+ }
}
// static methods
diff --git a/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronTupleTypeNavigator.cpp b/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronTupleTypeNavigator.cpp
index a58ea955e6b80287c79934bd6a73486c67f99a4a..9f9d44403a7159ecc4746e027cee37ac1cd9cb6c 100644
--- a/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronTupleTypeNavigator.cpp
+++ b/source/RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronTupleTypeNavigator.cpp
@@ -45,6 +45,11 @@ namespace armarx
type(o)
{
CheckAronPtrForNull("AronTupleTypeNavigator", "AronTupleTypeNavigator", getPath(), o);
+
+ for (const auto& t : type->elementTypes)
+ {
+ acceptedTypeNavigators.push_back(FromAronType(t));
+ }
}
type::AronTupleTypePtr AronTupleTypeNavigator::toAronTupleTypePtr() const
diff --git a/source/RobotAPI/libraries/aron/aroncore/navigators/visitors/AronDataVisitor.cpp b/source/RobotAPI/libraries/aron/aroncore/navigators/visitors/DataVisitor.cpp
similarity index 62%
rename from source/RobotAPI/libraries/aron/aroncore/navigators/visitors/AronDataVisitor.cpp
rename to source/RobotAPI/libraries/aron/aroncore/navigators/visitors/DataVisitor.cpp
index 42f8912de06188a929790d310069f16bb62cc8eb..4715af4f46adba0d69ee0f1b7212930a21d3c866 100644
--- a/source/RobotAPI/libraries/aron/aroncore/navigators/visitors/AronDataVisitor.cpp
+++ b/source/RobotAPI/libraries/aron/aroncore/navigators/visitors/DataVisitor.cpp
@@ -1,4 +1,4 @@
-#include "AronDataVisitor.h"
+#include "DataVisitor.h"
#include <SimoxUtility/meta/type_name.h>
@@ -9,27 +9,27 @@ namespace armarx::aron::visitor
{
- AronDataVisitor::AronDataVisitor()
+ DataVisitor::DataVisitor()
{
}
- AronDataVisitor::~AronDataVisitor()
+ DataVisitor::~DataVisitor()
{
}
- bool AronDataVisitor::applyTo(data::AronDataPtr& data)
+ bool DataVisitor::applyTo(data::AronDataPtr& data)
{
ARMARX_CHECK_NOT_NULL(data);
- return applyTo(*AronDataNavigator::FromAronData(data));
+ return applyTo(*DataNavigator::FromAronData(data));
}
- bool AronDataVisitor::applyTo(AronDataVisitor::AronDataNavigator& navigator)
+ bool DataVisitor::applyTo(DataVisitor::DataNavigator& navigator)
{
return this->applyTo("", navigator);
}
- bool AronDataVisitor::applyTo(const std::string& key, AronDataNavigator& navigator)
+ bool DataVisitor::applyTo(const std::string& key, DataNavigator& navigator)
{
auto handleCollection = [this, key](auto & n)
{
@@ -48,11 +48,11 @@ namespace armarx::aron::visitor
return true;
};
- if (auto n = dynamic_cast<AronDictDataNavigator*>(&navigator))
+ if (auto n = dynamic_cast<DictDataNavigator*>(&navigator))
{
return handleCollection(*n);
}
- else if (auto n = dynamic_cast<AronListDataNavigator*>(&navigator))
+ else if (auto n = dynamic_cast<ListDataNavigator*>(&navigator))
{
return handleCollection(*n);
}
@@ -60,38 +60,38 @@ namespace armarx::aron::visitor
ARMARX_CHECK_EQUAL(navigator.childrenSize(), 0)
<< simox::meta::get_type_name(navigator);
- if (auto n = dynamic_cast<AronBoolDataNavigator*>(&navigator))
+ if (auto n = dynamic_cast<BoolDataNavigator*>(&navigator))
{
return visit(key, *n);
}
- else if (auto n = dynamic_cast<AronDoubleDataNavigator*>(&navigator))
+ else if (auto n = dynamic_cast<DoubleDataNavigator*>(&navigator))
{
return visit(key, *n);
}
- else if (auto n = dynamic_cast<AronFloatDataNavigator*>(&navigator))
+ else if (auto n = dynamic_cast<FloatDataNavigator*>(&navigator))
{
return visit(key, *n);
}
- else if (auto n = dynamic_cast<AronIntDataNavigator*>(&navigator))
+ else if (auto n = dynamic_cast<IntDataNavigator*>(&navigator))
{
return visit(key, *n);
}
- else if (auto n = dynamic_cast<AronLongDataNavigator*>(&navigator))
+ else if (auto n = dynamic_cast<LongDataNavigator*>(&navigator))
{
return visit(key, *n);
}
- else if (auto n = dynamic_cast<AronNDArrayDataNavigator*>(&navigator))
+ else if (auto n = dynamic_cast<NDArrayDataNavigator*>(&navigator))
{
return visit(key, *n);
}
- else if (auto n = dynamic_cast<AronStringDataNavigator*>(&navigator))
+ else if (auto n = dynamic_cast<StringDataNavigator*>(&navigator))
{
return visit(key, *n);
}
ARMARX_CHECK(false) << "Unhandled AronDataNavigatorType '" << simox::meta::get_type_name(navigator) << "'.";
}
- bool AronDataVisitor::applyToChildren(AronDataVisitor::AronDictDataNavigator& navigator)
+ bool DataVisitor::applyToChildren(DictDataNavigator& navigator)
{
for (const std::string& key : navigator.getAllKeys())
{
@@ -107,7 +107,7 @@ namespace armarx::aron::visitor
return true;
}
- bool AronDataVisitor::applyToChildren(AronDataVisitor::AronListDataNavigator& navigator)
+ bool DataVisitor::applyToChildren(ListDataNavigator& navigator)
{
size_t i = 0;
for (auto& child : navigator.getChildren())
diff --git a/source/RobotAPI/libraries/aron/aroncore/navigators/visitors/AronDataVisitor.h b/source/RobotAPI/libraries/aron/aroncore/navigators/visitors/DataVisitor.h
similarity index 54%
rename from source/RobotAPI/libraries/aron/aroncore/navigators/visitors/AronDataVisitor.h
rename to source/RobotAPI/libraries/aron/aroncore/navigators/visitors/DataVisitor.h
index ec5527e2e51ad23a040768943e000746ec651c9f..a3c37ac2d02fb5274bd65b0b42a39b1d6ee4b397 100644
--- a/source/RobotAPI/libraries/aron/aroncore/navigators/visitors/AronDataVisitor.h
+++ b/source/RobotAPI/libraries/aron/aroncore/navigators/visitors/DataVisitor.h
@@ -1,8 +1,8 @@
#pragma once
-#include <optional>
+#include <string>
-#include "../datanavigator/AronAllDataNavigators.h"
+#include <RobotAPI/libraries/aron/aroncore/navigators/datanavigator/AronAllDataNavigators.h>
namespace armarx::aron::visitor
@@ -34,148 +34,148 @@ namespace armarx::aron::visitor
* @see `aron::datanavigator::AronDataNavigator`
* @see `std::stack` To manage data with stack semantics.
*/
- class AronDataVisitor
+ class DataVisitor
{
public:
// Define types to ease writing code in this and derived classes.
- using AronDataNavigator = datanavigator::AronDataNavigator;
+ using DataNavigator = datanavigator::AronDataNavigator;
- using AronDictDataNavigator = datanavigator::AronDictDataNavigator;
- using AronListDataNavigator = datanavigator::AronListDataNavigator;
+ using DictDataNavigator = datanavigator::AronDictDataNavigator;
+ using ListDataNavigator = datanavigator::AronListDataNavigator;
- using AronBoolDataNavigator = datanavigator::AronBoolDataNavigator;
- using AronDoubleDataNavigator = datanavigator::AronDoubleDataNavigator;
- using AronFloatDataNavigator = datanavigator::AronFloatDataNavigator;
- using AronIntDataNavigator = datanavigator::AronIntDataNavigator;
- using AronLongDataNavigator = datanavigator::AronLongDataNavigator;
- using AronNDArrayDataNavigator = datanavigator::AronNDArrayDataNavigator;
- using AronStringDataNavigator = datanavigator::AronStringDataNavigator;
+ using BoolDataNavigator = datanavigator::AronBoolDataNavigator;
+ using DoubleDataNavigator = datanavigator::AronDoubleDataNavigator;
+ using FloatDataNavigator = datanavigator::AronFloatDataNavigator;
+ using IntDataNavigator = datanavigator::AronIntDataNavigator;
+ using LongDataNavigator = datanavigator::AronLongDataNavigator;
+ using NDArrayDataNavigator = datanavigator::AronNDArrayDataNavigator;
+ using StringDataNavigator = datanavigator::AronStringDataNavigator;
public:
- AronDataVisitor();
- virtual ~AronDataVisitor();
+ DataVisitor();
+ virtual ~DataVisitor();
bool applyTo(data::AronDataPtr& aronData);
- bool applyTo(AronDataNavigator& navigator);
- bool applyTo(const std::string& key, AronDataNavigator& navigator);
+ bool applyTo(DataNavigator& navigator);
+ bool applyTo(const std::string& key, DataNavigator& navigator);
- virtual bool visitEnter(AronDictDataNavigator& dict)
+ virtual bool visitEnter(DictDataNavigator& dict)
{
(void) dict;
return true;
}
- virtual bool visitExit(AronDictDataNavigator& dict)
+ virtual bool visitExit(DictDataNavigator& dict)
{
(void) dict;
return true;
}
- virtual bool visitEnter(AronListDataNavigator& list)
+ virtual bool visitEnter(ListDataNavigator& list)
{
(void) list;
return true;
}
- virtual bool visitExit(AronListDataNavigator& list)
+ virtual bool visitExit(ListDataNavigator& list)
{
(void) list;
return true;
}
- virtual bool visit(AronBoolDataNavigator& b)
+ virtual bool visit(BoolDataNavigator& b)
{
(void) b;
return true;
}
- virtual bool visit(AronDoubleDataNavigator& d)
+ virtual bool visit(DoubleDataNavigator& d)
{
(void) d;
return true;
}
- virtual bool visit(AronFloatDataNavigator& f)
+ virtual bool visit(FloatDataNavigator& f)
{
(void) f;
return true;
}
- virtual bool visit(AronIntDataNavigator& i)
+ virtual bool visit(IntDataNavigator& i)
{
(void) i;
return true;
}
- virtual bool visit(AronLongDataNavigator& l)
+ virtual bool visit(LongDataNavigator& l)
{
(void) l;
return true;
}
- virtual bool visit(AronNDArrayDataNavigator& array)
+ virtual bool visit(NDArrayDataNavigator& array)
{
(void) array;
return true;
}
- virtual bool visit(AronStringDataNavigator& string)
+ virtual bool visit(StringDataNavigator& string)
{
(void) string;
return true;
}
- virtual bool visitEnter(const std::string& key, AronDictDataNavigator& dict)
+ virtual bool visitEnter(const std::string& key, DictDataNavigator& dict)
{
(void) key;
return visitEnter(dict);
}
- virtual bool visitExit(const std::string& key, AronDictDataNavigator& dict)
+ virtual bool visitExit(const std::string& key, DictDataNavigator& dict)
{
(void) key;
return visitExit(dict);
}
- virtual bool visitEnter(const std::string& key, AronListDataNavigator& list)
+ virtual bool visitEnter(const std::string& key, ListDataNavigator& list)
{
(void) key;
return visitEnter(list);
}
- virtual bool visitExit(const std::string& key, AronListDataNavigator& list)
+ virtual bool visitExit(const std::string& key, ListDataNavigator& list)
{
(void) key;
return visitExit(list);
}
- virtual bool visit(const std::string& key, AronBoolDataNavigator& b)
+ virtual bool visit(const std::string& key, BoolDataNavigator& b)
{
(void) key;
return visit(b);
}
- virtual bool visit(const std::string& key, AronDoubleDataNavigator& d)
+ virtual bool visit(const std::string& key, DoubleDataNavigator& d)
{
(void) key;
return visit(d);
}
- virtual bool visit(const std::string& key, AronFloatDataNavigator& f)
+ virtual bool visit(const std::string& key, FloatDataNavigator& f)
{
(void) key;
return visit(f);
}
- virtual bool visit(const std::string& key, AronIntDataNavigator& i)
+ virtual bool visit(const std::string& key, IntDataNavigator& i)
{
(void) key;
return visit(i);
}
- virtual bool visit(const std::string& key, AronLongDataNavigator& l)
+ virtual bool visit(const std::string& key, LongDataNavigator& l)
{
(void) key;
return visit(l);
}
- virtual bool visit(const std::string& key, AronNDArrayDataNavigator& array)
+ virtual bool visit(const std::string& key, NDArrayDataNavigator& array)
{
(void) key;
return visit(array);
}
- virtual bool visit(const std::string& key, AronStringDataNavigator& string)
+ virtual bool visit(const std::string& key, StringDataNavigator& string)
{
(void) key;
return visit(string);
@@ -184,8 +184,8 @@ namespace armarx::aron::visitor
private:
- bool applyToChildren(AronDictDataNavigator& navigator);
- bool applyToChildren(AronListDataNavigator& navigator);
+ bool applyToChildren(DictDataNavigator& navigator);
+ bool applyToChildren(ListDataNavigator& navigator);
};
@@ -203,58 +203,58 @@ namespace armarx::aron::visitor
struct MyDerivedAronDataVisitor : public armarx::aron::visitor::AronDataVisitor
{
- bool visitEnter(AronDictDataNavigator& dict) override
+ bool visitEnter(DictDataNavigator& dict) override
{
(void) dict;
return true;
}
- bool visitExit(AronDictDataNavigator& dict) override
+ bool visitExit(DictDataNavigator& dict) override
{
(void) dict;
return true;
}
- bool visitEnter(AronListDataNavigator& list) override
+ bool visitEnter(ListDataNavigator& list) override
{
(void) list;
return true;
}
- bool visitExit(AronListDataNavigator& list) override
+ bool visitExit(ListDataNavigator& list) override
{
(void) list;
return true;
}
- bool visit(AronBoolDataNavigator& b) override
+ bool visit(BoolDataNavigator& b) override
{
(void) b;
return true;
}
- bool visit(AronDoubleDataNavigator& d) override
+ bool visit(DoubleDataNavigator& d) override
{
(void) d;
return true;
}
- bool visit(AronFloatDataNavigator& f) override
+ bool visit(FloatDataNavigator& f) override
{
(void) f;
return true;
}
- bool visit(AronIntDataNavigator& i) override
+ bool visit(IntDataNavigator& i) override
{
(void) i;
return true;
}
- bool visit(AronLongDataNavigator& l) override
+ bool visit(LongDataNavigator& l) override
{
(void) l;
return true;
}
- bool visit(AronNDArrayDataNavigator& array) override
+ bool visit(NDArrayDataNavigator& array) override
{
(void) array;
return true;
}
- bool visit(AronStringDataNavigator& string) override
+ bool visit(StringDataNavigator& string) override
{
(void) string;
return true;
diff --git a/source/RobotAPI/libraries/aron/aroncore/navigators/visitors/TypedDataVisitor.cpp b/source/RobotAPI/libraries/aron/aroncore/navigators/visitors/TypedDataVisitor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7936152be03cd09575812d80a48e3fd5eb18a24c
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/aroncore/navigators/visitors/TypedDataVisitor.cpp
@@ -0,0 +1,182 @@
+#include "TypedDataVisitor.h"
+
+#include <SimoxUtility/meta/type_name.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+
+namespace armarx::aron::visitor
+{
+
+
+ TypedDataVisitor::TypedDataVisitor()
+ {
+ }
+
+ TypedDataVisitor::~TypedDataVisitor()
+ {
+ }
+
+ bool TypedDataVisitor::applyTo(TypeNavigator& type, DataNavigator& data)
+ {
+ return this->applyTo("", type, data);
+ }
+
+ template <class DataT, class TypeT>
+ bool TypedDataVisitor::applyToCollection(TypeT& type, const std::string& key, DataNavigator& data)
+ {
+ DataT& dataCast = dynamic_cast<DataT&>(data);
+ if (!visitEnter(type, key, dataCast))
+ {
+ return false;
+ }
+ if (!applyToChildren(type, dataCast))
+ {
+ return false;
+ }
+ if (!visitExit(type, key, dataCast))
+ {
+ return false;
+ }
+ return true;
+ }
+
+ bool TypedDataVisitor::applyTo(const std::string& key, TypeNavigator& type, DataNavigator& data)
+ {
+ if (auto t = dynamic_cast<ObjectTypeNavigator*>(&type))
+ {
+ return applyToCollection<DictDataNavigator>(*t, key, data);
+ }
+ else if (auto t = dynamic_cast<DictTypeNavigator*>(&type))
+ {
+ return applyToCollection<DictDataNavigator>(*t, key, data);
+ }
+ else if (auto t = dynamic_cast<ListTypeNavigator*>(&type))
+ {
+ return applyToCollection<ListDataNavigator>(*t, key, data);
+ }
+ else if (auto t = dynamic_cast<TupleTypeNavigator*>(&type))
+ {
+ return applyToCollection<ListDataNavigator>(*t, key, data);
+ }
+
+ ARMARX_CHECK_EQUAL(type.childrenSize(), 0) << simox::meta::get_type_name(type);
+ ARMARX_CHECK_EQUAL(data.childrenSize(), 0) << simox::meta::get_type_name(data);
+
+ if (auto t = dynamic_cast<BoolTypeNavigator*>(&type))
+ {
+ return visit(*t, key, dynamic_cast<BoolDataNavigator&>(data));
+ }
+ else if (auto t = dynamic_cast<DoubleTypeNavigator*>(&type))
+ {
+ return visit(*t, key, dynamic_cast<DoubleDataNavigator&>(data));
+ }
+ else if (auto t = dynamic_cast<FloatTypeNavigator*>(&type))
+ {
+ return visit(*t, key, dynamic_cast<FloatDataNavigator&>(data));
+ }
+ else if (auto t = dynamic_cast<IntTypeNavigator*>(&type))
+ {
+ return visit(*t, key, dynamic_cast<IntDataNavigator&>(data));
+ }
+ else if (auto t = dynamic_cast<LongTypeNavigator*>(&type))
+ {
+ return visit(*t, key, dynamic_cast<LongDataNavigator&>(data));
+ }
+ else if (auto t = dynamic_cast<StringTypeNavigator*>(&type))
+ {
+ return visit(*t, key, dynamic_cast<StringDataNavigator&>(data));
+ }
+
+ if (auto t = dynamic_cast<EigenMatrixTypeNavigator*>(&type))
+ {
+ return visit(*t, key, dynamic_cast<NDArrayDataNavigator&>(data));
+ }
+ else if (auto t = dynamic_cast<IVTCByteImageTypeNavigator*>(&type))
+ {
+ return visit(*t, key, dynamic_cast<NDArrayDataNavigator&>(data));
+ }
+ else if (auto t = dynamic_cast<OpenCVMatTypeNavigator*>(&type))
+ {
+ return visit(*t, key, dynamic_cast<NDArrayDataNavigator&>(data));
+ }
+ else if (auto t = dynamic_cast<PCLPointCloudTypeNavigator*>(&type))
+ {
+ return visit(*t, key, dynamic_cast<NDArrayDataNavigator&>(data));
+ }
+
+ ARMARX_CHECK(false) << "Unhandled AronTypeNavigatorType '" << simox::meta::get_type_name(type) << "'."
+ << "\n(Data: '" << simox::meta::get_type_name(data) << "')";
+ }
+
+ bool TypedDataVisitor::applyToChildren(ObjectTypeNavigator& type, DictDataNavigator& data)
+ {
+ for (const std::string& key : type.getAllKeys())
+ {
+ TypeNavigator::PointerType childType = type.getAcceptedType(key);
+ DataNavigator::PointerType childData = data.getElement(key);
+ if (childType && childData)
+ {
+ if (!applyTo(key, *childType, *childData))
+ {
+ return false;
+ }
+ }
+ }
+ return true;
+ }
+
+ bool TypedDataVisitor::applyToChildren(DictTypeNavigator& type, DictDataNavigator& data)
+ {
+ TypeNavigator::PointerType childType = type.getAcceptedType();
+ for (const std::string& key : data.getAllKeys())
+ {
+ DataNavigator::PointerType childData = data.getElement(key);
+ if (childType && childData)
+ {
+ if (!applyTo(key, *childType, *childData))
+ {
+ return false;
+ }
+ }
+ }
+ return true;
+ }
+
+ bool TypedDataVisitor::applyToChildren(ListTypeNavigator& type, ListDataNavigator& data)
+ {
+ TypeNavigator::PointerType childType = type.getAcceptedType();
+ size_t i = 0;
+ for (const DataNavigator::PointerType& childData : data.getElements())
+ {
+ if (childType && childData)
+ {
+ if (!applyTo(std::to_string(i), *childType, *childData))
+ {
+ return false;
+ }
+ }
+ ++i;
+ }
+ return true;
+ }
+
+ bool TypedDataVisitor::applyToChildren(TupleTypeNavigator& type, ListDataNavigator& data)
+ {
+ unsigned int i = 0;
+ for (const TypeNavigator::PointerType& childType : type.getAcceptedTypes())
+ {
+ DataNavigator::PointerType childData = data.getElement(i);
+ if (childType && childData)
+ {
+ if (!applyTo(std::to_string(i), *childType, *childData))
+ {
+ return false;
+ }
+ }
+ ++i;
+ }
+ return true;
+ }
+
+}
diff --git a/source/RobotAPI/libraries/aron/aroncore/navigators/visitors/TypedDataVisitor.h b/source/RobotAPI/libraries/aron/aroncore/navigators/visitors/TypedDataVisitor.h
new file mode 100644
index 0000000000000000000000000000000000000000..ac67b9fbde35432be646b824eccd34c89de7feac
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/aroncore/navigators/visitors/TypedDataVisitor.h
@@ -0,0 +1,397 @@
+#pragma once
+
+#include <RobotAPI/libraries/aron/aroncore/navigators/datanavigator/AronAllDataNavigators.h>
+#include <RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronAllTypeNavigators.h>
+
+
+namespace armarx::aron::visitor
+{
+
+ /**
+ * @brief A visitor traversing an `aron::Type::AronTypePtr` using
+ * `aron::Typenavigator::AronTypeNavigator`s, taking care of type checking,
+ * casting and hierarchical traversal.
+ *
+ * To use it, create a class deriving from `AronTypeVisitor` and
+ * override the overloads of `visitEnter()`, `visitExit()` and `visit()`
+ * for the types you would like to handle (see blow for a copy-and-paste
+ * example).
+ * Then, instantiate your derived visitor and call `applyTo()`, passing
+ * the `AronTypePtr` or the root `AronTypeNavigator`.
+ *
+ * For dicts and lists, you will get a `visitEnter()`/`visitExit()` pair.
+ * For types without children (e.g. primitive types), you get a single
+ * `visit()` call.
+ *
+ * Each visit function should return true to continue traversal or
+ * false to stop it. All default implementations return true.
+ *
+ * To the the location of the passed navigator in the original `AronType`,
+ * use `navigator.getPath()`.
+ *
+ * @see `aron::Type::AronTypePtr`
+ * @see `aron::Typenavigator::AronTypeNavigator`
+ * @see `std::stack` To manage Type with stack semantics.
+ */
+ class TypedDataVisitor
+ {
+ public:
+
+ // Define types to ease writing code in this and derived classes.
+
+ // Data - possible data representations
+ using DataNavigator = datanavigator::AronDataNavigator;
+
+ using DictDataNavigator = datanavigator::AronDictDataNavigator;
+ using ListDataNavigator = datanavigator::AronListDataNavigator;
+
+ using BoolDataNavigator = datanavigator::AronBoolDataNavigator;
+ using DoubleDataNavigator = datanavigator::AronDoubleDataNavigator;
+ using FloatDataNavigator = datanavigator::AronFloatDataNavigator;
+ using IntDataNavigator = datanavigator::AronIntDataNavigator;
+ using LongDataNavigator = datanavigator::AronLongDataNavigator;
+ using StringDataNavigator = datanavigator::AronStringDataNavigator;
+
+ using NDArrayDataNavigator = datanavigator::AronNDArrayDataNavigator;
+
+
+ // Type
+ using TypeNavigator = typenavigator::AronTypeNavigator;
+
+ // Dict-valued
+ using DictTypeNavigator = typenavigator::AronDictTypeNavigator;
+ using ObjectTypeNavigator = typenavigator::AronObjectTypeNavigator;
+
+ // List-valued
+ using ListTypeNavigator = typenavigator::AronListTypeNavigator;
+ using TupleTypeNavigator = typenavigator::AronTupleTypeNavigator;
+
+ // Primitive-valued
+ using BoolTypeNavigator = typenavigator::AronBoolTypeNavigator;
+ using DoubleTypeNavigator = typenavigator::AronDoubleTypeNavigator;
+ using FloatTypeNavigator = typenavigator::AronFloatTypeNavigator;
+ using IntTypeNavigator = typenavigator::AronIntTypeNavigator;
+ using LongTypeNavigator = typenavigator::AronLongTypeNavigator;
+ using StringTypeNavigator = typenavigator::AronStringTypeNavigator;
+
+ // Array-valued
+ using EigenMatrixTypeNavigator = typenavigator::AronEigenMatrixTypeNavigator;
+ using IVTCByteImageTypeNavigator = typenavigator::AronIVTCByteImageTypeNavigator;
+ using OpenCVMatTypeNavigator = typenavigator::AronOpenCVMatTypeNavigator;
+ using PCLPointCloudTypeNavigator = typenavigator::AronPCLPointCloudTypeNavigator;
+
+
+ public:
+
+ TypedDataVisitor();
+ virtual ~TypedDataVisitor();
+
+
+ bool applyTo(TypeNavigator& type, DataNavigator& data);
+ bool applyTo(const std::string& key, TypeNavigator& type, DataNavigator& data);
+
+
+ virtual bool visitEnter(DictTypeNavigator& type, DictDataNavigator& data)
+ {
+ (void) type, (void) data;
+ return true;
+ }
+ virtual bool visitExit(DictTypeNavigator& type, DictDataNavigator& data)
+ {
+ (void) type, (void) data;
+ return true;
+ }
+
+ virtual bool visitEnter(ObjectTypeNavigator& type, DictDataNavigator& data)
+ {
+ (void) type, (void) data;
+ return true;
+ }
+ virtual bool visitExit(ObjectTypeNavigator& type, DictDataNavigator& data)
+ {
+ (void) type, (void) data;
+ return true;
+ }
+
+ virtual bool visitEnter(ListTypeNavigator& type, ListDataNavigator& data)
+ {
+ (void) type, (void) data;
+ return true;
+ }
+ virtual bool visitExit(ListTypeNavigator& type, ListDataNavigator& data)
+ {
+ (void) type, (void) data;
+ return true;
+ }
+
+ virtual bool visitEnter(TupleTypeNavigator& type, ListDataNavigator& data)
+ {
+ (void) type, (void) data;
+ return true;
+ }
+ virtual bool visitExit(TupleTypeNavigator& type, ListDataNavigator& data)
+ {
+ (void) type, (void) data;
+ return true;
+ }
+
+
+ virtual bool visit(BoolTypeNavigator& type, BoolDataNavigator& data)
+ {
+ (void) type, (void) data;
+ return true;
+ }
+ virtual bool visit(DoubleTypeNavigator& type, DoubleDataNavigator& data)
+ {
+ (void) type, (void) data;
+ return true;
+ }
+ virtual bool visit(FloatTypeNavigator& type, FloatDataNavigator& data)
+ {
+ (void) type, (void) data;
+ return true;
+ }
+ virtual bool visit(IntTypeNavigator& type, IntDataNavigator& data)
+ {
+ (void) type, (void) data;
+ return true;
+ }
+ virtual bool visit(LongTypeNavigator& type, LongDataNavigator& data)
+ {
+ (void) type, (void) data;
+ return true;
+ }
+ virtual bool visit(StringTypeNavigator& type, StringDataNavigator& data)
+ {
+ (void) type, (void) data;
+ return true;
+ }
+
+
+ virtual bool visit(EigenMatrixTypeNavigator& type, NDArrayDataNavigator& data)
+ {
+ (void) type, (void) type, (void) data;
+ return true;
+ }
+ virtual bool visit(IVTCByteImageTypeNavigator& type, NDArrayDataNavigator& data)
+ {
+ (void) type, (void) type, (void) data;
+ return true;
+ }
+ virtual bool visit(OpenCVMatTypeNavigator& type, NDArrayDataNavigator& data)
+ {
+ (void) type, (void) type, (void) data;
+ return true;
+ }
+ virtual bool visit(PCLPointCloudTypeNavigator& type, NDArrayDataNavigator& data)
+ {
+ (void) type, (void) type, (void) data;
+ return true;
+ }
+
+
+
+ virtual bool visitEnter(DictTypeNavigator& type, const std::string& key, DictDataNavigator& data)
+ {
+ (void) key;
+ visitEnter(type, data);
+ return true;
+ }
+ virtual bool visitExit(DictTypeNavigator& type, const std::string& key, DictDataNavigator& data)
+ {
+ (void) key;
+ visitExit(type, data);
+ return true;
+ }
+
+ virtual bool visitEnter(ObjectTypeNavigator& type, const std::string& key, DictDataNavigator& data)
+ {
+ (void) key;
+ visitEnter(type, data);
+ return true;
+ }
+ virtual bool visitExit(ObjectTypeNavigator& type, const std::string& key, DictDataNavigator& data)
+ {
+ (void) key;
+ visitExit(type, data);
+ return true;
+ }
+
+ virtual bool visitEnter(ListTypeNavigator& type, const std::string& key, ListDataNavigator& data)
+ {
+ (void) key;
+ visitEnter(type, data);
+ return true;
+ }
+ virtual bool visitExit(ListTypeNavigator& type, const std::string& key, ListDataNavigator& data)
+ {
+ (void) key;
+ visitExit(type, data);
+ return true;
+ }
+
+ virtual bool visitEnter(TupleTypeNavigator& type, const std::string& key, ListDataNavigator& data)
+ {
+ (void) key;
+ visitEnter(type, data);
+ return true;
+ }
+ virtual bool visitExit(TupleTypeNavigator& type, const std::string& key, ListDataNavigator& data)
+ {
+ (void) key;
+ visitExit(type, data);
+ return true;
+ }
+
+
+ virtual bool visit(BoolTypeNavigator& type, const std::string& key, BoolDataNavigator& data)
+ {
+ (void) key;
+ visit(type, data);
+ return true;
+ }
+ virtual bool visit(DoubleTypeNavigator& type, const std::string& key, DoubleDataNavigator& data)
+ {
+ (void) key;
+ visit(type, data);
+ return true;
+ }
+ virtual bool visit(FloatTypeNavigator& type, const std::string& key, FloatDataNavigator& data)
+ {
+ (void) key;
+ visit(type, data);
+ return true;
+ }
+ virtual bool visit(IntTypeNavigator& type, const std::string& key, IntDataNavigator& data)
+ {
+ (void) key;
+ visit(type, data);
+ return true;
+ }
+ virtual bool visit(LongTypeNavigator& type, const std::string& key, LongDataNavigator& data)
+ {
+ (void) key;
+ visit(type, data);
+ return true;
+ }
+ virtual bool visit(StringTypeNavigator& type, const std::string& key, StringDataNavigator& data)
+ {
+ (void) key;
+ visit(type, data);
+ return true;
+ }
+
+
+ virtual bool visit(EigenMatrixTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data)
+ {
+ (void) type, (void) key;
+ visit(type, data);
+ return true;
+ }
+ virtual bool visit(IVTCByteImageTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data)
+ {
+ (void) type, (void) key;
+ visit(type, data);
+ return true;
+ }
+ virtual bool visit(OpenCVMatTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data)
+ {
+ (void) type, (void) key;
+ visit(type, data);
+ return true;
+ }
+ virtual bool visit(PCLPointCloudTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data)
+ {
+ (void) type, (void) key;
+ visit(type, data);
+ return true;
+ }
+
+
+
+ private:
+
+ template <class DataT, class TypeT>
+ bool applyToCollection(TypeT& type, const std::string& key, DataNavigator& data);
+
+ bool applyToChildren(ObjectTypeNavigator& type, DictDataNavigator& data);
+ bool applyToChildren(DictTypeNavigator& type, DictDataNavigator& data);
+ bool applyToChildren(ListTypeNavigator& type, ListDataNavigator& data);
+ bool applyToChildren(TupleTypeNavigator& type, ListDataNavigator& data);
+
+ };
+
+}
+
+
+/* Copy-and-paste example below.
+ * Remove functions you don't need.
+ * Add `const std::string& key, ` to parameter list if you need an items's key
+ * in the parent container.
+ */
+
+#if 0
+
+struct MyDerivedAronTypeVisitor : public armarx::aron::visitor::AronTypeVisitor
+{
+
+ bool visitEnter(DictTypeNavigator& dict) override
+ {
+ (void) dict;
+ return true;
+ }
+ bool visitExit(DictTypeNavigator& dict) override
+ {
+ (void) dict;
+ return true;
+ }
+ bool visitEnter(ListTypeNavigator& list) override
+ {
+ (void) list;
+ return true;
+ }
+ bool visitExit(ListTypeNavigator& list) override
+ {
+ (void) list;
+ return true;
+ }
+
+ bool visit(BoolTypeNavigator& b) override
+ {
+ (void) b;
+ return true;
+ }
+ bool visit(DoubleTypeNavigator& d) override
+ {
+ (void) d;
+ return true;
+ }
+ bool visit(FloatTypeNavigator& f) override
+ {
+ (void) f;
+ return true;
+ }
+ bool visit(IntTypeNavigator& i) override
+ {
+ (void) i;
+ return true;
+ }
+ bool visit(LongTypeNavigator& l) override
+ {
+ (void) l;
+ return true;
+ }
+ bool visit(NDArrayTypeNavigator& array) override
+ {
+ (void) array;
+ return true;
+ }
+ bool visit(StringTypeNavigator& string) override
+ {
+ (void) string;
+ return true;
+ }
+
+};
+
+#endif
diff --git a/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.cpp b/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.cpp
index 08fbb612d15a9734c86d1043aee74088a330171a..9f2dc660d9945879bf66c7d5d37e9d15a0865926 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.cpp
+++ b/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.cpp
@@ -168,9 +168,15 @@ void RobotStateObserver::udpatePoseDatafields(const FramedPoseBaseMap& poseMap)
}
}
-DatafieldRefBasePtr RobotStateObserver::getPoseDatafield(const std::string& nodeName, const Ice::Current&) const
+void RobotStateObserver::getPoseDatafield_async(
+ const AMD_RobotStateObserverInterface_getPoseDatafieldPtr& amd,
+ const std::string& nodeName,
+ const Ice::Current&) const
{
- return getDatafieldRefByName(TCP_POSE_CHANNEL, nodeName);
+ addWorkerJob("RobotStateObserver::getPoseDatafield", [this, amd, nodeName]
+ {
+ return getDatafieldRefByName(TCP_POSE_CHANNEL, nodeName);
+ });
}
void RobotStateObserver::updateNodeVelocities(const NameValueMap& jointVel, long timestampMicroSeconds)
diff --git a/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.h b/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.h
index e3acfd611dc6487744decc1c4d3183e33f74c383..8f0899834450db1b5674bdebf0bfa2443ab4de8e 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.h
+++ b/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.h
@@ -99,7 +99,10 @@ namespace armarx
// RobotStateObserverInterface interface
public:
- DatafieldRefBasePtr getPoseDatafield(const std::string& nodeName, const Ice::Current&) const override;
+ void getPoseDatafield_async(
+ const AMD_RobotStateObserverInterface_getPoseDatafieldPtr& amd,
+ const std::string& nodeName,
+ const Ice::Current&) const override;
};
using RobotStateObserverPtr = IceInternal::Handle<RobotStateObserver>;