Improve DebugDrawerHelper

* add draw functions for sequences of poses or lines

source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp

@@ -25,6 +25,8 @@
 
 #include <VirtualRobot/math/Helpers.h>
 
+#include <ArmarXCore/util/CPPUtility/Iterator.h>
+
 #include <RobotAPI/libraries/core/Pose.h>
 
 using namespace math;
@@ -38,6 +40,7 @@ DebugDrawerHelper::DebugDrawerHelper(const DebugDrawerInterfacePrx& debugDrawerP
 {
 }
 
+//1st order
 void DebugDrawerHelper::drawPose(const std::string& name, const Eigen::Matrix4f& pose)
 {
     CHECK_AND_ADD(name, DrawElementType::Pose)
@@ -74,12 +77,6 @@ void DebugDrawerHelper::drawLine(const std::string& name, const Eigen::Vector3f&
     debugDrawerPrx->setLineVisu(layerName, name, makeGlobal(p1), makeGlobal(p2), width, color);
 }
 
-void DebugDrawerHelper::drawLine(const std::string& name, const Eigen::Vector3f& p1, const Eigen::Vector3f& p2)
-{
-    CHECK_AND_ADD(name, DrawElementType::Line)
-    drawLine(name, p1, p2, defaults.lineWidth, defaults.lineColor);
-}
-
 void DebugDrawerHelper::drawText(const std::string& name, const Eigen::Vector3f& p1, const std::string& text, const DrawColor& color, int size)
 {
     CHECK_AND_ADD(name, DrawElementType::Text)
@@ -123,6 +120,62 @@ void DebugDrawerHelper::setRobotConfig(const std::string& name, const std::map<s
     debugDrawerPrx->updateRobotConfig(layerName, name, config);
 }
 
+//2nd order
+void DebugDrawerHelper::drawPoses(const std::string& prefix, const std::vector<Eigen::Matrix4f>& poses)
+{
+    for (const auto& [idx, pose] : MakeIndexedContainer(poses))
+    {
+        drawPose(prefix + std::to_string(idx), pose);
+    }
+}
+void DebugDrawerHelper::drawPoses(const std::string& prefix, const std::vector<Eigen::Matrix4f>& poses, float scale)
+{
+    for (const auto& [idx, pose] : MakeIndexedContainer(poses))
+    {
+        drawPose(prefix + std::to_string(idx), pose, scale);
+    }
+}
+
+void DebugDrawerHelper::drawLine(const std::string& name, const Eigen::Vector3f& p1, const Eigen::Vector3f& p2)
+{
+    drawLine(name, p1, p2, defaults.lineWidth, defaults.lineColor);
+}
+
+void DebugDrawerHelper::drawLines(const std::string& prefix, const std::vector<Eigen::Vector3f>& ps, float width, const DrawColor& color)
+{
+    for (std::size_t idx = 1; idx < ps.size(); ++idx)
+    {
+        drawLine(prefix + std::to_string(idx), ps.at(idx - 1), ps.at(idx), width, color);
+    }
+}
+void DebugDrawerHelper::drawLines(const std::string& prefix, const std::vector<Eigen::Vector3f>& ps)
+{
+    for (std::size_t idx = 1; idx < ps.size(); ++idx)
+    {
+        drawLine(prefix + std::to_string(idx), ps.at(idx - 1), ps.at(idx));
+    }
+}
+void DebugDrawerHelper::drawLines(const std::string& prefix, const std::vector<Eigen::Matrix4f>& ps, float width, const DrawColor& color)
+{
+    for (std::size_t idx = 1; idx < ps.size(); ++idx)
+    {
+        drawLine(prefix + std::to_string(idx),
+                 ps.at(idx - 1).topRightCorner<3, 1>(),
+                 ps.at(idx).topRightCorner<3, 1>(),
+                 width, color);
+    }
+}
+void DebugDrawerHelper::drawLines(const std::string& prefix, const std::vector<Eigen::Matrix4f>& ps)
+{
+    for (std::size_t idx = 1; idx < ps.size(); ++idx)
+    {
+        drawLine(prefix + std::to_string(idx),
+                 ps.at(idx - 1).topRightCorner<3, 1>(),
+                 ps.at(idx).topRightCorner<3, 1>());
+    }
+}
+
+//utility
 void DebugDrawerHelper::clearLayer()
 {
     debugDrawerPrx->clearLayer(layerName);

source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h

@@ -69,6 +69,7 @@ namespace armarx
 
         DebugDrawerHelper(const DebugDrawerInterfacePrx& debugDrawerPrx, const std::string& layerName, const VirtualRobot::RobotPtr& robot);
 
+        //1st order draw functions (direct calls to proxy)
         void drawPose(const std::string& name, const Eigen::Matrix4f& pose);
         void drawPose(const std::string& name, const Eigen::Matrix4f& pose, float scale);
 
@@ -77,7 +78,6 @@ namespace armarx
         void drawBox(const std::string& name, const Eigen::Matrix4f& pose, const Eigen::Vector3f& size, const DrawColor& color);
 
         void drawLine(const std::string& name, const Eigen::Vector3f& p1, const Eigen::Vector3f& p2, float width, const DrawColor& color);
-        void drawLine(const std::string& name, const Eigen::Vector3f& p1, const Eigen::Vector3f& p2);
 
         void drawText(const std::string& name, const Eigen::Vector3f& p1, const std::string& text, const DrawColor& color, int size);
 
@@ -90,6 +90,18 @@ namespace armarx
         void drawRobot(const std::string& name, const std::string& robotFile, const std::string& armarxProject, const Eigen::Matrix4f& pose, const DrawColor& color);
         void setRobotConfig(const std::string& name, const std::map<std::string, float>& config);
 
+        //2nd order draw functions (call 1st order)
+        void drawPoses(const std::string& prefix, const std::vector<Eigen::Matrix4f>& poses);
+        void drawPoses(const std::string& prefix, const std::vector<Eigen::Matrix4f>& poses, float scale);
+
+        void drawLine(const std::string& name, const Eigen::Vector3f& p1, const Eigen::Vector3f& p2);
+
+        void drawLines(const std::string& prefix, const std::vector<Eigen::Vector3f>& ps, float width, const DrawColor& color);
+        void drawLines(const std::string& prefix, const std::vector<Eigen::Vector3f>& ps);
+        void drawLines(const std::string& prefix, const std::vector<Eigen::Matrix4f>& ps, float width, const DrawColor& color);
+        void drawLines(const std::string& prefix, const std::vector<Eigen::Matrix4f>& ps);
+
+        //utility
         void clearLayer();
         void cyclicCleanup();