diff --git a/include/mc_rbdyn/gui/RobotSurface.h b/include/mc_rbdyn/gui/RobotSurface.h index 39be3a80ab..c7dcc54b62 100644 --- a/include/mc_rbdyn/gui/RobotSurface.h +++ b/include/mc_rbdyn/gui/RobotSurface.h @@ -10,6 +10,10 @@ namespace mc_rbdyn::gui { +static const mc_rtc::gui::LineConfig defaultSurfaceConfig = []() { + return mc_rtc::gui::LineConfig{mc_rtc::gui::Color::Green, 0.01}; +}(); + /** Helper function to create a GUI element from a surface object inside a robot * * Multiple elements can be added based on the surface type (e.g. for a planar surface, the polygon and the normal are @@ -19,6 +23,8 @@ namespace mc_rbdyn::gui * * \param category Category where the object is added * + * \param cfg Configuration for the surface polygon + * * \param robot Robot that the surfaces belongs to, this reference is captured by the GUI and should survive * * \param name Name of the surface added to the GUI, the object should remain in the robot while it is in the @@ -28,6 +34,22 @@ namespace mc_rbdyn::gui * * \returns The names of the elements added by the function */ +MC_RBDYN_DLLAPI std::vector addSurfaceToGUI(mc_rtc::gui::StateBuilder & gui, + const std::vector & category, + const mc_rtc::gui::LineConfig & cfg, + const mc_rbdyn::Robot & robot, + const std::string & name, + const std::optional & publishName = std::nullopt); + +/** Helper function to create a GUI element from a surface object inside a robot + * + * \see std::vector addSurfaceToGUI(mc_rtc::gui::StateBuilder &, + const std::vector &, + const mc_rtc::gui::LineConfig &, + const mc_rbdyn::Robot &, + const std::string &, + const std::optional & publishName); + */ MC_RBDYN_DLLAPI std::vector addSurfaceToGUI(mc_rtc::gui::StateBuilder & gui, const std::vector & category, const mc_rbdyn::Robot & robot, diff --git a/src/mc_rbdyn/gui/RobotSurface.cpp b/src/mc_rbdyn/gui/RobotSurface.cpp index f6eafb84b6..ac536c4124 100644 --- a/src/mc_rbdyn/gui/RobotSurface.cpp +++ b/src/mc_rbdyn/gui/RobotSurface.cpp @@ -6,9 +6,18 @@ namespace mc_rbdyn::gui { +std::vector addSurfaceToGUI(mc_rtc::gui::StateBuilder & gui, + const std::vector & category, + const mc_rbdyn::Robot & robot, + const std::string & name, + const std::optional & publishName) +{ + return addSurfaceToGUI(gui, category, defaultSurfaceConfig, robot, name, publishName); +} std::vector addSurfaceToGUI(mc_rtc::gui::StateBuilder & gui, const std::vector & category, + const mc_rtc::gui::LineConfig & cfg, const mc_rbdyn::Robot & robot, const std::string & name, const std::optional & publishName) @@ -29,7 +38,7 @@ std::vector addSurfaceToGUI(mc_rtc::gui::StateBuilder & gui, [&cylinder]() { return mc_rtc::gui::CylinderParameters{cylinder.radius(), cylinder.width()}; }, - get_pose, mc_rtc::gui::Color::Green)); + get_pose, cfg.color)); } else if(surface.type() == "planar") { @@ -38,7 +47,7 @@ std::vector addSurfaceToGUI(mc_rtc::gui::StateBuilder & gui, std::vector points; points.resize(plan.points().size()); publish_surface(mc_rtc::gui::Polygon( - publishName.value_or(name), mc_rtc::gui::LineConfig{mc_rtc::gui::Color::Green, 0.01}, + publishName.value_or(name), cfg, [&plan, points, get_pose]() mutable -> const std::vector & { auto pose = get_pose(); diff --git a/utils/RobotVisualizer.cpp b/utils/RobotVisualizer.cpp index 9a70c305b9..30a6bea550 100644 --- a/utils/RobotVisualizer.cpp +++ b/utils/RobotVisualizer.cpp @@ -153,10 +153,12 @@ void RobotVisualizer::addRobot() })); if(show_surfaces) { addSurface(name); } } + addSurfaceConfigurationGUI(); } void RobotVisualizer::addConvexConfigurationGUI() { + builder.removeElement({"Robot", "Convexes", "Convex Display Configuration"}, "Apply Convex Configuration"); builder.addElement({"Robot", "Convexes", "Convex Display Configuration"}, mc_rtc::gui::Form( "Apply Convex Configuration", @@ -193,6 +195,35 @@ void RobotVisualizer::addConvexConfigurationGUI() mc_rtc::gui::FormCheckbox("Apply to all", false, true))); } +void RobotVisualizer::addSurfaceConfigurationGUI() +{ + builder.removeElement({"Robot", "Surfaces", "Surface Display Configuration"}, "Apply Surface Configuration"); + builder.addElement({"Robot", "Surfaces", "Surface Display Configuration"}, + mc_rtc::gui::Form( + "Apply Surface Configuration", + [this](const mc_rtc::Configuration & data) + { + surfaceConfig.color = data("Color"); + surfaceConfig.width = data("Width"); + if(data("Apply to all")) + { + for(const auto & [name, selected] : selected_surfaces) + { + if(selected) + { + removeSurface(name); + addSurface(name); + } + } + } + builder.removeCategory({"Robot", "Surfaces", "Configuration"}); + addSurfaceConfigurationGUI(); + }, + mc_rtc::gui::FormArrayInput("Color", false, surfaceConfig.color), + mc_rtc::gui::FormNumberInput("Width", false, surfaceConfig.width), + mc_rtc::gui::FormCheckbox("Apply to all", false, true))); +} + void RobotVisualizer::removeRobot() { selected_convexes.clear(); @@ -207,7 +238,7 @@ void RobotVisualizer::addSurface(const std::string & name) if(selected_surfaces[name]) { return; } selected_surfaces[name] = true; surfaces_elements[name] = - mc_rbdyn::gui::addSurfaceToGUI(builder, {"Robot", "Surface objects"}, robots->robot(), name); + mc_rbdyn::gui::addSurfaceToGUI(builder, {"Robot", "Surface objects"}, surfaceConfig, robots->robot(), name); } void RobotVisualizer::removeSurface(const std::string & name) diff --git a/utils/RobotVisualizer.h b/utils/RobotVisualizer.h index d4243af253..cf847a7d17 100644 --- a/utils/RobotVisualizer.h +++ b/utils/RobotVisualizer.h @@ -1,8 +1,8 @@ #include #include -#include "mc_rbdyn/gui/RobotConvex.h" -#include "mc_rtc/gui/types.h" +#include +#include struct RobotVisualizer { @@ -26,6 +26,7 @@ struct RobotVisualizer mc_control::ControllerServer server{0.005, server_config}; mc_rtc::gui::StateBuilder builder; mc_rtc::gui::PolyhedronConfig convexConfig = mc_rbdyn::gui::defaultConvexConfig; + mc_rtc::gui::LineConfig surfaceConfig = mc_rbdyn::gui::defaultSurfaceConfig; std::vector available_robots; int selected_robot = -1; @@ -49,6 +50,8 @@ struct RobotVisualizer void addSurface(const std::string & name); + void addSurfaceConfigurationGUI(); + void removeSurface(const std::string & name); void addFrame(const std::string & name);