From ab755b2d2b641c903dfa52067b9c17ce7210204b Mon Sep 17 00:00:00 2001
From: "Christian R. G. Dreher" <c.dreher@kit.edu>
Date: Sun, 26 Apr 2020 12:38:58 +0200
Subject: [PATCH] Implement ellipsoid visualization.
---
.../RobotAPI/components/ArViz/CMakeLists.txt | 1 +
.../components/ArViz/Client/Elements.h | 34 +++++++++++
.../ArViz/Coin/RegisterVisualizationTypes.cpp | 4 +-
.../ArViz/Coin/VisualizationEllipsoid.h | 59 +++++++++++++++++++
.../components/ArViz/Example/ArVizExample.cpp | 11 ++++
.../ArViz/Introspection/json_elements.cpp | 14 +++++
.../ArViz/Introspection/json_elements.h | 4 ++
.../register_element_json_serializers.cpp | 1 +
source/RobotAPI/interface/ArViz/Elements.ice | 6 ++
9 files changed, 133 insertions(+), 1 deletion(-)
create mode 100644 source/RobotAPI/components/ArViz/Coin/VisualizationEllipsoid.h
diff --git a/source/RobotAPI/components/ArViz/CMakeLists.txt b/source/RobotAPI/components/ArViz/CMakeLists.txt
index abef16bad..bdc666ed5 100644
--- a/source/RobotAPI/components/ArViz/CMakeLists.txt
+++ b/source/RobotAPI/components/ArViz/CMakeLists.txt
@@ -38,6 +38,7 @@ Coin/ElementVisualizer.h
# Inventor
Coin/VisualizationBox.h
Coin/VisualizationCylinder.h
+Coin/VisualizationEllipsoid.h
Coin/VisualizationSphere.h
Coin/VisualizationPose.h
Coin/VisualizationLine.h
diff --git a/source/RobotAPI/components/ArViz/Client/Elements.h b/source/RobotAPI/components/ArViz/Client/Elements.h
index 02d2c5b10..63239bc47 100644
--- a/source/RobotAPI/components/ArViz/Client/Elements.h
+++ b/source/RobotAPI/components/ArViz/Client/Elements.h
@@ -153,12 +153,46 @@ namespace armarx::viz
}
};
+
+ class Ellipsoid : public ElementOps<Ellipsoid, data::ElementEllipsoid>
+ {
+ public:
+ Ellipsoid(const std::string& name) :
+ ElementOps(name)
+ {
+ data_->curvature.e0 = 1;
+ data_->curvature.e1 = 1;
+ data_->curvature.e2 = 1;
+ }
+
+ Ellipsoid& size(const Eigen::Vector3f& size)
+ {
+ data_->size.e0 = size.x();
+ data_->size.e1 = size.y();
+ data_->size.e2 = size.z();
+
+ return *this;
+ }
+
+ Ellipsoid& curvature(const Eigen::Vector3f& curvature)
+ {
+ ARMARX_WARNING << "Stub! Curvature not yet implemented. Has no effect.";
+ data_->curvature.e0 = curvature.x();
+ data_->curvature.e1 = curvature.y();
+ data_->curvature.e2 = curvature.z();
+
+ return *this;
+ }
+ };
+
+
class Pose : public ElementOps<Pose, data::ElementPose>
{
public:
using ElementOps::ElementOps;
};
+
class Text : public ElementOps<Text, data::ElementText>
{
public:
diff --git a/source/RobotAPI/components/ArViz/Coin/RegisterVisualizationTypes.cpp b/source/RobotAPI/components/ArViz/Coin/RegisterVisualizationTypes.cpp
index 69e20c6d0..70d49b71b 100644
--- a/source/RobotAPI/components/ArViz/Coin/RegisterVisualizationTypes.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/RegisterVisualizationTypes.cpp
@@ -3,6 +3,7 @@
#include "VisualizationBox.h"
#include "VisualizationCylinder.h"
#include "VisualizationSphere.h"
+#include "VisualizationEllipsoid.h"
#include "VisualizationPose.h"
#include "VisualizationLine.h"
#include "VisualizationText.h"
@@ -19,11 +20,12 @@ void armarx::viz::CoinVisualizer::registerVisualizationTypes()
{
using namespace armarx::viz::coin;
- elementVisualizers.reserve(13);
+ elementVisualizers.reserve(14);
registerVisualizerFor<VisualizationBox>();
registerVisualizerFor<VisualizationCylinder>();
registerVisualizerFor<VisualizationSphere>();
+ registerVisualizerFor<VisualizationEllipsoid>();
registerVisualizerFor<VisualizationPose>();
registerVisualizerFor<VisualizationLine>();
registerVisualizerFor<VisualizationText>();
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationEllipsoid.h b/source/RobotAPI/components/ArViz/Coin/VisualizationEllipsoid.h
new file mode 100644
index 000000000..5d9520501
--- /dev/null
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationEllipsoid.h
@@ -0,0 +1,59 @@
+#pragma once
+
+#include "ElementVisualizer.h"
+
+#include <RobotAPI/interface/ArViz/Elements.h>
+#include <Inventor/nodes/SoCone.h>
+#include <Inventor/nodes/SoCylinder.h>
+#include <Inventor/nodes/SoSphere.h>
+#include <Inventor/nodes/SoTranslation.h>
+
+#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
+#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.h>
+
+namespace armarx::viz::coin
+{
+ struct VisualizationEllipsoid : TypedElementVisualization<SoSeparator>
+ {
+ using ElementType = data::ElementEllipsoid;
+
+ bool update(ElementType const& element)
+ {
+ auto color = element.color;
+ constexpr float conv = 1.0f / 255.0f;
+ const float r = color.r * conv;
+ const float g = color.g * conv;
+ const float b = color.b * conv;
+ const float a = color.a * conv;
+
+ VirtualRobot::VisualizationNodePtr ellipsoid_node;
+ {
+ // Params.
+ SoMaterial* mat = new SoMaterial;
+ mat->diffuseColor.setValue(r, g, b);
+ mat->ambientColor.setValue(r, g, b);
+ mat->transparency.setValue(1. - a);
+ const bool show_axes = false; // Do not show axes. If needed, draw a Pose instead.
+
+ SoSeparator* res = new SoSeparator();
+ res->ref();
+ SoUnits* u = new SoUnits();
+ u->units = SoUnits::MILLIMETERS;
+ res->addChild(u);
+ res->addChild(VirtualRobot::CoinVisualizationFactory::CreateEllipse(
+ element.size.e0, element.size.e1, element.size.e2, mat,
+ show_axes));
+ ellipsoid_node.reset(new VirtualRobot::CoinVisualizationNode(res));
+ res->unref();
+ }
+
+ SoNode* ellipsoid = dynamic_cast<VirtualRobot::CoinVisualizationNode&>(
+ *ellipsoid_node).getCoinVisualization();
+
+ node->removeAllChildren();
+ node->addChild(ellipsoid);
+
+ return true;
+ }
+ };
+}
diff --git a/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp b/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp
index 4df1b1f39..e508a7844 100644
--- a/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp
+++ b/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp
@@ -69,6 +69,17 @@ namespace armarx
layer.add(box);
}
+ {
+ const float pos_y = 100. * std::cos(timeInSeconds);
+ const Eigen::Vector3f pos{0.f, pos_y, 150.f};
+
+ viz::Ellipsoid ell = viz::Ellipsoid{"ellipsoid"}
+ .position(pos)
+ .color(viz::Color::blue())
+ .size(Eigen::Vector3f{50, 50, 100});
+
+ layer.add(ell);
+ }
{
Eigen::Vector3f pos = Eigen::Vector3f::Zero();
pos.y() = 100.0f * std::sin(timeInSeconds);
diff --git a/source/RobotAPI/components/ArViz/Introspection/json_elements.cpp b/source/RobotAPI/components/ArViz/Introspection/json_elements.cpp
index 14fbb8bb3..1b1285ee9 100644
--- a/source/RobotAPI/components/ArViz/Introspection/json_elements.cpp
+++ b/source/RobotAPI/components/ArViz/Introspection/json_elements.cpp
@@ -187,6 +187,20 @@ namespace armarx::viz
}
+ void data::to_json(nlohmann::json& j, const ElementEllipsoid& ellipsoid)
+ {
+ json::to_json_base(j, ellipsoid);
+ j["size"] = ellipsoid.size;
+ j["curvature"] = ellipsoid.curvature;
+ }
+ void data::from_json(const nlohmann::json& j, ElementEllipsoid& ellipsoid)
+ {
+ json::from_json_base(j, ellipsoid);
+ ellipsoid.size = j.at("size");
+ ellipsoid.curvature = j.at("curvature");
+ }
+
+
void data::to_json(nlohmann::json& j, const ElementText& text)
{
json::to_json_base(j, text);
diff --git a/source/RobotAPI/components/ArViz/Introspection/json_elements.h b/source/RobotAPI/components/ArViz/Introspection/json_elements.h
index 15b59b676..161017340 100644
--- a/source/RobotAPI/components/ArViz/Introspection/json_elements.h
+++ b/source/RobotAPI/components/ArViz/Introspection/json_elements.h
@@ -74,6 +74,10 @@ namespace armarx::viz::data
void from_json(const nlohmann::json& j, ElementSphere& sphere);
+ void to_json(nlohmann::json& j, const ElementEllipsoid& sphere);
+ void from_json(const nlohmann::json& j, ElementEllipsoid& sphere);
+
+
void to_json(nlohmann::json& j, const ElementText& text);
void from_json(const nlohmann::json& j, ElementText& text);
diff --git a/source/RobotAPI/components/ArViz/Introspection/register_element_json_serializers.cpp b/source/RobotAPI/components/ArViz/Introspection/register_element_json_serializers.cpp
index f5b2b44c4..d839d39d6 100644
--- a/source/RobotAPI/components/ArViz/Introspection/register_element_json_serializers.cpp
+++ b/source/RobotAPI/components/ArViz/Introspection/register_element_json_serializers.cpp
@@ -8,6 +8,7 @@ void armarx::viz::json::ElementJsonSerializers::registerElements()
registerSerializer<data::ElementArrowCircle>(viz::data::to_json, viz::data::from_json);
registerSerializer<data::ElementBox>(viz::data::to_json, viz::data::from_json);
registerSerializer<data::ElementCylinder>(viz::data::to_json, viz::data::from_json);
+ registerSerializer<data::ElementEllipsoid>(viz::data::to_json, viz::data::from_json);
registerSerializer<data::ElementLine>(viz::data::to_json, viz::data::from_json);
registerSerializer<data::ElementMesh>(viz::data::to_json, viz::data::from_json);
registerSerializer<data::ElementObject>(viz::data::to_json, viz::data::from_json);
diff --git a/source/RobotAPI/interface/ArViz/Elements.ice b/source/RobotAPI/interface/ArViz/Elements.ice
index d0da8aefa..4b13ca3e1 100644
--- a/source/RobotAPI/interface/ArViz/Elements.ice
+++ b/source/RobotAPI/interface/ArViz/Elements.ice
@@ -67,6 +67,12 @@ module data
float radius = 10.0f;
};
+ class ElementEllipsoid extends Element
+ {
+ Vector3f size;
+ Vector3f curvature;
+ };
+
class ElementCylinder extends Element
{
float height = 10.0f;
--
GitLab