Text 2D Visualizations

ci/install_magnum.sh

@@ -16,7 +16,7 @@ if [ "$TRAVIS_OS_NAME" == "linux" ]; then
     cd magnum
     mkdir build && cd build
     # Ubuntu
-    cmake -DCMAKE_BUILD_TYPE=Release -DWITH_AUDIO=ON -DWITH_DEBUGTOOLS=ON -DWITH_GL=ON -DWITH_MESHTOOLS=ON -DWITH_PRIMITIVES=ON -DWITH_SCENEGRAPH=ON -DWITH_SHADERS=ON -DWITH_TEXT=ON -DWITH_TEXTURETOOLS=ON -DWITH_TRADE=ON -DWITH_GLFWAPPLICATION=ON -DWITH_WINDOWLESSGLXAPPLICATION=ON -DWITH_OPENGLTESTER=ON -DWITH_ANYAUDIOIMPORTER=ON -DWITH_ANYIMAGECONVERTER=ON -DWITH_ANYIMAGEIMPORTER=ON -DWITH_ANYSCENEIMPORTER=ON -DWITH_MAGNUMFONT=ON -DWITH_OBJIMPORTER=ON -DWITH_TGAIMPORTER=ON -DWITH_WAVAUDIOIMPORTER=ON .. # this will enable almost all features of Magnum that are not necessarily needed for robot_dart (please refer to the documentation of Magnum for more details on selecting only the ones that you need)
+    cmake -DCMAKE_BUILD_TYPE=Release -DWITH_AUDIO=ON -DWITH_DEBUGTOOLS=ON -DWITH_GL=ON -DWITH_MESHTOOLS=ON -DWITH_PRIMITIVES=ON -DWITH_SCENEGRAPH=ON -DWITH_SHADERS=ON -DWITH_TEXT=ON -DWITH_TEXTURETOOLS=ON -DWITH_TRADE=ON -DWITH_GLFWAPPLICATION=ON -DWITH_WINDOWLESSGLXAPPLICATION=ON -DWITH_OPENGLTESTER=ON -DWITH_ANYAUDIOIMPORTER=ON -DWITH_ANYIMAGECONVERTER=ON -DWITH_ANYIMAGEIMPORTER=ON -DWITH_ANYSCENEIMPORTER=ON -DWITH_MAGNUMFONT=ON -DWITH_OBJIMPORTER=ON -DWITH_TGAIMPORTER=ON -DWITH_WAVAUDIOIMPORTER=ON ..
     make -j
     sudo make install
     cd ../..
@@ -25,7 +25,7 @@ if [ "$TRAVIS_OS_NAME" == "linux" ]; then
     git clone https://github.com/mosra/magnum-plugins.git
     cd magnum-plugins
     mkdir build && cd build
-    cmake -DCMAKE_BUILD_TYPE=Release -DWITH_ASSIMPIMPORTER=ON -DWITH_DDSIMPORTER=ON -DWITH_JPEGIMPORTER=ON -DWITH_OPENGEXIMPORTER=ON -DWITH_PNGIMPORTER=ON -DWITH_TINYGLTFIMPORTER=ON .. # this will enable quite a few Magnum Plugins that are not necessarily needed for robot_dart (please refer to the documentation of Magnum for more details on selecting only the ones that you need)
+    cmake -DCMAKE_BUILD_TYPE=Release -DWITH_ASSIMPIMPORTER=ON -DWITH_DDSIMPORTER=ON -DWITH_JPEGIMPORTER=ON -DWITH_OPENGEXIMPORTER=ON -DWITH_PNGIMPORTER=ON -DWITH_TINYGLTFIMPORTER=ON -DWITH_STBTRUETYPEFONT=ON ..
     make -j
     sudo make install
     cd ../..
@@ -46,6 +46,6 @@ else
     # We need a newer version than 2019.10 for Magnum
     HOMEBREW_NO_AUTO_UPDATE=1 brew install --HEAD mosra/magnum/corrade
     HOMEBREW_NO_AUTO_UPDATE=1 brew install --HEAD mosra/magnum/magnum
-    HOMEBREW_NO_AUTO_UPDATE=1 brew install --HEAD mosra/magnum/magnum-plugins
-    HOMEBREW_NO_AUTO_UPDATE=1 brew install --HEAD mosra/magnum/magnum-integration
+    HOMEBREW_NO_AUTO_UPDATE=1 brew install --HEAD mosra/magnum/magnum-plugins --with-assimp
+    HOMEBREW_NO_AUTO_UPDATE=1 brew install --HEAD mosra/magnum/magnum-integration --with-dartsim --with-eigen
 fi

src/examples/cameras.cpp

@@ -9,7 +9,13 @@
 class MyApp : public robot_dart::gui::magnum::GlfwApplication {
 public:
     explicit MyApp(int argc, char** argv, robot_dart::RobotDARTSimu* simu, const robot_dart::gui::magnum::GraphicsConfiguration& configuration = robot_dart::gui::magnum::GraphicsConfiguration())
-        : GlfwApplication(argc, argv, simu, configuration) {}
+        : GlfwApplication(argc, argv, simu, configuration)
+    {
+        // we synchronize by default if we have the graphics activated
+        simu->scheduler().set_sync(true);
+        // enable summary text when graphics activated
+        simu->enable_summary_text(true);
+    }
 
 protected:
     void keyPressEvent(KeyEvent& event) override

src/python/eigen.cpp

@@ -0,0 +1,135 @@
+#include "robot_dart.hpp"
+
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
+
+#include <pybind11/eigen.h>
+
+namespace robot_dart {
+    namespace python {
+        void py_eigen(py::module& module)
+        {
+            // The following is copied and adapted from DART: https://github.com/dartsim/dart/blob/c9186b042c406835984e76ae3b9bdd8579cd0312/python/dartpy/eigen_geometry_pybind.cpp#L131
+            using T = double;
+
+            // Do not return references to matrices (e.g. `Eigen::Ref<>`) so that we have
+            // tighter control over validation.
+
+            auto m = module.def_submodule("math");
+
+            // Affine2d
+            {
+                using Class = Eigen::Transform<T, 2, Eigen::Affine>;
+                ::pybind11::class_<Class> py_class(m, "Affine2");
+                py_class
+                    .def(::pybind11::init([]() {
+                        return Class::Identity();
+                    }))
+                    .def_static("Identity", []() {
+                        return Class::Identity();
+                    })
+                    .def(::pybind11::init([](const Eigen::Matrix<T, 3, 3>& matrix) {
+                        Class out(matrix);
+                        return out;
+                    }),
+                        ::pybind11::arg("matrix"))
+                    .def(::pybind11::init([](const Eigen::Matrix<T, 2, 2>& rotation, const Eigen::Matrix<T, 2, 1>& translation) {
+                        Class out = Class::Identity();
+                        out.linear() = rotation;
+                        out.translation() = translation;
+                        return out;
+                    }),
+                        ::pybind11::arg("rotation"), ::pybind11::arg("translation"))
+                    .def(::pybind11::init([](const Class& other) {
+                        return other;
+                    }),
+                        ::pybind11::arg("other"))
+                    .def("matrix", [](const Class* self) -> Eigen::Matrix<T, 3, 3> {
+                        return self->matrix();
+                    })
+                    .def("set_matrix", [](Class* self, const Eigen::Matrix<T, 3, 3>& matrix) {
+                        Class update(matrix);
+                        *self = update;
+                    })
+                    .def("translation", [](const Class* self) -> Eigen::Matrix<T, 2, 1> {
+                        return self->translation();
+                    })
+                    .def("set_translation", [](Class* self, const Eigen::Matrix<T, 2, 1>& translation) {
+                        self->translation() = translation;
+                    })
+                    .def("rotation", [](const Class* self) -> Eigen::Matrix<T, 2, 2> {
+                        return self->linear();
+                    })
+                    .def("set_rotation", [](Class* self, const Eigen::Matrix<T, 2, 2>& rotation) {
+                        self->linear() = rotation;
+                    })
+                    .def("__str__", [](::pybind11::object self) {
+                        return ::pybind11::str(self.attr("matrix")());
+                    })
+                    // Do not define operator `__mul__` until we have the Python3 `@`
+                    // operator so that operations are similar to those of arrays.
+                    .def(
+                        "multiply", [](const Class& self, const Class& other) {
+                            return self * other;
+                        },
+                        ::pybind11::arg("other"))
+                    .def(
+                        "multiply", [](const Class& self, const Eigen::Matrix<T, 2, 1>& position) {
+                            return self * position;
+                        },
+                        ::pybind11::arg("position"))
+                    .def("inverse", [](const Class* self) {
+                        return self->inverse();
+                    })
+                    //============================
+                    // Begin: added by robot_dart
+                    //============================
+                    .def(
+                        "translate", [](Class* self, const Eigen::Matrix<T, 2, 1>& other) {
+                            self->translate(other);
+                        },
+                        ::pybind11::arg("other"))
+                    .def(
+                        "pretranslate", [](Class* self, const Eigen::Matrix<T, 2, 1>& other) {
+                            self->pretranslate(other);
+                        },
+                        ::pybind11::arg("other"))
+                    .def(
+                        "rotate", [](Class* self, const Eigen::Matrix<T, 2, 2>& other) {
+                            self->rotate(other);
+                        },
+                        ::pybind11::arg("other"))
+                    .def(
+                        "prerotate", [](Class* self, const Eigen::Matrix<T, 2, 2>& other) {
+                            self->prerotate(other);
+                        },
+                        ::pybind11::arg("other"))
+                    .def(
+                        "scale", [](Class* self, const Eigen::Matrix<T, 2, 1>& other) {
+                            self->scale(other);
+                        },
+                        ::pybind11::arg("other"))
+                    .def(
+                        "prescale", [](Class* self, const Eigen::Matrix<T, 2, 1>& other) {
+                            self->prescale(other);
+                        },
+                        ::pybind11::arg("other"))
+                    .def(
+                        "shear", [](Class* self, const T& sx, const T& sy) {
+                            self->shear(sx, sy);
+                        },
+                        ::pybind11::arg("sx"), ::pybind11::arg("sy"))
+                    // .def( // For some reason this does not compile
+                    //     "preshear", [](Class* self, const T& sx, const T& sy) {
+                    //         self->preshear(sx, sy);
+                    //     },
+                    //     ::pybind11::arg("sx"), ::pybind11::arg("sy"))
+                    //==========================
+                    // End: added by robot_dart
+                    //==========================
+                    ;
+                ::pybind11::implicitly_convertible<Eigen::Matrix<T, 3, 3>, Class>();
+            }
+        }
+    } // namespace python
+} // namespace robot_dart

src/python/gui.cpp

@@ -127,6 +127,9 @@ namespace robot_dart {
                     py::arg("look_at") = Eigen::Vector3d(0, 0, 0),
                     py::arg("up") = Eigen::Vector3d(0, 0, 1))
 
+                .def("width", &BaseWindowedGraphics::width)
+                .def("height", &BaseWindowedGraphics::height)
+
                 .def("clear_lights", &Graphics::clear_lights)
                 .def("add_light", &Graphics::add_light, py::keep_alive<2, 1>())
                 .def("lights", &Graphics::lights)
@@ -166,6 +169,9 @@ namespace robot_dart {
                     py::arg("look_at") = Eigen::Vector3d(0, 0, 0),
                     py::arg("up") = Eigen::Vector3d(0, 0, 1))
 
+                .def("width", &BaseWindowlessGraphics::width)
+                .def("height", &BaseWindowlessGraphics::height)
+
                 .def("clear_lights", &WindowlessGraphics::clear_lights)
                 .def("add_light", &WindowlessGraphics::add_light, py::keep_alive<2, 1>())
                 .def("lights", &WindowlessGraphics::lights)

src/python/robot_dart.cc

@@ -10,6 +10,7 @@ PYBIND11_MODULE(RobotDART, m)
 
     m.doc() = "RobotDART: Python API of robot_dart";
 
+    py_eigen(m);
     py_simu(m);
     py_robot(m);
     py_control(m);

src/python/robot_dart.hpp

@@ -9,6 +9,7 @@ namespace robot_dart {
         void py_control(py::module& m);
         void py_utils(py::module& m);
         void py_sensors(py::module& m);
+        void py_eigen(py::module& m);
 
 #ifdef GRAPHIC
         void py_gui(py::module& m);

src/python/simu.cpp

@@ -48,6 +48,18 @@ namespace robot_dart {
 
                 .def("__call__", &Descriptor::operator());
 
+            // TextData
+            using simu::TextData;
+            py::class_<TextData, std::shared_ptr<TextData>>(m, "TextData")
+                .def(py::init<const std::string&, const Eigen::Affine2d&, const Eigen::Vector4d&>(),
+                    py::arg("text"),
+                    py::arg("transformation") = Eigen::Affine2d::Identity(),
+                    py::arg("color") = Eigen::Vector4d(1, 1, 1, 1))
+
+                .def_readwrite("text", &TextData::text)
+                .def_readwrite("transformation", &TextData::transformation)
+                .def_readwrite("color", &TextData::color);
+
             // RobotDARTSimu class
             py::class_<RobotDARTSimu>(m, "RobotDARTSimu")
                 .def(py::init<double>(),
@@ -119,6 +131,14 @@ namespace robot_dart {
                 .def("remove_robot", (void (RobotDARTSimu::*)(size_t)) & RobotDARTSimu::remove_robot)
                 .def("clear_robots", &RobotDARTSimu::clear_robots)
 
+                .def("enable_summary_text", &RobotDARTSimu::enable_summary_text,
+                    py::arg("enable") = true)
+
+                .def("add_text", &RobotDARTSimu::add_text, py::return_value_policy::reference,
+                    py::arg("text"),
+                    py::arg("transformation") = Eigen::Affine2d::Identity(),
+                    py::arg("color") = Eigen::Vector4d(1, 1, 1, 1))
+
                 .def("add_floor", &RobotDARTSimu::add_floor,
                     py::arg("floor_width") = 10.,
                     py::arg("floor_height") = 0.1,

src/robot_dart/gui/base.hpp

@@ -27,6 +27,9 @@ namespace robot_dart {
             virtual Image image() { return Image(); }
             virtual GrayscaleImage depth_image() { return GrayscaleImage(); }
             virtual GrayscaleImage raw_depth_image() { return GrayscaleImage(); }
+
+            virtual size_t width() const { return 0; }
+            virtual size_t height() const { return 0; }
         };
     } // namespace gui
 } // namespace robot_dart

src/robot_dart/gui/magnum/base_application.cpp

@@ -8,6 +8,7 @@
 #include <dart/dynamics/SoftMeshShape.hpp>
 
 #include <Corrade/Containers/StridedArrayView.h>
+#include <Corrade/Utility/Resource.h>
 
 #include <Magnum/GL/CubeMapTexture.h>
 #include <Magnum/GL/DefaultFramebuffer.h>
@@ -160,6 +161,30 @@ namespace robot_dart {
                     .setCount(axis_data.indexCount())
                     .addVertexBuffer(std::move(axis_vertices), 0, Magnum::Shaders::VertexColor3D::Position{}, Magnum::Shaders::VertexColor3D::Color4{})
                     .setIndexBuffer(std::move(axis_indices), 0, compressed.second);
+
+                /* Initialize text visualization */
+                Corrade::Utility::Resource rs("RobotDARTShaders");
+                _font = _font_manager.loadAndInstantiate("TrueTypeFont");
+                if (_font) {
+                    _font->openData(rs.getRaw("SourceSansPro-Regular.ttf"), 180.0f);
+
+                    /* Glyphs we need to render everything */
+                    /* Latin characters for now only */
+                    _glyph_cache.reset(new Magnum::Text::DistanceFieldGlyphCache{Magnum::Vector2i{2048}, Magnum::Vector2i{512}, 22});
+                    _font->fillGlyphCache(*_glyph_cache,
+                        "abcdefghijklmnopqrstuvwxyz"
+                        "ABCDEFGHIJKLMNOPQRSTUVWXYZ"
+                        "0123456789:-+,.!° ");
+
+                    /* Initialize dynamic text */
+                    _dynamic_text.reset(new Magnum::Text::Renderer2D(*_font, *_glyph_cache, 32.0f, Magnum::Text::Alignment::TopLeft));
+                    /* Reserve 100 characters for drawing debug text */
+                    _dynamic_text->reserve(100, Magnum::GL::BufferUsage::DynamicDraw, Magnum::GL::BufferUsage::StaticDraw);
+
+                    /* Initialize text shader */
+                    _text_shader.reset(new Magnum::Shaders::DistanceFieldVector2D);
+                    _text_shader->bindVectorTexture(_glyph_cache->texture());
+                }
             }
 
             void BaseApplication::clear_lights()
@@ -614,6 +639,10 @@ namespace robot_dart {
                 _shadow_color_cube_map.reset();
                 _3D_axis_shader.reset();
                 _3D_axis_mesh.reset();
+                _text_shader.reset();
+                _glyph_cache.reset();
+                _font.reset();
+                _dynamic_text.reset();
 
                 _camera.reset();
                 _shadow_camera.reset();

src/robot_dart/gui/magnum/base_application.hpp

@@ -27,7 +27,11 @@
 #else
 #include <Magnum/Platform/WindowlessCglApplication.h>
 #endif
+#include <Magnum/Shaders/DistanceFieldVector.h>
 #include <Magnum/Shaders/VertexColor.h>
+#include <Magnum/Text/AbstractFont.h>
+#include <Magnum/Text/DistanceFieldGlyphCache.h>
+#include <Magnum/Text/Renderer.h>
 
 #include <Magnum/DartIntegration/World.h>
 
@@ -152,6 +156,8 @@ namespace robot_dart {
                 // Access to members
                 Magnum::Shaders::VertexColor3D& axes_shader() { return *_3D_axis_shader; }
                 Magnum::GL::Mesh& axes_mesh() { return *_3D_axis_mesh; }
+                Magnum::Shaders::DistanceFieldVector2D* text_shader() { return &*_text_shader; }
+                Magnum::Text::Renderer2D* text_renderer() { return &*_dynamic_text; }
 
             protected:
                 /* Magnum */
@@ -188,6 +194,12 @@ namespace robot_dart {
                 /* Debug visualization */
                 std::unique_ptr<Magnum::GL::Mesh> _3D_axis_mesh;
                 std::unique_ptr<Magnum::Shaders::VertexColor3D> _3D_axis_shader;
+                /* Text visualization */
+                std::unique_ptr<Magnum::Shaders::DistanceFieldVector2D> _text_shader;
+                Corrade::PluginManager::Manager<Magnum::Text::AbstractFont> _font_manager;
+                Corrade::Containers::Pointer<Magnum::Text::DistanceFieldGlyphCache> _glyph_cache;
+                Corrade::Containers::Pointer<Magnum::Text::AbstractFont> _font;
+                Corrade::Containers::Pointer<Magnum::Text::Renderer2D> _dynamic_text;
 
                 /* Importer */
                 Corrade::PluginManager::Manager<Magnum::Trade::AbstractImporter> _importer_manager;

src/robot_dart/gui/magnum/base_graphics.hpp

@@ -30,6 +30,9 @@ namespace robot_dart {
 
                 virtual ~BaseGraphics() {}
 
+                size_t width() const override { return _magnum_app->camera().width(); }
+                size_t height() const override { return _magnum_app->camera().height(); }
+
                 bool done() const override
                 {
                     return _magnum_app->done();

src/robot_dart/gui/magnum/glfw_application.cpp

@@ -53,7 +53,7 @@ namespace robot_dart {
             {
                 Magnum::GL::defaultFramebuffer.setViewport({{}, event.framebufferSize()});
 
-                _camera->set_viewport(event.windowSize());
+                _camera->set_viewport(event.framebufferSize());
             }
 
             void GlfwApplication::drawEvent()
@@ -81,7 +81,7 @@ namespace robot_dart {
                     _camera->strafe(_speed_strafe);
 
                     /* Draw with main camera */
-                    _camera->draw(_drawables, Magnum::GL::defaultFramebuffer, Magnum::PixelFormat::RGB8Unorm, _simu, *_3D_axis_shader, *_3D_axis_mesh, _draw_debug);
+                    _camera->draw(_drawables, Magnum::GL::defaultFramebuffer, Magnum::PixelFormat::RGB8Unorm, _simu, *_3D_axis_shader, *_3D_axis_mesh, &*_text_shader, &*_dynamic_text, _draw_debug);
 
                     swapBuffers();
                 }

src/robot_dart/gui/magnum/graphics.cpp

@@ -7,6 +7,8 @@ namespace robot_dart {
             {
                 // we synchronize by default if we have the graphics activated
                 simu->scheduler().set_sync(true);
+                // enable summary text when graphics activated
+                simu->enable_summary_text(true);
             }
         } // namespace magnum
     } // namespace gui