-
Notifications
You must be signed in to change notification settings - Fork 1.3k
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
[geometry] Establish Meshcat in C++ (step 4)
Adds MeshcatVisualizer Resolves #13038
- Loading branch information
1 parent
1fa98f7
commit 53e346d
Showing
8 changed files
with
721 additions
and
27 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,154 @@ | ||
#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(const std::shared_ptr<Meshcat>& meshcat, | ||
MeshcatVisualizerParams params) | ||
: systems::LeafSystem<T>(systems::SystemTypeTag<MeshcatVisualizer>{}), | ||
meshcat_(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 proximity, perception, or " | ||
"illustration"); | ||
} | ||
|
||
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>::DeletePrefix() 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, | ||
const std::shared_ptr<Meshcat>& meshcat, MeshcatVisualizerParams params) { | ||
return AddToBuilder(builder, scene_graph.get_query_output_port(), meshcat, | ||
std::move(params)); | ||
} | ||
|
||
template <typename T> | ||
const MeshcatVisualizer<T>& MeshcatVisualizer<T>::AddToBuilder( | ||
systems::DiagramBuilder<T>* builder, | ||
const systems::OutputPort<T>& query_object_port, | ||
const std::shared_ptr<Meshcat>& meshcat, MeshcatVisualizerParams params) { | ||
auto& visualizer = *builder->template AddSystem<MeshcatVisualizer<T>>( | ||
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); | ||
|
||
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. | ||
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)); | ||
} | ||
} | ||
|
||
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 { | ||
DeletePrefix(); | ||
return systems::EventStatus::Succeeded(); | ||
} | ||
|
||
} // namespace geometry | ||
} // namespace drake | ||
|
||
DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS( | ||
class ::drake::geometry::MeshcatVisualizer) |
Oops, something went wrong.