diff --git a/examples/Ur5WithSystems.cpp b/examples/Ur5WithSystems.cpp index 9e29b96..f161b8d 100644 --- a/examples/Ur5WithSystems.cpp +++ b/examples/Ur5WithSystems.cpp @@ -273,17 +273,30 @@ int main(int argc, char **argv) { GuiSystem gui_system{ renderer, [&](const Renderer &r) { + IMGUI_CHECKVERSION(); + static bool demo_window_open = true; static bool show_about_window = false; static bool show_plane_vis = true; - ImGui::Begin("Renderer info & controls", nullptr, - ImGuiWindowFlags_AlwaysAutoResize); + if (show_about_window) + showCandlewickAboutWindow(&show_about_window); + + ImGuiWindowFlags window_flags = 0; + window_flags |= ImGuiWindowFlags_AlwaysAutoResize; + window_flags |= ImGuiWindowFlags_MenuBar; + ImGui::Begin("Renderer info & controls", nullptr, window_flags); + + if (ImGui::BeginMenuBar()) { + ImGui::MenuItem("About Candlewick", NULL, &show_about_window); + ImGui::EndMenuBar(); + } ImGui::Text("Video driver: %s", SDL_GetCurrentVideoDriver()); + ImGui::SameLine(); + ImGui::Text("Device driver: %s", r.device.driverName()); ImGui::Text("Display pixel density: %.2f / scale: %.2f", r.window.pixelDensity(), r.window.displayScale()); - ImGui::Text("Device driver: %s", r.device.driverName()); ImGui::SeparatorText("Camera"); bool ortho_change, persp_change; ortho_change = ImGui::RadioButton("Orthographic", (int *)&g_cameraType, @@ -348,9 +361,6 @@ int main(int argc, char **argv) { ImGui::ColorEdit4("plane color", plane_obj.materials[0].baseColor.data()); - if (ImGui::Button("About candlewick")) - show_about_window = true; - ImGui::End(); ImGui::SetNextWindowCollapsed(true, ImGuiCond_Once); diff --git a/examples/Visualizer.cpp b/examples/Visualizer.cpp index 04ddfd9..436dc27 100644 --- a/examples/Visualizer.cpp +++ b/examples/Visualizer.cpp @@ -10,24 +10,32 @@ #include #include +namespace cdw = candlewick; using namespace candlewick::multibody; using std::chrono::steady_clock; int main(int argc, char **argv) { CLI::App app{"Visualizer example"}; argv = app.ensure_utf8(argv); - double fps; + std::vector window_dims{1920u, 1080u}; + double fps = 50.0; + app.add_option("--dims", window_dims, "Window dimensions.") + ->capture_default_str(); app.add_option("--fps", fps, "Framerate") - ->default_val(50); + ->default_str("50.0"); CLI11_PARSE(app, argc, argv); + if (window_dims.size() != 2) { + cdw::terminate_with_message("Expected only two values for argument --dims"); + } + pin::Model model; pin::GeometryModel geom_model; robot_descriptions::loadModelsFromToml("ur.toml", "ur5_gripper", model, &geom_model, NULL); - Visualizer visualizer{{1920, 1280}, model, geom_model}; + Visualizer visualizer{{window_dims[0], window_dims[1]}, model, geom_model}; assert(!visualizer.hasExternalData()); Eigen::VectorXd q0 = pin::neutral(model); diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 7ade014..196f755 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -17,6 +17,7 @@ ADD_PROJECT_DEPENDENCY( avcodec swscale ) +find_package(ZeroMQ REQUIRED) add_library( candlewick_core @@ -129,6 +130,13 @@ if(BUILD_PINOCCHIO_VISUALIZER) INCLUDES DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} ) + + find_package(cppzmq) + add_executable(candlewick_visualizer candlewick/runtime/main.cpp) + target_link_libraries( + candlewick_visualizer + PRIVATE cppzmq candlewick_multibody + ) endif() # install headers diff --git a/src/candlewick/core/errors.cpp b/src/candlewick/core/errors.cpp index 9bb086d..c28b158 100644 --- a/src/candlewick/core/errors.cpp +++ b/src/candlewick/core/errors.cpp @@ -9,9 +9,10 @@ RAIIException::RAIIException(std::string_view msg, location.file_name(), location.line(), msg.data())) {} namespace detail { - std::string _error_message_impl(const char *fname, std::string_view _fmtstr, + std::string _error_message_impl(std::string_view fname, + std::string_view fmtstr, std::format_args args) { - return std::format("{:s} :: {:s}", fname, std::vformat(_fmtstr, args)); + return std::format("{:s} :: {:s}", fname, std::vformat(fmtstr, args)); } } // namespace detail } // namespace candlewick diff --git a/src/candlewick/core/errors.h b/src/candlewick/core/errors.h index 4257b30..531a81e 100644 --- a/src/candlewick/core/errors.h +++ b/src/candlewick/core/errors.h @@ -21,7 +21,8 @@ struct RAIIException : std::runtime_error { namespace detail { - std::string _error_message_impl(const char *fname, std::string_view fmtstr, + std::string _error_message_impl(std::string_view fname, + std::string_view fmtstr, std::format_args args); template @@ -38,11 +39,9 @@ namespace detail { inline void terminate_with_message( std::string_view msg, std::source_location location = std::source_location::current()) { - SDL_LogError( - SDL_LOG_CATEGORY_APPLICATION, "%s", + throw std::runtime_error( detail::error_message_format(location.function_name(), "{:s}", msg) .c_str()); - ::std::terminate(); } [[noreturn]] diff --git a/src/candlewick/multibody/RobotScene.cpp b/src/candlewick/multibody/RobotScene.cpp index 30d6dab..afc0338 100644 --- a/src/candlewick/multibody/RobotScene.cpp +++ b/src/candlewick/multibody/RobotScene.cpp @@ -2,12 +2,10 @@ #include "Components.h" #include "LoadPinocchioGeometry.h" #include "../core/Components.h" -#include "../core/errors.h" #include "../core/Shader.h" #include "../core/Components.h" #include "../core/TransformUniforms.h" #include "../core/Camera.h" -#include "../core/errors.h" #include #include @@ -26,15 +24,6 @@ struct alignas(16) light_ubo_t { alignas(16) GpuMat4 projMat; }; -template - requires std::is_enum_v -[[noreturn]] void -invalid_enum(const char *msg, T type, - std::source_location location = std::source_location::current()) { - terminate_with_message( - std::format("{:s} - {:s}", msg, magic_enum::enum_name(type)), location); -} - void updateRobotTransforms(entt::registry ®istry, const pin::GeometryData &geom_data) { auto robot_view = @@ -77,9 +66,7 @@ auto RobotScene::pinGeomToPipeline(const coal::CollisionGeometry &geom) void add_pipeline_tag_component(entt::registry ®, entt::entity ent, RobotScene::PipelineType type) { magic_enum::enum_switch( - [®, ent](auto pt) { - reg.emplace>(ent); - }, + [®, ent](auto pt) { reg.emplace>(ent); }, type); } @@ -165,11 +152,18 @@ void RobotScene::loadModels(const pin::GeometryModel &geom_model, const auto &geom_obj = geom_model.geometryObjects[geom_id]; auto meshDatas = loadGeometryObject(geom_obj); PipelineType pipeline_type = pinGeomToPipeline(*geom_obj.geometry); - auto mesh = createMeshFromBatch(device(), meshDatas, true); + Mesh mesh = createMeshFromBatch(device(), meshDatas, true); SDL_assert(validateMesh(mesh)); - // local copy for use const auto layout = mesh.layout(); + if (!renderPipelines[pipeline_type]) { + SDL_Log("Building pipeline for type %s", + magic_enum::enum_name(pipeline_type).data()); + renderPipelines[pipeline_type] = + createPipeline(layout, m_renderer.getSwapchainTextureFormat(), + m_renderer.depthFormat(), pipeline_type); + SDL_assert(renderPipelines[pipeline_type]); + } // add entity for this geometry entt::entity entity = m_registry.create(); @@ -191,16 +185,6 @@ void RobotScene::loadModels(const pin::GeometryModel &geom_model, ShadowPassInfo::create(m_renderer, layout, m_config.shadow_config); } } - - if (!renderPipelines[pipeline_type]) { - SDL_Log("Building pipeline for type %s", - magic_enum::enum_name(pipeline_type).data()); - SDL_GPUGraphicsPipeline *pipeline = - createPipeline(layout, m_renderer.getSwapchainTextureFormat(), - m_renderer.depthFormat(), pipeline_type); - SDL_assert(pipeline); - renderPipelines[pipeline_type] = pipeline; - } } m_initialized = true; } @@ -210,11 +194,10 @@ void RobotScene::updateTransforms() { } void RobotScene::collectOpaqueCastables() { - auto all_view = - m_registry.view>( - entt::exclude); + auto all_view = m_registry.view>( + entt::exclude); m_castables.clear(); @@ -331,7 +314,7 @@ void RobotScene::renderPBRTriangleGeometry(CommandBuffer &command_buffer, auto all_view = m_registry.view>( + pipeline_tag>( entt::exclude); for (auto [ent, tr, obj] : all_view.each()) { const Mat4f modelView = camera.view * tr; @@ -378,12 +361,11 @@ void RobotScene::renderOtherGeometry(CommandBuffer &command_buffer, auto env_view = m_registry.view>( + pipeline_tag>( entt::exclude); - for (auto [entity, tr, obj] : env_view.each()) { - auto &modelMat = tr; + for (auto &&[entity, tr, obj] : env_view.each()) { const Mesh &mesh = obj.mesh; - const Mat4f mvp = viewProj * modelMat; + const Mat4f mvp = viewProj * tr; const auto &color = obj.materials[0].baseColor; command_buffer .pushVertexUniform(VertexUniformSlots::TRANSFORM, &mvp, sizeof(mvp)) @@ -400,9 +382,10 @@ void RobotScene::release() { if (!device()) return; - m_registry.clear(); + clearEnvironment(); + clearRobotGeometries(); - for (auto &pipeline : renderPipelines) { + for (auto *pipeline : renderPipelines) { SDL_ReleaseGPUGraphicsPipeline(device(), pipeline); pipeline = nullptr; } diff --git a/src/candlewick/multibody/RobotScene.h b/src/candlewick/multibody/RobotScene.h index 25ca14d..38605ca 100644 --- a/src/candlewick/multibody/RobotScene.h +++ b/src/candlewick/multibody/RobotScene.h @@ -16,6 +16,19 @@ #include namespace candlewick { + +/// \brief Terminate the application after encountering an invalid enum value. +template + requires std::is_enum_v +[[noreturn]] void +invalid_enum(const char *msg, T type, + std::source_location location = std::source_location::current()) { + char out[64]; + SDL_snprintf(out, 64ul, "Invalid enum: %s - %s", msg, + magic_enum::enum_name(type).data()); + terminate_with_message(out, location); +} + namespace multibody { void updateRobotTransforms(entt::registry ®istry, @@ -63,7 +76,7 @@ namespace multibody { } } - template using pipeline_tag_component = entt::tag; + template using pipeline_tag = entt::tag; struct PipelineConfig { // shader set diff --git a/src/candlewick/multibody/VisualizerClient.h b/src/candlewick/multibody/VisualizerClient.h new file mode 100644 index 0000000..4b07e87 --- /dev/null +++ b/src/candlewick/multibody/VisualizerClient.h @@ -0,0 +1,13 @@ +#pragma once + +#include "Multibody.h" +#include +#include + +namespace candlewick { +namespace multibody { + class VisualizerClient final : public pin::visualizers::BaseVisualizer { + public: + }; +} // namespace multibody +} // namespace candlewick diff --git a/src/candlewick/runtime/main.cpp b/src/candlewick/runtime/main.cpp new file mode 100644 index 0000000..100d6c8 --- /dev/null +++ b/src/candlewick/runtime/main.cpp @@ -0,0 +1,63 @@ +#include +#include +#include "candlewick/multibody/Multibody.h" +#include "candlewick/multibody/Visualizer.h" + +#include +#include + +#include +#include +#include + +namespace cdw = candlewick; +namespace pin = pinocchio; +using cdw::multibody::Visualizer; + +int main(int argc, char **argv) { + CLI::App app{"Candlewick visualizer runtime"}; + argv = app.ensure_utf8(argv); + + std::vector window_dims{1920u, 1080u}; + app.add_option("--dims", window_dims, "Window dimensions.") + ->capture_default_str(); + + CLI11_PARSE(app, argc, argv); + + if (window_dims.size() != 2) { + cdw::terminate_with_message("Expected only two values for argument --dims"); + } + + zmq::context_t ctx; + zmq::socket_t sock{ctx, zmq::socket_type::pull}; + sock.bind("tcp://127.0.0.1:12000"); + const std::string endpoint = sock.get(zmq::sockopt::last_endpoint); + printf("ZMQ endpoint: %s\n", endpoint.c_str()); + + pin::Model model; + pin::GeometryModel geom_model; + + std::vector recv_models; + + printf("Receiving Model and GeometryModel...\n"); + + const auto ret = zmq::recv_multipart(sock, std::back_inserter(recv_models)); + assert(*ret == 2); + printf("Received %zu messages\n", ret.value()); + + model.loadFromString(recv_models[0].to_string()); + geom_model.loadFromString(recv_models[1].to_string()); + + Visualizer::Config config; + config.width = window_dims[0]; + config.height = window_dims[1]; + Visualizer viz{config, model, geom_model}; + + Eigen::VectorXd q0 = pin::neutral(model); + + while (!viz.shouldExit()) { + viz.display(q0); + } + + return 0; +} diff --git a/tests/test_client.py b/tests/test_client.py new file mode 100644 index 0000000..3280621 --- /dev/null +++ b/tests/test_client.py @@ -0,0 +1,19 @@ +import pinocchio as pin +import example_robot_data as erd +import zmq + + +PORT = 12000 + +ctx = zmq.Context.instance() +sock: zmq.Socket = ctx.socket(zmq.SocketType.PUSH) +sock.connect(f"tcp://127.0.0.1:{PORT}") + +robot: pin.RobotWrapper = erd.load("solo12") +model = robot.model +geom_model = robot.visual_model + +model_str = model.saveToString() +geom_str = geom_model.saveToString() +sock.send_multipart([model_str.encode(), geom_str.encode()]) +print("Sent")