diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp b/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp index d4d68c30e9a198e5a50ba7238774f2cfaf3cb9b5..88b1806f31933d8d4b188b9005968f3017be4327 100644 --- a/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp +++ b/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); diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h b/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h index 334fa002eaa8cc1c8be7023757a48edd9cf5e620..521ed9cf56d9fbabbc81be3bb41337912d628ce5 100644 --- a/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h +++ b/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();