[geometry] Establish Meshcat in C++ (step 4)

geometry/BUILD.bazel

@@ -36,6 +36,8 @@ drake_cc_package_library(
         ":internal_frame",
         ":internal_geometry",
         ":meshcat",
+        ":meshcat_visualizer",
+        ":meshcat_visualizer_params",
         ":proximity_engine",
         ":proximity_properties",
         ":rgba",
@@ -380,9 +382,18 @@ drake_cc_binary(
     name = "meshcat_manual_test",
     testonly = True,
     srcs = ["test/meshcat_manual_test.cc"],
-    data = ["//systems/sensors:test_models"],
+    data = [
+        "//manipulation/models/iiwa_description:models",
+        "//systems/sensors:test_models",
+    ],
     visibility = ["//visibility:private"],
-    deps = [":meshcat"],
+    deps = [
+        ":meshcat",
+        ":meshcat_visualizer",
+        "//multibody/parsing",
+        "//multibody/plant",
+        "//systems/analysis:simulator",
+    ],
 )
 
 drake_py_binary(
@@ -407,6 +418,48 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_library(
+    name = "meshcat_visualizer_params",
+    hdrs = ["meshcat_visualizer_params.h"],
+    deps = [
+        ":geometry_roles",
+        ":rgba",
+    ],
+)
+
+drake_cc_library(
+    name = "meshcat_visualizer",
+    srcs = ["meshcat_visualizer.cc"],
+    hdrs = ["meshcat_visualizer.h"],
+    deps = [
+        ":geometry_roles",
+        ":meshcat",
+        ":meshcat_visualizer_params",
+        ":rgba",
+        ":scene_graph",
+        "//common:essential",
+        "//math:geometric_transform",
+        "//systems/framework:context",
+        "//systems/framework:diagram_builder",
+        "//systems/framework:leaf_system",
+    ],
+)
+
+drake_cc_googletest(
+    name = "meshcat_visualizer_test",
+    data = [
+        "//manipulation/models/iiwa_description:models",
+    ],
+    deps = [
+        ":meshcat_visualizer",
+        "//common/test_utilities:expect_throws_message",
+        "//multibody/parsing",
+        "//multibody/plant",
+        "//systems/analysis:simulator",
+        "//systems/primitives:constant_vector_source",
+    ],
+)
+
 # -----------------------------------------------------
 
 filegroup(

geometry/meshcat.cc

@@ -1,9 +1,11 @@
 #include "drake/geometry/meshcat.h"
 
+#include <exception>
 #include <fstream>
 #include <future>
 #include <map>
 #include <optional>
+#include <set>
 #include <sstream>
 #include <string>
 #include <thread>
@@ -68,6 +70,12 @@ struct PerSocketData {
 using WebSocket = uWS::WebSocket<kSsl, kIsServer, PerSocketData>;
 using MsgPackMap = std::map<std::string, msgpack::object>;
 
+// The maximum "backpressure" allowed each websocket, in bytes.  This
+// effectively sets a maximum size for messages, which may include mesh files
+// and texture maps.  50mb is a guess at a compromise that is safely larger than
+// reasonable meshfiles.
+constexpr static double kMaxBackPressure{50 * 1024 * 1024};
+
 class SceneTreeElement {
  public:
   // Member access methods (object_, transform_, and properties_ should be
@@ -187,7 +195,8 @@ class MeshcatShapeReifier : public ShapeReifier {
  public:
   DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(MeshcatShapeReifier);
 
-  explicit MeshcatShapeReifier(std::string uuid) : uuid_(std::move(uuid)) {}
+  explicit MeshcatShapeReifier(std::string uuid)
+      : uuid_(std::move(uuid)) {}
 
   ~MeshcatShapeReifier() = default;
 
@@ -225,6 +234,8 @@ class MeshcatShapeReifier : public ShapeReifier {
   }
 
   void ImplementGeometry(const HalfSpace&, void*) override {
+    // TODO(russt): Use PlaneGeometry with fields width, height,
+    // widthSegments, heightSegments
     drake::log()->warn("Meshcat does not display HalfSpace geometry (yet).");
   }
 
@@ -292,6 +303,14 @@ class MeshcatShapeReifier : public ShapeReifier {
     // We simply dump the binary contents of the file into the data field of the
     // message.  The javascript meshcat takes care of the rest.
     int size = input.tellg();
+    if (size > kMaxBackPressure) {
+      throw std::runtime_error(fmt::format(
+          "The meshfile at {} is too large for the current websocket setup.  "
+          "Size {} is greater than the max backpressure {}.  You will either "
+          "need to reduce the size of your mesh, or modify the code to "
+          "increase the allowance.",
+          mesh.filename(), size, kMaxBackPressure));
+    }
     input.seekg(0, std::ios::beg);
     geometry.data.resize(size);
     input.read(geometry.data.data(), size);
@@ -337,8 +356,12 @@ class Meshcat::WebSocketPublisher {
 
   ~WebSocketPublisher() {
     DRAKE_DEMAND(std::this_thread::get_id() == main_thread_id_);
-    loop_->defer(
-        [socket = listen_socket_]() { us_listen_socket_close(0, socket); });
+    loop_->defer([this]() {
+      us_listen_socket_close(0, listen_socket_);
+      for (auto* ws : websockets_) {
+        ws->close();
+      }
+    });
     websocket_thread_.join();
   }
 
@@ -552,6 +575,15 @@ class Meshcat::WebSocketPublisher {
       ws->subscribe("all");
       // Update this new connection with previously published data.
       SendTree(ws);
+      websockets_.emplace(ws);
+    };
+    // TODO(russt): I could increase this more if necessary (when it was too
+    // low, some SetObject messages were dropped).  But at some point the real
+    // fix is to actually throttle the sending (e.g. by slowing down the main
+    // thread).
+    behavior.maxBackpressure = kMaxBackPressure;
+    behavior.close = [this](WebSocket* ws, int, std::string_view) {
+      websockets_.erase(ws);
     };
 
     uWS::App app =
@@ -621,6 +653,7 @@ class Meshcat::WebSocketPublisher {
   // they are pointing to should be only used in the websocket thread.
   uWS::App* app_{nullptr};
   us_listen_socket_t* listen_socket_{nullptr};
+  std::set<WebSocket*> websockets_{};
 
   // This pointer should only be accessed in the main thread, but the Loop
   // object itself should be only used in the websocket thread, with one

geometry/meshcat.h

@@ -72,8 +72,6 @@ in the visualizer.
 - All user objects can easily be cleared by a single, parameter-free call to
 Delete(). You are welcome to use absolute paths to organize your data, but the
 burden on tracking and cleaning them up lie on you.
-
-
 */
 class Meshcat {
  public:
@@ -97,7 +95,7 @@ class Meshcat {
   @ref meshcat_path.  Any objects previously set at this `path` will be
   replaced.
   @param path a "/"-delimited string indicating the path in the scene tree.
-              See @ref meshcat_path for the semantics.
+              See @ref meshcat_path "Meshcat paths" for the semantics.
   @param shape a Shape that specifies the geometry of the object.
   @param rgba an Rgba that specifies the (solid) color of the object.
   */
@@ -111,7 +109,7 @@ class Meshcat {
   the transform of "/foo" will move the objects at "/foo/box1" and
   "/foo/robots/HAL9000".
   @param path a "/"-delimited string indicating the path in the scene tree.
-              See @ref meshcat_path for the semantics.
+              See @ref meshcat_path "Meshcat paths" for the semantics.
   @param X_ParentPath the relative transform from the path to its immediate
   parent.
   */
@@ -126,8 +124,8 @@ class Meshcat {
   @verbatim
   meshcat.SetProperty("/Background", "visible", false);
   @endverbatim
-  will turn off the background. See @ref meshcat_path for more details about
-  these properties and how to address them.
+  will turn off the background. See @ref meshcat_path "Meshcat paths" for more
+  details about these properties and how to address them.
 
   @param path a "/"-delimited string indicating the path in the scene tree.
               See @ref meshcat_path for the semantics.
@@ -141,8 +139,8 @@ class Meshcat {
   meshcat.SetProperty("/Cameras/default/rotated/<object>", "zoom", 2.0);
   meshcat.SetProperty("/Lights/DirectionalLight/<object>", "intensity", 1.0);
   @endverbatim
-  See @ref meshcat_path for more details about these properties and how to
-  address them.
+  See @ref meshcat_path "Meshcat paths" for more details about these properties
+  and how to address them.
 
   @param path a "/"-delimited string indicating the path in the scene tree.
               See @ref meshcat_path for the semantics.

geometry/meshcat_visualizer.cc

@@ -0,0 +1,170 @@
+#include "drake/geometry/meshcat_visualizer.h"
+
+#include <memory>
+#include <string>
+#include <utility>
+
+#include <fmt/format.h>
+
+#include "drake/geometry/utilities.h"
+
+namespace drake {
+namespace geometry {
+
+template <typename T>
+MeshcatVisualizer<T>::MeshcatVisualizer(std::shared_ptr<Meshcat> meshcat,
+                                        MeshcatVisualizerParams params)
+    : systems::LeafSystem<T>(systems::SystemTypeTag<MeshcatVisualizer>{}),
+      meshcat_(std::move(meshcat)),
+      params_(std::move(params)) {
+  DRAKE_DEMAND(meshcat_ != nullptr);
+  DRAKE_DEMAND(params_.publish_period >= 0.0);
+  if (params_.role == Role::kUnassigned) {
+    throw std::runtime_error(
+        "MeshcatVisualizer cannot be used for geometries with the "
+        "Role::kUnassigned value. Please choose kProximity, kPerception, or "
+        "kIllustration");
+  }
+
+  this->DeclarePeriodicPublishEvent(params_.publish_period, 0.0,
+                                    &MeshcatVisualizer<T>::UpdateMeshcat);
+  this->DeclareForcedPublishEvent(&MeshcatVisualizer<T>::UpdateMeshcat);
+
+  if (params_.delete_prefix_on_initialization_event) {
+    this->DeclareInitializationPublishEvent(
+        &MeshcatVisualizer<T>::OnInitialization);
+  }
+
+  query_object_input_port_ =
+      this->DeclareAbstractInputPort("query_object", Value<QueryObject<T>>())
+          .get_index();
+}
+
+template <typename T>
+template <typename U>
+MeshcatVisualizer<T>::MeshcatVisualizer(const MeshcatVisualizer<U>& other)
+    : MeshcatVisualizer(other.meshcat_, other.params_) {}
+
+template <typename T>
+void MeshcatVisualizer<T>::Delete() const {
+  meshcat_->Delete(params_.prefix);
+  version_ = GeometryVersion();
+}
+
+template <typename T>
+const MeshcatVisualizer<T>& MeshcatVisualizer<T>::AddToBuilder(
+    systems::DiagramBuilder<T>* builder, const SceneGraph<T>& scene_graph,
+    std::shared_ptr<Meshcat> meshcat, MeshcatVisualizerParams params) {
+  return AddToBuilder(builder, scene_graph.get_query_output_port(),
+                      std::move(meshcat), std::move(params));
+}
+
+template <typename T>
+const MeshcatVisualizer<T>& MeshcatVisualizer<T>::AddToBuilder(
+    systems::DiagramBuilder<T>* builder,
+    const systems::OutputPort<T>& query_object_port,
+    std::shared_ptr<Meshcat> meshcat, MeshcatVisualizerParams params) {
+  auto& visualizer = *builder->template AddSystem<MeshcatVisualizer<T>>(
+      std::move(meshcat), std::move(params));
+  builder->Connect(query_object_port, visualizer.query_object_input_port());
+  return visualizer;
+}
+
+template <typename T>
+systems::EventStatus MeshcatVisualizer<T>::UpdateMeshcat(
+    const systems::Context<T>& context) const {
+  const auto& query_object =
+      query_object_input_port().template Eval<QueryObject<T>>(context);
+  const GeometryVersion& current_version =
+      query_object.inspector().geometry_version();
+
+  if (!version_.IsSameAs(current_version, params_.role)) {
+    SetObjects(query_object.inspector());
+    version_ = current_version;
+  }
+  SetTransforms(query_object);
+
+  return systems::EventStatus::Succeeded();
+}
+
+template <typename T>
+void MeshcatVisualizer<T>::SetObjects(
+    const SceneGraphInspector<T>& inspector) const {
+  // Frames registered previously that are not set again here should be deleted.
+  std::map <FrameId, std::string> frames_to_delete{};
+  dynamic_frames_.swap(frames_to_delete);
+
+  // Geometries registered previously that are not set again here should be
+  // deleted.
+  std::map <GeometryId, std::string> geometries_to_delete{};
+  geometries_.swap(geometries_to_delete);
+
+  // TODO(SeanCurtis-TRI): Mimic the full tree structure in SceneGraph.
+  // SceneGraph supports arbitrary hierarchies of frames just like Meshcat.
+  // This code is arbitrarily flattening it because the current SceneGraph API
+  // is insufficient to support walking the tree.
+  for (FrameId frame_id : inspector.GetAllFrameIds()) {
+    std::string frame_path =
+        frame_id == inspector.world_frame_id()
+            ? params_.prefix
+            : fmt::format("{}/{}", params_.prefix, inspector.GetName(frame_id));
+    // MultibodyPlant declares frames with SceneGraph using "::". We replace
+    // those with `/` here to expose the full tree to Meshcat.
+    size_t pos = 0;
+    while ((pos = frame_path.find("::", pos)) != std::string::npos) {
+      frame_path.replace(pos++, 2, "/");
+    }
+    if (frame_id != inspector.world_frame_id() &&
+        inspector.NumGeometriesForFrameWithRole(frame_id, params_.role) > 0) {
+      dynamic_frames_[frame_id] = frame_path;
+      frames_to_delete.erase(frame_id);  // Don't delete this one.
+    }
+
+    for (GeometryId geom_id : inspector.GetGeometries(frame_id, params_.role)) {
+      // Note: We use the frame_path/id instead of instance.GetName(geom_id),
+      // which is a garbled mess of :: and _ and a memory address by default
+      // when coming from MultibodyPlant.
+      // TODO(russt): Use the geometry names if/when they are cleaned up.
+      const std::string path =
+          fmt::format("{}/{}", frame_path, geom_id.get_value());
+      const Rgba rgba = inspector.GetProperties(geom_id, params_.role)
+          ->GetPropertyOrDefault("phong", "diffuse", params_.default_color);
+
+      meshcat_->SetObject(path, inspector.GetShape(geom_id), rgba);
+      meshcat_->SetTransform(path, inspector.GetPoseInFrame(geom_id));
+      geometries_[geom_id] = path;
+      geometries_to_delete.erase(geom_id);  // Don't delete this one.
+    }
+  }
+
+  for (const auto& [geom_id, path] : geometries_to_delete) {
+    unused(geom_id);
+    meshcat_->Delete(path);
+  }
+  for (const auto& [frame_id, path] : frames_to_delete) {
+    unused(frame_id);
+    meshcat_->Delete(path);
+  }
+}
+
+template <typename T>
+void MeshcatVisualizer<T>::SetTransforms(
+    const QueryObject<T>& query_object) const {
+  for (const auto& [frame_id, path] : dynamic_frames_) {
+    meshcat_->SetTransform(path, internal::convert_to_double(
+                                     query_object.GetPoseInWorld(frame_id)));
+  }
+}
+
+template <typename T>
+systems::EventStatus MeshcatVisualizer<T>::OnInitialization(
+    const systems::Context<T>&) const {
+  Delete();
+  return systems::EventStatus::Succeeded();
+}
+
+}  // namespace geometry
+}  // namespace drake
+
+DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS(
+    class ::drake::geometry::MeshcatVisualizer)