diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
index c8659d536fcdf4a19b3b257b15162582407f8643..31d5d60da516783f06524f104b677b04c64bad7a 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
@@ -59,6 +59,7 @@
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/XML/RobotIO.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
+#include <VirtualRobot/Visualization/TriMeshModel.h>
using namespace VirtualRobot;
@@ -741,6 +742,58 @@ namespace armarx
layer.mainNode->addChild(sep);
}
+
+ void DebugDrawerComponent::drawTriMesh(const DebugDrawerComponent::TriMeshData& d)
+ {
+ ScopedRecursiveLockPtr l = getScopedVisuLock();
+
+ auto& layer = requestLayer(d.layerName);
+
+ removeTriMesh(d.layerName, d.name);
+
+ if (!d.active)
+ {
+ return;
+ }
+
+ TriMeshModelPtr triMesh = TriMeshModelPtr(new TriMeshModel());
+
+ for(DrawColor color : d.triMesh.colors)
+ {
+ triMesh->addColor(VirtualRobot::VisualizationFactory::Color(color.r, color.g, color.b, color.a));
+ }
+
+ for(DebugDrawerVertex v : d.triMesh.vertices)
+ {
+ triMesh->addVertex(Eigen::Vector3f(v.x, v.y, v.z));
+ }
+
+ for(DebugDrawerFace f : d.triMesh.faces)
+ {
+ MathTools::TriangleFace face;
+
+ face.id1 = f.vertex1.vertexID;
+ face.idNormal1 = f.vertex1.normalID;
+ face.idColor1 = f.vertex1.colorID;
+
+ face.id2 = f.vertex2.vertexID;
+ face.idNormal2 = f.vertex2.normalID;
+ face.idColor2 = f.vertex2.colorID;
+
+ face.id3 = f.vertex3.vertexID;
+ face.idNormal3 = f.vertex3.normalID;
+ face.idColor3 = f.vertex3.colorID;
+
+ triMesh->addFace(face);
+ }
+
+ SoSeparator* sep = new SoSeparator();
+ SoNode* c = CoinVisualizationFactory::getCoinVisualization(triMesh);
+ sep->addChild(c);
+ layer.addedPolygonVisualizations[d.name] = sep;
+ layer.mainNode->addChild(sep);
+ }
+
void DebugDrawerComponent::drawArrow(const DebugDrawerComponent::ArrowData& d)
{
ScopedRecursiveLockPtr l = getScopedVisuLock();
@@ -1130,6 +1183,26 @@ namespace armarx
layer.addedPolygonVisualizations.erase(name);
}
+ void DebugDrawerComponent::removeTriMesh(const std::string& layerName, const std::string& name)
+ {
+ ScopedRecursiveLockPtr l = getScopedVisuLock();
+
+ if (!hasLayer(layerName))
+ {
+ return;
+ }
+
+ auto& layer = layers.at(layerName);
+
+ if (layer.addedTriMeshVisualizations.find(name) == layer.addedTriMeshVisualizations.end())
+ {
+ return;
+ }
+
+ layer.mainNode->removeChild(layer.addedTriMeshVisualizations[name]);
+ layer.addedTriMeshVisualizations.erase(name);
+ }
+
void DebugDrawerComponent::removeArrow(const std::string& layerName, const std::string& name)
{
ScopedRecursiveLockPtr l = getScopedVisuLock();
@@ -1639,6 +1712,41 @@ namespace armarx
removePolygonVisu(DEBUG_LAYER_NAME, polygonName);
}
+ void DebugDrawerComponent::setTriMeshVisu(const std::string& layerName, const std::string& triMeshName, const DebugDrawerTriMesh& triMesh, const Ice::Current&)
+ {
+ {
+ ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ std::string entryName = "__" + layerName + "__" + triMeshName + "__";
+ TriMeshData& d = accumulatedUpdateData.triMeshes[entryName];
+ d.triMesh = triMesh;
+ d.layerName = layerName;
+ d.name = triMeshName;
+ d.active = true;
+ }
+ }
+
+ void DebugDrawerComponent::setTriMeshDebugLayerVisu(const std::string& triMeshName, const DebugDrawerTriMesh& triMesh, const Ice::Current&)
+ {
+ setTriMeshVisu(DEBUG_LAYER_NAME, triMeshName, triMesh);
+ }
+
+ void DebugDrawerComponent::removeTriMeshVisu(const std::string& layerName, const std::string& triMeshName, const Ice::Current&)
+ {
+ {
+ ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ std::string entryName = "__" + layerName + "__" + triMeshName + "__";
+ TriMeshData& d = accumulatedUpdateData.triMeshes[entryName];
+ d.layerName = layerName;
+ d.name = triMeshName;
+ d.active = false;
+ }
+ }
+
+ void DebugDrawerComponent::removeTriMeshDebugLayerVisu(const std::string& triMeshName, const Ice::Current&)
+ {
+ removeTriMeshVisu(DEBUG_LAYER_NAME, triMeshName);
+ }
+
void DebugDrawerComponent::setArrowVisu(const std::string& layerName, const std::string& arrowName, const Vector3BasePtr& position, const Vector3BasePtr& direction, const DrawColor& color, float length, float width, const Ice::Current&)
{
{
@@ -1865,6 +1973,11 @@ namespace armarx
removePolygonVisu(layerName, i.first);
}
+ for (const auto & i : layer.addedTriMeshVisualizations)
+ {
+ removeTriMeshVisu(layerName, i.first);
+ }
+
for (const auto & i : layer.added24BitColoredPointCloudVisualizations)
{
remove24BitColoredPointCloudVisu(layerName, i.first);
@@ -2020,6 +2133,13 @@ namespace armarx
accumulatedUpdateData.arrows.clear();
+ for (auto i = accumulatedUpdateData.triMeshes.begin(); i != accumulatedUpdateData.triMeshes.end(); i++)
+ {
+ drawTriMesh(i->second);
+ }
+
+ accumulatedUpdateData.triMeshes.clear();
+
for (auto i = accumulatedUpdateData.robots.begin(); i != accumulatedUpdateData.robots.end(); i++)
{
ARMARX_DEBUG << "update visu / drawRobot for robot " << i->first;
@@ -2254,6 +2374,22 @@ namespace armarx
}
}
+ {
+ auto i1 = accumulatedUpdateData.triMeshes.begin();
+
+ while (i1 != accumulatedUpdateData.triMeshes.end())
+ {
+ if (boost::starts_with(i1->first, entryName))
+ {
+ i1 = accumulatedUpdateData.triMeshes.erase(i1);
+ }
+ else
+ {
+ i1++;
+ }
+ }
+ }
+
{
auto i1 = accumulatedUpdateData.arrows.begin();
@@ -2325,6 +2461,7 @@ namespace armarx
::armarx::LayerInformationSequence seq {};
ScopedRecursiveLockPtr l = getScopedVisuLock();
+ // @@@ TODO: FIXME: count does not include all maps!
for (const auto & layer : layers)
{
int count = layer.second.addedCoordVisualizations.size() +
@@ -2336,6 +2473,8 @@ namespace armarx
layer.second.addedCylinderVisualizations.size() +
layer.second.addedPointCloudVisualizations.size() +
layer.second.addedArrowVisualizations.size() +
+ layer.second.addedPolygonVisualizations.size() +
+ layer.second.addedTriMeshVisualizations.size() +
layer.second.addedRobotVisualizations.size() +
layer.second.addedCustomVisualizations.size();
::armarx::LayerInformation info = {layer.first, layer.second.visible, count};
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
index f7e64a70ace4afac9cea956773f24720dbef3d62..dbf74aacdb8907dc0714f916104e43938e556bb7 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
@@ -205,6 +205,11 @@ namespace armarx
virtual void removePolygonVisu(const std::string& layerName, const std::string& polygonName, const ::Ice::Current& = ::Ice::Current());
virtual void removePolygonDebugLayerVisu(const std::string& polygonName, const ::Ice::Current& = ::Ice::Current());
+ void setTriMeshVisu(const std::string& layerName, const std::string& triMeshName, const DebugDrawerTriMesh& triMesh, const ::Ice::Current& = ::Ice::Current());
+ void setTriMeshDebugLayerVisu(const std::string& triMeshName, const DebugDrawerTriMesh& triMesh, const ::Ice::Current& = ::Ice::Current());
+ void removeTriMeshVisu(const std::string& layerName, const std::string& triMeshName, const ::Ice::Current& = ::Ice::Current());
+ void removeTriMeshDebugLayerVisu(const std::string& triMeshName, const ::Ice::Current& = ::Ice::Current());
+
virtual void setArrowVisu(const std::string& layerName, const std::string& arrowName, const ::armarx::Vector3BasePtr& position, const ::armarx::Vector3BasePtr& direction, const DrawColor& color, float length, float width, const ::Ice::Current& = ::Ice::Current());
virtual void setArrowDebugLayerVisu(const std::string& arrowName, const ::armarx::Vector3BasePtr& position, const ::armarx::Vector3BasePtr& direction, const DrawColor& color, float length, float width, const ::Ice::Current& = ::Ice::Current());
virtual void removeArrowVisu(const std::string& layerName, const std::string& arrowName, const ::Ice::Current& = ::Ice::Current());
@@ -360,6 +365,11 @@ namespace armarx
VirtualRobot::VisualizationFactory::Color colorInner;
VirtualRobot::VisualizationFactory::Color colorBorder;
};
+ struct TriMeshData : public DrawData
+ {
+ DebugDrawerTriMesh triMesh;
+ };
+
struct ArrowData : public DrawData
{
Eigen::Vector3f position;
@@ -404,6 +414,7 @@ namespace armarx
std::map<std::string, ColoredPointCloudData> coloredpointcloud;
std::map<std::string, Colored24BitPointCloudData> colored24Bitpointcloud;
std::map<std::string, PolygonData> polygons;
+ std::map<std::string, TriMeshData> triMeshes;
std::map<std::string, ArrowData> arrows;
std::map<std::string, RobotData> robots;
@@ -435,6 +446,7 @@ namespace armarx
void drawColoredPointCloud(const ColoredPointCloudData& d);
void draw24BitColoredPointCloud(const Colored24BitPointCloudData& d);
void drawPolygon(const PolygonData& d);
+ void drawTriMesh(const TriMeshData& d);
void drawArrow(const ArrowData& d);
void drawRobot(const RobotData& d);
@@ -449,6 +461,7 @@ namespace armarx
void removeColoredPointCloud(const std::string& layerName, const std::string& name);
void remove24BitColoredPointCloud(const std::string& layerName, const std::string& name);
void removePolygon(const std::string& layerName, const std::string& name);
+ void removeTriMesh(const std::string& layerName, const std::string& name);
void removeArrow(const std::string& layerName, const std::string& name);
void removeRobot(const std::string& layerName, const std::string& name);
@@ -478,6 +491,7 @@ namespace armarx
std::map<std::string, SoSeparator*> addedColoredPointCloudVisualizations;
std::map<std::string, SoSeparator*> added24BitColoredPointCloudVisualizations;
std::map<std::string, SoSeparator*> addedPolygonVisualizations;
+ std::map<std::string, SoSeparator*> addedTriMeshVisualizations;
std::map<std::string, SoSeparator*> addedArrowVisualizations;
std::map<std::string, SoSeparator*> addedRobotVisualizations;
std::map<std::string, SoSeparator*> addedCustomVisualizations; // these separators are only used by derived classes and have to start with a material node
@@ -534,6 +548,7 @@ namespace armarx
//PeriodicTask<DebugDrawerComponent>::pointer_type execTaskVisuUpdates;
SoTimerSensor* timerSensor;
void removeAccumulatedData(const std::string& layerName);
+
};
typedef IceInternal::Handle<DebugDrawerComponent> DebugDrawerComponentPtr;
diff --git a/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice b/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
index d11b43cd2e7530ea954efaad2137ce0def00c11a..7322f721a7092a2b2baff60646fdd9e864a06a35 100644
--- a/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
+++ b/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
@@ -44,6 +44,7 @@ module armarx
float b;
float a;
};
+ sequence<DrawColor> DrawColorSequence;
struct DrawColor24Bit
{
@@ -65,6 +66,63 @@ module armarx
sequence<LayerInformation> LayerInformationSequence;
+ /*!
+ * \brief Represents a 3D point or a 3D normal.
+ */
+ struct DebugDrawerVertex
+ {
+ float x;
+ float y;
+ float z;
+ };
+ sequence<DebugDrawerVertex> DebugDrawerVertexSequence;
+
+ /*!
+ * \brief A triangle face consiting of 3 3D points.
+ */
+ struct DebugDrawerSimpleFace
+ {
+ DebugDrawerVertex vertex1;
+ DebugDrawerVertex vertex2;
+ DebugDrawerVertex vertex3;
+ };
+ sequence<DebugDrawerSimpleFace> DebugDrawerSimpleFaceSequence;
+
+ struct DebugDrawerSimpleTriMesh
+ {
+ DebugDrawerSimpleFaceSequence faces;
+ DrawColor color;
+ };
+
+ /*!
+ * \brief An ID reference vertex struct containing position, normal and color IDs
+ */
+ struct DebugDrawerVertexID
+ {
+ int vertexID;
+ int normalID;
+ int colorID;
+ };
+
+ /*!
+ * \brief A triangle face consisting of 3 position-normal-color-vertices.
+ */
+ struct DebugDrawerFace
+ {
+ DebugDrawerVertexID vertex1;
+ DebugDrawerVertexID vertex2;
+ DebugDrawerVertexID vertex3;
+ };
+ sequence<DebugDrawerFace> DebugDrawerFaceSequence;
+
+ struct DebugDrawerTriMesh
+ {
+ DebugDrawerFaceSequence faces;
+ DebugDrawerVertexSequence vertices;
+ DrawColorSequence colors;
+ };
+
+
const int three = 3;
struct DebugDrawerPointCloudElement
@@ -157,6 +215,7 @@ module armarx
void setColoredPointCloudVisu(string layerName, string pointCloudName, DebugDrawerColoredPointCloud pointCloud);
void set24BitColoredPointCloudVisu(string layerName, string pointCloudName, DebugDrawer24BitColoredPointCloud pointCloud);
void setPolygonVisu(string layerName, string polygonName, PolygonPointList polygonPoints, DrawColor colorInner, DrawColor colorBorder, float lineWidth);
+ void setTriMeshVisu(string layerName, string triMeshName, DebugDrawerTriMesh triMesh);
void setArrowVisu(string layerName, string arrowName, Vector3Base position, Vector3Base direction, DrawColor color, float length, float width);
void setCylinderVisu(string layerName, string cylinderName, Vector3Base globalPosition, Vector3Base direction, float length, float radius, DrawColor color);
@@ -197,6 +256,7 @@ module armarx
void setPointCloudDebugLayerVisu(string pointCloudName, DebugDrawerPointCloud pointCloud);
void set24BitColoredPointCloudDebugLayerVisu(string pointCloudName, DebugDrawer24BitColoredPointCloud pointCloud);
void setPolygonDebugLayerVisu(string polygonName, PolygonPointList polygonPoints, DrawColor colorInner, DrawColor colorBorder, float lineWidth);
+ void setTriMeshDebugLayerVisu(string triMeshName, DebugDrawerTriMesh triMesh);
void setArrowDebugLayerVisu(string arrowName, Vector3Base position, Vector3Base direction, DrawColor color, float length, float width);
void setCylinderDebugLayerVisu(string cylinderName, Vector3Base globalPosition, Vector3Base direction, float length, float radius, DrawColor color);
/*!
@@ -214,6 +274,7 @@ module armarx
void removeColoredPointCloudVisu(string layerName, string pointCloudName);
void remove24BitColoredPointCloudVisu(string layerName, string pointCloudName);
void removePolygonVisu(string layerName, string polygonName);
+ void removeTriMeshVisu(string layerName, string triMeshName);
void removeArrowVisu(string layerName, string arrowName);
void removeCylinderVisu(string layerName, string cylinderName);
@@ -231,6 +292,7 @@ module armarx
void removeColoredPointCloudDebugLayerVisu(string pointCloudName);
void remove24BitColoredPointCloudDebugLayerVisu(string pointCloudName);
void removePolygonDebugLayerVisu(string polygonName);
+ void removeTriMeshDebugLayerVisu(string triMeshName);
void removeArrowDebugLayerVisu(string arrowName);
void removeCylinderDebugLayerVisu(string cylinderName);