From 1c8556db4b0297e0487975c15e9b04689d53e474 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Mon, 3 May 2021 15:43:45 +0200
Subject: [PATCH] Draw floor in object memory
---
.../libraries/armem_objects/CMakeLists.txt | 3 +
.../armem_objects/aron_forward_declarations.h | 8 ++
.../armem_objects/server/class/FloorVis.cpp | 77 +++++++++++++++++++
.../armem_objects/server/class/FloorVis.h | 58 ++++++++++++++
.../armem_objects/server/class/Segment.cpp | 5 ++
.../armem_objects/server/class/Segment.h | 2 +
.../server/instance/SegmentAdapter.cpp | 1 +
7 files changed, 154 insertions(+)
create mode 100644 source/RobotAPI/libraries/armem_objects/aron_forward_declarations.h
create mode 100644 source/RobotAPI/libraries/armem_objects/server/class/FloorVis.cpp
create mode 100644 source/RobotAPI/libraries/armem_objects/server/class/FloorVis.h
diff --git a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
index 114f2b794..a26840ac7 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 000000000..9416a033d
--- /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 000000000..08927d1e7
--- /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 000000000..4c3be719f
--- /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 fcb0d8156..38b143f2b 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 92fa7c182..7356e6a1d 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 f570ca9b4..3773c087b 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);
}
--
GitLab