diff --git a/scenarios/ArMemObjectMemory/config/ArticulatedObjectExampleMemoryWriterClient.cfg b/scenarios/ArMemObjectMemory/config/ArticulatedObjectExampleMemoryWriterClient.cfg
index bbfc7a0ca3a8eddd35f8c447efb327bbbe260c48..5082b0ffc78b15d395b64deb6f0ba3c14ac38ea9 100644
--- a/scenarios/ArMemObjectMemory/config/ArticulatedObjectExampleMemoryWriterClient.cfg
+++ b/scenarios/ArMemObjectMemory/config/ArticulatedObjectExampleMemoryWriterClient.cfg
@@ -75,6 +75,30 @@ ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.ArMemMemor
ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.ProviderName = ExampleProvider
+# ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.write.ArMemMemoryNameSystem: No Description
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: no
+# - Required: no
+ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.write.ArMemMemoryNameSystem = MemoryNameSystem
+
+
+# ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.write.MemoryName: No Description
+# Attributes:
+# - Default: Object
+# - Case sensitivity: no
+# - Required: no
+ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.write.MemoryName = Object
+
+
+# ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.write.ProviderName: No Description
+# Attributes:
+# - Default: ExampleProvider
+# - Case sensitivity: no
+# - Required: no
+ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.write.ProviderName = ExampleProvider
+
+
# ArmarX.ArticulatedObjectExampleMemoryWriterClient.tpc.pub.DebugObserver: Name of the `DebugObserver` topic to publish data to.
# Attributes:
# - Default: DebugObserver
@@ -225,18 +249,3 @@ ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.ProviderNa
# ArmarX.Verbosity = Info
-# ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.write.ArMemMemoryNameSystem:
-# Attributes:
-ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.write.ArMemMemoryNameSystem = MemoryNameSystem
-
-
-# ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.write.MemoryName:
-# Attributes:
-ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.write.MemoryName = Object
-
-
-# ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.write.ProviderName:
-# Attributes:
-ArmarX.ArticulatedObjectExampleMemoryWriterClient.mem.obj.articulated.write.ProviderName = ExampleProvider
-
-
diff --git a/scenarios/ArMemObjectMemory/config/ObjectMemory.cfg b/scenarios/ArMemObjectMemory/config/ObjectMemory.cfg
index 88ad0e549f399971ccb3aecafa7d32ef7df4be42..67e86da4ae2afbacca3e22a5eb4bc878f801145f 100644
--- a/scenarios/ArMemObjectMemory/config/ObjectMemory.cfg
+++ b/scenarios/ArMemObjectMemory/config/ObjectMemory.cfg
@@ -166,6 +166,58 @@
# ArmarX.ObjectMemory.mem.MemoryName = Object
+# ArmarX.ObjectMemory.mem.articulated.cls.CoreSegmentName: Name of the object instance core segment.
+# Attributes:
+# - Default: ArticulatedObjectClass
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.articulated.cls.CoreSegmentName = ArticulatedObjectClass
+
+
+# ArmarX.ObjectMemory.mem.articulated.cls.DiscardSnapshotsWhileAttached: If true, no new snapshots are stored while an object is attached to a robot node.
+# If false, new snapshots are stored, but the attachment is kept in the new snapshots.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.articulated.cls.DiscardSnapshotsWhileAttached = true
+
+
+# ArmarX.ObjectMemory.mem.articulated.cls.MaxHistorySize: Maximal size of object poses history (-1 for infinite).
+# Attributes:
+# - Default: -1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.articulated.cls.MaxHistorySize = -1
+
+
+# ArmarX.ObjectMemory.mem.articulated.inst.CoreSegmentName: Name of the object instance core segment.
+# Attributes:
+# - Default: ArticulatedObjectInstance
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.articulated.inst.CoreSegmentName = ArticulatedObjectInstance
+
+
+# ArmarX.ObjectMemory.mem.articulated.inst.DiscardSnapshotsWhileAttached: If true, no new snapshots are stored while an object is attached to a robot node.
+# If false, new snapshots are stored, but the attachment is kept in the new snapshots.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.articulated.inst.DiscardSnapshotsWhileAttached = true
+
+
+# ArmarX.ObjectMemory.mem.articulated.inst.MaxHistorySize: Maximal size of object poses history (-1 for infinite).
+# Attributes:
+# - Default: -1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.articulated.inst.MaxHistorySize = -1
+
+
# ArmarX.ObjectMemory.mem.cls.CoreSegmentName: Name of the object clazz core segment.
# Attributes:
# - Default: Class
@@ -453,14 +505,6 @@
# ArmarX.ObjectMemory.tpc.pub.MemoryListener = MemoryUpdates
-# ArmarX.ObjectMemory.tpc.sub.ArticulatedObjectTopic: Name of the `ArticulatedObjectTopic` topic to subscribe to.
-# Attributes:
-# - Default: ArticulatedObjectTopic
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ObjectMemory.tpc.sub.ArticulatedObjectTopic = ArticulatedObjectTopic
-
-
# ArmarX.ObjectMemory.tpc.sub.ObjectPoseTopic: Name of the `ObjectPoseTopic` topic to subscribe to.
# Attributes:
# - Default: ObjectPoseTopic
diff --git a/source/RobotAPI/components/armem/client/ArticulatedObjectExampleMemoryWriterClient/ArticulatedObjectExampleMemoryWriterClient.cpp b/source/RobotAPI/components/armem/client/ArticulatedObjectExampleMemoryWriterClient/ArticulatedObjectExampleMemoryWriterClient.cpp
index 4d45a76b229a11ac6570fb519ff3b2eb0cadd048..efe2de65b74e5f4a06b76983b1b1ae3f2e5097c4 100644
--- a/source/RobotAPI/components/armem/client/ArticulatedObjectExampleMemoryWriterClient/ArticulatedObjectExampleMemoryWriterClient.cpp
+++ b/source/RobotAPI/components/armem/client/ArticulatedObjectExampleMemoryWriterClient/ArticulatedObjectExampleMemoryWriterClient.cpp
@@ -103,7 +103,7 @@ namespace armarx::articulated_object
CycleUtil cycle(IceUtil::Time::milliSeconds(100));
IceUtil::Time start = TimeUtil::GetTime();
- CycleUtil c(1000);
+ CycleUtil c(100);
while (not task->isStopped())
{
@@ -113,11 +113,11 @@ namespace armarx::articulated_object
const float t = float((now - start).toSecondsDouble());
// move joints at certain frequency
- const float k = (1 + std::sin(t / (M_2_PIf32 * 0.2))) / 2; // in [0,1]
+ const float k = (1 + std::sin(t / (M_2_PIf32))) / 2; // in [0,1]
const std::map<std::string, float> jointValues
{
- {"dishwasher_door_joint", 0.2 * k},
+ {"dishwasher_door_joint", M_PIf32 / 2 * k},
{"drawer_joint", 350 * k}
};
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
index 2d62c6c7e0b975e7729f989cb37ee3542905649f..338bdc68adb2918c40707048664c773447404fa4 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
@@ -54,6 +54,9 @@ namespace armarx::armem::server::obj
classSegment.defineProperties(defs, prefix + "cls.");
instance::SegmentAdapter::defineProperties(defs, prefix + "inst.");
+ articulatedObjectInstanceSegment.defineProperties(defs, prefix + "articulated.inst.");
+ articulatedObjectClassSegment.defineProperties(defs, prefix + "articulated.cls.");
+
return defs;
}
@@ -62,7 +65,11 @@ namespace armarx::armem::server::obj
instance::SegmentAdapter(server::ComponentPluginUser::iceMemory,
server::ComponentPluginUser::workingMemoryMutex),
classSegment(server::ComponentPluginUser::iceMemory,
- server::ComponentPluginUser::workingMemoryMutex)
+ server::ComponentPluginUser::workingMemoryMutex),
+ articulatedObjectClassSegment(server::ComponentPluginUser::iceMemory,
+ server::ComponentPluginUser::workingMemoryMutex),
+ articulatedObjectInstanceSegment(server::ComponentPluginUser::iceMemory,
+ server::ComponentPluginUser::workingMemoryMutex)
{
}
@@ -77,6 +84,7 @@ namespace armarx::armem::server::obj
instance::SegmentAdapter::init();
+ // class segment
try
{
classSegment.init();
@@ -93,6 +101,43 @@ namespace armarx::armem::server::obj
{
ARMARX_ERROR << "Failed to init class segment for unknown reason.";
}
+
+ // articulated object class segment
+ try
+ {
+ articulatedObjectClassSegment.init();
+ }
+ catch (const LocalException& e)
+ {
+ ARMARX_ERROR << "Failed to init class segment. Reason: \n" << e.what();
+ }
+ catch (const std::exception& e)
+ {
+ ARMARX_ERROR << "Failed to init class segment. Reason: \n" << e.what();
+ }
+ catch (...)
+ {
+ ARMARX_ERROR << "Failed to init class segment for unknown reason.";
+ }
+
+ // articulated object instance segment
+ try
+ {
+ articulatedObjectInstanceSegment.setArticulatedObjectClassSegment(articulatedObjectClassSegment);
+ articulatedObjectInstanceSegment.init();
+ }
+ catch (const LocalException& e)
+ {
+ ARMARX_ERROR << "Failed to init class segment. Reason: \n" << e.what();
+ }
+ catch (const std::exception& e)
+ {
+ ARMARX_ERROR << "Failed to init class segment. Reason: \n" << e.what();
+ }
+ catch (...)
+ {
+ ARMARX_ERROR << "Failed to init class segment for unknown reason.";
+ }
}
void ObjectMemory::onConnectComponent()
@@ -112,19 +157,27 @@ namespace armarx::armem::server::obj
robotStateComponent,
robot,
kinematicUnitObserver,
- ArVizComponentPluginUser::arviz,
+ ArVizComponentPluginUser::getArvizClient(),
debugObserver
);
classSegment.connect(
- ArVizComponentPluginUser::arviz
+ ArVizComponentPluginUser::getArvizClient()
);
+ articulatedObjectClassSegment.connect(
+ ArVizComponentPluginUser::getArvizClient()
+ );
- {
- std::lock_guard g(server::ComponentPluginUser::workingMemoryMutex);
- server::ComponentPluginUser::workingMemory.addCoreSegment("ArticulatedObjectInstance"); // TODO , arondto::::toInitialAronType());
- server::ComponentPluginUser::workingMemory.addCoreSegment("ArticulatedObjectClass"); // TODO , arondto::::toInitialAronType());
- }
+ articulatedObjectInstanceSegment.connect(
+ ArVizComponentPluginUser::getArvizClient()
+ );
+
+
+ // {
+ // std::lock_guard g(server::ComponentPluginUser::workingMemoryMutex);
+ // // server::ComponentPluginUser::workingMemory.addCoreSegment("ArticulatedObjectInstance"); // TODO , arondto::::toInitialAronType());
+ // server::ComponentPluginUser::workingMemory.addCoreSegment("ArticulatedObjectClass"); // TODO , arondto::::toInitialAronType());
+ // }
createRemoteGuiTab();
RemoteGui_startRunningTask();
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
index 65bd9c472828faa2b8b7f58679c4922c1aef389d..822fabbe56fad5158337c81c9c0089aa5d7233c2 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
@@ -38,6 +38,8 @@
#include <RobotAPI/libraries/armem_objects/server/class/Segment.h>
#include <RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h>
+#include <RobotAPI/libraries/armem_objects/server/articulated_object_instance/Segment.h>
+#include <RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.h>
#define ICE_CURRENT_ARG const Ice::Current& = Ice::emptyCurrent
@@ -108,6 +110,9 @@ namespace armarx::armem::server::obj
clazz::Segment classSegment;
+ articulated_object_class::Segment articulatedObjectClassSegment;
+ articulated_object_instance::Segment articulatedObjectInstanceSegment;
+
struct RemoteGuiTab : armarx::RemoteGui::Client::Tab
{
@@ -118,6 +123,6 @@ namespace armarx::armem::server::obj
};
-}
+} // namespace armarx::armem::server::obj
#undef ICE_CURRENT_ARG
diff --git a/source/RobotAPI/libraries/armem/core/MemoryID.h b/source/RobotAPI/libraries/armem/core/MemoryID.h
index 09415ec4abd73185f77050c89683b326f3c3e9d0..7ddf2130801767eabb60c01caba1d2dc427caba3 100644
--- a/source/RobotAPI/libraries/armem/core/MemoryID.h
+++ b/source/RobotAPI/libraries/armem/core/MemoryID.h
@@ -90,19 +90,19 @@ namespace armarx::armem
bool hasMemoryName() const
{
- return memoryName.size() > 0;
+ return !memoryName.empty();
}
bool hasCoreSegmentName() const
{
- return coreSegmentName.size() > 0;
+ return !coreSegmentName.empty();
}
bool hasProviderSegmentName() const
{
- return providerSegmentName.size() > 0;
+ return !providerSegmentName.empty();
}
bool hasEntityName() const
{
- return entityName.size() > 0;
+ return !entityName.empty();
}
bool hasTimestamp() const
{
@@ -254,7 +254,7 @@ namespace armarx::armem
*/
bool contains(const MemoryID& general, const MemoryID& specific);
-}
+} // namespace armarx::armem
namespace std
diff --git a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
index 47c5b78eafc5c9cded97c85ccd8f95ebf382670c..b491f16ce361bb4c5ca46b1f46104b6cb302711e 100644
--- a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
@@ -28,13 +28,17 @@ armarx_add_library(
server/instance/RobotHeadMovement.h
server/instance/Visu.h
- # server/articulated_object/Segment.h
+ server/articulated_object_class/Segment.h
+ server/articulated_object_instance/Segment.h
# server/articulated_object/SegmentAdapter.h
server/articulated_object_instance/Visu.h
client/articulated_object/Reader.cpp
client/articulated_object/Writer.cpp
+ articulated_object_conversions.cpp
+
+
SOURCES
aron_conversions.cpp
@@ -47,7 +51,9 @@ armarx_add_library(
server/instance/RobotHeadMovement.cpp
server/instance/Visu.cpp
- # server/articulated_object/Segment.cpp
+ server/articulated_object_class/Segment.cpp
+
+ server/articulated_object_instance/Segment.cpp
# server/articulated_object/SegmentAdapter.cpp
server/articulated_object_instance/Visu.cpp
@@ -55,6 +61,8 @@ armarx_add_library(
client/articulated_object/Writer.h
client/articulated_object/interfaces.h
+ articulated_object_conversions.h
+
)
diff --git a/source/RobotAPI/libraries/armem_objects/aron/RobotDescription.xml b/source/RobotAPI/libraries/armem_objects/aron/RobotDescription.xml
index 84ab00dc170c94880205ce0f9d1327eaf98b0aac..edce8c802a4e77cb571492700422b7c04eb7e1a1 100644
--- a/source/RobotAPI/libraries/armem_objects/aron/RobotDescription.xml
+++ b/source/RobotAPI/libraries/armem_objects/aron/RobotDescription.xml
@@ -1,13 +1,12 @@
<!--This class cotains information on how to create a VirtualRobot::Robot -->
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
- <AronIncludes>
- <Include include="<RobotAPI/libraries/aron/common/aron/PackagePath.xml>" />
- </AronIncludes>
<CodeIncludes>
<Include include="<RobotAPI/libraries/aron/common/aron/PackagePath.aron.generated.h>" />
</CodeIncludes>
-
+ <AronIncludes>
+ <Include include="<RobotAPI/libraries/aron/common/aron/PackagePath.xml>" />
+ </AronIncludes>
<GenerateTypes>
<Object name='armarx::armem::arondto::RobotDescription'>
diff --git a/source/RobotAPI/libraries/armem_objects/articulated_object_conversions.cpp b/source/RobotAPI/libraries/armem_objects/articulated_object_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..03cac43717941e8da1a3a738d6c15aad1aa8987d
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/articulated_object_conversions.cpp
@@ -0,0 +1,65 @@
+#include "articulated_object_conversions.h"
+
+#include <filesystem>
+
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+
+#include "RobotAPI/libraries/armem/core/workingmemory/EntityInstance.h"
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+
+#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
+
+
+namespace fs = ::std::filesystem;
+
+namespace armarx::armem::articulated_object
+{
+
+ std::optional<RobotDescription> convertRobotDescription(const armem::wm::EntityInstance& instance)
+ {
+ arondto::RobotDescription aronRobotDescription;
+ try
+ {
+ aronRobotDescription.fromAron(instance.data());
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << "Conversion to RobotDescription failed!";
+ return std::nullopt;
+ }
+
+
+ RobotDescription robotDescription
+ {
+ .name = "",
+ .xml = ::armarx::PackagePath("", fs::path("")) // initialize empty, no default c'tor
+ };
+
+ fromAron(aronRobotDescription, robotDescription);
+
+ return robotDescription;
+ }
+
+
+ std::optional<RobotState> convertRobotState(const armem::wm::EntityInstance& instance)
+ {
+ arondto::RobotState aronRobotState;
+ try
+ {
+ aronRobotState.fromAron(instance.data());
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << "Conversion to RobotState failed!";
+ return std::nullopt;
+ }
+
+ RobotState robotState;
+ fromAron(aronRobotState, robotState);
+
+ return robotState;
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem_objects/articulated_object_conversions.h b/source/RobotAPI/libraries/armem_objects/articulated_object_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..ab29163c347bca6cfa78aa1b10581955140a3fc1
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/articulated_object_conversions.h
@@ -0,0 +1,16 @@
+#pragma once
+
+#include <optional>
+
+#include "types.h"
+
+namespace armarx::armem::wm
+{
+ class EntityInstance;
+}
+
+namespace armarx::armem::articulated_object
+{
+ std::optional<RobotDescription> convertRobotDescription(const armem::wm::EntityInstance& instance);
+ std::optional<RobotState> convertRobotState(const armem::wm::EntityInstance& instance);
+} // namespace armarx::armem::articulated_object
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
index 3d2fd18ddb5c3591da7ce3dafe5394815eedd360..19359fabc2dfd68c475b31241cd8f2b24b2b9fa0 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
@@ -1,15 +1,17 @@
#include "Reader.h"
#include <mutex>
+#include <optional>
#include "ArmarXCore/core/logging/Logging.h"
+#include <ArmarXCore/core/PackagePath.h>
+
#include "RobotAPI/libraries/armem/core/Time.h"
#include "RobotAPI/libraries/armem/client/query/Builder.h"
#include "RobotAPI/libraries/armem_objects/aron_conversions.h"
#include <RobotAPI/libraries/armem_objects/aron/Robot.aron.generated.h>
-#include <optional>
-#include <ArmarXCore/core/PackagePath.h>
+#include "RobotAPI/libraries/armem_objects/articulated_object_conversions.h"
namespace fs = ::std::filesystem;
@@ -113,24 +115,6 @@ namespace armarx::armem::articulated_object
return getRobotState(qResult.memory);
}
- std::optional<RobotState> convertRobotState(const armem::wm::EntityInstance& instance)
- {
- arondto::RobotState aronRobotState;
- try
- {
- aronRobotState.fromAron(instance.data());
- }
- catch (...)
- {
- ARMARX_WARNING << "Conversion failed!";
- return std::nullopt;
- }
-
- RobotState robotState;
- fromAron(aronRobotState, robotState);
-
- return robotState;
- }
std::optional<RobotState> Reader::getRobotState(const armarx::armem::wm::Memory& memory) const
{
@@ -161,30 +145,6 @@ namespace armarx::armem::articulated_object
return convertRobotState(instance);
}
- std::optional<RobotDescription> convertRobotDescription(const armem::wm::EntityInstance& instance)
- {
- arondto::RobotDescription aronRobotDescription;
- try
- {
- aronRobotDescription.fromAron(instance.data());
- }
- catch (...)
- {
- ARMARX_WARNING << "Conversion failed!";
- return std::nullopt;
- }
-
-
- RobotDescription robotDescription
- {
- .name = "",
- .xml = ::armarx::PackagePath("", fs::path("")) // initialize empty, no default c'tor
- };
-
- fromAron(aronRobotDescription, robotDescription);
-
- return robotDescription;
- }
std::optional<RobotDescription> Reader::getRobotDescription(const armarx::armem::wm::Memory& memory) const
@@ -216,6 +176,4 @@ namespace armarx::armem::articulated_object
return convertRobotDescription(instance);
}
-
-
} // namespace armarx::armem::articulated_object
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
index c81775c85945249ce762443038fe5a6cf292257e..cd047cf6cc81dca29061f679537d787935ebfa75 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
@@ -1,3 +1,24 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
#pragma once
#include <mutex>
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
index 74ec9a1de548eba30b470271080c0721eb43fb7d..9e9b2075846219ca9390f56f3474c839d169dc52 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
@@ -1,3 +1,24 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
#pragma once
#include <mutex>
diff --git a/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e2babe5079ff5a0e00d07317d163c089644db77b
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.cpp
@@ -0,0 +1,127 @@
+#include "Segment.h"
+
+#include <sstream>
+
+#include <ArmarXCore/core/time/TimeUtil.h>
+#include "ArmarXCore/core/logging/Logging.h"
+
+#include "RobotAPI/libraries/aron/common/aron_conversions.h"
+
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/Visitor.h>
+#include "RobotAPI/libraries/armem/core/MemoryID.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/server/MemoryToIceAdapter.h>
+
+#include <RobotAPI/libraries/armem_objects/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+#include "RobotAPI/libraries/armem_objects/articulated_object_conversions.h"
+
+
+namespace armarx::armem::server::obj::articulated_object_class
+{
+
+ Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter, std::mutex& memoryMutex) :
+ iceMemory(memoryToIceAdapter),
+ memoryMutex(memoryMutex)
+ {
+ Logging::setTag("ArticulatedObjectInstanceSegment");
+ }
+
+ Segment::~Segment() = default;
+
+ void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ defs->optional(p.coreClassSegmentName, prefix + "CoreSegmentName", "Name of the object instance core segment.");
+ defs->optional(p.maxHistorySize, prefix + "MaxHistorySize", "Maximal size of object poses history (-1 for infinite).");
+ }
+
+ void Segment::init()
+ {
+ ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
+
+ coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreClassSegmentName, arondto::Robot::toInitialAronType());
+ coreSegment->setMaxHistorySize(p.maxHistorySize);
+ }
+
+ void Segment::connect(viz::Client arviz)
+ {
+ // this->visu = std::make_unique<Visu>(arviz, *this);
+ }
+
+
+ std::unordered_map<armem::MemoryID, ::armarx::armem::articulated_object::ArticulatedObjectDescription> Segment::getKnownObjectClasses() const
+ {
+ std::unordered_map<armem::MemoryID, ::armarx::armem::articulated_object::ArticulatedObjectDescription> objects;
+
+ for (const auto& [_, provSeg] : iceMemory.workingMemory->getCoreSegment(p.coreClassSegmentName))
+ {
+ for (const auto& [name, entity] : provSeg.entities())
+ {
+ const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
+ const auto description = articulated_object::convertRobotDescription(entityInstance);
+
+ if (not description)
+ {
+ ARMARX_WARNING << "Could not convert entity instance to 'RobotDescription'";
+ continue;
+ }
+
+ ARMARX_INFO << "Key is " << armem::MemoryID(entity.id());
+
+ objects.emplace(armem::MemoryID(entity.id()), *description);
+ }
+ }
+
+ ARMARX_IMPORTANT << "Number of known articulated object classes: " << objects.size();
+
+ return objects;
+ }
+
+
+
+ // void Segment::RemoteGui::setup(const Segment& data)
+ // {
+ // using namespace armarx::RemoteGui::Client;
+
+ // maxHistorySize.setValue(std::max(1, int(data.p.maxHistorySize)));
+ // maxHistorySize.setRange(1, 1e6);
+ // infiniteHistory.setValue(data.p.maxHistorySize == -1);
+ // discardSnapshotsWhileAttached.setValue(data.p.discardSnapshotsWhileAttached);
+
+ // GridLayout grid;
+ // int row = 0;
+ // grid.add(Label("Max History Size"), {row, 0}).add(maxHistorySize, {row, 1});
+ // row++;
+ // grid.add(Label("Infinite History Size"), {row, 0}).add(infiniteHistory, {row, 1});
+ // row++;
+ // grid.add(Label("Discard Snapshots while Attached"), {row, 0}).add(discardSnapshotsWhileAttached, {row, 1});
+ // row++;
+
+ // group.setLabel("Data");
+ // group.addChild(grid);
+ // }
+
+ // void Segment::RemoteGui::update(Segment& data)
+ // {
+ // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged()
+ // || discardSnapshotsWhileAttached.hasValueChanged())
+ // {
+ // std::scoped_lock lock(data.memoryMutex);
+
+ // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
+ // {
+ // data.p.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
+ // if (data.coreSegment)
+ // {
+ // data.coreSegment->setMaxHistorySize(long(data.p.maxHistorySize));
+ // }
+ // }
+
+ // data.p.discardSnapshotsWhileAttached = discardSnapshotsWhileAttached.getValue();
+ // }
+ // }
+
+} // namespace armarx::armem::server::obj::articulated_object_class
diff --git a/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.h b/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.h
new file mode 100644
index 0000000000000000000000000000000000000000..3c17b51f3ac00db12123e3bd0b0dcb61e77c11a7
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.h
@@ -0,0 +1,105 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <string>
+#include <optional>
+#include <mutex>
+#include <unordered_map>
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include "ArmarXCore/core/application/properties/PropertyDefinitionContainer.h"
+
+// #include "ArmarXGui/libraries/RemoteGui/Client/Widgets.h"
+
+#include "RobotAPI/components/ArViz/Client/Client.h"
+
+#include "RobotAPI/libraries/armem/core/MemoryID.h"
+#include "RobotAPI/libraries/armem_objects/types.h"
+
+namespace armarx::armem
+{
+ namespace server
+ {
+ class MemoryToIceAdapter;
+ }
+
+ namespace wm
+ {
+ class CoreSegment;
+ }
+} // namespace armarx::armem
+
+
+namespace armarx::armem::server::obj::articulated_object_class
+{
+ class Visu;
+
+ class Segment : public armarx::Logging
+ {
+ public:
+ Segment(server::MemoryToIceAdapter& iceMemory,
+ std::mutex& memoryMutex);
+
+ virtual ~Segment();
+
+ void connect(viz::Client arviz);
+
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
+
+ void init();
+
+ std::unordered_map<armem::MemoryID, ::armarx::armem::articulated_object::ArticulatedObjectDescription> getKnownObjectClasses() const;
+
+
+ private:
+
+ server::MemoryToIceAdapter& iceMemory;
+ wm::CoreSegment* coreSegment = nullptr;
+ std::mutex& memoryMutex;
+
+ struct Properties
+ {
+ std::string coreClassSegmentName = "ArticulatedObjectClass";
+ int64_t maxHistorySize = -1;
+ };
+ Properties p;
+
+ // std::unique_ptr<Visu> visu;
+
+ public:
+
+ // struct RemoteGui
+ // {
+ // armarx::RemoteGui::Client::GroupBox group;
+
+ // armarx::RemoteGui::Client::IntSpinBox maxHistorySize;
+ // armarx::RemoteGui::Client::CheckBox infiniteHistory;
+ // armarx::RemoteGui::Client::CheckBox discardSnapshotsWhileAttached;
+
+ // void setup(const Segment& data);
+ // void update(Segment& data);
+ // };
+
+ };
+
+} // namespace armarx::armem::server::obj::articulated_object_class
diff --git a/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Segment.cpp
index 67623c29d02f519f812a5f9ebc65e08a6a955562..b542e7a544e45b50cf148a3318c52c9a9ba91ba7 100644
--- a/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Segment.cpp
@@ -1,449 +1,182 @@
#include "Segment.h"
-#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+#include <sstream>
+
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+
+#include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
+
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/Visitor.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/aron/MemoryID.aron.generated.h>
-#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
-#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
-#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
-#include <RobotAPI/libraries/aron/common/aron/Robot.aron.generated.h>
-
-#include <RobotAPI/libraries/core/FramedPose.h>
-#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-
-#include <ArmarXCore/core/time/TimeUtil.h>
+#include <RobotAPI/libraries/armem_objects/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron/RobotDescription.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+#include <RobotAPI/libraries/armem_objects/articulated_object_conversions.h>
+#include <RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.h>
-#include <sstream>
+#include "Visu.h"
-#include "RobotAPI/libraries/aron/common/aron_conversions.h"
+namespace simox::alg
+{
+ /// Get the keys of `map` in a vector.
+ template <class K, class V, template<class...> class MapT = std::unordered_map, class...Ts>
+ std::vector<K> get_keys2(const MapT<K, V, Ts...>& map)
+ {
+ std::vector<K> keys;
+ if constexpr(std::is_same_v<std::unordered_map<K, V, Ts...>, MapT<K, V, Ts...>>)
+ {
+ keys.reserve(map.size());
+ }
+ for (const auto& [k, v] : map)
+ {
+ keys.emplace_back(k);
+ }
+ return keys;
+ }
+}
-namespace armarx::armem::server::obj::articulated_object
+namespace armarx::armem::server::obj::articulated_object_instance
{
Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter, std::mutex& memoryMutex) :
iceMemory(memoryToIceAdapter),
memoryMutex(memoryMutex)
{
- Logging::setTag("InstanceSegment");
-
- // oobbCache.setFetchFn([this](const ObjectID & id) -> std::optional<simox::OrientedBoxf>
- // {
- // // Try to get OOBB from repository.
- // if (std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id))
- // {
- // try
- // {
- // return objectInfo->loadOOBB();
- // }
- // catch (const std::ios_base::failure& e)
- // {
- // // Give up - no OOBB information.
- // ARMARX_WARNING << "Could not get OOBB of object " << id << ".\n- " << e.what();
- // return std::nullopt;
- // }
- // }
- // else
- // {
- // return std::nullopt;
- // }
- // });
-
- // classNameToDatasetCache.setFetchFn([this](const std::string & className)
- // {
- // std::optional<ObjectInfo> objectInfo = objectFinder.findObject(className);
- // return objectInfo ? objectInfo->dataset() : "";
- // });
+ Logging::setTag("ArticulatedObjectInstanceSegment");
}
- void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ Segment::~Segment() = default;
+
+ void Segment::defineProperties(armarx::PropertyDefinitionsPtr& defs, const std::string& prefix)
{
- defs->optional(p.coreSegmentName, prefix + "CoreSegmentName", "Name of the object instance core segment.");
+ defs->optional(p.coreInstanceSegmentName, prefix + "CoreSegmentName", "Name of the object instance core segment.");
defs->optional(p.maxHistorySize, prefix + "MaxHistorySize", "Maximal size of object poses history (-1 for infinite).");
- defs->optional(p.discardSnapshotsWhileAttached, prefix + "DiscardSnapshotsWhileAttached",
- "If true, no new snapshots are stored while an object is attached to a robot node.\n"
- "If false, new snapshots are stored, but the attachment is kept in the new snapshots.");
-
- // decay.defineProperties(defs, prefix + "decay.");
}
void Segment::init()
{
ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
- coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreSegmentName, arondto::ObjectInstance::toInitialAronType());
+ coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreInstanceSegmentName, arondto::Robot::toInitialAronType());
coreSegment->setMaxHistorySize(p.maxHistorySize);
- }
+ }
- Segment::CommitStats Segment::commitArticulatedObjects(
- const std::string& providerName,
- const ::armarx::articulated_object::data::ArticulatedObjects& objects)
+ void Segment::connect(viz::Client arviz)
{
- Segment::CommitStats stats;
-
- ::armarx::articulated_object::ArticulatedObjects boObjects;
- fromIce(objects, boObjects);
-
- commitArticulatedObjects(providerName, boObjects);
+ this->visu = std::make_unique<Visu>(arviz, *this);
+ visu->init();
+ }
- return stats;
+ void Segment::setArticulatedObjectClassSegment(const articulated_object_class::Segment& segment)
+ {
+ classSegment = &segment;
}
- void Segment::commitArticulatedObjects(
- const std::string& providerName,
- const ::armarx::articulated_object::ArticulatedObjects& objects)
+ ::armarx::armem::articulated_object::ArticulatedObjects Segment::getArticulatedObjects() const
{
- ARMARX_CHECK_NOT_NULL(coreSegment);
+ ARMARX_CHECK_NOT_NULL(classSegment);
+ const auto knownObjectClasses = classSegment->getKnownObjectClasses();
- // Update memory.
- const MemoryID providerSegmentID = coreSegment->id().withProviderSegmentName(providerName);
- if (!coreSegment->hasProviderSegment(providerSegmentID.providerSegmentName))
+ const auto resolveDescriptionLink = [&](auto & articulatedObject, const auto & aronDescriptionLink) -> bool
{
- coreSegment->addProviderSegment(providerSegmentID.providerSegmentName);
- }
+ armem::MemoryID descriptionLink;
+ fromAron(aronDescriptionLink, descriptionLink);
- Commit commit;
- for (const Robot &object : objects)
- {
- EntityUpdate &update = commit.updates.emplace_back();
- {
- update.entityID = providerSegmentID.withEntityName(object.description.str());
- update.timeArrived = TimeUtil::GetTime();
- update.timeCreated = object.timestamp;
-
- armarx::arondto::Robot dto;
- toAron(dto, object);
- // Search for object class.
+ ARMARX_DEBUG << "Lookup key is " << descriptionLink;
+ const auto keys = simox::alg::get_keys(knownObjectClasses);
+ ARMARX_DEBUG << "Known keys " << keys;
- update.instancesData.push_back(dto.toAron());
- }
+ const auto it = knownObjectClasses.find(descriptionLink);
+ if (it == knownObjectClasses.end())
+ {
+ ARMARX_WARNING << "Unknown object class " ; //<< aronArticulatedObject.description;
+ return false;
}
- // TODO check if this should be inside loop
- iceMemory.commit(commit);
- }
-
-
+ articulatedObject.description = it->second;
+ return true;
+ };
- // Segment::CommitStats Segment::commitObjectPoses(
- // const std::string& providerName,
- // const objpose::data::ProvidedObjectPoseSeq& providedPoses,
- // std::optional<armem::Time> discardUpdatesUntil)
- // {
- // CommitStats stats;
-
- // // Build new poses.
- // objpose::ObjectPoseSeq newObjectPoses;
- // stats.numUpdated = 0;
- // for (const objpose::data::ProvidedObjectPose& provided : providedPoses)
- // {
- // const IceUtil::Time timestamp = IceUtil::Time::microSeconds(provided.timestampMicroSeconds);
-
- // // Check whether we have an old snapshot for this object.
- // std::optional<objpose::ObjectPose> previousPose;
- // const armem::wm::Entity* entity = findObjectEntity(armarx::fromIce(provided.objectID), providerName);
- // if (entity)
- // {
- // const arondto::ObjectInstance data = getLatestInstanceData(*entity);
-
- // previousPose = objpose::ObjectPose();
- // fromAron(data, *previousPose);
- // }
-
- // bool discard = false;
- // if (discardUpdatesUntil && timestamp < discardUpdatesUntil.value())
- // {
- // // Dicard updates temporarily (e.g. due to head movement).
- // discard = true;
- // }
- // else if (previousPose)
- // {
- // if (p.discardSnapshotsWhileAttached && previousPose->attachment)
- // {
- // // Discard update due to active attachemnt.
- // discard = true;
- // }
- // else if (timestamp == previousPose->timestamp)
- // {
- // // Discard update as it is not new.
- // discard = true;
- // }
- // }
-
- // if (!discard)
- // {
- // // Update the entity.
- // stats.numUpdated++;
-
- // objpose::ObjectPose& newPose = newObjectPoses.emplace_back();
- // newPose.fromProvidedPose(provided, robot);
-
- // if (previousPose && previousPose->attachment)
- // {
- // // Keep current attachment.
- // ARMARX_CHECK(!p.discardSnapshotsWhileAttached);
- // newPose.attachment = previousPose->attachment;
- // }
-
- // if (newPose.objectID.dataset().empty())
- // {
- // // Try to find the data set.
- // const std::string dataset = classNameToDatasetCache.get(newPose.objectID.className());
- // if (!dataset.empty())
- // {
- // newPose.objectID = { dataset, newPose.objectID.className(), newPose.objectID.instanceName() };
- // }
- // }
- // if (!provided.localOOBB)
- // {
- // // Try to load oobb from disk.
- // newPose.localOOBB = getObjectOOBB(newPose.objectID);
- // }
- // }
- // }
-
- // commitObjectPoses(providerName, newObjectPoses);
-
- // return stats;
- // }
-
- // void Segment::commitObjectPoses(const std::string& providerName, const ObjectPoseSeq& objectPoses)
- // {
- // ARMARX_CHECK_NOT_NULL(coreSegment);
-
- // // Update memory.
- // const MemoryID providerSegmentID = coreSegment->id().withProviderSegmentName(providerName);
- // if (!coreSegment->hasProviderSegment(providerSegmentID.providerSegmentName))
- // {
- // coreSegment->addProviderSegment(providerSegmentID.providerSegmentName);
- // }
+ ::armarx::armem::articulated_object::ArticulatedObjects objects;
+ for (const auto& [_, provSeg] : iceMemory.workingMemory->getCoreSegment(p.coreInstanceSegmentName))
+ {
+ for (const auto& [_, entity] : provSeg.entities())
+ {
+ const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
- // Commit commit;
- // for (const objpose::ObjectPose& pose : objectPoses)
- // {
- // EntityUpdate& update = commit.updates.emplace_back();
- // {
- // update.entityID = providerSegmentID.withEntityName(pose.objectID.str());
- // update.timeArrived = TimeUtil::GetTime();
- // update.timeCreated = pose.timestamp;
- // update.confidence = pose.confidence;
-
- // arondto::ObjectInstance dto;
- // toAron(dto, pose);
- // // Search for object class.
- // if (auto instance = findClassInstance(pose.objectID))
- // {
- // toAron(dto.classID, instance->id());
- // }
- // update.instancesData.push_back(dto.toAron());
- // }
+ arondto::Robot aronArticulatedObject;
+ aronArticulatedObject.fromAron(entityInstance.data());
- // iceMemory.commit(commit);
- // }
- // }
+ armem::articulated_object::ArticulatedObject articulatedObject;
+ fromAron(aronArticulatedObject, articulatedObject);
+ // resolve memory link for description
+ const arondto::MemoryID& aronDescriptionLink = aronArticulatedObject.description;
+ if (not resolveDescriptionLink(articulatedObject, aronDescriptionLink))
+ {
+ continue;
+ }
- wm::CoreSegment& Segment::getCoreSegment()
- {
- ARMARX_CHECK_NOT_NULL(coreSegment);
- return *coreSegment;
- }
+ objects.push_back(articulatedObject);
+ }
+ }
- const wm::CoreSegment& Segment::getCoreSegment() const
- {
- ARMARX_CHECK_NOT_NULL(coreSegment);
- return *coreSegment;
+ return objects;
}
- // objpose::ObjectPoseSeq Segment::getObjectPoses(IceUtil::Time now)
- // {
- // ObjectPoseSeq objectPoses = getLatestObjectPoses();
- // updateObjectPoses(objectPoses, now);
- // return filterObjectPoses(objectPoses);
- // }
- // objpose::ObjectPoseSeq Segment::getObjectPosesByProvider(
- // const std::string& providerName,
- // IceUtil::Time now)
+ // void Segment::RemoteGui::setup(const Segment& data)
// {
- // ARMARX_CHECK_NOT_NULL(coreSegment);
- // ObjectPoseSeq objectPoses = getLatestObjectPoses(coreSegment->getProviderSegment(providerName));
- // updateObjectPoses(objectPoses, now);
- // return filterObjectPoses(objectPoses);
+ // using namespace armarx::RemoteGui::Client;
+
+ // maxHistorySize.setValue(std::max(1, int(data.p.maxHistorySize)));
+ // maxHistorySize.setRange(1, 1e6);
+ // infiniteHistory.setValue(data.p.maxHistorySize == -1);
+ // discardSnapshotsWhileAttached.setValue(data.p.discardSnapshotsWhileAttached);
+
+ // GridLayout grid;
+ // int row = 0;
+ // grid.add(Label("Max History Size"), {row, 0}).add(maxHistorySize, {row, 1});
+ // row++;
+ // grid.add(Label("Infinite History Size"), {row, 0}).add(infiniteHistory, {row, 1});
+ // row++;
+ // grid.add(Label("Discard Snapshots while Attached"), {row, 0}).add(discardSnapshotsWhileAttached, {row, 1});
+ // row++;
+
+ // group.setLabel("Data");
+ // group.addChild(grid);
// }
- // armem::wm::Entity* Segment::findObjectEntity(const ObjectID& objectID, const std::string& providerName)
+ // void Segment::RemoteGui::update(Segment& data)
// {
- // ARMARX_CHECK_NOT_NULL(coreSegment);
- // armem::MemoryID entityID = armem::MemoryID().withEntityName(objectID.str());
- // if (providerName.empty())
+ // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged()
+ // || discardSnapshotsWhileAttached.hasValueChanged())
// {
- // for (auto& [_, prov] : *coreSegment)
+ // std::scoped_lock lock(data.memoryMutex);
+
+ // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
// {
- // if (prov.hasEntity(entityID.entityName))
+ // data.p.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
+ // if (data.coreSegment)
// {
- // return &prov.getEntity(entityID);
+ // data.coreSegment->setMaxHistorySize(long(data.p.maxHistorySize));
// }
// }
- // return nullptr;
- // }
- // else
- // {
- // entityID.providerSegmentName = providerName;
- // if (coreSegment->hasProviderSegment(providerName))
- // {
- // armem::wm::ProviderSegment& prov = coreSegment->getProviderSegment(providerName);
- // return prov.hasEntity(entityID.entityName) ? &prov.getEntity(entityID) : nullptr;
- // }
- // else
- // {
- // return nullptr;
- // }
- // }
- // }
-
- // void Segment::updateObjectPoses(ObjectPoseSeq& objectPoses, IceUtil::Time now)
- // {
- // bool agentSynchronized = false;
-
- // for (ObjectPose& objectPose : objectPoses)
- // {
- // updateObjectPose(objectPose, now, robot, agentSynchronized);
+ // data.p.discardSnapshotsWhileAttached = discardSnapshotsWhileAttached.getValue();
// }
// }
-
- // void Segment::updateObjectPoses(
- // ObjectPoseSeq& objectPoses,
- // IceUtil::Time now,
- // VirtualRobot::RobotPtr agent,
- // bool& agentSynchronized) const
- // {
- // for (ObjectPose& pose : objectPoses)
- // {
- // updateObjectPose(pose, now, agent, agentSynchronized);
- // }
- // }
-
-
- // void Segment::updateObjectPose(
- // ObjectPose& objectPose,
- // IceUtil::Time now,
- // VirtualRobot::RobotPtr agent,
- // bool& agentSynchronized) const
- // {
- // updateAttachement(objectPose, agent, agentSynchronized);
-
- // if (decay.enabled)
- // {
- // decay.updateConfidence(objectPose, now);
- // }
- // }
-
-
- // objpose::ObjectPoseSeq Segment::filterObjectPoses(const ObjectPoseSeq& objectPoses) const
- // {
- // ObjectPoseSeq result;
- // for (const ObjectPose& objectPose : objectPoses)
- // {
- // if (!(decay.enabled && objectPose.confidence < decay.removeObjectsBelowConfidence))
- // {
- // result.push_back(objectPose);
- // }
- // }
- // return result;
- // }
-
-
-
- ::armarx::articulated_object::provider::ProviderInfo Segment::getProviderInfo(const std::string& providerName)
- {
- try
- {
- return providers.at(providerName);
- }
- catch (const std::out_of_range&)
- {
- std::stringstream ss;
- ss << "No provider with name '" << providerName << "' available.\n";
- ss << "Available are:\n";
- for (const auto& [name, _] : providers)
- {
- ss << "- '" << name << "'\n";
- }
- throw std::out_of_range(ss.str());
- }
- }
-
-
- std::optional<wm::EntityInstance> Segment::findClassInstance(const ObjectID& objectID)
- {
- const ObjectID classID = { objectID.dataset(), objectID.className() };
- try
- {
- for (const auto& [_, provSeg] : iceMemory.workingMemory->getCoreSegment("Class"))
- {
- return provSeg.getEntity(classID.str()).getLatestSnapshot().getInstance(0);
- }
- return std::nullopt;
- }
- catch (const armem::error::ArMemError&)
- {
- // Some segment or entity did not exist.
- return std::nullopt;
- }
- }
-
-
-
- void Segment::RemoteGui::setup(const Segment& data)
- {
- using namespace armarx::RemoteGui::Client;
-
- maxHistorySize.setValue(std::max(1, int(data.p.maxHistorySize)));
- maxHistorySize.setRange(1, 1e6);
- infiniteHistory.setValue(data.p.maxHistorySize == -1);
- discardSnapshotsWhileAttached.setValue(data.p.discardSnapshotsWhileAttached);
-
- GridLayout grid;
- int row = 0;
- grid.add(Label("Max History Size"), {row, 0}).add(maxHistorySize, {row, 1});
- row++;
- grid.add(Label("Infinite History Size"), {row, 0}).add(infiniteHistory, {row, 1});
- row++;
- grid.add(Label("Discard Snapshots while Attached"), {row, 0}).add(discardSnapshotsWhileAttached, {row, 1});
- row++;
-
- group.setLabel("Data");
- group.addChild(grid);
- }
-
- void Segment::RemoteGui::update(Segment& data)
- {
- if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged()
- || discardSnapshotsWhileAttached.hasValueChanged())
- {
- std::scoped_lock lock(data.memoryMutex);
-
- if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
- {
- data.p.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
- if (data.coreSegment)
- {
- data.coreSegment->setMaxHistorySize(long(data.p.maxHistorySize));
- }
- }
-
- data.p.discardSnapshotsWhileAttached = discardSnapshotsWhileAttached.getValue();
- }
- }
-
-}
+} // namespace armarx::armem::server::obj::articulated_object_instance
diff --git a/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Segment.h b/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Segment.h
index caa3c3ff2d3009fda7343b051c8536f22cea1e49..2d50be817effddb795e8cd6adf1bda992d177e87 100644
--- a/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Segment.h
+++ b/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Segment.h
@@ -1,33 +1,54 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
#pragma once
-#include "ArmarXCore/core/application/properties/PropertyDefinitionContainer.h"
-#include "ArmarXGui/libraries/RemoteGui/Client/Widgets.h"
-#include <RobotAPI/interface/core/articulated_object/provider.h>
+#include <cstdint>
#include <map>
#include <string>
#include <optional>
#include <mutex>
-#include <SimoxUtility/caching/CacheMap.h>
-#include <SimoxUtility/shapes/OrientedBox.h>
+#include "ArmarXCore/core/application/properties/PropertyDefinitionContainer.h"
-#include <ArmarXCore/core/logging/Logging.h>
+#include "ArmarXGui/libraries/RemoteGui/Client/Widgets.h"
#include <RobotAPI/interface/core/RobotState.h>
-#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
-#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
-#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
-
#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
-#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
-#include <RobotAPI/libraries/aron/common/aron/new_types.h>
-#include <RobotAPI/interface/core/articulated_object/types.h>
+#include "RobotAPI/components/ArViz/Client/Client.h"
+
+#include "RobotAPI/libraries/armem_objects/types.h"
-namespace armarx::armem::server::obj::articulated_object
+
+namespace armarx::armem::server::obj::articulated_object_class
+{
+ class Segment;
+}
+
+namespace armarx::armem::server::obj::articulated_object_instance
{
+ class Visu;
class Segment : public armarx::Logging
{
@@ -37,106 +58,22 @@ namespace armarx::armem::server::obj::articulated_object
{
int numUpdated = 0;
};
- using ObjectPose = objpose::ObjectPose;
- using ObjectPoseSeq = objpose::ObjectPoseSeq;
- public:
-
Segment(server::MemoryToIceAdapter& iceMemory,
std::mutex& memoryMutex);
+ virtual ~Segment();
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
-
- void init();
-
-
- CommitStats commitArticulatedObjects(
- const std::string& providerName,
- const ::armarx::articulated_object::data::ArticulatedObjects& objects);
-
- void commitArticulatedObjects(
- const std::string& providerName,
- const ::armarx::articulated_object::ArticulatedObjects& objects
- );
-
- wm::CoreSegment& getCoreSegment();
- const wm::CoreSegment& getCoreSegment() const;
-
-
- // objpose::ObjectPoseSeq getObjectPoses(IceUtil::Time now);
- // objpose::ObjectPoseSeq getObjectPosesByProvider(const std::string& providerName, IceUtil::Time now);
+ void connect(viz::Client arviz);
- // wm::Entity* findObjectEntity(const ObjectID& objectID, const std::string& providerName = "");
- // std::optional<simox::OrientedBoxf> getObjectOOBB(const ObjectID& id);
+ void defineProperties(armarx::PropertyDefinitionsPtr& defs, const std::string& prefix = "");
- ::armarx::articulated_object::provider::ProviderInfo getProviderInfo(const std::string& providerName);
-
-
- // static ObjectPoseSeq getLatestObjectPoses(const wm::CoreSegment& coreSeg);
- // static ObjectPoseSeq getLatestObjectPoses(const wm::ProviderSegment& provSeg);
- // static ObjectPose getLatestObjectPose(const wm::Entity& entity);
-
- // static void getLatestObjectPoses(const wm::CoreSegment& coreSeg, ObjectPoseSeq& out);
- // static void getLatestObjectPoses(const wm::ProviderSegment& provSeg, ObjectPoseSeq& out);
- // static void getLatestObjectPose(const wm::Entity& entity, ObjectPose& out);
-
- // static arondto::ObjectInstance getLatestInstanceData(const wm::Entity& entity);
-
-
- private:
-
- ObjectPoseSeq getLatestObjectPoses() const;
-
- void updateObjectPoses(
- ObjectPoseSeq& objectPoses,
- IceUtil::Time now);
- void updateObjectPoses(
- ObjectPoseSeq& objectPoses,
- IceUtil::Time now,
- VirtualRobot::RobotPtr agent,
- bool& agentSynchronized
- ) const;
- void updateObjectPose(
- ObjectPose& objectPose,
- IceUtil::Time now,
- VirtualRobot::RobotPtr agent,
- bool& agentSynchronized
- ) const;
-
-
- ObjectPoseSeq filterObjectPoses(const ObjectPoseSeq& objectPoses) const;
-
-
- void storeDetachedSnapshot(
- wm::Entity& entity,
- const arondto::ObjectInstance& data,
- Time now,
- bool commitAttachedPose);
-
-
- std::optional<wm::EntityInstance> findClassInstance(const ObjectID& objectID);
-
-
- friend struct DetachVisitor;
-
-
- public:
-
- RobotStateComponentInterfacePrx robotStateComponent;
- VirtualRobot::RobotPtr robot;
-
- using ProviderMap = std::map<std::string, ::armarx::articulated_object::provider::ProviderInfo>;
-
- ProviderMap providers;
-
-
- // ObjectFinder objectFinder;
+ void init();
- /// Decay model.
- // Decay decay;
+ void setArticulatedObjectClassSegment(const articulated_object_class::Segment& segment);
+ ::armarx::armem::articulated_object::ArticulatedObjects getArticulatedObjects() const;
private:
@@ -144,37 +81,32 @@ namespace armarx::armem::server::obj::articulated_object
wm::CoreSegment* coreSegment = nullptr;
std::mutex& memoryMutex;
+ articulated_object_class::Segment const* classSegment;
+
struct Properties
{
- std::string coreSegmentName = "ArticulatedObjectInstance";
- long maxHistorySize = -1;
- bool discardSnapshotsWhileAttached = true;
+ std::string coreInstanceSegmentName = "ArticulatedObjectInstance";
+ int64_t maxHistorySize = -1;
};
Properties p;
-
- /// Caches results of attempts to retrieve the OOBB from ArmarXObjects.
- simox::caching::CacheMap<ObjectID, std::optional<simox::OrientedBoxf>> oobbCache;
-
- /// Class name -> dataset name.
- simox::caching::CacheMap<std::string, std::string> classNameToDatasetCache;
-
+ std::unique_ptr<Visu> visu;
public:
- struct RemoteGui
- {
- armarx::RemoteGui::Client::GroupBox group;
+ // struct RemoteGui
+ // {
+ // armarx::RemoteGui::Client::GroupBox group;
- armarx::RemoteGui::Client::IntSpinBox maxHistorySize;
- armarx::RemoteGui::Client::CheckBox infiniteHistory;
- armarx::RemoteGui::Client::CheckBox discardSnapshotsWhileAttached;
+ // armarx::RemoteGui::Client::IntSpinBox maxHistorySize;
+ // armarx::RemoteGui::Client::CheckBox infiniteHistory;
+ // armarx::RemoteGui::Client::CheckBox discardSnapshotsWhileAttached;
- void setup(const Segment& data);
- void update(Segment& data);
- };
+ // void setup(const Segment& data);
+ // void update(Segment& data);
+ // };
};
-} // namespace armarx::armem::server::obj::articulated_object
+} // namespace armarx::armem::server::obj::articulated_object_instance
diff --git a/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/SegmentAdapter.cpp b/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/SegmentAdapter.cpp
deleted file mode 100644
index 622b358cbffcd51b752185ab81654ade9725de93..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/SegmentAdapter.cpp
+++ /dev/null
@@ -1,512 +0,0 @@
-/*
- * 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::armem_objects::SegmentAdapter
- * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
- * @date 2021
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include "SegmentAdapter.h"
-
-#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
-#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
-
-#include <ArmarXCore/core/time/CycleUtil.h>
-#include <ArmarXCore/observers/variant/Variant.h>
-
-#include <VirtualRobot/Robot.h>
-
-#include <SimoxUtility/algorithm/get_map_keys_values.h>
-
-
-namespace armarx::armem::server::obj::instance
-{
-
- SegmentAdapter::SegmentAdapter(MemoryToIceAdapter& iceMemory, std::mutex& memoryMutex) :
- segment(iceMemory, memoryMutex),
- memoryMutex(memoryMutex)
- {
- }
-
- std::string SegmentAdapter::getName() const
- {
- return Logging::tag.tagName;
- }
-
- void SegmentAdapter::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
- {
- calibration.defineProperties(defs, prefix + "calibration.");
- segment.defineProperties(defs, prefix);
- robotHead.defineProperties(defs, prefix + "head.");
- visu.defineProperties(defs, prefix + "visu.");
- }
-
- void SegmentAdapter::init()
- {
- segment.setTag(getName());
- segment.decay.setTag(getName());
- robotHead.setTag(getName());
- visu.setTag(getName());
-
- segment.init();
- }
-
- void SegmentAdapter::connect(
- RobotStateComponentInterfacePrx robotStateComponent,
- VirtualRobot::RobotPtr robot,
- KinematicUnitObserverInterfacePrx kinematicUnitObserver,
- viz::Client arviz,
- DebugObserverInterfacePrx debugObserver
- )
- {
- this->debugObserver = debugObserver;
- this->arviz = arviz;
-
- segment.robot = robot;
- segment.robotStateComponent = robotStateComponent;
-
- robotHead.kinematicUnitObserver = kinematicUnitObserver;
- robotHead.debugObserver = debugObserver;
- robotHead.fetchDatafields();
-
- visu.arviz = arviz;
- if (!visu.updateTask)
- {
- visu.updateTask = new SimpleRunningTask<>([this]()
- {
- this->visualizeRun();
- });
- visu.updateTask->start();
- }
- }
-
- void SegmentAdapter::reportProviderAvailable(const std::string& providerName, const objpose::ProviderInfo& info, const Ice::Current&)
- {
- updateProviderInfo(providerName, info);
- }
-
-
- void SegmentAdapter::reportObjectPoses(
- const std::string& providerName, const objpose::data::ProvidedObjectPoseSeq& providedPoses, const Ice::Current&)
- {
- ARMARX_VERBOSE << "Received object " << providedPoses.size() << " poses from provider '" << providerName << "'.";
- updateObjectPoses(providerName, providedPoses);
- }
-
-
-
- void SegmentAdapter::reportArticulatedObjects(
- const std::string& providerName, const articulated_object::data::ArticulatedObjects& objects, const Ice::Current&)
- {
- ARMARX_INFO << "Received articulated objects " << objects.size() << " poses from provider '" << providerName << "'.";
- updateArticulatedObjects(providerName, objects);
- }
-
- void SegmentAdapter::updateProviderInfo(const std::string& providerName, const objpose::ProviderInfo& info)
- {
- if (!info.proxy)
- {
- ARMARX_WARNING << "Received availability signal by provider '" << providerName << "' "
- << "with invalid provider proxy.\nIgnoring provider '" << providerName << "'.";
- return;
- }
- {
- std::scoped_lock lock(memoryMutex);
- std::stringstream ss;
- for (const auto& id : info.supportedObjects)
- {
- ss << "- " << id << "\n";
- }
- ARMARX_VERBOSE << "Provider '" << providerName << "' available.\n"
- << "Supported objects: \n" << ss.str();
- segment.providers[providerName] = info;
- }
- }
-
- void SegmentAdapter::updateArticulatedObjects(
- const std::string& providerName, const articulated_object::data::ArticulatedObjects& objects)
- {
-
-
- }
-
- void SegmentAdapter::updateObjectPoses(
- const std::string& providerName,
- const objpose::data::ProvidedObjectPoseSeq& providedPoses)
- {
- TIMING_START(tReportObjectPoses);
-
- RobotHeadMovement::Discard discard;
- {
- std::scoped_lock lock(robotHeadMutex);
- discard = robotHead.getDiscard();
- }
- if (debugObserver)
- {
- StringVariantBaseMap map;
- map["Discarding All Updates"] = new Variant(discard.all ? 1.f : 0.f);
- if (discard.all)
- {
- map["Proportion Updated Poses"] = new Variant(0.f);
- }
- debugObserver->setDebugChannel(getName(), map);
- }
-
- if (discard.all)
- {
- return;
- }
-
- {
- std::scoped_lock lock(memoryMutex);
- RemoteRobot::synchronizeLocalClone(segment.robot, segment.robotStateComponent);
-
- if (segment.robot->hasRobotNode(calibration.robotNode))
- {
- VirtualRobot::RobotNodePtr robotNode = segment.robot->getRobotNode(calibration.robotNode);
- float value = robotNode->getJointValue();
- robotNode->setJointValue(value + calibration.offset);
- }
-
- TIMING_START(tCommitObjectPoses);
- Segment::CommitStats stats =
- segment.commitObjectPoses(providerName, providedPoses, discard.updatesUntil);
- TIMING_END_STREAM(tCommitObjectPoses, ARMARX_VERBOSE);
-
- if (debugObserver)
- {
- debugObserver->setDebugChannel(getName(),
- {
- { "Discarding All Updates", new Variant(discard.all ? 1 : 0) },
- { "Proportion Updated Poses", new Variant(static_cast<float>(stats.numUpdated) / providedPoses.size()) }
- });
- }
-
- handleProviderUpdate(providerName);
-
- TIMING_END_STREAM(tReportObjectPoses, ARMARX_VERBOSE);
- if (debugObserver)
- {
- debugObserver->setDebugChannel(getName(),
- {
- { "t ReportObjectPoses [ms]", new Variant(tReportObjectPoses.toMilliSecondsDouble()) },
- { "t MemorySetObjectPoses [ms]", new Variant(tCommitObjectPoses.toMilliSecondsDouble()) },
- });
- }
- }
- }
-
-
- void SegmentAdapter::handleProviderUpdate(const std::string& providerName)
- {
- // Initialized to 0 on first access.
- if (segment.providers.count(providerName) == 0)
- {
- segment.providers[providerName] = objpose::ProviderInfo();
- }
- }
-
-
- objpose::data::ObjectPoseSeq SegmentAdapter::getObjectPoses(const Ice::Current&)
- {
- TIMING_START(tGetObjectPoses);
-
- TIMING_START(tGetObjectPosesLock);
- std::scoped_lock lock(memoryMutex);
- TIMING_END_STREAM(tGetObjectPosesLock, ARMARX_VERBOSE);
-
- const IceUtil::Time now = TimeUtil::GetTime();
- const objpose::data::ObjectPoseSeq result = objpose::toIce(segment.getObjectPoses(now));
-
- TIMING_END_STREAM(tGetObjectPoses, ARMARX_VERBOSE);
-
- if (debugObserver)
- {
- debugObserver->setDebugChannel(getName(),
- {
- { "t GetObjectPoses() [ms]", new Variant(tGetObjectPoses.toMilliSecondsDouble()) },
- { "t GetObjectPoses() lock [ms]", new Variant(tGetObjectPosesLock.toMilliSecondsDouble()) }
- });
- }
-
- return result;
- }
-
- objpose::data::ObjectPoseSeq SegmentAdapter::getObjectPosesByProvider(const std::string& providerName, const Ice::Current&)
- {
- TIMING_START(GetObjectPoses);
-
- TIMING_START(GetObjectPosesLock);
- std::scoped_lock lock(memoryMutex);
- TIMING_END_STREAM(GetObjectPosesLock, ARMARX_VERBOSE);
-
- const IceUtil::Time now = TimeUtil::GetTime();
- const objpose::data::ObjectPoseSeq result = objpose::toIce(segment.getObjectPosesByProvider(providerName, now));
-
- TIMING_END_STREAM(GetObjectPoses, ARMARX_VERBOSE);
-
- if (debugObserver)
- {
- debugObserver->setDebugChannel(getName(),
- {
- { "t GetObjectPosesByProvider() [ms]", new Variant(GetObjectPoses.toMilliSecondsDouble()) },
- { "t GetObjectPosesByProvider() lock [ms]", new Variant(GetObjectPosesLock.toMilliSecondsDouble()) }
- });
- }
-
- return result;
- }
-
-
- objpose::observer::RequestObjectsOutput SegmentAdapter::requestObjects(
- const objpose::observer::RequestObjectsInput& input, const Ice::Current&)
- {
- std::map<std::string, objpose::provider::RequestObjectsInput> providerRequests;
- std::map<std::string, objpose::ObjectPoseProviderPrx> proxies;
-
- objpose::observer::RequestObjectsOutput output;
-
- auto updateProxy = [&](const std::string & providerName)
- {
- if (proxies.count(providerName) == 0)
- {
- if (auto it = segment.providers.find(providerName); it != segment.providers.end())
- {
- proxies[providerName] = it->second.proxy;
- }
- else
- {
- ARMARX_ERROR << "No proxy for provider ' " << providerName << "'.";
- proxies[providerName] = nullptr;
- }
- }
- };
-
- if (input.provider.size() > 0)
- {
- providerRequests[input.provider] = input.request;
- updateProxy(input.provider);
- }
- else
- {
- std::scoped_lock lock(memoryMutex);
- for (const auto& objectID : input.request.objectIDs)
- {
- bool found = true;
- for (const auto& [providerName, info] : segment.providers)
- {
- // ToDo: optimize look up.
- if (std::find(info.supportedObjects.begin(), info.supportedObjects.end(), objectID) != info.supportedObjects.end())
- {
- providerRequests[providerName].objectIDs.push_back(objectID);
- updateProxy(providerName);
- break;
- }
- }
- if (!found)
- {
- ARMARX_ERROR << "Did not find a provider for " << objectID << ".";
- output.results[objectID].providerName = "";
- }
- }
- }
-
- for (const auto& [providerName, request] : providerRequests)
- {
- if (objpose::ObjectPoseProviderPrx proxy = proxies.at(providerName); proxy)
- {
- ARMARX_INFO << "Requesting " << request.objectIDs.size() << " objects by provider '"
- << providerName << "' for " << request.relativeTimeoutMS << " ms.";
- objpose::provider::RequestObjectsOutput providerOutput = proxy->requestObjects(request);
-
- int successful = 0;
- for (const auto& [objectID, result] : providerOutput.results)
- {
- objpose::observer::ObjectRequestResult& res = output.results[objectID];
- res.providerName = providerName;
- res.result = result;
- successful += int(result.success);
- }
- ARMARX_INFO << successful << " of " << request.objectIDs.size() << " object requests successful.";
- }
- }
- return output;
- }
-
- objpose::ProviderInfoMap SegmentAdapter::getAvailableProvidersInfo(const Ice::Current&)
- {
- std::scoped_lock lock(memoryMutex);
- return segment.providers;
- }
-
- Ice::StringSeq SegmentAdapter::getAvailableProviderNames(const Ice::Current&)
- {
- std::scoped_lock lock(memoryMutex);
- return simox::alg::get_keys(segment.providers);
- }
-
- objpose::ProviderInfo SegmentAdapter::getProviderInfo(const std::string& providerName, const Ice::Current&)
- {
- std::scoped_lock lock(memoryMutex);
- return segment.getProviderInfo(providerName);
- }
-
- bool SegmentAdapter::hasProvider(const std::string& providerName, const Ice::Current&)
- {
- std::scoped_lock lock(memoryMutex);
- return segment.providers.count(providerName) > 0;
- }
-
-
- objpose::AttachObjectToRobotNodeOutput SegmentAdapter::attachObjectToRobotNode(
- const objpose::AttachObjectToRobotNodeInput& input, const Ice::Current&)
- {
- std::scoped_lock lock(memoryMutex);
- return segment.attachObjectToRobotNode(input);
- }
-
- objpose::DetachObjectFromRobotNodeOutput SegmentAdapter::detachObjectFromRobotNode(
- const objpose::DetachObjectFromRobotNodeInput& input, const Ice::Current&)
- {
- std::scoped_lock lock(memoryMutex);
- return segment.detachObjectFromRobotNode(input);
- }
-
- objpose::DetachAllObjectsFromRobotNodesOutput SegmentAdapter::detachAllObjectsFromRobotNodes(
- const objpose::DetachAllObjectsFromRobotNodesInput& input, const Ice::Current&)
- {
- std::scoped_lock lock(memoryMutex);
- return segment.detachAllObjectsFromRobotNodes(input);
- }
-
-
- objpose::AgentFramesSeq SegmentAdapter::getAttachableFrames(const Ice::Current&)
- {
- std::scoped_lock lock(memoryMutex);
-
- objpose::AgentFramesSeq output;
- std::vector<VirtualRobot::RobotPtr> agents = { segment.robot };
- for (VirtualRobot::RobotPtr agent : agents)
- {
- objpose::AgentFrames& frames = output.emplace_back();
- frames.agent = agent->getName();
- frames.frames = agent->getRobotNodeNames();
- }
- return output;
- }
-
- objpose::SignalHeadMovementOutput
- SegmentAdapter::signalHeadMovement(const objpose::SignalHeadMovementInput& input, const Ice::Current&)
- {
- std::scoped_lock lock(robotHeadMutex);
- return robotHead.signalHeadMovement(input);
- }
-
-
- void SegmentAdapter::visualizeRun()
- {
- CycleUtil cycle(static_cast<int>(1000 / visu.frequencyHz));
- while (visu.updateTask && !visu.updateTask->isStopped())
- {
- {
- std::scoped_lock lock(visuMutex);
-
- if (visu.enabled)
- {
- TIMING_START(Visu);
-
- objpose::ObjectPoseSeq objectPoses;
- ObjectFinder objectFinder;
- visu.minConfidence = -1;
- {
- std::scoped_lock lock(memoryMutex);
-
- const IceUtil::Time now = TimeUtil::GetTime();
- objectPoses = segment.getObjectPoses(now);
- objectFinder = segment.objectFinder;
- if (segment.decay.enabled)
- {
- visu.minConfidence = segment.decay.removeObjectsBelowConfidence;
- }
- }
- const std::vector<viz::Layer> layers = visu.visualizeCommit(objectPoses, objectFinder);
- arviz.commit(layers);
-
- TIMING_END_STREAM(Visu, ARMARX_VERBOSE);
-
- if (debugObserver)
- {
- debugObserver->setDebugChannel(getName(),
- {
- { "t Visualize [ms]", new Variant(Visu.toMilliSecondsDouble()) },
- });
- }
- }
- }
- cycle.waitForCycleDuration();
- }
- }
-
-
- void SegmentAdapter::Calibration::defineProperties(PropertyDefinitionsPtr defs, const std::string& prefix)
- {
- defs->optional(robotNode, prefix + "robotNode", "Robot node which can be calibrated.");
- defs->optional(offset, prefix + "offset", "Offset for the node to be calibrated.");
- }
-
-
-
- void SegmentAdapter::RemoteGui::setup(const SegmentAdapter& adapter)
- {
- using namespace armarx::RemoteGui::Client;
-
- this->visu.setup(adapter.visu);
- this->segment.setup(adapter.segment);
- this->decay.setup(adapter.segment.decay);
- this->robotHead.setup(adapter.robotHead);
-
- layout = VBoxLayout
- {
- this->visu.group, this->segment.group, this->decay.group, this->robotHead.group,
- VSpacer()
- };
-
- group.setLabel("Instance");
- group.addChild(layout);
- }
-
- void SegmentAdapter::RemoteGui::update(SegmentAdapter& adapter)
- {
- // Non-atomic variables need to be guarded by a mutex if accessed by multiple threads
- {
- std::scoped_lock lock(adapter.visuMutex);
- this->visu.update(adapter.visu);
- }
- {
- std::scoped_lock lock(adapter.memoryMutex);
- this->segment.update(adapter.segment);
- this->decay.update(adapter.segment.decay);
- }
- {
- std::scoped_lock lock(adapter.robotHeadMutex);
- this->robotHead.update(adapter.robotHead);
- }
- }
-
-}
-
diff --git a/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/SegmentAdapter.h b/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/SegmentAdapter.h
deleted file mode 100644
index bf152eb74ecdd7961db56a34bfb9df1855d9e5a9..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/SegmentAdapter.h
+++ /dev/null
@@ -1,117 +0,0 @@
-/*
- * 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::armem_objects::Adapter
- * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
- * @date 2021
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include <RobotAPI/interface/core/articulated_object/types.h>
-#include <mutex>
-
-#include <VirtualRobot/VirtualRobot.h>
-
-#include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
-
-#include <RobotAPI/interface/armem/server/ObjectMemoryInterface.h>
-#include <RobotAPI/interface/core/RobotState.h>
-
-#include <RobotAPI/components/ArViz/Client/Client.h>
-
-#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
-#include <RobotAPI/libraries/armem_objects/server/articulated_object/Segment.h>
-// #include <RobotAPI/libraries/armem_objects/server/articulated_object/Decay.h>
-// #include <RobotAPI/libraries/armem_objects/server/articulated_object/Visu.h>
-// #include <RobotAPI/libraries/armem_objects/server/articulated_object/RobotHeadMovement.h>
-
-
-#define ICE_CURRENT_ARG const Ice::Current& = Ice::emptyCurrent
-
-
-namespace armarx::armem::server::obj::articulated_object
-{
-
- /**
- * @brief Helps implementing the `armarx::armem::server::ObjectInstanceSegmentInterface`.
- */
- class SegmentAdapter :
- virtual public armarx::Logging
- // , virtual public armarx::armem::server::ArticulatedObjectInstanceSegment
- {
- public:
-
- SegmentAdapter(MemoryToIceAdapter& iceMemory, std::mutex& memoryMutex);
-
- std::string getName() const;
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
-
- void init();
- void connect(
- viz::Client arviz,
- DebugObserverInterfacePrx debugObserver
- );
-
- // ARTICULATED OBJECTS
- void reportArticulatedObjects(const std::string& providerName, const ::armarx::articulated_object::data::ArticulatedObjects& objects, ICE_CURRENT_ARG) override;
-
- private:
-
- void updateProviderInfo(const std::string& providerName, const objpose::ProviderInfo& info);
-
- void updateObjectPoses(const std::string& providerName, const objpose::data::ProvidedObjectPoseSeq& providedPoses);
- void handleProviderUpdate(const std::string& providerName);
-
-
- void updateArticulatedObjects(const std::string& providerName, const ::armarx::articulated_object::data::ArticulatedObjects& objects);
-
- // Visualization
-
- void visualizeRun();
-
-
- private:
-
- viz::Client arviz;
- DebugObserverInterfacePrx debugObserver;
-
- articulated_object::Segment segment;
- std::mutex& memoryMutex;
-
- // articulated_object::Visu visu;
- // std::mutex visuMutex;
-
- public:
-
- struct RemoteGui
- {
- armarx::RemoteGui::Client::GroupBox group;
- armarx::RemoteGui::Client::VBoxLayout layout;
-
- // articulated_object::Visu::RemoteGui visu;
- articulated_object::Segment::RemoteGui segment;
-
- void setup(const SegmentAdapter& adapter);
- void update(SegmentAdapter& adapter);
- };
-
- };
-
-}
-
-#undef ICE_CURRENT_ARG
diff --git a/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.cpp b/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.cpp
index 40ac1f359ecbb274180de2b61265e8c442420d92..385866ce9453fd8db576da3f3b26965c7f36549a 100644
--- a/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.cpp
@@ -1,4 +1,6 @@
#include "Visu.h"
+#include "ArmarXCore/core/logging/Logging.h"
+#include "ArmarXCore/core/time/CycleUtil.h"
#include <algorithm>
@@ -8,28 +10,17 @@
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+#include "Segment.h"
namespace armarx::armem::server::obj::articulated_object_instance
{
void Visu::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
{
- defs->optional(enabled, prefix + "enabled",
+ defs->optional(p.enabled, prefix + "enabled",
"Enable or disable visualization of objects.");
- defs->optional(frequencyHz, prefix + "frequenzyHz",
+ defs->optional(p.frequencyHz, prefix + "frequenzyHz",
"Frequency of visualization.");
- defs->optional(inGlobalFrame, prefix + "inGlobalFrame",
- "If true, show global poses. If false, show poses in robot frame.");
- defs->optional(alpha, prefix + "alpha",
- "Alpha of objects (1 = solid, 0 = transparent).");
- defs->optional(alphaByConfidence, prefix + "alphaByConfidence",
- "If true, use the pose confidence as alpha (if < 1.0).");
- defs->optional(oobbs, prefix + "oobbs",
- "Enable showing oriented bounding boxes.");
- defs->optional(objectFrames, prefix + "objectFrames",
- "Enable showing object frames.");
- defs->optional(objectFramesScale, prefix + "objectFramesScale",
- "Scaling of object frames.");
}
@@ -47,6 +38,7 @@ namespace armarx::armem::server::obj::articulated_object_instance
void Visu::visualizeObjects(viz::Layer& layer, const armarx::armem::articulated_object::ArticulatedObjects& objects) const
{
+ ARMARX_INFO << "Entering visualizeObjects";
const auto visualizeObject = [&](const armarx::armem::articulated_object::ArticulatedObject & obj)
{
@@ -54,66 +46,127 @@ namespace armarx::armem::server::obj::articulated_object_instance
// clang-format off
auto robot = viz::Robot(obj.description.name)
- .file(xmlPath.package, xmlPath.path)
+ // .file(xmlPath.package, xmlPath.path)
+ .file("ArmarXObjects", "./data/ArmarXObjects/Environment/mobile-kitchen/dishwasher-only/dishwasher.xml")
.joints(obj.config.jointMap)
.pose(obj.config.globalPose);
+
+ robot.useFullModel();
// clang-format on
layer.add(robot);
};
std::for_each(objects.begin(), objects.end(), visualizeObject);
- }
+ ARMARX_INFO << "Done visualizeObjects";
+ }
- void Visu::RemoteGui::setup(const Visu& visu)
+ void Visu::init()
{
- using namespace armarx::RemoteGui::Client;
-
- enabled.setValue(visu.enabled);
- inGlobalFrame.setValue(visu.inGlobalFrame);
- alpha.setRange(0, 1.0);
- alpha.setValue(visu.alpha);
- alphaByConfidence.setValue(visu.alphaByConfidence);
- oobbs.setValue(visu.oobbs);
- objectFrames.setValue(visu.objectFrames);
+ updateTask = new SimpleRunningTask<>([this]()
{
- float max = 10000;
- objectFramesScale.setRange(0, max);
- objectFramesScale.setDecimals(2);
- objectFramesScale.setSteps(int(10 * max));
- objectFramesScale.setValue(visu.objectFramesScale);
- }
-
- GridLayout grid;
- int row = 0;
- grid.add(Label("Enabled"), {row, 0}).add(enabled, {row, 1});
- row++;
- grid.add(Label("Global Frame"), {row, 0}).add(inGlobalFrame, {row, 1});
- row++;
- grid.add(Label("Alpha"), {row, 0}).add(alpha, {row, 1}, {1, 3});
- row++;
- grid.add(Label("Alpha by Confidence"), {row, 0}).add(alphaByConfidence, {row, 1});
- row++;
- grid.add(Label("OOBB"), {row, 0}).add(oobbs, {row, 1});
- row++;
- grid.add(Label("Object Frames"), {row, 0}).add(objectFrames, {row, 1});
- grid.add(Label("Scale:"), {row, 2}).add(objectFramesScale, {row, 3});
- row++;
-
- group.setLabel("Visualization");
- group.addChild(grid);
+ this->visualizeRun();
+ });
+ updateTask->start();
}
- void Visu::RemoteGui::update(Visu& visu)
+
+ // void Visu::RemoteGui::setup(const Visu& visu)
+ // {
+ // using namespace armarx::RemoteGui::Client;
+
+ // enabled.setValue(visu.enabled);
+ // inGlobalFrame.setValue(visu.inGlobalFrame);
+ // alpha.setRange(0, 1.0);
+ // alpha.setValue(visu.alpha);
+ // alphaByConfidence.setValue(visu.alphaByConfidence);
+ // oobbs.setValue(visu.oobbs);
+ // objectFrames.setValue(visu.objectFrames);
+ // {
+ // float max = 10000;
+ // objectFramesScale.setRange(0, max);
+ // objectFramesScale.setDecimals(2);
+ // objectFramesScale.setSteps(int(10 * max));
+ // objectFramesScale.setValue(visu.objectFramesScale);
+ // }
+
+ // GridLayout grid;
+ // int row = 0;
+ // grid.add(Label("Enabled"), {row, 0}).add(enabled, {row, 1});
+ // row++;
+ // grid.add(Label("Global Frame"), {row, 0}).add(inGlobalFrame, {row, 1});
+ // row++;
+ // grid.add(Label("Alpha"), {row, 0}).add(alpha, {row, 1}, {1, 3});
+ // row++;
+ // grid.add(Label("Alpha by Confidence"), {row, 0}).add(alphaByConfidence, {row, 1});
+ // row++;
+ // grid.add(Label("OOBB"), {row, 0}).add(oobbs, {row, 1});
+ // row++;
+ // grid.add(Label("Object Frames"), {row, 0}).add(objectFrames, {row, 1});
+ // grid.add(Label("Scale:"), {row, 2}).add(objectFramesScale, {row, 3});
+ // row++;
+
+ // group.setLabel("Visualization");
+ // group.addChild(grid);
+ // }
+
+ // void Visu::RemoteGui::update(Visu& visu)
+ // {
+ // visu.enabled = enabled.getValue();
+ // visu.inGlobalFrame = inGlobalFrame.getValue();
+ // visu.alpha = alpha.getValue();
+ // visu.alphaByConfidence = alphaByConfidence.getValue();
+ // visu.oobbs = oobbs.getValue();
+ // visu.objectFrames = objectFrames.getValue();
+ // visu.objectFramesScale = objectFramesScale.getValue();
+ // }
+
+
+ void Visu::visualizeRun()
{
- visu.enabled = enabled.getValue();
- visu.inGlobalFrame = inGlobalFrame.getValue();
- visu.alpha = alpha.getValue();
- visu.alphaByConfidence = alphaByConfidence.getValue();
- visu.oobbs = oobbs.getValue();
- visu.objectFrames = objectFrames.getValue();
- visu.objectFramesScale = objectFramesScale.getValue();
+ CycleUtil cycle(static_cast<int>(1000 / p.frequencyHz));
+ while (updateTask && not updateTask->isStopped())
+ {
+ {
+ // std::scoped_lock lock(visuMutex);
+ ARMARX_IMPORTANT << "Update task";
+
+ if (p.enabled)
+ {
+ // TIMING_START(Visu);
+
+ const auto articulatedObjects = segment.getArticulatedObjects();
+
+ ARMARX_INFO << "Found " << articulatedObjects.size() << " articulated objects";
+
+ viz::Layer layer = arviz.layer("ArticulatedObjectInstances");
+
+ ARMARX_INFO << "visualizing objects";
+ visualizeObjects(layer, articulatedObjects);
+
+ ARMARX_INFO << "Committing objects";
+
+ arviz.commit({layer});
+
+ ARMARX_INFO << "Done committing";
+
+
+ // TIMING_END_STREAM(Visu, ARMARX_VERBOSE);
+
+ // if (debugObserver)
+ // {
+ // debugObserver->setDebugChannel(getName(),
+ // {
+ // { "t Visualize [ms]", new Variant(Visu.toMilliSecondsDouble()) },
+ // });
+ // }
+ }
+ }
+ cycle.waitForCycleDuration();
+ }
}
-}
+
+
+} // namespace armarx::armem::server::obj::articulated_object_instance
diff --git a/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.h b/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.h
index 9ef5548a9230d999fbe70b46041c912089763786..54aea5e09f0d3000596c83d2eb8ffbafcbf8b81e 100644
--- a/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.h
+++ b/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.h
@@ -1,17 +1,35 @@
-#pragma once
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#pragma once
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/services/tasks/TaskUtil.h>
-#include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
+// #include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
#include <RobotAPI/components/ArViz/Client/Client.h>
-#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
#include <RobotAPI/libraries/armem_objects/types.h>
-#include <VirtualRobot/VirtualRobot.h>
namespace armarx
{
@@ -19,6 +37,7 @@ namespace armarx
}
namespace armarx::armem::server::obj::articulated_object_instance
{
+ class Segment;
/**
* @brief Models decay of object localizations by decreasing the confidence
@@ -28,70 +47,55 @@ namespace armarx::armem::server::obj::articulated_object_instance
{
public:
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "visu.");
+ Visu(const viz::Client& arviz, const Segment& segment): arviz(arviz), segment(segment) {}
- // std::vector<viz::Layer> visualizeCommit(
- // const std::map<std::string, objpose::ObjectPoseSeq>& objectPoses,
- // const ObjectFinder& objectFinder
- // ) const;
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "visu.");
- // /// Visualize the given object poses, with one layer per provider.
- // std::vector<viz::Layer> visualizeCommit(
- // const objpose::ObjectPoseSeq& objectPoses,
- // const ObjectFinder& objectFinder
- // ) const;
+ void init();
+ protected:
viz::Layer visualizeProvider(
const std::string& providerName,
const armarx::armem::articulated_object::ArticulatedObjects& objects
) const;
- void visualizeObjects(viz::Layer& layer, const armarx::armem::articulated_object::ArticulatedObjects& objects) const;
-
-
-
- // void visualizeArticulatedObjects(
- // viz::Layer& layer,
- // const objpose::ObjectPose& objectPose,
- // const ObjectFinder& objectFinder
- // ) const;
-
+ void visualizeObjects(
+ viz::Layer& layer,
+ const armarx::armem::articulated_object::ArticulatedObjects& objects
+ ) const;
- public:
+ private:
viz::Client arviz;
+ const Segment& segment;
- bool enabled = true;
- float frequencyHz = 25;
+ struct Properties
+ {
+ bool enabled = true;
+ float frequencyHz = 25;
+ } p;
- bool inGlobalFrame = true;
- float minConfidence = -1;
- float alpha = 1.0;
- bool alphaByConfidence = false;
- bool oobbs = false;
- bool objectFrames = false;
- float objectFramesScale = 1.0;
SimpleRunningTask<>::pointer_type updateTask;
+ void visualizeRun();
+ // struct RemoteGui
+ // {
+ // armarx::RemoteGui::Client::GroupBox group;
- struct RemoteGui
- {
- armarx::RemoteGui::Client::GroupBox group;
-
- armarx::RemoteGui::Client::CheckBox enabled;
+ // armarx::RemoteGui::Client::CheckBox enabled;
- armarx::RemoteGui::Client::CheckBox inGlobalFrame;
- armarx::RemoteGui::Client::FloatSlider alpha;
- armarx::RemoteGui::Client::CheckBox alphaByConfidence;
- armarx::RemoteGui::Client::CheckBox oobbs;
- armarx::RemoteGui::Client::CheckBox objectFrames;
- armarx::RemoteGui::Client::FloatSpinBox objectFramesScale;
+ // armarx::RemoteGui::Client::CheckBox inGlobalFrame;
+ // armarx::RemoteGui::Client::FloatSlider alpha;
+ // armarx::RemoteGui::Client::CheckBox alphaByConfidence;
+ // armarx::RemoteGui::Client::CheckBox oobbs;
+ // armarx::RemoteGui::Client::CheckBox objectFrames;
+ // armarx::RemoteGui::Client::FloatSpinBox objectFramesScale;
- void setup(const Visu& visu);
- void update(Visu& visu);
- };
+ // // void setup(const Visu& visu);
+ // // void update(Visu& visu);
+ // };
};
-}
+} // namespace armarx::armem::server::obj::articulated_object_instance
diff --git a/source/RobotAPI/libraries/armem_objects/types.h b/source/RobotAPI/libraries/armem_objects/types.h
index a127850fe89fde6a635e360aba51e4a13f7d95b7..5d2b4f76c83fa5ed3e823cc2d1528d941dcad6f5 100644
--- a/source/RobotAPI/libraries/armem_objects/types.h
+++ b/source/RobotAPI/libraries/armem_objects/types.h
@@ -4,6 +4,7 @@
#include <map>
#include <vector>
+#include <filesystem>
#include <Eigen/Geometry>
@@ -19,7 +20,7 @@ namespace armarx::armem
// IceUtil::Time timestamp;
std::string name;
- PackagePath xml;
+ PackagePath xml{"", std::filesystem::path("")};
};