From 1751cdefb04ca1cd8643974883a295d2ad6f17e8 Mon Sep 17 00:00:00 2001
From: Peter Kaiser <peter.kaiser@kit.edu>
Date: Thu, 4 Dec 2014 15:30:46 +0100
Subject: [PATCH] DebugDrawer: Added simple visualization for point clouds
---
.../DebugDrawer/DebugDrawerComponent.cpp | 83 ++++++++++++++++++-
.../DebugDrawer/DebugDrawerComponent.h | 8 ++
.../visualization/DebugDrawerInterface.ice | 11 +++
3 files changed, 101 insertions(+), 1 deletion(-)
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
index d491dec54..7c5ca25fb 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
@@ -33,6 +33,7 @@
#include <Inventor/nodes/SoFont.h>
#include <Inventor/nodes/SoText2.h>
#include <Inventor/nodes/SoSphere.h>
+#include <Inventor/nodes/SoComplexity.h>
using namespace VirtualRobot;
@@ -227,6 +228,47 @@ void DebugDrawerComponent::drawSphere(const std::string& layerName, const std::s
layer.mainNode->addChild(sep);
}
+void DebugDrawerComponent::drawPointCloud(const std::string &layerName, const std::string &name, const DebugDrawerPointCloud &pointCloud)
+{
+ ScopedRecursiveLockPtr l = getScopedLock();
+ ARMARX_DEBUG << "drawPointCloud";
+
+ auto& layer=requestLayer(layerName);
+
+ removePointCloud(layerName,name);
+
+ Eigen::Vector3f currentPos(0,0,0);
+ SoSeparator *sep = new SoSeparator;
+
+ SoComplexity *comp = new SoComplexity;
+ comp->value = 0;
+ sep->addChild(comp);
+
+ // This is a trivial way of visualizing point clouds.
+ // It is not intended for full-speed visualizations
+ for(auto &point : pointCloud)
+ {
+ SoMaterial *mat = new SoMaterial;
+ mat->ambientColor.setValue(point.color.r, point.color.g, point.color.b);
+ mat->diffuseColor.setValue(point.color.r, point.color.g, point.color.b);
+ mat->transparency.setValue(1 - point.color.a);
+ sep->addChild(mat);
+
+ Eigen::Vector3f t = Vector3Ptr::dynamicCast(point.position)->toEigen() - currentPos;
+ SoTransform *tr = new SoTransform;
+ tr->translation.setValue(t.x(), t.y(), t.z());
+ sep->addChild(tr);
+ currentPos += t;
+
+ SoSphere *s = new SoSphere;
+ s->radius = 10;
+ sep->addChild(s);
+ }
+
+ layer.addedPointCloudVisualizations[name] = sep;
+ layer.mainNode->addChild(sep);
+}
+
void DebugDrawerComponent::removeLine(const std::string& layerName, const std::string &name)
@@ -296,6 +338,23 @@ void DebugDrawerComponent::removeSphere(const std::string &layerName, const std:
layer.addedSphereVisualizations.erase(name);
}
+void DebugDrawerComponent::removePointCloud(const std::string &layerName, const std::string &name)
+{
+ if(!hasLayer(layerName))
+ {
+ return;
+ }
+ ScopedRecursiveLockPtr l = getScopedLock();
+ auto& layer=layers.at(layerName);
+ if(layer.addedPointCloudVisualizations.find(name) == layer.addedPointCloudVisualizations.end())
+ {
+ return;
+ }
+
+ layer.mainNode->removeChild(layer.addedPointCloudVisualizations[name]);
+ layer.addedPointCloudVisualizations.erase(name);
+}
+
void DebugDrawerComponent::removeCoordSystem(const std::string& layerName, const std::string &name)
{
if(!hasLayer(layerName))
@@ -436,6 +495,26 @@ void DebugDrawerComponent::removeSphereDebugLayerVisu(const std::string &sphereN
removeSphereVisu(DEBUG_LAYER_NAME, sphereName);
}
+void DebugDrawerComponent::setPointCloudVisu(const std::string &layerName, const std::string &pointCloudName, const DebugDrawerPointCloud &pointCloud, const Ice::Current &)
+{
+ drawPointCloud(layerName, pointCloudName, pointCloud);
+}
+
+void DebugDrawerComponent::setPointCloudDebugLayerVisu(const std::string &pointCloudName, const DebugDrawerPointCloud &pointCloud, const Ice::Current &)
+{
+ setPointCloudVisu(DEBUG_LAYER_NAME, pointCloudName, pointCloud);
+}
+
+void DebugDrawerComponent::removePointCloudVisu(const std::string &layerName, const std::string &pointCloudName, const Ice::Current &)
+{
+ removePointCloud(layerName, pointCloudName);
+}
+
+void DebugDrawerComponent::removePointCloudDebugLayerVisu(const std::string &pointCloudName, const Ice::Current &)
+{
+ removePointCloudVisu(DEBUG_LAYER_NAME, pointCloudName);
+}
+
void DebugDrawerComponent::clearLayer(const std::string &layerName, const Ice::Current &)
{
if(!hasLayer(layerName))
@@ -452,6 +531,7 @@ void DebugDrawerComponent::clearLayer(const std::string &layerName, const Ice::C
layer.addedBoxVisualizations.clear();
layer.addedTextVisualizations.clear();
layer.addedSphereVisualizations.clear();
+ layer.addedPointCloudVisualizations.clear();
layer.mainNode->removeAllChildren();
}
@@ -569,7 +649,8 @@ StringSequence DebugDrawerComponent::layerNames(const ::Ice::Current&)
layer.second.addedLineVisualizations.size()+
layer.second.addedBoxVisualizations.size()+
layer.second.addedTextVisualizations.size()+
- layer.second.addedSphereVisualizations.size();
+ layer.second.addedSphereVisualizations.size()+
+ layer.second.addedPointCloudVisualizations.size();
::armarx::LayerInformation info={layer.first,layer.second.visible,count};
seq.push_back(info);
}
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
index cb475529e..a18825177 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
@@ -115,6 +115,11 @@ public:
virtual void removeSphereVisu(const std::string &layerName, const std::string &sphereName, const ::Ice::Current& = ::Ice::Current());
virtual void removeSphereDebugLayerVisu(const std::string& sphereName, const ::Ice::Current& = ::Ice::Current());
+ virtual void setPointCloudVisu(const std::string &layerName, const std::string &pointCloudName, const DebugDrawerPointCloud &pointCloud, const ::Ice::Current& = ::Ice::Current());
+ virtual void setPointCloudDebugLayerVisu(const std::string &pointCloudName, const DebugDrawerPointCloud &pointCloud, const ::Ice::Current& = ::Ice::Current());
+ virtual void removePointCloudVisu(const std::string &layerName, const std::string &pointCloudName, const ::Ice::Current& = ::Ice::Current());
+ virtual void removePointCloudDebugLayerVisu(const std::string& pointCloudName, const ::Ice::Current& = ::Ice::Current());
+
virtual void clearLayer(const std::string& layerName, const ::Ice::Current& = ::Ice::Current());
virtual void clearDebugLayer(const ::Ice::Current& = ::Ice::Current());
@@ -148,12 +153,14 @@ protected:
void drawBox(const std::string& layerName, const std::string &name, Eigen::Matrix4f &globalPose, float width, float height, float depth, VirtualRobot::VisualizationFactory::Color &color);
void drawText(const std::string& layerName, const std::string &name, const std::string &text, const Eigen::Vector3f &position, const VirtualRobot::VisualizationFactory::Color &color, int size);
void drawSphere(const std::string& layerName, const std::string &name, const Eigen::Vector3f &position, const VirtualRobot::VisualizationFactory::Color &color, float radius);
+ void drawPointCloud(const std::string& layerName, const std::string &name, const DebugDrawerPointCloud &pointCloud);
void removeCoordSystem(const std::string& layerName, const std::string &name);
void removeLine(const std::string& layerName, const std::string &name);
void removeBox(const std::string& layerName, const std::string &name);
void removeText(const std::string& layerName, const std::string &name);
void removeSphere(const std::string& layerName, const std::string &name);
+ void removePointCloud(const std::string& layerName, const std::string &name);
/**
* @brief Contains data for a layer.
@@ -166,6 +173,7 @@ protected:
std::map<std::string, SoSeparator*> addedBoxVisualizations;
std::map<std::string, SoSeparator*> addedTextVisualizations;
std::map<std::string, SoSeparator*> addedSphereVisualizations;
+ std::map<std::string, SoSeparator*> addedPointCloudVisualizations;
bool visible;
};
diff --git a/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice b/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
index c68a0d1ba..0b10385d4 100644
--- a/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
+++ b/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
@@ -56,6 +56,13 @@ module armarx
sequence<LayerInformation> LayerInformationSequence;
+ struct DebugDrawerPointCloudElement
+ {
+ Vector3Base position;
+ DrawColor color;
+ };
+ sequence<DebugDrawerPointCloudElement> DebugDrawerPointCloud;
+
/*!
* \brief A layered drawing interface.
* All drawing operations are identified with a layer name in order to distinguish different drawing entitties.
@@ -76,6 +83,7 @@ module armarx
void setBoxVisu(string layerName, string boxName, PoseBase globalPose, Vector3Base dimensions, DrawColor color);
void setTextVisu(string layerName, string textName, string text, Vector3Base globalPosition, DrawColor color, int size);
void setSphereVisu(string layerName, string sphereName, Vector3Base globalPosition, DrawColor color, float radius);
+ void setPointCloudVisu(string layerName, string pointCloudName, DebugDrawerPointCloud pointCloud);
/*!
* \brief setPoseVisu draws on the "debug" layer
@@ -88,6 +96,7 @@ module armarx
void setBoxDebugLayerVisu(string boxName, PoseBase globalPose, Vector3Base dimensions, DrawColor color);
void setTextDebugLayerVisu(string textName, string text, Vector3Base globalPosition, DrawColor color, int size);
void setSphereDebugLayerVisu(string sphereName, Vector3Base globalPosition, DrawColor color, float radius);
+ void setPointCloudDebugLayerVisu(string pointCloudName, DebugDrawerPointCloud pointCloud);
/*!
* \brief Remove visualization of coordinate system.
@@ -99,6 +108,7 @@ module armarx
void removeBoxVisu(string layerName, string boxName);
void removeTextVisu(string layerName, string textName);
void removeSphereVisu(string layerName, string sphereName);
+ void removePointCloudVisu(string layerName, string pointCloudName);
/*!
* \brief Removes pose from the "debug" layer.
@@ -108,6 +118,7 @@ module armarx
void removeBoxDebugLayerVisu(string boxName);
void removeTextDebugLayerVisu(string textName);
void removeSphereDebugLayerVisu(string sphereName);
+ void removePointCloudDebugLayerVisu(string pointCloudName);
/*!
* \brief clear removes all visualizations for the given layer
--
GitLab