diff --git a/.gitlab/CODEOWNERS b/.gitlab/CODEOWNERS
index af86e2ce7810f25704e7afbbbb3072e53e36b487..2a5d5735c0809d89c455cfced4ecd1ca96bc961d 100644
--- a/.gitlab/CODEOWNERS
+++ b/.gitlab/CODEOWNERS
@@ -1,17 +1,17 @@
# ARON
-/source/RobotAPI/libraries/aron/ @fratty @dreher
+/source/RobotAPI/libraries/aron/ @peller @dreher
-/source/RobotAPI/interface/aron/ @fratty @dreher
-/source/RobotAPI/interface/aron.ice @fratty @dreher
+/source/RobotAPI/interface/aron/ @peller @dreher
+/source/RobotAPI/interface/aron.ice @peller @dreher
# ArMem
-/source/RobotAPI/components/armem/ @fratty @RainerKartmann @dreher
+/source/RobotAPI/components/armem/ @peller @kartmann @dreher
-/source/RobotAPI/libraries/armem/ @fratty @RainerKartmann @dreher
-/source/RobotAPI/libraries/armem_robot_localization/ @FabianReister
-/source/RobotAPI/libraries/armem_robot_mapping/ @FabianReister
+/source/RobotAPI/libraries/armem/ @peller @kartmann @dreher
+/source/RobotAPI/libraries/armem_robot_localization/ @reister
+/source/RobotAPI/libraries/armem_robot_mapping/ @reister
/source/RobotAPI/interface/armem/ @fratty @RainerKartmann @dreher
/source/RobotAPI/interface/armem.ice @fratty @RainerKartmann @dreher
diff --git a/scenarios/ArMemExample/config/ExampleMemory.cfg b/scenarios/ArMemExample/config/ExampleMemory.cfg
index 7989108bc28741f5c638fe53de5608ecaf8c7612..251382f2af086afccf73924421ea31a2aad01017 100644
--- a/scenarios/ArMemExample/config/ExampleMemory.cfg
+++ b/scenarios/ArMemExample/config/ExampleMemory.cfg
@@ -143,21 +143,12 @@ ArmarX.ArMemExampleMemory.tpc.pub.MemoryListener = MemoryUpdates
# ArmarX.ExampleMemory.mem.MemoryName = Example
-# ArmarX.ExampleMemory.mem.ltm..buffer.storeFreq: Frequency to store the buffer to the LTM in Hz.
+# ArmarX.ExampleMemory.mem.ltm.configuration:
# Attributes:
-# - Default: 10
+# - Default: {}
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ExampleMemory.mem.ltm..buffer.storeFreq = 10
-
-
-# ArmarX.ExampleMemory.mem.ltm.depthImageExtractor.Enabled:
-# Attributes:
-# - Default: true
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ExampleMemory.mem.ltm.depthImageExtractor.Enabled = true
+# ArmarX.ExampleMemory.mem.ltm.configuration = {}
# ArmarX.ExampleMemory.mem.ltm.enabled:
@@ -169,100 +160,6 @@ ArmarX.ArMemExampleMemory.tpc.pub.MemoryListener = MemoryUpdates
# ArmarX.ExampleMemory.mem.ltm.enabled = false
-# ArmarX.ExampleMemory.mem.ltm.exrConverter.Enabled:
-# Attributes:
-# - Default: true
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ExampleMemory.mem.ltm.exrConverter.Enabled = true
-
-
-# ArmarX.ExampleMemory.mem.ltm.imageExtractor.Enabled:
-# Attributes:
-# - Default: true
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ExampleMemory.mem.ltm.imageExtractor.Enabled = true
-
-
-# ArmarX.ExampleMemory.mem.ltm.memFreqFilter.Enabled:
-# Attributes:
-# - Default: false
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ExampleMemory.mem.ltm.memFreqFilter.Enabled = false
-
-
-# ArmarX.ExampleMemory.mem.ltm.memFreqFilter.WaitingTime: Waiting time in MS after each LTM update.
-# Attributes:
-# - Default: -1
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ExampleMemory.mem.ltm.memFreqFilter.WaitingTime = -1
-
-
-# ArmarX.ExampleMemory.mem.ltm.pngConverter.Enabled:
-# Attributes:
-# - Default: true
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ExampleMemory.mem.ltm.pngConverter.Enabled = true
-
-
-# ArmarX.ExampleMemory.mem.ltm.sizeToCompressDataInMegaBytes: The size in MB to compress away the current export. Exports are numbered (lower number means newer).
-# Attributes:
-# - Default: 1024
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ExampleMemory.mem.ltm.sizeToCompressDataInMegaBytes = 1024
-
-
-# ArmarX.ExampleMemory.mem.ltm.snapEqFilter.Enabled:
-# Attributes:
-# - Default: false
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ExampleMemory.mem.ltm.snapEqFilter.Enabled = false
-
-
-# ArmarX.ExampleMemory.mem.ltm.snapEqFilter.MaxWaitingTime: Max Waiting time in MS after each Entity update.
-# Attributes:
-# - Default: -1
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ExampleMemory.mem.ltm.snapEqFilter.MaxWaitingTime = -1
-
-
-# ArmarX.ExampleMemory.mem.ltm.snapFreqFilter.Enabled:
-# Attributes:
-# - Default: false
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ExampleMemory.mem.ltm.snapFreqFilter.Enabled = false
-
-
-# ArmarX.ExampleMemory.mem.ltm.snapFreqFilter.WaitingTime: Waiting time in MS after each Entity update.
-# Attributes:
-# - Default: -1
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ExampleMemory.mem.ltm.snapFreqFilter.WaitingTime = -1
-
-
-# ArmarX.ExampleMemory.mem.ltm.storagepath: The path to the memory storage (the memory will be stored in a seperate subfolder).
-# Attributes:
-# - Default: Default value not mapped.
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ExampleMemory.mem.ltm.storagepath = Default value not mapped.
-
-
# ArmarX.ExampleMemory.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
# Set to false to use this memory as a stand-alone.
# Attributes:
diff --git a/scenarios/SkillProviderTest/config/SkillsMemory.cfg b/scenarios/SkillProviderTest/config/SkillsMemory.cfg
index 711047a4029175550111268cf857a613c2dd48b2..8b2bf04855a21e5a8ca955da89ba3a309460241e 100644
--- a/scenarios/SkillProviderTest/config/SkillsMemory.cfg
+++ b/scenarios/SkillProviderTest/config/SkillsMemory.cfg
@@ -167,21 +167,12 @@
# ArmarX.SkillMemory.mem.MemoryName = Skill
-# ArmarX.SkillMemory.mem.ltm..buffer.storeFreq: Frequency to store the buffer to the LTM in Hz.
+# ArmarX.SkillMemory.mem.ltm.configuration:
# Attributes:
-# - Default: 10
+# - Default: {}
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SkillMemory.mem.ltm..buffer.storeFreq = 10
-
-
-# ArmarX.SkillMemory.mem.ltm.depthImageExtractor.Enabled:
-# Attributes:
-# - Default: true
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.SkillMemory.mem.ltm.depthImageExtractor.Enabled = true
+# ArmarX.SkillMemory.mem.ltm.configuration = {}
# ArmarX.SkillMemory.mem.ltm.enabled:
@@ -193,100 +184,6 @@
# ArmarX.SkillMemory.mem.ltm.enabled = false
-# ArmarX.SkillMemory.mem.ltm.exrConverter.Enabled:
-# Attributes:
-# - Default: true
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.SkillMemory.mem.ltm.exrConverter.Enabled = true
-
-
-# ArmarX.SkillMemory.mem.ltm.imageExtractor.Enabled:
-# Attributes:
-# - Default: true
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.SkillMemory.mem.ltm.imageExtractor.Enabled = true
-
-
-# ArmarX.SkillMemory.mem.ltm.memFreqFilter.Enabled:
-# Attributes:
-# - Default: false
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.SkillMemory.mem.ltm.memFreqFilter.Enabled = false
-
-
-# ArmarX.SkillMemory.mem.ltm.memFreqFilter.WaitingTime: Waiting time in MS after each LTM update.
-# Attributes:
-# - Default: -1
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.SkillMemory.mem.ltm.memFreqFilter.WaitingTime = -1
-
-
-# ArmarX.SkillMemory.mem.ltm.pngConverter.Enabled:
-# Attributes:
-# - Default: true
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.SkillMemory.mem.ltm.pngConverter.Enabled = true
-
-
-# ArmarX.SkillMemory.mem.ltm.sizeToCompressDataInMegaBytes: The size in MB to compress away the current export. Exports are numbered (lower number means newer).
-# Attributes:
-# - Default: 1024
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.SkillMemory.mem.ltm.sizeToCompressDataInMegaBytes = 1024
-
-
-# ArmarX.SkillMemory.mem.ltm.snapEqFilter.Enabled:
-# Attributes:
-# - Default: false
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.SkillMemory.mem.ltm.snapEqFilter.Enabled = false
-
-
-# ArmarX.SkillMemory.mem.ltm.snapEqFilter.MaxWaitingTime: Max Waiting time in MS after each Entity update.
-# Attributes:
-# - Default: -1
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.SkillMemory.mem.ltm.snapEqFilter.MaxWaitingTime = -1
-
-
-# ArmarX.SkillMemory.mem.ltm.snapFreqFilter.Enabled:
-# Attributes:
-# - Default: false
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.SkillMemory.mem.ltm.snapFreqFilter.Enabled = false
-
-
-# ArmarX.SkillMemory.mem.ltm.snapFreqFilter.WaitingTime: Waiting time in MS after each Entity update.
-# Attributes:
-# - Default: -1
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.SkillMemory.mem.ltm.snapFreqFilter.WaitingTime = -1
-
-
-# ArmarX.SkillMemory.mem.ltm.storagepath: The path to the memory storage (the memory will be stored in a seperate subfolder).
-# Attributes:
-# - Default: Default value not mapped.
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.SkillMemory.mem.ltm.storagepath = Default value not mapped.
-
-
# ArmarX.SkillMemory.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
# Set to false to use this memory as a stand-alone.
# Attributes:
diff --git a/source/RobotAPI/components/ArViz/Client/Elements.cpp b/source/RobotAPI/components/ArViz/Client/Elements.cpp
index f479f4ef5456fc7a88d0f3a0fa87d2e77e9aab66..d97fea3b03d55ddb82a379628197a5f04ffc3d74 100644
--- a/source/RobotAPI/components/ArViz/Client/Elements.cpp
+++ b/source/RobotAPI/components/ArViz/Client/Elements.cpp
@@ -107,9 +107,9 @@ namespace armarx::viz
float angle = std::acos(naturalDir.dot(dir));
if (cross.squaredNorm() < 1.0e-12f)
{
- // Directions are almost colinear ==> Do no rotation
+ // Directions are almost colinear ==> Angle is either 0 or 180 deg
cross = Eigen::Vector3f::UnitX();
- angle = 0.0f;
+ // Keep angle
}
Eigen::Vector3f axis = cross.normalized();
Eigen::Quaternionf ori(Eigen::AngleAxisf(angle, axis));
diff --git a/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp b/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp
index 2c93faf7fa4a464909bd6dea214c74022571474f..e52609fa085feac2dc4ef8fc93f68127aef8b040 100644
--- a/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp
+++ b/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp
@@ -275,13 +275,18 @@ namespace armarx
void fillExampleLayer(viz::Layer& layer, double timeInSeconds)
{
+ for (int i = 0; i < 6; ++i)
{
- Eigen::Vector3f pos = Eigen::Vector3f::Zero();
- pos.z() = +300.0f;
+ Eigen::Vector3f pos = Eigen::Vector3f(-200.0, 200.0, 300);
+ pos.x() += -300 * i;
+
+ Eigen::Vector3f normal = Eigen::Vector3f::Zero();
+ normal(i / 2) = (i % 2 == 0 ? 1.0 : -1.0);
- viz::ArrowCircle circle = viz::ArrowCircle("circle")
+ viz::ArrowCircle circle = viz::ArrowCircle("circle " + std::to_string(i))
.position(pos)
.radius(100.0f)
+ .normal(normal)
.width(10.0f)
.color(viz::Color::fromRGBA(255, 0, 255));
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
index eab8bf5e6dd721395647519d3e04fecc62ffbb7f..f77e01607b0036a2f9525b88dc919f0f1882608b 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
@@ -47,6 +47,7 @@ namespace armarx::armem::server::robot_state
RobotStateMemory::RobotStateMemory() :
descriptionSegment(iceAdapter()),
proprioceptionSegment(iceAdapter()),
+ exteroceptionSegment(iceAdapter()),
localizationSegment(iceAdapter()),
commonVisu(descriptionSegment, proprioceptionSegment, localizationSegment)
{
@@ -86,6 +87,7 @@ namespace armarx::armem::server::robot_state
setMemoryName(armem::robot_state::memoryID.memoryName);
descriptionSegment.defineProperties(defs, prefix + "desc.");
+ exteroceptionSegment.defineProperties(defs, prefix + "ext.");
proprioceptionSegment.defineProperties(defs, prefix + "prop.");
localizationSegment.defineProperties(defs, prefix + "loc.");
commonVisu.defineProperties(defs, prefix + "visu.");
@@ -104,6 +106,7 @@ namespace armarx::armem::server::robot_state
{
descriptionSegment.init();
proprioceptionSegment.init();
+ exteroceptionSegment.init();
localizationSegment.init();
commonVisu.init();
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
index 09280c4691a479a3bbd612131565775669bf21d1..70513a0cf56ff0cd6d82603cc3266fb405e5af3d 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
@@ -27,26 +27,25 @@
#include <ArmarXCore/core/Component.h>
+#include <RobotAPI/interface/core/RobotLocalization.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
#include <RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h>
-
+#include <RobotAPI/libraries/armem_robot_state/server/common/Visu.h>
#include <RobotAPI/libraries/armem_robot_state/server/description/Segment.h>
#include <RobotAPI/libraries/armem_robot_state/server/localization/Segment.h>
-#include <RobotAPI/libraries/armem_robot_state/server/common/Visu.h>
-#include <RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h>
#include <RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h>
#include <RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.h>
#include <RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.h>
-
-#include <RobotAPI/interface/core/RobotLocalization.h>
+#include <RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h>
+#include <RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.h>
namespace armarx::plugins
{
class DebugObserverComponentPlugin;
class RobotUnitComponentPlugin;
-}
+} // namespace armarx::plugins
namespace armarx::armem::server::robot_state
{
@@ -68,7 +67,6 @@ namespace armarx::armem::server::robot_state
virtual public armarx::GlobalRobotPoseProvider
{
public:
-
RobotStateMemory();
virtual ~RobotStateMemory() override;
@@ -77,11 +75,12 @@ namespace armarx::armem::server::robot_state
// GlobalRobotPoseProvider interface
- armarx::PoseBasePtr getGlobalRobotPose(Ice::Long timestamp, const std::string& robotName, const ::Ice::Current&) override;
+ armarx::PoseBasePtr getGlobalRobotPose(Ice::Long timestamp,
+ const std::string& robotName,
+ const ::Ice::Current&) override;
protected:
-
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
void onInitComponent() override;
@@ -91,13 +90,11 @@ namespace armarx::armem::server::robot_state
private:
-
void startRobotUnitStream();
void stopRobotUnitStream();
private:
-
armarx::plugins::DebugObserverComponentPlugin* debugObserver = nullptr;
@@ -109,6 +106,9 @@ namespace armarx::armem::server::robot_state
proprioception::Segment proprioceptionSegment;
armem::data::MemoryID robotUnitProviderID;
+ // - exteroception
+ exteroception::Segment exteroceptionSegment;
+
// - localization
localization::Segment localizationSegment;
@@ -129,11 +129,12 @@ namespace armarx::armem::server::robot_state
proprioception::RobotStateWriter writer;
// queue
- TripleBuffer<std::vector<proprioception::RobotUnitData>> dataBuffer;
+ using Queue = armarx::armem::server::robot_state::proprioception::Queue;
+ Queue dataBuffer;
bool waitForRobotUnit = false;
};
RobotUnit robotUnit;
};
-} // namespace armarx::armem::server::robot_state
+} // namespace armarx::armem::server::robot_state
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.cpp b/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.cpp
index bc50155edc0d42a775c63ade67e35f38e34fd8c5..985ef8171792570e6dad35d7a0abb09d78492006 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.cpp
+++ b/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.cpp
@@ -20,6 +20,8 @@ namespace armarx::skills::provider
default_params.some_float = 5;
default_params.some_int = 42;
default_params.some_text = "YOLO";
+ default_params.some_list_of_matrices.push_back(Eigen::Matrix3f::Zero());
+ //default_params.some_matrix = Eigen::Matrix3f::Zero();
return SkillDescription{
"HelloWorld",
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/aron/HelloWorldAcceptedType.xml b/source/RobotAPI/components/skills/SkillProviderExample/aron/HelloWorldAcceptedType.xml
index 553a93ad0898ddc40d112e097af9c95ca801da55..a2423c560b8548c265af0ae14602b8771fe46218 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/aron/HelloWorldAcceptedType.xml
+++ b/source/RobotAPI/components/skills/SkillProviderExample/aron/HelloWorldAcceptedType.xml
@@ -1,6 +1,17 @@
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
<GenerateTypes>
+ <IntEnum name="HelloWorldInteEnum">
+ <EnumValue key="INT_ENUM_VALUE_0" value="0" />
+ <EnumValue key="INT_ENUM_VALUE_1" value="1" />
+ <EnumValue key="INT_ENUM_VALUE_2" value="2" />
+ <EnumValue key="INT_ENUM_VALUE_3" value="3" />
+ <EnumValue key="INT_ENUM_VALUE_4" value="4" />
+ <EnumValue key="INT_ENUM_VALUE_5" value="5" />
+ <EnumValue key="INT_ENUM_VALUE_6" value="6" />
+ <EnumValue key="INT_ENUM_VALUE_7" value="7" />
+ <EnumValue key="INT_ENUM_VALUE_8" value="8" />
+ </IntEnum>
<Object name='armarx::skills::Example::HelloWorldAcceptedType'>
<ObjectChild key='some_int'>
<Int />
@@ -11,6 +22,23 @@
<ObjectChild key='some_text'>
<String />
</ObjectChild>
+
+ <ObjectChild key='some_list_of_matrices'>
+ <List>
+ <Matrix rows="3" cols="3" type="float32" />
+ </List>
+ </ObjectChild>
+
+ <ObjectChild key='some_dict_of_matrices'>
+ <Dict>
+ <Matrix rows="3" cols="3" type="float32" />
+ </Dict>
+ </ObjectChild>
+
+ <ObjectChild key='some_matrix'>
+ <Matrix rows="3" cols="3" type="float32" />
+ </ObjectChild>
+
</Object>
</GenerateTypes>
diff --git a/source/RobotAPI/components/units/HandUnit.h b/source/RobotAPI/components/units/HandUnit.h
index 3816605266f1bc44dce40a752bfe94b14af8177b..0241caff84c93709619c0bcabae2736d443e5c1b 100644
--- a/source/RobotAPI/components/units/HandUnit.h
+++ b/source/RobotAPI/components/units/HandUnit.h
@@ -172,7 +172,7 @@ namespace armarx
// HandUnitInterface interface
public:
- std::string getHandName(const Ice::Current&) override;
+ std::string getHandName(const Ice::Current& = Ice::emptyCurrent) override;
void setJointForces(const NameValueMap& targetJointForces, const Ice::Current&) override;
void sendJointCommands(const NameCommandMap& targetJointCommands, const Ice::Current&) override;
};
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/CMakeLists.txt b/source/RobotAPI/gui-plugins/SkillManagerPlugin/CMakeLists.txt
index 89a90ab482f9916ff11ad149faa1a3fc1b7d483c..b40d5a2fcf9c480a71f0576d049ad0a311813815 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/CMakeLists.txt
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/CMakeLists.txt
@@ -9,15 +9,22 @@ set(SOURCES
aronTreeWidget/visitors/AronTreeWidgetConverter.cpp
aronTreeWidget/visitors/AronTreeWidgetSetter.cpp
aronTreeWidget/visitors/AronTreeWidgetModalCreator.cpp
+ aronTreeWidget/visitors/AronTreeWidgetContextMenu.cpp
aronTreeWidget/Data.cpp
+ aronTreeWidget/widgets/CustomWidget.cpp
+ aronTreeWidget/widgets/EditMatrixWidget.cpp
+ aronTreeWidget/widgets/IntEnumWidget.cpp
+ aronTreeWidget/ListDictHelper.cpp
+ aronTreeWidget/widgets/QuaternionWidget.cpp
aronTreeWidget/AronTreeWidgetItem.cpp
aronTreeWidget/AronTreeWidgetController.cpp
- aronTreeWidget/modal/AronTreeWidgetModal.cpp
- aronTreeWidget/modal/int_long/AronTreeWidgetIntInputModalController.cpp
- aronTreeWidget/modal/float_double/AronTreeWidgetFloatInputModalController.cpp
- aronTreeWidget/modal/bool/AronTreeWidgetBoolInputModalController.cpp
aronTreeWidget/modal/text/AronTreeWidgetTextInputModalController.cpp
aronTreeWidget/modal/dict/AronTreeWidgetDictInputModalController.cpp
+ aronTreeWidget/modal/float_double/AronTreeWidgetFloatInputModalController.cpp
+ aronTreeWidget/modal/int_long/AronTreeWidgetIntInputModalController.cpp
+ aronTreeWidget/modal/bool/AronTreeWidgetBoolInputModalController.cpp
+ aronTreeWidget/modal/AronTreeWidgetModal.cpp
+ ColorPalettes.cpp
SkillManagerMonitorWidgetController.cpp
)
@@ -26,25 +33,29 @@ set(HEADERS
aronTreeWidget/visitors/AronTreeWidgetConverter.h
aronTreeWidget/visitors/AronTreeWidgetSetter.h
aronTreeWidget/visitors/AronTreeWidgetModalCreator.h
+ aronTreeWidget/visitors/AronTreeWidgetContextMenu.h
aronTreeWidget/Data.h
+ aronTreeWidget/widgets/NDArrayHelper.h
+ aronTreeWidget/widgets/EditMatrixWidget.h
+ aronTreeWidget/widgets/CustomWidget.h
+ aronTreeWidget/widgets/IntEnumWidget.h
+ aronTreeWidget/ListDictHelper.h
+ aronTreeWidget/widgets/QuaternionWidget.h
aronTreeWidget/AronTreeWidgetItem.h
aronTreeWidget/AronTreeWidgetController.h
aronTreeWidget/modal/AronTreeWidgetModal.h
- aronTreeWidget/modal/int_long/AronTreeWidgetIntInputModalController.h
- aronTreeWidget/modal/float_double/AronTreeWidgetFloatInputModalController.h
- aronTreeWidget/modal/bool/AronTreeWidgetBoolInputModalController.h
aronTreeWidget/modal/text/AronTreeWidgetTextInputModalController.h
aronTreeWidget/modal/dict/AronTreeWidgetDictInputModalController.h
+ aronTreeWidget/modal/float_double/AronTreeWidgetFloatInputModalController.h
+ aronTreeWidget/modal/int_long/AronTreeWidgetIntInputModalController.h
+ aronTreeWidget/modal/bool/AronTreeWidgetBoolInputModalController.h
+ ColorPalettes.h
SkillManagerMonitorWidgetController.h
)
set(GUI_UIS
SkillManagerMonitorWidget.ui
- aronTreeWidget/modal/int_long/AronTreeWidgetIntInputModal.ui
- aronTreeWidget/modal/float_double/AronTreeWidgetFloatInputModal.ui
- aronTreeWidget/modal/bool/AronTreeWidgetBoolInputModal.ui
aronTreeWidget/modal/text/AronTreeWidgetTextInputModal.ui
- aronTreeWidget/modal/dict/AronTreeWidgetDictInputModal.ui
)
# Add more libraries you depend on here, e.g. ${QT_LIBRARIES}.
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/ColorPalettes.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/ColorPalettes.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..05cd7a5cc08224561af8df64cf6be2347adb3e37
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/ColorPalettes.cpp
@@ -0,0 +1,32 @@
+#include "ColorPalettes.h"
+
+namespace armarx::gui_color_palette
+{
+ QPalette
+ getErrorPalette()
+ {
+ QPalette errorPalette;
+ errorPalette.setColor(QPalette::Base, Qt::red);
+ return errorPalette;
+ }
+ QPalette
+ getNormalPalette()
+ {
+
+ QPalette normalPalette;
+
+ normalPalette.setColor(QPalette::Base, Qt::white);
+ return normalPalette;
+ }
+
+ QPalette
+ getIndirectErrorPalette()
+ {
+ QPalette indirectErr;
+ static QColor orange = QColor::fromRgb(255, 165, 0);
+ // orange color
+ indirectErr.setColor(QPalette::Base, orange);
+ return indirectErr;
+ }
+
+} // namespace armarx::gui_color_palette
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/ColorPalettes.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/ColorPalettes.h
new file mode 100644
index 0000000000000000000000000000000000000000..ea2c247a9428559de4be7bdb8fc1497c14b1ea00
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/ColorPalettes.h
@@ -0,0 +1,11 @@
+#pragma once
+#include <QPalette>
+
+namespace armarx::gui_color_palette
+{
+ QPalette getNormalPalette();
+
+ QPalette getErrorPalette();
+
+ QPalette getIndirectErrorPalette();
+} // namespace armarx::gui_color_palette
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidget.ui b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidget.ui
index 3aa3984c65aad167af2b49e94ab34130c880c998..1e80202efb18b2aca77e8ba45b87827e362ae233 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidget.ui
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidget.ui
@@ -6,145 +6,187 @@
<rect>
<x>0</x>
<y>0</y>
- <width>1015</width>
- <height>498</height>
+ <width>1060</width>
+ <height>657</height>
</rect>
</property>
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Expanding" vsizetype="Expanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
<property name="windowTitle">
<string>SkillManagerMonitorWidget</string>
</property>
- <layout class="QVBoxLayout" name="verticalLayout">
- <item>
- <widget class="QGroupBox" name="groupBoxActiveSkills">
+ <layout class="QGridLayout" name="gridLayout_3">
+ <item row="0" column="0">
+ <widget class="QSplitter" name="splitter_2">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
- <property name="title">
- <string>Active Skills</string>
- </property>
- <layout class="QGridLayout" name="gridLayout_3">
- <item row="0" column="0">
- <widget class="QListWidget" name="listWidgetActiveSkills">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Expanding" vsizetype="Expanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- </widget>
- </item>
- </layout>
- </widget>
- </item>
- <item>
- <widget class="QSplitter" name="splitter">
<property name="orientation">
- <enum>Qt::Horizontal</enum>
+ <enum>Qt::Vertical</enum>
+ </property>
+ <property name="childrenCollapsible">
+ <bool>false</bool>
</property>
- <widget class="QGroupBox" name="groupBoxSkills">
+ <widget class="QGroupBox" name="groupBoxActiveSkills">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Expanding" vsizetype="Expanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
<property name="title">
- <string>Manager</string>
+ <string>Active Skills</string>
</property>
- <layout class="QGridLayout" name="gridLayout">
- <item row="2" column="0">
- <widget class="QLabel" name="label">
- <property name="text">
- <string>Update Frequency:</string>
+ <layout class="QGridLayout" name="gridLayout_4">
+ <item row="0" column="0">
+ <widget class="QListWidget" name="listWidgetActiveSkills">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Expanding" vsizetype="Expanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
</property>
</widget>
</item>
- <item row="3" column="0" colspan="3">
- <widget class="QTreeWidget" name="treeWidgetSkills">
- <column>
+ </layout>
+ </widget>
+ <widget class="QSplitter" name="splitter">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Expanding" vsizetype="Expanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <widget class="QGroupBox" name="groupBoxSkills">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Expanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="title">
+ <string>Manager</string>
+ </property>
+ <layout class="QGridLayout" name="gridLayout">
+ <item row="2" column="0">
+ <widget class="QLabel" name="label">
<property name="text">
- <string>Skill</string>
+ <string>Update Frequency:</string>
</property>
- </column>
- <column>
- <property name="text">
- <string>HasType</string>
+ </widget>
+ </item>
+ <item row="3" column="0" colspan="3">
+ <widget class="QTreeWidget" name="treeWidgetSkills">
+ <column>
+ <property name="text">
+ <string>Skill</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string>HasType</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string>State</string>
+ </property>
+ </column>
+ </widget>
+ </item>
+ <item row="2" column="1">
+ <widget class="QDoubleSpinBox" name="doubleSpinBoxUpdateFreq"/>
+ </item>
+ </layout>
+ </widget>
+ <widget class="QGroupBox" name="groupBoxSkillDetails">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Expanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="title">
+ <string>Skill Details</string>
+ </property>
+ <layout class="QGridLayout" name="gridLayout_2">
+ <item row="1" column="0" colspan="4">
+ <widget class="QTreeWidget" name="treeWidgetSkillDetails">
+ <property name="contextMenuPolicy">
+ <enum>Qt::CustomContextMenu</enum>
+ </property>
+ <property name="editTriggers">
+ <set>QAbstractItemView::DoubleClicked|QAbstractItemView::EditKeyPressed</set>
</property>
- </column>
- <column>
+ <column>
+ <property name="text">
+ <string>Key</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string>Value</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string>Type</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string>defaultValue</string>
+ </property>
+ </column>
+ </widget>
+ </item>
+ <item row="6" column="3">
+ <widget class="QPushButton" name="pushButtonExecuteSkill">
<property name="text">
- <string>State</string>
+ <string>Request Execution</string>
</property>
- </column>
- </widget>
- </item>
- <item row="2" column="1">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxUpdateFreq"/>
- </item>
- </layout>
- </widget>
- <widget class="QGroupBox" name="groupBoxSkillDetails">
- <property name="title">
- <string>Skill Details</string>
- </property>
- <layout class="QGridLayout" name="gridLayout_2">
- <item row="1" column="0" colspan="4">
- <widget class="QTreeWidget" name="treeWidgetSkillDetails">
- <column>
+ </widget>
+ </item>
+ <item row="0" column="0">
+ <widget class="QPushButton" name="pushButtonPaste">
<property name="text">
- <string>Key</string>
+ <string>Set from clipboard</string>
</property>
- </column>
- <column>
+ </widget>
+ </item>
+ <item row="6" column="0">
+ <widget class="QPushButton" name="pushButtonStopSkill">
<property name="text">
- <string>Value</string>
+ <string>Stop current skill</string>
</property>
- </column>
- <column>
+ </widget>
+ </item>
+ <item row="0" column="3">
+ <widget class="QPushButton" name="pushButtonReset">
<property name="text">
- <string>Type</string>
+ <string>Reset args</string>
</property>
- </column>
- <column>
+ </widget>
+ </item>
+ <item row="0" column="1">
+ <widget class="QPushButton" name="pushButtonCopy">
<property name="text">
- <string>defaultValue</string>
+ <string>Copy args to clipboard</string>
</property>
- </column>
- </widget>
- </item>
- <item row="6" column="3">
- <widget class="QPushButton" name="pushButtonExecuteSkill">
- <property name="text">
- <string>Request Execution</string>
- </property>
- </widget>
- </item>
- <item row="0" column="0">
- <widget class="QPushButton" name="pushButtonPaste">
- <property name="text">
- <string>Set from clipboard</string>
- </property>
- </widget>
- </item>
- <item row="6" column="0">
- <widget class="QPushButton" name="pushButtonStopSkill">
- <property name="text">
- <string>Stop current skill</string>
- </property>
- </widget>
- </item>
- <item row="0" column="3">
- <widget class="QPushButton" name="pushButtonReset">
- <property name="text">
- <string>Reset args</string>
- </property>
- </widget>
- </item>
- <item row="0" column="1">
- <widget class="QPushButton" name="pushButtonCopy">
- <property name="text">
- <string>Copy args to clipboard</string>
- </property>
- </widget>
- </item>
- </layout>
+ </widget>
+ </item>
+ </layout>
+ </widget>
</widget>
</widget>
</item>
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp
index 27a954593ad99d252410ffec11d086689c1d9e07..edc9eebb6d619dd09de033ac2959b4858d263246 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp
@@ -20,52 +20,57 @@
* GNU General Public License
*/
+#include "SkillManagerMonitorWidgetController.h"
+
#include <string>
-#include "SkillManagerMonitorWidgetController.h"
+#include <RobotAPI/libraries/skills/provider/Skill.h>
-#include "aronTreeWidget/visitors/AronTreeWidgetCreator.h"
#include "aronTreeWidget/visitors/AronTreeWidgetConverter.h"
+#include "aronTreeWidget/visitors/AronTreeWidgetCreator.h"
#include "aronTreeWidget/visitors/AronTreeWidgetModalCreator.h"
-#include <RobotAPI/libraries/skills/provider/Skill.h>
-
// modals
#include "aronTreeWidget/modal/text/AronTreeWidgetTextInputModalController.h"
// debug
-#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
-
-#include <QDoubleSpinBox>
#include <QClipboard>
+#include <QDoubleSpinBox>
+
+#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
#include "aronTreeWidget/Data.h"
//config
namespace armarx
{
- QPointer<QDialog> SkillManagerMonitorWidgetController::getConfigDialog(QWidget* parent)
+ QPointer<QDialog>
+ SkillManagerMonitorWidgetController::getConfigDialog(QWidget* parent)
{
if (!dialog)
{
dialog = new SimpleConfigDialog(parent);
- dialog->addProxyFinder<skills::manager::dti::SkillManagerInterfacePrx>("SkillManager", "", "Skill*");
+ dialog->addProxyFinder<skills::manager::dti::SkillManagerInterfacePrx>(
+ "SkillManager", "", "Skill*");
}
return qobject_cast<SimpleConfigDialog*>(dialog);
}
- void SkillManagerMonitorWidgetController::configured()
+ void
+ SkillManagerMonitorWidgetController::configured()
{
observerName = dialog->getProxyName("SkillManager");
}
- void SkillManagerMonitorWidgetController::loadSettings(QSettings* settings)
+ void
+ SkillManagerMonitorWidgetController::loadSettings(QSettings* settings)
{
observerName = settings->value("SkillManager", "SkillManager").toString().toStdString();
}
- void SkillManagerMonitorWidgetController::saveSettings(QSettings* settings)
+ void
+ SkillManagerMonitorWidgetController::saveSettings(QSettings* settings)
{
settings->setValue("SkillManager", QString::fromStdString(observerName));
}
-}
+} // namespace armarx
// Others
namespace armarx
@@ -81,29 +86,40 @@ namespace armarx
widget.doubleSpinBoxUpdateFreq->setSuffix(" Hz");
refreshSkillsResultTimer = new QTimer(this);
- refreshSkillsResultTimer->setInterval(1000 / 5); // Keep this stable.
+ refreshSkillsResultTimer->setInterval(1000 / 5); // Keep this stable.
refreshSkillsResultTimer->start();
- connect(widget.doubleSpinBoxUpdateFreq, &QDoubleSpinBox::editingFinished,
- this, &SkillManagerMonitorWidgetController::updateTimerFrequency);
- connect(refreshSkillsResultTimer, &QTimer::timeout,
- this, &SkillManagerMonitorWidgetController::refreshSkills);
-
- connect(widget.pushButtonCopy, &QPushButton::clicked,
- this, &SkillManagerMonitorWidgetController::copyCurrentConfig);
- connect(widget.pushButtonPaste, &QPushButton::clicked,
- this, &SkillManagerMonitorWidgetController::pasteCurrentConfig);
-
- connect(widget.pushButtonExecuteSkill, &QPushButton::clicked,
- this, &SkillManagerMonitorWidgetController::executeSkill);
- connect(widget.pushButtonStopSkill, &QPushButton::clicked,
- this, &SkillManagerMonitorWidgetController::stopSkill);
-
- connect(widget.treeWidgetSkills, &QTreeWidget::currentItemChanged,
- this, &SkillManagerMonitorWidgetController::skillSelectionChanged);
-
- connect(widget.treeWidgetSkillDetails, &QTreeWidget::itemDoubleClicked,
- this, &SkillManagerMonitorWidgetController::onTreeWidgetItemDoubleClicked);
+ connect(widget.doubleSpinBoxUpdateFreq,
+ &QDoubleSpinBox::editingFinished,
+ this,
+ &SkillManagerMonitorWidgetController::updateTimerFrequency);
+ connect(refreshSkillsResultTimer,
+ &QTimer::timeout,
+ this,
+ &SkillManagerMonitorWidgetController::refreshSkills);
+
+ connect(widget.pushButtonCopy,
+ &QPushButton::clicked,
+ this,
+ &SkillManagerMonitorWidgetController::copyCurrentConfig);
+ connect(widget.pushButtonPaste,
+ &QPushButton::clicked,
+ this,
+ &SkillManagerMonitorWidgetController::pasteCurrentConfig);
+
+ connect(widget.pushButtonExecuteSkill,
+ &QPushButton::clicked,
+ this,
+ &SkillManagerMonitorWidgetController::executeSkill);
+ connect(widget.pushButtonStopSkill,
+ &QPushButton::clicked,
+ this,
+ &SkillManagerMonitorWidgetController::stopSkill);
+
+ connect(widget.treeWidgetSkills,
+ &QTreeWidget::currentItemChanged,
+ this,
+ &SkillManagerMonitorWidgetController::skillSelectionChanged);
}
SkillManagerMonitorWidgetController::~SkillManagerMonitorWidgetController()
@@ -111,25 +127,38 @@ namespace armarx
}
- void SkillManagerMonitorWidgetController::onInitComponent()
+ void
+ SkillManagerMonitorWidgetController::reconnectToSkillManager()
+ {
+ if (connected)
+ {
+ getProxy(manager, observerName, 1000);
+ }
+ }
+
+ void
+ SkillManagerMonitorWidgetController::onInitComponent()
{
usingProxy(observerName);
}
- void SkillManagerMonitorWidgetController::onConnectComponent()
+ void
+ SkillManagerMonitorWidgetController::onConnectComponent()
{
- getProxy(manager, observerName);
widget.groupBoxSkills->setTitle(QString::fromStdString(observerName));
- widget.treeWidgetSkillDetails->setEditTriggers(QAbstractItemView::EditTrigger::NoEditTriggers);
+ widget.treeWidgetSkillDetails->setEditTriggers(
+ QAbstractItemView::EditTrigger::NoEditTriggers);
widget.treeWidgetSkillDetails->setColumnHidden(3, true);
connected = true;
}
- void SkillManagerMonitorWidgetController::onDisconnectComponent()
+ void
+ SkillManagerMonitorWidgetController::onDisconnectComponent()
{
connected = false;
+ manager = nullptr;
// reset all
skills.clear();
@@ -140,25 +169,30 @@ namespace armarx
selectedSkill.skillName = "";
}
- void SkillManagerMonitorWidgetController::updateTimerFrequency()
+ void
+ SkillManagerMonitorWidgetController::updateTimerFrequency()
{
int f = static_cast<int>(std::round(1000 / widget.doubleSpinBoxUpdateFreq->value()));
refreshSkillsResultTimer->setInterval(f);
}
- void SkillManagerMonitorWidgetController::refreshSkills()
+ void
+ SkillManagerMonitorWidgetController::refreshSkills()
{
- static std::map<skills::provider::dto::Execution::Status, std::string> ExecutionStatus2String = {
- {skills::provider::dto::Execution::Status::Aborted, "Aborted"},
- {skills::provider::dto::Execution::Status::Failed, "Failed"},
- {skills::provider::dto::Execution::Status::Idle, "Not yet started"},
- {skills::provider::dto::Execution::Status::Running, "Running"},
- {skills::provider::dto::Execution::Status::Scheduled, "Scheduled"},
- {skills::provider::dto::Execution::Status::Succeeded, "Succeeded"}
- };
-
- if (!connected)
+ static std::map<skills::provider::dto::Execution::Status, std::string>
+ ExecutionStatus2String = {
+ {skills::provider::dto::Execution::Status::Aborted, "Aborted"},
+ {skills::provider::dto::Execution::Status::Failed, "Failed"},
+ {skills::provider::dto::Execution::Status::Idle, "Not yet started"},
+ {skills::provider::dto::Execution::Status::Running, "Running"},
+ {skills::provider::dto::Execution::Status::Scheduled, "Scheduled"},
+ {skills::provider::dto::Execution::Status::Succeeded, "Succeeded"}};
+
+ if (!manager)
+ {
+ reconnectToSkillManager();
return;
+ }
/* CHECK OWN SKILLS LIST */
// remove non-existing ones
@@ -196,7 +230,9 @@ namespace armarx
while (i < widget.treeWidgetSkills->topLevelItemCount())
{
QTreeWidgetItem* item = widget.treeWidgetSkills->topLevelItem(i);
- if (std::find(removedProviders.begin(), removedProviders.end(), item->text(0).toStdString()) != removedProviders.end())
+ if (std::find(removedProviders.begin(),
+ removedProviders.end(),
+ item->text(0).toStdString()) != removedProviders.end())
{
delete widget.treeWidgetSkills->takeTopLevelItem(i);
}
@@ -209,7 +245,8 @@ namespace armarx
// add new providers
for (const auto& [providerName, providerSkills] : skills)
{
- if (auto it = std::find(newProviders.begin(), newProviders.end(), providerName); it != newProviders.end())
+ if (auto it = std::find(newProviders.begin(), newProviders.end(), providerName);
+ it != newProviders.end())
{
auto item = new QTreeWidgetItem(widget.treeWidgetSkills);
item->setText(0, QString::fromStdString(providerName));
@@ -225,7 +262,7 @@ namespace armarx
// update status and active skills window
std::map<skills::SkillID, std::string> activeSkillsAndPrefixes;
auto managerStatuses = manager->getSkillExecutionStatuses();
- for (int i = 0; i < widget.treeWidgetSkills->topLevelItemCount(); ++i)
+ for (int i = 0; i < widget.treeWidgetSkills->topLevelItemCount(); ++i)
{
try
{
@@ -240,11 +277,15 @@ namespace armarx
skills::SkillID currentSkillId(providerName, skillItem->text(0).toStdString());
auto statusForSkill = allStatusesForProvider.at(currentSkillId.skillName);
- skillItem->setText(2, QString::fromStdString(ExecutionStatus2String.at(statusForSkill.header.status)));
+ skillItem->setText(2,
+ QString::fromStdString(ExecutionStatus2String.at(
+ statusForSkill.header.status)));
- if (not statusForSkill.header.executorName.empty()) // it means that the skill was called by someone
+ if (not statusForSkill.header.executorName
+ .empty()) // it means that the skill was called by someone
{
- activeSkillsAndPrefixes.insert({currentSkillId, statusForSkill.header.executorName});
+ activeSkillsAndPrefixes.insert(
+ {currentSkillId, statusForSkill.header.executorName});
}
}
}
@@ -261,7 +302,8 @@ namespace armarx
{
auto prefixedStr = id.toString(prefix);
bool longest = true;
- for (const auto& [id2, prefix2] : activeSkillsAndPrefixes) // check if there is a deeper skill currently executing
+ for (const auto& [id2, prefix2] :
+ activeSkillsAndPrefixes) // check if there is a deeper skill currently executing
{
auto prefixedStr2 = id.toString(prefix2);
if (prefixedStr == prefixedStr2)
@@ -278,12 +320,14 @@ namespace armarx
if (longest)
{
- widget.listWidgetActiveSkills->addItem(QString::fromStdString(id.toString() + ": " + id.toString(prefix)));
+ widget.listWidgetActiveSkills->addItem(
+ QString::fromStdString(id.toString() + ": " + id.toString(prefix)));
}
}
}
- void SkillManagerMonitorWidgetController::executeSkill()
+ void
+ SkillManagerMonitorWidgetController::executeSkill()
{
if (selectedSkill.providerName.empty() or selectedSkill.skillName.empty())
{
@@ -307,12 +351,14 @@ namespace armarx
exInfo.skillId = {selectedSkill.providerName, selectedSkill.skillName};
exInfo.params = aron::data::Dict::ToAronDictDTO(data);
- ARMARX_IMPORTANT << "Executing skill from GUI: " << selectedSkill.providerName << "/" << selectedSkill.skillName << ". The data was: " << data;
+ ARMARX_IMPORTANT << "Executing skill from GUI: " << selectedSkill.providerName << "/"
+ << selectedSkill.skillName << ". The data was: " << data;
// Note that we execute the skill in a seperate thread so that the GUI thread does not freeze.
manager->begin_executeSkill(exInfo);
}
- void SkillManagerMonitorWidgetController::stopSkill()
+ void
+ SkillManagerMonitorWidgetController::stopSkill()
{
if (selectedSkill.providerName.empty() or selectedSkill.skillName.empty())
{
@@ -325,11 +371,14 @@ namespace armarx
return;
}
- ARMARX_INFO << "Stopping skill from GUI: " << selectedSkill.providerName << "/" << selectedSkill.skillName;
+ ARMARX_INFO << "Stopping skill from GUI: " << selectedSkill.providerName << "/"
+ << selectedSkill.skillName;
manager->abortSkill(selectedSkill.providerName, selectedSkill.skillName);
}
- void SkillManagerMonitorWidgetController::skillSelectionChanged(QTreeWidgetItem* current, QTreeWidgetItem*)
+ void
+ SkillManagerMonitorWidgetController::skillSelectionChanged(QTreeWidgetItem* current,
+ QTreeWidgetItem*)
{
widget.groupBoxSkillDetails->setEnabled(false);
@@ -352,10 +401,12 @@ namespace armarx
newSelectedSkill.skillName = current->text(0).toStdString();
// setup groupBox
- widget.groupBoxSkillDetails->setTitle(QString::fromStdString(newSelectedSkill.providerName + "/" + newSelectedSkill.skillName));
+ widget.groupBoxSkillDetails->setTitle(QString::fromStdString(
+ newSelectedSkill.providerName + "/" + newSelectedSkill.skillName));
widget.groupBoxSkillDetails->setEnabled(true);
- if (newSelectedSkill.providerName == selectedSkill.providerName and newSelectedSkill.skillName == selectedSkill.skillName)
+ if (newSelectedSkill.providerName == selectedSkill.providerName and
+ newSelectedSkill.skillName == selectedSkill.skillName)
{
return;
}
@@ -370,37 +421,46 @@ namespace armarx
auto skillDesc = skills.at(selectedSkill.providerName).at(selectedSkill.skillName);
{
- auto it = new QTreeWidgetItem(widget.treeWidgetSkillDetails,
- {QString::fromStdString("Name"), QString::fromStdString(skillDesc.skillName)});
+ auto it = new QTreeWidgetItem(
+ widget.treeWidgetSkillDetails,
+ {QString::fromStdString("Name"), QString::fromStdString(skillDesc.skillName)});
widget.treeWidgetSkillDetails->addTopLevelItem(it);
}
{
- auto it = new QTreeWidgetItem(widget.treeWidgetSkillDetails,
- {QString::fromStdString("Robot"), QString::fromStdString(simox::alg::join(skillDesc.robots, ", "))});
+ auto it = new QTreeWidgetItem(
+ widget.treeWidgetSkillDetails,
+ {QString::fromStdString("Robot"),
+ QString::fromStdString(simox::alg::join(skillDesc.robots, ", "))});
widget.treeWidgetSkillDetails->addTopLevelItem(it);
}
{
auto it = new QTreeWidgetItem(widget.treeWidgetSkillDetails,
- {QString::fromStdString("Description"), QString::fromStdString(skillDesc.description)});
+ {QString::fromStdString("Description"),
+ QString::fromStdString(skillDesc.description)});
widget.treeWidgetSkillDetails->addTopLevelItem(it);
}
{
- auto it = new QTreeWidgetItem(widget.treeWidgetSkillDetails,
- {QString::fromStdString("Timeout"), QString::fromStdString(std::to_string(skillDesc.timeoutMs)) + " ms"});
+ auto it = new QTreeWidgetItem(
+ widget.treeWidgetSkillDetails,
+ {QString::fromStdString("Timeout"),
+ QString::fromStdString(std::to_string(skillDesc.timeoutMs)) + " ms"});
widget.treeWidgetSkillDetails->addTopLevelItem(it);
}
- skillsArgumentsTreeWidgetItem = new QTreeWidgetItem(widget.treeWidgetSkillDetails, {QString::fromStdString("Arguments")});
+ skillsArgumentsTreeWidgetItem = new QTreeWidgetItem(widget.treeWidgetSkillDetails,
+ {QString::fromStdString("Arguments")});
auto aron_args = aron::type::Object::FromAronObjectDTO(skillDesc.acceptedType);
auto default_args = aron::data::Dict::FromAronDictDTO(skillDesc.defaultParams);
- aronTreeWidgetController = std::make_shared<AronTreeWidgetController>(widget.treeWidgetSkillDetails, skillsArgumentsTreeWidgetItem, aron_args, default_args);
+ aronTreeWidgetController = std::make_shared<AronTreeWidgetController>(
+ widget.treeWidgetSkillDetails, skillsArgumentsTreeWidgetItem, aron_args, default_args);
}
- aron::data::DictPtr SkillManagerMonitorWidgetController::getConfigAsAron() const
+ aron::data::DictPtr
+ SkillManagerMonitorWidgetController::getConfigAsAron() const
{
// create argument aron (if there is an accepted type set)
if (aronTreeWidgetController)
@@ -410,7 +470,8 @@ namespace armarx
return nullptr;
}
- void SkillManagerMonitorWidgetController::copyCurrentConfig()
+ void
+ SkillManagerMonitorWidgetController::copyCurrentConfig()
{
auto data = getConfigAsAron();
if (!data)
@@ -423,7 +484,8 @@ namespace armarx
clipboard->setText(QString::fromStdString(json.dump(2)));
}
- void SkillManagerMonitorWidgetController::pasteCurrentConfig()
+ void
+ SkillManagerMonitorWidgetController::pasteCurrentConfig()
{
QClipboard* clipboard = QApplication::clipboard();
std::string s = clipboard->text().toStdString();
@@ -438,34 +500,10 @@ namespace armarx
aronTreeWidgetController->setFromAron(data);
}
- void SkillManagerMonitorWidgetController::resetCurrentConfig()
+ void
+ SkillManagerMonitorWidgetController::resetCurrentConfig()
{
// TODO
}
- void SkillManagerMonitorWidgetController::onTreeWidgetItemDoubleClicked(QTreeWidgetItem* item, int column)
- {
- if (!item)
- {
- return;
- }
-
- if (column == 1)
- {
- if (item->flags() & Qt::ItemIsEditable) // we use the flag to indicate whether the item is editable or not
- {
- // we assume its aron item
- AronTreeWidgetItem* aItem = AronTreeWidgetItem::DynamicCastAndCheck(item);
- std::string name = aItem->text(aron_tree_widget::constantes::TREE_WIDGET_ITEM_NAME).toStdString();
- std::string type = aItem->text(aron_tree_widget::constantes::TREE_WIDGET_ITEM_TYPE).toStdString();
-
- // why visitor?!?!?
- AronTreeWidgetModalCreatorVisitor v(name, aItem, widget.treeWidgetSkillDetails);
- aron::type::visit(v, aItem->aronType);
- auto modal = v.createdModal;
- modal->exec();
- }
- }
- }
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.h
index 64256f2d6530d766726df37ec5fc60c9429d3e70..d9a9379bdfdea0ee56c9c5f3e601ef68a5ce75b5 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.h
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.h
@@ -22,25 +22,24 @@
#pragma once
#include <stack>
-#include <vector>
#include <thread>
+#include <vector>
+
+#include <QTimer>
+
#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
-#include <RobotAPI/interface/skills/SkillMemoryInterface.h>
-
#include <RobotAPI/gui-plugins/SkillManagerPlugin/ui_SkillManagerMonitorWidget.h>
-
-#include "aronTreeWidget/AronTreeWidgetController.h"
-
-#include <RobotAPI/libraries/aron/core/type/variant/All.h>
+#include <RobotAPI/interface/skills/SkillMemoryInterface.h>
#include <RobotAPI/libraries/aron/core/data/variant/All.h>
+#include <RobotAPI/libraries/aron/core/type/variant/All.h>
#include <RobotAPI/libraries/aron/core/type/visitor/variant/VariantVisitor.h>
-#include <QTimer>
+#include "aronTreeWidget/AronTreeWidgetController.h"
namespace armarx
{
@@ -71,6 +70,8 @@ namespace armarx
return "Skills.Manager";
}
+ void reconnectToSkillManager();
+
void onInitComponent() override;
void onConnectComponent() override;
void onDisconnectComponent() override;
@@ -88,7 +89,6 @@ namespace armarx
void pasteCurrentConfig();
void resetCurrentConfig();
- void onTreeWidgetItemDoubleClicked(QTreeWidgetItem * item, int column);
private:
aron::data::DictPtr getConfigAsAron() const;
@@ -120,11 +120,13 @@ namespace armarx
AronTreeWidgetControllerPtr aronTreeWidgetController = nullptr;
// others
- std::atomic_bool connected = false;
QTimer* refreshSkillsResultTimer;
// skillExecutions
std::vector<std::thread> executions;
+
+ // connected flag
+ std::atomic_bool connected = false;
};
}
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/TODO b/source/RobotAPI/gui-plugins/SkillManagerPlugin/TODO
new file mode 100644
index 0000000000000000000000000000000000000000..d25aead078c039b5724f9a6f05abe669ed1caf50
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/TODO
@@ -0,0 +1,3 @@
+- keep set values around to not loose precision of values that have not been manipulated (in AronTreeWidgetItem, as well as Matrix and Quaternion)
+- get rid of any todos present
+
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/AronTreeWidgetController.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/AronTreeWidgetController.cpp
index 0ef63ff506ebb1f624f7380e61309f02c522b915..52110b494a2bfc38a361d8354eb18efccc760617 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/AronTreeWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/AronTreeWidgetController.cpp
@@ -1,43 +1,62 @@
#include "AronTreeWidgetController.h"
+#include <RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetModalCreator.h>
+
+#include "visitors/AronTreeWidgetContextMenu.h"
#include "visitors/AronTreeWidgetConverter.h"
#include "visitors/AronTreeWidgetSetter.h"
+
namespace armarx
{
- AronTreeWidgetController::AronTreeWidgetController(QTreeWidget* tree, QTreeWidgetItem* parent, const aron::type::ObjectPtr& type, const aron::data::DictPtr& data):
- parent(parent),
- tree(tree),
- type(type)
+ AronTreeWidgetController::AronTreeWidgetController(QTreeWidget* tree,
+ QTreeWidgetItem* parent,
+ const aron::type::ObjectPtr& type,
+ const aron::data::DictPtr& data) :
+ parent(parent), tree(tree), type(type)
{
- if (type) // if there is a type set, we create a tree widget from the typp
+ connect(tree,
+ SIGNAL(customContextMenuRequested(const QPoint&)),
+ this,
+ SLOT(ShowContextMenu(const QPoint&)));
+ connect(tree,
+ &QTreeWidget::itemDoubleClicked,
+ this,
+ &AronTreeWidgetController::onTreeWidgetItemDoubleClicked);
+
+ if (type) // if there is a type set, we create a tree widget from the type
{
- AronTreeWidgetCreatorVisitor v;
+ AronTreeWidgetCreatorVisitor v(parent);
+ v.setTopLevelWidget(tree);
aron::type::visit(v, type);
- if (v.createdQWidgetItem)
- {
- parent->addChild(v.createdQWidgetItem);
- }
-
if (data) // check if there is a default argument set. Prefill the GUI with it
{
setFromAron(data);
}
}
- else if(data) // there is no type but a default configuration. Prefill the GUI with the default arguments
+ // there is no type but a default configuration. Prefill the GUI with the default arguments
+ else if (data)
{
+ // TODO: There is no visitor for that (yet)...
// create type from data, ...
}
else
{
new QTreeWidgetItem(parent, {QString::fromStdString("No args")});
}
+
+ // connect change handling after args init
+ connect(tree,
+ &QTreeWidget::itemChanged,
+ this,
+ &AronTreeWidgetController::onTreeWidgetItemChanged);
}
- aron::data::DictPtr AronTreeWidgetController::convertToAron() const
+ aron::data::DictPtr
+ AronTreeWidgetController::convertToAron() const
{
- if (parent)
+ if (parent && type)
{
AronTreeWidgetConverterVisitor v(parent, 0);
aron::type::visit(v, type);
@@ -48,7 +67,8 @@ namespace armarx
return nullptr;
}
- void AronTreeWidgetController::setFromAron(const aron::data::DictPtr& data)
+ void
+ AronTreeWidgetController::setFromAron(const aron::data::DictPtr& data)
{
if (parent)
{
@@ -56,4 +76,87 @@ namespace armarx
aron::data::visit(v, data);
}
}
-}
+ void
+ AronTreeWidgetController::ShowContextMenu(const QPoint& pos)
+ {
+ tree->blockSignals(true);
+
+ auto idx = tree->indexAt(pos);
+ AronTreeWidgetItem* clickedItem = AronTreeWidgetItem::DynamicCast(tree->itemAt(pos));
+ if (clickedItem)
+ {
+ AronTreeWidgetContextMenuVisitor visitor(clickedItem, pos, tree, idx.row());
+
+ aron::type::visit(visitor, clickedItem->aronType);
+ visitor.showMenuAndExecute();
+ }
+
+ tree->blockSignals(false);
+ }
+ void
+ AronTreeWidgetController::onTreeWidgetItemDoubleClicked(QTreeWidgetItem* item, int column)
+ {
+ if (!item)
+ {
+ return;
+ }
+ tree->blockSignals(true);
+
+ auto* aronItem = AronTreeWidgetItem::DynamicCast(item);
+ if (column == 1 && aronItem)
+ {
+
+
+ std::string name =
+ aronItem->text(aron_tree_widget::constantes::TREE_WIDGET_ITEM_NAME).toStdString();
+ // depending on aron type, create extra gui element.
+ AronTreeWidgetModalCreatorVisitor v(name, aronItem, tree);
+ aron::type::visit(v, aronItem->aronType);
+ auto modal = v.createdModal;
+
+ // if no modal is created, we instead use the edit field directly
+ if (modal)
+ {
+ modal->exec();
+ }
+ else if (aronItem->col1Editable)
+ {
+ item->treeWidget()->editItem(item, column);
+ }
+ }
+ else if (column == 0 && aronItem && aronItem->col0Editable)
+ {
+ item->treeWidget()->editItem(item, column);
+ }
+
+ tree->blockSignals(false);
+ }
+
+ void
+ AronTreeWidgetController::onTreeWidgetItemChanged(QTreeWidgetItem* item, int column)
+ {
+ tree->blockSignals(true);
+
+ auto* aronElem = AronTreeWidgetItem::DynamicCast(item);
+ if (aronElem)
+ {
+ aronElem->onUserChange(column);
+ }
+ // start conversion for entire tree -- this also sets the highlighting
+ if (parent->childCount() == 1)
+ {
+ auto* aronTreeRoot = AronTreeWidgetItem::DynamicCast(parent->child(0));
+ aronTreeRoot->resetError();
+ if (aronTreeRoot)
+ {
+ AronTreeWidgetConverterVisitor v(parent, 0);
+ aron::type::visit(v, type);
+ aronTreeRoot->setValueErrorState(v.hasDirectError(), v.onlyChildFailedConversion());
+ }
+ }
+ // else perhaps the GUI was stopped or died.
+
+ tree->blockSignals(false);
+ }
+
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/AronTreeWidgetController.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/AronTreeWidgetController.h
index c70b0277b41684a7144319a3c9d07fcf7dd31668..c3c2c9c2c913fba11fdd4dfc362e98297c4338f4 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/AronTreeWidgetController.h
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/AronTreeWidgetController.h
@@ -1,24 +1,30 @@
#pragma once
#include <stack>
-#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include "Data.h"
+#include <QTreeWidget>
+
+#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
-#include <QTreeWidget>
-#include "visitors/AronTreeWidgetCreator.h"
#include "AronTreeWidgetItem.h"
+#include "Data.h"
+#include "visitors/AronTreeWidgetCreator.h"
namespace armarx
{
- class AronTreeWidgetController
+ // Main controller for any AronTreeWidget GUI. It attaches itself to the parent and needs the active widget.
+ // It's responsible to handle all signals for non-widget fields and click events.
+ class AronTreeWidgetController : public QObject
{
-
+ Q_OBJECT
public:
- AronTreeWidgetController(QTreeWidget* tree, QTreeWidgetItem* parent, const aron::type::ObjectPtr& type, const aron::data::DictPtr& data = nullptr);
+ AronTreeWidgetController(QTreeWidget* tree,
+ QTreeWidgetItem* parent,
+ const aron::type::ObjectPtr& type,
+ const aron::data::DictPtr& data = nullptr);
aron::data::DictPtr convertToAron() const;
void setFromAron(const aron::data::DictPtr&);
@@ -28,7 +34,19 @@ namespace armarx
QTreeWidget* tree;
aron::type::ObjectPtr type;
+
+ private slots:
+ // allows most primitive fields to be directly editable in the tree
+ // only String will open up a new widget
+ void onTreeWidgetItemDoubleClicked(QTreeWidgetItem* item, int column);
+ // check the new user input. Maybe undo if the field must not be edited.
+ // Also highlight if the input cannot be parsed. (Or the errors are now fixed)
+ void onTreeWidgetItemChanged(QTreeWidgetItem* item, int column);
+
+ public slots:
+ // hook for items to show a context menu (add / delete element)
+ void ShowContextMenu(const QPoint&);
};
using AronTreeWidgetControllerPtr = std::shared_ptr<AronTreeWidgetController>;
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/AronTreeWidgetItem.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/AronTreeWidgetItem.cpp
index e53161bb8b2ee4aee0c34ac2fc82c3214894f0cd..29d895f7b0f9666ebf42acd2bf3046ac90d7f1db 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/AronTreeWidgetItem.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/AronTreeWidgetItem.cpp
@@ -1,21 +1,24 @@
#include "AronTreeWidgetItem.h"
+#include <QAction>
+#include <QMenu>
+
#include <RobotAPI/libraries/aron/core/type/variant/All.h>
+#include "../ColorPalettes.h"
+#include "visitors/AronTreeWidgetConverter.h"
+#include "widgets/CustomWidget.h"
+
namespace armarx
{
- AronTreeWidgetItem* AronTreeWidgetItem::DynamicCast(QTreeWidgetItem* i)
+ AronTreeWidgetItem*
+ AronTreeWidgetItem::DynamicCast(QTreeWidgetItem* i)
{
return dynamic_cast<AronTreeWidgetItem*>(i);
}
- AronTreeWidgetItem* AronTreeWidgetItem::copy()
- {
- AronTreeWidgetItem* ret = new AronTreeWidgetItem(*this);
- return ret;
- }
-
- AronTreeWidgetItem* AronTreeWidgetItem::DynamicCastAndCheck(QTreeWidgetItem* i)
+ AronTreeWidgetItem*
+ AronTreeWidgetItem::DynamicCastAndCheck(QTreeWidgetItem* i)
{
if (!i)
{
@@ -25,4 +28,174 @@ namespace armarx
ARMARX_CHECK_NOT_NULL(c);
return c;
}
-}
+
+ bool
+ AronTreeWidgetItem::isValueErrorneous()
+ {
+ return itemValueError;
+ }
+
+ void
+ AronTreeWidgetItem::setValueErrorState(bool isErrorSource, bool isTransitiveError)
+ {
+ itemValueError |= isErrorSource;
+ transitiveValueError |= isTransitiveError;
+
+
+ if (CustomWidget::DynamicCast(treeWidget()->itemWidget(this, 1)))
+ {
+ // The widgets handle errors themselves
+ return;
+ }
+ auto palette = gui_color_palette::getNormalPalette();
+ if (itemValueError)
+ {
+ palette = gui_color_palette::getErrorPalette();
+ }
+ else if (transitiveValueError)
+ {
+ palette = gui_color_palette::getIndirectErrorPalette();
+ }
+
+ QTreeWidgetItem::setBackground(1, QBrush(palette.color(QPalette::Base)));
+ }
+
+ void
+ AronTreeWidgetItem::setKeyErrorState(bool hasError)
+ {
+ ARMARX_CHECK(col0Editable); //only editable keys should call this function!
+ auto palette =
+ hasError ? gui_color_palette::getErrorPalette() : gui_color_palette::getNormalPalette();
+
+ keyValueError = hasError;
+
+ QTreeWidgetItem::setBackground(1, QBrush(palette.color(QPalette::Base)));
+ }
+
+ void
+ AronTreeWidgetItem::resetError()
+ {
+ keyValueError = false;
+ itemValueError = false;
+ transitiveValueError = false;
+ // also reset children
+ for (int i = 0; i < childCount(); ++i)
+ {
+ auto* arChild = AronTreeWidgetItem::DynamicCastAndCheck(QTreeWidgetItem::child(i));
+ arChild->resetError();
+ }
+ }
+
+
+ void
+ AronTreeWidgetItem::checkKeyValidityOfChildren()
+ {
+ ARMARX_CHECK(aronType->getDescriptor() == aron::type::Descriptor::DICT);
+ // return if check failed
+ if (aronType->getDescriptor() != aron::type::Descriptor::DICT)
+ {
+ return;
+ }
+ // iterate through children; memorize keys
+ std::map<QString, std::vector<int>> found_keys;
+ auto numChildren = childCount();
+ for (int i = 0; i < numChildren; ++i)
+ {
+ auto* casted = AronTreeWidgetItem::DynamicCastAndCheck(child(i));
+ if (!casted)
+ {
+ // soft error here, we already report it above. - Definetly programming error
+ continue;
+ }
+ auto& vec = found_keys[casted->text(0)];
+ vec.push_back(i);
+ }
+ // highlight keys that conflict
+ // memorize children that are not ok
+ std::set<int> errorneous_indices;
+ for (auto [key, vals] : found_keys)
+ {
+ if (vals.size() > 1)
+ {
+ for (int i : vals)
+ {
+ auto* casted = AronTreeWidgetItem::DynamicCastAndCheck(child(i));
+ if (!casted)
+ {
+ // soft error here, we already report it above. - Definetly programming error
+ continue;
+ }
+ casted->setKeyErrorState(true);
+ errorneous_indices.emplace(i);
+ }
+ }
+ }
+ // clear potential error state of other elements
+ for (int i = 0; i < numChildren; ++i)
+ {
+ if (errorneous_indices.find(i) != errorneous_indices.end())
+ {
+ continue;
+ }
+ auto* casted = AronTreeWidgetItem::DynamicCastAndCheck(child(i));
+ if (!casted)
+ {
+ // soft error here, we already report it above. - Definetly programming error
+ continue;
+ }
+ casted->setKeyErrorState(false);
+ }
+ }
+
+ void
+ AronTreeWidgetItem::onUserChange(int changedColumn)
+ {
+ QTreeWidgetItem* qParent = QTreeWidgetItem::parent();
+ ARMARX_CHECK(qParent);
+ AronTreeWidgetItem* aronParent = DynamicCast(qParent);
+ if (changedColumn == 0)
+ {
+ if (col0Editable)
+ {
+ // Topmost should always be an Object and col0 is not editable in the child then...
+ // -> aronParent should never be nullptr
+ ARMARX_CHECK(aronParent);
+ // check if the element is child of a dict. If so, validate uniqueness of keys
+ if (aronParent->aronType->getDescriptor() == aron::type::Descriptor::DICT)
+ {
+ aronParent->checkKeyValidityOfChildren();
+ }
+ }
+ else
+ {
+ // maybe while editing keys, we try to edit a key that should not change by tabbing.
+ // Catch that here and undo the edit
+ preventIllegalKeyChange();
+ }
+ }
+ }
+
+ void
+ AronTreeWidgetItem::preventIllegalKeyChange()
+ {
+ if (!col0Editable)
+ {
+ setText(0, unchangeableKey);
+ }
+ }
+
+ AronTreeWidgetItem::AronTreeWidgetItem(bool editKey,
+ bool editVal,
+ QString key,
+ aron::type::VariantPtr type) :
+ aronType(type), col0Editable(editKey), col1Editable(editVal)
+ {
+ this->setText(0, key);
+ // add hook to check for edited keys for children of dictionaries
+ if (!editKey)
+ {
+ unchangeableKey = std::move(key);
+ }
+ }
+
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/AronTreeWidgetItem.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/AronTreeWidgetItem.h
index 3d6d0c2a329c5a4376fb24a406dadf706d6b898b..9106cb3192526f15355445deb0c6f1654921c17a 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/AronTreeWidgetItem.h
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/AronTreeWidgetItem.h
@@ -1,39 +1,65 @@
#pragma once
-#include <stack>
-#include <ArmarXCore/core/system/ImportExportComponent.h>
+#include <QTreeWidget>
-#include "Data.h"
+#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <RobotAPI/libraries/aron/core/type/variant/Variant.h>
-#include <QTreeWidget>
+#include "Data.h"
namespace armarx
{
+ // Internal derived QtTreeWidgetItem class. Contains additional information to compare its data agains aron types.
+ // It contains 3 columns: Key ; Value ; Type
+ // The key can only be edited if the parent object is a dict
class AronTreeWidgetItem : public QObject, public QTreeWidgetItem
{
Q_OBJECT
public:
- AronTreeWidgetItem(const AronTreeWidgetItem& other) :
- QObject(),
- QTreeWidgetItem(other)
- {
- aronType = other.aronType;
- }
+ AronTreeWidgetItem(bool editKey, bool editVal, QString key, aron::type::VariantPtr type);
using QTreeWidgetItem::QTreeWidgetItem;
- AronTreeWidgetItem* copy(); // differs from clone!!!!
-
static AronTreeWidgetItem* DynamicCast(QTreeWidgetItem*);
-
static AronTreeWidgetItem* DynamicCastAndCheck(QTreeWidgetItem*);
aron::type::VariantPtr aronType;
+ // if editing the first column should be allowed
+ const bool col0Editable = false;
+ // if editing the second column should be allowed
+ const bool col1Editable = false;
+
+ bool isValueErrorneous();
+ // marks the gui in a specific color. Also stores if there is currently an error.
+ // the only way to reset the color back to normal is with resetError() (error is sticky)
+ void setValueErrorState(bool isErrorSource, bool isTransitiveError);
+ void setKeyErrorState(bool hasError);
+ // reset error storage that influences coloring for this and all children
+ void resetError();
+
+ // Checks if the children of a dict are unique
+ // should not be called on other types! (does nothing then)
+ void checkKeyValidityOfChildren();
+
+ // main logic on changes. Gets called from the onTreeWidgetItemChanged() slot in AronTreeWidgetController.
+ // (This class cannot directly consume this signal, at least I did not find a nice way...)
+ void onUserChange(int changedColumn);
+
+ private:
+ // because the editable keyword counts for all columns, it is possible to TAB into uneditable keys.
+ // We do not want those to change, so just change them back once they finished editing.
+ void preventIllegalKeyChange();
+ bool itemValueError = false;
+ bool transitiveValueError = false;
+ bool keyValueError = false;
+ // hacky storage of previous key value for items that usually should not be editable.
+ // Problem: Dict-keys are editable, one can tab from that to any other value field and edit it.
+ // Fix: Once the change is commited, the AronTreeWidgetController notices the change and checks if it was allowed.
+ QString unchangeableKey = "";
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/ListDictHelper.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/ListDictHelper.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9866e8bc9f8c35a00e6411331461511e4c031e9f
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/ListDictHelper.cpp
@@ -0,0 +1,28 @@
+#include "ListDictHelper.h"
+
+namespace armarx::misc
+{
+ QString
+ generateNumElementsText(int num)
+ {
+ QString numElemsText = "<";
+ if (num == 0)
+ {
+ numElemsText.append("no");
+ }
+ else
+ {
+ numElemsText.append(QString::number(num));
+ }
+ if (num > 1 || num == 0)
+ {
+ numElemsText.append(" elements>");
+ }
+ else
+ {
+ numElemsText.append(" element>");
+ }
+ return numElemsText;
+ }
+
+} // namespace armarx::misc
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/ListDictHelper.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/ListDictHelper.h
new file mode 100644
index 0000000000000000000000000000000000000000..c5d991ad2861aef5c32fc82c033c9090070f13b5
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/ListDictHelper.h
@@ -0,0 +1,8 @@
+#include <QString>
+
+
+namespace armarx::misc
+{
+ // helper that generates a string on how many items are available
+ QString generateNumElementsText(int num);
+} // namespace armarx::misc
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/dict/AronTreeWidgetDictInputModalController.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/dict/AronTreeWidgetDictInputModalController.cpp
index f7d83d98b6cde491cad878822b69b50adb34d355..71a88e54fcf49d77ea5de6804ef06a11e90e6ae8 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/dict/AronTreeWidgetDictInputModalController.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/dict/AronTreeWidgetDictInputModalController.cpp
@@ -1,6 +1,7 @@
#include "AronTreeWidgetDictInputModalController.h"
#include <RobotAPI/libraries/aron/core/type/variant/All.h>
+
#include "../../visitors/AronTreeWidgetCreator.h"
namespace armarx
@@ -29,7 +30,7 @@ namespace armarx
{
for (const auto& added : addedItems)
{
- item->addChild(added->copy());
+ //item->addChild(added->copy());
}
AronTreeWidgetModal::submit();
@@ -59,12 +60,14 @@ namespace armarx
auto d = aron::type::Dict::DynamicCastAndCheck(t);
auto ac = d->getAcceptedType();
- AronTreeWidgetCreatorVisitor v;
+ AronTreeWidgetCreatorVisitor v(nullptr);
+ v.setTopLevelWidget(widget.treeWidgetDict);
aron::type::visit(v, ac);
if (v.createdQWidgetItem)
{
- v.createdQWidgetItem->setText(aron_tree_widget::constantes::TREE_WIDGET_ITEM_NAME, s);
+ v.createdQWidgetItem->setText(aron_tree_widget::constantes::TREE_WIDGET_ITEM_NAME,
+ s);
addedItems.push_back(v.createdQWidgetItem);
widget.treeWidgetDict->addTopLevelItem(v.createdQWidgetItem);
}
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetContextMenu.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetContextMenu.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..98e271cd81c8180af051e094169f63b58176b0e8
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetContextMenu.cpp
@@ -0,0 +1,243 @@
+#include "AronTreeWidgetContextMenu.h"
+
+#include <QMenu>
+#include <QPoint>
+#include <QTreeWidget>
+#include <QTreeWidgetItem>
+
+#include "../AronTreeWidgetItem.h"
+#include "../ListDictHelper.h"
+#include "AronTreeWidgetCreator.h"
+
+namespace armarx
+{
+ AronTreeWidgetContextMenuVisitor::AronTreeWidgetContextMenuVisitor(
+ AronTreeWidgetItem* i,
+ const QPoint& pos,
+ QTreeWidget* contextMenuParent,
+ int x) :
+ parentItem(i), contextMenuParent(contextMenuParent), pos(pos), index(x)
+ {
+ }
+
+ void
+ AronTreeWidgetContextMenuVisitor::addDeleteAction()
+ {
+ auto* castedParent = AronTreeWidgetItem::DynamicCast(parentItem->QTreeWidgetItem::parent());
+ if (!castedParent)
+ {
+ // must be top level element
+ return;
+ }
+ auto aronType = castedParent->aronType->getDescriptor();
+ if (aron::type::Descriptor::DICT == aronType || aron::type::Descriptor::LIST == aronType)
+ {
+ QMenu contextMenu("Context menu", contextMenuParent);
+ actions.emplace_back("remove element", contextMenuParent);
+ action_callbacks.push_back([this]() mutable { this->executeDelete(); });
+ }
+ }
+
+ void
+ AronTreeWidgetContextMenuVisitor::executeDelete()
+ {
+ auto* containerPtr = parentItem->QTreeWidgetItem::parent();
+ containerPtr->removeChild(parentItem);
+ auto* castedContainer = AronTreeWidgetItem::DynamicCast(containerPtr);
+
+ // if the parent item is a List, we need to redo the numbers
+ if (castedContainer &&
+ castedContainer->aronType->getDescriptor() == aron::type::Descriptor::LIST)
+ {
+ // start renumbering from the removed child onwards
+ for (int i = index; i < castedContainer->childCount(); ++i)
+ {
+ std::string numberString = std::to_string(i);
+ castedContainer->child(i)->setText(0, numberString.c_str());
+ }
+ // This displays the number of children also when the list is collapsed
+ QString numElemsText = misc::generateNumElementsText(castedContainer->childCount());
+ containerPtr->setText(1, numElemsText);
+ // set italic
+ auto currFont = castedContainer->font(1);
+ currFont.setItalic(true);
+ castedContainer->setFont(1, currFont);
+ }
+ }
+
+
+ void
+ AronTreeWidgetContextMenuVisitor::visitAronVariant(const aron::type::ObjectPtr&)
+ {
+ addDeleteAction();
+ }
+
+ // lol
+ void
+ armarx::AronTreeWidgetContextMenuVisitor::addAddAction()
+ {
+ actions.emplace_back("Add element", contextMenuParent);
+ action_callbacks.push_back([this]() mutable { this->executeAddElement(); });
+ }
+
+ void
+ AronTreeWidgetContextMenuVisitor::executeAddElement()
+ {
+ AronTreeWidgetCreatorVisitor creator(parentItem);
+ aron::type::visit(creator, parentItem->aronType->getChildren()[0]);
+
+ if (!creator.createdQWidgetItem)
+ {
+ throw std::runtime_error("Creation of TreeElementChild failed unexpectedly");
+ }
+ // if it is a list, we update the number of children at the top
+ auto* castedContainer = AronTreeWidgetItem::DynamicCast(parentItem);
+
+ // if the parent item is a List, we need to redo the numbers
+ if (castedContainer &&
+ castedContainer->aronType->getDescriptor() == aron::type::Descriptor::LIST)
+ {
+ // This displays the number of children also when the list is collapsed
+ auto numElemsText = misc::generateNumElementsText(castedContainer->childCount());
+ castedContainer->setText(1, numElemsText);
+ // set italic
+ auto currFont = castedContainer->font(1);
+ currFont.setItalic(true);
+ castedContainer->setFont(1, currFont);
+ }
+ }
+
+ void
+ AronTreeWidgetContextMenuVisitor::visitAronVariant(const aron::type::DictPtr&)
+ {
+ addAddAction();
+ addDeleteAction();
+ }
+
+ void
+ AronTreeWidgetContextMenuVisitor::visitAronVariant(const aron::type::PairPtr&)
+ {
+ addDeleteAction();
+ }
+
+ void
+ AronTreeWidgetContextMenuVisitor::visitAronVariant(const aron::type::TuplePtr&)
+ {
+ addDeleteAction();
+ }
+
+ void
+ AronTreeWidgetContextMenuVisitor::visitAronVariant(const aron::type::ListPtr&)
+ {
+ addAddAction();
+ addDeleteAction();
+ }
+
+ void
+ AronTreeWidgetContextMenuVisitor::visitAronVariant(const aron::type::NDArrayPtr&)
+ {
+ addDeleteAction();
+ }
+
+ void
+ AronTreeWidgetContextMenuVisitor::visitAronVariant(const aron::type::MatrixPtr&)
+ {
+ addDeleteAction();
+ }
+
+ void
+ AronTreeWidgetContextMenuVisitor::visitAronVariant(const aron::type::QuaternionPtr&)
+ {
+ addDeleteAction();
+ }
+
+ void
+ AronTreeWidgetContextMenuVisitor::visitAronVariant(const aron::type::ImagePtr&)
+ {
+ addDeleteAction();
+ }
+
+ void
+ AronTreeWidgetContextMenuVisitor::visitAronVariant(const aron::type::PointCloudPtr&)
+ {
+ addDeleteAction();
+ }
+
+ void
+ AronTreeWidgetContextMenuVisitor::visitAronVariant(const aron::type::IntEnumPtr&)
+ {
+ addDeleteAction();
+ }
+
+ void
+ AronTreeWidgetContextMenuVisitor::visitAronVariant(const aron::type::IntPtr&)
+ {
+ addDeleteAction();
+ }
+
+ void
+ AronTreeWidgetContextMenuVisitor::visitAronVariant(const aron::type::LongPtr&)
+ {
+ addDeleteAction();
+ }
+
+ void
+ AronTreeWidgetContextMenuVisitor::visitAronVariant(const aron::type::FloatPtr&)
+ {
+ addDeleteAction();
+ }
+
+ void
+ AronTreeWidgetContextMenuVisitor::visitAronVariant(const aron::type::DoublePtr&)
+ {
+ addDeleteAction();
+ }
+
+ void
+ AronTreeWidgetContextMenuVisitor::visitAronVariant(const aron::type::BoolPtr&)
+ {
+ addDeleteAction();
+ }
+
+ void
+ AronTreeWidgetContextMenuVisitor::visitAronVariant(const aron::type::StringPtr&)
+ {
+ addDeleteAction();
+ }
+
+ void
+ AronTreeWidgetContextMenuVisitor::visitUnknown(Input&)
+ {
+ ARMARX_WARNING << "Tried to open Context menu on unknown aron type";
+ }
+
+ void
+ AronTreeWidgetContextMenuVisitor::showMenuAndExecute()
+ {
+ QMenu menu("Context Menu", contextMenuParent);
+ for (auto& el : actions)
+ {
+ menu.addAction(&el);
+ }
+ auto* chosenAction = menu.exec(contextMenuParent->mapToGlobal(pos));
+
+ if (!chosenAction)
+ {
+ return;
+ }
+
+ // not elegant, but is a small loop anyway
+ auto it = actions.begin();
+ size_t count = 0;
+ while (it != actions.end())
+ {
+ if (chosenAction == &*it)
+ {
+ action_callbacks[count]();
+ break;
+ }
+ ++it;
+ ++count;
+ }
+ }
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetContextMenu.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetContextMenu.h
new file mode 100644
index 0000000000000000000000000000000000000000..510f68292b91bf9934dcecd4300d698e64d516b3
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetContextMenu.h
@@ -0,0 +1,67 @@
+#pragma once
+
+
+#include <QAction>
+
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
+#include <RobotAPI/libraries/aron/core/type/variant/All.h>
+#include <RobotAPI/libraries/aron/core/type/visitor/variant/VariantVisitor.h>
+
+class QTreeWidget;
+class QPoint;
+
+namespace armarx
+{
+
+ class AronTreeWidgetItem;
+
+ // Visitor on aron types. It creates a context menu dependent on the type of the AronTreeWidgetItem.
+ // Its only used for Lists and Dicts.
+ class AronTreeWidgetContextMenuVisitor : public armarx::aron::type::ConstVariantVisitor
+ {
+
+ AronTreeWidgetItem* parentItem;
+ QTreeWidget* contextMenuParent;
+ const QPoint& pos;
+ int index;
+
+ public:
+ AronTreeWidgetContextMenuVisitor() = delete;
+ AronTreeWidgetContextMenuVisitor(AronTreeWidgetItem* i,
+ const QPoint& pos,
+ QTreeWidget* contextMenuParent,
+ int x);
+
+ void visitAronVariant(const aron::type::ObjectPtr&) final;
+ void visitAronVariant(const aron::type::DictPtr&) final;
+ void visitAronVariant(const aron::type::PairPtr&) final;
+ void visitAronVariant(const aron::type::TuplePtr&) final;
+ void visitAronVariant(const aron::type::ListPtr&) final;
+ void visitAronVariant(const aron::type::NDArrayPtr&) final;
+ void visitAronVariant(const aron::type::MatrixPtr&) final;
+ void visitAronVariant(const aron::type::QuaternionPtr&) final;
+ void visitAronVariant(const aron::type::ImagePtr&) final;
+ void visitAronVariant(const aron::type::PointCloudPtr&) final;
+ void visitAronVariant(const aron::type::IntEnumPtr&) final;
+ void visitAronVariant(const aron::type::IntPtr&) final;
+ void visitAronVariant(const aron::type::LongPtr&) final;
+ void visitAronVariant(const aron::type::FloatPtr&) final;
+ void visitAronVariant(const aron::type::DoublePtr&) final;
+ void visitAronVariant(const aron::type::BoolPtr&) final;
+ void visitAronVariant(const aron::type::StringPtr&) final;
+ void visitUnknown(Input&) final;
+
+ void showMenuAndExecute();
+
+ private:
+ std::list<QAction> actions;
+ std::vector<std::function<void()>> action_callbacks;
+
+ // Creates a remove option if the element is a direct child of a list or dict
+ void addDeleteAction();
+ void executeDelete();
+
+ void addAddAction();
+ void executeAddElement();
+ };
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetConverter.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetConverter.cpp
index fbdc653abc5fa7cab715fc8abc7aa200de0243c1..c0a8088db7b5ad4e4e1db955b42e9316164038b3 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetConverter.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetConverter.cpp
@@ -26,16 +26,68 @@
#include "AronTreeWidgetConverter.h"
// armarx
+#include <SimoxUtility/algorithm/string.h>
+
#include <ArmarXCore/core/logging/Logging.h>
+#include "RobotAPI/libraries/aron/core/data/variant/All.h"
+
// qt
#include <QTreeWidgetItem>
+#include "../widgets/EditMatrixWidget.h"
+#include "../widgets/IntEnumWidget.h"
+#include "../widgets/QuaternionWidget.h"
+
+
namespace armarx
{
+ bool
+ AronTreeWidgetConverterVisitor::isConversionSuccessful()
+ {
+ return !isDirectError && !hasTransitiveError;
+ }
+
+ bool
+ AronTreeWidgetConverterVisitor::onlyChildFailedConversion()
+ {
+ return hasTransitiveError;
+ }
+
+ bool
+ AronTreeWidgetConverterVisitor::hasDirectError() const
+ {
+ return isDirectError;
+ }
+
+ void
+ AronTreeWidgetConverterVisitor::handleErrors(AronTreeWidgetConverterVisitor childV,
+ bool ownFault)
+ {
+ ARMARX_TRACE;
+ isDirectError |= ownFault;
+ hasTransitiveError |= childV.isDirectError || childV.hasTransitiveError;
+
+ auto* aronItem = AronTreeWidgetItem::DynamicCast(parentItem->child(index));
+ ARMARX_CHECK(aronItem);
+ aronItem->setValueErrorState(isDirectError, hasTransitiveError);
+ }
+
+ void
+ AronTreeWidgetConverterVisitor::handleErrors(bool ownFault)
+ {
+ ARMARX_TRACE;
+ isDirectError = ownFault;
+ auto* aronItem = AronTreeWidgetItem::DynamicCast(parentItem->child(index));
+ ARMARX_CHECK(aronItem);
+ aronItem->setValueErrorState(isDirectError, false);
+ }
+
+
void
AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::ObjectPtr& i)
{
+ ARMARX_TRACE;
auto createdAronDict = std::make_shared<aron::data::Dict>(i->getPath());
createdAron = createdAronDict;
QTreeWidgetItem* el = parentItem->child(index);
@@ -43,16 +95,22 @@ namespace armarx
unsigned int x = 0;
for (const auto& [key, value] : i->getMemberTypes())
{
+ ARMARX_TRACE;
AronTreeWidgetConverterVisitor v(el, x++);
aron::type::visit(v, value);
- createdAronDict->addElement(key, v.createdAron);
+ handleErrors(v);
+ if (v.isConversionSuccessful())
+ {
+ createdAronDict->addElement(key, v.createdAron);
+ }
}
}
void
AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::DictPtr& i)
{
+ ARMARX_TRACE;
auto createdAronDict = std::make_shared<aron::data::Dict>(i->getPath());
createdAron = createdAronDict;
QTreeWidgetItem* el = parentItem->child(index);
@@ -62,71 +120,206 @@ namespace armarx
auto it = el->child(x);
AronTreeWidgetConverterVisitor v(el, x);
aron::type::visit(v, i->getAcceptedType());
-
- if (v.createdAron)
+ auto key = it->text(0).toStdString();
+ // TODO: handle key errors more elegantly / separately, fine for now
+ handleErrors(v, createdAronDict->hasElement(key));
+ if (v.createdAron && v.isConversionSuccessful() && !createdAronDict->hasElement(key))
{
- createdAronDict->addElement(it->text(0).toStdString(), v.createdAron);
+ createdAronDict->addElement(key, v.createdAron);
}
}
}
void
- AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::PairPtr& i)
+ AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::ListPtr& i)
{
- // TODO
+ ARMARX_TRACE;
+ auto createdAronList = std::make_shared<aron::data::List>(i->getPath());
+ createdAron = createdAronList;
+ auto* elem = parentItem->child(index);
+ auto childrenTypes = i->getChildren();
+ ARMARX_CHECK(childrenTypes.size() == 1);
+ for (int j = 0; j < elem->childCount(); ++j)
+ {
+ AronTreeWidgetConverterVisitor convVisitor(elem, j);
+ aron::type::visit(convVisitor, childrenTypes[0]);
+ handleErrors(convVisitor);
+
+ if (convVisitor.createdAron && convVisitor.isConversionSuccessful())
+ {
+ createdAronList->addElement(convVisitor.createdAron);
+ }
+ }
}
+
void
- AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::TuplePtr& i)
+ AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::PairPtr& i)
{
- // TODO
+ ARMARX_TRACE;
+ auto createdAronPair = std::make_shared<aron::data::List>(i->getPath());
+ createdAron = createdAronPair;
+ auto* elem = parentItem->child(index);
+
+ for (int j = 0; j < 2; ++j)
+ {
+ AronTreeWidgetConverterVisitor convVisitor(elem, j);
+ handleErrors(convVisitor);
+ if (convVisitor.createdAron && convVisitor.isConversionSuccessful())
+ {
+ createdAronPair->addElement(convVisitor.createdAron);
+ }
+ }
}
void
- AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::ListPtr& i)
+ AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::TuplePtr& i)
{
- // TODO
+ ARMARX_TRACE;
+ auto createdAronList = std::make_shared<aron::data::List>(i->getPath());
+ createdAron = createdAronList;
+ QTreeWidgetItem* el = parentItem->child(index);
+
+ for (int x = 0; x < el->childCount(); ++x)
+ {
+ auto* it = el->child(x);
+ AronTreeWidgetConverterVisitor v(it, x);
+ aron::type::visit(v, i->getAcceptedType(x));
+ handleErrors(v);
+
+ if (v.createdAron)
+ {
+ createdAronList->addElement(v.createdAron);
+ }
+ }
}
void
AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::NDArrayPtr& i)
{
- // TODO
+ ARMARX_TRACE;
+ ARMARX_ERROR << "Currently do not support supplying raw NDArrays!";
}
void
AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::MatrixPtr& i)
{
- // TODO
+ ARMARX_TRACE;
+ auto createdMatrix = std::make_shared<aron::data::NDArray>(i->getPath());
+ int dataSize = 0;
+ switch (i->getElementType())
+ {
+ case armarx::aron::type::matrix::INT16:
+ dataSize = 2;
+ break;
+ case armarx::aron::type::matrix::INT32:
+ case armarx::aron::type::matrix::FLOAT32:
+ dataSize = 4;
+ break;
+ case armarx::aron::type::matrix::FLOAT64:
+ case armarx::aron::type::matrix::INT64:
+ dataSize = 8;
+ break;
+ };
+ createdMatrix->setShape({i->getRows(), i->getCols(), dataSize});
+ createdMatrix->setType(i->getFullName());
+ int totalByteSize = i->getRows() * i->getCols() * dataSize;
+ createdAron = createdMatrix;
+
+ auto* currElem = parentItem->child(index);
+ auto* rootWidget = currElem->treeWidget();
+ ARMARX_CHECK(rootWidget);
+ auto* widget = rootWidget->itemWidget(currElem, 1);
+ auto* matrixWidget = EditMatrixWidget::DynamicCastAndCheck(widget);
+
+ handleErrors(matrixWidget->hasParseErrors());
+ if (matrixWidget->hasParseErrors())
+ {
+ return;
+ }
+ // write to aron data
+ std::vector<unsigned char> elems;
+ elems.reserve(totalByteSize);
+ // CAUTION: Raw data has column based storage
+ for (size_t col = 0; col < (size_t)i->getCols(); ++col)
+ {
+ for (size_t row = 0; row < (size_t)i->getRows(); ++row)
+ {
+ // gets us directly the byte wise format
+ auto parsed = matrixWidget->parseElement(row, col);
+ // append vector to vector
+ elems.insert(elems.end(), parsed.begin(), parsed.end());
+ }
+ }
+ createdMatrix->setData(totalByteSize, elems.data());
}
void
AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::QuaternionPtr& i)
{
- // TODO
+ ARMARX_TRACE;
+ auto createdQuat = std::make_shared<aron::data::NDArray>(i->getPath());
+ createdAron = createdQuat;
+ int dataSize = i->getElementType() == aron::type::quaternion::ElementType::FLOAT32 ? 4 : 8;
+ createdQuat->setShape({1, 4, dataSize});
+ createdQuat->setType(i->getFullName());
+ auto* currTreeElem = parentItem->child(index);
+ auto* itemWidget = currTreeElem->treeWidget()->itemWidget(currTreeElem, 1);
+ auto* quatWidget = QuaternionWidget::DynamicCastAndCheck(itemWidget);
+
+ // error handling
+ handleErrors(quatWidget->hasParseErrors());
+ if (quatWidget->hasParseErrors())
+ {
+ return;
+ }
+
+ // write to aron data
+ auto serialized = quatWidget->parseAllToNDArray();
+ if ((int)serialized.size() != dataSize * 4)
+ {
+ ARMARX_ERROR
+ << "serialized quaternions did not return byte sequence of correct length!";
+ }
+ createdQuat->setData(serialized.size(), serialized.data());
}
void
AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::ImagePtr& i)
{
+ ARMARX_TRACE;
// TODO
}
void
AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::PointCloudPtr& i)
{
+ ARMARX_TRACE;
// TODO
}
void
AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::IntEnumPtr& i)
{
- // TODO
+ ARMARX_TRACE;
+ QTreeWidgetItem* el = parentItem->child(index);
+ auto* genericWidget = el->treeWidget()->itemWidget(el, 1);
+ auto* intEnumWidget = IntEnumWidget::DynamicCastAndCheck(genericWidget);
+ if (!intEnumWidget)
+ {
+ // already reporting error; continue here
+ return;
+ }
+ bool success;
+ std::tie(success, createdAron) = intEnumWidget->parseToAron();
+
+ handleErrors(!success);
}
void
AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::IntPtr& i)
{
+ ARMARX_TRACE;
auto createdAronInt = std::make_shared<aron::data::Int>(i->getPath());
createdAron = createdAronInt;
QTreeWidgetItem* el = parentItem->child(index);
@@ -137,14 +330,25 @@ namespace armarx
createdAronInt->setValue(0);
return;
}
-
- int val = std::stoi(str);
- createdAronInt->setValue(val);
+ try
+ {
+ int val = simox::alg::to_<int>(str);
+ createdAronInt->setValue(val);
+ }
+ catch (const simox::error::SimoxError& err)
+ {
+ handleErrors();
+ ARMARX_VERBOSE << "Conversion from String to Int failed. Error:\"" << err.what()
+ << "\"";
+ return;
+ }
+ handleErrors(false);
}
void
AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::LongPtr& i)
{
+ ARMARX_TRACE;
auto createdAronLong = std::make_shared<aron::data::Long>(i->getPath());
createdAron = createdAronLong;
QTreeWidgetItem* el = parentItem->child(index);
@@ -152,15 +356,27 @@ namespace armarx
std::string str = el->text(1).toStdString();
if (str.empty())
{
+ //TODO: similar behaviour for rest?
str = el->text(3).toStdString();
}
-
- createdAronLong->fromString(str);
+ try
+ {
+ createdAronLong->setValue(simox::alg::to_<long>(str));
+ }
+ catch (const simox::error::SimoxError& err)
+ {
+ handleErrors();
+ ARMARX_VERBOSE << "Conversion from String to Long failed. Error:\"" << err.what()
+ << "\"";
+ return;
+ }
+ handleErrors(false);
}
void
AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::FloatPtr& i)
{
+ ARMARX_TRACE;
auto createdAronFloat = std::make_shared<aron::data::Float>(i->getPath());
createdAron = createdAronFloat;
QTreeWidgetItem* el = parentItem->child(index);
@@ -170,13 +386,24 @@ namespace armarx
{
str = el->text(3).toStdString();
}
-
- createdAronFloat->fromString(str);
+ try
+ {
+ createdAronFloat->setValue(simox::alg::to_<float>(str));
+ }
+ catch (const simox::error::SimoxError& err)
+ {
+ handleErrors();
+ ARMARX_VERBOSE << "Conversion from String to Float failed. Error:\"" << err.what()
+ << "\"";
+ return;
+ }
+ handleErrors(false);
}
void
AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::DoublePtr& i)
{
+ ARMARX_TRACE;
auto createdAronDouble = std::make_shared<aron::data::Double>(i->getPath());
createdAron = createdAronDouble;
QTreeWidgetItem* el = parentItem->child(index);
@@ -186,13 +413,24 @@ namespace armarx
{
str = el->text(3).toStdString();
}
-
- createdAronDouble->fromString(str);
+ try
+ {
+ createdAronDouble->setValue(simox::alg::to_<double>(str));
+ }
+ catch (const simox::error::SimoxError& err)
+ {
+ handleErrors();
+ ARMARX_VERBOSE << "Conversion from String to Double failed. Error:\"" << err.what()
+ << "\"";
+ return;
+ }
+ handleErrors(false);
}
void
AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::BoolPtr& i)
{
+ ARMARX_TRACE;
auto createdAronBool = std::make_shared<aron::data::Bool>(i->getPath());
createdAron = createdAronBool;
QTreeWidgetItem* el = parentItem->child(index);
@@ -202,24 +440,36 @@ namespace armarx
{
str = el->text(3).toStdString();
}
-
- createdAronBool->fromString(str);
+ try
+ {
+ createdAronBool->setValue(simox::alg::to_<bool>(str));
+ }
+ catch (const simox::error::SimoxError& err)
+ {
+ handleErrors();
+ ARMARX_VERBOSE << "Conversion from String to Bool failed. Error:\"" << err.what()
+ << "\"";
+ return;
+ }
+ handleErrors(false);
}
void
AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::StringPtr& i)
{
+ ARMARX_TRACE;
auto createdAronString = std::make_shared<aron::data::String>(i->getPath());
createdAron = createdAronString;
QTreeWidgetItem* el = parentItem->child(index);
std::string str = el->text(1).toStdString();
- createdAronString->fromString(str);
+ createdAronString->setValue(str);
}
- void AronTreeWidgetConverterVisitor::visitUnknown(Input&)
+ void
+ AronTreeWidgetConverterVisitor::visitUnknown(Input&)
{
- ARMARX_WARNING_S << "Received an unknown type when trying to convert a skill argument type to an aron data object.";
+ ARMARX_WARNING_S << "Received an unknown type when trying to convert a skill argument type "
+ "to an aron data object.";
}
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetConverter.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetConverter.h
index eb056cdc2a65f14ef4d884124d6cb14ac2e9ed7e..c127ac5e49276e878221dc39a5db8a7b821ccec7 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetConverter.h
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetConverter.h
@@ -21,8 +21,8 @@
*/
#pragma once
-#include <RobotAPI/libraries/aron/core/type/variant/All.h>
#include <RobotAPI/libraries/aron/core/data/variant/All.h>
+#include <RobotAPI/libraries/aron/core/type/variant/All.h>
#include <RobotAPI/libraries/aron/core/type/visitor/variant/VariantVisitor.h>
// forward declarations of qt
@@ -32,8 +32,7 @@ class QTreeWidgetItem;
namespace armarx
{
// Conversion from TreeView to aron data
- class AronTreeWidgetConverterVisitor :
- public armarx::aron::type::ConstVariantVisitor
+ class AronTreeWidgetConverterVisitor : public armarx::aron::type::ConstVariantVisitor
{
public:
QTreeWidgetItem* parentItem;
@@ -41,10 +40,27 @@ namespace armarx
aron::data::VariantPtr createdAron = nullptr;
AronTreeWidgetConverterVisitor() = delete;
- AronTreeWidgetConverterVisitor(QTreeWidgetItem* i, int x) :
- parentItem(i), index(x)
- {}
+ AronTreeWidgetConverterVisitor(QTreeWidgetItem* i, int x) : parentItem(i), index(x)
+ {
+ }
+ // if the conversion was successful after calling visit()
+ bool isConversionSuccessful();
+ // returns true if this type itself was sucessfully parsed, but some contained object failed.
+ // also false if there is no error
+ bool onlyChildFailedConversion();
+
+ bool hasDirectError() const;
+
+ private:
+ bool isDirectError = false;
+ bool hasTransitiveError = false;
+ // adds all errors from other visitor to our own error collection -> collecting errors
+ // with ownFault, we also add this node to the collection
+ void handleErrors(AronTreeWidgetConverterVisitor childV, bool ownFault = false);
+ // we are the cause...
+ void handleErrors(bool ownFault = true);
+ public:
void visitAronVariant(const aron::type::ObjectPtr&) final;
void visitAronVariant(const aron::type::DictPtr&) final;
void visitAronVariant(const aron::type::PairPtr&) final;
@@ -64,6 +80,4 @@ namespace armarx
void visitAronVariant(const aron::type::StringPtr&) final;
void visitUnknown(Input&) final;
};
-}
-
-
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetCreator.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetCreator.cpp
index ee43334bb26862322610746269fee97a16d0cccd..84cb58585e3b4e7f3a058db66f3b5f3a40f54aea 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetCreator.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetCreator.cpp
@@ -22,28 +22,98 @@
#include <string>
+#include "../widgets/EditMatrixWidget.h"
+#include "../widgets/IntEnumWidget.h"
+#include "../widgets/QuaternionWidget.h"
+
// base class
#include "AronTreeWidgetCreator.h"
// data
+#include <QComboBox>
+
#include "../AronTreeWidgetItem.h"
#include "../Data.h"
+#include "../ListDictHelper.h"
+#include "AronTreeWidgetContextMenu.h"
namespace armarx
{
+
+ AronTreeWidgetCreatorVisitor::AronTreeWidgetCreatorVisitor(QTreeWidgetItem* parentInstance) :
+ parentOfCreatedObj(parentInstance)
+ {
+ // The parent of the root aron Tree Widget will not be aron type.
+ // there an explicit setTopLevelWidget() is required.
+ auto* aronParent = AronTreeWidgetItem::DynamicCast(parentInstance);
+ if (aronParent)
+ {
+ toplevelWidget = aronParent->treeWidget();
+ }
+ }
+
+ std::string
+ AronTreeWidgetCreatorVisitor::generateUniqueKeyFromSet(std::set<std::string>&& usedKeys)
+ {
+ size_t num = 0;
+ while (true)
+ {
+ std::string proposedName = this->defaultMapKeyName + std::to_string(num);
+ auto it = usedKeys.find(proposedName);
+ if (it == usedKeys.end())
+ {
+ break;
+ }
+ ++num;
+ }
+ return this->defaultMapKeyName + std::to_string(num);
+ }
+
void
- AronTreeWidgetCreatorVisitor::createSimpleTreeViewWidget(Input& i, const std::string& defaul)
+ AronTreeWidgetCreatorVisitor::insertNewTreeViewWidget(Input& i, const std::string& defaul)
{
ARMARX_CHECK_NOT_NULL(i);
auto key = i->getPath().getLastElement();
- createdQWidgetItem = new AronTreeWidgetItem();
- createdQWidgetItem->aronType = i;
- createdQWidgetItem->setText(0, QString::fromStdString(key));
+ bool isDictChild = false;
+ // edit key, to be a unique string, if the parent is a dict
+ auto* aronParent = AronTreeWidgetItem::DynamicCast(parentOfCreatedObj);
+ if (aronParent && aronParent->aronType->getDescriptor() == aron::type::Descriptor::DICT)
+ {
+ isDictChild = true;
+ std::set<std::string> usedKeys;
+ for (int i = 0; i < parentOfCreatedObj->childCount(); ++i)
+ {
+ auto* sibling = AronTreeWidgetItem::DynamicCast(parentOfCreatedObj->child(i));
+ if (sibling)
+ {
+ usedKeys.insert(sibling->text(0).toStdString());
+ }
+ }
+ key = generateUniqueKeyFromSet(std::move(usedKeys));
+ }
+ // if it's a list -> choose the right number
+ else if (aronParent &&
+ aronParent->aronType->getDescriptor() == aron::type::Descriptor::LIST)
+ {
+ key = std::to_string(parentOfCreatedObj->childCount());
+ }
+
+ createdQWidgetItem =
+ new AronTreeWidgetItem(isDictChild, editableValue, QString::fromStdString(key), i);
createdQWidgetItem->setText(1, QString::fromStdString(defaul));
createdQWidgetItem->setText(2, QString::fromStdString(i->getShortName()));
- createdQWidgetItem->setText(3, QString::fromStdString(aron_tree_widget::constantes::ITEM_EMPTY_MESSAGE) /*QString::fromStdString(i->getDefaultFromString())*/);
- createdQWidgetItem->setFlags(createdQWidgetItem->flags() | Qt::ItemIsEditable);
+ createdQWidgetItem->setText(
+ 3,
+ QString::fromStdString(
+ aron_tree_widget::constantes::
+ ITEM_EMPTY_MESSAGE) /*QString::fromStdString(i->getDefaultFromString())*/);
+
+ if (editableValue || isDictChild)
+ {
+ createdQWidgetItem->setFlags(createdQWidgetItem->flags() | Qt::ItemIsEditable);
+ }
+ parentOfCreatedObj->addChild(createdQWidgetItem);
}
void
@@ -57,73 +127,203 @@ namespace armarx
key = i->getPath().getLastElement();
}
- createdQWidgetItem = new AronTreeWidgetItem();
- createdQWidgetItem->setText(0, QString::fromStdString(key));
+ createdQWidgetItem =
+ new AronTreeWidgetItem(editableValue, false, QString::fromStdString(key), i);
+ parentOfCreatedObj->addChild(createdQWidgetItem);
for (const auto& [key, value] : i->getMemberTypes())
{
- AronTreeWidgetCreatorVisitor v;
+ AronTreeWidgetCreatorVisitor v(createdQWidgetItem);
aron::type::visit(v, value);
- if (v.createdQWidgetItem)
- {
- createdQWidgetItem->addChild(v.createdQWidgetItem);
- }
+ assert(v.createdQWidgetItem);
}
}
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::DictPtr& i)
- { createSimpleTreeViewWidget(i, ""); }
+ {
+ ARMARX_CHECK_NOT_NULL(i);
+
+ insertNewTreeViewWidget(i, "");
+ // The DictType has only one member, its key-type. This also must be present
+ ARMARX_CHECK_EQUAL(i->getChildren().size(), 1);
+ }
void
- AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::PairPtr& i)
- { createSimpleTreeViewWidget(i, ""); }
+ AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::PairPtr& pair)
+ {
+ // create default, uneditable tree widget item.
+ insertNewTreeViewWidget(pair, "");
+ // attach two children
+ ARMARX_CHECK(pair->getChildren().size() == 2);
+ for (size_t i = 0; i < 2; ++i)
+ {
+ AronTreeWidgetCreatorVisitor v(createdQWidgetItem);
+ aron::type::visit(v, pair->getChildren()[i]);
+ if (v.createdQWidgetItem)
+ {
+ std::string descr = "p[" + std::to_string(i) + "]";
+ v.createdQWidgetItem->setText(0, descr.c_str());
+ }
+ }
+ }
void
- AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::TuplePtr& i)
- { createSimpleTreeViewWidget(i, ""); }
+ AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::TuplePtr& tuple)
+ {
+ // CAUTION; UNTESTED
+ // create default, uneditable tree widget item.
+ insertNewTreeViewWidget(tuple, "");
+ // attach all children
+ for (size_t i = 0; i < tuple->getChildren().size(); ++i)
+ {
+ AronTreeWidgetCreatorVisitor v(createdQWidgetItem);
+ aron::type::visit(v, tuple->getChildren()[i]);
+ if (v.createdQWidgetItem)
+ {
+ std::string descr = "tup[" + std::to_string(i) + "]";
+ v.createdQWidgetItem->setText(0, descr.c_str());
+ }
+ }
+ }
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::ListPtr& i)
- { createSimpleTreeViewWidget(i, ""); }
+ {
+ insertNewTreeViewWidget(i, "");
+ auto txt = misc::generateNumElementsText(0);
+ createdQWidgetItem->setText(1, txt);
+ }
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::NDArrayPtr& i)
- { createSimpleTreeViewWidget(i, ""); }
+ {
+ insertNewTreeViewWidget(i, "");
+ }
+
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::MatrixPtr& i)
- { createSimpleTreeViewWidget(i, ""); }
+ {
+ editableValue = false;
+ insertNewTreeViewWidget(i, "");
+ // special code to print the type of base type used
+ QString type = "";
+ switch (i->getElementType())
+ {
+ case armarx::aron::type::matrix::INT16:
+ type = "<int16>";
+ break;
+ case armarx::aron::type::matrix::INT32:
+ type = "<int32>";
+ break;
+ case armarx::aron::type::matrix::INT64:
+ type = "<int64>";
+ break;
+ case armarx::aron::type::matrix::FLOAT32:
+ type = "<float>";
+ break;
+ case armarx::aron::type::matrix::FLOAT64:
+ type = "<double>";
+ break;
+ }
+ type = createdQWidgetItem->text(2) + type;
+ createdQWidgetItem->setText(2, type);
+
+ // Widget fiddling source: https://blog.manash.io/quick-qt-6-how-to-add-qpushbutton-or-widgets-to-a-qtreewidget-as-qtreewidgetitem-2ae9f54c0e5f
+ // overlay custom widget in column 1
+ auto* toplevelWidget = createdQWidgetItem->treeWidget();
+ EditMatrixWidget* matWidget = new EditMatrixWidget(
+ i->getRows(), i->getCols(), i->getElementType(), createdQWidgetItem);
+ toplevelWidget->setItemWidget(createdQWidgetItem, 1, matWidget);
+ }
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::QuaternionPtr& i)
- { createSimpleTreeViewWidget(i, ""); }
+ {
+ editableValue = false;
+ insertNewTreeViewWidget(i, "");
+ // special code to print the type of base type used
+ QString type = "";
+ switch (i->getElementType())
+ {
+ case armarx::aron::type::quaternion::FLOAT32:
+ type = "<float>";
+ break;
+ case armarx::aron::type::quaternion::FLOAT64:
+ type = "<double>";
+ break;
+ }
+ type = createdQWidgetItem->text(2) + type;
+ createdQWidgetItem->setText(2, type);
+
+ auto* toplevelWidget = createdQWidgetItem->treeWidget();
+ QuaternionWidget* quatWidget =
+ new QuaternionWidget(i->getElementType(), createdQWidgetItem);
+ toplevelWidget->setItemWidget(createdQWidgetItem, 1, quatWidget);
+ }
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::ImagePtr& i)
- { createSimpleTreeViewWidget(i, ""); }
+ {
+ insertNewTreeViewWidget(i, "");
+ }
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::PointCloudPtr& i)
- { createSimpleTreeViewWidget(i, ""); }
+ {
+ insertNewTreeViewWidget(i, "");
+ }
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::IntEnumPtr& i)
- { createSimpleTreeViewWidget(i, ""); }
+ {
+ editableValue = false;
+ ARMARX_CHECK_NOT_NULL(i);
+ ARMARX_CHECK_GREATER(i->getAcceptedValueNames().size(), 0);
+
+ insertNewTreeViewWidget(i, "");
+ IntEnumWidget* widget = new IntEnumWidget(i, createdQWidgetItem);
+ createdQWidgetItem->treeWidget()->setItemWidget(createdQWidgetItem, 1, widget);
+ }
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::IntPtr& i)
- { createSimpleTreeViewWidget(i, "0"); }
+ {
+ editableValue = true;
+ insertNewTreeViewWidget(i, "0");
+ }
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::LongPtr& i)
- { createSimpleTreeViewWidget(i, "0"); }
+ {
+ editableValue = true;
+ insertNewTreeViewWidget(i, "0");
+ }
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::FloatPtr& i)
- { createSimpleTreeViewWidget(i, "0.0"); }
+ {
+ editableValue = true;
+ insertNewTreeViewWidget(i, "0.0");
+ }
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::DoublePtr& i)
- { createSimpleTreeViewWidget(i, "0.0"); }
+ {
+ editableValue = true;
+ insertNewTreeViewWidget(i, "0.0");
+ }
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::BoolPtr& i)
- { createSimpleTreeViewWidget(i, "false"); }
+ {
+ editableValue = true;
+ insertNewTreeViewWidget(i, "false");
+ }
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::StringPtr& i)
- { createSimpleTreeViewWidget(i, ""); }
+ {
+ insertNewTreeViewWidget(i, "");
+ }
- void AronTreeWidgetCreatorVisitor::visitUnknown(Input&)
+ void
+ AronTreeWidgetCreatorVisitor::visitUnknown(Input&)
{
- ARMARX_WARNING_S << "Received an unknown type when trying to create a tree view widget for a skill argument type.";
+ ARMARX_WARNING_S << "Received an unknown type when trying to create a tree view widget for "
+ "a skill argument type.";
}
-}
+ void
+ AronTreeWidgetCreatorVisitor::setTopLevelWidget(QTreeWidget* widget)
+ {
+ toplevelWidget = widget;
+ }
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetCreator.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetCreator.h
index 63dde7ef0be04d8d790c5f8434ae1b82faeb107a..b3d2ffb54cca6bf7fe8aac2395d733f3ba458817 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetCreator.h
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetCreator.h
@@ -21,8 +21,10 @@
*/
#pragma once
-#include <RobotAPI/libraries/aron/core/type/variant/All.h>
+#include <QTreeWidgetItem>
+
#include <RobotAPI/libraries/aron/core/data/variant/All.h>
+#include <RobotAPI/libraries/aron/core/type/variant/All.h>
#include <RobotAPI/libraries/aron/core/type/visitor/variant/VariantVisitor.h>
namespace armarx
@@ -30,15 +32,20 @@ namespace armarx
class AronTreeWidgetItem;
// Convert aron type to tree widgets
- class AronTreeWidgetCreatorVisitor :
- public armarx::aron::type::ConstVariantVisitor
+ class AronTreeWidgetCreatorVisitor : public armarx::aron::type::ConstVariantVisitor
{
public:
- AronTreeWidgetItem* createdQWidgetItem;
+ AronTreeWidgetItem* createdQWidgetItem = nullptr;
+
+ AronTreeWidgetCreatorVisitor() = delete;
- AronTreeWidgetCreatorVisitor() = default;
+ // Takes the parent tree element and attaches the newly crated object during visit() to it.
+ // This allows us to also use information of the parent object during creation (like if this child is a dict entry or part of a list)
+ // IMPORTANT: For the root element, manually set the topLevelWidget after this constructor
+ AronTreeWidgetCreatorVisitor(QTreeWidgetItem* parentInstance);
+
+ void insertNewTreeViewWidget(Input& i, const std::string&);
- void createSimpleTreeViewWidget(Input& i, const std::string&);
void visitAronVariant(const aron::type::ObjectPtr&) final;
void visitAronVariant(const aron::type::DictPtr& i) final;
@@ -58,7 +65,17 @@ namespace armarx
void visitAronVariant(const aron::type::BoolPtr& i) final;
void visitAronVariant(const aron::type::StringPtr& i) final;
void visitUnknown(Input&) final;
- };
-}
+ // setter for the widget attachment point. Only needs to be manually set for the root element.
+ void setTopLevelWidget(QTreeWidget* widget);
+ private:
+ void handleEditable();
+ std::string generateUniqueKeyFromSet(std::set<std::string>&& usedKeys);
+ QTreeWidgetItem* parentOfCreatedObj = nullptr;
+ QTreeWidget* toplevelWidget = nullptr;
+ const std::string defaultMapKeyName = "key_";
+ // controls, if values (column 1) can be edited directly
+ bool editableValue = false;
+ };
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetModalCreator.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetModalCreator.cpp
index 7ff248bd58499028bcf43a158f83f67c1259b43c..fc58634bb9bc02834d4e72663e3c16ffe36d6de6 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetModalCreator.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetModalCreator.cpp
@@ -28,7 +28,7 @@
// modals
#include "../modal/text/AronTreeWidgetTextInputModalController.h"
-#include "../modal/dict/AronTreeWidgetDictInputModalController.h"
+//#include "../modal/dict/AronTreeWidgetDictInputModalController.h"
// qt
#include <QTreeWidget>
@@ -37,84 +37,6 @@
namespace armarx
{
- void AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::ObjectPtr& i)
- {
- // should not happen, right?
- }
-
- void
- AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::DictPtr& i)
- {
- createdModal = std::make_shared<AronTreeWidgetDictInputModalController>(label, item, parent);
- }
-
- void
- AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::PairPtr& i)
- {
- }
-
- void
- AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::TuplePtr& i)
- { }
-
- void
- AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::ListPtr& i)
- { }
-
- void
- AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::NDArrayPtr& i)
- { }
-
- void
- AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::MatrixPtr& i)
- { }
-
- void
- AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::QuaternionPtr& i)
- { }
-
- void
- AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::ImagePtr& i)
- { }
-
- void
- AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::PointCloudPtr& i)
- { }
-
- void
- AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::IntEnumPtr& i)
- { }
-
- void
- AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::IntPtr& i)
- {
- createdModal = std::make_shared<AronTreeWidgetTextInputModalController>(label, item, parent);
- }
-
- void
- AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::LongPtr& i)
- {
- createdModal = std::make_shared<AronTreeWidgetTextInputModalController>(label, item, parent);
- }
-
- void
- AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::FloatPtr& i)
- {
- createdModal = std::make_shared<AronTreeWidgetTextInputModalController>(label, item, parent);
- }
-
- void
- AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::DoublePtr& i)
- {
- createdModal = std::make_shared<AronTreeWidgetTextInputModalController>(label, item, parent);
- }
-
- void
- AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::BoolPtr& i)
- {
- createdModal = std::make_shared<AronTreeWidgetTextInputModalController>(label, item, parent);
- }
-
void
AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::StringPtr& i)
{
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetModalCreator.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetModalCreator.h
index e22930a1080977d0feb6475c72a3b6c848d5d8e5..f4c8f0223a039c6b9be926d6bddf360bce32f629 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetModalCreator.h
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetModalCreator.h
@@ -29,15 +29,17 @@
namespace armarx
{
- // Convert aron type to tree widget
+ // Convert aron type to tree widget.
+ // (Widgets are only created for string types to enter longer texts.
+ // However, the visitor implementation allows modals for differnt types. Might be useful in the future..)
class AronTreeWidgetModalCreatorVisitor :
public armarx::aron::type::ConstVariantVisitor
{
public:
- std::string label;
- AronTreeWidgetItem* item;
- QTreeWidget* parent;
- AronTreeWidgetModalControllerPtr createdModal;
+ std::string label = "";
+ AronTreeWidgetItem* item = nullptr;
+ QTreeWidget* parent = nullptr;
+ AronTreeWidgetModalControllerPtr createdModal = nullptr;
AronTreeWidgetModalCreatorVisitor() = delete;
AronTreeWidgetModalCreatorVisitor(const std::string& label, AronTreeWidgetItem* item, QTreeWidget* parent) :
@@ -46,22 +48,6 @@ namespace armarx
parent(parent)
{}
- void visitAronVariant(const aron::type::ObjectPtr&) final;
- void visitAronVariant(const aron::type::DictPtr& i) final;
- void visitAronVariant(const aron::type::PairPtr& i) final;
- void visitAronVariant(const aron::type::TuplePtr& i) final;
- void visitAronVariant(const aron::type::ListPtr& i) final;
- void visitAronVariant(const aron::type::NDArrayPtr& i) final;
- void visitAronVariant(const aron::type::MatrixPtr& i) final;
- void visitAronVariant(const aron::type::QuaternionPtr& i) final;
- void visitAronVariant(const aron::type::ImagePtr& i) final;
- void visitAronVariant(const aron::type::PointCloudPtr& i) final;
- void visitAronVariant(const aron::type::IntEnumPtr& i) final;
- void visitAronVariant(const aron::type::IntPtr& i) final;
- void visitAronVariant(const aron::type::LongPtr& i) final;
- void visitAronVariant(const aron::type::FloatPtr& i) final;
- void visitAronVariant(const aron::type::DoublePtr& i) final;
- void visitAronVariant(const aron::type::BoolPtr& i) final;
void visitAronVariant(const aron::type::StringPtr& i) final;
void visitUnknown(Input&) final;
};
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetSetter.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetSetter.cpp
index 87d99fe162344661a0511f49563c5d601342fe11..cf719f8a2d1360d574fb8ebcac867378c3fd6f1b 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetSetter.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetSetter.cpp
@@ -20,34 +20,106 @@
* GNU General Public License
*/
+#include "AronTreeWidgetSetter.h"
+
#include <string>
-#include "AronTreeWidgetSetter.h"
+#include "../ListDictHelper.h"
+#include "../widgets/EditMatrixWidget.h"
+#include "../widgets/IntEnumWidget.h"
+#include "../widgets/QuaternionWidget.h"
+#include "AronTreeWidgetContextMenu.h"
+#include "AronTreeWidgetCreator.h"
+
+
+template <typename T>
+std::string
+usString(T number, size_t precision = 3)
+{
+ std::stringstream ss;
+ const char* locale = "C";
+ ss.imbue(std::locale(locale));
+
+ ss << std::fixed << std::setprecision(precision) << number;
+ return ss.str();
+}
+
//visitors
namespace armarx
{
- bool AronTreeWidgetSetterVisitor::checkTreeWidgetItemForSimilarName(const std::string& name) const
+ bool
+ AronTreeWidgetSetterVisitor::checkTreeWidgetItemForSimilarName(const std::string& name) const
{
QTreeWidgetItem* el = parentItem->child(index);
+
+ // do not check attribute name, if the element is part of a list or map
+ auto* castedThis = AronTreeWidgetItem::DynamicCast(el->parent());
+ if (castedThis)
+ {
+ auto descr = castedThis->aronType->getDescriptor();
+ if (descr == aron::type::Descriptor::LIST || descr == aron::type::Descriptor::DICT)
+ {
+ return true;
+ }
+ }
std::string n = el->text(0).toStdString();
if (name != n)
{
- ARMARX_WARNING_S << "Could not set a tree widget value for the element with key '" << name << "' because it is different from the expected name '" << n << "'.";
+ ARMARX_WARNING_S << "Could not set a tree widget value for the element with key '"
+ << name << "' because it is different from the expected name '" << n
+ << "'.";
return false;
}
return true;
}
- void AronTreeWidgetSetterVisitor::visitAronVariant(const aron::data::DictPtr& i)
+ void
+ AronTreeWidgetSetterVisitor::adjustNumberOfChildren(AronTreeWidgetItem* parent,
+ size_t numChildren)
+ {
+ if (((size_t)parent->childCount()) < numChildren)
+ {
+ // The type to create must be the only child of the current aron type
+ ARMARX_CHECK_EQUAL(parent->aronType->childrenSize(), 1);
+ size_t childrenToAdd = numChildren - parent->childCount();
+ for (size_t j = 0; j < childrenToAdd; ++j)
+ {
+ AronTreeWidgetCreatorVisitor childCreator(parent);
+ aron::type::visit(childCreator, parent->aronType->getChildren()[0]);
+ ARMARX_CHECK_NOT_NULL(childCreator.createdQWidgetItem);
+ }
+ }
+ else if ((size_t)parent->childCount() > numChildren)
+ {
+ size_t numChilds = (size_t)parent->childCount() - numChildren;
+ // pop the last child
+ for (size_t j = 0; j < numChilds; ++j)
+ {
+ parent->removeChild(parent->child(parent->childCount() - 1));
+ }
+ }
+ }
+
+
+ void
+ AronTreeWidgetSetterVisitor::visitAronVariant(const aron::data::DictPtr& i)
{
- if (i->getPath().size() == 0 || checkTreeWidgetItemForSimilarName(i->getPath().getLastElement())) // either it is the root or it has a name
+ // either it is the root or it has a name
+ if (i->getPath().size() == 0 ||
+ checkTreeWidgetItemForSimilarName(i->getPath().getLastElement()))
{
QTreeWidgetItem* el = parentItem->child(index);
+ auto* aronTreeWidget = AronTreeWidgetItem::DynamicCastAndCheck(el);
+ // allocate enough child items
+ adjustNumberOfChildren(aronTreeWidget, i->childrenSize());
+ // write child values
unsigned int x = 0;
for (const auto& [key, value] : i->getElements())
{
+ el->child(x)->setText(0, {key.c_str()});
+
AronTreeWidgetSetterVisitor v(el, x++);
aron::data::visit(v, value);
}
@@ -55,72 +127,224 @@ namespace armarx
}
}
- void AronTreeWidgetSetterVisitor::visitAronVariant(const aron::data::ListPtr& i)
+ void
+ AronTreeWidgetSetterVisitor::visitAronVariant(const aron::data::ListPtr& i)
{
if (checkTreeWidgetItemForSimilarName(i->getPath().getLastElement()))
{
QTreeWidgetItem* el = parentItem->child(index);
+ auto* aronTreeWidget = AronTreeWidgetItem::DynamicCastAndCheck(el);
+ adjustNumberOfChildren(aronTreeWidget, i->childrenSize());
unsigned int x = 0;
for (const auto& value : i->getElements())
{
- AronTreeWidgetSetterVisitor v(el, x++);
+ AronTreeWidgetSetterVisitor v(el, x);
aron::data::visit(v, value);
+ auto* currChild = el->child(x);
+ std::string listNum = usString(x);
+ currChild->setText(0, listNum.c_str());
+
+ ++x;
+ }
+ // This displays the number of children also when the list is collapsed
+ QString numElemsText = misc::generateNumElementsText(i->getElements().size());
+ aronTreeWidget->setText(1, numElemsText);
+ // set italic
+ auto currFont = aronTreeWidget->font(1);
+ currFont.setItalic(true);
+ aronTreeWidget->setFont(1, currFont);
+ }
+ }
+
+
+ void
+ visitMatrix(EditMatrixWidget* matrixWidget,
+ const std::shared_ptr<armarx::aron::type::Matrix>& matrixType,
+ const aron::data::NDArrayPtr& arr)
+ {
+ auto elemType = matrixType->getElementType();
+ auto* rawData = arr->getData();
+ // string can convert any item
+ auto toString = [elemType, rawData](size_t elementNr) -> std::string
+ {
+ switch (elemType)
+ {
+ case aron::type::matrix::ElementType::FLOAT32:
+ {
+ static_assert(sizeof(float) == 4);
+ float* interpreted = reinterpret_cast<float*>(rawData);
+ float laundered = std::launder(interpreted)[elementNr];
+ return usString<float>(laundered);
+ }
+ case aron::type::matrix::ElementType::FLOAT64:
+ {
+ static_assert(sizeof(double) == 8);
+ double* interpreted = reinterpret_cast<double*>(rawData);
+ float laundered = std::launder(interpreted)[elementNr];
+ return usString<double>(laundered);
+ }
+ case aron::type::matrix::ElementType::INT16:
+ {
+ int16_t* interpreted = reinterpret_cast<int16_t*>(rawData);
+ int16_t laudered = std::launder(interpreted)[elementNr];
+ return usString<int16_t>(laudered);
+ }
+ case aron::type::matrix::ElementType::INT32:
+ {
+ int32_t* interpreted = reinterpret_cast<int32_t*>(rawData);
+ int32_t laudered = std::launder(interpreted)[elementNr];
+ return usString<int32_t>(laudered);
+ }
+ case aron::type::matrix::ElementType::INT64:
+ {
+ int64_t* interpreted = reinterpret_cast<int64_t*>(rawData);
+ int64_t laudered = std::launder(interpreted)[elementNr];
+ return usString<int64_t>(laudered);
+ }
+ }
+ return "Error!";
+ };
+
+
+ for (size_t row = 0; (int)row < matrixType->getRows(); ++row)
+ {
+ for (size_t col = 0; (int)col < matrixType->getCols(); ++col)
+ {
+ matrixWidget->setText(row, col, toString(col * matrixType->getRows() + row));
+ }
+ }
+ }
+ void
+ visitQuaternion(QuaternionWidget* quatWidget,
+ std::shared_ptr<armarx::aron::type::Quaternion>& quatType,
+ const aron::data::NDArrayPtr& arr)
+ {
+ auto elemType = quatType->getElementType();
+ auto rawData = arr->getData();
+ auto shape = arr->getShape();
+ // string can convert any item
+ auto toString = [elemType, rawData](size_t elementNr) -> std::string
+ {
+ switch (elemType)
+ {
+ case aron::type::quaternion::ElementType::FLOAT32:
+ {
+ static_assert(sizeof(float) == 4);
+ float* interpreted = reinterpret_cast<float*>(rawData);
+ float laundered = std::launder(interpreted)[elementNr];
+ return usString<float>(laundered);
+ }
+ case aron::type::quaternion::ElementType::FLOAT64:
+ {
+ static_assert(sizeof(double) == 8);
+ double* interpreted = reinterpret_cast<double*>(rawData);
+
+ float laundered = std::launder(interpreted)[elementNr];
+ return usString<double>(laundered);
+ }
}
+ return "Error!";
+ };
+ for (size_t i = 0; i < 4; ++i)
+ {
+ quatWidget->setText((QuaternionWidget::QuaternionComponents)i, toString(i));
}
}
- void AronTreeWidgetSetterVisitor::visitAronVariant(const aron::data::NDArrayPtr& i)
+ void
+ AronTreeWidgetSetterVisitor::visitAronVariant(const aron::data::NDArrayPtr& arr)
{
+ // Matrices are handled as NDArray. Raw ndarrays cannot be created currently
+ auto* castedEl = AronTreeWidgetItem::DynamicCast(parentItem->child(index));
+ ARMARX_CHECK(castedEl);
+
+ auto matrixCast = aron::type::Matrix::DynamicCast(castedEl->aronType);
+ auto quaternionCast = aron::type::Quaternion::DynamicCast(castedEl->aronType);
+
+ auto* rootWidget = castedEl->treeWidget();
+ ARMARX_CHECK(rootWidget);
+ auto* matrixWidget = EditMatrixWidget::DynamicCast(rootWidget->itemWidget(castedEl, 1));
+ auto* quaternionWidget = QuaternionWidget::DynamicCast(rootWidget->itemWidget(castedEl, 1));
+ if (matrixCast && matrixWidget)
+ {
+ visitMatrix(matrixWidget, matrixCast, arr);
+ }
+ else if (quaternionCast && quaternionWidget)
+ {
+ visitQuaternion(quaternionWidget, quaternionCast, arr);
+ }
+ else
+ {
+ ARMARX_ERROR
+ << "we do not support raw NDArrays. Ask Fabian Peller for more information.";
+ }
}
- void AronTreeWidgetSetterVisitor::visitAronVariant(const aron::data::IntPtr& i)
+ void
+ AronTreeWidgetSetterVisitor::visitAronVariant(const aron::data::IntPtr& i)
{
if (checkTreeWidgetItemForSimilarName(i->getPath().getLastElement()))
{
QTreeWidgetItem* el = parentItem->child(index);
- el->setText(1, QString::fromStdString(std::to_string(i->getValue())));
+ auto* enumWidget = IntEnumWidget::DynamicCast(el->treeWidget()->itemWidget(el, 1));
+ auto newText = QString::fromStdString(usString<int>(i->getValue()));
+ if (enumWidget)
+ {
+ // Its an IntEnum! -> Ask the custom widget
+ enumWidget->setText(newText);
+ }
+ else
+ {
+ // Its just an int. -> do the QTreeWidgetItem call
+ el->setText(1, newText);
+ }
}
}
- void AronTreeWidgetSetterVisitor::visitAronVariant(const aron::data::LongPtr& i)
+ void
+ AronTreeWidgetSetterVisitor::visitAronVariant(const aron::data::LongPtr& i)
{
if (checkTreeWidgetItemForSimilarName(i->getPath().getLastElement()))
{
QTreeWidgetItem* el = parentItem->child(index);
- el->setText(1, QString::fromStdString(std::to_string(i->getValue())));
+ el->setText(1, QString::fromStdString(usString<long>(i->getValue())));
}
}
- void AronTreeWidgetSetterVisitor::visitAronVariant(const aron::data::FloatPtr& i)
+ void
+ AronTreeWidgetSetterVisitor::visitAronVariant(const aron::data::FloatPtr& i)
{
if (checkTreeWidgetItemForSimilarName(i->getPath().getLastElement()))
{
QTreeWidgetItem* el = parentItem->child(index);
- el->setText(1, QString::fromStdString(std::to_string(i->getValue())));
+ el->setText(1, QString::fromStdString(usString<float>(i->getValue())));
}
}
- void AronTreeWidgetSetterVisitor::visitAronVariant(const aron::data::DoublePtr& i)
+ void
+ AronTreeWidgetSetterVisitor::visitAronVariant(const aron::data::DoublePtr& i)
{
if (checkTreeWidgetItemForSimilarName(i->getPath().getLastElement()))
{
QTreeWidgetItem* el = parentItem->child(index);
- el->setText(1, QString::fromStdString(std::to_string(i->getValue())));
+ el->setText(1, QString::fromStdString(usString<double>(i->getValue())));
}
}
- void AronTreeWidgetSetterVisitor::visitAronVariant(const aron::data::BoolPtr& i)
+ void
+ AronTreeWidgetSetterVisitor::visitAronVariant(const aron::data::BoolPtr& i)
{
if (checkTreeWidgetItemForSimilarName(i->getPath().getLastElement()))
{
QTreeWidgetItem* el = parentItem->child(index);
- el->setText(1, QString::fromStdString(std::to_string(i->getValue())));
+ el->setText(1, QString::fromStdString(usString<bool>(i->getValue())));
}
}
- void AronTreeWidgetSetterVisitor::visitAronVariant(const aron::data::StringPtr& i)
+ void
+ AronTreeWidgetSetterVisitor::visitAronVariant(const aron::data::StringPtr& i)
{
if (checkTreeWidgetItemForSimilarName(i->getPath().getLastElement()))
{
@@ -129,9 +353,10 @@ namespace armarx
}
}
- void AronTreeWidgetSetterVisitor::visitUnknown(Input&)
+ void
+ AronTreeWidgetSetterVisitor::visitUnknown(Input&)
{
- ARMARX_WARNING_S << "Received an unknown type when trying to set a skill argument type from an aron data object.";
+ ARMARX_WARNING_S << "Received an unknown type when trying to set a skill argument type "
+ "from an aron data object.";
}
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetSetter.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetSetter.h
index 336be31e20d4ff44496a8a2f8424d7dd3c1419f1..5090c12e82d28498efe8e1e63015e659f1e1abce 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetSetter.h
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetSetter.h
@@ -23,17 +23,18 @@
#include <stack>
-#include "../AronTreeWidgetItem.h"
-
-#include <RobotAPI/libraries/aron/core/type/variant/All.h>
#include <RobotAPI/libraries/aron/core/data/variant/All.h>
#include <RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.h>
+#include <RobotAPI/libraries/aron/core/type/variant/All.h>
+
+#include "../AronTreeWidgetItem.h"
namespace armarx
{
- // Conversion from TreeView to aron data
- class AronTreeWidgetSetterVisitor :
- public armarx::aron::data::ConstVariantVisitor
+ // Conversion from aron data to Tree View data
+ // This should be used to programatically change data to be displayed in the GUI.
+ // This code will not be called, if changes are made on the GUI side.
+ class AronTreeWidgetSetterVisitor : public armarx::aron::data::ConstVariantVisitor
{
public:
QTreeWidgetItem* parentItem;
@@ -41,9 +42,9 @@ namespace armarx
AronTreeWidgetItem* qWidgetItem;
AronTreeWidgetSetterVisitor() = delete;
- AronTreeWidgetSetterVisitor(QTreeWidgetItem* i, int x) :
- parentItem(i), index(x)
- {}
+ AronTreeWidgetSetterVisitor(QTreeWidgetItem* i, int x) : parentItem(i), index(x)
+ {
+ }
virtual void visitAronVariant(const aron::data::DictPtr&) final;
virtual void visitAronVariant(const aron::data::ListPtr&) final;
@@ -58,7 +59,6 @@ namespace armarx
private:
bool checkTreeWidgetItemForSimilarName(const std::string& name) const;
+ void adjustNumberOfChildren(AronTreeWidgetItem* parent, size_t numChildren);
};
-}
-
-
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/CustomWidget.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/CustomWidget.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..51836fde9dc7a71fa44740c415fbaf03c3df79e7
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/CustomWidget.cpp
@@ -0,0 +1,42 @@
+#include "CustomWidget.h"
+
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+
+namespace armarx
+{
+ CustomWidget::CustomWidget(QTreeWidgetItem* overlayingItem) : overlayingItem(overlayingItem)
+ {
+ // connect to general QTreeWidgetItem callback.
+ // This also lets the conversion start for other objects. (Not just this widget)
+ ARMARX_CHECK(connect(this,
+ SIGNAL(elemChanged(QTreeWidgetItem*, int)),
+ overlayingItem->treeWidget(),
+ SIGNAL(itemChanged(QTreeWidgetItem*, int))));
+ }
+
+ CustomWidget*
+ CustomWidget::DynamicCast(QWidget* elem)
+ {
+ return dynamic_cast<CustomWidget*>(elem);
+ }
+
+ CustomWidget*
+ CustomWidget::DynamicCastAndCheck(QWidget* elem)
+ {
+ if (!elem)
+ {
+ return nullptr;
+ }
+ auto* casted = DynamicCast(elem);
+ ARMARX_CHECK_NOT_NULL(casted);
+ return casted;
+ }
+
+ void
+ CustomWidget::setSupressSignals(bool doSupress)
+ {
+ QObject::blockSignals(doSupress);
+ }
+
+
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/CustomWidget.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/CustomWidget.h
new file mode 100644
index 0000000000000000000000000000000000000000..49477ffa148e6a5c38279015a356571e107dadb7
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/CustomWidget.h
@@ -0,0 +1,26 @@
+#pragma once
+
+#include <QTreeWidgetItem>
+#include <QWidget>
+
+namespace armarx
+{
+
+ class CustomWidget : public QWidget
+ {
+ Q_OBJECT
+
+ public:
+ CustomWidget(QTreeWidgetItem* overlayingItem);
+ static CustomWidget* DynamicCast(QWidget*);
+ static CustomWidget* DynamicCastAndCheck(QWidget*);
+
+ virtual void setSupressSignals(bool doSupress);
+
+ protected:
+ QTreeWidgetItem* overlayingItem;
+
+ signals:
+ void elemChanged(QTreeWidgetItem* elem, int col);
+ };
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/EditMatrixWidget.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/EditMatrixWidget.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8e01710f03cf9fabe1f9f50b060bc058016099c8
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/EditMatrixWidget.cpp
@@ -0,0 +1,250 @@
+#include "EditMatrixWidget.h"
+
+#include <QGridLayout>
+#include <QLabel>
+
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+#include "ArmarXCore/core/logging/Logging.h"
+
+#include "../../ColorPalettes.h"
+#include "../visitors/AronTreeWidgetCreator.h"
+#include "NDArrayHelper.h"
+
+namespace armarx
+{
+ EditMatrixWidget::EditMatrixWidget(long numRows,
+ long numCols,
+ aron::type::matrix::ElementType elemType,
+ QTreeWidgetItem* currentItem) :
+ CustomWidget(currentItem)
+ {
+ realRows = numRows;
+ realCols = numCols;
+ this->elemType = elemType;
+ // init surrounding layout
+ outerVerticalLayout = new QVBoxLayout();
+ innerHorizontalLayout = new QHBoxLayout();
+ outerVerticalLayout->addItem(innerHorizontalLayout);
+
+
+ QGridLayout* innerGrid = new QGridLayout();
+ const long createRows = std::min(numRows, MAX_ROWS_DISPLAY);
+ const long createCols = std::min(numCols, MAX_COLS_DISPLAY);
+
+ for (long i = 0; i < createRows * createCols; ++i)
+ {
+ auto* edit = new QLineEdit();
+ dispElements.push_back(edit);
+ int currRow = i / createCols;
+ int currCol = i % createCols;
+
+ innerGrid->addWidget(edit, currRow, currCol);
+ std::stringstream ss;
+ ss << "(" << currRow << ", " << currCol << ")";
+ std::string text = ss.str();
+
+ edit->setText(text.c_str());
+ }
+ innerHorizontalLayout->addItem(innerGrid);
+
+ // check, if we need to signal to the user, that not all elements are displayed.
+ if (numRows > MAX_ROWS_DISPLAY)
+ {
+ QLabel* fullLabel = new QLabel("...");
+ outerVerticalLayout->addWidget(fullLabel);
+ }
+ if (numCols > MAX_COLS_DISPLAY)
+ {
+ QLabel* fullLabel = new QLabel("...");
+ innerHorizontalLayout->addWidget(fullLabel);
+ }
+ setLayout(outerVerticalLayout);
+
+ // add hidden elements in vector
+ {
+ const auto hiddenCols = numCols - createCols;
+ // the 0th element holds all columns that are
+ // to the right of the displayed cols AND besides the displayed rows!
+ hiddenElems.push_back(std::vector<std::string>(hiddenCols * createRows, ""));
+
+ // add all rows that are hidden
+ for (long i = 0; i < numRows - createRows; ++i)
+ {
+ // add full cols; everything is hidden here
+ hiddenElems.push_back(std::vector<std::string>(numCols, ""));
+ }
+ }
+
+ for (size_t i = 0; i < dispElements.size(); ++i)
+ {
+ ARMARX_CHECK(connect(
+ dispElements[i], SIGNAL(editingFinished()), this, SLOT(matrixElementChanged())));
+ }
+ }
+
+ EditMatrixWidget*
+ EditMatrixWidget::DynamicCast(QWidget* elem)
+ {
+ return dynamic_cast<EditMatrixWidget*>(elem);
+ }
+
+ EditMatrixWidget*
+ EditMatrixWidget::DynamicCastAndCheck(QWidget* elem)
+ {
+ if (!elem)
+ {
+ return nullptr;
+ }
+ auto* casted = DynamicCast(elem);
+ ARMARX_CHECK_NOT_NULL(casted);
+ return casted;
+ }
+
+ void
+ EditMatrixWidget::setText(long row, long col, const std::string& str)
+ {
+ ARMARX_CHECK(row < realRows);
+ ARMARX_CHECK(col < realCols);
+ auto dispCols = getDisplayedCols();
+ auto dispRows = getDisplayedRows();
+ if (row < dispRows && col < dispCols)
+ {
+ dispElements[row * dispCols + col]->setText(str.c_str());
+ dispElements[row * dispCols + col]->setCursorPosition(0);
+ }
+ else if (row < dispRows)
+ {
+ long idx = row * (realCols - dispCols) + col - dispCols;
+ hiddenElems.at(0).at(idx) = str;
+ }
+ else
+ {
+ hiddenElems.at(row - dispRows + 1).at(col) = str;
+ }
+ }
+
+ std::string
+ EditMatrixWidget::getText(long row, long col)
+ {
+ ARMARX_CHECK(row < realRows);
+ ARMARX_CHECK(col < realCols);
+ const auto dispRows = getDisplayedRows();
+ const auto dispCols = getDisplayedCols();
+ if (row < dispRows && col < dispCols)
+ {
+ auto txt = dispElements.at(row * dispCols + col)->text();
+ return txt.toStdString();
+ }
+ else if (row < getDisplayedRows())
+ {
+ // the stuff besides the displayed rows
+ long idx = row * (realCols - dispCols) + col - dispCols;
+ return hiddenElems.at(0).at(idx);
+ }
+ else
+ {
+ // stuff beneath displayed rows
+ return hiddenElems.at(row - dispRows + 1).at(col);
+ }
+ }
+
+ void
+ EditMatrixWidget::highlightUnparsableEntries()
+ {
+ auto dispRows = getDisplayedRows();
+ auto dispCols = getDisplayedCols();
+ for (long row = 0; row < dispRows; ++row)
+ {
+ for (long col = 0; col < dispCols; ++col)
+ {
+ auto parsed = parseElement(row, col);
+ if (parsed.empty())
+ {
+ dispElements.at(row * dispCols + col)
+ ->setPalette(gui_color_palette::getErrorPalette());
+ }
+ else
+ {
+ dispElements.at(row * dispCols + col)
+ ->setPalette(gui_color_palette::getNormalPalette());
+ }
+ }
+ }
+ }
+
+ bool
+ EditMatrixWidget::hasParseErrors()
+ {
+ // also parse the hidden stuff!
+ for (long row = 0; row < realRows; ++row)
+ {
+ for (long col = 0; col < realCols; ++col)
+ {
+ auto parsed = parseElement(row, col);
+ if (parsed.empty())
+ {
+ if (row >= getDisplayedRows() || col >= getDisplayedCols())
+ {
+ ARMARX_ERROR
+ << "Programming error! Could not parse content in EditMatrixWidget "
+ "that was set programatically from inside the SkillManagerPlugin. "
+ "The error occured in row "
+ << row << " and col " << col << ".";
+ }
+ return true;
+ }
+ }
+ }
+ return false;
+ }
+
+ std::vector<unsigned char>
+ EditMatrixWidget::parseElement(long row, long col)
+ {
+ std::string str = getText(row, col);
+ try
+ {
+ switch (elemType)
+ {
+ case armarx::aron::type::matrix::INT16:
+ return NDArrayHelper::toByteVector<int16_t>(str);
+
+ case armarx::aron::type::matrix::INT32:
+ return NDArrayHelper::toByteVector<int32_t>(str);
+
+ case armarx::aron::type::matrix::INT64:
+ return NDArrayHelper::toByteVector<int64_t>(str);
+
+ case armarx::aron::type::matrix::FLOAT32:
+ return NDArrayHelper::toByteVector<float>(str);
+
+ case armarx::aron::type::matrix::FLOAT64:
+ return NDArrayHelper::toByteVector<double>(str);
+ }
+ }
+ catch (const simox::error::SimoxError& err)
+ {
+ return {};
+ }
+ return {};
+ }
+
+ long
+ EditMatrixWidget::getDisplayedRows() const
+ {
+ return std::min(realRows, MAX_ROWS_DISPLAY);
+ }
+ long
+ EditMatrixWidget::getDisplayedCols() const
+ {
+ return std::min(realCols, MAX_COLS_DISPLAY);
+ }
+ void
+ EditMatrixWidget::matrixElementChanged()
+ {
+ blockSignals(true);
+ highlightUnparsableEntries();
+ blockSignals(false);
+ emit elemChanged(overlayingItem, 1);
+ }
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/EditMatrixWidget.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/EditMatrixWidget.h
new file mode 100644
index 0000000000000000000000000000000000000000..1fdaaa9630a09ab4a73f83fc05fa065fd0663e4d
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/EditMatrixWidget.h
@@ -0,0 +1,81 @@
+#pragma once
+#include <vector>
+
+#include <QHBoxLayout>
+#include <QLineEdit>
+#include <QObject>
+#include <QTreeWidgetItem>
+#include <QVBoxLayout>
+
+#include "RobotAPI/libraries/aron/core/type/variant/ndarray/Matrix.h"
+
+#include "CustomWidget.h"
+
+namespace armarx
+{
+ // Custom Widget used to display Matrices
+ // MAX_ROWS_DISPLAY and MAX_COLS_DISPLAY are the maximal number of elements displayable
+ // everything else will not be displayed. However, there can still be made changes to this data with setText()
+ class EditMatrixWidget : public CustomWidget
+ {
+ Q_OBJECT
+ public:
+ EditMatrixWidget() = delete;
+ EditMatrixWidget(long numRows,
+ long numCols,
+ aron::type::matrix::ElementType elemType,
+ QTreeWidgetItem* currentWidget);
+
+ static EditMatrixWidget* DynamicCast(QWidget*);
+ static EditMatrixWidget* DynamicCastAndCheck(QWidget*);
+
+ // Sets the text on all visible rows and cols.
+ void setText(long row, long col, const std::string& str);
+ std::string getText(long row, long col);
+ void highlightUnparsableEntries();
+ bool hasParseErrors();
+ // returns an empty vector if parsing failed. Else, contains the individual bytes.
+ std::vector<unsigned char> parseElement(long row, long col);
+
+
+ private:
+ // Dimensions of the underlying Type to represent
+ long realRows = 1;
+ long realCols = 1;
+
+ // maximum of rows / cols to display
+ static constexpr long MAX_ROWS_DISPLAY = 5;
+ static constexpr long MAX_COLS_DISPLAY = 5;
+ long getDisplayedRows() const;
+ long getDisplayedCols() const;
+
+ // Want to add dots to show the user that not all is displayed. This needs some wrapping layouts
+ QVBoxLayout* outerVerticalLayout;
+ QHBoxLayout* innerHorizontalLayout;
+
+ // Using row major layout
+ std::vector<QLineEdit*> dispElements;
+ // rows<cols<text>>: stores get and set messages for hidden elements (if there is not enough space for all elements)
+ std::vector<std::vector<std::string>> hiddenElems;
+
+ template <typename T>
+ static std::vector<unsigned char> toByteVector(const std::string& str);
+
+ // TODO: It would be nice to have this code somewhere else, this way, at least the GUI-responses
+ // are restricted to this Widget (signal & slot stuff)
+ aron::type::matrix::ElementType elemType;
+
+ private slots:
+ void matrixElementChanged();
+ };
+
+ template <typename T>
+ std::vector<unsigned char>
+ EditMatrixWidget::toByteVector(const std::string& str)
+ {
+ auto val = simox::alg::to_<T>(str);
+ std::vector<unsigned char> res(sizeof(val), 0);
+ *reinterpret_cast<T*>(res.data()) = val;
+ return res;
+ }
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/IntEnumWidget.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/IntEnumWidget.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bfe1be2bf154768ae5ff49eff04d13caf492d133
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/IntEnumWidget.cpp
@@ -0,0 +1,139 @@
+#include "IntEnumWidget.h"
+
+#include <QHBoxLayout>
+
+#include "RobotAPI/libraries/aron/core/type/variant/All.h"
+
+#include "../../ColorPalettes.h"
+#include "../visitors/AronTreeWidgetConverter.h"
+
+namespace armarx
+{
+ IntEnumWidget::IntEnumWidget(const aron::type::IntEnumPtr& enumPtr,
+ QTreeWidgetItem* currentItem) :
+ CustomWidget(currentItem), element_type(enumPtr)
+ {
+ auto names = enumPtr->getAcceptedValueNames();
+ auto outerLayout = new QHBoxLayout();
+ outerLayout->setContentsMargins(0, 0, 0, 0);
+ innerWidget = new QComboBox();
+ outerLayout->addWidget(innerWidget);
+
+ for (const auto& n : names)
+ {
+ innerWidget->addItem(n.c_str());
+ }
+ innerWidget->setInsertPolicy(QComboBox::NoInsert);
+ innerWidget->setEditable(true);
+ innerWidget->setCurrentIndex(0);
+
+ setLayout(outerLayout);
+ ARMARX_CHECK(connect(innerWidget,
+ SIGNAL(currentTextChanged(const QString&)),
+ this,
+ SLOT(textChanged(const QString&))));
+ }
+
+ IntEnumWidget*
+ IntEnumWidget::DynamicCast(QWidget* widget)
+ {
+ return dynamic_cast<IntEnumWidget*>(widget);
+ }
+
+ IntEnumWidget*
+ IntEnumWidget::DynamicCastAndCheck(QWidget* elem)
+ {
+ if (!elem)
+ {
+ return nullptr;
+ }
+ auto* casted = DynamicCast(elem);
+ ARMARX_CHECK_NOT_NULL(casted);
+ return casted;
+ }
+
+ bool
+ IntEnumWidget::hasParseErrors()
+ {
+ return parseToAron().first;
+ }
+
+ void
+ IntEnumWidget::setText(const QString& text)
+ {
+ innerWidget->setEditText(text);
+ highlightUnparsableEntries();
+ }
+
+ QString
+ IntEnumWidget::getText()
+ {
+ return innerWidget->currentText();
+ }
+
+ std::pair<bool, aron::data::IntPtr>
+ IntEnumWidget::parseToAron()
+ {
+ auto crAron = std::make_shared<aron::data::Int>(element_type->getPath());
+
+ // first look for strings in value map
+ auto valueMap = element_type->getAcceptedValueMap();
+ std::string toParse = getText().toStdString();
+
+ auto searchRes = valueMap.find(toParse);
+ if (searchRes != valueMap.end())
+ {
+ crAron->setValue(searchRes->second);
+ return {true, crAron};
+ }
+ // maybe its the number directly
+ auto accVals = element_type->getAcceptedValues();
+ int res;
+ try
+ {
+ res = simox::alg::to_<int>(toParse);
+ }
+ catch (const simox::error::SimoxError& err)
+ {
+ ARMARX_VERBOSE << "Failed to parse IntEnum: Could not convert \'" << toParse
+ << "\' to an int. It also was not one of the accepted strings.";
+ return {false, nullptr};
+ }
+ if (std::find(accVals.begin(), accVals.end(), res) != accVals.end())
+ {
+
+ crAron->setValue(res);
+ }
+ else
+ {
+ ARMARX_VERBOSE << "Parsed int " << res
+ << "but this int is not part of the accepted values.";
+ return {false, nullptr};
+ }
+ return {true, crAron};
+ }
+
+ void
+ IntEnumWidget::highlightUnparsableEntries()
+ {
+ auto parsed = parseToAron().first;
+ if (parsed)
+ {
+ innerWidget->setPalette(gui_color_palette::getNormalPalette());
+ }
+ else
+ {
+ innerWidget->setPalette(gui_color_palette::getErrorPalette());
+ }
+ }
+
+ void
+ IntEnumWidget::textChanged(const QString&)
+ {
+ setSupressSignals(true);
+ highlightUnparsableEntries();
+ setSupressSignals(false);
+ // fire change signal
+ emit CustomWidget::elemChanged(overlayingItem, 1);
+ }
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/IntEnumWidget.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/IntEnumWidget.h
new file mode 100644
index 0000000000000000000000000000000000000000..095ea480f00a6962bb0ebf407c4f43a8379a59e3
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/IntEnumWidget.h
@@ -0,0 +1,42 @@
+#pragma once
+
+#include <QComboBox>
+
+#include "RobotAPI/libraries/aron/core/data/variant/primitive/Int.h"
+#include "RobotAPI/libraries/aron/core/type/variant/forward_declarations.h"
+
+#include "../AronTreeWidgetItem.h"
+#include "CustomWidget.h"
+
+namespace armarx
+{
+ // Custom wrapper around the QComboBox widget to represent IntEnums
+ // This class handles parsing once the user edited the field and is also responsible
+ // to parse the according aron data from text
+ class IntEnumWidget : public CustomWidget
+ {
+ Q_OBJECT
+ public:
+ IntEnumWidget(const aron::type::IntEnumPtr&, QTreeWidgetItem* currentItem);
+ // needs to be called after the constructor, because qt needs the meta object to be fully constructed at this point
+ void postCtorCall();
+
+ static IntEnumWidget* DynamicCast(QWidget*);
+ static IntEnumWidget* DynamicCastAndCheck(QWidget*);
+
+ bool hasParseErrors();
+ void setText(const QString& text);
+ QString getText();
+ std::pair<bool, aron::data::IntPtr> parseToAron();
+
+ private:
+ const aron::type::IntEnumPtr element_type;
+ QComboBox* innerWidget;
+
+ bool noParseErrors = true;
+
+ void highlightUnparsableEntries();
+ private slots:
+ void textChanged(const QString&);
+ };
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/NDArrayHelper.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/NDArrayHelper.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a9599a2dbdfd456e1237542d34006f5d4c4c9bbf
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/NDArrayHelper.cpp
@@ -0,0 +1 @@
+#include "NDArrayHelper.h"
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/NDArrayHelper.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/NDArrayHelper.h
new file mode 100644
index 0000000000000000000000000000000000000000..ed91b874cc5f91ee24cf95eb6b46013da7b9d590
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/NDArrayHelper.h
@@ -0,0 +1,30 @@
+#pragma once
+#include <string>
+#include <vector>
+
+#include <SimoxUtility/algorithm/string.h>
+
+class NDArrayHelper
+{
+public:
+ // Uses the simox utility to parse floatingpoint and fixed precision types
+ // it then writes the result into a byte vector
+ // an empty byte vector encodes a parsing error.
+ template <typename T>
+ std::vector<unsigned char> static toByteVector(const std::string& str)
+ {
+ try
+ {
+ auto val = simox::alg::to_<T>(str);
+ std::vector<unsigned char> res(sizeof(val), 0);
+ T* reinterpPtr = reinterpret_cast<T*>(res.data());
+ T* laundered = std::launder(reinterpPtr);
+ *laundered = val;
+ return res;
+ }
+ catch (const simox::error::SimoxError& err)
+ {
+ return {};
+ }
+ }
+};
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/QuaternionWidget.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/QuaternionWidget.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b1d6fa3b946a704b180109c2598b663e2ba03bad
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/QuaternionWidget.cpp
@@ -0,0 +1,160 @@
+#include "QuaternionWidget.h"
+
+#include <QHBoxLayout>
+#include <QLabel>
+#include <QLineEdit>
+
+#include "../../ColorPalettes.h"
+#include "NDArrayHelper.h"
+namespace armarx
+{
+
+ QuaternionWidget*
+ QuaternionWidget::DynamicCast(QWidget* elem)
+ {
+ return dynamic_cast<QuaternionWidget*>(elem);
+ }
+
+ QuaternionWidget*
+ QuaternionWidget::DynamicCastAndCheck(QWidget* elem)
+ {
+ if (!elem)
+ {
+ return nullptr;
+ }
+ auto* casted = DynamicCast(elem);
+ ARMARX_CHECK_NOT_NULL(casted);
+ return casted;
+ }
+
+ void
+ QuaternionWidget::setText(QuaternionComponents col, const std::string& str)
+ {
+ size_t idx = (size_t)col;
+ xyzw_components.at(idx)->setText(str.c_str());
+ highlightUnparsableEntries();
+ }
+
+ bool
+ QuaternionWidget::hasParseErrors()
+ {
+ return parseErrors;
+ }
+
+ armarx::QuaternionWidget::QuaternionWidget(aron::type::quaternion::ElementType elType,
+ QTreeWidgetItem* currentItem) :
+ CustomWidget(currentItem), element_type(elType)
+ {
+ auto outerLayout = new QHBoxLayout();
+ auto labelCol1 = new QVBoxLayout();
+ auto labelCol2 = new QVBoxLayout();
+ auto xzEdit = new QVBoxLayout();
+ auto ywEdit = new QVBoxLayout();
+
+ outerLayout->addLayout(labelCol1);
+ outerLayout->addLayout(xzEdit);
+ outerLayout->addLayout(labelCol2);
+ outerLayout->addLayout(ywEdit);
+
+ xyzw_components.reserve(4);
+ for (size_t i = 0; i < 4; ++i)
+ {
+ xyzw_components.push_back(new QLineEdit());
+ }
+
+ // Tabbing order >> Viewing order. Thats why the indeces are a bit switched
+ labelCol1->addWidget(new QLabel("x"));
+ xzEdit->addWidget(xyzw_components.at(0));
+
+ labelCol2->addWidget(new QLabel("z"));
+ ywEdit->addWidget(xyzw_components.at(2));
+
+ labelCol1->addWidget(new QLabel("y"));
+ xzEdit->addWidget(xyzw_components.at(1));
+
+ labelCol2->addWidget(new QLabel("w"));
+ ywEdit->addWidget(xyzw_components.at(3));
+
+ for (const auto& el : xyzw_components)
+ {
+ ARMARX_CHECK(
+ connect(el, SIGNAL(editingFinished()), this, SLOT(quaternionElementChanged())));
+ }
+
+ setLayout(outerLayout);
+ highlightUnparsableEntries();
+ }
+
+ void
+ QuaternionWidget::highlightUnparsableEntries()
+ {
+ parseErrors = false;
+ for (size_t i = 0; i < 4; ++i)
+ {
+ bool ok = false;
+ switch (element_type)
+ {
+ case armarx::aron::type::quaternion::FLOAT32:
+ {
+ float flt;
+ std::tie(ok, flt) = parseQuatVals<float>((QuaternionComponents)i);
+ break;
+ }
+ case armarx::aron::type::quaternion::FLOAT64:
+ {
+ double dbl;
+ std::tie(ok, dbl) = parseQuatVals<double>((QuaternionComponents)i);
+ break;
+ }
+ }
+ if (ok)
+ {
+ xyzw_components.at(i)->setPalette(gui_color_palette::getNormalPalette());
+ }
+ else
+ {
+ xyzw_components.at(i)->setPalette(gui_color_palette::getErrorPalette());
+ parseErrors = true;
+ }
+ }
+ }
+
+ std::vector<unsigned char>
+ QuaternionWidget::parseAllToNDArray()
+ {
+ std::vector<unsigned char> res;
+ bool success = true;
+ if (element_type == aron::type::quaternion::FLOAT32)
+ {
+ res.reserve(16);
+ for (size_t i = 0; i < 4; ++i)
+ {
+ auto bytevec =
+ NDArrayHelper::toByteVector<float>(xyzw_components.at(i)->text().toStdString());
+ success &= !bytevec.empty();
+ res.insert(res.end(), bytevec.begin(), bytevec.end());
+ }
+ }
+ else
+ {
+ res.reserve(32);
+ for (size_t i = 0; i < 4; ++i)
+ {
+ auto bytevec = NDArrayHelper::toByteVector<double>(
+ xyzw_components.at(i)->text().toStdString());
+ success &= !bytevec.empty();
+ res.insert(res.end(), bytevec.begin(), bytevec.end());
+ }
+ }
+ return (success) ? res : std::vector<unsigned char>();
+ }
+
+ void
+ QuaternionWidget::quaternionElementChanged()
+ {
+ CustomWidget::setSupressSignals(true);
+ highlightUnparsableEntries();
+ CustomWidget::setSupressSignals(false);
+ emit CustomWidget::elemChanged(overlayingItem, 1);
+ }
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/QuaternionWidget.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/QuaternionWidget.h
new file mode 100644
index 0000000000000000000000000000000000000000..d55d2bba35e2712d1d161c83ed86b06b300bcf2d
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/QuaternionWidget.h
@@ -0,0 +1,68 @@
+#pragma once
+#include <vector>
+
+#include <QLineEdit>
+#include <QObject>
+#include <QVBoxLayout>
+
+#include "RobotAPI/libraries/aron/core/type/variant/ndarray/Matrix.h"
+
+#include "CustomWidget.h"
+
+
+namespace armarx
+{
+ // Custom Widget which represents a Quaternion - This class does not normalize the inputs
+ // It can parse its data to NDArray and also handles user edit signals itself. (And also the highlighting if errors occur)
+ class QuaternionWidget : public CustomWidget
+ {
+ Q_OBJECT
+ public:
+ QuaternionWidget(aron::type::quaternion::ElementType type, QTreeWidgetItem* currentItem);
+
+ static QuaternionWidget* DynamicCast(QWidget*);
+ static QuaternionWidget* DynamicCastAndCheck(QWidget*);
+
+ enum struct QuaternionComponents
+ {
+ X = 0,
+ Y,
+ Z,
+ W
+ };
+ void setText(QuaternionComponents col, const std::string& str);
+ std::string getText(QuaternionComponents col);
+ bool hasParseErrors();
+ std::vector<unsigned char> parseAllToNDArray();
+
+ private:
+ std::vector<QLineEdit*> xyzw_components;
+ aron::type::quaternion::ElementType element_type;
+ bool parseErrors = false;
+
+ void highlightUnparsableEntries();
+ template <typename T>
+ std::pair<bool, T> parseQuatVals(QuaternionComponents comp);
+
+ private slots:
+ void quaternionElementChanged();
+ };
+
+ template <typename T>
+ std::pair<bool, T>
+ QuaternionWidget::parseQuatVals(QuaternionWidget::QuaternionComponents comp)
+ {
+ // size_t data_size = element_type == aron::type::quaternion::ElementType::FLOAT32 ? 4 : 8;
+ try
+ {
+ T val = simox::alg::to_<T>(xyzw_components.at((size_t)comp)->text().toStdString());
+ return {true, val};
+ }
+ catch (const simox::error::SimoxError& err)
+ {
+ return {false, NAN};
+ }
+ }
+
+
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/build/.cmake/api/v1/query/cache-v2 b/source/RobotAPI/gui-plugins/SkillManagerPlugin/build/.cmake/api/v1/query/cache-v2
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/build/.cmake/api/v1/query/cmakeFiles-v1 b/source/RobotAPI/gui-plugins/SkillManagerPlugin/build/.cmake/api/v1/query/cmakeFiles-v1
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/build/.cmake/api/v1/query/codemodel-v2 b/source/RobotAPI/gui-plugins/SkillManagerPlugin/build/.cmake/api/v1/query/codemodel-v2
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/build/qtcsettings.cmake b/source/RobotAPI/gui-plugins/SkillManagerPlugin/build/qtcsettings.cmake
new file mode 100644
index 0000000000000000000000000000000000000000..fccf6a67eee120cbb1092f841fda5cca870a5a14
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/build/qtcsettings.cmake
@@ -0,0 +1,8 @@
+# This file is managed by Qt Creator, do not edit!
+
+set("CMAKE_BUILD_TYPE" "RelWithDebInfo" CACHE "STRING" "" FORCE)
+set("CMAKE_PROJECT_INCLUDE_BEFORE" "/common/homes/students/bodmer/opt/qtcreator/qtcreator/share/qtcreator/package-manager/auto-setup.cmake" CACHE "PATH" "" FORCE)
+set("QT_QMAKE_EXECUTABLE" "/usr/lib/qt5/bin/qmake" CACHE "STRING" "" FORCE)
+set("CMAKE_PREFIX_PATH" "/usr" CACHE "STRING" "" FORCE)
+set("CMAKE_C_COMPILER" "/usr/bin/gcc" CACHE "STRING" "" FORCE)
+set("CMAKE_CXX_COMPILER" "/usr/bin/g++" CACHE "STRING" "" FORCE)
\ No newline at end of file
diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index 5d9d3c04b278f8032125f98cf82d63aaa8253155..9e290598fee8047f853044e54ea3775b62ab49f7 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -108,6 +108,7 @@ set(SLICE_FILES
aron.ice
aron/Aron.ice
+ aron/test/AronConversionTestInterface.ice
armem.ice
diff --git a/source/RobotAPI/interface/aron/test/AronConversionTestInterface.ice b/source/RobotAPI/interface/aron/test/AronConversionTestInterface.ice
new file mode 100644
index 0000000000000000000000000000000000000000..924f40c6d8664123c822eb9f7eb338d8d50b52d5
--- /dev/null
+++ b/source/RobotAPI/interface/aron/test/AronConversionTestInterface.ice
@@ -0,0 +1,61 @@
+/*
+ * 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 SpeechX::aron_cpp_to_python_conv_test
+ * author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * date 2023
+ * copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#pragma once
+
+#include <RobotAPI/interface/aron/Aron.ice>
+
+
+module armarx { module aron { module test
+{
+
+module dto
+{
+
+ struct TestAronConversionRequest
+ {
+ string aronClassName;
+ ::armarx::aron::data::dto::Dict probe;
+ };
+ struct TestAronConversionResponse
+ {
+ bool success;
+ string errorMessage;
+
+ ::armarx::aron::data::dto::Dict probe;
+ };
+
+};
+
+
+module dti
+{
+
+ interface AronConversionTestInterface
+ {
+ dto::TestAronConversionResponse testAronConversion(dto::TestAronConversionRequest req);
+ };
+
+};
+
+};};};
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
index 4b61a29ecf41a7af19336ccdc81d30c7dc32189a..f5516353eecf459e4d2cde12db3737b396908972 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
@@ -104,6 +104,7 @@ namespace armarx
{
return findObject(id.dataset(), id.className());
}
+
std::optional<ObjectInfo> ObjectFinder::findObject(const objpose::ObjectPose& obj) const
{
return findObject(obj.objectID);
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
index fea3677bd5aa3d0d2b762beb32e2c7ff8d6cc9ea..6d20564f660aee832698ca9d1eda297b56de9f3c 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
@@ -59,12 +59,17 @@ namespace armarx
return _id.str();
}
- ObjectInfo::path ObjectInfo::objectDirectory() const
+ ObjectInfo::path ObjectInfo::objectDirectory(const bool fixDataPath) const
{
+ if(fixDataPath)
+ {
+ return _relObjectsPath / _id.dataset() / _id.className();
+ }
+
return path(_packageName) / _relObjectsPath / _id.dataset() / _id.className();
}
- PackageFileLocation ObjectInfo::file(const std::string& _extension, const std::string& suffix) const
+ PackageFileLocation ObjectInfo::file(const std::string& _extension, const std::string& suffix, const bool fixDataPath) const
{
std::string extension = _extension;
if (extension.at(0) != '.')
@@ -75,8 +80,8 @@ namespace armarx
PackageFileLocation loc;
loc.package = _packageName;
- loc.relativePath = objectDirectory() / filename;
- loc.absolutePath = _absPackageDataDir / loc.relativePath;
+ loc.relativePath = objectDirectory(fixDataPath) / filename;
+ loc.absolutePath = _absPackageDataDir / objectDirectory(false) / filename;
return loc;
}
@@ -86,14 +91,49 @@ namespace armarx
return file(".xml");
}
+ PackageFileLocation ObjectInfo::urdf() const
+ {
+ return file(".urdf");
+ }
+
+ PackageFileLocation ObjectInfo::sdf() const
+ {
+ return file(".sdf");
+ }
+
+ std::optional<PackageFileLocation> ObjectInfo::getModel() const
+ {
+ if (fs::is_regular_file(simoxXML().absolutePath))
+ {
+ return simoxXML();
+ }
+ else if (fs::is_regular_file(urdf().absolutePath))
+ {
+ return urdf();
+ }
+ else if (fs::is_regular_file(sdf().absolutePath))
+ {
+ return sdf();
+ }
+ else
+ {
+ return std::nullopt;
+ }
+ }
+
PackageFileLocation ObjectInfo::articulatedSimoxXML() const
{
- return file(".xml", "_articulated");
+ return file(".xml", "_articulated", true);
}
PackageFileLocation ObjectInfo::articulatedUrdf() const
{
- return file(".urdf", "_articulated");
+ return file(".urdf", "_articulated", true);
+ }
+
+ PackageFileLocation ObjectInfo::articulatedSdf() const
+ {
+ return file(".sdf", "_articulated");
}
std::optional<PackageFileLocation> ObjectInfo::getArticulatedModel() const
@@ -106,6 +146,10 @@ namespace armarx
{
return articulatedUrdf();
}
+ else if (fs::is_regular_file(articulatedSdf().absolutePath))
+ {
+ return articulatedSdf();
+ }
else
{
return std::nullopt;
@@ -293,5 +337,3 @@ std::ostream& armarx::operator<<(std::ostream& os, const ObjectInfo& rhs)
{
return os << rhs.id();
}
-
-
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h
index 70fd46b47fb7a8a3c992dfe5d6c3d4676915aed7..ccbd4eb1be32116d8fc7ced05d8148f027fefccf 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h
@@ -76,14 +76,19 @@ namespace armarx
std::string idStr() const;
- PackageFileLocation file(const std::string& extension, const std::string& suffix = "") const;
+ PackageFileLocation file(const std::string& extension, const std::string& suffix = "", bool fixDataPath = false) const;
PackageFileLocation simoxXML() const;
+ PackageFileLocation urdf() const;
+ PackageFileLocation sdf() const;
+ /// Return the Simox XML, URDF or SDF, if one exists.
+ std::optional<PackageFileLocation> getModel() const;
PackageFileLocation articulatedSimoxXML() const;
PackageFileLocation articulatedUrdf() const;
- /// Return the articulated Simox XML or URDF, if one exists.
+ PackageFileLocation articulatedSdf() const;
+ /// Return the articulated Simox XML, URDF or SDF, if one exists.
std::optional<PackageFileLocation> getArticulatedModel() const;
@@ -131,7 +136,7 @@ namespace armarx
private:
- path objectDirectory() const;
+ path objectDirectory(bool fixDataPath) const;
std::optional<std::vector<std::string>> loadNames(const std::string& jsonKey) const;
diff --git a/source/RobotAPI/libraries/ArmarXObjects/json_conversions.cpp b/source/RobotAPI/libraries/ArmarXObjects/json_conversions.cpp
index cf42e169e67f0f18aef427c65fd24a73160983bf..446b625e4e91359fadd8dd43b4dacdc460a9d513 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/json_conversions.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/json_conversions.cpp
@@ -83,7 +83,7 @@ void armarx::objects::to_json(simox::json::json& j, const SceneObject& rhs)
j["instanceName"] = rhs.instanceName;
j["collection"] = rhs.collection;
j["position"] = rhs.position;
- j["orientation"] = rhs.orientation;
+ j["orientation"] = rhs.orientation.normalized();
if (rhs.isStatic.has_value())
{
j["isStatic"] = rhs.isStatic.value();
@@ -106,6 +106,7 @@ void armarx::objects::from_json(const simox::json::json& j, SceneObject& rhs)
j.at("collection").get_to(rhs.collection);
j.at("position").get_to(rhs.position);
j.at("orientation").get_to(rhs.orientation);
+ rhs.orientation.normalize();
if (j.count("isStatic"))
{
diff --git a/source/RobotAPI/libraries/CMakeLists.txt b/source/RobotAPI/libraries/CMakeLists.txt
index ecdd9f825e6442376cd19cd0c3a61a2ac590f51b..057440c94c97f8428c191c2d9c7a83edc95cbf51 100644
--- a/source/RobotAPI/libraries/CMakeLists.txt
+++ b/source/RobotAPI/libraries/CMakeLists.txt
@@ -38,3 +38,4 @@ add_subdirectory(skills)
add_subdirectory(RobotUnitDataStreamingReceiver)
add_subdirectory(GraspingUtility)
+add_subdirectory(obstacle_avoidance)
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.cpp b/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.cpp
index ee722453048da777dbad507f6f9b4e54e25a1865..c4b170313f7d9d71d09b055ffd497b9b658af5ac 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.cpp
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.cpp
@@ -58,7 +58,8 @@ namespace armarx
GraspTrajectoryPtr
GraspTrajectory::ReadFromReader(const RapidXmlReaderPtr& reader)
{
- RapidXmlReaderNode root = reader->getRoot("GraspTrajectory");
+ RapidXmlReaderNode root = reader->getRoot();
+
GraspTrajectoryPtr traj;
for (const RapidXmlReaderNode& kpNode : root.nodes("Keypoint"))
{
diff --git a/source/RobotAPI/libraries/armem/client/util/MemoryListener.h b/source/RobotAPI/libraries/armem/client/util/MemoryListener.h
index ceb765e60db13404fbe170900342cff554b3affe..521d9ce6981e9503d90515b0fc10c518eedaae5a 100644
--- a/source/RobotAPI/libraries/armem/client/util/MemoryListener.h
+++ b/source/RobotAPI/libraries/armem/client/util/MemoryListener.h
@@ -63,12 +63,16 @@ namespace armarx::armem::client::util
};
subscribe(subscriptionID, cb);
}
+
template <class CalleeT>
void subscribe(const MemoryID& subscriptionID, CalleeT* callee, MemberCallbackUpdatedOnly<CalleeT> callback)
{
auto cb = [callee, callback](const MemoryID&, const std::vector<MemoryID>& updatedSnapshotIDs)
{
- (callee->*callback)(updatedSnapshotIDs);
+ if(callee)
+ {
+ (callee->*callback)(updatedSnapshotIDs);
+ }
};
subscribe(subscriptionID, cb);
}
diff --git a/source/RobotAPI/libraries/armem/core/base/MemoryBase.h b/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
index b6497f8ff421feb16b7f7b07ede1f2d3e57c2266..efc18d58c99ee298b71ce2e775a012d7e6050b04 100644
--- a/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
@@ -261,12 +261,12 @@ namespace armarx::armem::base
* @param commit The commit.
* @return The resulting memory IDs.
*/
- std::vector<UpdateResult> update(const Commit& commit)
+ std::vector<UpdateResult> update(const Commit& commit, const bool addMissingCoreSegmentDuringUpdate = false, const bool checkMemoryName = true)
{
std::vector<UpdateResult> results;
for (const EntityUpdate& update : commit.updates)
{
- results.push_back(this->update(update));
+ results.push_back(this->update(update, addMissingCoreSegmentDuringUpdate, checkMemoryName));
}
return results;
}
@@ -276,11 +276,14 @@ namespace armarx::armem::base
* @param update The update.
* @return The resulting entity snapshot's ID.
*/
- UpdateResult update(const EntityUpdate& update)
+ UpdateResult update(const EntityUpdate& update, const bool addMissingCoreSegmentDuringUpdate = false, const bool checkMemoryName = true)
{
- this->_checkContainerName(update.entityID.memoryName, this->name());
+ if (checkMemoryName)
+ {
+ this->_checkContainerName(update.entityID.memoryName, this->name());
+ }
- auto [inserted, coreSeg] = _addCoreSegmentIfMissing(update.entityID.coreSegmentName);
+ auto [inserted, coreSeg] = _addCoreSegmentIfMissing(update.entityID.coreSegmentName, addMissingCoreSegmentDuringUpdate);
// Update entry.
UpdateResult ret(coreSeg->update(update));
@@ -349,14 +352,14 @@ namespace armarx::armem::base
protected:
- std::pair<bool, CoreSegmentT*> _addCoreSegmentIfMissing(const std::string& coreSegmentName)
+ std::pair<bool, CoreSegmentT*> _addCoreSegmentIfMissing(const std::string& coreSegmentName, const bool addMissingCoreSegmentDuringUpdate)
{
CoreSegmentT* coreSeg = nullptr;
auto it = this->_container.find(coreSegmentName);
if (it == this->_container.end())
{
- if (_addMissingCoreSegmentDuringUpdate)
+ if (addMissingCoreSegmentDuringUpdate)
{
// Insert into map.
coreSeg = &addCoreSegment(coreSegmentName);
@@ -373,13 +376,5 @@ namespace armarx::armem::base
return {false, coreSeg};
}
}
-
-
- public:
-
- // ToDo: Remove from base structure - this should be specific to the server versions.
- // If necessary at all.
- bool _addMissingCoreSegmentDuringUpdate = false;
-
};
}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryItem.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryItem.cpp
index 6f38d6ea21317b44d37045c985f15d5efe2103da..d5318e1f1e6ec9933ddc7e6b50f3894044111917 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryItem.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryItem.cpp
@@ -33,4 +33,9 @@ namespace armarx::armem::server::ltm
{
return _id.getLeafItem();
}
+
+ void MemoryItem::setMemoryName(const std::string& memoryName)
+ {
+ _id.memoryName = memoryName;
+ }
} // namespace armarx::armem::server::ltm
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryItem.h b/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryItem.h
index 570231509806a4f6a56d55ab4a4a2d7442de8807..b8d17fe471c17e62aa188406648515e5a32f384a 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryItem.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryItem.h
@@ -21,6 +21,7 @@ namespace armarx::armem::server::ltm
std::string name() const;
virtual void setMemoryID(const MemoryID&);
+ void setMemoryName(const std::string& memoryName);
protected:
std::shared_ptr<Processors> processors;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/detail/util/filesystem_util.cpp b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/util/filesystem_util.cpp
index 6b6bfdc1be88e30507e2a5d865b94c44abfb090d..1a40e26a04923bccb123b4b07f00ab9be190efa9 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/detail/util/filesystem_util.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/util/filesystem_util.cpp
@@ -1,5 +1,7 @@
#include "filesystem_util.h"
+#include <algorithm>
+
#include <RobotAPI/libraries/armem/core/error/ArMemError.h>
diff --git a/source/RobotAPI/libraries/armem/server/plugins/Plugin.h b/source/RobotAPI/libraries/armem/server/plugins/Plugin.h
index 02c89e5f323b4376ca4493f6b0f1e8efa22f3b60..aa6afd8171ff50cf43b955ff145a4770cf8f24b7 100644
--- a/source/RobotAPI/libraries/armem/server/plugins/Plugin.h
+++ b/source/RobotAPI/libraries/armem/server/plugins/Plugin.h
@@ -41,7 +41,7 @@ namespace armarx::armem::server::plugins
public:
- /// Set the name of the wm and the ltm (if enabled).
+ /// Set the name of the wm and the ltm
void setMemoryName(const std::string& memoryName);
diff --git a/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp b/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
index 622031b7aead835f8d91b9eeac4ac06362d731a1..b9c789398b0d8ce5c6fb17e0c2cb44fb5bc5b531 100644
--- a/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
+++ b/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
@@ -150,7 +150,7 @@ namespace armarx::armem::gui
connect(this, &This::connected, this, &This::startPeriodicUpdateTimer);
connect(updateWidget, &armem::gui::PeriodicUpdateWidget::update, this, &This::startQueries);
- connect(periodicUpdateTimer, &QTimer::timeout, this, &This::updateAvailableMemories);
+ connect(periodicUpdateTimer, &QTimer::timeout, this, &This::updateListOfActiveMemories);
connect(periodicUpdateTimer, &QTimer::timeout, this, &This::processQueryResults);
connect(memoryGroup->queryWidget(), &armem::gui::QueryWidget::storeInLTM, this, &This::queryAndStoreInLTM);
@@ -267,7 +267,7 @@ namespace armarx::armem::gui
{
std::stringstream ss;
ss << "Memory name '" << memoryName
- << "' is unknown. Known are: " << simox::alg::get_keys(memoryData);
+ << "' is unknown. Known are: " << simox::alg::join(simox::alg::get_keys(memoryData), ", ");
statusLabel->setText(QString::fromStdString(ss.str()));
return nullptr;
}
@@ -288,7 +288,7 @@ namespace armarx::armem::gui
{
TIMING_START(MemoryStore);
- auto enabledMemories = memoryGroup->queryWidget()->enabledMemories();
+ auto enabledMemories = memoryGroup->queryWidget()->getEnabledMemories();
for (auto& [name, reader] : memoryReaders)
{
// skip if memory should not be queried
@@ -321,7 +321,7 @@ namespace armarx::armem::gui
TIMING_START(MemoryStartRecording);
- auto enabledMemories = memoryGroup->queryWidget()->enabledMemories();
+ auto enabledMemories = memoryGroup->queryWidget()->getEnabledMemories();
for (auto& [name, reader] : memoryReaders)
{
// skip if memory should not be queried
@@ -341,7 +341,7 @@ namespace armarx::armem::gui
TIMING_START(MemoryStopRecording);
- auto enabledMemories = memoryGroup->queryWidget()->enabledMemories();
+ auto enabledMemories = memoryGroup->queryWidget()->getEnabledMemories();
for (auto& [name, reader] : memoryReaders)
{
// skip if memory should not be queried
@@ -406,7 +406,8 @@ namespace armarx::armem::gui
TIMING_START(MemoryExport)
std::string status;
- diskControl->storeOnDisk(directory, memoryData, &status);
+ std::vector<wm::Memory> memoryDataVec = simox::alg::get_values(memoryData);
+ diskControl->storeOnDisk(directory, memoryDataVec, &status);
statusLabel->setText(QString::fromStdString(status));
TIMING_END_STREAM(MemoryExport, ARMARX_VERBOSE)
@@ -418,19 +419,29 @@ namespace armarx::armem::gui
{
std::string status;
- std::map<std::string, wm::Memory> data =
+ std::map<std::filesystem::path, wm::Memory> data =
diskControl->loadFromDisk(directory, memoryGroup->queryInput(), &status);
- for (auto& [name, memory] : data)
+ for (auto& [path, memory] : data)
{
+ std::string name = memory.id().memoryName;
+ auto commit = armem::toCommit(memory);
+
if (memoryWriters.count(name) > 0)
{
- auto commit = armem::toCommit(memory);
memoryWriters.at(name).commit(commit);
}
else
{
- ARMARX_WARNING << "No memory with name " << name << " available for commit.";
+ ARMARX_INFO << "No memory with name '" << name << "' available for commit. Create new virtual memory.";
+
+ // Please note: Here we assume that a memory server with the same name does not exist.
+ // I think this assumption is ok, since nobody should use filepaths as memory name.
+ // Nonetheless, we did not restrict the user to do so...
+ std::string virtualMemoryName = name + " (at " + path.string() + ")";
+ wm::Memory virtualMemory(virtualMemoryName);
+ virtualMemory.update(commit, true, false);
+ memoryData[virtualMemoryName] = virtualMemory;
}
}
@@ -487,7 +498,7 @@ namespace armarx::armem::gui
// Can't use a structured binding here because you can't capture those in a lambda
// according to the C++ standard.
- auto enabledMemories = memoryGroup->queryWidget()->enabledMemories();
+ auto enabledMemories = memoryGroup->queryWidget()->getEnabledMemories();
for (const auto& pair : memoryReaders)
{
// skip if memory should not be queried
@@ -592,7 +603,7 @@ namespace armarx::armem::gui
}
// Perhaps remove entries
- auto enabledMemories = memoryGroup->queryWidget()->enabledMemories();
+ auto enabledMemories = memoryGroup->queryWidget()->getEnabledMemories();
for (auto it = memoryData.begin(); it != memoryData.end();)
{
// Drop all entries in memoryData which are not in memoryReaders anymore.
@@ -764,7 +775,7 @@ namespace armarx::armem::gui
void
- MemoryViewer::updateAvailableMemories()
+ MemoryViewer::updateListOfActiveMemories()
{
if (is_connected and mns) // mns must be connected and mns must be available
{
@@ -773,11 +784,13 @@ namespace armarx::armem::gui
memoryReaders = mns.getAllReaders(true);
memoryWriters = mns.getAllWriters(true);
- std::vector<std::string> convVec;
- std::transform(memoryReaders.begin(), memoryReaders.end(), std::back_inserter(convVec), [](const auto& p){return p.first;});
+ std::vector<std::string> activeMemoryNames;
+
+ // add all active memories to update list
+ std::transform(memoryReaders.begin(), memoryReaders.end(), std::back_inserter(activeMemoryNames), [](const auto& p){return p.first;});
TIMING_START(GuiUpdateAvailableMemories);
- memoryGroup->queryWidget()->update(convVec);
+ memoryGroup->queryWidget()->update(activeMemoryNames);
TIMING_END_STREAM(GuiUpdateAvailableMemories, ARMARX_VERBOSE);
}
catch (...)
@@ -796,14 +809,16 @@ namespace armarx::armem::gui
void
MemoryViewer::updateMemoryTree()
{
- std::map<std::string, const armem::wm::Memory*> convMap;
+ std::map<std::string, const armem::wm::Memory*> memoriesToUpdate;
+
+ //auto checkboxStates = memoryGroup->queryWidget()->getAvailableMemoryStates();
for (auto& [name, data] : memoryData)
{
- convMap[name] = &data;
+ memoriesToUpdate[name] = &data;
}
TIMING_START(GuiUpdateMemoryTree)
- memoryGroup->tree()->update(convMap);
+ memoryGroup->tree()->update(memoriesToUpdate);
TIMING_END_STREAM(GuiUpdateMemoryTree, ARMARX_VERBOSE)
if (debugObserver)
diff --git a/source/RobotAPI/libraries/armem_gui/MemoryViewer.h b/source/RobotAPI/libraries/armem_gui/MemoryViewer.h
index 7be0874b265375827d22067e4325edcbf6c4bd12..69fb59d1c2f83b53ebd6587cfd63c148bcab9b7f 100644
--- a/source/RobotAPI/libraries/armem_gui/MemoryViewer.h
+++ b/source/RobotAPI/libraries/armem_gui/MemoryViewer.h
@@ -110,7 +110,7 @@ namespace armarx::armem::gui
void processQueryResults();
void updateMemoryTree();
- void updateAvailableMemories();
+ void updateListOfActiveMemories();
signals:
diff --git a/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.cpp b/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.cpp
index fe443952ce26bf9546c9238eaf1fa234f13368f6..a490d63436a484cee322d167a02675710c695260 100644
--- a/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.cpp
+++ b/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.cpp
@@ -78,7 +78,7 @@ namespace armarx::armem::gui::disk
void
ControlWidget::storeOnDisk(
QString directory,
- const std::map<std::string, wm::Memory> memoryData,
+ const std::vector<wm::Memory> memoryData,
std::string* outStatus)
{
std::filesystem::path path(directory.toUtf8().constData());
@@ -92,8 +92,9 @@ namespace armarx::armem::gui::disk
else
{
int numStored = 0;
- for (const auto& [name, data] : memoryData)
+ for (const auto& data : memoryData)
{
+ std::string name = data.id().memoryName;
if (std::filesystem::is_regular_file(path / name))
{
status << "Could not export memory '" << name << "' to " << path << ": Cannot overwrite existing file.\n";
@@ -116,7 +117,7 @@ namespace armarx::armem::gui::disk
}
- std::map<std::string, wm::Memory>
+ std::map<std::filesystem::path, wm::Memory>
ControlWidget::loadFromDisk(
QString directory,
const armem::client::QueryInput& _queryInput,
@@ -124,7 +125,7 @@ namespace armarx::armem::gui::disk
{
std::filesystem::path path(directory.toUtf8().constData());
- std::map<std::string, wm::Memory> memoryData;
+ std::map<std::filesystem::path, wm::Memory> memoryData;
auto setStatus = [&](const std::string& s){
if (outStatus)
@@ -161,7 +162,7 @@ namespace armarx::armem::gui::disk
}
}
- // TODO: Only add data that matchs query?
+ // TODO: Only add data that matches query?
// We use LTM as query target for the disk
// armem::client::QueryInput queryInput = _queryInput;
// queryInput.addQueryTargetToAll(armem::query::data::QueryTarget::LTM);
@@ -172,8 +173,8 @@ namespace armarx::armem::gui::disk
if (std::filesystem::is_directory(p))
{
armem::server::ltm::disk::Memory ltm(p.parent_path(), p.filename());
- armem::wm::Memory memory = ltm.loadAllAndResolve(); // load list of references
- memoryData[memory.name()] = std::move(memory);
+ armem::wm::Memory memory = ltm.loadAllAndResolve(); // load list of references and load data
+ memoryData[p] = memory;
numLoaded++;
}
diff --git a/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.h b/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.h
index b14c9b0a0810acbafba74638e6ebd87989e81165..5eb35b96057f0639a30d94df4dada8ecd38afd9a 100644
--- a/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.h
+++ b/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.h
@@ -5,6 +5,7 @@
#include <QWidget>
#include <QString>
+#include <filesystem>
class QPushButton;
@@ -26,10 +27,10 @@ namespace armarx::armem::gui::disk
void
storeOnDisk(
QString directory,
- const std::map<std::string, wm::Memory> memoryData,
+ const std::vector<wm::Memory> memoryData,
std::string* outStatus = nullptr);
- std::map<std::string, wm::Memory>
+ std::map<std::filesystem::path, wm::Memory>
loadFromDisk(
QString directory,
const armem::client::QueryInput& queryInput,
diff --git a/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.cpp b/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.cpp
index 54e5aaec313825ae63bf8f15351339fce9288129..a3ce95672ec49cf3a3cffb426f28a04be4b43ccc 100644
--- a/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.cpp
+++ b/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.cpp
@@ -162,8 +162,35 @@ namespace armarx::armem::gui
return _dropDisabledCheckBox->isChecked();
}
+ std::map<std::string, QueryWidget::ActiveMemoryState> QueryWidget::getAvailableMemoryStates() const
+ {
+ std::scoped_lock l(enabledMemoriesMutex);
+
+ std::map<std::string, QueryWidget::ActiveMemoryState> states;
+ int maxIndex = _availableMemoriesGroupBox->layout()->count();
+ for (int i = 0; i < maxIndex; ++i)
+ {
+ auto w = _availableMemoriesGroupBox->layout()->itemAt(i)->widget();
+ QCheckBox* box = static_cast<QCheckBox*>(w);
+ std::string memoryName = box->text().toStdString();
+ if (box->isEnabled() && box->isChecked())
+ {
+ states[memoryName] = ActiveMemoryState::FoundAndChecked;
+ }
+ else if (box->isEnabled() && !box->isChecked())
+ {
+ states[memoryName] = ActiveMemoryState::FoundAndNotChecked;
+ }
+ else
+ {
+ states[memoryName] = ActiveMemoryState::NotFound;
+ }
+ }
+ return states;
+ }
+
- std::vector<std::string> QueryWidget::enabledMemories() const
+ std::vector<std::string> QueryWidget::getEnabledMemories() const
{
std::scoped_lock l(enabledMemoriesMutex);
diff --git a/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.h b/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.h
index d3d534e5b4896d51d5d0a104852c1d6c64af982f..623b78c4126b6a09acd636c93b9d4c9b6f4934cf 100644
--- a/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.h
+++ b/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.h
@@ -22,6 +22,13 @@ namespace armarx::armem::gui
public:
+ enum class ActiveMemoryState
+ {
+ FoundAndChecked,
+ FoundAndNotChecked,
+ NotFound
+ };
+
QueryWidget();
armem::query::DataMode dataMode() const;
@@ -31,7 +38,9 @@ namespace armarx::armem::gui
int queryLinkRecursionDepth() const;
- std::vector<std::string> enabledMemories() const;
+ std::map<std::string, ActiveMemoryState> getAvailableMemoryStates() const;
+
+ std::vector<std::string> getEnabledMemories() const;
void update(const std::vector<std::string>& memoryNames);
diff --git a/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.cpp b/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.cpp
index ec17f944c60747160a3306032fa89c13899d6b86..e7ceabba97898778af1c13d9ad9c805357f79ac2 100644
--- a/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.cpp
@@ -6,11 +6,10 @@
#include <map>
#include <optional>
#include <ostream>
+#include <type_traits>
#include <utility>
#include <vector>
-#include <type_traits>
-
// ICE
#include <IceUtil/Handle.h>
#include <IceUtil/Time.h>
@@ -34,15 +33,13 @@
#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
// RobotAPI Armem
-#include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
-#include <RobotAPI/libraries/armem/util/util.h>
-
#include <RobotAPI/libraries/armem/client/Query.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/selectors.h>
-
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/util/util.h>
#include <RobotAPI/libraries/armem_laser_scans/aron/LaserScan.aron.generated.h>
#include <RobotAPI/libraries/armem_laser_scans/aron_conversions.h>
#include <RobotAPI/libraries/armem_laser_scans/types.h>
@@ -62,21 +59,22 @@ namespace armarx::armem::laser_scans::client
Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
ARMARX_DEBUG << "TransformReader: registerPropertyDefinitions";
- registerPropertyDefinitions(def);
const std::string prefix = propertyPrefix;
-
}
- void Reader::connect()
+ void
+ Reader::connect()
{
// Wait for the memory to become available and add it as dependency.
- ARMARX_IMPORTANT << "MappingDataReader: Waiting for memory '"
- << constants::memoryName << "' ...";
+ ARMARX_IMPORTANT << "MappingDataReader: Waiting for memory '" << constants::memoryName
+ << "' ...";
try
{
- memoryReader = memoryNameSystem.useReader(MemoryID().withMemoryName(constants::memoryName));
- ARMARX_IMPORTANT << "MappingDataReader: Connected to memory '" << constants::memoryName << "'";
+ memoryReader =
+ memoryNameSystem.useReader(MemoryID().withMemoryName(constants::memoryName));
+ ARMARX_IMPORTANT << "MappingDataReader: Connected to memory '" << constants::memoryName
+ << "'";
}
catch (const armem::error::CouldNotResolveMemoryServer& e)
{
@@ -91,27 +89,43 @@ namespace armarx::armem::laser_scans::client
armarx::armem::client::query::Builder qb;
ARMARX_VERBOSE << "Query for agent: " << query.agent
- << " memory name: " << constants::memoryName;
+ << " memory name: " << constants::memoryName;
if (query.sensorList.empty()) // all sensors
{
// clang-format off
- qb
+ auto& snapshots = qb
.coreSegments().withName(constants::memoryName)
.providerSegments().withName(query.agent)
.entities().all()
- .snapshots().timeRange(query.timeRange.min, query.timeRange.max);
+ .snapshots();
// clang-format on
+ if (query.timeRange.has_value())
+ {
+ snapshots.timeRange(query.timeRange->min, query.timeRange->max);
+ }
+ else
+ {
+ snapshots.latest();
+ }
}
else
{
// clang-format off
- qb
+ auto& snapshots = qb
.coreSegments().withName(constants::memoryName)
.providerSegments().withName(query.agent)
.entities().withNames(query.sensorList)
- .snapshots().timeRange(query.timeRange.min, query.timeRange.max);
+ .snapshots();
// clang-format on
+ if (query.timeRange.has_value())
+ {
+ snapshots.timeRange(query.timeRange->min, query.timeRange->max);
+ }
+ else
+ {
+ snapshots.latest();
+ }
}
return qb;
@@ -126,16 +140,14 @@ namespace armarx::armem::laser_scans::client
ARMARX_WARNING << "No entities!";
}
- const auto convert =
- [](const auto& aronLaserScanStamped,
- const wm::EntityInstance & ei) -> LaserScanStamped
+ const auto convert = [](const auto& aronLaserScanStamped,
+ const wm::EntityInstance& ei) -> LaserScanStamped
{
LaserScanStamped laserScanStamped;
fromAron(aronLaserScanStamped, laserScanStamped);
const auto ndArrayNavigator =
- aron::data::NDArray::DynamicCast(
- ei.data()->getElement("scan"));
+ aron::data::NDArray::DynamicCast(ei.data()->getElement("scan"));
ARMARX_CHECK_NOT_NULL(ndArrayNavigator);
@@ -145,47 +157,48 @@ namespace armarx::armem::laser_scans::client
};
// loop over all entities and their snapshots
- providerSegment.forEachEntity([&outV, &convert](const wm::Entity & entity)
- {
- // If we don't need this warning, we could directly iterate over the snapshots.
- if (entity.empty())
+ providerSegment.forEachEntity(
+ [&outV, &convert](const wm::Entity& entity)
{
- ARMARX_WARNING << "Empty history for " << entity.id();
- }
- ARMARX_DEBUG << "History size: " << entity.size();
-
- entity.forEachInstance([&outV, &convert](const wm::EntityInstance & entityInstance)
- {
- if (const auto o = tryCast<arondto::LaserScanStamped>(entityInstance))
+ // If we don't need this warning, we could directly iterate over the snapshots.
+ if (entity.empty())
{
- outV.push_back(convert(*o, entityInstance));
+ ARMARX_WARNING << "Empty history for " << entity.id();
}
+ ARMARX_DEBUG << "History size: " << entity.size();
+
+ entity.forEachInstance(
+ [&outV, &convert](const wm::EntityInstance& entityInstance)
+ {
+ if (const auto o = tryCast<arondto::LaserScanStamped>(entityInstance))
+ {
+ outV.push_back(convert(*o, entityInstance));
+ }
+ return true;
+ });
return true;
});
- return true;
- });
return outV;
}
- Reader::Result Reader::queryData(const Query& query) const
+ Reader::Result
+ Reader::queryData(const Query& query) const
{
const auto qb = buildQuery(query);
ARMARX_DEBUG << "[MappingDataReader] query ... ";
- const armem::client::QueryResult qResult =
- memoryReader.query(qb.buildQueryInput());
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
ARMARX_DEBUG << "[MappingDataReader] result: " << qResult;
if (not qResult.success)
{
- ARMARX_WARNING << "Failed to query data from memory: "
- << qResult.errorMessage;
- return {.laserScans = {},
- .sensors = {},
- .status = Result::Status::Error,
+ ARMARX_WARNING << "Failed to query data from memory: " << qResult.errorMessage;
+ return {.laserScans = {},
+ .sensors = {},
+ .status = Result::Status::Error,
.errorMessage = qResult.errorMessage};
}
@@ -195,16 +208,17 @@ namespace armarx::armem::laser_scans::client
const auto laserScans = asLaserScans(providerSegment);
std::vector<std::string> sensors;
- providerSegment.forEachEntity([&sensors](const wm::Entity & entity)
- {
- sensors.push_back(entity.name());
- return true;
- });
+ providerSegment.forEachEntity(
+ [&sensors](const wm::Entity& entity)
+ {
+ sensors.push_back(entity.name());
+ return true;
+ });
- return {.laserScans = laserScans,
- .sensors = sensors,
- .status = Result::Status::Success,
+ return {.laserScans = laserScans,
+ .sensors = sensors,
+ .status = Result::Status::Success,
.errorMessage = ""};
}
-} // namespace armarx::armem::vision::laser_scans::client
+} // namespace armarx::armem::laser_scans::client
diff --git a/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.h b/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.h
index 70703521336f8ac0b1fdf9649df4a3777d95f431..55beab204b12a7f72432ef2e38789f1856058934 100644
--- a/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.h
+++ b/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.h
@@ -72,7 +72,8 @@ namespace armarx::armem::laser_scans::client
{
std::string agent;
- TimeRange timeRange;
+ // if not provided, only latest is queried
+ std::optional<TimeRange> timeRange;
// if empty, all sensors will be queried
std::vector<std::string> sensorList;
diff --git a/source/RobotAPI/libraries/armem_laser_scans/server/Visu.cpp b/source/RobotAPI/libraries/armem_laser_scans/server/Visu.cpp
index 2387e7319e2646857d9c821647f32d136ffdd969..551e4792c2eba3b5e19d2ebf468f2d090a53af41 100644
--- a/source/RobotAPI/libraries/armem_laser_scans/server/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_laser_scans/server/Visu.cpp
@@ -301,8 +301,7 @@ namespace armarx::armem::server::laser_scans
if (robots.count(name) == 0)
{
ARMARX_CHECK_NOT_NULL(virtualRobotReader);
- const auto robot = virtualRobotReader->getRobot(
- name, timestamp, VirtualRobot::RobotIO::RobotDescription::eStructure);
+ const auto robot = virtualRobotReader->getRobot(name);
if (robot)
{
@@ -324,7 +323,7 @@ namespace armarx::armem::server::laser_scans
}
else
{
- ARMARX_VERBOSE << "Faield to synchronize robot `" << name << "`";
+ ARMARX_INFO << deactivateSpam(10) << "Failed to synchronize robot `" << name << "`";
}
}
return entry.first;
diff --git a/source/RobotAPI/libraries/armem_objects/aron/ObjectClass.xml b/source/RobotAPI/libraries/armem_objects/aron/ObjectClass.xml
index a90821817037a2deb733474743f33221048ab24b..e2d695429d3c4193ad97df106821f5bb912a0028 100644
--- a/source/RobotAPI/libraries/armem_objects/aron/ObjectClass.xml
+++ b/source/RobotAPI/libraries/armem_objects/aron/ObjectClass.xml
@@ -27,6 +27,22 @@ Core segment type of Object/Class.
<armarx::arondto::PackagePath />
</ObjectChild>
+ <ObjectChild key="urdfPath">
+ <armarx::arondto::PackagePath />
+ </ObjectChild>
+
+ <ObjectChild key="articulatedUrdfPath">
+ <armarx::arondto::PackagePath />
+ </ObjectChild>
+
+ <ObjectChild key="sdfPath">
+ <armarx::arondto::PackagePath />
+ </ObjectChild>
+
+ <ObjectChild key="articulatedSdfPath">
+ <armarx::arondto::PackagePath />
+ </ObjectChild>
+
<ObjectChild key="meshWrlPath">
<armarx::arondto::PackagePath />
</ObjectChild>
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp
index c16ff257603b9fded0a5ea72737b92b40667fdb6..a2b0ddd6b2985997156cfa6c82998de249409e5a 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp
@@ -47,7 +47,7 @@ namespace armarx::armem::articulated_object
ARMARX_DEBUG << "Object " << name << " available";
auto obj = VirtualRobot::RobotIO::loadRobot(
- ArmarXDataPath::resolvePath(it->xml.serialize().path),
+ it->xml.toSystemPath(),
VirtualRobot::RobotIO::eStructure);
if (not obj)
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
index 54049d8bbeb899b350dfab4906a125157ea527b9..0b466d404fbc41d5eee48819418dd5b5cae35d1f 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
@@ -3,44 +3,53 @@
#include <Eigen/Core>
#include <Eigen/Geometry>
+#include <VirtualRobot/Robot.h>
+
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
-
-#include <VirtualRobot/Robot.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
namespace armarx::armem::articulated_object
{
- armem::articulated_object::ArticulatedObject convert(
- const VirtualRobot::Robot& obj,
- const armem::Time& timestamp)
+ armem::articulated_object::ArticulatedObject
+ convert(const VirtualRobot::Robot& obj, const armem::Time& timestamp)
{
ARMARX_DEBUG << "Filename is " << obj.getFilename();
- // TODO(fabian.reister): remove "PriorKnowledgeData" below
+ const std::vector<std::string> packages =
+ armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
+ const std::string package = armarx::ArmarXDataPath::getProject(packages, obj.getFilename());
- return armem::articulated_object::ArticulatedObject
+ // make sure that the relative path is without the 'package/' prefix
+ const std::string fileRelPath = [&obj, &package]() -> std::string
{
- .description = {
- .name = obj.getType(),
- .xml = PackagePath(armarx::ArmarXDataPath::getProject(
- {"PriorKnowledgeData"}, obj.getFilename()),
- obj.getFilename())
- },
- .instance = obj.getName(),
- .config = {
- .timestamp = timestamp,
- .globalPose = Eigen::Affine3f(obj.getRootNode()->getGlobalPose()),
- .jointMap = obj.getJointValues()
- },
- .timestamp = timestamp};
+ if (simox::alg::starts_with(obj.getFilename(), package))
+ {
+ // remove "package" + "/"
+ const std::string fixedFilename = obj.getFilename().substr(package.size() + 1, -1);
+ return fixedFilename;
+ }
+
+ return obj.getFilename();
+ }();
+
+ return armem::articulated_object::ArticulatedObject{
+ .description = {.name = obj.getType(),
+ .xml = PackagePath(armarx::ArmarXDataPath::getProject(
+ {package}, fileRelPath),
+ obj.getFilename())},
+ .instance = obj.getName(),
+ .config = {.timestamp = timestamp,
+ .globalPose = Eigen::Affine3f(obj.getRootNode()->getGlobalPose()),
+ .jointMap = obj.getJointValues()},
+ .timestamp = timestamp};
}
bool
- ArticulatedObjectWriter::storeArticulatedObject(
- const VirtualRobot::RobotPtr& articulatedObject,
- const armem::Time& timestamp)
+ ArticulatedObjectWriter::storeArticulatedObject(const VirtualRobot::RobotPtr& articulatedObject,
+ const armem::Time& timestamp)
{
ARMARX_CHECK_NOT_NULL(articulatedObject);
diff --git a/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
index a95e98d011e52c7f6251a2119b66b971b9d5a045..4dcf3262724cd6a9626a967f9582d4514a478d89 100644
--- a/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
@@ -191,7 +191,11 @@ namespace armarx::armem::server::obj::clazz
}
};
setPathIfExists(data.simoxXmlPath, info.simoxXML());
+ setPathIfExists(data.urdfPath, info.urdf());
+ setPathIfExists(data.sdfPath, info.sdf());
setPathIfExists(data.articulatedSimoxXmlPath, info.articulatedSimoxXML());
+ setPathIfExists(data.articulatedUrdfPath, info.articulatedUrdf());
+ setPathIfExists(data.articulatedSdfPath, info.articulatedSdf());
setPathIfExists(data.meshObjPath, info.wavefrontObj());
setPathIfExists(data.meshWrlPath, info.meshWrl());
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index 887674e048a41f2a93996a0d6fbdb91e90b7f901..c17ce69020b2b7f2c363219339754919fc805723 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -222,12 +222,21 @@ namespace armarx::armem::server::obj::instance
stats.numUpdated++;
VirtualRobot::RobotPtr robot = robots.get(provided.robotName, provided.providerName);
+
+ if(not robot)
+ {
+ ARMARX_INFO << deactivateSpam(1) << "Failed to retrieve robot `" << provided.robotName << "`.";
+ }
+
// robot may be null!
// Update the robot to obtain correct local -> global transformation
if (robot and robotSyncTimestamp != timestamp)
{
- ARMARX_CHECK(robots.reader->synchronizeRobot(*robot, timestamp));
+ ARMARX_CHECK(robots.reader->synchronizeRobot(*robot, timestamp))
+ << "Failed to synchronize robot to timestamp " << timestamp
+ << ". This is " << (Clock::Now() - timestamp).toSecondsDouble() << " seconds in the past.";
+
robotSyncTimestamp = timestamp;
@@ -1302,14 +1311,13 @@ namespace armarx::armem::server::obj::instance
{
// Try to fetch the robot.
ARMARX_CHECK_NOT_NULL(reader);
- bool warnings = false;
VirtualRobot::RobotPtr robot = reader->getRobot(
robotName, Clock::Now(),
- VirtualRobot::RobotIO::RobotDescription::eStructure, warnings);
+ VirtualRobot::RobotIO::RobotDescription::eStructure);
if (robot)
{
- reader->synchronizeRobot(*robot, Clock::Now());
+ ARMARX_CHECK(reader->synchronizeRobot(*robot, Clock::Now()));
// Store robot if valid.
loaded.emplace(robotName, robot);
}
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
index 3a13f6722e5cd2c0f824c630a83367b47af1de72..514096731b83167b902dfe4198deae82725837ed 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
@@ -194,7 +194,7 @@ namespace armarx::armem::server::obj::instance
{
viz::Robot robot(key);
robot.pose(pose);
- robot.file(model->package, model->relativePath);
+ robot.file(model->package, model->package + "/" + model->relativePath);
robot.joints(objectPose.objectJointValues);
layer.add(robot);
diff --git a/source/RobotAPI/libraries/armem_robot/types.h b/source/RobotAPI/libraries/armem_robot/types.h
index 345564d4b859af306acecdc82d00096c4a559351..2bc45c144b53e3900e8c7041c54d33ee0d205f56 100644
--- a/source/RobotAPI/libraries/armem_robot/types.h
+++ b/source/RobotAPI/libraries/armem_robot/types.h
@@ -40,6 +40,15 @@ namespace armarx::armem::robot
Eigen::Vector3f torque;
};
+ using ToFArray = Eigen::MatrixXf;
+
+ struct ToF
+ {
+ ToFArray array;
+ std::string frame;
+ };
+
+
struct RobotState
{
using JointMap = std::map<std::string, float>;
diff --git a/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt b/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
index a9c0ceb1b8cd552915819a052a2ffd73bbfdec29..8dda90015cd030197cd4af43fd2b9d534968fc19 100644
--- a/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
@@ -65,6 +65,7 @@ armarx_enable_aron_file_generation_for_target(
ARON_FILES
aron/JointState.xml
aron/Proprioception.xml
+ aron/Exteroception.xml
aron/TransformHeader.xml
aron/Transform.xml
)
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron/Exteroception.xml b/source/RobotAPI/libraries/armem_robot_state/aron/Exteroception.xml
new file mode 100644
index 0000000000000000000000000000000000000000..80f72e06059a39e60e6709a5e7acb28c63ee3395
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/aron/Exteroception.xml
@@ -0,0 +1,35 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ <SystemInclude include="Eigen/Core" />
+ <SystemInclude include="Eigen/Geometry" />
+ </CodeIncludes>
+
+ <GenerateTypes>
+
+ <Object name="armarx::armem::exteroception::arondto::ToF">
+
+ <ObjectChild key="array">
+ <Matrix rows="-1" cols="-1" type="float32" />
+ </ObjectChild>
+
+ </Object>
+
+
+ <Object name="armarx::armem::arondto::Exteroception">
+
+ <ObjectChild key="iterationID">
+ <long />
+ </ObjectChild>
+
+ <ObjectChild key="tof">
+ <Dict>
+ <armarx::armem::exteroception::arondto::ToF/>
+ </Dict>
+ </ObjectChild>
+
+ </Object>
+
+
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml b/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml
index c7ac7670d399329bb6674dad3243a048153534f8..d5472cbde89434e346f116e6b6f461bf4f2ca34e 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml
+++ b/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml
@@ -234,8 +234,6 @@
</ObjectChild>
</Object>
-
-
<Object name="armarx::armem::arondto::Proprioception">
@@ -257,7 +255,6 @@
</Dict>
</ObjectChild>
-
<ObjectChild key="extraFloats">
<Dict>
<Float />
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
index 1c18fe5b78799b1b2f9555a33a8997e7edb60d28..3749c892d5e46e6ce0f7a848a86e8059ed70991c 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
@@ -6,6 +6,7 @@
#include <RobotAPI/libraries/armem_robot_state/aron/JointState.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Exteroception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/types.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
@@ -97,4 +98,14 @@ namespace armarx::armem
aron::toAron(dto.torque, bo.torque);
}
+
+ void fromAron(const armarx::armem::exteroception::arondto::ToF& dto, robot::ToFArray& bo){
+ bo = dto.array;
+ }
+
+ void toAron(armarx::armem::exteroception::arondto::ToF& dto, const robot::ToFArray& bo){
+ dto.array = bo;
+ }
+
+
} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
index 75c339c7fc4aad7f24b5b52befdf2e7f1451b345..f78efc200ee18205523ddf7ea5c60e8c6949bf9f 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
@@ -40,6 +40,11 @@ namespace armarx::armem
struct ForceTorque;
}
+ namespace exteroception::arondto
+ {
+ struct ToF;
+ }
+
namespace arondto
{
struct Transform;
@@ -65,5 +70,8 @@ namespace armarx::armem
void fromAron(const armarx::armem::prop::arondto::ForceTorque& dto, robot::ForceTorque& bo);
void toAron(armarx::armem::prop::arondto::ForceTorque& dto, const robot::ForceTorque& bo);
+ void fromAron(const armarx::armem::exteroception::arondto::ToF& dto, robot::ToFArray& bo);
+ void toAron(armarx::armem::exteroception::arondto::ToF& dto, const robot::ToFArray& bo);
+
} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
index 611fa0e0d3f5bb931f5d64a60c70735d698c623f..eaa2e9ca07b063eaba3a71887eda2d14fa839c16 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
@@ -22,6 +22,7 @@
#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
#include <RobotAPI/libraries/armem_robot/types.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Exteroception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/JointState.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
@@ -121,7 +122,7 @@ namespace armarx::armem::robot_state
const auto elapsedTime = armem::Time::Now() - tsStartFunctionInvokation;
if (elapsedTime > syncTimeout)
{
- ARMARX_WARNING << "Could not synchronize object " << obj.description.name;
+ ARMARX_VERBOSE << "Could not synchronize object " << obj.description.name;
return false;
}
@@ -135,11 +136,7 @@ namespace armarx::armem::robot_state
}
std::optional<robot::RobotDescription>
- RobotReader::queryDescription(
- const std::string& name,
- const armem::Time& timestamp,
- bool warnings
- )
+ RobotReader::queryDescription(const std::string& name, const armem::Time& timestamp)
{
const auto sanitizedTimestamp = timestamp.isValid() ? timestamp : Clock::Now();
@@ -159,11 +156,8 @@ namespace armarx::armem::robot_state
if (not memoryReader)
{
- if (warnings)
- {
- ARMARX_WARNING << "Memory reader is null. Did you forget to call "
- "RobotReader::connect() in onConnectComponent()?";
- }
+ ARMARX_WARNING << "Memory reader is null. Did you forget to call "
+ "RobotReader::connect() in onConnectComponent()?";
return std::nullopt;
}
@@ -182,10 +176,7 @@ namespace armarx::armem::robot_state
}
catch (...)
{
- if (warnings)
- {
- ARMARX_WARNING << "query description failure" << GetHandledExceptionString();
- }
+ ARMARX_VERBOSE << "Query description failure" << GetHandledExceptionString();
}
return std::nullopt;
@@ -198,14 +189,14 @@ namespace armarx::armem::robot_state
const auto jointMap = queryJointState(description, timestamp);
if (not jointMap)
{
- ARMARX_WARNING << "Failed to query joint state for robot '" << description.name << "'.";
+ ARMARX_VERBOSE << "Failed to query joint state for robot '" << description.name << "'.";
return std::nullopt;
}
const auto globalPose = queryGlobalPose(description, timestamp);
if (not globalPose)
{
- ARMARX_WARNING << "Failed to query global pose for robot " << description.name;
+ ARMARX_VERBOSE << "Failed to query global pose for robot " << description.name;
return std::nullopt;
}
@@ -235,7 +226,7 @@ namespace armarx::armem::robot_state
}
catch (...)
{
- ARMARX_WARNING << GetHandledExceptionString();
+ ARMARX_VERBOSE << GetHandledExceptionString();
return std::nullopt;
}
}
@@ -268,7 +259,7 @@ namespace armarx::armem::robot_state
if (not qResult.success) /* c++20 [[unlikely]] */
{
- ARMARX_WARNING << qResult.errorMessage;
+ ARMARX_VERBOSE << qResult.errorMessage;
return std::nullopt;
}
@@ -276,7 +267,7 @@ namespace armarx::armem::robot_state
}
catch (...)
{
- ARMARX_WARNING << deactivateSpam(1) << "Failed to query joint state. Reason: "
+ ARMARX_VERBOSE << deactivateSpam(1) << "Failed to query joint state. Reason: "
<< GetHandledExceptionString();
return std::nullopt;
}
@@ -306,7 +297,7 @@ namespace armarx::armem::robot_state
if (not qResult.success) /* c++20 [[unlikely]] */
{
- ARMARX_WARNING << qResult.errorMessage;
+ ARMARX_VERBOSE << qResult.errorMessage;
return {};
}
@@ -339,7 +330,7 @@ namespace armarx::armem::robot_state
if (not qResult.success) /* c++20 [[unlikely]] */
{
- ARMARX_WARNING << qResult.errorMessage;
+ ARMARX_VERBOSE << qResult.errorMessage;
return std::nullopt;
}
@@ -352,7 +343,8 @@ namespace armarx::armem::robot_state
{
try
{
- const auto result = transformReader.getGlobalPose(description.name, constants::robotRootNodeName, timestamp);
+ const auto result = transformReader.getGlobalPose(
+ description.name, constants::robotRootNodeName, timestamp);
if (not result)
{
return std::nullopt;
@@ -362,7 +354,7 @@ namespace armarx::armem::robot_state
}
catch (...)
{
- ARMARX_WARNING << deactivateSpam(1) << "Failed to query global pose. Reason: "
+ ARMARX_VERBOSE << deactivateSpam(1) << "Failed to query global pose. Reason: "
<< GetHandledExceptionString();
return std::nullopt;
}
@@ -381,7 +373,7 @@ namespace armarx::armem::robot_state
if (providerSegment.empty())
{
- ARMARX_WARNING << "No entity found";
+ ARMARX_VERBOSE << "No entity found";
return std::nullopt;
}
@@ -394,9 +386,10 @@ namespace armarx::armem::robot_state
instance = &i;
return false; // break
});
- if (!instance)
+
+ if (instance == nullptr)
{
- ARMARX_WARNING << "No entity snapshots found";
+ ARMARX_VERBOSE << "No entity snapshots found";
return std::nullopt;
}
@@ -436,6 +429,11 @@ namespace armarx::armem::robot_state
coreSegment.forEachEntity(
[&jointMap](const wm::Entity& entity)
{
+ if (not entity.getLatestSnapshot().hasInstance(0))
+ {
+ return;
+ }
+
const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
const auto proprioception =
@@ -525,7 +523,7 @@ namespace armarx::armem::robot_state
if (not qResult.success) /* c++20 [[unlikely]] */
{
- ARMARX_WARNING << qResult.errorMessage;
+ ARMARX_VERBOSE << qResult.errorMessage;
return std::nullopt;
}
@@ -558,7 +556,7 @@ namespace armarx::armem::robot_state
if (not qResult.success) /* c++20 [[unlikely]] */
{
- ARMARX_WARNING << qResult.errorMessage;
+ ARMARX_VERBOSE << qResult.errorMessage;
return std::nullopt;
}
@@ -566,6 +564,37 @@ namespace armarx::armem::robot_state
}
+ std::optional<std::map<RobotReader::Hand, robot::ToFArray>>
+ RobotReader::queryToF(const robot::RobotDescription& description,
+ const armem::Time& timestamp) const
+ {
+ // Query all entities from provider.
+ armem::client::query::Builder qb;
+
+ ARMARX_DEBUG << "Querying ToF data for robot: " << description;
+
+ // clang-format off
+ qb
+ .coreSegments().withName(constants::exteroceptionCoreSegment)
+ .providerSegments().withName(description.name) // agent
+ .entities().all() // TODO
+ .snapshots().beforeOrAtTime(timestamp);
+ // clang-format on
+
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+
+ if (not qResult.success) /* c++20 [[unlikely]] */
+ {
+ ARMARX_VERBOSE << qResult.errorMessage;
+ return std::nullopt;
+ }
+
+ return getToF(qResult.memory, description.name);
+ }
+
+
std::optional<robot::PlatformState>
RobotReader::getRobotPlatformState(const armarx::armem::wm::Memory& memory,
const std::string& name) const
@@ -580,6 +609,11 @@ namespace armarx::armem::robot_state
coreSegment.forEachEntity(
[&platformState](const wm::Entity& entity)
{
+ if (not entity.getLatestSnapshot().hasInstance(0))
+ {
+ return;
+ }
+
const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
const auto proprioception =
@@ -623,6 +657,11 @@ namespace armarx::armem::robot_state
coreSegment.forEachEntity(
[&forceTorques](const wm::Entity& entity)
{
+ if (not entity.getLatestSnapshot().hasInstance(0))
+ {
+ return;
+ }
+
const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
const auto proprioception =
@@ -675,6 +714,47 @@ namespace armarx::armem::robot_state
return forceTorques;
}
+ std::map<RobotReader::Hand, robot::ToFArray>
+ RobotReader::getToF(const armarx::armem::wm::Memory& memory, const std::string& name) const
+ {
+ std::map<RobotReader::Hand, robot::ToFArray> tofs;
+
+ // clang-format off
+ const armem::wm::CoreSegment& coreSegment = memory
+ .getCoreSegment(constants::exteroceptionCoreSegment);
+ // clang-format on
+
+ coreSegment.forEachEntity(
+ [&tofs](const wm::Entity& entity)
+ {
+ ARMARX_DEBUG << "Processing ToF element";
+
+ if (not entity.getLatestSnapshot().hasInstance(0))
+ {
+ return;
+ }
+ const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
+
+ const auto exteroception =
+ tryCast<::armarx::armem::arondto::Exteroception>(entityInstance);
+ ARMARX_CHECK(exteroception.has_value());
+
+
+ for (const auto& [handName, dtoFt] : exteroception->tof)
+ {
+ ARMARX_DEBUG << "Processing ToF element for hand `" << handName << "`";
+
+ robot::ToFArray tof;
+ fromAron(dtoFt, tof);
+
+ const auto hand = fromHandName(handName);
+ tofs.emplace(hand, tof);
+ }
+ });
+
+ return tofs;
+ }
+
std::optional<robot::RobotDescription>
RobotReader::getRobotDescription(const armarx::armem::wm::Memory& memory,
@@ -691,11 +771,77 @@ namespace armarx::armem::robot_state
{ instance = &i; });
if (instance == nullptr)
{
- ARMARX_WARNING << "No entity snapshots found in provider segment `" << name << "`";
+ ARMARX_VERBOSE << "No entity snapshots found in provider segment `" << name << "`";
return std::nullopt;
}
return robot::convertRobotDescription(*instance);
}
+ std::vector<robot::RobotDescription>
+ RobotReader::getRobotDescriptions(const armarx::armem::wm::Memory& memory) const
+ {
+ const armem::wm::CoreSegment& coreSegment =
+ memory.getCoreSegment(constants::descriptionCoreSegment);
+
+ std::vector<robot::RobotDescription> descriptions;
+
+ coreSegment.forEachInstance(
+ [&descriptions](const wm::EntityInstance& instance)
+ {
+ if (const std::optional<robot::RobotDescription> desc =
+ robot::convertRobotDescription(instance))
+ {
+ descriptions.push_back(desc.value());
+ }
+ });
+
+ return descriptions;
+ }
+
+ std::vector<robot::RobotDescription>
+ RobotReader::queryDescriptions(const armem::Time& timestamp)
+ {
+ const auto sanitizedTimestamp = timestamp.isValid() ? timestamp : Clock::Now();
+
+ // Query all entities from provider.
+ armem::client::query::Builder qb;
+
+ // clang-format off
+ qb
+ .coreSegments().withName(constants::descriptionCoreSegment)
+ .providerSegments().all()
+ .entities().all()
+ .snapshots().beforeOrAtTime(sanitizedTimestamp);
+ // clang-format on
+
+ ARMARX_DEBUG << "Lookup query in reader";
+
+ if (not memoryReader)
+ {
+ ARMARX_WARNING << "Memory reader is null. Did you forget to call "
+ "RobotReader::connect() in onConnectComponent()?";
+ return {};
+ }
+
+ try
+ {
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+
+ if (not qResult.success) /* c++20 [[unlikely]] */
+ {
+ return {};
+ }
+
+ return getRobotDescriptions(qResult.memory);
+ }
+ catch (...)
+ {
+ ARMARX_VERBOSE << "Query description failure" << GetHandledExceptionString();
+ }
+
+ return {};
+ }
} // namespace armarx::armem::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
index bb63c714180325961c434d61e4a8e31b78a84045..a9cddc6b2afd5258271f5718b4f1c82d0dd6a995 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
@@ -23,6 +23,7 @@
#include <mutex>
#include <optional>
+#include <vector>
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
@@ -50,7 +51,7 @@ namespace armarx::armem::robot_state
void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def);
- bool synchronize(robot::Robot& obj, const armem::Time& timestamp) override;
+ [[nodiscard]] bool synchronize(robot::Robot& obj, const armem::Time& timestamp) override;
std::optional<robot::Robot> get(const std::string& name,
const armem::Time& timestamp) override;
@@ -58,8 +59,9 @@ namespace armarx::armem::robot_state
const armem::Time& timestamp) override;
std::optional<robot::RobotDescription> queryDescription(const std::string& name,
- const armem::Time& timestamp,
- bool warnings = true);
+ const armem::Time& timestamp);
+
+ std::vector<robot::RobotDescription> queryDescriptions(const armem::Time& timestamp);
std::optional<robot::RobotState> queryState(const robot::RobotDescription& description,
const armem::Time& timestamp);
@@ -100,6 +102,10 @@ namespace armarx::armem::robot_state
const armem::Time& start,
const armem::Time& end) const;
+
+ std::optional<std::map<Hand, robot::ToFArray>>
+ queryToF(const robot::RobotDescription& description, const armem::Time& timestamp) const;
+
/**
* @brief retrieve the robot's pose in the odometry frame.
*
@@ -125,6 +131,9 @@ namespace armarx::armem::robot_state
std::optional<robot::RobotDescription>
getRobotDescription(const armarx::armem::wm::Memory& memory, const std::string& name) const;
+ std::vector<robot::RobotDescription>
+ getRobotDescriptions(const armarx::armem::wm::Memory& memory) const;
+
std::optional<robot::RobotState::JointMap>
getRobotJointState(const armarx::armem::wm::Memory& memory, const std::string& name) const;
@@ -142,6 +151,9 @@ namespace armarx::armem::robot_state
std::map<RobotReader::Hand, std::map<armem::Time, robot::ForceTorque>>
getForceTorques(const armarx::armem::wm::Memory& memory, const std::string& name) const;
+ std::map<RobotReader::Hand, robot::ToFArray> getToF(const armarx::armem::wm::Memory& memory,
+ const std::string& name) const;
+
struct Properties
{
} properties;
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
index 8c1f8dfb0ad4f6f833d27191588fc8d766998235..225e048864e69a7c349b9e53163d41ed1e0eef6b 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
@@ -38,16 +38,16 @@ namespace armarx::armem::robot_state
bool
VirtualRobotReader::synchronizeRobot(VirtualRobot::Robot& robot, const armem::Time& timestamp)
{
- const static auto packages = armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
- const auto package = armarx::ArmarXDataPath::getProject(packages, robot.getFilename());
+ // const static auto packages = armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
+ // const auto package = armarx::ArmarXDataPath::getProject(packages, robot.getFilename());
const robot::RobotDescription robotDescription{
- .name = robot.getName(), .xml = PackagePath{package, robot.getFilename()}};
+ .name = robot.getName(), .xml = PackagePath{"", ""}};
const auto robotState = queryState(robotDescription, timestamp);
if (not robotState)
{
- ARMARX_WARNING << "Querying robot state failed for robot `" << robot.getName() << "` "
+ ARMARX_VERBOSE << "Querying robot state failed for robot `" << robot.getName() << "` "
<< "(type `"<< robot.getType() << "`)!";
return false;
}
@@ -61,19 +61,16 @@ namespace armarx::armem::robot_state
VirtualRobot::RobotPtr
VirtualRobotReader::getRobot(const std::string& name,
const armem::Time& timestamp,
- const VirtualRobot::RobotIO::RobotDescription& loadMode,
- bool warnings)
+ const VirtualRobot::RobotIO::RobotDescription& loadMode)
{
ARMARX_INFO << deactivateSpam(60)
<< "Querying robot description for robot '" << name << "'";
- const auto description = queryDescription(name, timestamp, warnings);
+ const auto description = queryDescription(name, timestamp);
if (not description)
{
- if (warnings)
- {
- ARMARX_WARNING << "The description of robot `" << name << "` is not a available!";
- }
+ ARMARX_VERBOSE << "The description of robot `" << name << "` is not a available!";
+
return nullptr;
}
@@ -134,7 +131,7 @@ namespace armarx::armem::robot_state
Clock::WaitFor(sleepAfterFailure);
}
- ARMARX_WARNING << "Failed to get synchronized robot `" << name << "`";
+ ARMARX_VERBOSE << "Failed to get synchronized robot `" << name << "`";
return nullptr;
}
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
index d19e0483fdfa04f00579ba85b1e830f58feee241..ab30cbc1317d98f802dc24cbe5d844da02b038e9 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
@@ -27,7 +27,6 @@
#include "RobotReader.h"
-
namespace armarx::armem::robot_state
{
/**
@@ -47,14 +46,14 @@ namespace armarx::armem::robot_state
void connect();
void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def);
- bool synchronizeRobot(VirtualRobot::Robot& robot, const armem::Time& timestamp);
+ [[nodiscard]] bool synchronizeRobot(VirtualRobot::Robot& robot,
+ const armem::Time& timestamp);
[[nodiscard]] VirtualRobot::RobotPtr
getRobot(const std::string& name,
const armem::Time& timestamp = armem::Time::Invalid(),
const VirtualRobot::RobotIO::RobotDescription& loadMode =
- VirtualRobot::RobotIO::RobotDescription::eStructure,
- bool warnings = true);
+ VirtualRobot::RobotIO::RobotDescription::eStructure);
[[nodiscard]] VirtualRobot::RobotPtr
getSynchronizedRobot(const std::string& name,
@@ -71,14 +70,12 @@ namespace armarx::armem::robot_state
private:
-
[[nodiscard]] VirtualRobot::RobotPtr
_getSynchronizedRobot(const std::string& name,
const armem::Time& timestamp = armem::Time::Invalid(),
const VirtualRobot::RobotIO::RobotDescription& loadMode =
- VirtualRobot::RobotIO::RobotDescription::eStructure,
+ VirtualRobot::RobotIO::RobotDescription::eStructure,
bool blocking = true);
-
};
} // namespace armarx::armem::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/constants.h b/source/RobotAPI/libraries/armem_robot_state/client/common/constants.h
index d9efcbcf795b46ddb4e9647919d38c0c617c1234..5d093c195addda7c64c378d919d38f09987cdfc9 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/constants.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/constants.h
@@ -28,6 +28,7 @@ namespace armarx::armem::robot_state::constants
inline const std::string descriptionCoreSegment = "Description";
inline const std::string localizationCoreSegment = "Localization";
inline const std::string proprioceptionCoreSegment = "Proprioception";
+ inline const std::string exteroceptionCoreSegment = "Exteroception";
inline const std::string descriptionEntityName = "description";
diff --git a/source/RobotAPI/libraries/armem_robot_state/memory_ids.cpp b/source/RobotAPI/libraries/armem_robot_state/memory_ids.cpp
index 926c2e843e252efdc9c4711bbe2b03a3a7394bba..2f219eff39376ed3ecc44e47b0780fd1328785a0 100644
--- a/source/RobotAPI/libraries/armem_robot_state/memory_ids.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/memory_ids.cpp
@@ -30,6 +30,7 @@ namespace armarx::armem
const MemoryID robot_state::descriptionSegmentID { memoryID.withCoreSegmentName("Description") };
const MemoryID robot_state::proprioceptionSegmentID { memoryID.withCoreSegmentName("Proprioception") };
+ const MemoryID robot_state::exteroceptionSegmentID { memoryID.withCoreSegmentName("Exteroception") };
const MemoryID robot_state::localizationSegmentID { memoryID.withCoreSegmentName("Localization") };
diff --git a/source/RobotAPI/libraries/armem_robot_state/memory_ids.h b/source/RobotAPI/libraries/armem_robot_state/memory_ids.h
index 1d8008d4db8d7a71da0a8ca47420040060382ca6..f653236b1f62ec8aef905d45a7ee86af079b05b9 100644
--- a/source/RobotAPI/libraries/armem_robot_state/memory_ids.h
+++ b/source/RobotAPI/libraries/armem_robot_state/memory_ids.h
@@ -32,6 +32,7 @@ namespace armarx::armem::robot_state
extern const MemoryID descriptionSegmentID;
extern const MemoryID proprioceptionSegmentID;
+ extern const MemoryID exteroceptionSegmentID;
extern const MemoryID localizationSegmentID;
} // namespace armarx::armem::objects
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/CMakeLists.txt b/source/RobotAPI/libraries/armem_robot_state/server/CMakeLists.txt
index 169d59f1a86d6a1bea1b9e54033409d23fb3cd7f..3ad1d50684d89c246e6123a6ef5d2055779eab6f 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_robot_state/server/CMakeLists.txt
@@ -43,10 +43,16 @@ armarx_add_library(
proprioception/RobotUnitData.h
proprioception/RobotUnitReader.h
+ proprioception/converters/ConverterInterface.h
proprioception/converters/Armar6Converter.h
proprioception/converters/ConverterTools.h
proprioception/converters/ConverterRegistry.h
- proprioception/converters/ConverterInterface.h
+
+ exteroception/converters/ConverterInterface.h
+ exteroception/converters/ConverterTools.h
+ exteroception/converters/ConverterRegistry.h
+ exteroception/converters/ArmarDEConverter.h
+ exteroception/Segment.h
description/Segment.h
@@ -67,6 +73,12 @@ armarx_add_library(
proprioception/converters/ConverterRegistry.cpp
proprioception/converters/ConverterInterface.cpp
+ exteroception/converters/ConverterInterface.cpp
+ exteroception/converters/ConverterTools.cpp
+ exteroception/converters/ConverterRegistry.cpp
+ exteroception/converters/ArmarDEConverter.cpp
+ exteroception/Segment.cpp
+
description/Segment.cpp
)
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..63f24d0f46a156adc023b3b64b4f27d344b151c6
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.cpp
@@ -0,0 +1,50 @@
+#include "Segment.h"
+
+#include <filesystem>
+
+#include <SimoxUtility/algorithm/string/string_tools.h>
+
+#include <ArmarXCore/core/application/properties/PluginAll.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
+
+#include <RobotAPI/libraries/armem/client/Writer.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/query/query_fns.h>
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
+#include <RobotAPI/libraries/armem_robot/types.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Exteroception.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/memory_ids.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+
+
+namespace armarx::armem::server::robot_state::exteroception
+{
+
+ Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
+ Base(memoryToIceAdapter,
+ armem::robot_state::exteroceptionSegmentID.coreSegmentName,
+ arondto::Exteroception::ToAronType())
+ {
+ }
+
+ Segment::~Segment() = default;
+
+
+ // void
+ // Segment::onConnect(const RobotUnitInterfacePrx& robotUnitPrx)
+ // {
+ // robotUnit = robotUnitPrx;
+
+ // // store the robot description linked to the robot unit in this segment
+ // }
+
+
+} // namespace armarx::armem::server::robot_state::exteroception
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.h b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.h
new file mode 100644
index 0000000000000000000000000000000000000000..e8471563d640800a44d4516d6eccdd7d7e355ccd
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.h
@@ -0,0 +1,55 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2023
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+#include <optional>
+#include <string>
+#include <unordered_map>
+
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
+#include <RobotAPI/libraries/armem/server/segment/SpecializedCoreSegment.h>
+#include <RobotAPI/libraries/armem/server/segment/SpecializedProviderSegment.h>
+#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/server/forward_declarations.h>
+
+
+namespace armarx::armem::server::robot_state::exteroception
+{
+
+ class Segment : public segment::SpecializedCoreSegment
+ {
+ using Base = segment::SpecializedCoreSegment;
+
+ public:
+ Segment(server::MemoryToIceAdapter& iceMemory);
+ ~Segment() override;
+
+
+ // void onConnect(const RobotUnitInterfacePrx& robotUnitPrx);
+
+
+ private:
+ // RobotUnitInterfacePrx robotUnit;
+ };
+
+} // namespace armarx::armem::server::robot_state::exteroception
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ArmarDEConverter.cpp b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ArmarDEConverter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d71bb045b32028df15bb37150892175e1ec8ec72
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ArmarDEConverter.cpp
@@ -0,0 +1,130 @@
+#include "ArmarDEConverter.h"
+#include <cmath>
+#include <string>
+
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <SimoxUtility/algorithm/advanced.h>
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+
+#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Exteroception.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+
+#include <RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.h>
+
+#include "ConverterTools.h"
+
+
+namespace armarx::armem::server::robot_state::exteroception
+{
+
+ ArmarDEConverter::ArmarDEConverter() = default;
+ ArmarDEConverter::~ArmarDEConverter() = default;
+
+
+ aron::data::DictPtr
+ ArmarDEConverter::convert(
+ const RobotUnitDataStreaming::TimeStep& data,
+ const RobotUnitDataStreaming::DataStreamingDescription& description)
+ {
+ arondto::Exteroception dto;
+ dto.iterationID = data.iterationId;
+
+ for (const auto& [dataEntryName, dataEntry] : description.entries)
+ {
+ process(dto, dataEntryName, {data, dataEntry});
+ }
+
+ // resize to square
+ for(auto& [_, tof] : dto.tof)
+ {
+ const int sr = std::sqrt(tof.array.size());
+ const bool isSquare = (sr * sr == tof.array.size());
+
+ if(tof.array.size() > 0 and isSquare)
+ {
+ tof.array.resize(sr, sr);
+ }
+ }
+
+
+ return dto.toAron();
+ }
+
+
+ void ArmarDEConverter::process(
+ arondto::Exteroception& dto,
+ const std::string& entryName,
+ const ConverterValue& value)
+ {
+ const std::vector<std::string> split = simox::alg::split(entryName, ".", false, false);
+ ARMARX_CHECK_GREATER_EQUAL(split.size(), 3);
+ const std::set<size_t> acceptedSizes{3, 4, 5};
+ ARMARX_CHECK_GREATER(acceptedSizes.count(split.size()), 0)
+ << "Data entry name could not be parsed (exected 3 or 4 or 5 components between '.'): "
+ << "\n- split: '" << split << "'";
+
+ const std::string& category = split.at(0);
+ // const std::string& name = split.at(1);
+ const std::string& field = split.at(2);
+ ARMARX_CHECK_EQUAL(category, "sens") << category << " | " << entryName;
+
+ if (simox::alg::contains(field, "tofDepth"))
+ {
+ // ARMARX_DEBUG << "Processing ToF sensor data";
+ processToFEntry(dto.tof, split, value);
+ }
+
+ }
+
+
+
+
+ void ArmarDEConverter::processToFEntry(
+ std::map<std::string, armarx::armem::exteroception::arondto::ToF>& tofs,
+ const std::vector<std::string>& split,
+ const ConverterValue& value)
+ {
+ // split e.g. "sens.LeftHand.tofDepth.element_15" (split at dot)
+
+ ARMARX_CHECK_EQUAL(split.size(), 4);
+ ARMARX_CHECK_EQUAL(split.at(2), "tofDepth");
+
+ const std::string& name = split.at(1);
+
+ // element 0 sens
+ // element 1 is either LeftHand or RightHand
+
+ const std::map<std::string, std::string> sidePrefixMap
+ {
+ {"LeftHand", "Left"},
+ {"RightHand", "Right"}
+ };
+
+ auto it = sidePrefixMap.find(name);
+ ARMARX_CHECK(it != sidePrefixMap.end()) << name;
+
+ const std::string& side = it->second;
+ processToFEntry(tofs[side], split, value);
+ }
+
+ void ArmarDEConverter::processToFEntry(armarx::armem::exteroception::arondto::ToF& tof,
+ const std::vector<std::string>& split,
+ const ConverterValue& value)
+ {
+ // split, e.g., element_12
+ const std::vector<std::string> elements = simox::alg::split(split.back(), "_");
+ ARMARX_CHECK_EQUAL(elements.size(), 2);
+
+ const int idx = std::stoi(elements.at(1));
+
+ if(tof.array.size() < (idx +1))
+ {
+ tof.array.resize(idx+1, 1);
+ }
+
+ tof.array(idx) = getValueAs<float>(value);
+ }
+
+
+}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ArmarDEConverter.h b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ArmarDEConverter.h
new file mode 100644
index 0000000000000000000000000000000000000000..bf6c482a06bfd4be24fca5ad78a320b5b6834042
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ArmarDEConverter.h
@@ -0,0 +1,51 @@
+#pragma once
+
+#include <map>
+#include <string>
+
+#include <Eigen/Core>
+
+#include <RobotAPI/libraries/armem_robot_state/aron/Exteroception.aron.generated.h>
+
+#include "ConverterInterface.h"
+
+
+namespace armarx::armem::server::robot_state::exteroception
+{
+ struct ConverterValue;
+ // class ConverterTools;
+
+
+ class ArmarDEConverter : public ConverterInterface
+ {
+ public:
+ ArmarDEConverter();
+ ~ArmarDEConverter() override;
+
+
+ aron::data::DictPtr
+ convert(const RobotUnitDataStreaming::TimeStep& data,
+ const RobotUnitDataStreaming::DataStreamingDescription& description) override;
+
+
+ protected:
+ void process(arondto::Exteroception& dtoExteroception,
+ const std::string& entryName,
+ const ConverterValue& value);
+
+
+ private:
+
+
+ void processToFEntry(std::map<std::string, armarx::armem::exteroception::arondto::ToF>& fts,
+ const std::vector<std::string>& split,
+ const ConverterValue& value);
+
+ void processToFEntry(armarx::armem::exteroception::arondto::ToF& tof,
+ const std::vector<std::string>& split,
+ const ConverterValue& value);
+
+
+ // std::unique_ptr<ConverterTools> tools;
+ };
+} // namespace armarx::armem::server::robot_state::exteroception
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterInterface.cpp b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterInterface.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4ba33e6c5f308cd6213c8ccf02018ad63ba9440b
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterInterface.cpp
@@ -0,0 +1,11 @@
+#include "ConverterInterface.h"
+
+
+namespace armarx::armem::server::robot_state::exteroception
+{
+
+ ConverterInterface::~ConverterInterface()
+ {
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterInterface.h b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterInterface.h
new file mode 100644
index 0000000000000000000000000000000000000000..5ba0ecc3c8aeaf5c2b3f3c03d9f36ed83269fb5e
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterInterface.h
@@ -0,0 +1,32 @@
+#pragma once
+
+#include <memory>
+
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+
+namespace armarx::RobotUnitDataStreaming
+{
+ struct TimeStep;
+ struct DataStreamingDescription;
+ struct DataEntry;
+}
+namespace armarx::armem::arondto
+{
+ class Proprioception;
+}
+
+namespace armarx::armem::server::robot_state::exteroception
+{
+ class ConverterInterface
+ {
+ public:
+
+ virtual ~ConverterInterface();
+
+ virtual
+ aron::data::DictPtr convert(
+ const RobotUnitDataStreaming::TimeStep& data,
+ const RobotUnitDataStreaming::DataStreamingDescription& description) = 0;
+
+ };
+}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterRegistry.cpp b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterRegistry.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ef7296b669504e0f8d0aff8b4cd3991fe6ec10b3
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterRegistry.cpp
@@ -0,0 +1,31 @@
+#include "ConverterRegistry.h"
+
+#include "ArmarDEConverter.h"
+
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+
+
+namespace armarx::armem::server::robot_state::exteroception
+{
+
+ ConverterRegistry::ConverterRegistry()
+ {
+ add<ArmarDEConverter>("ArmarDE");
+ // add<Armar7Converter>("Armar7");
+ }
+
+
+ ConverterInterface*
+ ConverterRegistry::get(const std::string& key) const
+ {
+ auto it = converters.find(key);
+ return it != converters.end() ? it->second.get() : nullptr;
+ }
+
+
+ std::vector<std::string> ConverterRegistry::getKeys() const
+ {
+ return simox::alg::get_keys(converters);
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterRegistry.h b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterRegistry.h
new file mode 100644
index 0000000000000000000000000000000000000000..1e1cac9a18eb3d901b0ca0f61eccf08395a279b5
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterRegistry.h
@@ -0,0 +1,36 @@
+#pragma once
+
+#include <map>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "ConverterInterface.h"
+
+
+namespace armarx::armem::server::robot_state::exteroception
+{
+ class ConverterRegistry
+ {
+ public:
+
+ ConverterRegistry();
+
+
+ template <class ConverterT, class ...Args>
+ void add(const std::string& key, Args... args)
+ {
+ converters[key].reset(new ConverterT(args...));
+ }
+
+
+ ConverterInterface* get(const std::string& key) const;
+ std::vector<std::string> getKeys() const;
+
+
+ private:
+
+ std::map<std::string, std::unique_ptr<ConverterInterface>> converters;
+
+ };
+}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterTools.cpp b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterTools.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6086a9aee5f2adb8d997a65302084e0c902d747c
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterTools.cpp
@@ -0,0 +1,179 @@
+#include "ConverterTools.h"
+
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <SimoxUtility/algorithm/advanced.h>
+
+#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+
+
+namespace armarx::armem::server::robot_state
+{
+
+ std::optional<std::string>
+ exteroception::findByPrefix(const std::string& key, const std::set<std::string>& prefixes)
+ {
+ for (const auto& prefix : prefixes)
+ {
+ if (simox::alg::starts_with(key, prefix))
+ {
+ return prefix;
+ }
+ }
+ return std::nullopt;
+ }
+}
+
+
+namespace armarx::armem::server::robot_state::exteroception
+{
+ ConverterTools::ConverterTools()
+ {
+ {
+ vector3fSetters["X"] = [](Eigen::Vector3f & v, float f)
+ {
+ v.x() = f;
+ };
+ vector3fSetters["Y"] = [](Eigen::Vector3f & v, float f)
+ {
+ v.y() = f;
+ };
+ vector3fSetters["Z"] = [](Eigen::Vector3f & v, float f)
+ {
+ v.z() = f;
+ };
+ vector3fSetters["x"] = vector3fSetters["X"];
+ vector3fSetters["y"] = vector3fSetters["Y"];
+ vector3fSetters["z"] = vector3fSetters["Z"];
+ vector3fSetters["Rotation"] = vector3fSetters["Z"];
+ }
+ {
+ platformPoseGetters["acceleration"] = [](prop::arondto::Platform & p)
+ {
+ return &p.acceleration;
+ };
+ platformPoseGetters["relativePosition"] = [](prop::arondto::Platform & p)
+ {
+ return &p.relativePosition;
+ };
+ platformPoseGetters["velocity"] = [](prop::arondto::Platform & p)
+ {
+ return &p.velocity;
+ };
+ platformIgnored.insert("absolutePosition");
+ }
+ {
+ ftGetters["gravCompF"] = [](prop::arondto::ForceTorque & ft)
+ {
+ return &ft.gravityCompensationForce;
+ };
+ ftGetters["gravCompT"] = [](prop::arondto::ForceTorque & ft)
+ {
+ return &ft.gravityCompensationTorque;
+ };
+ ftGetters["f"] = [](prop::arondto::ForceTorque & ft)
+ {
+ return &ft.force;
+ };
+ ftGetters["t"] = [](prop::arondto::ForceTorque & ft)
+ {
+ return &ft.torque;
+ };
+ }
+ {
+ jointGetters["acceleration"] = [](prop::arondto::Joints & j)
+ {
+ return &j.acceleration;
+ };
+ jointGetters["gravityTorque"] = [](prop::arondto::Joints & j)
+ {
+ return &j.gravityTorque;
+ };
+ jointGetters["inertiaTorque"] = [](prop::arondto::Joints & j)
+ {
+ return &j.inertiaTorque;
+ };
+ jointGetters["inverseDynamicsTorque"] = [](prop::arondto::Joints & j)
+ {
+ return &j.inverseDynamicsTorque;
+ };
+ jointGetters["position"] = [](prop::arondto::Joints & j)
+ {
+ return &j.position;
+ };
+ jointGetters["torque"] = [](prop::arondto::Joints & j)
+ {
+ return &j.torque;
+ };
+ jointGetters["velocity"] = [](prop::arondto::Joints & j)
+ {
+ return &j.velocity;
+ };
+ }
+ {
+
+#define ADD_SCALAR_SETTER(container, name, type) \
+ container [ #name ] = []( \
+ prop::arondto::Joints & dto, \
+ const std::vector<std::string>& split, \
+ const ConverterValue & value) \
+ { \
+ dto. name [split.at(1)] = getValueAs< type >(value); \
+ }
+
+ ADD_SCALAR_SETTER(jointSetters, position, float);
+ ADD_SCALAR_SETTER(jointSetters, velocity, float);
+ ADD_SCALAR_SETTER(jointSetters, acceleration, float);
+
+ ADD_SCALAR_SETTER(jointSetters, relativePosition, float);
+ ADD_SCALAR_SETTER(jointSetters, filteredVelocity, float);
+
+ ADD_SCALAR_SETTER(jointSetters, currentTarget, float);
+ ADD_SCALAR_SETTER(jointSetters, positionTarget, float);
+ ADD_SCALAR_SETTER(jointSetters, velocityTarget, float);
+
+ ADD_SCALAR_SETTER(jointSetters, torque, float);
+ ADD_SCALAR_SETTER(jointSetters, inertiaTorque, float);
+ ADD_SCALAR_SETTER(jointSetters, gravityTorque, float);
+ ADD_SCALAR_SETTER(jointSetters, gravityCompensatedTorque, float);
+ ADD_SCALAR_SETTER(jointSetters, inverseDynamicsTorque, float);
+ ADD_SCALAR_SETTER(jointSetters, torqueTicks, int);
+
+ ADD_SCALAR_SETTER(jointSetters, positionTarget, float);
+ ADD_SCALAR_SETTER(jointSetters, currentTarget, float);
+ ADD_SCALAR_SETTER(jointSetters, positionTarget, float);
+
+ // "temperature" handled below
+ // ADD_SCALAR_SETTER(jointSetters, temperature, float);
+
+ ADD_SCALAR_SETTER(jointSetters, motorCurrent, float);
+ ADD_SCALAR_SETTER(jointSetters, maxTargetCurrent, float);
+
+ ADD_SCALAR_SETTER(jointSetters, sensorBoardUpdateRate, float);
+ ADD_SCALAR_SETTER(jointSetters, I2CUpdateRate, float);
+
+ ADD_SCALAR_SETTER(jointSetters, JointStatusEmergencyStop, bool);
+ ADD_SCALAR_SETTER(jointSetters, JointStatusEnabled, bool);
+ ADD_SCALAR_SETTER(jointSetters, JointStatusError, int);
+ ADD_SCALAR_SETTER(jointSetters, JointStatusOperation, int);
+
+
+#define ADD_VECTOR3_SETTER(container, name, type) \
+ container [ #name ] = [this]( \
+ prop::arondto::Joints & dto, \
+ const std::vector<std::string>& split, \
+ const ConverterValue & value) \
+ { \
+ auto& vec = dto. name [split.at(1)]; \
+ auto& setter = this->vector3fSetters.at(split.at(3)); \
+ setter(vec, getValueAs< type >(value)); \
+ }
+
+ ADD_VECTOR3_SETTER(jointSetters, angularVelocity, float);
+ ADD_VECTOR3_SETTER(jointSetters, linearAcceleration, float);
+
+ // ADD_GETTER(jointVectorGetters, orientation, float);
+ }
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterTools.h b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterTools.h
new file mode 100644
index 0000000000000000000000000000000000000000..7460ebb133a01387c0f29a472961366877254506
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterTools.h
@@ -0,0 +1,106 @@
+#pragma once
+
+#include <map>
+#include <set>
+#include <string>
+
+#include <Eigen/Core>
+
+#include <SimoxUtility/algorithm/string/string_tools.h>
+
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
+
+#include "ConverterInterface.h"
+
+
+namespace armarx::armem::server::robot_state::exteroception
+{
+
+ struct ConverterValue
+ {
+ const RobotUnitDataStreaming::TimeStep& data;
+ const RobotUnitDataStreaming::DataEntry& entry;
+ };
+
+
+ template <class T>
+ T
+ getValueAs(const ConverterValue& value)
+ {
+ return RobotUnitDataStreamingReceiver::GetAs<T>(value.data, value.entry);
+ }
+
+
+ /**
+ * @brief Search
+ * @param key
+ * @param prefixes
+ * @return
+ */
+ std::optional<std::string>
+ findByPrefix(const std::string& key, const std::set<std::string>& prefixes);
+
+
+ template <class ValueT>
+ ValueT
+ findByPrefix(const std::string& key, const std::map<std::string, ValueT>& map)
+ {
+ for (const auto& [prefix, value] : map)
+ {
+ if (simox::alg::starts_with(key, prefix))
+ {
+ return value;
+ }
+ }
+ return nullptr;
+ }
+
+
+ template <class ValueT>
+ ValueT
+ findBySuffix(const std::string& key, const std::map<std::string, ValueT>& map)
+ {
+ for (const auto& [suffix, value] : map)
+ {
+ if (simox::alg::ends_with(key, suffix))
+ {
+ return value;
+ }
+ }
+ return nullptr;
+ }
+
+
+
+ class ConverterTools
+ {
+ public:
+
+ ConverterTools();
+
+
+ public:
+
+ std::map<std::string, std::function<void(Eigen::Vector3f&, float)> > vector3fSetters;
+
+ std::map<std::string, std::function<std::map<std::string, float>*(prop::arondto::Joints&)> > jointGetters;
+ std::map<std::string, std::function<std::map<std::string, Eigen::Vector3f>*(prop::arondto::Joints&)> > jointVectorGetters;
+
+ using JointSetter = std::function<void(prop::arondto::Joints& dto, const std::vector<std::string>& split, const ConverterValue& value)>;
+ std::map<std::string, JointSetter> jointSetters;
+
+ std::map<std::string, std::function<Eigen::Vector3f*(prop::arondto::Platform&)> > platformPoseGetters;
+ std::set<std::string> platformIgnored;
+
+ std::map<std::string, std::function<Eigen::Vector3f*(prop::arondto::ForceTorque&)> > ftGetters;
+
+
+ std::map<std::string, std::string> sidePrefixMap
+ {
+ { "R", "Right" },
+ { "L", "Left" },
+ };
+
+ };
+}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
index 87df7cb013d99543485fdc8868aa83254bf52ffe..847ec98c82966408545a50bf9d7fa93fc7081d9f 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
@@ -29,142 +29,168 @@
#include <SimoxUtility/math/convert/rpy_to_mat3f.h>
// ArmarX
+#include "ArmarXCore/core/logging/Logging.h"
#include "ArmarXCore/core/time/Metronome.h"
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <ArmarXCore/core/time/CycleUtil.h>
#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
#include "RobotAPI/libraries/armem_robot_state/client/common/constants.h"
-#include <RobotAPI/libraries/core/FramedPose.h>
-#include <RobotAPI/libraries/aron/core/data/variant/All.h>
-#include <RobotAPI/libraries/armem/core/Time.h>
-#include <RobotAPI/libraries/armem/core/ice_conversions.h>
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
-#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/server/localization/Segment.h>
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
namespace armarx::armem::server::robot_state::proprioception
{
- void RobotStateWriter::connect(armarx::plugins::DebugObserverComponentPlugin& debugObserver)
+ void
+ RobotStateWriter::connect(armarx::plugins::DebugObserverComponentPlugin& debugObserverPlugin)
{
// Thread-local copy of debug observer helper.
this->debugObserver =
- DebugObserverHelper(Logging::tag.tagName, debugObserver.getDebugObserver(), true);
+ DebugObserverHelper(Logging::tag.tagName, debugObserverPlugin.getDebugObserver(), true);
}
- static float toDurationMs(
- std::chrono::high_resolution_clock::time_point start,
- std::chrono::high_resolution_clock::time_point end)
+ static float
+ toDurationMs(std::chrono::high_resolution_clock::time_point start,
+ std::chrono::high_resolution_clock::time_point end)
{
auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
return duration.count() / 1000.f;
}
- void RobotStateWriter::run(
- float pollFrequency,
- TripleBuffer<std::vector<RobotUnitData>>& dataBuffer,
- MemoryToIceAdapter& memory,
- localization::Segment& localizationSegment)
+ void
+ RobotStateWriter::run(float pollFrequency,
+ Queue& dataBuffer,
+ MemoryToIceAdapter& memory,
+ localization::Segment& localizationSegment)
{
- Metronome metronome(Frequency::HertzDouble(pollFrequency));
-
while (task and not task->isStopped())
{
- const std::vector<RobotUnitData>& batch = dataBuffer.getUpToDateReadBuffer();
- if (debugObserver)
- {
- debugObserver->setDebugObserverDatafield("RobotStateWriter | Queue Size", batch.size());
- }
+ // if (debugObserver)
+ // {
+ // debugObserver->setDebugObserverDatafield("RobotStateWriter | Queue Size", dataBuffer.size());
+ // }
- if (not batch.empty())
+ RobotUnitData robotUnitData;
+ if (const auto status = dataBuffer.wait_pull(robotUnitData);
+ status == boost::queue_op_status::success)
{
auto start = std::chrono::high_resolution_clock::now();
auto endBuildUpdate = start, endProprioception = start, endLocalization = start;
- Update update = buildUpdate(batch);
+ Update update = buildUpdate(robotUnitData);
endBuildUpdate = std::chrono::high_resolution_clock::now();
// Commits lock the core segments.
- // Proprioception
- armem::CommitResult result = memory.commitLocking(update.proprioception);
+ // Proprioception + Exteroception
+ armem::CommitResult resultProprioception = memory.commitLocking(update.proprioception);
+
+ ARMARX_DEBUG << deactivateSpam(1) << VAROUT(update.exteroception.updates.size());
+ armem::CommitResult resultExteroception = memory.commitLocking(update.exteroception);
endProprioception = std::chrono::high_resolution_clock::now();
// Localization
for (const armem::robot_state::Transform& transform : update.localization)
{
- localizationSegment.doLocked([&localizationSegment, &transform]()
- {
- localizationSegment.commitTransform(transform);
- });
+ localizationSegment.doLocked(
+ [&localizationSegment, &transform]()
+ { localizationSegment.commitTransform(transform); });
}
endLocalization = std::chrono::high_resolution_clock::now();
- if (not result.allSuccess())
+ if (not resultProprioception.allSuccess())
+ {
+ ARMARX_WARNING << "Could not commit data to proprioception segment in memory. Error message: "
+ << resultProprioception.allErrorMessages();
+ }
+
+ if (not resultExteroception.allSuccess())
{
- ARMARX_WARNING << "Could not commit data to memory. Error message: " << result.allErrorMessages();
+ ARMARX_WARNING << "Could not commit data to exteroception segment in memory. Error message: "
+ << resultExteroception.allErrorMessages();
}
+
if (debugObserver)
{
auto end = std::chrono::high_resolution_clock::now();
- debugObserver->setDebugObserverDatafield("RobotStateWriter | t Commit (ms)", toDurationMs(start, end));
- debugObserver->setDebugObserverDatafield("RobotStateWriter | t Commit 1. Build Update (ms)", toDurationMs(start, endBuildUpdate));
- debugObserver->setDebugObserverDatafield("RobotStateWriter | t Commit 2. Proprioception (ms)", toDurationMs(endBuildUpdate, endProprioception));
- debugObserver->setDebugObserverDatafield("RobotStateWriter | t Commit 3. Localization (ms)", toDurationMs(endProprioception, endLocalization));
+ debugObserver->setDebugObserverDatafield("RobotStateWriter | t Commit (ms)",
+ toDurationMs(start, end));
+ debugObserver->setDebugObserverDatafield(
+ "RobotStateWriter | t Commit 1. Build Update (ms)",
+ toDurationMs(start, endBuildUpdate));
+ debugObserver->setDebugObserverDatafield(
+ "RobotStateWriter | t Commit 2. Proprioception (ms)",
+ toDurationMs(endBuildUpdate, endProprioception));
+ debugObserver->setDebugObserverDatafield(
+ "RobotStateWriter | t Commit 3. Localization (ms)",
+ toDurationMs(endProprioception, endLocalization));
}
}
+ else
+ {
+ ARMARX_WARNING << "Queue pull was not successful. "
+ << static_cast<std::underlying_type<boost::queue_op_status>::type>(
+ status);
+ }
if (debugObserver)
{
debugObserver->sendDebugObserverBatch();
}
- metronome.waitForNextTick();
}
}
- RobotStateWriter::Update RobotStateWriter::buildUpdate(const std::vector<RobotUnitData>& dataQueue)
+ RobotStateWriter::Update
+ RobotStateWriter::buildUpdate(const RobotUnitData& data)
{
- ARMARX_CHECK_NOT_EMPTY(dataQueue);
- ARMARX_DEBUG << "RobotStateWriter: Commit batch of " << dataQueue.size() << " timesteps to memory...";
-
// Send batch to memory
Update update;
- for (const RobotUnitData& data: dataQueue)
- {
- {
- armem::EntityUpdate& up = update.proprioception.add();
- up.entityID = properties.robotUnitProviderID.withEntityName(properties.robotUnitProviderID.providerSegmentName);
- up.timeCreated = data.timestamp;
- up.instancesData = { data.proprioception };
- }
+ if(data.proprioception){
+ armem::EntityUpdate& up = update.proprioception.add();
+ up.entityID = properties.robotUnitProviderID.withEntityName(
+ properties.robotUnitProviderID.providerSegmentName);
+ up.entityID.coreSegmentName =::armarx::armem::robot_state::constants::proprioceptionCoreSegment;
+ up.timeCreated = data.timestamp;
+ up.instancesData = {data.proprioception};
+ }
- // Extract odometry data.
- const std::string platformKey = "platform";
- if (data.proprioception->hasElement(platformKey))
- {
- ARMARX_DEBUG << "Found odometry data.";
- auto platformData = aron::data::Dict::DynamicCastAndCheck(data.proprioception->getElement(platformKey));
- update.localization.emplace_back(getTransform(platformData, data.timestamp));
- }
- else
- {
- ARMARX_INFO << "No odometry data! "
- << "(No element '" << platformKey << "' in proprioception data.)"
- << "\nIf you are using a mobile platform this should not have happened."
- << "\nThis error is only logged once."
- << "\nThese keys exist: " << data.proprioception->getAllKeys()
- ;
- noOdometryDataLogged = true;
- }
+ // Exteroception
+ if(data.exteroception){
+ armem::EntityUpdate& up = update.exteroception.add();
+ up.entityID = properties.robotUnitProviderID.withEntityName(
+ properties.robotUnitProviderID.providerSegmentName);
+ up.entityID.coreSegmentName = ::armarx::armem::robot_state::constants::exteroceptionCoreSegment;
+ up.timeCreated = data.timestamp;
+ up.instancesData = {data.exteroception};
+ }
+
+ // Extract odometry data.
+ ARMARX_CHECK_NOT_NULL(data.proprioception);
+ const std::string platformKey = "platform";
+ if (data.proprioception->hasElement(platformKey))
+ {
+ ARMARX_DEBUG << "Found odometry data.";
+ auto platformData =
+ aron::data::Dict::DynamicCastAndCheck(data.proprioception->getElement(platformKey));
+ update.localization.emplace_back(getTransform(platformData, data.timestamp));
+ }
+ else
+ {
+ ARMARX_INFO << "No odometry data! "
+ << "(No element '" << platformKey << "' in proprioception data.)"
+ << "\nIf you are using a mobile platform this should not have happened."
+ << "\nThis error is only logged once."
+ << "\nThese keys exist: " << data.proprioception->getAllKeys();
+ noOdometryDataLogged = true;
}
return update;
@@ -172,9 +198,8 @@ namespace armarx::armem::server::robot_state::proprioception
armem::robot_state::Transform
- RobotStateWriter::getTransform(
- const aron::data::DictPtr& platformData,
- const Time& timestamp) const
+ RobotStateWriter::getTransform(const aron::data::DictPtr& platformData,
+ const Time& timestamp) const
{
prop::arondto::Platform platform;
platform.fromAron(platformData);
@@ -182,7 +207,7 @@ namespace armarx::armem::server::robot_state::proprioception
const Eigen::Vector3f& relPose = platform.relativePosition;
Eigen::Affine3f odometryPose = Eigen::Affine3f::Identity();
- odometryPose.translation() << relPose.x(), relPose.y(), 0; // TODO set height
+ odometryPose.translation() << relPose.x(), relPose.y(), 0; // TODO set height
odometryPose.linear() = simox::math::rpy_to_mat3f(0.f, 0.f, relPose.z());
armem::robot_state::Transform transform;
@@ -195,4 +220,4 @@ namespace armarx::armem::server::robot_state::proprioception
return transform;
}
-}
+} // namespace armarx::armem::server::robot_state::proprioception
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h
index d70566ebf5c3aa6808d0e14af9c1af0471ac31c7..78863697a61ecd20c4947188a04965d5990c6f7b 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h
@@ -55,11 +55,14 @@ namespace armarx::armem::server::robot_state::proprioception
{
public:
- void connect(armarx::plugins::DebugObserverComponentPlugin& debugObserver);
+ void connect(armarx::plugins::DebugObserverComponentPlugin& debugObserverPlugin);
+
+ using Queue = armarx::armem::server::robot_state::proprioception::Queue;
+
/// Reads data from `dataQueue` and commits to the memory.
void run(float pollFrequency,
- TripleBuffer<std::vector<RobotUnitData>>& dataBuffer,
+ Queue& dataBuffer,
MemoryToIceAdapter& memory,
localization::Segment& localizationSegment
);
@@ -68,9 +71,10 @@ namespace armarx::armem::server::robot_state::proprioception
struct Update
{
armem::Commit proprioception;
+ armem::Commit exteroception;
std::vector<armem::robot_state::Transform> localization;
};
- Update buildUpdate(const std::vector<RobotUnitData>& dataQueue);
+ Update buildUpdate(const RobotUnitData& dataQueue);
private:
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.h
index c0029b4fbccece40156be49f05ca1efa4215850c..8d904f249029cf9fd5ce010657cb4324f3ba5551 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.h
@@ -2,8 +2,10 @@
#include <memory>
-#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+#include <boost/thread/concurrent_queues/sync_queue.hpp>
+
#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
namespace armarx::armem::server::robot_state::proprioception
{
@@ -12,6 +14,8 @@ namespace armarx::armem::server::robot_state::proprioception
{
Time timestamp;
aron::data::DictPtr proprioception;
+ aron::data::DictPtr exteroception;
};
-}
+ using Queue = boost::sync_queue<RobotUnitData>;
+} // namespace armarx::armem::server::robot_state::proprioception
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp
index 5794287c7231e07f9ffa0db967090f6d415c8f29..53021dc7d88f9362a967f395b1381d07934f271e 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp
@@ -1,27 +1,15 @@
#include "RobotUnitReader.h"
-#include <RobotAPI/libraries/armem_robot_state/server/proprioception/converters/Armar6Converter.h>
-#include <RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.h>
-#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
-
-#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
-#include <RobotAPI/libraries/aron/core/data/variant/primitive/Long.h>
-#include <RobotAPI/libraries/aron/core/data/variant/primitive/String.h>
+#include <filesystem>
+#include <istream>
-#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
#include "ArmarXCore/core/time/Frequency.h"
#include "ArmarXCore/core/time/Metronome.h"
-#include "ArmarXCore/core/time/forward_declarations.h"
-#include "ArmarXCore/util/CPPUtility/TripleBuffer.h"
-#include <ArmarXCore/core/time/CycleUtil.h>
#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
-#include <SimoxUtility/algorithm/string/string_tools.h>
-
-#include <istream>
-#include <filesystem>
-#include <fstream>
+#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
namespace armarx::armem::server::robot_state::proprioception
@@ -30,32 +18,39 @@ namespace armarx::armem::server::robot_state::proprioception
RobotUnitReader::RobotUnitReader() = default;
- void RobotUnitReader::connect(
- armarx::plugins::RobotUnitComponentPlugin& robotUnitPlugin,
- armarx::plugins::DebugObserverComponentPlugin& debugObserverPlugin,
- const std::string& robotTypeName)
+ void
+ RobotUnitReader::connect(armarx::plugins::RobotUnitComponentPlugin& robotUnitPlugin,
+ armarx::plugins::DebugObserverComponentPlugin& debugObserverPlugin,
+ const std::string& robotTypeName)
{
{
- converter = converterRegistry.get(robotTypeName);
- ARMARX_CHECK_NOT_NULL(converter)
- << "No converter for robot type '" << robotTypeName << "' available. \n"
- << "Known are: " << converterRegistry.getKeys();
+ converterProprioception = proprioceptionConverterRegistry.get(robotTypeName);
+ ARMARX_CHECK_NOT_NULL(converterProprioception)
+ << "No converter for robot type '" << robotTypeName << "' available. \n"
+ << "Known are: " << proprioceptionConverterRegistry.getKeys();
config.loggingNames.push_back(properties.sensorPrefix);
receiver = robotUnitPlugin.startDataStreaming(config);
description = receiver->getDataDescription();
}
+
+ {
+ converterExteroception = exteroceptionConverterRegistry.get(robotTypeName);
+ ARMARX_CHECK_NOT_NULL(converterProprioception)
+ << "No converter for robot type '" << robotTypeName << "' available. \n"
+ << "Known are: " << exteroceptionConverterRegistry.getKeys();
+ }
+
{
// Thread-local copy of debug observer helper.
- debugObserver =
- DebugObserverHelper(Logging::tag.tagName, debugObserverPlugin.getDebugObserver(), true);
+ debugObserver = DebugObserverHelper(
+ Logging::tag.tagName, debugObserverPlugin.getDebugObserver(), true);
}
}
- void RobotUnitReader::run(
- float pollFrequency,
- TripleBuffer<std::vector<RobotUnitData>>& dataBuffer)
+ void
+ RobotUnitReader::run(float pollFrequency, Queue& dataBuffer)
{
Metronome metronome(Frequency::HertzDouble(pollFrequency));
@@ -63,8 +58,8 @@ namespace armarx::armem::server::robot_state::proprioception
{
if (std::optional<RobotUnitData> commit = fetchAndConvertLatestRobotUnitData())
{
- dataBuffer.getWriteBuffer().push_back(std::move(commit.value()));
- dataBuffer.commitWrite();
+ // will lock a mutex
+ dataBuffer.push(std::move(commit.value()));
}
if (debugObserver)
@@ -77,9 +72,10 @@ namespace armarx::armem::server::robot_state::proprioception
}
- std::optional<RobotUnitData> RobotUnitReader::fetchAndConvertLatestRobotUnitData()
+ std::optional<RobotUnitData>
+ RobotUnitReader::fetchAndConvertLatestRobotUnitData()
{
- ARMARX_CHECK_NOT_NULL(converter);
+ ARMARX_CHECK_NOT_NULL(converterProprioception);
const std::optional<RobotUnitDataStreaming::TimeStep> data = fetchLatestData();
if (not data.has_value())
@@ -91,23 +87,36 @@ namespace armarx::armem::server::robot_state::proprioception
auto start = std::chrono::high_resolution_clock::now();
RobotUnitData result;
- result.proprioception = converter->convert(data.value(), description);
+
+ if(converterProprioception != nullptr)
+ {
+ result.proprioception = converterProprioception->convert(data.value(), description);
+ }
+
+ if(converterExteroception != nullptr)
+ {
+ result.exteroception = converterExteroception->convert(data.value(), description);
+ }
+
result.timestamp = Time(Duration::MicroSeconds(data->timestampUSec));
auto stop = std::chrono::high_resolution_clock::now();
auto duration = std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
- ARMARX_DEBUG << "RobotUnitReader: The total time needed to convert the data to commit: " << duration;
+ ARMARX_DEBUG << "RobotUnitReader: The total time needed to convert the data to commit: "
+ << duration;
if (debugObserver)
{
- debugObserver->setDebugObserverDatafield("RobotUnitReader | t Read+Group [ms]", duration.count() / 1000.f);
+ debugObserver->setDebugObserverDatafield("RobotUnitReader | t Read+Group [ms]",
+ duration.count() / 1000.f);
}
return result;
}
- std::optional<RobotUnitDataStreaming::TimeStep> RobotUnitReader::fetchLatestData()
+ std::optional<RobotUnitDataStreaming::TimeStep>
+ RobotUnitReader::fetchLatestData()
{
std::deque<RobotUnitDataStreaming::TimeStep>& data = receiver->getDataBuffer();
if (debugObserver)
@@ -127,4 +136,4 @@ namespace armarx::armem::server::robot_state::proprioception
}
-}
+} // namespace armarx::armem::server::robot_state::proprioception
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.h
index 5abf7378378828005795d10401a2ee7cc1ea5aa4..88313402f217eb8f46470aafd796465354f6cc68 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.h
@@ -1,12 +1,11 @@
#pragma once
-#include <queue>
#include <map>
#include <memory>
#include <optional>
+#include <queue>
#include <string>
-#include "ArmarXCore/util/CPPUtility/TripleBuffer.h"
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/services/tasks/TaskUtil.h>
#include <ArmarXCore/libraries/DebugObserverHelper/DebugObserverHelper.h>
@@ -15,51 +14,51 @@
#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h>
#include "RobotUnitData.h"
-#include "converters/ConverterRegistry.h"
#include "converters/ConverterInterface.h"
+#include "../exteroception/converters/ConverterInterface.h"
+#include "converters/ConverterRegistry.h"
+#include "../exteroception/converters/ConverterRegistry.h"
namespace armarx::plugins
{
class RobotUnitComponentPlugin;
class DebugObserverComponentPlugin;
-}
+} // namespace armarx::plugins
namespace armarx
{
using RobotUnitDataStreamingReceiverPtr = std::shared_ptr<class RobotUnitDataStreamingReceiver>;
}
+
+// TODO move this out of proprioception. it is now for proprioception and exteroception
namespace armarx::armem::server::robot_state::proprioception
{
class RobotUnitReader : public armarx::Logging
{
public:
-
RobotUnitReader();
+ using Queue = armarx::armem::server::robot_state::proprioception::Queue;
- void connect(
- armarx::plugins::RobotUnitComponentPlugin& robotUnitPlugin,
- armarx::plugins::DebugObserverComponentPlugin& debugObserverPlugin,
- const std::string& robotTypeName);
+ void connect(armarx::plugins::RobotUnitComponentPlugin& robotUnitPlugin,
+ armarx::plugins::DebugObserverComponentPlugin& debugObserverPlugin,
+ const std::string& robotTypeName);
/// Reads data from `handler` and fills `dataQueue`.
- void run(float pollFrequency,
- TripleBuffer<std::vector<RobotUnitData>>& dataBuffer);
+ void run(float pollFrequency, Queue& dataBuffer);
std::optional<RobotUnitData> fetchAndConvertLatestRobotUnitData();
private:
-
/// Fetch the latest timestep and clear the robot unit buffer.
std::optional<RobotUnitDataStreaming::TimeStep> fetchLatestData();
public:
-
struct Properties
{
std::string sensorPrefix = "sens.*";
@@ -73,11 +72,12 @@ namespace armarx::armem::server::robot_state::proprioception
RobotUnitDataStreamingReceiverPtr receiver;
RobotUnitDataStreaming::DataStreamingDescription description;
- ConverterRegistry converterRegistry;
- ConverterInterface* converter = nullptr;
+ ConverterRegistry proprioceptionConverterRegistry;
+ exteroception::ConverterRegistry exteroceptionConverterRegistry;
+ proprioception::ConverterInterface* converterProprioception = nullptr;
+ exteroception::ConverterInterface* converterExteroception = nullptr;
armarx::SimpleRunningTask<>::pointer_type task = nullptr;
-
};
-}
+} // namespace armarx::armem::server::robot_state::proprioception
diff --git a/source/RobotAPI/libraries/aron/CMakeLists.txt b/source/RobotAPI/libraries/aron/CMakeLists.txt
index ce4f7bc99cb8d923decfdd7fdb79390837e79150..e1ccc8560ff75b018fbc958d0f1febf0512d9590 100644
--- a/source/RobotAPI/libraries/aron/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/CMakeLists.txt
@@ -2,3 +2,4 @@ add_subdirectory(core)
add_subdirectory(converter)
add_subdirectory(codegeneration)
add_subdirectory(common)
+add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/Generator.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/Generator.cpp
index 0c9fab0b1fe8e71ae2c9ad3b8fcf2fb32ec03ddc..be87200f51f745b314ef7f3f7c2a67beb18d332a 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/Generator.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/Generator.cpp
@@ -208,10 +208,13 @@ namespace armarx::aron::codegenerator::cpp
{
CppMethodPtr m = CppMethodPtr(new CppMethod("virtual ~" + name + "()"));
m->setBlock(b);
+
+ m->setEnforceBlockGeneration(true);
return m;
}
CppMethodPtr m = CppMethodPtr(new CppMethod("virtual ~" + name + "() = default;"));
+ m->setEnforceBlockGeneration(false);
return m;
}
@@ -224,6 +227,8 @@ namespace armarx::aron::codegenerator::cpp
CppMethodPtr m = CppMethodPtr(new CppMethod("virtual void resetSoft() override", doc.str()));
CppBlockPtr b = this->getResetSoftBlock("");
m->setBlock(b);
+
+ m->setEnforceBlockGeneration(true);
return m;
}
@@ -236,6 +241,8 @@ namespace armarx::aron::codegenerator::cpp
CppMethodPtr m = CppMethodPtr(new CppMethod("virtual void resetHard() override", doc.str()));
CppBlockPtr b = this->getResetHardBlock("");
m->setBlock(b);
+
+ m->setEnforceBlockGeneration(true);
return m;
}
@@ -252,6 +259,8 @@ namespace armarx::aron::codegenerator::cpp
std::string dummy;
b->appendBlock(this->getWriteTypeBlock("", "", Path(), dummy));
m->setBlock(b);
+
+ m->setEnforceBlockGeneration(true);
return m;
}
@@ -273,6 +282,7 @@ namespace armarx::aron::codegenerator::cpp
b->addLineAsBlock("throw ::armarx::aron::error::AronException(__PRETTY_FUNCTION__, std::string(\"An error occured during the write method of an aron generated class. The full error log was:\\n\") + " + ARON_VARIABLE_PREFIX + "_e.what());");
m->setBlock(b);
+ m->setEnforceBlockGeneration(true);
return m;
}
@@ -296,6 +306,8 @@ namespace armarx::aron::codegenerator::cpp
b->addLine("catch(const std::exception& " + ARON_VARIABLE_PREFIX + "_e)");
b->addLineAsBlock("throw ::armarx::aron::error::AronException(__PRETTY_FUNCTION__, std::string(\"An error occured during the read method of an aron generated class. The full error log was:\\n\") + " + ARON_VARIABLE_PREFIX + "_e.what());");
m->setBlock(b);
+
+ m->setEnforceBlockGeneration(true);
return m;
}
@@ -308,6 +320,8 @@ namespace armarx::aron::codegenerator::cpp
CppMethodPtr m = CppMethodPtr(new CppMethod(info.returnType + " " + info.methodName + "() const", doc.str()));
m->addLine(info.writerClassType + " writer;");
m->addLine("return " + info.enforceConversion + "(this->write(writer))" + info.enforceMemberAccess + ";");
+
+ m->setEnforceBlockGeneration(true);
return m;
}
@@ -320,6 +334,8 @@ namespace armarx::aron::codegenerator::cpp
CppMethodPtr m = CppMethodPtr(new CppMethod("void " + info.methodName + "(const " + info.argumentType + "& input)", doc.str()));
m->addLine(info.readerClassType + " reader;");
m->addLine("this->read(reader, " + info.enforceConversion + "(input)" + info.enforceMemberAccess + ");");
+
+ m->setEnforceBlockGeneration(true);
return m;
}
@@ -333,6 +349,8 @@ namespace armarx::aron::codegenerator::cpp
m->addLine(info.returnType + " t;");
m->addLine("t.fromAron(input);");
m->addLine("return t;");
+
+ m->setEnforceBlockGeneration(true);
return m;
}
@@ -345,6 +363,8 @@ namespace armarx::aron::codegenerator::cpp
CppMethodPtr m = CppMethodPtr(new CppMethod("static " + info.returnType + " " + info.methodName + "()", doc.str()));
m->addLine(info.writerClassType + " writer;");
m->addLine("return " + info.enforceConversion + "(writeType(writer))" + info.enforceMemberAccess + ";");
+
+ m->setEnforceBlockGeneration(true);
return m;
}
@@ -359,6 +379,8 @@ namespace armarx::aron::codegenerator::cpp
CppBlockPtr b = this->getEqualsBlock("", "i");
b->addLine("return true;");
m->setBlock(b);
+
+ m->setEnforceBlockGeneration(true);
return m;
}
@@ -588,5 +610,3 @@ namespace armarx::aron::codegenerator::cpp
return block_if_data;
}
}
-
-
diff --git a/source/RobotAPI/libraries/aron/core/aron_conversions.h b/source/RobotAPI/libraries/aron/core/aron_conversions.h
index d0f88cab840be59b86bbeb22d7229afb9f9f0b48..d27d6415877d4c15f9ddf36310e656590f8efd7e 100644
--- a/source/RobotAPI/libraries/aron/core/aron_conversions.h
+++ b/source/RobotAPI/libraries/aron/core/aron_conversions.h
@@ -3,12 +3,29 @@
#include <map>
#include <memory>
#include <optional>
+#include <type_traits>
#include <vector>
#include "Path.h"
namespace armarx::aron
{
+
+namespace detail
+{
+
+ // Helper concept to avoid ambiguities
+ template<typename DtoT, typename BoT>
+ concept DtoAndBoAreSame = std::is_same<DtoT, BoT>::value;
+
+ template <class ...>
+ struct is_optional : public std::false_type {};
+
+ template <class ...Ts>
+ struct is_optional<std::optional<Ts...>> : public std::true_type {};
+
+}
+
/**
* Framework for converting ARON DTOs (Data Transfer Objects) to C++ BOs
* (Business Objects) and back.
@@ -51,12 +68,6 @@ namespace armarx::aron
* }
* @endcode
*/
-
- // Helper concept to avoid ambiguities
- template<typename DtoT, typename BoT>
- concept DtoAndBoAreSame = std::is_same<DtoT, BoT>::value;
-
-
// Same type
template <class T>
void toAron(T& dto, const T& bo)
@@ -139,6 +150,24 @@ namespace armarx::aron
}
}
+
+ // One-sided optional
+ template <class DtoT, class BoT>
+ requires (not detail::is_optional<BoT>::value)
+ void toAron(std::optional<DtoT>& dto, const BoT& bo)
+ {
+ dto = DtoT{};
+ toAron(*dto, bo);
+ }
+ template <class DtoT, class BoT>
+ requires (not detail::is_optional<DtoT>::value)
+ void fromAron(DtoT& dto, const std::optional<BoT>& bo)
+ {
+ bo = BoT{};
+ fromAron(dto, *bo);
+ }
+
+
// Flag-controlled optional
template <class DtoT, class BoT>
void toAron(DtoT& dto, bool& dtoValid, const BoT& bo, bool boValid)
@@ -154,6 +183,7 @@ namespace armarx::aron
}
}
+
template <class DtoT, class BoT>
void fromAron(const DtoT& dto, bool dtoValid, BoT& bo, bool& boValid)
{
@@ -222,7 +252,7 @@ namespace armarx::aron
// std::map
template <class DtoKeyT, class DtoValueT, class BoKeyT, class BoValueT>
- requires (!(DtoAndBoAreSame<DtoKeyT, BoKeyT> and DtoAndBoAreSame<DtoValueT, BoValueT>))
+ requires (!(detail::DtoAndBoAreSame<DtoKeyT, BoKeyT> and detail::DtoAndBoAreSame<DtoValueT, BoValueT>))
void toAron(std::map<DtoKeyT, DtoValueT>& dtoMap,
const std::map<BoKeyT, BoValueT>& boMap)
{
@@ -236,7 +266,7 @@ namespace armarx::aron
}
}
template <class DtoKeyT, class DtoValueT, class BoKeyT, class BoValueT>
- requires (!(DtoAndBoAreSame<DtoKeyT, BoKeyT> and DtoAndBoAreSame<DtoValueT, BoValueT>))
+ requires (!(detail::DtoAndBoAreSame<DtoKeyT, BoKeyT> and detail::DtoAndBoAreSame<DtoValueT, BoValueT>))
void fromAron(const std::map<DtoKeyT, DtoValueT>& dtoMap,
std::map<BoKeyT, BoValueT>& boMap)
{
@@ -252,7 +282,7 @@ namespace armarx::aron
template <class DtoKeyT, class DtoValueT, class BoKeyT, class BoValueT>
- requires (!(DtoAndBoAreSame<DtoKeyT, BoKeyT> and DtoAndBoAreSame<DtoValueT, BoValueT>))
+ requires (!(detail::DtoAndBoAreSame<DtoKeyT, BoKeyT> and detail::DtoAndBoAreSame<DtoValueT, BoValueT>))
std::map<DtoKeyT, DtoValueT> toAron(const std::map<BoKeyT, BoValueT>& boMap)
{
std::map<DtoKeyT, DtoValueT> dtoMap;
@@ -343,20 +373,20 @@ namespace armarx
// std::vector
template <class DtoT, class BoT>
- requires (!aron::DtoAndBoAreSame<DtoT, BoT>)
+ requires (!aron::detail::DtoAndBoAreSame<DtoT, BoT>)
void toAron(std::vector<DtoT>& dtos, const std::vector<BoT>& bos)
{
armarx::aron::toAron(dtos, bos);
}
template <class DtoT, class BoT>
- requires (!aron::DtoAndBoAreSame<DtoT, BoT>)
+ requires (!aron::detail::DtoAndBoAreSame<DtoT, BoT>)
void fromAron(const std::vector<DtoT>& dtos, std::vector<BoT>& bos)
{
armarx::aron::fromAron(dtos, bos);
}
template <class DtoT, class BoT>
- requires (!aron::DtoAndBoAreSame<DtoT, BoT>)
+ requires (!aron::detail::DtoAndBoAreSame<DtoT, BoT>)
std::vector<DtoT> toAron(const std::vector<BoT>& bos)
{
return armarx::aron::toAron<DtoT, BoT>(bos);
@@ -365,13 +395,13 @@ namespace armarx
// std::map
template <class DtoKeyT, class DtoValueT, class BoKeyT, class BoValueT>
- requires (!(aron::DtoAndBoAreSame<DtoKeyT, BoKeyT> and aron::DtoAndBoAreSame<DtoValueT, BoValueT>))
+ requires (!(aron::detail::DtoAndBoAreSame<DtoKeyT, BoKeyT> and aron::detail::DtoAndBoAreSame<DtoValueT, BoValueT>))
void toAron(std::map<DtoKeyT, DtoValueT>& dtoMap, const std::map<BoKeyT, BoValueT>& boMap)
{
armarx::aron::toAron(dtoMap, boMap);
}
template <class DtoKeyT, class DtoValueT, class BoKeyT, class BoValueT>
- requires (!(aron::DtoAndBoAreSame<DtoKeyT, BoKeyT> and aron::DtoAndBoAreSame<DtoValueT, BoValueT>))
+ requires (!(aron::detail::DtoAndBoAreSame<DtoKeyT, BoKeyT> and aron::detail::DtoAndBoAreSame<DtoValueT, BoValueT>))
void fromAron(const std::map<DtoKeyT, DtoValueT>& dtoMap, std::map<BoKeyT, BoValueT>& boMap)
{
armarx::aron::fromAron(dtoMap, boMap);
@@ -379,7 +409,7 @@ namespace armarx
template <class DtoKeyT, class DtoValueT, class BoKeyT, class BoValueT>
- requires (!(aron::DtoAndBoAreSame<DtoKeyT, BoKeyT> and aron::DtoAndBoAreSame<DtoValueT, BoValueT>))
+ requires (!(aron::detail::DtoAndBoAreSame<DtoKeyT, BoKeyT> and aron::detail::DtoAndBoAreSame<DtoValueT, BoValueT>))
std::map<DtoKeyT, DtoValueT> toAron(const std::map<BoKeyT, BoValueT>& boMap)
{
armarx::aron::toAron<DtoKeyT, DtoValueT, BoKeyT, BoValueT>(boMap);
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.cpp b/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.cpp
index 111068b393c86b681179544c4d60fd02953f3f36..483088aa7ffaf706271072d5e13d55f26b8e4ec4 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.cpp
@@ -142,6 +142,7 @@ namespace armarx::aron::data
{
if (t.empty())
{
+ ARMARX_TRACE;
throw error::AronException(__PRETTY_FUNCTION__, "The type cannot be empty", getPath());
}
@@ -166,6 +167,7 @@ namespace armarx::aron::data
type::VariantPtr NDArray::recalculateType() const
{
+ ARMARX_TRACE;
throw error::NotImplementedYetException(__PRETTY_FUNCTION__);
}
@@ -179,29 +181,35 @@ namespace armarx::aron::data
case type::Descriptor::MATRIX:
{
auto casted = type::Matrix::DynamicCastAndCheck(type);
+ ARMARX_TRACE;
return (aron->shape.size() == 3 && aron->shape[0] == casted->getRows() && aron->shape[1] == casted->getCols());
}
case type::Descriptor::QUATERNION:
{
auto casted = type::Quaternion::DynamicCastAndCheck(type);
+ ARMARX_TRACE;
return (aron->shape.size() == 3 && aron->shape[0] == 1 && aron->shape[1] == 4);
}
case type::Descriptor::POINTCLOUD:
{
auto casted = type::PointCloud::DynamicCastAndCheck(type);
+ ARMARX_TRACE;
return (aron->shape.size() == 3);
}
case type::Descriptor::IMAGE:
{
auto casted = type::Image::DynamicCastAndCheck(type);
+ ARMARX_TRACE;
return (aron->shape.size() == 3);
}
case type::Descriptor::NDARRAY:
{
auto casted = type::NDArray::DynamicCastAndCheck(type);
+ ARMARX_TRACE;
return (aron->shape.size() == (unsigned int) casted->getNumberDimensions());
}
default:
+ ARMARX_TRACE;
return false;
}
}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.cpp b/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.cpp
index 478c6a2c80a4984fcc4fb19ee9dba611eb384af9..9724ae6279a2b2a2140d5964d7d101f6e9fbabc8 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.cpp
@@ -25,10 +25,12 @@
#include "Dict.h"
// ArmarX
-#include <RobotAPI/libraries/aron/core/data/variant/Factory.h>
-
#include <SimoxUtility/algorithm/string/string_conversion.h>
+#include <ArmarXCore/util/CPPUtility/trace.h>
+
+#include <RobotAPI/libraries/aron/core/data/variant/Factory.h>
+
namespace armarx::aron::data
{
@@ -135,6 +137,7 @@ namespace armarx::aron::data
{
if (hasElement(key))
{
+ ARMARX_TRACE;
throw error::AronException(__PRETTY_FUNCTION__, "The key '"+key+"' already exists in a aron dict.");
}
setElement(key, data);
@@ -150,6 +153,7 @@ namespace armarx::aron::data
auto it = childrenNavigators.find(key);
if (it == childrenNavigators.end())
{
+ ARMARX_TRACE;
throw error::AronException(__PRETTY_FUNCTION__, "Could not find key '" + key + "'. But I found the following keys: [" + simox::alg::join(this->getAllKeys(), ", ") + "]", getPath());
}
return it->second;
@@ -201,8 +205,10 @@ namespace armarx::aron::data
return "armarx::aron::data::Dict";
}
+ // TODO
type::VariantPtr Dict::recalculateType() const
{
+ ARMARX_TRACE;
throw error::NotImplementedYetException(__PRETTY_FUNCTION__);
}
@@ -216,42 +222,67 @@ namespace armarx::aron::data
{
case type::Descriptor::OBJECT:
{
+ ARMARX_TRACE;
auto objectTypeNav = type::Object::DynamicCastAndCheck(type);
for (const auto& [key, nav] : childrenNavigators)
{
if (!objectTypeNav->hasMemberType(key))
{
+ ARMARX_TRACE;
return false;
}
- if (!nav || !objectTypeNav->getMemberType(key))
+
+ auto childTypeNav = objectTypeNav->getMemberType(key);
+ if (!nav)
{
- return false;
+ ARMARX_TRACE;
+ if (childTypeNav && childTypeNav->getMaybe() == type::Maybe::NONE)
+ {
+ ARMARX_TRACE;
+ return false;
+ }
+ ARMARX_TRACE;
+ return true;
}
- if (!nav->fullfillsType(objectTypeNav->getMemberType(key)))
+ if (!nav->fullfillsType(childTypeNav))
{
+ ARMARX_TRACE;
return false;
}
}
+ ARMARX_TRACE;
return true;
}
case type::Descriptor::DICT:
{
+ ARMARX_TRACE;
auto dictTypeNav = type::Dict::DynamicCastAndCheck(type);
for (const auto& [key, nav] : childrenNavigators)
{
(void) key;
- if (!nav || !dictTypeNav->getAcceptedType())
+ auto childTypeNav = dictTypeNav->getAcceptedType();
+ if (!nav)
{
- return false;
+ ARMARX_TRACE;
+ if (childTypeNav && childTypeNav->getMaybe() == type::Maybe::NONE)
+ {
+ ARMARX_TRACE;
+ return false;
+ }
+ ARMARX_TRACE;
+ return true;
}
- if (!nav->fullfillsType(dictTypeNav->getAcceptedType()))
+ if (!nav->fullfillsType(childTypeNav))
{
+ ARMARX_TRACE;
return false;
}
}
+ ARMARX_TRACE;
return true;
}
default:
+ ARMARX_TRACE;
return false;
}
}
@@ -275,11 +306,13 @@ namespace armarx::aron::data
{
if (!path.hasElement())
{
+ ARMARX_TRACE;
throw error::AronException(__PRETTY_FUNCTION__, "Could not navigate without a valid path", path);
}
std::string el = path.getFirstElement();
if (!hasElement(el))
{
+ ARMARX_TRACE;
throw error::ValueNotValidException(__PRETTY_FUNCTION__, "Could not find an element of a path.", el, path);
}
@@ -292,6 +325,7 @@ namespace armarx::aron::data
Path next = path.withDetachedFirstElement();
if (!childrenNavigators.at(el))
{
+ ARMARX_TRACE;
throw error::AronException(__PRETTY_FUNCTION__, "Could not navigate into a NULL member. Seems like the member is optional and not set.", next);
}
return childrenNavigators.at(el)->navigateAbsolute(next);
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/container/List.cpp b/source/RobotAPI/libraries/aron/core/data/variant/container/List.cpp
index 84bf1a2005b2d57f03065de58f65cf31870c8ea8..c53b51ee1b04c3e903b4ddd718b9793d75303412 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/container/List.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/container/List.cpp
@@ -26,10 +26,12 @@
// ArmarX
#include <ArmarXCore/core/exceptions/Exception.h>
+#include <ArmarXCore/util/CPPUtility/trace.h>
+
#include <RobotAPI/libraries/aron/core/data/variant/Factory.h>
#include <RobotAPI/libraries/aron/core/type/variant/container/List.h>
-#include <RobotAPI/libraries/aron/core/type/variant/container/Tuple.h>
#include <RobotAPI/libraries/aron/core/type/variant/container/Pair.h>
+#include <RobotAPI/libraries/aron/core/type/variant/container/Tuple.h>
namespace armarx::aron::data
{
@@ -125,7 +127,8 @@ namespace armarx::aron::data
{
if (i > aron->elements.size())
{
- error::AronException(__PRETTY_FUNCTION__, "Cannot set a listelement at index " + std::to_string(i) + " because this list has size " + std::to_string(aron->elements.size()));
+ ARMARX_TRACE;
+ throw error::AronException(__PRETTY_FUNCTION__, "Cannot set a listelement at index " + std::to_string(i) + " because this list has size " + std::to_string(aron->elements.size()));
}
if (i == aron->elements.size())
@@ -163,6 +166,7 @@ namespace armarx::aron::data
{
if (i >= childrenNavigators.size())
{
+ ARMARX_TRACE;
throw error::ValueNotValidException(__PRETTY_FUNCTION__, "The index is out of bounds (size = " + std::to_string(childrenNavigators.size()) + ")", std::to_string(i), getPath());
}
return childrenNavigators[i];
@@ -206,6 +210,7 @@ namespace armarx::aron::data
// TODO
type::VariantPtr List::recalculateType() const
{
+ ARMARX_TRACE;
throw error::NotImplementedYetException(__PRETTY_FUNCTION__);
}
@@ -218,18 +223,28 @@ namespace armarx::aron::data
{
case type::Descriptor::LIST:
{
+ ARMARX_TRACE;
auto listTypeNav = type::List::DynamicCastAndCheck(type);
for (const auto& nav : childrenNavigators)
{
- if (!nav && !listTypeNav->getAcceptedType())
+ auto childTypeNav = listTypeNav->getAcceptedType();
+ if (!nav)
{
- return false;
+ if (childTypeNav && childTypeNav->getMaybe() == type::Maybe::NONE)
+ {
+ ARMARX_TRACE;
+ return false;
+ }
+ ARMARX_TRACE;
+ return true;
}
- if (!nav->fullfillsType(listTypeNav->getAcceptedType()))
+ if (!nav->fullfillsType(childTypeNav))
{
+ ARMARX_TRACE;
return false;
}
}
+ ARMARX_TRACE;
return true;
}
case type::Descriptor::TUPLE:
@@ -238,15 +253,30 @@ namespace armarx::aron::data
unsigned int i = 0;
for (const auto& nav : childrenNavigators)
{
- if (!nav && !tupleTypeNav->getAcceptedType(i))
+ if (!tupleTypeNav->hasAcceptedType(i))
+ {
+ ARMARX_TRACE;
+ return false;
+ }
+
+ auto childTypeNav = tupleTypeNav->getAcceptedType(i);
+ if (!nav)
{
+ if (childTypeNav && childTypeNav->getMaybe() == type::Maybe::NONE)
+ {
+ ARMARX_TRACE;
+ return false;
+ }
+ ARMARX_TRACE;
return false;
}
if (!nav->fullfillsType(tupleTypeNav->getAcceptedType(i)))
{
+ ARMARX_TRACE;
return false;
}
}
+ ARMARX_TRACE;
return true;
}
case type::Descriptor::PAIR:
@@ -254,19 +284,36 @@ namespace armarx::aron::data
auto pairTypeNav = type::Pair::DynamicCastAndCheck(type);
if (childrenSize() != 2)
{
+ ARMARX_TRACE;
return false;
}
- if (!childrenNavigators[0] && !pairTypeNav->getFirstAcceptedType())
+ auto firstChildTypeNav = pairTypeNav->getFirstAcceptedType();
+ if (!childrenNavigators[0])
{
+ if (firstChildTypeNav && firstChildTypeNav->getMaybe() == type::Maybe::NONE)
+ {
+ ARMARX_TRACE;
+ return false;
+ }
+ ARMARX_TRACE;
return false;
}
- if (!childrenNavigators[1] && !pairTypeNav->getSecondAcceptedType())
+ auto secondChildTypeNav = pairTypeNav->getSecondAcceptedType();
+ if (!childrenNavigators[1])
{
+ if (secondChildTypeNav && secondChildTypeNav->getMaybe() == type::Maybe::NONE)
+ {
+ ARMARX_TRACE;
+ return false;
+ }
+ ARMARX_TRACE;
return false;
}
- return childrenNavigators[0]->fullfillsType(pairTypeNav->getFirstAcceptedType()) && childrenNavigators[1]->fullfillsType(pairTypeNav->getSecondAcceptedType());
+ ARMARX_TRACE;
+ return childrenNavigators[0]->fullfillsType(firstChildTypeNav) && childrenNavigators[1]->fullfillsType(secondChildTypeNav);
}
default:
+ ARMARX_TRACE;
return false;
}
}
@@ -290,6 +337,7 @@ namespace armarx::aron::data
unsigned int i = std::stoi(path.getFirstElement());
if (!hasElement(i))
{
+ ARMARX_TRACE;
throw error::ValueNotValidException(__PRETTY_FUNCTION__, "Could not find an element of a path.", std::to_string(i), std::to_string(childrenSize()));
}
@@ -302,6 +350,7 @@ namespace armarx::aron::data
Path next = path.withDetachedFirstElement();
if (!childrenNavigators.at(i))
{
+ ARMARX_TRACE;
throw error::AronException(__PRETTY_FUNCTION__, "Could not navigate into a NULL member. Seems like the member is optional and not set.", next);
}
return childrenNavigators.at(i)->navigateAbsolute(next);
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.cpp b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.cpp
index 0e68bbc6dc16c373dd63c4626a45f5b1377afad4..2f7fe6fc8e486c43c32875c1cf072e721146c402 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.cpp
@@ -95,6 +95,7 @@ namespace armarx::aron::data
}
else
{
+ ARMARX_TRACE;
throw error::AronException(__PRETTY_FUNCTION__, "Could not set from string. Got: '" + setter + "'");
}
}
diff --git a/source/RobotAPI/libraries/aron/core/rw.h b/source/RobotAPI/libraries/aron/core/rw.h
index 6257f2422687591b0991afed8a0577ee127207ef..95b6aac606f47d3c46de34ac5ec077ded28c41a4 100644
--- a/source/RobotAPI/libraries/aron/core/rw.h
+++ b/source/RobotAPI/libraries/aron/core/rw.h
@@ -26,7 +26,7 @@ namespace armarx::aron
}
template<class ReaderT, class DtoT, class BoT>
- requires (data::isReader<ReaderT> && !DtoAndBoAreSame<DtoT, BoT>)
+ requires (data::isReader<ReaderT> && !detail::DtoAndBoAreSame<DtoT, BoT>)
inline void read(ReaderT& aron_r, typename ReaderT::InputType& input, BoT& ret)
{
DtoT aron;
@@ -36,7 +36,7 @@ namespace armarx::aron
}
template<class WriterT, class DtoT, class BoT>
- requires (data::isWriter<WriterT> && !DtoAndBoAreSame<DtoT, BoT>)
+ requires (data::isWriter<WriterT> && !detail::DtoAndBoAreSame<DtoT, BoT>)
inline void write(WriterT& aron_w, const BoT& input, typename WriterT::ReturnType& ret, const armarx::aron::Path& aron_p = armarx::aron::Path())
{
DtoT aron;
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/container/Tuple.cpp b/source/RobotAPI/libraries/aron/core/type/variant/container/Tuple.cpp
index 0879de17b8e9258a211646e2b7c8d7c4bdc37ace..51d45e3975bb162460fd06893e91f388ac52b9f1 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/container/Tuple.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/container/Tuple.cpp
@@ -64,6 +64,11 @@ namespace armarx::aron::type
return acceptedTypes;
}
+ bool Tuple::hasAcceptedType(unsigned int i) const
+ {
+ return i < acceptedTypes.size();
+ }
+
VariantPtr Tuple::getAcceptedType(unsigned int i) const
{
return acceptedTypes[i];
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/container/Tuple.h b/source/RobotAPI/libraries/aron/core/type/variant/container/Tuple.h
index d464f83269d5074e1ec6e759119b433c76de0f7d..3281584c15d6b4bb3e50c83ce598634a2bbec324 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/container/Tuple.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/container/Tuple.h
@@ -46,6 +46,7 @@ namespace armarx::aron::type
// public member functions
std::vector<VariantPtr> getAcceptedTypes() const;
+ bool hasAcceptedType(unsigned int i) const;
VariantPtr getAcceptedType(unsigned int i) const;
void addAcceptedType(const VariantPtr&);
diff --git a/source/RobotAPI/libraries/aron/test/AronConversionTester.cpp b/source/RobotAPI/libraries/aron/test/AronConversionTester.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..aac543a04b6ba1696dedc7a21c7d4ea125896e4e
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/test/AronConversionTester.cpp
@@ -0,0 +1,38 @@
+/**
+ * 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::aron_cpp_to_python_conv_test
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2023
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#include "AronConversionTester.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+
+namespace armarx::aron::test
+{
+
+ AronConversionTester::AronConversionTester(dti::AronConversionTestInterfacePrx python) : interface(python)
+ {
+ ARMARX_CHECK(python);
+ }
+
+
+} // namespace armarx::aron::test
diff --git a/source/RobotAPI/libraries/aron/test/AronConversionTester.h b/source/RobotAPI/libraries/aron/test/AronConversionTester.h
new file mode 100644
index 0000000000000000000000000000000000000000..b7ad4b8dfdcf55a37f8efc3116c27440d925d428
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/test/AronConversionTester.h
@@ -0,0 +1,111 @@
+/**
+ * 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::aron_cpp_to_python_conv_test
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2023
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#pragma once
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <RobotAPI/interface/aron/test/AronConversionTestInterface.h>
+
+
+namespace armarx::aron::test
+{
+
+ /**
+ * @brief Helper class for implementing distributed ARON conversion tests
+ * based on the `dti::AronConversionTestInterfacePrx`.
+ *
+ * Example usage:
+ *
+ * @code
+ *
+ * armarx::aron::test::dti::AronConversionTestInterfacePrx pythonComponent = ...;
+ *
+ * auto myAronClassProbeFn = []()
+ * {
+ * my::arondto::MyAronClass probe;
+ * probe.data = "42";
+ * return probe,
+ * };
+ *
+ * armarx::aron::test::AronConversionTester tester(pythonComponent);
+ *
+ * tester.test<my::arondto::MyAronClass>(myAronClassProbeFn, "MyAronClass");
+ *
+ * @endcode
+ *
+ * Note that the `pythonComponent` must point to a corresponding
+ * implementation of the `AronConversionTestInterfacePrx`.
+ */
+ class AronConversionTester
+ {
+ public:
+ template <class AronClassT>
+ using ProbeFn = std::function<AronClassT()>;
+
+
+ public:
+ AronConversionTester(dti::AronConversionTestInterfacePrx interface);
+
+ /**
+ * @brief Test the conversion of a specific ARON class.
+ *
+ * @param probeFn A factory function creating a test instance of the
+ * ARON class.
+ * @param aronClassName The name of the ARON class. Can be used by the
+ * other component to decide which class to convert to.
+ */
+ template <class AronClassT>
+ void
+ test(ProbeFn<AronClassT> probeFn, const std::string& aronClassName)
+ {
+ std::stringstream ss;
+ ss << "Test for ARON class '" << aronClassName << "': ";
+
+ const AronClassT probe = probeFn();
+
+ dto::TestAronConversionRequest req;
+ req.aronClassName = aronClassName;
+ req.probe = probe.toAronDTO();
+
+ dto::TestAronConversionResponse res = interface->testAronConversion(req);
+
+ if (res.success)
+ {
+ const AronClassT probeOut = AronClassT::FromAron(res.probe);
+ ARMARX_CHECK(probeOut == probe);
+
+ ARMARX_IMPORTANT << ss.str() << "Success";
+ }
+ else
+ {
+ ARMARX_WARNING << ss.str() << "Conversion in Python component failed: \n" << res.errorMessage;
+ }
+ }
+
+ public:
+ dti::AronConversionTestInterfacePrx interface;
+ };
+
+} // namespace armarx::aron::test
diff --git a/source/RobotAPI/libraries/aron/test/CMakeLists.txt b/source/RobotAPI/libraries/aron/test/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..3a199acf0f19c13f6253e66947e540bfe3950efd
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/test/CMakeLists.txt
@@ -0,0 +1,25 @@
+set(LIB_NAME arontest)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+
+armarx_add_library(
+ LIBS
+ # ArmarXCore
+ ArmarXCore
+ # RobotAPI
+ RobotAPICore
+ RobotAPIInterfaces
+ aron
+
+ HEADERS
+ AronConversionTester.h
+
+ SOURCES
+ AronConversionTester.cpp
+)
+
+
+add_library(aron::test ALIAS arontest)
+add_library(${PROJECT_NAME}::Aron::test ALIAS arontest)
diff --git a/source/RobotAPI/libraries/obstacle_avoidance/CMakeLists.txt b/source/RobotAPI/libraries/obstacle_avoidance/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..dfd09c4a46abb303d29f4a0e5385fb6aa0a16b7b
--- /dev/null
+++ b/source/RobotAPI/libraries/obstacle_avoidance/CMakeLists.txt
@@ -0,0 +1,28 @@
+set(LIB_NAME obstacle_avoidance)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+armarx_add_library(
+ LIBS
+ # ArmarX
+ ArmarXCore
+ # This package
+ RobotAPI::Core
+ RobotAPI::armem
+ RobotAPI::armem_vision
+ RobotAPI::ArmarXObjects
+ aroncommon
+ # System / External
+# Eigen3::Eigen
+ HEADERS
+ CollisionModelHelper.h
+ SOURCES
+ CollisionModelHelper.cpp
+)
+
+add_library(
+ "RobotAPI::${LIB_NAME}"
+ ALIAS
+ ${LIB_NAME}
+)
diff --git a/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.cpp b/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d53a8dd6a89f0cd37f609c9a4742d97d86f4deba
--- /dev/null
+++ b/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.cpp
@@ -0,0 +1,179 @@
+/*
+ * 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
+ * @author Christoph Pohl ( christoph dot pohl at kit dot edu )
+ * @date 17.02.23
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "CollisionModelHelper.h"
+
+#include <VirtualRobot/ManipulationObject.h>
+#include <VirtualRobot/SceneObjectSet.h>
+#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
+#include <VirtualRobot/Visualization/TriMeshModel.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/components/ArViz/Client/elements/Mesh.h>
+
+
+namespace armarx::obstacle_avoidance
+{
+
+ VirtualRobot::ManipulationObjectPtr
+ CollisionModelHelper::asManipulationObject(const objpose::ObjectPose& objectPose)
+ {
+ const ObjectFinder finder;
+
+ const VirtualRobot::SceneObjectSetPtr sceneObjects(new VirtualRobot::SceneObjectSet);
+ if (auto obstacle = finder.loadManipulationObject(objectPose))
+ {
+ obstacle->setGlobalPose(objectPose.objectPoseGlobal);
+ return obstacle;
+ }
+
+ ARMARX_WARNING << "Failed to load scene object `" << objectPose.objectID << "`";
+ return nullptr;
+ }
+
+ VirtualRobot::SceneObjectSetPtr
+ CollisionModelHelper::asSceneObjects(const objpose::ObjectPoseSeq& objectPoses)
+ {
+ const ObjectFinder finder;
+
+ VirtualRobot::SceneObjectSetPtr sceneObjects(new VirtualRobot::SceneObjectSet);
+ for (const auto& objectPose : objectPoses)
+ {
+ if (auto obstacle = finder.loadManipulationObject(objectPose))
+ {
+ obstacle->setGlobalPose(objectPose.objectPoseGlobal);
+ sceneObjects->addSceneObject(obstacle);
+ }
+ }
+
+ return sceneObjects;
+ }
+
+ VirtualRobot::SceneObjectSetPtr
+ CollisionModelHelper::asSceneObjects(const OccupancyGrid& occupancyGrid,
+ const OccupancyGridHelper::Params& params)
+ {
+ const OccupancyGridHelper ocHelper(occupancyGrid, params);
+ const auto obstacles = ocHelper.obstacles();
+
+ const float boxSize = occupancyGrid.resolution;
+ const float resolution = occupancyGrid.resolution;
+
+ VirtualRobot::SceneObjectSetPtr sceneObjects(new VirtualRobot::SceneObjectSet);
+
+ ARMARX_CHECK_EQUAL(occupancyGrid.frame, GlobalFrame)
+ << "Only occupancy grid in global frame supported.";
+
+ VirtualRobot::CoinVisualizationFactory factory;
+
+ const auto& world_T_map = occupancyGrid.pose;
+
+ for (int x = 0; x < obstacles.rows(); x++)
+ {
+ for (int y = 0; y < obstacles.cols(); y++)
+ {
+ if (obstacles(x, y))
+ {
+ const Eigen::Vector3f pos{
+ static_cast<float>(x) * resolution, static_cast<float>(y) * resolution, 0};
+
+ // FIXME: change to Isometry3f
+ Eigen::Affine3f map_T_obj = Eigen::Affine3f::Identity();
+ map_T_obj.translation() = pos;
+
+ Eigen::Affine3f world_T_obj = world_T_map * map_T_obj;
+
+ // ARMARX_INFO << world_T_obj.translation();
+
+ auto cube = factory.createBox(boxSize, boxSize, boxSize);
+
+ const VirtualRobot::CollisionModelPtr collisionModel(
+ new VirtualRobot::CollisionModel(cube));
+
+ const VirtualRobot::SceneObjectPtr sceneObject(new VirtualRobot::SceneObject(
+ "box_" + std::to_string(sceneObjects->getSize()), cube, collisionModel));
+ sceneObject->setGlobalPose(world_T_obj.matrix());
+
+ sceneObjects->addSceneObject(sceneObject);
+ }
+ }
+ }
+
+ return sceneObjects;
+ }
+
+ CollisionModelHelper::CollisionModelHelper(const objpose::ObjectPoseClient& client) :
+ objectPoseClient_(client)
+ {
+ }
+
+ VirtualRobot::SceneObjectSetPtr
+ CollisionModelHelper::fetchSceneObjects()
+ {
+ const objpose::ObjectPoseSeq objectPoses = objectPoseClient_.fetchObjectPoses();
+ return asSceneObjects(objectPoses);
+ }
+
+ CollisionModelHelper::ManipulationObjectSetPtr
+ CollisionModelHelper::fetchManipulationObjects()
+ {
+ const objpose::ObjectPoseSeq objectPoses = objectPoseClient_.fetchObjectPoses();
+ return asManipulationObjects(objectPoses);
+ }
+
+ CollisionModelHelper::ManipulationObjectSetPtr
+ CollisionModelHelper::asManipulationObjects(const objpose::ObjectPoseSeq& objectPoses)
+ {
+ ManipulationObjectSet set;
+
+ for (const auto& pose : objectPoses)
+ {
+ set.emplace_back(*asManipulationObject(pose));
+ }
+
+ return std::make_shared<ManipulationObjectSet>(set);
+ }
+
+ void
+ CollisionModelHelper::visualizeCollisionModel(const VirtualRobot::CollisionModelPtr& model,
+ viz::Client& arviz)
+ {
+ armarx::viz::Mesh mesh(model->getName());
+ auto faces = model->getTriMeshModel()->faces;
+ std::vector<armarx::viz::data::Face> viz_faces;
+ std::transform(
+ faces.begin(),
+ faces.end(),
+ std::back_inserter(viz_faces),
+ [](const auto& face)
+ {
+ return armarx::viz::data::Face(
+ face.id1, face.id2, face.id3, face.idColor1, face.idColor2, face.idColor3);
+ });
+ mesh.vertices(model->getTriMeshModel()->vertices)
+ .faces(viz_faces)
+ .pose(model->getGlobalPose());
+ arviz.commitLayerContaining("CollisionModel", mesh);
+ }
+
+} // namespace armarx::obstacle_avoidance
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.h b/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.h
new file mode 100644
index 0000000000000000000000000000000000000000..d56a2d5bda01b7492f6b73f5ce3f9416bf7b0db7
--- /dev/null
+++ b/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.h
@@ -0,0 +1,62 @@
+/*
+ * 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
+ * @author Christoph Pohl ( christoph dot pohl at kit dot edu )
+ * @date 17.02.23
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <RobotAPI/components/ArViz/Client/Client.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h>
+#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
+#include <RobotAPI/libraries/armem_vision/OccupancyGridHelper.h>
+#include <RobotAPI/libraries/armem_vision/types.h>
+
+namespace armarx::obstacle_avoidance
+{
+
+ class CollisionModelHelper
+ {
+ public:
+ using ManipulationObjectSet = std::vector<VirtualRobot::ManipulationObject>;
+ using ManipulationObjectSetPtr = std::shared_ptr<ManipulationObjectSet>;
+
+ static VirtualRobot::ManipulationObjectPtr
+ asManipulationObject(const objpose::ObjectPose& objectPose);
+ static ManipulationObjectSetPtr
+ asManipulationObjects(const objpose::ObjectPoseSeq& objectPoses);
+ static VirtualRobot::SceneObjectSetPtr
+ asSceneObjects(const objpose::ObjectPoseSeq& objectPoses);
+ static VirtualRobot::SceneObjectSetPtr
+ asSceneObjects(const armem::vision::OccupancyGrid& occupancyGrid,
+ const OccupancyGridHelper::Params& params);
+ static void visualizeCollisionModel(const VirtualRobot::CollisionModelPtr& model,
+ viz::Client& arviz);
+
+
+ CollisionModelHelper(const objpose::ObjectPoseClient& client);
+ VirtualRobot::SceneObjectSetPtr fetchSceneObjects();
+ ManipulationObjectSetPtr fetchManipulationObjects();
+
+ private:
+ objpose::ObjectPoseClient objectPoseClient_;
+ };
+} // namespace armarx::obstacle_avoidance
diff --git a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp
index 23a708aceace48317d04884a8579fd21001b92c3..60a38b465ddb8bd9d6cbdac5ec4952606624880c 100644
--- a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp
@@ -33,7 +33,9 @@ namespace armarx
}
else
{
- ARMARX_INFO << "Trying to add a provider with name '" << info.providerName << "' but the provider already exists.";
+ ARMARX_INFO << "Trying to add a provider with name '" << info.providerName << "' but the provider already exists. "
+ << "Overwriting the old provider info.";
+ skillProviderMap[info.providerName] = info.provider;
}
}
@@ -47,7 +49,7 @@ namespace armarx
}
else
{
- ARMARX_INFO << "Trying to remove a provider with name '" << providerName << "' but it couldn't found.";
+ ARMARX_INFO << "Trying to remove a provider with name '" << providerName << "' but it couldn't be found.";
}
}
@@ -79,31 +81,64 @@ namespace armarx
providerName = info.skillId.providerName;
}
+ bool remove = false;
if (auto it = skillProviderMap.find(providerName); it != skillProviderMap.end())
{
- skills::callback::dti::SkillProviderCallbackInterfacePrx myPrx;
- getProxy(myPrx, -1);
+ const auto& n = it->first;
+ const auto& s = it->second;
- skills::provider::dto::SkillExecutionRequest exInfo;
- exInfo.skillName = info.skillId.skillName;
- exInfo.executorName = info.executorName;
- exInfo.callbackInterface = myPrx;
- exInfo.params = info.params;
+ if (s)
+ {
+ skills::callback::dti::SkillProviderCallbackInterfacePrx myPrx;
+ getProxy(myPrx, -1);
+
+ skills::provider::dto::SkillExecutionRequest exInfo;
+ exInfo.skillName = info.skillId.skillName;
+ exInfo.executorName = info.executorName;
+ exInfo.callbackInterface = myPrx;
+ exInfo.params = info.params;
+
+ return s->executeSkill(exInfo);
+ }
+ else
+ {
+ remove = true;
+ }
- return it->second->executeSkill(exInfo);
+ if (remove)
+ {
+ std::scoped_lock l(skillProviderMapMutex);
+ if (auto it = skillProviderMap.find(providerName); it != skillProviderMap.end())
+ {
+ ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '" << n << "' during execution. Removing it from skills.";
+ it = skillProviderMap.erase(it);
+ }
+ }
}
else
{
ARMARX_ERROR << "Could not execute a skill of provider '" + providerName + "' because the provider does not exist.";
throw skills::error::SkillException(__PRETTY_FUNCTION__, "Skill execution failed. Could not execute a skill of provider '" + providerName + "' because the provider does not exist.");
}
+ return {}; // Never happens
}
void SkillManagerComponentPluginUser::abortSkill(const std::string& providerName, const std::string& skillName, const Ice::Current ¤t)
{
+ std::scoped_lock l(skillProviderMapMutex);
if (auto it = skillProviderMap.find(providerName); it != skillProviderMap.end())
{
- it->second->abortSkill(skillName);
+ const auto& n = it->first;
+ const auto& s = it->second;
+ if (s)
+ {
+ s->abortSkill(skillName);
+ }
+ else
+ {
+ ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '" << n << "'. Removing it from skills.";
+ it = skillProviderMap.erase(it);
+ }
}
}
@@ -116,10 +151,23 @@ namespace armarx
skills::manager::dto::SkillDescriptionMapMap SkillManagerComponentPluginUser::getSkillDescriptions(const Ice::Current ¤t)
{
skills::manager::dto::SkillDescriptionMapMap ret;
- for (const auto& [n, s] : skillProviderMap)
+
+ std::scoped_lock l(skillProviderMapMutex);
+ for (auto it = skillProviderMap.cbegin(); it != skillProviderMap.cend();)
{
- skills::provider::dto::SkillDescriptionMap m = s->getSkillDescriptions();
- ret.insert({n, m});
+ const auto& n = it->first;
+ const auto& s = it->second;
+ if (s)
+ {
+ skills::provider::dto::SkillDescriptionMap m = s->getSkillDescriptions();
+ ret.insert({n, m});
+ ++it;
+ }
+ else
+ {
+ ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '" << n << "'. Removing it from skills.";
+ it = skillProviderMap.erase(it);
+ }
}
return ret;
}
@@ -127,10 +175,23 @@ namespace armarx
skills::manager::dto::SkillStatusUpdateMapMap SkillManagerComponentPluginUser::getSkillExecutionStatuses(const Ice::Current ¤t)
{
skills::manager::dto::SkillStatusUpdateMapMap ret;
- for (const auto& [n, s] : skillProviderMap)
+
+ std::scoped_lock l(skillProviderMapMutex);
+ for (auto it = skillProviderMap.cbegin(); it != skillProviderMap.cend();)
{
- skills::provider::dto::SkillStatusUpdateMap m = s->getSkillExecutionStatuses();
- ret.insert({n, m});
+ const auto& n = it->first;
+ const auto& s = it->second;
+ if (s)
+ {
+ skills::provider::dto::SkillStatusUpdateMap m = s->getSkillExecutionStatuses();
+ ret.insert({n, m});
+ it++;
+ }
+ else
+ {
+ ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '" << n << "'. Removing it from skills.";
+ it = skillProviderMap.erase(it);
+ }
}
return ret;
}
diff --git a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
index 1fb8c3ac52010943e01b044d0e99ce144739a2b6..81e91b131294d542c286659e0691056ea0bc0894 100644
--- a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
+++ b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
@@ -86,7 +86,7 @@ namespace armarx
}
catch (const std::exception& ex)
{
- std::string message = "SkillError 101e: An error occured during the check whether skill '" + skillName + "' is available. The error was: " + ex.what();
+ std::string message = "SkillError 101e: An error occured during the check whether skill '" + skillName + "' is available. The error was: " + GetHandledExceptionString();
ARMARX_ERROR_S << message;
resetExecParam();
return TerminatedSkillStatusUpdate({{skill->getSkillId(), executorName, parameterization, createErrorMessage(message)}, TerminatedSkillStatus::Failed});
@@ -102,7 +102,7 @@ namespace armarx
}
catch (const std::exception& ex)
{
- std::string message = "SkillError 201e: An error occured during the reset of skill '" + skillName + "'. The error was: " + ex.what();
+ std::string message = "SkillError 201e: An error occured during the reset of skill '" + skillName + "'. The error was: " + GetHandledExceptionString();
ARMARX_ERROR_S << message;
updateStatus(SkillStatus::Failed);
@@ -116,7 +116,7 @@ namespace armarx
}
catch (const std::exception& ex)
{
- std::string message = "SkillError 301e: An error occured during waiting for skill dependencies of skill '" + skillName + "'. The error was: " + ex.what();
+ std::string message = "SkillError 301e: An error occured during waiting for skill dependencies of skill '" + skillName + "'. The error was: " + GetHandledExceptionString();
ARMARX_ERROR_S << message;
updateStatus(SkillStatus::Failed);
@@ -143,7 +143,7 @@ namespace armarx
}
catch (const std::exception& ex)
{
- std::string message = "SkillError 401e: An error occured during the initialization of skill '" + skillName + "'. The error was: " + ex.what();
+ std::string message = "SkillError 401e: An error occured during the initialization of skill '" + skillName + "'. The error was: " + GetHandledExceptionString();
ARMARX_ERROR_S << message;
skill->exitSkill({executorName, aron_params}); // try to exit skill. Ignore return value
@@ -174,7 +174,7 @@ namespace armarx
}
catch (const std::exception& ex)
{
- std::string message = "SkillError 501e: An error occured during the main method of skill '" + skillName + "'. The error was: " + ex.what();
+ std::string message = "SkillError 501e: An error occured during the main method of skill '" + skillName + "'. The error was: " + GetHandledExceptionString();
ARMARX_ERROR_S << message;
skill->exitSkill({executorName, aron_params}); // try to exit skill. Ignore return value
@@ -199,7 +199,7 @@ namespace armarx
}
catch (const std::exception& ex)
{
- std::string message = "SkillError 601e: An error occured during the exit method of skill '" + skillName + "'. The error was: " + ex.what();
+ std::string message = "SkillError 601e: An error occured during the exit method of skill '" + skillName + "'. The error was: " + GetHandledExceptionString();
ARMARX_ERROR_S << message;
updateStatus(SkillStatus::Failed);
diff --git a/source/RobotAPI/libraries/skills/provider/mixins/RobotWritingSkillMixin.h b/source/RobotAPI/libraries/skills/provider/mixins/RobotWritingSkillMixin.h
new file mode 100644
index 0000000000000000000000000000000000000000..34856452c4b8bd3b4e61dd69ee03eb122d409566
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/mixins/RobotWritingSkillMixin.h
@@ -0,0 +1,28 @@
+#pragma once
+
+
+// Others
+#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.h>
+
+namespace armarx::skills::mixin
+{
+ struct RobotWritingSkillMixin
+ {
+ armem::robot_state::VirtualRobotWriter robotWriter;
+
+ RobotWritingSkillMixin(armem::client::MemoryNameSystem& mns) :
+ robotWriter(mns)
+ {}
+ };
+
+ struct SpecificRobotWritingSkillMixin
+ {
+ std::string robotName;
+ armem::robot_state::VirtualRobotWriter robotWriter;
+
+ SpecificRobotWritingSkillMixin(const std::string& rn, armem::client::MemoryNameSystem& mns) :
+ robotName(rn),
+ robotWriter(mns)
+ {}
+ };
+}