diff --git a/CMakeLists.txt b/CMakeLists.txt
index a0f73bd4fe3434516a9528fe308fe8c2c25ce29f..f6aa2024fb4bb00397189d1dc3d17239ff5d4a2f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -14,7 +14,7 @@ set(ARMARX_ENABLE_AUTO_CODE_FORMATTING TRUE)
armarx_project("RobotAPI")
depends_on_armarx_package(ArmarXGui "OPTIONAL")
-set(ArmarX_Simox_VERSION 2.3.16)
+set(ArmarX_Simox_VERSION 2.3.25)
find_package(Simox ${ArmarX_Simox_VERSION} QUIET)
if (Simox_FOUND)
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
index 599a5182e51e34346a68d4556b9092696de78a20..099c5cf601995e3c65e52c0efa4235b8c851e745 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
@@ -43,6 +43,7 @@
#include <Inventor/nodes/SoTranslation.h>
#include <Inventor/nodes/SoMaterialBinding.h>
#include <Inventor/nodes/SoDrawStyle.h>
+#include <Inventor/nodes/SoLineSet.h>
#include <Inventor/SbVec3f.h>
#include <Inventor/fields/SoMFVec3f.h>
#include <Inventor/fields/SoMFColor.h>
@@ -440,6 +441,85 @@ namespace armarx
ARMARX_DEBUG << "drawLine2" << flush;
}
+ void DebugDrawerComponent::drawLineSet(const DebugDrawerComponent::LineSetData& d)
+ {
+ ScopedRecursiveLockPtr l = getScopedVisuLock();
+ ARMARX_DEBUG << "drawLineSet";
+
+ auto& layer = requestLayer(d.layerName);
+
+ removeBox(d.layerName, d.name);
+
+ if (!d.active)
+ {
+ return;
+ }
+
+ if (d.lineSet.points.size() % 2 != 0)
+ {
+ ARMARX_WARNING << "A line set requires an even amount of points";
+ return;
+ }
+
+ if (d.lineSet.points.size() != 2 * d.lineSet.intensities.size())
+ {
+ ARMARX_WARNING << "Amounts of points and intensities have to match for a line set";
+ return;
+ }
+
+ // Initialize color map for affordance visualization
+ std::vector<VirtualRobot::VisualizationFactory::Color> colors;
+ colors.push_back(VirtualRobot::VisualizationFactory::Color(d.lineSet.colorNoIntensity.r, d.lineSet.colorNoIntensity.g, d.lineSet.colorNoIntensity.b));
+ colors.push_back(VirtualRobot::VisualizationFactory::Color(d.lineSet.colorFullIntensity.r, d.lineSet.colorFullIntensity.g, d.lineSet.colorFullIntensity.b));
+ VirtualRobot::ColorMap visualizationColorMap = VirtualRobot::ColorMap::customColorMap(colors);
+
+ // Initialize visualization nodes
+ SoSeparator* sep = new SoSeparator;
+
+ SoMaterialBinding* binding = new SoMaterialBinding;
+ binding->value = SoMaterialBinding::PER_PART;
+ sep->addChild(binding);
+
+ SoDrawStyle* lineStyle = new SoDrawStyle;
+ lineStyle->lineWidth.setValue(d.lineSet.lineWidth);
+ sep->addChild(lineStyle);
+
+ SoCoordinate3* coordinateNode = new SoCoordinate3;
+ sep->addChild(coordinateNode);
+
+ SoMaterial* materialNode = new SoMaterial;
+ sep->addChild(materialNode);
+
+ SoLineSet* lineSetNode = new SoLineSet;
+ lineSetNode->startIndex.setValue(0);
+ sep->addChild(lineSetNode);
+
+
+ // Allocate index and material arrays
+ SbVec3f* coordinateValues = new SbVec3f[d.lineSet.points.size()];
+ int32_t* lineSetValues = new int32_t[d.lineSet.intensities.size()];
+ SbColor* colorValues = new SbColor[d.lineSet.intensities.size()];
+
+ for (unsigned int i = 0; i < d.lineSet.intensities.size(); i++)
+ {
+ lineSetValues[i] = 2;
+
+ coordinateValues[2 * i].setValue(d.lineSet.points[2 * i].x, d.lineSet.points[2 * i].y, d.lineSet.points[2 * i].z);
+ coordinateValues[2 * i + 1].setValue(d.lineSet.points[2 * i + 1].x, d.lineSet.points[2 * i + 1].y, d.lineSet.points[2 * i + 1].z);
+
+ VirtualRobot::VisualizationFactory::Color c = visualizationColorMap.getColor(d.lineSet.intensities[i]);
+ colorValues[i].setValue(c.r, c.g, c.b);
+ }
+
+ coordinateNode->point.setValuesPointer(d.lineSet.points.size(), coordinateValues);
+ lineSetNode->numVertices.setValuesPointer(d.lineSet.intensities.size(), lineSetValues);
+ materialNode->ambientColor.setValuesPointer(d.lineSet.intensities.size(), colorValues);
+ materialNode->diffuseColor.setValuesPointer(d.lineSet.intensities.size(), colorValues);
+
+ layer.addedLineSetVisualizations[d.name] = sep;
+ layer.mainNode->addChild(sep);
+ }
+
void DebugDrawerComponent::drawBox(const BoxData& d)
{
ScopedRecursiveLockPtr l = getScopedVisuLock();
@@ -908,6 +988,18 @@ namespace armarx
layer.addedLineVisualizations.erase(name);
}
+ void DebugDrawerComponent::removeLineSet(const std::string& layerName, const std::string& name)
+ {
+ {
+ ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ std::string entryName = "__" + layerName + "__" + name + "__";
+ LineSetData& d = accumulatedUpdateData.lineSet[entryName];
+ d.layerName = layerName;
+ d.name = name;
+ d.active = false;
+ }
+ }
+
void DebugDrawerComponent::removeBox(const std::string& layerName, const std::string& name)
{
ScopedRecursiveLockPtr l = getScopedVisuLock();
@@ -1259,6 +1351,35 @@ namespace armarx
removeLineVisu(DEBUG_LAYER_NAME, lineName);
}
+ void DebugDrawerComponent::setLineSetVisu(const std::string& layerName, const std::string& lineSetName, const DebugDrawerLineSet& lineSet, const Ice::Current&)
+ {
+ ARMARX_DEBUG << VAROUT(layerName) << VAROUT(lineSetName);
+ {
+ ScopedRecursiveLockPtr l = getScopedAccumulatedDataLock();
+ std::string entryName = "__" + layerName + "__" + lineSetName + "__";
+ LineSetData& d = accumulatedUpdateData.lineSet[entryName];
+ d.lineSet = lineSet;
+ d.layerName = layerName;
+ d.name = lineSetName;
+ d.active = true;
+ }
+ }
+
+ void DebugDrawerComponent::setLineSetDebugLayerVisu(const std::string& lineSetName, const DebugDrawerLineSet& lineSet, const Ice::Current&)
+ {
+ setLineSetVisu(DEBUG_LAYER_NAME, lineSetName, lineSet);
+ }
+
+ void DebugDrawerComponent::removeLineSetVisu(const std::string& layerName, const std::string& lineSetName, const Ice::Current&)
+ {
+
+ }
+
+ void DebugDrawerComponent::removeLineSetDebugLayerVisu(const std::string& lineSetName, const Ice::Current&)
+ {
+ removeLineSetVisu(DEBUG_LAYER_NAME, lineSetName);
+ }
+
void DebugDrawerComponent::setBoxVisu(const std::string& layerName, const std::string& boxName, const PoseBasePtr& globalPose, const Vector3BasePtr& dimensions, const DrawColor& color, const Ice::Current&)
{
Eigen::Matrix4f gp = PosePtr::dynamicCast(globalPose)->toEigen();
@@ -1828,6 +1949,13 @@ namespace armarx
accumulatedUpdateData.line.clear();
+ for (auto i = accumulatedUpdateData.lineSet.begin(); i != accumulatedUpdateData.lineSet.end(); i++)
+ {
+ drawLineSet(i->second);
+ }
+
+ accumulatedUpdateData.lineSet.clear();
+
for (auto i = accumulatedUpdateData.sphere.begin(); i != accumulatedUpdateData.sphere.end(); i++)
{
drawSphere(i->second);
@@ -2008,6 +2136,22 @@ namespace armarx
}
}
+ {
+ auto i1 = accumulatedUpdateData.lineSet.begin();
+
+ while (i1 != accumulatedUpdateData.lineSet.end())
+ {
+ if (boost::starts_with(i1->first, entryName))
+ {
+ i1 = accumulatedUpdateData.lineSet.erase(i1);
+ }
+ else
+ {
+ i1++;
+ }
+ }
+ }
+
{
auto i1 = accumulatedUpdateData.sphere.begin();
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
index c231930010733505f544495a5f8e02fd5b372855..63aa758748bf9de5222e0e1e8b0b0850ccb1c6c6 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
@@ -157,6 +157,11 @@ namespace armarx
virtual void removeLineVisu(const std::string& layerName, const std::string& lineName, const ::Ice::Current& = ::Ice::Current());
virtual void removeLineDebugLayerVisu(const std::string& lineName, const ::Ice::Current& = ::Ice::Current());
+ virtual void setLineSetVisu(const std::string& layerName, const std::string& lineSetName, const DebugDrawerLineSet& lineSet, const ::Ice::Current& = ::Ice::Current());
+ virtual void setLineSetDebugLayerVisu(const std::string& lineSetName, const DebugDrawerLineSet& lineSet, const ::Ice::Current& = ::Ice::Current());
+ virtual void removeLineSetVisu(const std::string& layerName, const std::string& lineSetName, const ::Ice::Current& = ::Ice::Current());
+ virtual void removeLineSetDebugLayerVisu(const std::string& lineSetName, const ::Ice::Current& = ::Ice::Current());
+
virtual void setBoxVisu(const std::string& layerName, const std::string& boxName, const ::armarx::PoseBasePtr& globalPose, const ::armarx::Vector3BasePtr& dimensions, const DrawColor& color, const ::Ice::Current& = ::Ice::Current());
virtual void setBoxDebugLayerVisu(const std::string& boxName, const ::armarx::PoseBasePtr& globalPose, const ::armarx::Vector3BasePtr& dimensions, const DrawColor& color, const ::Ice::Current& = ::Ice::Current());
virtual void removeBoxVisu(const std::string& layerName, const std::string& boxName, const ::Ice::Current& = ::Ice::Current());
@@ -301,6 +306,10 @@ namespace armarx
float scale;
VirtualRobot::VisualizationFactory::Color color;
};
+ struct LineSetData : public DrawData
+ {
+ DebugDrawerLineSet lineSet;
+ };
struct BoxData : public DrawData
{
Eigen::Matrix4f globalPose;
@@ -384,6 +393,7 @@ namespace armarx
{
std::map<std::string, CoordData> coord;
std::map<std::string, LineData> line;
+ std::map<std::string, LineSetData> lineSet;
std::map<std::string, BoxData> box;
std::map<std::string, TextData> text;
std::map<std::string, SphereData> sphere;
@@ -414,6 +424,7 @@ namespace armarx
void drawCoordSystem(const CoordData& d);
void drawLine(const LineData& d);
+ void drawLineSet(const LineSetData& d);
void drawBox(const BoxData& d);
void drawText(const TextData& d);
void drawSphere(const SphereData& d);
@@ -427,6 +438,7 @@ namespace armarx
void removeCoordSystem(const std::string& layerName, const std::string& name);
void removeLine(const std::string& layerName, const std::string& name);
+ void removeLineSet(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);
@@ -455,6 +467,7 @@ namespace armarx
SoSeparator* mainNode;
std::map<std::string, SoSeparator*> addedCoordVisualizations;
std::map<std::string, SoSeparator*> addedLineVisualizations;
+ std::map<std::string, SoSeparator*> addedLineSetVisualizations;
std::map<std::string, SoSeparator*> addedBoxVisualizations;
std::map<std::string, SoSeparator*> addedTextVisualizations;
std::map<std::string, SoSeparator*> addedSphereVisualizations;
diff --git a/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice b/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
index 198ff86013a71e0cf4c6131b53aaac18a90e8e3a..8ffc400d4901806595a88f52333bea04cedc4486 100644
--- a/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
+++ b/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
@@ -109,6 +109,19 @@ module armarx
float pointSize = three;
};
+ sequence<DebugDrawerPointCloudElement> DebugDrawerLineSetPointList;
+ sequence<float> DebugDrawerLineSetIntensityList;
+
+ struct DebugDrawerLineSet
+ {
+ float lineWidth;
+ DrawColor colorNoIntensity;
+ DrawColor colorFullIntensity;
+
+ DebugDrawerLineSetPointList points;
+ DebugDrawerLineSetIntensityList intensities;
+ };
+
sequence< Vector3Base > PolygonPointList;
enum DrawStyle { FullModel, CollisionModel };
@@ -134,6 +147,7 @@ module armarx
void setPoseVisu(string layerName, string poseName, PoseBase globalPose);
void setScaledPoseVisu(string layerName, string poseName, PoseBase globalPose, float scale);
void setLineVisu(string layerName, string lineName, Vector3Base globalPosition1, Vector3Base globalPosition2, float lineWidth, DrawColor color);
+ void setLineSetVisu(string layerName, string lineSetName, DebugDrawerLineSet lineSet);
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);
@@ -174,6 +188,7 @@ module armarx
void setPoseDebugLayerVisu(string poseName, PoseBase globalPose);
void setScaledPoseDebugLayerVisu(string poseName, PoseBase globalPose, float scale);
void setLineDebugLayerVisu(string lineName, Vector3Base globalPosition1, Vector3Base globalPosition2, float lineWidth, DrawColor color);
+ void setLineSetDebugLayerVisu(string lineSetName, DebugDrawerLineSet lineSet);
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);
@@ -189,6 +204,7 @@ module armarx
*/
void removePoseVisu(string layerName, string poseName);
void removeLineVisu(string layerName, string lineName);
+ void removeLineSetVisu(string layerName, string lineSetName);
void removeBoxVisu(string layerName, string boxName);
void removeTextVisu(string layerName, string textName);
void removeSphereVisu(string layerName, string sphereName);
@@ -205,6 +221,7 @@ module armarx
*/
void removePoseDebugLayerVisu(string poseName);
void removeLineDebugLayerVisu(string lineName);
+ void removeLineSetDebugLayerVisu(string lineSetName);
void removeBoxDebugLayerVisu(string boxName);
void removeTextDebugLayerVisu(string textName);
void removeSphereDebugLayerVisu(string sphereName);