diff --git a/ci/install_magnum.sh b/ci/install_magnum.sh index 704e67e2..26e0a98a 100755 --- a/ci/install_magnum.sh +++ b/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 diff --git a/src/examples/cameras.cpp b/src/examples/cameras.cpp index 40c4c3d4..780f67f3 100644 --- a/src/examples/cameras.cpp +++ b/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 diff --git a/src/python/eigen.cpp b/src/python/eigen.cpp new file mode 100644 index 00000000..98a64467 --- /dev/null +++ b/src/python/eigen.cpp @@ -0,0 +1,135 @@ +#include "robot_dart.hpp" + +#include +#include + +#include + +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; + ::pybind11::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& matrix) { + Class out(matrix); + return out; + }), + ::pybind11::arg("matrix")) + .def(::pybind11::init([](const Eigen::Matrix& rotation, const Eigen::Matrix& 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 { + return self->matrix(); + }) + .def("set_matrix", [](Class* self, const Eigen::Matrix& matrix) { + Class update(matrix); + *self = update; + }) + .def("translation", [](const Class* self) -> Eigen::Matrix { + return self->translation(); + }) + .def("set_translation", [](Class* self, const Eigen::Matrix& translation) { + self->translation() = translation; + }) + .def("rotation", [](const Class* self) -> Eigen::Matrix { + return self->linear(); + }) + .def("set_rotation", [](Class* self, const Eigen::Matrix& 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& 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& other) { + self->translate(other); + }, + ::pybind11::arg("other")) + .def( + "pretranslate", [](Class* self, const Eigen::Matrix& other) { + self->pretranslate(other); + }, + ::pybind11::arg("other")) + .def( + "rotate", [](Class* self, const Eigen::Matrix& other) { + self->rotate(other); + }, + ::pybind11::arg("other")) + .def( + "prerotate", [](Class* self, const Eigen::Matrix& other) { + self->prerotate(other); + }, + ::pybind11::arg("other")) + .def( + "scale", [](Class* self, const Eigen::Matrix& other) { + self->scale(other); + }, + ::pybind11::arg("other")) + .def( + "prescale", [](Class* self, const Eigen::Matrix& 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, Class>(); + } + } + } // namespace python +} // namespace robot_dart \ No newline at end of file diff --git a/src/python/gui.cpp b/src/python/gui.cpp index 3385d7d5..a5de9e3b 100644 --- a/src/python/gui.cpp +++ b/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) diff --git a/src/python/robot_dart.cc b/src/python/robot_dart.cc index 0a0e9b1a..1d87a15a 100644 --- a/src/python/robot_dart.cc +++ b/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); diff --git a/src/python/robot_dart.hpp b/src/python/robot_dart.hpp index 46c9c8c0..c71bc19c 100644 --- a/src/python/robot_dart.hpp +++ b/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); diff --git a/src/python/simu.cpp b/src/python/simu.cpp index 663e727e..59420303 100644 --- a/src/python/simu.cpp +++ b/src/python/simu.cpp @@ -48,6 +48,18 @@ namespace robot_dart { .def("__call__", &Descriptor::operator()); + // TextData + using simu::TextData; + py::class_>(m, "TextData") + .def(py::init(), + 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_(m, "RobotDARTSimu") .def(py::init(), @@ -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, diff --git a/src/robot_dart/gui/base.hpp b/src/robot_dart/gui/base.hpp index 87815e1b..299bc5ee 100644 --- a/src/robot_dart/gui/base.hpp +++ b/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 diff --git a/src/robot_dart/gui/magnum/base_application.cpp b/src/robot_dart/gui/magnum/base_application.cpp index e4099da7..43cb4869 100644 --- a/src/robot_dart/gui/magnum/base_application.cpp +++ b/src/robot_dart/gui/magnum/base_application.cpp @@ -8,6 +8,7 @@ #include #include +#include #include #include @@ -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(); diff --git a/src/robot_dart/gui/magnum/base_application.hpp b/src/robot_dart/gui/magnum/base_application.hpp index 3ec412af..f4a3a280 100644 --- a/src/robot_dart/gui/magnum/base_application.hpp +++ b/src/robot_dart/gui/magnum/base_application.hpp @@ -27,7 +27,11 @@ #else #include #endif +#include #include +#include +#include +#include #include @@ -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 _3D_axis_mesh; std::unique_ptr _3D_axis_shader; + /* Text visualization */ + std::unique_ptr _text_shader; + Corrade::PluginManager::Manager _font_manager; + Corrade::Containers::Pointer _glyph_cache; + Corrade::Containers::Pointer _font; + Corrade::Containers::Pointer _dynamic_text; /* Importer */ Corrade::PluginManager::Manager _importer_manager; diff --git a/src/robot_dart/gui/magnum/base_graphics.hpp b/src/robot_dart/gui/magnum/base_graphics.hpp index 62647b5d..5eb42bbf 100644 --- a/src/robot_dart/gui/magnum/base_graphics.hpp +++ b/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(); diff --git a/src/robot_dart/gui/magnum/glfw_application.cpp b/src/robot_dart/gui/magnum/glfw_application.cpp index aa95c212..cf1ba0f9 100644 --- a/src/robot_dart/gui/magnum/glfw_application.cpp +++ b/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(); } diff --git a/src/robot_dart/gui/magnum/graphics.cpp b/src/robot_dart/gui/magnum/graphics.cpp index 67d480ef..e101e7c1 100644 --- a/src/robot_dart/gui/magnum/graphics.cpp +++ b/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 diff --git a/src/robot_dart/gui/magnum/gs/camera.cpp b/src/robot_dart/gui/magnum/gs/camera.cpp index e8983b2e..6611a9f7 100644 --- a/src/robot_dart/gui/magnum/gs/camera.cpp +++ b/src/robot_dart/gui/magnum/gs/camera.cpp @@ -16,6 +16,7 @@ #include #include #include +#include #include #include @@ -250,7 +251,7 @@ namespace robot_dart { #endif } - void Camera::draw(Magnum::SceneGraph::DrawableGroup3D& drawables, Magnum::GL::AbstractFramebuffer& framebuffer, Magnum::PixelFormat format, RobotDARTSimu* simu, Magnum::Shaders::VertexColor3D& axes_shader, Magnum::GL::Mesh& axes_mesh, bool draw_debug) + void Camera::draw(Magnum::SceneGraph::DrawableGroup3D& drawables, Magnum::GL::AbstractFramebuffer& framebuffer, Magnum::PixelFormat format, RobotDARTSimu* simu, Magnum::Shaders::VertexColor3D& axes_shader, Magnum::GL::Mesh& axes_mesh, Magnum::Shaders::DistanceFieldVector2D* text_shader, Magnum::Text::Renderer2D* text_renderer, bool draw_debug) { // TO-DO: Maybe check if world moved? std::vector, Magnum::Matrix4>> @@ -280,6 +281,7 @@ namespace robot_dart { /* Draw debug */ if (draw_debug) { + /* Draw axes */ std::vector> axes = simu->gui_data()->drawing_axes(); for (auto& axis : axes) { Magnum::Matrix4 world_transform = Magnum::Matrix4(Magnum::Matrix4d(axis.first->getWorldTransform().matrix())); @@ -288,6 +290,31 @@ namespace robot_dart { axes_shader.setTransformationProjectionMatrix(_camera->projectionMatrix() * _camera->cameraMatrix() * world_transform * scaling) .draw(axes_mesh); } + + /* Draw text */ + if (text_shader && text_renderer) { + using namespace Magnum::Math::Literals; + Magnum::GL::Renderer::disable(Magnum::GL::Renderer::Feature::DepthTest); + Magnum::GL::Renderer::disable(Magnum::GL::Renderer::Feature::FaceCulling); + + for (auto& text : simu->gui_data()->drawing_texts()) { + text_renderer->render(text->text); + // std::cout << text_renderer->rectangle().sizeX() << std::endl; + auto viewport = Magnum::Vector2{_camera->viewport()}; + auto big = viewport.max(); + auto scaling = Magnum::Vector2{big / 1024.f}; + (*text_shader) + .setTransformationProjectionMatrix(Magnum::Matrix3::projection(viewport) * Magnum::Matrix3(Magnum::Matrix3d(text->transformation)) * Magnum::Matrix3::scaling(scaling)) + // .setTransformationProjectionMatrix(Magnum::Matrix3::projection(Magnum::Vector2{_camera->viewport()}) * Magnum::Matrix3::translation(Magnum::Vector2{-text_renderer->rectangle().sizeX() / 2.f, -text_renderer->rectangle().sizeY() / 2.f}) * Magnum::Matrix3(Magnum::Matrix3d(text.transformation))) + .setColor(Magnum::Vector4(Magnum::Vector4d(text->color))) + .setOutlineRange(0.5f, 1.0f) + .setSmoothness(0.075f) + .draw(text_renderer->mesh()); + } + + Magnum::GL::Renderer::enable(Magnum::GL::Renderer::Feature::DepthTest); + Magnum::GL::Renderer::enable(Magnum::GL::Renderer::Feature::FaceCulling); + } } if (_recording) { diff --git a/src/robot_dart/gui/magnum/gs/camera.hpp b/src/robot_dart/gui/magnum/gs/camera.hpp index 603a27c9..2e43d96e 100644 --- a/src/robot_dart/gui/magnum/gs/camera.hpp +++ b/src/robot_dart/gui/magnum/gs/camera.hpp @@ -14,7 +14,9 @@ #include #include #include +#include #include +#include namespace robot_dart { namespace gui { @@ -67,7 +69,7 @@ namespace robot_dart { Corrade::Containers::Optional& image() { return _image; } Corrade::Containers::Optional& depth_image() { return _depth_image; } - void draw(Magnum::SceneGraph::DrawableGroup3D& drawables, Magnum::GL::AbstractFramebuffer& framebuffer, Magnum::PixelFormat format, RobotDARTSimu* simu, Magnum::Shaders::VertexColor3D& axes_shader, Magnum::GL::Mesh& axes_mesh, bool draw_debug = true); + void draw(Magnum::SceneGraph::DrawableGroup3D& drawables, Magnum::GL::AbstractFramebuffer& framebuffer, Magnum::PixelFormat format, RobotDARTSimu* simu, Magnum::Shaders::VertexColor3D& axes_shader, Magnum::GL::Mesh& axes_mesh, Magnum::Shaders::DistanceFieldVector2D* text_shader, Magnum::Text::Renderer2D* text_renderer, bool draw_debug = true); private: Object3D* _yaw_object; diff --git a/src/robot_dart/gui/magnum/resources/SourceSansPro-Regular.ttf b/src/robot_dart/gui/magnum/resources/SourceSansPro-Regular.ttf new file mode 100644 index 00000000..b422bf43 Binary files /dev/null and b/src/robot_dart/gui/magnum/resources/SourceSansPro-Regular.ttf differ diff --git a/src/robot_dart/gui/magnum/resources/resources.conf b/src/robot_dart/gui/magnum/resources/resources.conf index f35e415d..a18133b8 100644 --- a/src/robot_dart/gui/magnum/resources/resources.conf +++ b/src/robot_dart/gui/magnum/resources/resources.conf @@ -29,3 +29,6 @@ filename=CubeMapColor.frag [file] filename=compatibility.glsl + +[file] +filename=SourceSansPro-Regular.ttf diff --git a/src/robot_dart/gui/magnum/sensor/camera.cpp b/src/robot_dart/gui/magnum/sensor/camera.cpp index b8d5d2eb..0b320705 100644 --- a/src/robot_dart/gui/magnum/sensor/camera.cpp +++ b/src/robot_dart/gui/magnum/sensor/camera.cpp @@ -78,7 +78,7 @@ namespace robot_dart { _framebuffer.clear(Magnum::GL::FramebufferClear::Color | Magnum::GL::FramebufferClear::Depth); /* Draw with this camera */ - _camera->draw(_magnum_app->drawables(), _framebuffer, _format, _simu, _magnum_app->axes_shader(), _magnum_app->axes_mesh(), _draw_debug); + _camera->draw(_magnum_app->drawables(), _framebuffer, _format, _simu, _magnum_app->axes_shader(), _magnum_app->axes_mesh(), _magnum_app->text_shader(), _magnum_app->text_renderer(), _draw_debug); } std::string Camera::type() const { return "rgb_camera"; } diff --git a/src/robot_dart/gui/magnum/windowless_gl_application.cpp b/src/robot_dart/gui/magnum/windowless_gl_application.cpp index 3af041cb..6286f969 100644 --- a/src/robot_dart/gui/magnum/windowless_gl_application.cpp +++ b/src/robot_dart/gui/magnum/windowless_gl_application.cpp @@ -74,7 +74,7 @@ namespace robot_dart { _framebuffer.clear(Magnum::GL::FramebufferClear::Color | Magnum::GL::FramebufferClear::Depth); /* Draw with main camera */ - _camera->draw(_drawables, _framebuffer, _format, _simu, *_3D_axis_shader, *_3D_axis_mesh, _draw_debug); + _camera->draw(_drawables, _framebuffer, _format, _simu, *_3D_axis_shader, *_3D_axis_mesh, &*_text_shader, &*_dynamic_text, _draw_debug); // if (_index % 10 == 0) { // intptr_t tt = (intptr_t)_glx_context; diff --git a/src/robot_dart/gui/magnum/windowless_graphics.cpp b/src/robot_dart/gui/magnum/windowless_graphics.cpp index 3093f50d..2fe9d655 100644 --- a/src/robot_dart/gui/magnum/windowless_graphics.cpp +++ b/src/robot_dart/gui/magnum/windowless_graphics.cpp @@ -7,6 +7,8 @@ namespace robot_dart { { // we should not synchronize by default if we want windowless graphics (usually used only for sensors) simu->scheduler().set_sync(false); + // disable summary text when windowless graphics activated + simu->enable_summary_text(false); } } // namespace magnum } // namespace gui diff --git a/src/robot_dart/gui_data.hpp b/src/robot_dart/gui_data.hpp index d7930fcc..e1d023f8 100644 --- a/src/robot_dart/gui_data.hpp +++ b/src/robot_dart/gui_data.hpp @@ -4,6 +4,9 @@ #include "robot_dart_simu.hpp" #include +#include + +#include #include #include @@ -19,8 +22,33 @@ namespace robot_dart { std::unordered_map robot_data; std::unordered_map>> robot_axes; + std::vector> text_drawings; public: + std::shared_ptr add_text(const std::string& text, const Eigen::Affine2d& tf = Eigen::Affine2d::Identity(), Eigen::Vector4d color = Eigen::Vector4d(1, 1, 1, 1)) + { + text_drawings.emplace_back(new TextData{text, tf, color}); + + return text_drawings.back(); + } + + void remove_text(const std::shared_ptr& data) + { + for (size_t i = 0; i < text_drawings.size(); i++) { + if (text_drawings[i] == data) { + text_drawings.erase(text_drawings.begin() + i); + return; + } + } + } + + void remove_text(size_t index) + { + if (index >= text_drawings.size()) + return; + text_drawings.erase(text_drawings.begin() + index); + } + void update_robot(const std::shared_ptr& robot) { auto robot_ptr = &*robot; @@ -84,6 +112,8 @@ namespace robot_dart { return axes; } + + const std::vector>& drawing_texts() const { return text_drawings; } }; } // namespace simu } // namespace robot_dart diff --git a/src/robot_dart/robot_dart_simu.cpp b/src/robot_dart/robot_dart_simu.cpp index d9bb790e..e5d1ff6f 100644 --- a/src/robot_dart/robot_dart_simu.cpp +++ b/src/robot_dart/robot_dart_simu.cpp @@ -2,6 +2,8 @@ #include "gui_data.hpp" #include "utils.hpp" +#include + #include #include #include @@ -153,7 +155,20 @@ namespace robot_dart { } _old_index++; - _scheduler.step(); + double rt = _scheduler.step(); + if (_summary_text) { + std::ostringstream out; + out.precision(3); + out << std::fixed << "Sim. Time: " << _world->getTime() << "s" << std::endl + << "Time: " << rt << "s"; + + _summary_text->text = out.str(); + + Eigen::Affine2d tf = Eigen::Affine2d::Identity(); + tf.translate(Eigen::Vector2d(-static_cast(_graphics->width()) / 2., _graphics->height() / 2.)); + + _summary_text->transformation = tf; + } return _break; } @@ -392,6 +407,23 @@ namespace robot_dart { simu::GUIData* RobotDARTSimu::gui_data() { return &(*_gui_data); } + void RobotDARTSimu::enable_summary_text(bool enable) + { + if (!_summary_text && enable) { + _summary_text = _gui_data->add_text("Sim. Time: 0s"); + } + else if (!enable) { + if (_summary_text) + _gui_data->remove_text(_summary_text); + _summary_text = nullptr; + } + } + + std::shared_ptr RobotDARTSimu::add_text(const std::string& text, const Eigen::Affine2d& tf, Eigen::Vector4d color) + { + return _gui_data->add_text(text, tf, color); + } + void RobotDARTSimu::add_floor(double floor_width, double floor_height, const Eigen::Vector6d& pose, const std::string& floor_name) { // We do not want 2 floors with the same name! diff --git a/src/robot_dart/robot_dart_simu.hpp b/src/robot_dart/robot_dart_simu.hpp index e70c072a..44c3ef27 100644 --- a/src/robot_dart/robot_dart_simu.hpp +++ b/src/robot_dart/robot_dart_simu.hpp @@ -12,7 +12,15 @@ namespace robot_dart { namespace simu { struct GUIData; - } + + // We use the Abode Source Sans Pro font: https://github.com/adobe-fonts/source-sans-pro + // This font is licensed under SIL Open Font License 1.1: https://github.com/adobe-fonts/source-sans-pro/blob/release/LICENSE.md + struct TextData { + std::string text; + Eigen::Affine2d transformation; + Eigen::Vector4d color; + }; + } // namespace simu class RobotDARTSimu { public: @@ -106,6 +114,8 @@ namespace robot_dart { void clear_robots(); simu::GUIData* gui_data(); + void enable_summary_text(bool enable = true); + std::shared_ptr add_text(const std::string& text, const Eigen::Affine2d& tf = Eigen::Affine2d::Identity(), Eigen::Vector4d color = Eigen::Vector4d(1, 1, 1, 1)); void add_floor(double floor_width = 10.0, double floor_height = 0.1, const Eigen::Vector6d& pose = Eigen::Vector6d::Zero(), const std::string& floor_name = "floor"); void add_checkerboard_floor(double floor_width = 10.0, double floor_height = 0.1, double size = 1., const Eigen::Vector6d& pose = Eigen::Vector6d::Zero(), const std::string& floor_name = "checkerboard_floor"); @@ -137,6 +147,7 @@ namespace robot_dart { std::vector _robots; std::shared_ptr _graphics; std::unique_ptr _gui_data; + std::shared_ptr _summary_text = nullptr; Scheduler _scheduler; int _physics_freq = -1, _control_freq = -1, _graphics_freq = 40; }; diff --git a/src/robot_dart/scheduler.cpp b/src/robot_dart/scheduler.cpp index 3dd2477d..0542b8d2 100644 --- a/src/robot_dart/scheduler.cpp +++ b/src/robot_dart/scheduler.cpp @@ -3,7 +3,7 @@ namespace robot_dart { bool Scheduler::schedule(int frequency) { - if (_max_frequency == -1 && _sync) + if (_max_frequency == -1) _start = clock_t::now(); _max_frequency = std::max(_max_frequency, frequency); @@ -31,19 +31,21 @@ namespace robot_dart { _sync = sync; } - void Scheduler::step() + double Scheduler::step() { _current_time += _dt; _current_step += 1; + auto end = clock_t::now(); + std::chrono::duration real = end - _start; if (_sync) { auto expected = std::chrono::microseconds(int(_current_time * 1e6)); - auto end = clock_t::now(); - std::chrono::duration real = end - _start; std::chrono::duration adjust = expected - real; if (adjust.count() > 0) std::this_thread::sleep_for(adjust); } + + return real.count() * 1e-6; } } // namespace robot_dart \ No newline at end of file diff --git a/src/robot_dart/scheduler.hpp b/src/robot_dart/scheduler.hpp index c16859db..5378b8f1 100644 --- a/src/robot_dart/scheduler.hpp +++ b/src/robot_dart/scheduler.hpp @@ -23,8 +23,9 @@ namespace robot_dart { /// call this at the end of the loop (see examples) /// this will synchronize with real time if requested - /// and increase the counter - void step(); + /// and increase the counter; + /// returns the real-time (in seconds) + double step(); void reset(double dt, bool sync = false, double current_time = 0.); diff --git a/waf_tools/magnum.py b/waf_tools/magnum.py index 7165fa73..729e3227 100644 --- a/waf_tools/magnum.py +++ b/waf_tools/magnum.py @@ -123,6 +123,7 @@ def fatal(required, msg): # OSX/Mac uses .dylib and GNU/Linux .so suffix = 'dylib' if conf.env['DEST_OS'] == 'darwin' else 'so' + modules_suffix = 'so' # Magnum depends on several libraries and we cannot make the assumption that # someone installed all of them in the same directory! @@ -467,7 +468,7 @@ def fatal(required, msg): # we need the full lib_dir in order to be able to link to the plugins # or not? because they are loaded dynamically # we need to set the libpath for the static plugins only - lib_dir = get_directory('magnum/'+lib_path_suffix+lib+'.'+suffix, libs_check, True) + lib_dir = get_directory('magnum/'+lib_path_suffix+lib+'.'+modules_suffix, libs_check, True) magnum_component_includes[component] = magnum_component_includes[component] + [include_dir] # magnum_component_libpaths[component] = magnum_component_libpaths[component] + [lib_dir] diff --git a/wscript b/wscript index ffcd23ca..7ec9e8b5 100644 --- a/wscript +++ b/wscript @@ -67,7 +67,7 @@ def configure(conf): conf.check_eigen(required=True, min_version=(3,2,92)) conf.check_dart(required=True) conf.check_corrade(components='Utility PluginManager', required=False) - conf.env['magnum_dep_libs'] = 'MeshTools Primitives Shaders SceneGraph GlfwApplication' + conf.env['magnum_dep_libs'] = 'MeshTools Primitives Shaders SceneGraph GlfwApplication Text MagnumFont' if conf.env['DEST_OS'] == 'darwin': conf.env['magnum_dep_libs'] += ' WindowlessCglApplication' else: @@ -75,7 +75,7 @@ def configure(conf): if len(conf.env.INCLUDES_Corrade): conf.check_magnum(components=conf.env['magnum_dep_libs'], required=False) if len(conf.env.INCLUDES_Magnum): - conf.check_magnum_plugins(components='AssimpImporter', required=False) + conf.check_magnum_plugins(components='AssimpImporter StbTrueTypeFont', required=False) conf.check_magnum_integration(components='Dart Eigen', required=False) conf.env['py_flags'] = '' @@ -90,8 +90,8 @@ def configure(conf): if conf.env.CXX_NAME in ["gcc", "g++"]: conf.env['py_flags'] = ' -fPIC' # we need -fPIC in some Linux/gcc combinations - # We require both Magnum DartIntegration and EigenIntegration - if len(conf.env.INCLUDES_MagnumIntegration_Dart) > 0 and len(conf.env.INCLUDES_MagnumIntegration_Eigen) > 0: + # We require Magnum DartIntegration, EigenIntegration, AssimpImporter, and StbTrueTypeFont + if len(conf.env.INCLUDES_MagnumIntegration_Dart) > 0 and len(conf.env.INCLUDES_MagnumIntegration_Eigen) > 0 and len(conf.env.INCLUDES_MagnumPlugins_AssimpImporter) > 0 and len(conf.env.INCLUDES_MagnumPlugins_StbTrueTypeFont) > 0: conf.get_env()['BUILD_MAGNUM'] = True conf.env['magnum_libs'] = magnum.get_magnum_dependency_libs(conf, conf.env['magnum_dep_libs']) + magnum_integration.get_magnum_integration_dependency_libs(conf, 'Dart Eigen')