Add realtime rate display to MeshcatVisualizer

This commit adds a stip chart displaying Drake simulator RTR in the meshcat visualizer web page. We depend on Stats.js to draw the chart.

release notes: none

geometry/BUILD.bazel

@@ -347,6 +347,22 @@ drake_cc_library(
     ],
 )
 
+genrule(
+    name = "stats_js_genrule",
+    srcs = ["@statsjs//:build/stats.min.js"],
+    outs = ["stats.min.js"],
+    cmd = "cp $< $@",
+    visibility = ["//visibility:private"],
+)
+
+genrule(
+    name = "msgpack_lite_js_genrule",
+    srcs = ["@msgpack_lite_js//:dist/msgpack.min.js"],
+    outs = ["msgpack.min.js"],
+    cmd = "cp $< $@",
+    visibility = ["//visibility:private"],
+)
+
 genrule(
     name = "meshcat_ico_genrule",
     srcs = ["//doc:favicon.ico"],
@@ -369,6 +385,8 @@ filegroup(
         "meshcat.html",
         ":meshcat.ico",
         ":meshcat.js",
+        ":msgpack.min.js",
+        ":stats.min.js",
     ],
     visibility = ["//visibility:private"],
 )

geometry/meshcat.cc

@@ -52,6 +52,10 @@ std::string LoadResource(const std::string& resource_name) {
 const std::string& GetUrlContent(std::string_view url_path) {
   static const drake::never_destroyed<std::string> meshcat_js(
       LoadResource("drake/geometry/meshcat.js"));
+  static const drake::never_destroyed<std::string> stats_js(
+      LoadResource("drake/geometry/stats.min.js"));
+  static const drake::never_destroyed<std::string> msgpack_lite_js(
+      LoadResource("drake/geometry/msgpack.min.js"));
   static const drake::never_destroyed<std::string> meshcat_ico(
       LoadResource("drake/geometry/meshcat.ico"));
   static const drake::never_destroyed<std::string> meshcat_html(
@@ -65,6 +69,12 @@ const std::string& GetUrlContent(std::string_view url_path) {
   if (url_path == "/meshcat.js") {
     return meshcat_js.access();
   }
+  if (url_path == "/stats.min.js") {
+    return stats_js.access();
+  }
+  if (url_path == "/msgpack.min.js") {
+    return msgpack_lite_js.access();
+  }
   if (url_path == "/favicon.ico") {
     return meshcat_ico.access();
   }
@@ -600,6 +610,23 @@ class Meshcat::Impl {
     return port_;
   }
 
+  void SetRealtimeRate(const double& rate) {
+    DRAKE_DEMAND(IsThread(main_thread_id_));
+    DRAKE_DEMAND(loop_ != nullptr);
+
+    internal::RealtimerateData data;
+    data.rate = rate;
+
+    Defer([this, data = std::move(data)]() {
+      DRAKE_DEMAND(IsThread(websocket_thread_id_));
+      DRAKE_DEMAND(app_ != nullptr);
+      std::stringstream message_stream;
+      msgpack::pack(message_stream, data);
+      std::string message = message_stream.str();
+      app_->publish("all", message, uWS::OpCode::BINARY, false);
+    });
+  }
+
   // This function is public via the PIMPL.
   std::string ws_url() const {
     DRAKE_DEMAND(IsThread(main_thread_id_));
@@ -1359,7 +1386,13 @@ class Meshcat::Impl {
       url = url.replace("https://", "wss://")
       url = url.replace("/index.html", "/")
       url = url.replace("/meshcat.html", "/")
-      viewer.connect(url);
+      connection = new WebSocket(url);
+      connection.binaryType = "arraybuffer";
+      connection.onmessage = (msg) => handle_message(msg);
+      connection.onclose = function(evt) {
+        console.log("onclose:", evt);
+      }
+      viewer.connection = connection
     } catch (e) {
       console.info("Not connected to MeshCat websocket server: ", e);
     })""";
@@ -1368,11 +1401,18 @@ class Meshcat::Impl {
     html.replace(pos, html_connect.size(), std::move(f.get()));
 
     // Insert the javascript directly into the html.
-    const std::string meshcat_src_link(" src=\"meshcat.js\"");
-    pos = html.find(meshcat_src_link);
-    DRAKE_DEMAND(pos != std::string::npos);
-    html.erase(pos, meshcat_src_link.size());
-    html.insert(pos+1, GetUrlContent("/meshcat.js"));
+    vector<pair<string, string>> js_paths{
+        {" src=\"meshcat.js\"", "/meshcat.js"},
+        {" src=\"stats.min.js\"", "/stats.min.js"},
+        {" src=\"msgpack.min.js\"", "/msgpack.min.js"},
+    };
+    for (const auto& js_pair : js_paths) {
+      const std::string src_link(js_pair.first);
+      pos = html.find(src_link);
+      DRAKE_DEMAND(pos != std::string::npos);
+      html.erase(pos, src_link.size());
+      html.insert(pos+1, GetUrlContent(js_pair.second));
+    }
 
     return html;
   }
@@ -1992,6 +2032,10 @@ void Meshcat::Delete(std::string_view path) {
   impl().Delete(path);
 }
 
+void Meshcat::SetRealtimeRate(const double& rate) {
+  impl().SetRealtimeRate(rate);
+}
+
 void Meshcat::SetProperty(std::string_view path, std::string property,
                           bool value) {
   impl().SetProperty(path, std::move(property), value);

geometry/meshcat.h

@@ -159,6 +159,12 @@ class Meshcat {
   void SetObject(std::string_view path, const Shape& shape,
                  const Rgba& rgba = Rgba(.9, .9, .9, 1.));
 
+  /** Sets the realtime that is displayed in the meshcat visualizer strip chart.
+   * @param rate the realtime rate value to be displayed, will be converted to
+   *             a percentage (multiplied by 100)
+   */
+  void SetRealtimeRate(const double& rate);
+
   // TODO(russt): SetObject with texture map.
 
   /** Sets the "object" at a given `path` in the scene tree to be

geometry/meshcat.html

@@ -1,5 +1,5 @@
 <!DOCTYPE html>
-<!-- This file is forked from dist/index.html in rdeits/meshcat. -->
+<!-- This file is forked from dist/index.html in rdeits/meshcat.-->
 <html>
 
 <head>
@@ -12,8 +12,45 @@
   </div>
 
   <script type="text/javascript" src="meshcat.js"></script>
+  <script type="text/javascript" src="stats.min.js"></script>
+  <script type="text/javascript" src="msgpack.min.js"></script>
   <script>
+    var stats = new Stats();
+    var rtrPanel = stats.addPanel( new Stats.Panel( 'rtr%', '#ff8', '#221' ) );
+    document.body.appendChild( stats.dom );
+    if ( self.performance && self.performance.memory ) {
+      // some browsers support performance.memory panel so our index becomes 3
+      stats.showPanel(3);
+    } else {
+      stats.showPanel(2);
+    }
+    var rtr = 0;
     var viewer = new MeshCat.Viewer(document.getElementById("meshcat-pane"));
+    viewer.animate = function () {
+      viewer.animator.update();
+        if (viewer.needs_render) {
+          viewer.render();
+        }
+    }
+
+    function animate() {
+      stats.begin();
+      rtrPanel.update(rtr*100, 100); // easier to read as percentage
+      viewer.animate()
+      stats.end();
+      requestAnimationFrame(animate);
+    }
+
+    function handle_message(ws_message) {
+      let decoded = msgpack.decode(new Uint8Array(ws_message.data));
+      if (decoded.type == "realtime_rate") {
+        rtr = decoded.rate;
+      } else {
+        viewer.handle_command(decoded)
+      }
+    }
+
+    requestAnimationFrame( animate );
     // Set background to match Drake Visualizer.
     viewer.set_property(['Background'], "top_color", [.95, .95, 1.0])
     viewer.set_property(['Background'], "bottom_color", [.32, .32, .35])
@@ -26,7 +63,13 @@
       url = url.replace("https://", "wss://")
       url = url.replace("/index.html", "/")
       url = url.replace("/meshcat.html", "/")
-      viewer.connect(url);
+      connection = new WebSocket(url);
+      connection.binaryType = "arraybuffer";
+      connection.onmessage = (msg) => handle_message(msg);
+      connection.onclose = function(evt) {
+        console.log("onclose:", evt);
+      }
+      viewer.connection = connection
     } catch (e) {
       console.info("Not connected to MeshCat websocket server: ", e);
     }

geometry/meshcat_types.h

@@ -367,6 +367,12 @@ struct SetTransformData {
   MSGPACK_DEFINE_MAP(type, path, matrix);
 };
 
+struct RealtimerateData {
+  std::string type{"realtime_rate"};
+  double rate;
+  MSGPACK_DEFINE_MAP(type, rate);
+};
+
 struct DeleteData {
   std::string type{"delete"};
   std::string path;

geometry/meshcat_visualizer.cc

@@ -6,6 +6,7 @@
 
 #include <fmt/format.h>
 
+#include "drake/common/extract_double.h"
 #include "drake/geometry/utilities.h"
 
 namespace drake {
@@ -95,10 +96,23 @@ systems::EventStatus MeshcatVisualizer<T>::UpdateMeshcat(
     version_ = current_version;
   }
   SetTransforms(context, query_object);
+  UpdateRealtimeRate(ExtractDoubleOrThrow(context.get_time()));
 
   return systems::EventStatus::Succeeded();
 }
 
+template <typename T>
+void MeshcatVisualizer<T>::UpdateRealtimeRate(const double& sim_time) const {
+  const auto current_wall_time = std::chrono::steady_clock::now();
+  std::chrono::duration<double> delta_wall_dur =
+      current_wall_time - prev_wall_time;
+  if (const auto delta_wall_sec = delta_wall_dur.count()) {
+    meshcat_->SetRealtimeRate((sim_time - prev_sim_time) / delta_wall_sec);
+  }
+  prev_sim_time = sim_time;
+  prev_wall_time = current_wall_time;
+}
+
 template <typename T>
 void MeshcatVisualizer<T>::SetObjects(
     const SceneGraphInspector<T>& inspector) const {

geometry/meshcat_visualizer.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <chrono>
 #include <map>
 #include <memory>
 #include <string>
@@ -140,8 +141,7 @@ class MeshcatVisualizer final : public systems::LeafSystem<T> {
   static MeshcatVisualizer<T>& AddToBuilder(
       systems::DiagramBuilder<T>* builder,
       const systems::OutputPort<T>& query_object_port,
-      std::shared_ptr<Meshcat> meshcat,
-      MeshcatVisualizerParams params = {});
+      std::shared_ptr<Meshcat> meshcat, MeshcatVisualizerParams params = {});
 
  private:
   /* MeshcatVisualizer of different scalar types can all access each other's
@@ -213,6 +213,13 @@ class MeshcatVisualizer final : public systems::LeafSystem<T> {
   frame in the animation. */
   bool recording_{false};
   bool set_transforms_while_recording_{true};
+
+  /* Calculate latest realtime rate and send to meshcat */
+  void UpdateRealtimeRate(const double& sim_time) const;
+
+  mutable double prev_sim_time{0};
+  mutable std::chrono::time_point<std::chrono::steady_clock> prev_wall_time{
+      std::chrono::steady_clock::now()};
 };
 
 /** A convenient alias for the MeshcatVisualizer class when using the `double`

geometry/test/meshcat_test.cc

@@ -903,8 +903,10 @@ GTEST_TEST(MeshcatTest, StaticHtml) {
   // Confirm that I have some base64 content.
   EXPECT_THAT(html, HasSubstr("data:application/octet-binary;base64"));
 
-  // Confirm that the meshcat.js link was replaced.
+  // Confirm that the js source links were replaced.
   EXPECT_THAT(html, ::testing::Not(HasSubstr("meshcat.js")));
+  EXPECT_THAT(html, ::testing::Not(HasSubstr("stats.min.js")));
+  EXPECT_THAT(html, ::testing::Not(HasSubstr("msgpack.min.js")));
 }
 
 }  // namespace

tools/workspace/BUILD.bazel

@@ -62,13 +62,15 @@ _DRAKE_EXTERNAL_PACKAGE_INSTALLS = ["@%s//:install" % p for p in [
     "lcm",
     "meshcat",
     "meshcat_python",
+    "msgpack_lite_js",
     "net_sf_jchart2d",
     "org_apache_xmlgraphics_commons",
     "petsc",
     "pybind11",
     "qhull",
     "sdformat",
     "spdlog",
+    "statsjs",
     "tinyobjloader",
     "usockets",
     "uwebsockets",

tools/workspace/default.bzl

@@ -54,6 +54,8 @@ load("@drake//tools/workspace/liblzma:repository.bzl", "liblzma_repository")
 load("@drake//tools/workspace/libpng:repository.bzl", "libpng_repository")
 load("@drake//tools/workspace/libtiff:repository.bzl", "libtiff_repository")
 load("@drake//tools/workspace/meshcat:repository.bzl", "meshcat_repository")
+load("@drake//tools/workspace/statsjs:repository.bzl", "statsjs_repository")
+load("@drake//tools/workspace/msgpack_lite_js:repository.bzl", "msgpack_lite_js_repository")  # noqa
 load("@drake//tools/workspace/meshcat_python:repository.bzl", "meshcat_python_repository")  # noqa
 load("@drake//tools/workspace/models:repository.bzl", "models_repository")
 load("@drake//tools/workspace/mosek:repository.bzl", "mosek_repository")
@@ -214,6 +216,10 @@ def add_default_repositories(excludes = [], mirrors = DEFAULT_MIRRORS):
         libtiff_repository(name = "libtiff")
     if "meshcat" not in excludes:
         meshcat_repository(name = "meshcat", mirrors = mirrors)
+    if "statsjs" not in excludes:
+        statsjs_repository(name = "statsjs", mirrors = mirrors)
+    if "msgpack_lite_js" not in excludes:
+        msgpack_lite_js_repository(name = "msgpack_lite_js", mirrors = mirrors)
     if "meshcat_python" not in excludes:
         meshcat_python_repository(name = "meshcat_python", mirrors = mirrors)
     if "models" not in excludes:

tools/workspace/msgpack_lite_js/BUILD.bazel

@@ -0,0 +1,8 @@
+# -*- python -*-
+
+# This file exists to make our directory into a Bazel package, so that our
+# neighboring *.bzl file can be loaded elsewhere.
+
+load("//tools/lint:lint.bzl", "add_lint_tests")
+
+add_lint_tests()

tools/workspace/msgpack_lite_js/package.BUILD.bazel

@@ -0,0 +1,18 @@
+# -*- python -*-
+
+load(
+    "@drake//tools/install:install.bzl",
+    "install",
+)
+
+licenses(["notice"])  # MIT
+
+package(default_visibility = ["//visibility:public"])
+
+exports_files(["dist/msgpack.min.js"])
+
+# Install the license file.
+install(
+    name = "install",
+    docs = ["LICENSE"],
+)

tools/workspace/msgpack_lite_js/repository.bzl

@@ -0,0 +1,16 @@
+# -*- mode: python -*-
+# vi: set ft=python :
+
+load("@drake//tools/workspace:github.bzl", "github_archive")
+
+def msgpack_lite_js_repository(
+        name,
+        mirrors = None):
+    github_archive(
+        name = name,
+        repository = "kawanet/msgpack-lite",
+        commit = "5b71d82cad4b96289a466a6403d2faaa3e254167",
+        sha256 = "e0a3f03a85fe7748257747b289a6062efdc2cfa5f08cefc3dbdebe45eae0904b",  # noqa
+        build_file = "@drake//tools/workspace/msgpack_lite_js:package.BUILD.bazel",  # noqa
+        mirrors = mirrors,
+    )

tools/workspace/statsjs/BUILD.bazel

@@ -0,0 +1,8 @@
+# -*- python -*-
+
+# This file exists to make our directory into a Bazel package, so that our
+# neighboring *.bzl file can be loaded elsewhere.
+
+load("//tools/lint:lint.bzl", "add_lint_tests")
+
+add_lint_tests()