Skip to content

Commit

Permalink
MeshcatVisualizer sets visibility directly without events (RobotLocom…
Browse files Browse the repository at this point in the history
…otion#19578)

Previously, the visibility parameter was not respected (a) before the
initialization event or (b) if delete_on_initialization_event was false.

Now, we update visibility exactly when we first transmit shapes to
Meshcat and we don't update visibility during (re-)initialization.
  • Loading branch information
RussTedrake committed Jul 3, 2023
1 parent dd28b0c commit 95e4496
Show file tree
Hide file tree
Showing 3 changed files with 33 additions and 19 deletions.
14 changes: 10 additions & 4 deletions geometry/meshcat_visualizer.cc
Original file line number Diff line number Diff line change
Expand Up @@ -63,7 +63,7 @@ MeshcatVisualizer<T>::MeshcatVisualizer(const MeshcatVisualizer<U>& other)
template <typename T>
void MeshcatVisualizer<T>::Delete() const {
meshcat_->Delete(params_.prefix);
version_ = GeometryVersion();
version_ = std::nullopt;
}

template <typename T>
Expand Down Expand Up @@ -125,8 +125,15 @@ systems::EventStatus MeshcatVisualizer<T>::UpdateMeshcat(
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)) {
if (!version_.has_value()) {
// When our current version is null, that means we haven't added any
// geometry to Meshcat yet, which means we also need to establish our
// default visibility just prior to sending the geometry.
meshcat_->SetProperty(params_.prefix, "visible",
params_.visible_by_default);
}
if (!version_.has_value() ||
!version_->IsSameAs(current_version, params_.role)) {
SetObjects(query_object.inspector());
version_ = current_version;
}
Expand Down Expand Up @@ -279,7 +286,6 @@ template <typename T>
systems::EventStatus MeshcatVisualizer<T>::OnInitialization(
const systems::Context<T>&) const {
Delete();
meshcat_->SetProperty(params_.prefix, "visible", params_.visible_by_default);
return systems::EventStatus::Succeeded();
}

Expand Down
3 changes: 2 additions & 1 deletion geometry/meshcat_visualizer.h
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@

#include <map>
#include <memory>
#include <optional>
#include <string>

#include "drake/geometry/geometry_roles.h"
Expand Down Expand Up @@ -193,7 +194,7 @@ class MeshcatVisualizer final : public systems::LeafSystem<T> {
before SetTransforms. This is intended to track the information in meshcat_,
and is therefore also a mutable member variable (instead of declared state).
*/
mutable GeometryVersion version_;
mutable std::optional<GeometryVersion> version_;

/* A store of the dynamic frames and their path. It is coupled with the
version_. This is only for efficiency; it does not represent undeclared
Expand Down
35 changes: 21 additions & 14 deletions geometry/test/meshcat_visualizer_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -62,6 +62,18 @@ class MeshcatVisualizerWithIiwaTest : public ::testing::Test {
context_ = diagram_->CreateDefaultContext();
}

void CheckVisible(const std::string& path, bool visibility) {
ASSERT_TRUE(meshcat_->HasPath(path));
const std::string property =
meshcat_->GetPackedProperty(path, "visible");
ASSERT_GT(property.size(), 0);
msgpack::object_handle oh =
msgpack::unpack(property.data(), property.size());
auto data = oh.get().as<internal::SetPropertyData<bool>>();
EXPECT_EQ(data.property, "visible");
EXPECT_EQ(data.value, visibility);
}

std::shared_ptr<Meshcat> meshcat_;
multibody::MultibodyPlant<double>* plant_{};
SceneGraph<double>* scene_graph_{};
Expand All @@ -73,6 +85,10 @@ class MeshcatVisualizerWithIiwaTest : public ::testing::Test {
TEST_F(MeshcatVisualizerWithIiwaTest, BasicTest) {
SetUpDiagram();

// Visibility remains unset until geometry gets added.
EXPECT_EQ(meshcat_->GetPackedProperty("/drake/visualizer", "visible").size(),
0);

EXPECT_FALSE(meshcat_->HasPath("/drake/visualizer/iiwa14"));
diagram_->ForcedPublish(*context_);
EXPECT_TRUE(meshcat_->HasPath("/drake/visualizer/iiwa14"));
Expand All @@ -81,6 +97,7 @@ TEST_F(MeshcatVisualizerWithIiwaTest, BasicTest) {
fmt::format("/drake/visualizer/iiwa14/iiwa_link_{}", link)),
"");
}
CheckVisible("/drake/visualizer", true);

// Confirm that the transforms change after running a simulation.
const std::string packed_X_W7 =
Expand Down Expand Up @@ -166,23 +183,13 @@ TEST_F(MeshcatVisualizerWithIiwaTest, NotVisibleByDefault) {
MeshcatVisualizerParams params;
params.visible_by_default = false;

// Create the diagram and publish both the initialization and periodic event.
// Create and run the diagram.
SetUpDiagram(params);
{
auto events = diagram_->AllocateCompositeEventCollection();
diagram_->GetInitializationEvents(*context_, events.get());
diagram_->Publish(*context_, events->get_publish_events());
diagram_->ForcedPublish(*context_);
}
systems::Simulator<double> simulator(*diagram_);
simulator.AdvanceTo(0.1);

// Confirm that the path was added but was set to be invisible.
ASSERT_TRUE(meshcat_->HasPath("/drake/visualizer"));
const std::string property =
meshcat_->GetPackedProperty("/drake/visualizer", "visible");
msgpack::object_handle oh = msgpack::unpack(property.data(), property.size());
auto data = oh.get().as<internal::SetPropertyData<bool>>();
EXPECT_EQ(data.property, "visible");
EXPECT_EQ(data.value, false);
CheckVisible("/drake/visualizer", false);
}

TEST_F(MeshcatVisualizerWithIiwaTest, DeletePrefixOnInitialization) {
Expand Down

0 comments on commit 95e4496

Please sign in to comment.