diff --git a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
index 114f2b79413e4ef5c79fcb650d29299598456ad4..a26840ac78305a9afcf6ab55498d5bdf45d9c017 100644
--- a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
@@ -17,7 +17,9 @@ armarx_add_library(
RobotAPI::libraries::armem
HEADERS
aron_conversions.h
+ aron_forward_declarations.h
+ server/class/FloorVis.h
server/class/Segment.h
server/instance/Segment.h
@@ -28,6 +30,7 @@ armarx_add_library(
SOURCES
aron_conversions.cpp
+ server/class/FloorVis.cpp
server/class/Segment.cpp
server/instance/Segment.cpp
diff --git a/source/RobotAPI/libraries/armem_objects/aron_forward_declarations.h b/source/RobotAPI/libraries/armem_objects/aron_forward_declarations.h
new file mode 100644
index 0000000000000000000000000000000000000000..9416a033d713f5c8dab28506538c1a56534dbb53
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/aron_forward_declarations.h
@@ -0,0 +1,8 @@
+#pragma once
+
+
+namespace armarx::armem::arondto
+{
+ class ObjectClass;
+ class ObjectInstance;
+}
diff --git a/source/RobotAPI/libraries/armem_objects/server/class/FloorVis.cpp b/source/RobotAPI/libraries/armem_objects/server/class/FloorVis.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..08927d1e77d1963234f42ea9ab861ec9a33f86c4
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/server/class/FloorVis.cpp
@@ -0,0 +1,77 @@
+#include "FloorVis.h"
+
+#include <RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h>
+#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
+
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+
+
+namespace armarx::armem::server::obj::clazz
+{
+
+ FloorVis::FloorVis()
+ {
+ }
+
+ void FloorVis::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ properties.define(defs, prefix);
+ }
+
+ void FloorVis::setArViz(armarx::viz::Client arviz)
+ {
+ this->arviz = arviz;
+ }
+
+ void FloorVis::updateFloorObject(const wm::CoreSegment& classCoreSegment)
+ {
+ viz::Layer layer = arviz.layer(properties.layerName);
+ if (properties.show)
+ {
+ const wm::Entity* entity = classCoreSegment.findEntity(MemoryID().withEntityName(properties.entityName));
+ if (entity)
+ {
+ ARMARX_INFO << "Drawing floor class '" << properties.entityName << "'.";
+ layer.add(makeFloorObject(*entity));
+ }
+ else
+ {
+ ARMARX_INFO << "Did not find floor class '" << properties.entityName << "'.";
+ }
+ }
+ arviz.commit(layer);
+ }
+
+ armarx::viz::Object FloorVis::makeFloorObject(const wm::Entity& classEntity)
+ {
+ const wm::EntityInstance& instance = classEntity.getLatestSnapshot().getInstance(0);
+ arondto::ObjectClass data;
+ data.fromAron(instance.data());
+ return makeFloorObject(classEntity.name(), data);
+ }
+
+ armarx::viz::Object FloorVis::makeFloorObject(
+ const std::string& name,
+ const arondto::ObjectClass& objectClass)
+ {
+ ARMARX_TRACE;
+ return armarx::viz::Object(name)
+ .file(objectClass.simoxXmlPath.package, objectClass.simoxXmlPath.path)
+ .position(Eigen::Vector3f(0, 0, properties.height));
+ }
+
+
+
+ void FloorVis::Properties::define(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ defs->optional(show, prefix + "Show", "Whether to show the floor.");
+ defs->optional(entityName, prefix + "EntityName", "Object class entity of the floor.");
+ defs->optional(layerName, prefix + "LayerName", "Layer to draw the floor on.");
+ defs->optional(height, prefix + "Height",
+ "Height (z) of the floor plane. \n"
+ "Set slightly below 0 to avoid z-fighting when drawing planes on the ground.");
+ }
+
+}
+
+
diff --git a/source/RobotAPI/libraries/armem_objects/server/class/FloorVis.h b/source/RobotAPI/libraries/armem_objects/server/class/FloorVis.h
new file mode 100644
index 0000000000000000000000000000000000000000..4c3be719fce4148970fa0ef16d8465d2df7a10d3
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/server/class/FloorVis.h
@@ -0,0 +1,58 @@
+#pragma once
+
+#include <string>
+
+#include <RobotAPI/components/ArViz/Client/Client.h>
+#include <RobotAPI/libraries/armem_objects/aron_forward_declarations.h>
+
+
+namespace armarx
+{
+ using PropertyDefinitionsPtr = IceUtil::Handle<class PropertyDefinitionContainer>;
+}
+namespace armarx::armem::wm
+{
+ class CoreSegment;
+ class Entity;
+}
+
+namespace armarx::armem::server::obj::clazz
+{
+ class FloorVis
+ {
+ public:
+
+ FloorVis();
+
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
+ void setArViz(armarx::viz::Client arviz);
+
+ /// Draw a the floor as a simox object.
+ /// @see `makeFloorObject()`
+ void updateFloorObject(const wm::CoreSegment& classCoreSegment);
+
+ armarx::viz::Object makeFloorObject(const wm::Entity& classEntity);
+ armarx::viz::Object makeFloorObject(const std::string& name, const arondto::ObjectClass& objectClass);
+
+
+ public:
+
+ struct Properties
+ {
+ bool show = true;
+
+ std::string entityName = "Environment/floor-20x20";
+ std::string layerName = "Floor";
+ bool height = 1;
+
+ void define(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
+ };
+
+ private:
+
+ Properties properties;
+
+ armarx::viz::Client arviz;
+
+ };
+}
diff --git a/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
index fcb0d8156e19658f98faf0da88f1da2223c86e43..38b143f2baded43c3a4f028174380abaef288dcf 100644
--- a/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
@@ -34,6 +34,8 @@ namespace armarx::armem::server::obj::clazz
defs->optional(p.objectsPackage, prefix + "ObjectsPackgage", "Name of the objects package to load from.");
defs->optional(p.loadFromObjectsPackage, prefix + "LoadFromObjectsPackage",
"If true, load the objects from the objects package on startup.");
+
+ floorVis.defineProperties(defs, prefix + "Floor.");
}
void Segment::init()
@@ -52,6 +54,9 @@ namespace armarx::armem::server::obj::clazz
void Segment::connect(viz::Client arviz)
{
this->arviz = arviz;
+
+ floorVis.setArViz(arviz);
+ floorVis.updateFloorObject(*coreSegment);
}
diff --git a/source/RobotAPI/libraries/armem_objects/server/class/Segment.h b/source/RobotAPI/libraries/armem_objects/server/class/Segment.h
index 92fa7c182db309aa743ebc6b3e863d9a76a03440..7356e6a1d9e490840f7c19b5e41e27506ddf2ecf 100644
--- a/source/RobotAPI/libraries/armem_objects/server/class/Segment.h
+++ b/source/RobotAPI/libraries/armem_objects/server/class/Segment.h
@@ -15,6 +15,7 @@
#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/server/class/FloorVis.h>
namespace armarx::armem::server::obj::clazz
@@ -51,6 +52,7 @@ namespace armarx::armem::server::obj::clazz
ObjectFinder objectFinder;
viz::Client arviz;
+ FloorVis floorVis;
struct Properties
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
index f570ca9b40c4e57f559cc55cdfdd535eba6e3d03..3773c087bf70fa68981be86696b01c6fae7645d6 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
@@ -472,6 +472,7 @@ namespace armarx::armem::server::obj::instance
VSpacer()
};
+ group = {};
group.setLabel("Instance");
group.addChild(layout);
}