[utils/mc_surfaces_visualization] Add ability to change surface config

jrl-umi3218 · Feb 5, 2024 · 6dbe368 · 6dbe368
1 parent c692881
commit 6dbe368
Show file tree

Hide file tree

Showing 4 changed files with 70 additions and 5 deletions.
diff --git 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<std::string> addSurfaceToGUI(mc_rtc::gui::StateBuilder & gui,
+                                                         const std::vector<std::string> & category,
+                                                         const mc_rtc::gui::LineConfig & cfg,
+                                                         const mc_rbdyn::Robot & robot,
+                                                         const std::string & name,
+                                                         const std::optional<std::string> & publishName = std::nullopt);
+
+/** Helper function to create a GUI element from a surface object inside a robot
+ *
+ * \see std::vector<std::string> addSurfaceToGUI(mc_rtc::gui::StateBuilder &,
+                                                         const std::vector<std::string> &,
+                                                         const mc_rtc::gui::LineConfig &,
+                                                         const mc_rbdyn::Robot &,
+                                                         const std::string &,
+                                                         const std::optional<std::string> & publishName);
+ */
 MC_RBDYN_DLLAPI std::vector<std::string> addSurfaceToGUI(mc_rtc::gui::StateBuilder & gui,
                                                          const std::vector<std::string> & category,
                                                          const mc_rbdyn::Robot & robot,

diff --git a/src/mc_rbdyn/gui/RobotSurface.cpp b/src/mc_rbdyn/gui/RobotSurface.cpp
@@ -6,9 +6,18 @@
 
 namespace mc_rbdyn::gui
 {
+std::vector<std::string> addSurfaceToGUI(mc_rtc::gui::StateBuilder & gui,
+                                         const std::vector<std::string> & category,
+                                         const mc_rbdyn::Robot & robot,
+                                         const std::string & name,
+                                         const std::optional<std::string> & publishName)
+{
+  return addSurfaceToGUI(gui, category, defaultSurfaceConfig, robot, name, publishName);
+}
 
 std::vector<std::string> addSurfaceToGUI(mc_rtc::gui::StateBuilder & gui,
                                          const std::vector<std::string> & category,
+                                         const mc_rtc::gui::LineConfig & cfg,
                                          const mc_rbdyn::Robot & robot,
                                          const std::string & name,
                                          const std::optional<std::string> & publishName)
@@ -29,7 +38,7 @@ std::vector<std::string> 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<std::string> addSurfaceToGUI(mc_rtc::gui::StateBuilder & gui,
     std::vector<Eigen::Vector3d> 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<Eigen::Vector3d> &
         {
           auto pose = get_pose();

diff --git 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
@@ -1,8 +1,8 @@
 #include <mc_rtc/gui.h>
 
 #include <mc_control/ControllerServer.h>
-#include "mc_rbdyn/gui/RobotConvex.h"
-#include "mc_rtc/gui/types.h"
+#include <mc_rbdyn/gui/RobotConvex.h>
+#include <mc_rbdyn/gui/RobotSurface.h>
 
 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<std::string> 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);