diff --git a/source/RobotAPI/components/ArViz/CMakeLists.txt b/source/RobotAPI/components/ArViz/CMakeLists.txt index e765f4162f65e1daaff52551166e489a4f3cdd60..c7e6c41e883f7392bd49ef64dd1d2bff9c68b910 100644 --- a/source/RobotAPI/components/ArViz/CMakeLists.txt +++ b/source/RobotAPI/components/ArViz/CMakeLists.txt @@ -27,6 +27,8 @@ Coin/VisualizerLine.h Coin/VisualizerText.h Coin/VisualizerArrow.h Coin/VisualizerArrowCircle.h +Coin/VisualizerPointCloud.h +Coin/VisualizerPolygon.h Coin/Visualizer.h ) diff --git a/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp b/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp index f44a31738783ad0ac574d153887e37b599204093..f92329158d1138dd1f61a21042fe8c67dc842944 100644 --- a/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp +++ b/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp @@ -8,6 +8,8 @@ #include "VisualizerText.h" #include "VisualizerArrow.h" #include "VisualizerArrowCircle.h" +#include "VisualizerPointCloud.h" +#include "VisualizerPolygon.h" #include <ArmarXCore/core/logging/Logging.h> @@ -55,6 +57,8 @@ namespace armarx registerVisualizer<VisualizerText>(); registerVisualizer<VisualizerArrow>(); registerVisualizer<VisualizerArrowCircle>(); + registerVisualizer<VisualizerPointCloud>(); + registerVisualizer<VisualizerPolygon>(); root = new SoSeparator; diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizerPointCloud.h b/source/RobotAPI/components/ArViz/Coin/VisualizerPointCloud.h new file mode 100644 index 0000000000000000000000000000000000000000..b7af2cd90a3a6d6bed4586cf9988b3631b8acb7f --- /dev/null +++ b/source/RobotAPI/components/ArViz/Coin/VisualizerPointCloud.h @@ -0,0 +1,73 @@ +#pragma once + +#include "ElementVisualizer.h" + +#include <RobotAPI/interface/ArViz/Elements.h> + +#include <Inventor/nodes/SoCoordinate3.h> +#include <Inventor/nodes/SoDrawStyle.h> +#include <Inventor/nodes/SoMaterialBinding.h> +#include <Inventor/nodes/SoPointSet.h> + +namespace armarx::viz::coin +{ + struct VisuDataPointCloud : TypedVisuData<SoSeparator> + { + VisuDataPointCloud() + { + pclMat = new SoMaterial; + + SoMaterialBinding* pclMatBind = new SoMaterialBinding; + pclMatBind->value = SoMaterialBinding::PER_PART; + + pclCoords = new SoCoordinate3; + pclStye = new SoDrawStyle; + + auto* sep = node(); + sep->addChild(pclMat); + sep->addChild(pclMatBind); + sep->addChild(pclCoords); + sep->addChild(pclStye); + sep->addChild(new SoPointSet); + } + + SoMaterial* pclMat; + SoCoordinate3* pclCoords; + SoDrawStyle* pclStye; + + std::vector<SbColor> colors; + std::vector<SbVec3f> coords; + }; + + struct VisualizerPointCloud: TypedElementVisualizer<VisualizerPointCloud, ElementPointCloud, VisuDataPointCloud> + { + void updateElement(ElementType const& element, DataType* data) + { + auto& pcl = element.points; + + auto& colors = data->colors; + colors.clear(); + colors.reserve(pcl.size()); + auto& coords = data->coords; + coords.clear(); + coords.reserve(pcl.size()); + + const float conv = 1.0f / 255.0f; + for (auto& point : pcl) + { + float r = point.color.r * conv; + float g = point.color.g * conv; + float b = point.color.b * conv; + colors.emplace_back(r, g, b); + coords.emplace_back(point.x, point.y, point.z); + } + data->pclMat->diffuseColor.setValues(0, colors.size(), colors.data()); + data->pclMat->ambientColor.setValues(0, colors.size(), colors.data()); + data->pclMat->transparency = element.transparency; + + data->pclCoords->point.setValues(0, coords.size(), coords.data()); + + data->pclStye->pointSize = pcl.size(); + } + }; +} diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizerPolygon.h b/source/RobotAPI/components/ArViz/Coin/VisualizerPolygon.h new file mode 100644 index 0000000000000000000000000000000000000000..70a44286d76ad4e4c256025b5b218652c99de873 --- /dev/null +++ b/source/RobotAPI/components/ArViz/Coin/VisualizerPolygon.h @@ -0,0 +1,88 @@ +#pragma once + +#include "ElementVisualizer.h" + +#include <RobotAPI/interface/ArViz/Elements.h> + +#include <Inventor/nodes/SoCoordinate3.h> +#include <Inventor/nodes/SoDrawStyle.h> +#include <Inventor/nodes/SoFaceSet.h> +#include <Inventor/nodes/SoLineSet.h> + +namespace armarx::viz::coin +{ + struct VisuDataPolygon : TypedVisuData<SoSeparator> + { + VisuDataPolygon() + { + coordinate3 = new SoCoordinate3; + + faceSet = new SoFaceSet; + + // create line around polygon + SoSeparator* lineSep = new SoSeparator; + + lineMaterial = new SoMaterial; + lineSep->addChild(lineMaterial); + lineSep->addChild(coordinate3); + + lineStyle = new SoDrawStyle(); + lineSep->addChild(lineStyle); + + lineSet = new SoLineSet; + lineSep->addChild(lineSet); + + auto* sep = node(); + sep->addChild(coordinate3); + sep->addChild(faceSet); + sep->addChild(lineSep); + } + SoCoordinate3* coordinate3; + SoFaceSet* faceSet; + SoDrawStyle* lineStyle; + SoLineSet* lineSet; + SoMaterial* lineMaterial; + }; + + struct VisualizerPolygon : TypedElementVisualizer<VisualizerPolygon, ElementPolygon, VisuDataPolygon> + { + void updateElement(ElementType const& element, DataType* data) + { + int pointSize = (int)element.points.size(); + + SoCoordinate3* coordinate3 = data->coordinate3; + int i = 0; + coordinate3->point.setNum(pointSize + 1); + for (auto& point : element.points) + { + SbVec3f pt(point.e0, point.e1, point.e2); + coordinate3->point.set1Value(i, pt); + i += 1; + } + + if (pointSize > 0) + { + // Add the first element as last element to close the loop + auto& first = element.points.front(); + SbVec3f pt0(first.e0, first.e1, first.e2); + coordinate3->point.set1Value(pointSize, pt0); + } + + data->faceSet->numVertices.set1Value(0, pointSize); + + // Line around polygon + const float conv = 1.0f / 255.0f; + float r = element.lineColor.r * conv; + float g = element.lineColor.g * conv; + float b = element.lineColor.b * conv; + float a = element.lineColor.a * conv; + data->lineMaterial->diffuseColor.setValue(r, g, b); + data->lineMaterial->ambientColor.setValue(r, g, b); + data->lineMaterial->transparency.setValue(1.0f - a); + + data->lineStyle->lineWidth.setValue(element.lineWidth); + + data->lineSet->numVertices.set1Value(0, pointSize + 1); + } + }; +} diff --git a/source/RobotAPI/components/ArVizExample/ArVizExample.cpp b/source/RobotAPI/components/ArVizExample/ArVizExample.cpp index 64e7eaf80c94b3a1dade213ebbc739bd929e6c9e..36d37145cb76caf994b43279703aca8dec0cdae0 100644 --- a/source/RobotAPI/components/ArVizExample/ArVizExample.cpp +++ b/source/RobotAPI/components/ArVizExample/ArVizExample.cpp @@ -200,6 +200,28 @@ void ArVizExample::update() layer.elements.push_back(circle); } + { + viz::ElementPolygonPtr poly(new viz::ElementPolygon); + poly->id = "poly"; + poly->action = viz::Element_UPDATE; + + poly->color = armarx::viz::Color{128, 0, 128, 255}; + poly->lineColor = {255, 0, 0, 255}; + poly->lineWidth = 10.0f; + + float t = 1.0f + std::sin(timeInSeconds); + float offset = 50.0f * t; + poly->points.push_back(armarx::Vector3f{-200.0f - offset, -200.0f - offset, 0.0f}); + poly->points.push_back(armarx::Vector3f{-200.0f, +200.0f, 0.0f}); + poly->points.push_back(armarx::Vector3f{+200.0f + offset, +200.0f + offset, 0.0f}); + poly->points.push_back(armarx::Vector3f{+200.0f, -200.0f, 0.0f}); + + Eigen::Vector3f pos = Eigen::Vector3f::Zero(); + pos.z() = +1000.0f; + poly->pose = makeGlobalPose(pos, Eigen::Quaternionf::Identity()); + + layer.elements.push_back(poly); + } topic->updateLayers({layer}); diff --git a/source/RobotAPI/interface/ArViz/Elements.ice b/source/RobotAPI/interface/ArViz/Elements.ice index 9e9fa13153f783b115e22aadf744787bd1c493f6..3c02903397a3509ca09a4bfda54cb9ace3af000e 100644 --- a/source/RobotAPI/interface/ArViz/Elements.ice +++ b/source/RobotAPI/interface/ArViz/Elements.ice @@ -98,7 +98,21 @@ module viz float width = 10.0f; }; - // TODO: Point cloud stuff + struct ColoredPoint + { + float x; + float y; + float z; + Color color; + }; + + sequence<ColoredPoint> ColoredPointList; + + class ElementPointCloud extends Element + { + ColoredPointList points; + float transparency = 0.0f; + }; // TODO: TriMesh: Use format like Simox