[meshcat] Add realtime rate display to MeshcatVisualizer

common/BUILD.bazel

@@ -57,6 +57,7 @@ drake_cc_package_library(
         ":symbolic_latex",
         ":symbolic_trigonometric_polynomial",
         ":temp_directory",
+        ":time",
         ":type_safe_index",
         ":unused",
         ":value",
@@ -599,6 +600,14 @@ drake_cc_library(
     ],
 )
 
+drake_cc_library(
+    name = "time",
+    hdrs = ["timer.h"],
+    deps = [
+        ":essential",
+    ],
+)
+
 drake_cc_library(
     name = "value",
     srcs = ["value.cc"],
@@ -1465,6 +1474,13 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "time_test",
+    deps = [
+        ":time",
+    ],
+)
+
 drake_cc_googletest(
     name = "drake_cc_googletest_main_test_device",
     args = ["--magic_number=1.0"],

common/test/time_test.cc

@@ -0,0 +1,20 @@
+#include "gtest/gtest.h"
+
+#include "drake/common/timer.h"
+
+namespace drake {
+GTEST_TEST(TimeTest, TimerFunction) {
+  std::unique_ptr<Timer> timer = std::make_unique<SteadyTimer>();
+  timer->Start();
+  // Assert that some positive time has passed
+  EXPECT_GT(timer->Tick().count(), 0.0);
+  EXPECT_GT(timer->Stop().count(), 0.0);
+}
+
+GTEST_TEST(TimeTest, ConstructorStartsTimer) {
+  SteadyTimer timer{};
+  // Assert that some positive time has passed
+  EXPECT_GT(timer.Tick().count(), 0.0);
+  EXPECT_GT(timer.Stop().count(), 0.0);
+}
+}  // namespace drake

common/timer.h

@@ -0,0 +1,37 @@
+#pragma once
+
+#include <chrono>
+
+/// @file
+/// Provides drake::Timer interface and drake::SteadyTimer for timing events
+
+namespace drake {
+
+/// Abstract base class for timing utility.
+struct Timer {
+  using duration = std::chrono::duration<double>;
+  virtual ~Timer() = default;
+  /// begin timing
+  virtual void Start() = 0;
+  /// stop timing
+  /// @return the ammount of time since the timer started
+  virtual duration Stop() = 0;
+  /// obtain multiple measurements from the same baseline
+  /// @return the ammount of time since the timer started
+  virtual duration Tick() = 0;
+};
+
+/// Implementation of timing utility that uses monotonic
+/// std::chrono::steady_clock
+class SteadyTimer : public Timer {
+ public:
+  using clock = std::chrono::steady_clock;
+  void Start() override { start_time = clock::now(); }
+  duration Tick() override { return duration(clock::now() - start_time); }
+  duration Stop() override { return Tick(); }
+
+ protected:
+  clock::time_point start_time{clock::now()};
+};
+
+}  // namespace drake

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"],
 )
@@ -517,6 +535,7 @@ drake_cc_library(
         ":scene_graph",
         "//common:essential",
         "//math:geometric_transform",
+        "//systems/analysis:instantaneous_realtime_rate_calculator",
         "//systems/framework:context",
         "//systems/framework:diagram_builder",
         "//systems/framework:leaf_system",

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,21 @@ class Meshcat::Impl {
     return port_;
   }
 
+  // This function is public via the PIMPL.
+  void SetRealtimeRate(double rate) {
+    DRAKE_DEMAND(IsThread(main_thread_id_));
+    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 +1384,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 +1399,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"));
+    std::vector<std::pair<std::string, std::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;
   }
@@ -1645,6 +1683,14 @@ class Meshcat::Impl {
         ws->send(message_stream.str());
       }
     }
+
+    // Tell client if the realtime rate plot should be hidden
+    internal::HideRealtimeRate rtr_hidden_msg{};
+    rtr_hidden_msg.hide = params_.hide_stats_plot;
+    std::stringstream rtr_msg_stream;
+    msgpack::pack(rtr_msg_stream, rtr_hidden_msg);
+    ws->send(rtr_msg_stream.str());
+
     if (inject_open_fault_.load()) {
       throw std::runtime_error(
           "InjectWebsocketThreadFault during socket open");
@@ -1992,6 +2038,10 @@ void Meshcat::Delete(std::string_view path) {
   impl().Delete(path);
 }
 
+void Meshcat::SetRealtimeRate(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

@@ -47,6 +47,10 @@ struct MeshcatParams {
     "all interfaces".
   */
   std::string web_url_pattern{"http://{host}:{port}"};
+
+  /** Determines whether or not to display the stats plot including realtime
+   * rate display. */
+  bool hide_stats_plot{false};
 };
 
 /** Provides an interface to %Meshcat (https://github.com/rdeits/meshcat).
@@ -347,6 +351,12 @@ class Meshcat {
   See @ref meshcat_path for the detailed semantics of deletion. */
   void Delete(std::string_view path = "");
 
+  /** Sets the realtime rate 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(double rate);
+
   /** Sets a single named property of the object at the given path. For example,
   @verbatim
   meshcat.SetProperty("/Background", "visible", false);

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,44 @@
   </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 );
+    stats.dom.id = "stats-plot";
+    stats.showPanel(stats.dom.children.length - 1)
+    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 if (decoded.type == "hide_realtime_rate") {
+       stats.dom.style.display = decoded.hide ? "none" : "block";
+       console.log("hiding", decoded.hide)
+      } 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 +62,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);
     }
@@ -42,6 +84,10 @@
       height: 100vh;
       overflow: hidden;
     }
+
+    #stats-plot {
+      display: none;
+    }
   </style>
   <script id="embedded-json"></script>
 </body>

geometry/meshcat_types.h

@@ -367,6 +367,24 @@ struct SetTransformData {
   MSGPACK_DEFINE_MAP(type, path, matrix);
 };
 
+// Note that this struct is unique to Drake's integration of meshcat; it is not
+// part of upstream meshcat.js. We handle it directly within meshcat.html,
+// without ever feeding it into meshcat.js.
+struct RealtimeRateData {
+  std::string type{"realtime_rate"};
+  double rate{};
+  MSGPACK_DEFINE_MAP(type, rate);
+};
+
+// Note that this struct is unique to Drake's integration of meshcat; it is not
+// part of upstream meshcat.js. We handle it directly within meshcat.html,
+// without ever feeding it into meshcat.js.
+struct HideRealtimeRate {
+  std::string type{"hide_realtime_rate"};
+  bool hide{false};
+  MSGPACK_DEFINE_MAP(type, hide);
+};
+
 struct DeleteData {
   std::string type{"delete"};
   std::string path;