diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
index 5973e08afb3e22be1965db36dfe82b425767c387..5bc4618273788950f036ab7f619b7bf63c9be53d 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
@@ -362,6 +362,47 @@ void DebugDrawerComponent::drawPointCloud(const PointCloudData &d)
layer.mainNode->addChild(sep);
}
+void DebugDrawerComponent::drawPolygon(const DebugDrawerComponent::PolygonData &d)
+{
+ ScopedRecursiveLockPtr l = getScopedLock();
+
+ auto& layer=requestLayer(d.layerName);
+
+ removePolygon(d.layerName,d.name);
+
+ if (!d.active)
+ return;
+
+ SoSeparator *sep = VirtualRobot::CoinVisualizationFactory::CreatePolygonVisualization(d.points,d.colorInner,d.colorBorder,d.lineWidth);
+ layer.addedPolygonVisualizations[d.name] = sep;
+ layer.mainNode->addChild(sep);
+}
+
+void DebugDrawerComponent::drawArrow(const DebugDrawerComponent::ArrowData &d)
+{
+ ScopedRecursiveLockPtr l = getScopedLock();
+
+ auto& layer=requestLayer(d.layerName);
+
+ removeArrow(d.layerName,d.name);
+
+ if (!d.active)
+ return;
+
+ SoSeparator *sep = new SoSeparator;
+
+ SoTransform *tr = new SoTransform;
+ tr->translation.setValue(d.position.x(), d.position.y(), d.position.z());
+ sep->addChild(tr);
+
+ SoSeparator *sepArrow = VirtualRobot::CoinVisualizationFactory::CreateArrow(d.direction,d.length,d.width,d.color);
+ sep->addChild(sepArrow);
+
+ layer.addedArrowVisualizations[d.name] = sep;
+ layer.mainNode->addChild(sep);
+
+}
+
void DebugDrawerComponent::removeLine(const std::string& layerName, const std::string &name)
{
ScopedRecursiveLockPtr l = getScopedLock();
@@ -446,6 +487,40 @@ void DebugDrawerComponent::removePointCloud(const std::string &layerName, const
layer.addedPointCloudVisualizations.erase(name);
}
+void DebugDrawerComponent::removePolygon(const std::string &layerName, const std::string &name)
+{
+ ScopedRecursiveLockPtr l = getScopedLock();
+ if(!hasLayer(layerName))
+ {
+ return;
+ }
+ auto& layer=layers.at(layerName);
+ if(layer.addedPolygonVisualizations.find(name) == layer.addedPolygonVisualizations.end())
+ {
+ return;
+ }
+
+ layer.mainNode->removeChild(layer.addedPolygonVisualizations[name]);
+ layer.addedPolygonVisualizations.erase(name);
+}
+
+void DebugDrawerComponent::removeArrow(const std::string &layerName, const std::string &name)
+{
+ ScopedRecursiveLockPtr l = getScopedLock();
+ if(!hasLayer(layerName))
+ {
+ return;
+ }
+ auto& layer=layers.at(layerName);
+ if(layer.addedArrowVisualizations.find(name) == layer.addedArrowVisualizations.end())
+ {
+ return;
+ }
+
+ layer.mainNode->removeChild(layer.addedArrowVisualizations[name]);
+ layer.addedArrowVisualizations.erase(name);
+}
+
void DebugDrawerComponent::removeCoordSystem(const std::string& layerName, const std::string &name)
{
ScopedRecursiveLockPtr l = getScopedLock();
@@ -759,6 +834,94 @@ void DebugDrawerComponent::removePointCloudDebugLayerVisu(const std::string &poi
removePointCloudVisu(DEBUG_LAYER_NAME, pointCloudName);
}
+void DebugDrawerComponent::setPolygonVisu(const std::string &layerName, const std::string &polygonName, const std::vector<Vector3BasePtr> &polygonPoints, const DrawColor &colorInner, const DrawColor &colorBorder, float lineWidth, const Ice::Current &)
+{
+ {
+ ScopedRecursiveLockPtr l = getScopedDataLock();
+ std::string entryName = "__" + layerName + "__" + polygonName + "__";
+ PolygonData &d = accumulatedUpdateData.polygons[entryName];
+
+ std::vector< Eigen::Vector3f > points;
+ for (size_t i=0;i<polygonPoints.size();i++)
+ {
+ Eigen::Vector3f p = Vector3Ptr::dynamicCast(polygonPoints.at(i))->toEigen();;
+ points.push_back(p);
+ }
+
+ d.points = points;
+ d.colorInner = VirtualRobot::VisualizationFactory::Color(colorInner.r, colorInner.g, colorInner.b, 1 - colorInner.a);;
+ d.colorBorder = VirtualRobot::VisualizationFactory::Color(colorBorder.r, colorBorder.g, colorBorder.b, 1 - colorBorder.a);;
+
+ d.lineWidth = lineWidth;
+ d.layerName = layerName;
+ d.name = polygonName;
+ d.active = true;
+ }
+}
+
+void DebugDrawerComponent::setPolygonDebugLayerVisu(const std::string &polygonName, const std::vector<Vector3BasePtr> &polygonPoints, const DrawColor &colorInner, const DrawColor &colorBorder, float lineWidth, const Ice::Current &)
+{
+ setPolygonVisu(DEBUG_LAYER_NAME, polygonName, polygonPoints, colorInner, colorBorder, lineWidth);
+}
+
+void DebugDrawerComponent::removePolygonVisu(const std::string &layerName, const std::string &polygonName, const Ice::Current &)
+{
+ {
+ ScopedRecursiveLockPtr l = getScopedDataLock();
+ std::string entryName = "__" + layerName + "__" + polygonName + "__";
+ PolygonData &d = accumulatedUpdateData.polygons[entryName];
+ d.layerName = layerName;
+ d.name = polygonName;
+ d.active = false;
+ }
+}
+
+void DebugDrawerComponent::removePolygonDebugLayerVisu(const std::string &polygonName, const Ice::Current &)
+{
+ removePolygonVisu(DEBUG_LAYER_NAME, polygonName);
+}
+
+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 &)
+{
+ {
+ ScopedRecursiveLockPtr l = getScopedDataLock();
+ std::string entryName = "__" + layerName + "__" + arrowName + "__";
+ ArrowData &d = accumulatedUpdateData.arrows[entryName];
+
+ d.position = Vector3Ptr::dynamicCast(position)->toEigen();
+ d.direction = Vector3Ptr::dynamicCast(direction)->toEigen();
+ d.color = VirtualRobot::VisualizationFactory::Color(color.r, color.g, color.b, 1 - color.a);;
+ d.length = length;
+ d.width = width;
+
+ d.layerName = layerName;
+ d.name = arrowName;
+ d.active = true;
+ }
+}
+
+void DebugDrawerComponent::setArrowDebugLayerVisu(const std::string &arrowName, const Vector3BasePtr &position, const Vector3BasePtr &direction, const DrawColor &color, float length, float width, const Ice::Current &)
+{
+ setArrowVisu(DEBUG_LAYER_NAME, arrowName, position, direction, color, length, width);
+}
+
+void DebugDrawerComponent::removeArrowVisu(const std::string &layerName, const std::string &arrowName, const Ice::Current &)
+{
+ {
+ ScopedRecursiveLockPtr l = getScopedDataLock();
+ std::string entryName = "__" + layerName + "__" + arrowName + "__";
+ ArrowData &d = accumulatedUpdateData.arrows[entryName];
+ d.layerName = layerName;
+ d.name = arrowName;
+ d.active = false;
+ }
+}
+
+void DebugDrawerComponent::removeArrowDebugLayerVisu(const std::string &arrowName, const Ice::Current &)
+{
+ removeArrowVisu(DEBUG_LAYER_NAME, arrowName);
+}
+
//todo: in some rare cases the mutex3D lock does not work and results in a broken coin timer setup. No updates of the scene will be drawn in this case
// -> check for a qt-thread solution
void DebugDrawerComponent::clearLayer(const std::string &layerName, const Ice::Current &)
@@ -877,6 +1040,18 @@ void DebugDrawerComponent::updateVisualization()
}
accumulatedUpdateData.pointcloud.clear();
+ for (auto i = accumulatedUpdateData.polygons.begin(); i != accumulatedUpdateData.polygons.end(); i++)
+ {
+ drawPolygon(i->second);
+ }
+ accumulatedUpdateData.polygons.clear();
+
+ for (auto i = accumulatedUpdateData.arrows.begin(); i != accumulatedUpdateData.arrows.end(); i++)
+ {
+ drawArrow(i->second);
+ }
+ accumulatedUpdateData.arrows.clear();
+
}
bool DebugDrawerComponent::hasLayer(const std::string& layerName, const ::Ice::Current&)
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
index 9240ae7e38e4ccde75f38e799d873aeb891fbec9..f8cbc853bf84c82208852b135b26e3d5a8e808d4 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
@@ -171,6 +171,16 @@ public:
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 setPolygonVisu(const std::string &layerName, const std::string &polygonName, const std::vector< ::armarx::Vector3BasePtr > &polygonPoints, const DrawColor &colorInner, const DrawColor &colorBorder, float lineWidth, const ::Ice::Current& = ::Ice::Current());
+ virtual void setPolygonDebugLayerVisu(const std::string &polygonName, const std::vector< ::armarx::Vector3BasePtr > &polygonPoints, const DrawColor &colorInner, const DrawColor &colorBorder, float lineWidth, const ::Ice::Current& = ::Ice::Current());
+ 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());
+
+ 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());
+ virtual void removeArrowDebugLayerVisu(const std::string& arrowName, 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());
@@ -255,6 +265,21 @@ protected:
{
DebugDrawerPointCloud pointCloud;
};
+ struct PolygonData : public DrawData
+ {
+ std::vector< Eigen::Vector3f > points;
+ float lineWidth;
+ VirtualRobot::VisualizationFactory::Color colorInner;
+ VirtualRobot::VisualizationFactory::Color colorBorder;
+ };
+ struct ArrowData : public DrawData
+ {
+ Eigen::Vector3f position;
+ Eigen::Vector3f direction;
+ float length;
+ float width;
+ VirtualRobot::VisualizationFactory::Color color;
+ };
struct UpdateData
{
@@ -264,6 +289,8 @@ protected:
std::map<std::string, TextData> text;
std::map<std::string, SphereData> sphere;
std::map<std::string, PointCloudData> pointcloud;
+ std::map<std::string, PolygonData> polygons;
+ std::map<std::string, ArrowData> arrows;
};
UpdateData accumulatedUpdateData;
@@ -277,6 +304,8 @@ protected:
void drawText(const TextData &d);
void drawSphere(const SphereData &d);
void drawPointCloud(const PointCloudData &d);
+ void drawPolygon(const PolygonData &d);
+ void drawArrow(const ArrowData &d);
void removeCoordSystem(const std::string& layerName, const std::string &name);
void removeLine(const std::string& layerName, const std::string &name);
@@ -284,6 +313,8 @@ protected:
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);
+ void removePolygon(const std::string& layerName, const std::string &name);
+ void removeArrow(const std::string& layerName, const std::string &name);
void setLayerVisibility(const std::string& layerName, bool visible);
@@ -299,6 +330,8 @@ protected:
std::map<std::string, SoSeparator*> addedTextVisualizations;
std::map<std::string, SoSeparator*> addedSphereVisualizations;
std::map<std::string, SoSeparator*> addedPointCloudVisualizations;
+ std::map<std::string, SoSeparator*> addedPolygonVisualizations;
+ std::map<std::string, SoSeparator*> addedArrowVisualizations;
bool visible;
};
diff --git a/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice b/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
index a278fd88a48297b7f525f367e8bb536b0f0ecebe..bc0454a50e84bd80453658fda212bdad2fca1f97 100644
--- a/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
+++ b/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
@@ -64,6 +64,8 @@ module armarx
};
sequence<DebugDrawerPointCloudElement> DebugDrawerPointCloud;
+ sequence< Vector3Base > PolygonPointList;
+
/*!
* \brief A layered drawing interface.
* All drawing operations are identified with a layer name in order to distinguish different drawing entitties.
@@ -85,6 +87,8 @@ module armarx
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);
+ void setPolygonVisu(string layerName, string polygonName, PolygonPointList polygonPoints, DrawColor colorInner, DrawColor colorBorder, float lineWidth);
+ void setArrowVisu(string layerName, string arrowName, Vector3Base position, Vector3Base direction, DrawColor color, float length, float width);
/*!
* \brief setPoseVisu draws on the "debug" layer
@@ -98,6 +102,8 @@ module armarx
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);
+ void setPolygonDebugLayerVisu(string polygonName, PolygonPointList polygonPoints, DrawColor colorInner, DrawColor colorBorder, float lineWidth);
+ void setArrowDebugLayerVisu(string arrowName, Vector3Base position, Vector3Base direction, DrawColor color, float length, float width);
/*!
* \brief Remove visualization of coordinate system.
@@ -110,6 +116,8 @@ module armarx
void removeTextVisu(string layerName, string textName);
void removeSphereVisu(string layerName, string sphereName);
void removePointCloudVisu(string layerName, string pointCloudName);
+ void removePolygonVisu(string layerName, string polygonName);
+ void removeArrowVisu(string layerName, string arrowName);
/*!
* \brief Removes pose from the "debug" layer.
@@ -120,6 +128,8 @@ module armarx
void removeTextDebugLayerVisu(string textName);
void removeSphereDebugLayerVisu(string sphereName);
void removePointCloudDebugLayerVisu(string pointCloudName);
+ void removePolygonDebugLayerVisu(string polygonName);
+ void removeArrowDebugLayerVisu(string arrowName);
/*!
* \brief clear removes all visualizations for the given layer