Added drawBox

source/RobotAPI/libraries/core/DebugDrawerTopic.cpp

@@ -82,7 +82,25 @@ namespace armarx
         }
     }
 
-    
+    void DebugDrawerTopic::drawBox(
+        const Eigen::Vector3f& position, const Eigen::Quaternionf& orientation,
+        const Eigen::Vector3f& extents,
+        const DrawColor& color, const std::string& name, const std::string& layer)
+    {
+        if (topic)
+        {
+            topic->setBoxVisu(layer, name, new Pose(new Vector3(position), new Quaternion(orientation)),
+                              new Vector3(extents), color);
+        }
+    }
+
+    void DebugDrawerTopic::drawBox(
+        const Eigen::Matrix4f& pose, const Eigen::Vector3f& extents,
+        const DrawColor& color, const std::string& name, const std::string& layer)
+    {
+        drawBox(math::Helpers::Position(pose), Eigen::Quaternionf(math::Helpers::Orientation(pose)),
+                extents, color, name, layer);
+    }
 
 
     void DebugDrawerTopic::drawPose(

source/RobotAPI/libraries/core/DebugDrawerTopic.h

@@ -25,7 +25,8 @@ namespace armarx
      * Methods arguments are ordered from specific to general. That is, common
      * arguments such as layer, name and color always come last. Less common
      * parameters (e.g. width of arrow/line), come before them. The first
-     * arguments are those specific to the respective function.
+     * arguments are those specific to the respective function (although
+     * pose-related arguments typically come first).
      * (Added methods should follow this pattern.)
      *
      * All methods check whether the internal topic proxy is set, and do
@@ -75,8 +76,14 @@ namespace armarx
         void sleepFor(const DurationT& duration);
 
 
-        void drawPose(const Eigen::Matrix4f& pose, const
-                      std::string& name, const std::string& layer = "pose");
+        void drawBox(const Eigen::Vector3f& position, const Eigen::Quaternionf& orientation,
+                     const Eigen::Vector3f& extents,
+                     const DrawColor& color, const std::string& name, const std::string& layer);
+        void drawBox(const Eigen::Matrix4f& pose, const Eigen::Vector3f& extents,
+                     const DrawColor& color, const std::string& name, const std::string& layer);
+
+        void drawPose(const Eigen::Matrix4f& pose,
+                      const std::string& name, const std::string& layer = "pose");
         void drawPose(const Eigen::Vector3f& pos, const Eigen::Quaternionf& ori,
                       const std::string& name, const std::string& layer = "pose");