diff --git a/examples/geometry_world/solar_system.cc b/examples/geometry_world/solar_system.cc index 4545d402a4a5..f6accd2c0667 100644 --- a/examples/geometry_world/solar_system.cc +++ b/examples/geometry_world/solar_system.cc @@ -28,6 +28,7 @@ using geometry::GeometrySystem; using geometry::Mesh; using geometry::SourceId; using geometry::Sphere; +using geometry::VisualMaterial; using systems::BasicVector; using systems::Context; using systems::ContinuousState; @@ -106,7 +107,7 @@ void SolarSystem::SetDefaultState(const systems::Context&, // origin, and the top of the arm is positioned at the given height. template void MakeArm(SourceId source_id, ParentId parent_id, double length, - double height, double radius, + double height, double radius, const VisualMaterial& material, GeometrySystem* geometry_system) { Isometry3 arm_pose = Isometry3::Identity(); // tilt it horizontally @@ -116,14 +117,14 @@ void MakeArm(SourceId source_id, ParentId parent_id, double length, geometry_system->RegisterGeometry( source_id, parent_id, make_unique( - arm_pose, make_unique(radius, length))); + arm_pose, make_unique(radius, length), material)); Isometry3 post_pose = Isometry3::Identity(); post_pose.translation() << length, 0, height/2; geometry_system->RegisterGeometry( source_id, parent_id, make_unique( - post_pose, make_unique(radius, height))); + post_pose, make_unique(radius, height), material)); } template @@ -132,6 +133,7 @@ void SolarSystem::AllocateGeometry(GeometrySystem* geometry_system) { body_offset_.reserve(kBodyCount); axes_.reserve(kBodyCount); + VisualMaterial post_material{Vector4d(0.3, 0.15, 0.05, 1)}; const double orrery_bottom = -1.5; const double pipe_radius = 0.05; @@ -139,8 +141,9 @@ void SolarSystem::AllocateGeometry(GeometrySystem* geometry_system) { // NOTE: we don't store the id of the sun geometry because we have no need // for subsequent access (the same is also true for dynamic geometries). geometry_system->RegisterAnchoredGeometry( - source_id_, make_unique(Isometry3::Identity(), - make_unique(1.f))); + source_id_, make_unique( + Isometry3::Identity(), make_unique(1.f), + VisualMaterial(Vector4d(1, 1, 0, 1)))); // The fixed post on which Sun sits and around which all planets rotate. const double post_height = 1; @@ -149,7 +152,8 @@ void SolarSystem::AllocateGeometry(GeometrySystem* geometry_system) { geometry_system->RegisterAnchoredGeometry( source_id_, make_unique( post_pose, - make_unique(pipe_radius, post_height))); + make_unique(pipe_radius, post_height), + post_material)); // Allocate the "celestial bodies": two planets orbiting on different planes, // each with a moon. @@ -172,10 +176,11 @@ void SolarSystem::AllocateGeometry(GeometrySystem* geometry_system) { Translation3{kEarthOrbitRadius, 0, -kEarthBottom}}; geometry_system->RegisterGeometry( source_id_, planet_id, - make_unique(X_EGe, make_unique(0.25f))); + make_unique(X_EGe, make_unique(0.25f), + VisualMaterial(Vector4d(0, 0, 1, 1)))); // Earth's orrery arm. MakeArm(source_id_, planet_id, kEarthOrbitRadius, -kEarthBottom, pipe_radius, - geometry_system); + post_material, geometry_system); // Luna's frame L is at the center of Earth's geometry (Ge). So, X_EL = X_EGe. const Isometry3& X_EL = X_EGe; @@ -195,8 +200,9 @@ void SolarSystem::AllocateGeometry(GeometrySystem* geometry_system) { Vector3(-1, 0.5, 0.5).normalized() * kLunaOrbitRadius; Isometry3 X_LGl{Translation3{luna_position}}; geometry_system->RegisterGeometry( - source_id_, luna_id, - make_unique(X_LGl, make_unique(0.075f))); + source_id_, luna_id, make_unique( + X_LGl, make_unique(0.075f), + VisualMaterial(Vector4d(0.5, 0.5, 0.35, 1)))); // Mars's frame M lies directly *below* the sun (to account for the orrery // arm). @@ -215,7 +221,8 @@ void SolarSystem::AllocateGeometry(GeometrySystem* geometry_system) { Translation3{kMarsOrbitRadius, 0, -orrery_bottom}}; GeometryId mars_geometry_id = geometry_system->RegisterGeometry( source_id_, planet_id, - make_unique(X_MGm, make_unique(kMarsSize))); + make_unique(X_MGm, make_unique(kMarsSize), + VisualMaterial(Vector4d(0.9, 0.1, 0, 1)))); std::string rings_absolute_path = FindResourceOrThrow("drake/examples/geometry_world/planet_rings.obj"); @@ -224,11 +231,12 @@ void SolarSystem::AllocateGeometry(GeometrySystem* geometry_system) { geometry_system->RegisterGeometry( source_id_, mars_geometry_id, make_unique( - X_GmGr, make_unique(rings_absolute_path, kMarsSize))); + X_GmGr, make_unique(rings_absolute_path, kMarsSize), + VisualMaterial(Vector4d(0.45, 0.9, 0, 1)))); // Mars's orrery arm. MakeArm(source_id_, planet_id, kMarsOrbitRadius, -orrery_bottom, pipe_radius, - geometry_system); + post_material, geometry_system); // Phobos's frame P is at the center of Mars's geometry (Gm). // So, X_MP = X_MGm. The normal of the plane is negated so it orbits in the @@ -245,8 +253,9 @@ void SolarSystem::AllocateGeometry(GeometrySystem* geometry_system) { const double kPhobosOrbitRadius = 0.34; Isometry3 X_PGp{Translation3{kPhobosOrbitRadius, 0, 0}}; geometry_system->RegisterGeometry( - source_id_, phobos_id, - make_unique(X_PGp, make_unique(0.06f))); + source_id_, phobos_id, make_unique( + X_PGp, make_unique(0.06f), + VisualMaterial(Vector4d(0.65, 0.6, 0.8, 1)))); DRAKE_DEMAND(static_cast(body_ids_.size()) == kBodyCount); } diff --git a/geometry/BUILD.bazel b/geometry/BUILD.bazel index 76758e8720d7..1852acc35a4b 100644 --- a/geometry/BUILD.bazel +++ b/geometry/BUILD.bazel @@ -85,12 +85,23 @@ drake_cc_library( hdrs = ["geometry_instance.h"], deps = [ ":geometry_ids", + ":materials", ":shape_specification", "//common:copyable_unique_ptr", "//common:essential", ], ) +drake_cc_library( + # NOTE: The material library and file name don't appear to match. This + # library will eventually contain multiple types of materials (e.g., + # contact, rendering, depth, etc.) + name = "materials", + srcs = ["visual_material.cc"], + hdrs = ["visual_material.h"], + deps = ["//common"], +) + drake_cc_library( name = "geometry_state", srcs = ["geometry_state.cc"], @@ -158,6 +169,7 @@ drake_cc_library( deps = [ ":geometry_ids", ":geometry_index", + ":materials", ":shape_specification", "//common:copyable_unique_ptr", "//common:essential", @@ -234,6 +246,14 @@ drake_cc_googletest( ], ) +drake_cc_googletest( + name = "geometry_visualization_test", + deps = [ + ":geometry_visualization", + "//common/test_utilities:expect_throws_message", + ], +) + drake_cc_googletest( name = "identifier_test", deps = [":identifier"], @@ -255,4 +275,11 @@ drake_cc_googletest( ], ) +drake_cc_googletest( + name = "visual_material_test", + deps = [ + ":materials", + ], +) + add_lint_tests() diff --git a/geometry/geometry_instance.cc b/geometry/geometry_instance.cc index 56b7363d88cd..db1653862b32 100644 --- a/geometry/geometry_instance.cc +++ b/geometry/geometry_instance.cc @@ -7,7 +7,15 @@ namespace geometry { GeometryInstance::GeometryInstance(const Isometry3& X_PG, std::unique_ptr shape) - : id_(GeometryId::get_new_id()), X_PG_(X_PG), shape_(std::move(shape)) {} + : GeometryInstance(X_PG, std::move(shape), VisualMaterial()) {} + +GeometryInstance::GeometryInstance(const Isometry3& X_PG, + std::unique_ptr shape, + const VisualMaterial& vis_material) + : id_(GeometryId::get_new_id()), + X_PG_(X_PG), + shape_(std::move(shape)), + visual_material_(vis_material) {} } // namespace geometry } // namespace drake diff --git a/geometry/geometry_instance.h b/geometry/geometry_instance.h index f5abf8d7d522..6214854e22a5 100644 --- a/geometry/geometry_instance.h +++ b/geometry/geometry_instance.h @@ -8,6 +8,7 @@ #include "drake/common/eigen_types.h" #include "drake/geometry/geometry_ids.h" #include "drake/geometry/shape_specification.h" +#include "drake/geometry/visual_material.h" namespace drake { namespace geometry { @@ -20,11 +21,19 @@ class GeometryInstance { public: DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(GeometryInstance) - /** Constructor. + /** Constructor with default visual material (see VisualMaterial default + constructor for details on what that color is). @param X_PG The pose of this geometry (`G`) in its parent's frame (`P`). @param shape The underlying shape for this geometry instance. */ GeometryInstance(const Isometry3& X_PG, std::unique_ptr shape); + /** Constructor. + @param X_PG The pose of this geometry (`G`) in its parent's frame (`P`). + @param shape The underlying shape for this geometry instance. + @param vis_material The visual material to apply to this geometry. */ + GeometryInstance(const Isometry3& X_PG, std::unique_ptr shape, + const VisualMaterial& vis_material); + /** Returns the globally unique id for this geometry specification. Every instantiation of %GeometryInstance will contain a unique id value. The id value is preserved across copies. After successfully registering this @@ -43,6 +52,8 @@ class GeometryInstance { /** Releases the shape from the instance. */ std::unique_ptr release_shape() { return std::move(shape_); } + const VisualMaterial& visual_material() const { return visual_material_; } + private: // The *globally* unique identifier for this instance. It is functionally // const (i.e. defined in construction) but not marked const to allow for @@ -54,6 +65,9 @@ class GeometryInstance { // The shape associated with this instance. copyable_unique_ptr shape_; + + // The "rendering" material -- e.g., OpenGl contexts and the like. + VisualMaterial visual_material_; }; } // namespace geometry } // namespace drake diff --git a/geometry/geometry_state.cc b/geometry/geometry_state.cc index 8635d2a6f280..19b38dde911a 100644 --- a/geometry/geometry_state.cc +++ b/geometry/geometry_state.cc @@ -268,7 +268,8 @@ GeometryId GeometryState::RegisterGeometry( geometries_.emplace( geometry_id, InternalGeometry(geometry->release_shape(), frame_id, geometry_id, - geometry->pose(), engine_index)); + geometry->pose(), engine_index, + geometry->visual_material())); // TODO(SeanCurtis-TRI): Enforcing the invariant that the indexes are // compactly distributed. Is there a more robust way to do this? DRAKE_ASSERT(static_cast(X_FG_.size()) == engine_index); @@ -354,8 +355,9 @@ GeometryId GeometryState::RegisterAnchoredGeometry( anchored_geometry_index_id_map_.push_back(geometry_id); anchored_geometries_.emplace( geometry_id, - InternalAnchoredGeometry(geometry->release_shape(), geometry_id, - geometry->pose(), engine_index)); + InternalAnchoredGeometry( + geometry->release_shape(), geometry_id, geometry->pose(), + engine_index, geometry->visual_material())); return geometry_id; } diff --git a/geometry/geometry_state.h b/geometry/geometry_state.h index 3bda075f0d27..7451c517a83a 100644 --- a/geometry/geometry_state.h +++ b/geometry/geometry_state.h @@ -19,11 +19,14 @@ #include "drake/geometry/proximity_engine.h" namespace drake { + namespace geometry { #ifndef DRAKE_DOXYGEN_CXX namespace internal { +class GeometryVisualizationImpl; + // A const range iterator through the keys of an unordered map. template class MapKeyRange { @@ -429,8 +432,10 @@ class GeometryState { convert(source.X_WF_, &X_WF_); } - // Allow geometry dispatch to peek into GeometryState. - friend void DispatchLoadMessage(const GeometryState&); + // NOTE: This friend class is responsible for evaluating the internals of + // a GeometryState and translating it into the appropriate visualization + // mechanism. + friend class internal::GeometryVisualizationImpl; // Allow GeometrySystem unique access to the state members to perform queries. friend class GeometrySystem; diff --git a/geometry/geometry_visualization.cc b/geometry/geometry_visualization.cc index 9cf1d55bb2a2..0647d6d966b0 100644 --- a/geometry/geometry_visualization.cc +++ b/geometry/geometry_visualization.cc @@ -10,7 +10,6 @@ #include "drake/geometry/shape_specification.h" #include "drake/lcm/drake_lcm.h" #include "drake/lcmt_viewer_geometry_data.hpp" -#include "drake/lcmt_viewer_load_robot.hpp" #include "drake/math/rotation_matrix.h" namespace drake { @@ -112,10 +111,12 @@ lcmt_viewer_geometry_data MakeGeometryData(const Shape& shape, } // namespace -void DispatchLoadMessage(const GeometryState& state) { +namespace internal { + +lcmt_viewer_load_robot GeometryVisualizationImpl::BuildLoadMessage( + const GeometryState& state) { using internal::InternalAnchoredGeometry; using internal::InternalGeometry; - using lcm::DrakeLcm; lcmt_viewer_load_robot message{}; // Populate the message. @@ -129,8 +130,6 @@ void DispatchLoadMessage(const GeometryState& state) { message.num_links = total_link_count; message.link.resize(total_link_count); - Eigen::Vector4d default_color(0.8, 0.8, 0.8, 1.0); - int link_index = 0; // Load anchored geometry into the world frame. { @@ -143,8 +142,10 @@ void DispatchLoadMessage(const GeometryState& state) { for (const auto& pair : state.anchored_geometries_) { const InternalAnchoredGeometry& geometry = pair.second; const Shape& shape = geometry.get_shape(); + const Eigen::Vector4d& color = + geometry.get_visual_material().diffuse(); message.link[0].geom[geom_index] = MakeGeometryData( - shape, geometry.get_pose_in_parent(), default_color); + shape, geometry.get_pose_in_parent(), color); ++geom_index; } link_index = 1; @@ -168,16 +169,28 @@ void DispatchLoadMessage(const GeometryState& state) { int geom_index = 0; for (GeometryId geom_id : frame.get_child_geometries()) { const InternalGeometry& geometry = state.geometries_.at(geom_id); - GeometryIndex index = state.geometries_.at(geom_id).get_engine_index(); - const Shape& shape = geometry.get_shape(); + GeometryIndex index = geometry.get_engine_index(); const Isometry3 X_FG = state.X_FG_.at(index); + const Shape& shape = geometry.get_shape(); + const Eigen::Vector4d& color = + geometry.get_visual_material().diffuse(); message.link[link_index].geom[geom_index] = - MakeGeometryData(shape, X_FG, default_color); + MakeGeometryData(shape, X_FG, color); ++geom_index; } ++link_index; } + return message; +} + +} // namespace internal + +void DispatchLoadMessage(const GeometryState& state) { + using lcm::DrakeLcm; + + lcmt_viewer_load_robot message = + internal::GeometryVisualizationImpl::BuildLoadMessage(state); // Send a load message. DrakeLcm lcm; Publish(&lcm, "DRAKE_VIEWER_LOAD_ROBOT", message); diff --git a/geometry/geometry_visualization.h b/geometry/geometry_visualization.h index 4cd2c13831cd..b6e9b76197ff 100644 --- a/geometry/geometry_visualization.h +++ b/geometry/geometry_visualization.h @@ -1,13 +1,31 @@ -#pragma once - /** @file Provides a set of functions to facilitate visualization operations based on geometry world state. */ +#pragma once + +#include "drake/geometry/geometry_state.h" +#include "drake/geometry/geometry_system.h" +#include "drake/lcmt_viewer_load_robot.hpp" + namespace drake { namespace geometry { -template class GeometrySystem; +#ifndef DRAKE_DOXYGEN_CXX +namespace internal { + +// Simple class declared as a friend to GeometryState to facilitate the creation +// of visualization artifacts directly from the contents of GeometryState. +class GeometryVisualizationImpl { + public: + // Given an instance of GeometryState, returns an lcm message sufficient + // to load the state's geometry. + static lcmt_viewer_load_robot BuildLoadMessage( + const GeometryState& state); +}; + +} // namespace internal +#endif // DRAKE_DOXYGEN_CXX /** Dispatches an LCM load message based on the registered geometry. It should be invoked _after_ registration is complete, but before context allocation. diff --git a/geometry/internal_geometry.cc b/geometry/internal_geometry.cc index 5354182155c3..1871816578ea 100644 --- a/geometry/internal_geometry.cc +++ b/geometry/internal_geometry.cc @@ -11,7 +11,16 @@ InternalGeometry::InternalGeometry(std::unique_ptr shape, const Isometry3& X_PG, GeometryIndex engine_index, const optional& parent_id) - : InternalGeometryBase(std::move(shape), geometry_id, X_PG), + : InternalGeometry(std::move(shape), frame_id, geometry_id, X_PG, + engine_index, VisualMaterial(), parent_id) {} + +InternalGeometry::InternalGeometry(std::unique_ptr shape, + FrameId frame_id, GeometryId geometry_id, + const Isometry3& X_PG, + GeometryIndex engine_index, + const VisualMaterial& vis_material, + const optional& parent_id) + : InternalGeometryBase(std::move(shape), geometry_id, X_PG, vis_material), frame_id_(frame_id), engine_index_(engine_index), parent_id_(parent_id) {} @@ -21,7 +30,14 @@ InternalAnchoredGeometry::InternalAnchoredGeometry() : InternalGeometryBase() {} InternalAnchoredGeometry::InternalAnchoredGeometry( std::unique_ptr shape, GeometryId geometry_id, const Isometry3& X_WG, AnchoredGeometryIndex engine_index) - : InternalGeometryBase(std::move(shape), geometry_id, X_WG), + : InternalAnchoredGeometry(std::move(shape), geometry_id, X_WG, + engine_index, VisualMaterial()) {} + +InternalAnchoredGeometry::InternalAnchoredGeometry( + std::unique_ptr shape, GeometryId geometry_id, + const Isometry3& X_WG, AnchoredGeometryIndex engine_index, + const VisualMaterial& vis_material) + : InternalGeometryBase(std::move(shape), geometry_id, X_WG, vis_material), engine_index_(engine_index) {} } // namespace internal diff --git a/geometry/internal_geometry.h b/geometry/internal_geometry.h index bb79b9966bc4..ae02e5eda998 100644 --- a/geometry/internal_geometry.h +++ b/geometry/internal_geometry.h @@ -11,6 +11,7 @@ #include "drake/geometry/geometry_ids.h" #include "drake/geometry/geometry_index.h" #include "drake/geometry/shape_specification.h" +#include "drake/geometry/visual_material.h" namespace drake { namespace geometry { @@ -28,15 +29,33 @@ class InternalGeometryBase { be nullptr, and the pose will be uninitialized. */ InternalGeometryBase() {} - /** Full constructor. + // TODO(SeanCurtis-TRI): Resolve the issue inherent in having a copyable class + // (with subclasses) that has no virtual destructor. In its current + // incarnation this is *not* used polymorphically; it is used for code + // re-use purposes. This should be made more rigorous. + + /** Default material, full constructor. @param shape The shape specification for this instance. @param geometry_id The identifier for _this_ geometry. @param X_PG The pose of the geometry G in the parent frame P. */ InternalGeometryBase(std::unique_ptr shape, GeometryId geometry_id, const Isometry3& X_PG) + : InternalGeometryBase(std::move(shape), geometry_id, X_PG, + VisualMaterial()) {} + + /** Full constructor. + @param shape The shape specification for this instance. + @param geometry_id The identifier for _this_ geometry. + @param X_PG The pose of the geometry G in the parent frame P. + @param engine_index The position in the geometry engine of this geometry. + @param vis_material The visual material for this geometry. */ + InternalGeometryBase(std::unique_ptr shape, GeometryId geometry_id, + const Isometry3& X_PG, + const VisualMaterial& vis_material) : shape_spec_(std::move(shape)), id_(geometry_id), - X_PG_(X_PG) {} + X_PG_(X_PG), + visual_material_(vis_material) {} /** Compares two %InternalGeometryBase instances for "equality". Two internal geometries are considered equal if they have the same geometry identifier. */ @@ -51,9 +70,13 @@ class InternalGeometryBase { } const Shape& get_shape() const { return *shape_spec_; } + GeometryId get_id() const { return id_; } + const Isometry3& get_pose_in_parent() const { return X_PG_; } + const VisualMaterial& get_visual_material() const { return visual_material_; } + private: // The specification for this instance's shape. copyable_unique_ptr shape_spec_; @@ -64,6 +87,11 @@ class InternalGeometryBase { // The pose of this geometry in the parent frame. The parent may be a frame or // another registered geometry. Isometry3 X_PG_; + + // TODO(SeanCurtis-TRI): Consider making this "optional" so that the values + // can be assigned at the frame level. + // The "rendering" material -- e.g., OpenGl contexts and the like. + VisualMaterial visual_material_; }; /** This class represents the internal representation of registered _dynamic_ @@ -77,6 +105,19 @@ class InternalGeometry : public InternalGeometryBase { the state documented in InternalGeometryBase(). */ InternalGeometry(); + /** Default material, full constructor. + @param shape The shape specification for this instance. + @param frame_id The identifier of the frame this belongs to. + @param geometry_id The identifier for _this_ geometry. + @param X_PG The pose of the geometry G in the parent frame P. The + parent may be a frame, or another registered geometry. + @param engine_index The position in the geometry engine of this geometry. + @param parent_id The optional id of the parent geometry. */ + InternalGeometry(std::unique_ptr shape, FrameId frame_id, + GeometryId geometry_id, + const Isometry3& X_PG, GeometryIndex engine_index, + const optional& parent_id = {}); + /** Full constructor. @param shape The shape specification for this instance. @param frame_id The identifier of the frame this belongs to. @@ -84,16 +125,22 @@ class InternalGeometry : public InternalGeometryBase { @param X_PG The pose of the geometry G in the parent frame P. The parent may be a frame, or another registered geometry. @param engine_index The position in the geometry engine of this geometry. + @param vis_material The visual material for this geometry. @param parent_id The optional id of the parent geometry. */ InternalGeometry(std::unique_ptr shape, FrameId frame_id, - GeometryId geometry_id, const Isometry3& X_PG, - GeometryIndex engine_index, + GeometryId geometry_id, + const Isometry3& X_PG, GeometryIndex engine_index, + const VisualMaterial& vis_material, const optional& parent_id = {}); FrameId get_frame_id() const { return frame_id_; } + optional get_parent_id() const { return parent_id_; } + void set_parent_id(GeometryId id) { parent_id_ = id; } + GeometryIndex get_engine_index() const { return engine_index_; } + void set_engine_index(GeometryIndex index) { engine_index_ = index; } /** Returns true if this geometry has a geometry parent and the parent has the @@ -158,7 +205,7 @@ class InternalAnchoredGeometry : public InternalGeometryBase { InternalGeometryBase(). */ InternalAnchoredGeometry(); - /** Full constructor. + /** Default material, full constructor. @param shape The shape specification for this instance. @param geometry_id The identifier for _this_ geometry. @param X_WG The pose of the geometry G in the world frame W. @@ -167,7 +214,19 @@ class InternalAnchoredGeometry : public InternalGeometryBase { const Isometry3& X_WG, AnchoredGeometryIndex engine_index); + /** Full constructor. + @param shape The shape specification for this instance. + @param geometry_id The identifier for _this_ geometry. + @param X_WG The pose of the geometry G in the world frame W. + @param engine_index The position in the geometry engine of this geometry. + @param vis_material The visual material for this geometry. */ + InternalAnchoredGeometry(std::unique_ptr shape, GeometryId geometry_id, + const Isometry3& X_WG, + AnchoredGeometryIndex engine_index, + const VisualMaterial& vis_material); + AnchoredGeometryIndex get_engine_index() const { return engine_index_; } + void set_engine_index(AnchoredGeometryIndex index) { engine_index_ = index; } private: diff --git a/geometry/test/geometry_visualization_test.cc b/geometry/test/geometry_visualization_test.cc new file mode 100644 index 000000000000..9ecbdbdafac7 --- /dev/null +++ b/geometry/test/geometry_visualization_test.cc @@ -0,0 +1,80 @@ +#include "drake/geometry/geometry_visualization.h" + +#include + +#include + +#include "drake/geometry/geometry_context.h" +#include "drake/geometry/geometry_frame.h" +#include "drake/geometry/geometry_ids.h" +#include "drake/geometry/geometry_instance.h" +#include "drake/geometry/geometry_system.h" +#include "drake/lcmt_viewer_geometry_data.hpp" +#include "drake/lcmt_viewer_load_robot.hpp" +#include "drake/systems/framework/context.h" + +namespace drake { +namespace geometry { +namespace { + +using drake::geometry::internal::GeometryVisualizationImpl; +using drake::systems::Context; +using Eigen::Isometry3d; +using Eigen::Vector4d; +using std::make_unique; +using std::unique_ptr; + +// TODO(SeanCurtis-TRI): Test with more complex scenarios. + +// Confirms that, for a simple scene, the message contains the right data. +GTEST_TEST(GeometryVisualization, SimpleScene) { + GeometrySystem system; + + const std::string source_name("source"); + SourceId source_id = system.RegisterSource(source_name); + + const std::string frame_name("frame"); + FrameId frame_id = system.RegisterFrame( + source_id, GeometryFrame(frame_name, Isometry3d::Identity())); + + // Floats because the lcm messages store floats. + const float radius = 1.25f; + const float r = 1.0; + const float g = 0.5f; + const float b = 0.25f; + const float a = 0.125f; + system.RegisterGeometry( + source_id, frame_id, + make_unique(Isometry3d::Identity(), + make_unique(radius), + VisualMaterial(Vector4d{r, g, b, a}))); + + unique_ptr> context = system.AllocateContext(); + const GeometryContext& geo_context = + dynamic_cast&>(*context.get()); + + lcmt_viewer_load_robot message = GeometryVisualizationImpl::BuildLoadMessage( + geo_context.get_geometry_state()); + + ASSERT_EQ(message.num_links, 1); + const lcmt_viewer_link_data& link = message.link[0]; + EXPECT_EQ(link.name, source_name + "::" + frame_name); + EXPECT_EQ(link.robot_num, 0); + ASSERT_EQ(link.num_geom, 1); + const lcmt_viewer_geometry_data& geometry = link.geom[0]; + // This is necessary because EXPECT_EQ takes a reference to the arguments and + // lcmt_viewer_geometry_data::SPHERE is a static constexpr defined inline and + // doesn't support odr usage. + const int8_t sphere_type = lcmt_viewer_geometry_data::SPHERE; + EXPECT_EQ(geometry.type, sphere_type); + EXPECT_EQ(geometry.num_float_data, 1); + EXPECT_EQ(geometry.float_data[0], radius); + EXPECT_EQ(geometry.color[0], r); + EXPECT_EQ(geometry.color[1], g); + EXPECT_EQ(geometry.color[2], b); + EXPECT_EQ(geometry.color[3], a); +} + +} // namespace +} // namespace geometry +} // namespace drake diff --git a/geometry/test/visual_material_test.cc b/geometry/test/visual_material_test.cc new file mode 100644 index 000000000000..ab865a3de34a --- /dev/null +++ b/geometry/test/visual_material_test.cc @@ -0,0 +1,31 @@ +#include "drake/geometry/visual_material.h" + +#include + +namespace drake { +namespace geometry { +namespace { + +GTEST_TEST(VisualMaterial, DefaultConstructor) { + // Documented as "light grey". + VisualMaterial material; + const Eigen::Vector4d& diffuse = material.diffuse(); + // Confirm grey -- r = g = b (by transitivity). + EXPECT_EQ(diffuse(0), diffuse(1)); + EXPECT_EQ(diffuse(1), diffuse(2)); + // Confirm "light" + EXPECT_GT(diffuse(0), 0.5); +} + +GTEST_TEST(VisualMaterial, FullConstructor) { + Eigen::Vector4d diffuse_in{0.1, 0.2, 0.3, 0.4}; + VisualMaterial material{diffuse_in}; + const Eigen::Vector4d& diffuse = material.diffuse(); + + for (int i = 0; i < 4; ++i) + EXPECT_EQ(diffuse(i), diffuse_in(i)); +} + +} // namespace +} // namespace geometry +} // namespace drake diff --git a/geometry/visual_material.cc b/geometry/visual_material.cc new file mode 100644 index 000000000000..7dcf0f8ec972 --- /dev/null +++ b/geometry/visual_material.cc @@ -0,0 +1,12 @@ +#include "drake/geometry/visual_material.h" + +namespace drake { +namespace geometry { + +VisualMaterial::VisualMaterial() {} + +VisualMaterial::VisualMaterial(const Eigen::Vector4d& diffuse) + : diffuse_(diffuse) {} + +} // namespace geometry +} // namespace drake diff --git a/geometry/visual_material.h b/geometry/visual_material.h new file mode 100644 index 000000000000..c253b4ef658a --- /dev/null +++ b/geometry/visual_material.h @@ -0,0 +1,32 @@ +#pragma once + +#include + +#include "drake/common/drake_copyable.h" + +namespace drake { +namespace geometry { + +/** Definition of material for simple visualization. Default materials are a + light grey. */ +class VisualMaterial final { + public: + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(VisualMaterial) + + /** Constructs a material with the default grey color. */ + VisualMaterial(); + + /** Constructs a material with the given diffuse color. */ + explicit VisualMaterial(const Eigen::Vector4d& diffuse); + + /** Returns the material's diffuse color. */ + const Eigen::Vector4d& diffuse() const { return diffuse_; } + + private: + // Default light grey color -- if this default changes, update the class + // documentation. + Eigen::Vector4d diffuse_{0.9, 0.9, 0.9, 1.0}; +}; + +} // namespace geometry +} // namespace drake diff --git a/multibody/rigid_body_plant/BUILD.bazel b/multibody/rigid_body_plant/BUILD.bazel index dcd69e067abb..3d4fd3346ef2 100644 --- a/multibody/rigid_body_plant/BUILD.bazel +++ b/multibody/rigid_body_plant/BUILD.bazel @@ -84,6 +84,7 @@ drake_cc_library( deps = [ "//geometry:geometry_ids", "//geometry:geometry_system", + "//geometry:materials", "//multibody:rigid_body_tree", ], ) diff --git a/multibody/rigid_body_plant/rigid_body_plant_bridge.cc b/multibody/rigid_body_plant/rigid_body_plant_bridge.cc index 7e0678baf548..ee636698a6ca 100644 --- a/multibody/rigid_body_plant/rigid_body_plant_bridge.cc +++ b/multibody/rigid_body_plant/rigid_body_plant_bridge.cc @@ -6,6 +6,7 @@ #include "drake/common/drake_assert.h" #include "drake/geometry/geometry_frame.h" #include "drake/geometry/geometry_instance.h" +#include "drake/geometry/visual_material.h" #include "drake/systems/framework/basic_vector.h" #include "drake/systems/framework/framework_common.h" @@ -22,6 +23,7 @@ using geometry::GeometrySystem; using geometry::Mesh; using geometry::Shape; using geometry::Sphere; +using geometry::VisualMaterial; template RigidBodyPlantBridge::RigidBodyPlantBridge( @@ -120,10 +122,13 @@ void RigidBodyPlantBridge::RegisterTree(GeometrySystem* geometry_system) { "are supported by RigidBodyPlantBridge"); } if (shape) { + // Visual element's "material" is simply the diffuse rgba values. + const Vector4& diffuse = visual_element.getMaterial(); geometry_system->RegisterGeometry( source_id_, body_id, std::make_unique( - X_FG, std::move(shape))); + X_FG, std::move(shape), + VisualMaterial(diffuse))); DRAKE_DEMAND(shape == nullptr); } }