From e3074ba05cdb4badd1be23292558f6f99ee46672 Mon Sep 17 00:00:00 2001 From: Sean Curtis Date: Tue, 28 May 2019 10:19:58 -0700 Subject: [PATCH] Clean up pass on geometry state tests This addresses a number of style and cosmetic issues encountered while integrating rendering into geometry state. This isolates the legacy issues so that new code can be considered without this noise in the signal. This doesn't change the *functionality* of the tests. It only changes spellings and the like: 1. Use of aliases to make code more compact. 2. Added const to tons of local variables. 3. Refactored the GeometryStateTest core into a new calss: GeometryStateTestBase (this will facilitate new tests in upcoming PRs). - Change the interface to SetUpSingleSourceTree() to make call sites more readable. - Remove to methods that are no longer used: - ExpectSourceDoesNotHaveFrames() - AssertSingleTreeCleared() - Rename vectors of poses: e.g., X_WF_ --> X_WFs_. 4. Make more fluent use of pose data (will lend itself to translation to RigidTransform). 5. Clarify, clean up comments. 6. Move ProximityRoleOnMesh to bottom. --- geometry/test/geometry_state_test.cc | 665 ++++++++++++++------------- 1 file changed, 342 insertions(+), 323 deletions(-) diff --git a/geometry/test/geometry_state_test.cc b/geometry/test/geometry_state_test.cc index ebbdd06a4a6e..71857871bb54 100644 --- a/geometry/test/geometry_state_test.cc +++ b/geometry/test/geometry_state_test.cc @@ -22,6 +22,18 @@ namespace drake { namespace geometry { +using Eigen::Isometry3d; +using Eigen::Translation3d; +using internal::InternalFrame; +using internal::InternalGeometry; +using std::make_unique; +using std::move; +using std::pair; +using std::set; +using std::string; +using std::unique_ptr; +using std::unordered_map; +using std::unordered_set; using std::vector; // Implementation of friend class that allows me to peek into the geometry state @@ -33,40 +45,34 @@ class GeometryStateTester { public: void set_state(GeometryState* state) { state_ = state; } - FrameId get_world_frame() const { - return internal::InternalFrame::world_frame_id(); - } + FrameId get_world_frame() const { return InternalFrame::world_frame_id(); } SourceId get_self_source_id() const { return state_->self_source_; } - const std::unordered_map& get_source_name_map() const { + const unordered_map& get_source_name_map() const { return state_->source_names_; } - const std::unordered_map& get_source_frame_id_map() - const { + const unordered_map& get_source_frame_id_map() const { return state_->source_frame_id_map_; } - const std::unordered_map& get_source_root_frame_map() - const { + const unordered_map& get_source_root_frame_map() const { return state_->source_root_frame_map_; } - const std::unordered_map>& + const unordered_map>& get_source_anchored_geometry_map() const { return state_->source_anchored_geometry_map_; } - const std::unordered_map& get_frames() - const { + const unordered_map& get_frames() const { return state_->frames_; } - const std::unordered_map& - get_geometries() const { + const unordered_map& get_geometries() const { return state_->geometries_; } @@ -112,7 +118,7 @@ class GeometryStateTester { return state_->peek_next_clique(); } - const internal::InternalGeometry* GetGeometry(GeometryId id) { + const InternalGeometry* GetGeometry(GeometryId id) const { return state_->GetGeometry(id); } @@ -122,13 +128,6 @@ class GeometryStateTester { namespace { -using Eigen::Isometry3d; -using internal::InternalFrame; -using internal::InternalGeometry; -using std::make_unique; -using std::move; -using std::unique_ptr; - // Class to aid in testing Shape introspection. Instantiated with a model // Shape instance, it registers a copy of that shape and confirms that the // introspected Shape matches in type and parameters. @@ -291,9 +290,31 @@ void ShapeMatcher::TestShapeParameters(const Convex& test) { } } -class GeometryStateTest : public ::testing::Test { - protected: - void SetUp() { +// A mask-style enumeration for indicating which of a set of roles should be +// assigned to the newly configured geometry. +enum class Assign { + kNone = 0, + kProximity = 1, + kIllustration = 2 +}; + +Assign operator&(Assign a, Assign b) { + return static_cast(static_cast(a) & static_cast(b)); +} + +// TODO(SeanCurtis-TRI): Add operator| when I start masking them together. + +// The fundamental base class for the geometry state tests; it provides +// utilities for configuring an owned geometry state. This class allows us to +// create arbitrary test harnesses using the same infrastructure. (See +// GeometryStateTest and RemoveRoleTests. +class GeometryStateTestBase { + public: + GeometryStateTestBase() = default; + + // The initialization to be done prior to each test; the base test class + // should invoke this in its SetUp() method. + void TestInit() { frame_ = make_unique("ref_frame"); instance_pose_.translation() << 10, 20, 30; instance_ = make_unique( @@ -302,7 +323,7 @@ class GeometryStateTest : public ::testing::Test { } // Utility method for adding a source to the state. - SourceId NewSource(const std::string& name = "") { + SourceId NewSource(const string& name = "") { return geometry_state_.RegisterNewSource(name == "" ? kSourceName : name); } @@ -347,34 +368,38 @@ class GeometryStateTest : public ::testing::Test { // Although the sibling geometries affixed to each frame overlap, the pairs // (g0, g1), (g2, g3), and (g4, g5) are implicitly filtered because they are // sibling geometries affixed to the same frame. - SourceId SetUpSingleSourceTree(bool assign_proximity_role = false) { + // + // By default, no roles are assigned to the geometries. However, roles can + // be assigned to *all* registered geometries by indicating the type of role + // to assign (via `roles_to_assign`). + SourceId SetUpSingleSourceTree(Assign roles_to_assign = Assign::kNone) { using std::to_string; source_id_ = NewSource(); // Create f0. - Isometry3 pose = Isometry3::Identity(); + Isometry3d pose = Isometry3d::Identity(); pose.translation() << 1, 2, 3; pose.linear() << 1, 0, 0, 0, 0, 1, 0, -1, 0; // 90° around x-axis. frames_.push_back(geometry_state_.RegisterFrame( source_id_, GeometryFrame("f0"))); - X_WF_.push_back(pose); - X_PF_.push_back(pose); + X_WFs_.push_back(pose); + X_PFs_.push_back(pose); // Create f1. pose.translation() << 10, 20, 30; pose.linear() << 0, 0, -1, 0, 1, 0, 1, 0, 0; // 90° around y-axis. frames_.push_back(geometry_state_.RegisterFrame( source_id_, GeometryFrame("f1"))); - X_WF_.push_back(pose); - X_PF_.push_back(pose); + X_WFs_.push_back(pose); + X_PFs_.push_back(pose); // Create f2. pose = pose.inverse(); frames_.push_back(geometry_state_.RegisterFrame( source_id_, frames_[1], GeometryFrame("f2"))); - X_WF_.push_back(X_WF_[1] * pose); - X_PF_.push_back(pose); + X_WFs_.push_back(X_WFs_[1] * pose); + X_PFs_.push_back(pose); // Add geometries to each frame. const Vector3 x_axis(1, 0, 0); @@ -387,34 +412,34 @@ class GeometryStateTest : public ::testing::Test { pose.linear() = AngleAxis(g_count * M_PI / 2.0, x_axis).matrix(); // Have the name reflect the frame and the index in the geometry. - const std::string& name = - to_string(frame_id) + "_g" + std::to_string(i); + const string& name = + to_string(frame_id) + "_g" + to_string(i); geometry_names_[g_count] = name; geometries_[g_count] = geometry_state_.RegisterGeometry( source_id_, frame_id, make_unique(pose, make_unique(1), name)); - if (assign_proximity_role) { - geometry_state_.AssignRole(source_id_, geometries_[g_count], - ProximityProperties()); - } - X_FG_.push_back(pose); + X_FGs_.push_back(pose); ++g_count; } } // Create anchored geometry. - X_WA_ = Isometry3::Identity(); - X_WA_.translation() << 0, 0, -1; + X_WA_ = Isometry3d{Translation3d{0, 0, -1}}; // This simultaneously tests the named registration function and // _implicitly_ tests registration of geometry against the world frame id // (as that is how `RegisterAnchoredGeometry()` works. anchored_geometry_ = geometry_state_.RegisterAnchoredGeometry( source_id_, make_unique( - X_WA_, make_unique(100, 100, 2), anchored_name_)); - if (assign_proximity_role) { - geometry_state_.AssignRole( - source_id_, anchored_geometry_, ProximityProperties()); + X_WA_, make_unique(100, 100, 2), anchored_name_)); + + if ((roles_to_assign & Assign::kProximity) != Assign::kNone) { + AssignProximityToSingleSourceTree(); + } + + if ((roles_to_assign & Assign::kIllustration) != Assign::kNone) { + AssignIllustrationToSingleSourceTree(); } + return source_id_; } @@ -440,53 +465,10 @@ class GeometryStateTest : public ::testing::Test { return 2; } - // This method confirms that the stored dummy identifiers don't map to any - // registered source identifier. This should only be invoked for scenarios - // where there is *only* the single source. - void AssertSingleTreeCleared() { - // Confirms frames have been cleared. - for (int f = 0; f < kFrameCount; ++f) { - DRAKE_EXPECT_THROWS_MESSAGE( - geometry_state_.BelongsToSource(frames_[f], source_id_), - std::logic_error, "Referenced frame \\d+ has not been registered."); - } - // Confirms geometries have been cleared. - for (int g = 0; g < kFrameCount * kGeometryCount; ++g) { - DRAKE_EXPECT_THROWS_MESSAGE( - geometry_state_.BelongsToSource(geometries_[g], source_id_), - std::logic_error, - "Referenced geometry \\d+ has not been registered."); - } - EXPECT_EQ(gs_tester_.get_source_frame_id_map().at(source_id_).size(), 0); - EXPECT_EQ(gs_tester_.get_source_frame_id_map().size(), 1); - EXPECT_EQ(gs_tester_.get_source_root_frame_map().at(source_id_).size(), 0); - EXPECT_EQ(gs_tester_.get_source_root_frame_map().size(), 1); - EXPECT_EQ(gs_tester_.get_frames().size(), 0); - EXPECT_EQ(gs_tester_.get_geometries().size(), 0); - EXPECT_EQ(gs_tester_.get_frame_parent_poses().size(), 0); - EXPECT_EQ(gs_tester_.get_geometry_world_poses().size(), 0); - } - - // Utility function for facilitating tests; confirms that the identified - // frame doesn't belong to the identified source. This explicitly tests the - // underlying state data structures. - void ExpectSourceDoesNotHaveFrame(SourceId source_id, FrameId frame_id) { - // Frame is not in the source-to-set-of-frame-ids mapping. - EXPECT_EQ(gs_tester_.get_source_frame_id_map().at(source_id).find(frame_id), - gs_tester_.get_source_frame_id_map().at(source_id).end()); - // Frame is not in the source-to-set-of-root-ids mapping. - EXPECT_EQ( - gs_tester_.get_source_root_frame_map().at(source_id).find(frame_id), - gs_tester_.get_source_root_frame_map().at(source_id).end()); - // Frame not in frames - EXPECT_EQ(gs_tester_.get_frames().find(frame_id), - gs_tester_.get_frames().end()); - } - // Members owned by the test class. unique_ptr frame_; unique_ptr instance_; - Isometry3 instance_pose_{Isometry3::Identity()}; + Isometry3d instance_pose_{Isometry3d::Identity()}; GeometryState geometry_state_; GeometryStateTester gs_tester_; @@ -500,24 +482,59 @@ class GeometryStateTest : public ::testing::Test { // The geometry ids created in the dummy tree instantiation. vector geometries_; // The names for all the geometries (as registered). - vector geometry_names_; + vector geometry_names_; // The single, anchored geometry id. GeometryId anchored_geometry_; // The registered name of the anchored geometry. - const std::string anchored_name_{"anchored"}; + const string anchored_name_{"anchored"}; // The id of the single-source tree. SourceId source_id_; // The poses of the frames in the world frame. - vector> X_WF_; + vector X_WFs_; // The poses of the frames in the parent's frame. - vector> X_PF_; + vector X_PFs_; // The poses of the dynamic geometries in the parent frame. - vector> X_FG_; + vector X_FGs_; // The pose of the anchored geometry in the world frame. - Isometry3 X_WA_; + Isometry3d X_WA_; // The default source name. - const std::string kSourceName{"default_source"}; + const string kSourceName{"default_source"}; + + private: + // Convenience method for assigning illustration properties to all geometries + // in the single source tree. + void AssignProximityToSingleSourceTree() { + ASSERT_TRUE(source_id_.is_valid()); + ProximityProperties properties; + AssignRoleToSingleSourceTree(properties); + } + + // Convenience method for assigning illustration properties to all geometries + // in the single source tree. + void AssignIllustrationToSingleSourceTree() { + ASSERT_TRUE(source_id_.is_valid()); + IllustrationProperties properties; + properties.AddProperty("phong", "diffuse", + Vector4{0.8, 0.8, 0.8, 1.0}); + AssignRoleToSingleSourceTree(properties); + } + + template + void AssignRoleToSingleSourceTree(const PropertyType& properties) { + ASSERT_TRUE(source_id_.is_valid()); + for (GeometryId id : geometries_) { + geometry_state_.AssignRole(source_id_, id, properties); + } + geometry_state_.AssignRole(source_id_, anchored_geometry_, properties); + } +}; + +class GeometryStateTest : public GeometryStateTestBase, public ::testing::Test { + protected: + void SetUp() override { + TestInit(); + } }; // Confirms that a new GeometryState has no data. @@ -532,8 +549,8 @@ TEST_F(GeometryStateTest, Constructor) { // Confirms that the registered shapes are correctly returned upon // introspection. TEST_F(GeometryStateTest, IntrospectShapes) { - SourceId source_id = geometry_state_.RegisterNewSource("test_source"); - FrameId frame_id = geometry_state_.RegisterFrame( + const SourceId source_id = geometry_state_.RegisterNewSource("test_source"); + const FrameId frame_id = geometry_state_.RegisterFrame( source_id, GeometryFrame("frame")); // Test across all valid shapes. @@ -583,7 +600,7 @@ TEST_F(GeometryStateTest, SourceRegistrationWithNames) { // Case: Successful registration of unique source id and name. SourceId s_id; - std::string name = "Unique"; + const string name = "Unique"; EXPECT_NO_THROW((s_id = geometry_state_.RegisterNewSource(name))); EXPECT_TRUE(geometry_state_.source_is_registered(s_id)); EXPECT_EQ(geometry_state_.get_source_name(s_id), name); @@ -603,7 +620,8 @@ TEST_F(GeometryStateTest, SourceRegistrationWithNames) { // create a state with interesting metrics. Also confirms the "is registered" // -ness of known valid sources and known invalid sources. TEST_F(GeometryStateTest, GeometryStatistics) { - SourceId dummy_source = SetUpSingleSourceTree(); + const SourceId dummy_source = SetUpSingleSourceTree(); + EXPECT_TRUE(geometry_state_.source_is_registered(dummy_source)); // Dummy source + self source. EXPECT_EQ(geometry_state_.get_num_sources(), 2); @@ -622,7 +640,7 @@ TEST_F(GeometryStateTest, GeometryStatistics) { geometry_state_.GetNumFrameGeometries(InternalFrame::world_frame_id())); EXPECT_EQ(geometry_state_.get_num_geometries(), single_tree_total_geometry_count()); - SourceId false_id = SourceId::get_new_id(); + const SourceId false_id = SourceId::get_new_id(); EXPECT_FALSE(geometry_state_.source_is_registered(false_id)); } @@ -669,16 +687,16 @@ void ExpectSuccessfulTransmogrification( EXPECT_EQ(ad_tester.get_dynamic_pose_index_id_map(), d_tester.get_dynamic_pose_index_id_map()); - // 3. Compare Isometry3 with Isometry3 + // 3. Compare Isometry3d with Isometry3 for (GeometryId id : ad_tester.get_geometry_index_id_map()) { EXPECT_TRUE(CompareMatrices( ad_tester.get_geometries().at(id).X_FG().matrix().block<3, 4>(0, 0), d_tester.get_geometries().at(id).X_FG().matrix().block<3, 4>(0, 0))); } - // 4. Compare Isometry3 with Isometry3 - auto test_ad_vs_double = [](const std::vector>& test, - const std::vector>& ref) { + // 4. Compare Isometry3 with Isometry3d + auto test_ad_vs_double = [](const vector>& test, + const vector& ref) { EXPECT_EQ(test.size(), ref.size()); for (size_t i = 0; i < ref.size(); ++i) { for (int row = 0; row < 3; ++row) { @@ -726,7 +744,7 @@ TEST_F(GeometryStateTest, Transmogrify) { TEST_F(GeometryStateTest, ValidateSingleSourceTree) { // NOTE: There are *two* sources -- the built-in self source id for geometry // state and the id added here. - SourceId s_id = SetUpSingleSourceTree(); + const SourceId s_id = SetUpSingleSourceTree(); // The source has *direct* access to all registered frames. { @@ -778,7 +796,7 @@ TEST_F(GeometryStateTest, ValidateSingleSourceTree) { const auto& frame_in_parent = gs_tester_.get_frame_parent_poses(); EXPECT_TRUE( CompareMatrices(frame_in_parent[frame.index()].matrix(), - X_PF_[i].matrix())); + X_PFs_[i].matrix())); }; // When added, all frames' poses w.r.t. their parents are the identity. @@ -786,13 +804,13 @@ TEST_F(GeometryStateTest, ValidateSingleSourceTree) { for (FrameId frame_id : frames_) { const auto& frame = internal_frames.at(frame_id); EXPECT_TRUE(CompareMatrices(frame_in_parent[frame.index()].matrix(), - Isometry3::Identity().matrix())); + Isometry3d::Identity().matrix())); } // Confirm posing positions the frames properly. FramePoseVector poses; for (int f = 0; f < static_cast(frames_.size()); ++f) { - poses.set_value(frames_[f], X_PF_[f]); + poses.set_value(frames_[f], X_PFs_[f]); } gs_tester_.SetFramePoses(s_id, poses); gs_tester_.FinalizePoseUpdate(); @@ -823,10 +841,10 @@ TEST_F(GeometryStateTest, ValidateSingleSourceTree) { // the documentation for GeometryState::GetPoseInParent() indicates). EXPECT_TRUE(CompareMatrices( geometry_state_.GetPoseInFrame(geometry.id()).matrix(), - X_FG_[i].matrix())); + X_FGs_[i].matrix())); EXPECT_TRUE(CompareMatrices( geometry_state_.GetPoseInParent(geometry.id()).matrix(), - X_FG_[i].matrix())); + X_FGs_[i].matrix())); EXPECT_EQ( gs_tester_.get_geometry_index_id_map()[geometry.index()], @@ -840,7 +858,7 @@ TEST_F(GeometryStateTest, ValidateSingleSourceTree) { // Tests the GetNum*Geometry*Methods. TEST_F(GeometryStateTest, GetNumGeometryTests) { - SetUpSingleSourceTree(true /* add proximity roles */); + SetUpSingleSourceTree(Assign::kProximity); EXPECT_EQ(single_tree_total_geometry_count(), geometry_state_.get_num_geometries()); @@ -863,16 +881,16 @@ TEST_F(GeometryStateTest, GetNumGeometryTests) { // Tests that an attempt to add a frame to an invalid source throws an exception // with meaningful message. TEST_F(GeometryStateTest, AddFrameToInvalidSource) { - SourceId s_id = SourceId::get_new_id(); // This is not a registered source. + const SourceId s_id = SourceId::get_new_id(); // Not a registered source. DRAKE_ASSERT_THROWS_MESSAGE( - geometry_state_.RegisterFrame(s_id, *frame_.get()), std::logic_error, + geometry_state_.RegisterFrame(s_id, *frame_), std::logic_error, "Referenced geometry source \\d+ is not registered."); } // Tests that a frame added to a valid source appears in the source's frames. TEST_F(GeometryStateTest, AddFirstFrameToValidSource) { - SourceId s_id = NewSource(); - FrameId fid = geometry_state_.RegisterFrame(s_id, *frame_.get()); + const SourceId s_id = NewSource(); + const FrameId fid = geometry_state_.RegisterFrame(s_id, *frame_); EXPECT_EQ(fid, frame_->id()); EXPECT_TRUE(geometry_state_.BelongsToSource(fid, s_id)); const auto &frame_set = geometry_state_.GetFramesForSource(s_id); @@ -886,8 +904,8 @@ TEST_F(GeometryStateTest, AddFirstFrameToValidSource) { // Tests that a frame added to a valid source which already has frames is // correctly appended. TEST_F(GeometryStateTest, AddFrameToSourceWithFrames) { - SourceId s_id = SetUpSingleSourceTree(); - FrameId fid = geometry_state_.RegisterFrame(s_id, *frame_); + const SourceId s_id = SetUpSingleSourceTree(); + const FrameId fid = geometry_state_.RegisterFrame(s_id, *frame_); EXPECT_EQ(fid, frame_->id()); EXPECT_TRUE(geometry_state_.BelongsToSource(fid, s_id)); const auto &frame_set = geometry_state_.GetFramesForSource(s_id); @@ -902,9 +920,9 @@ TEST_F(GeometryStateTest, AddFrameToSourceWithFrames) { // Tests that a frame added to a new source doesn't modify previously existing // sources. TEST_F(GeometryStateTest, AddFrameToNewSourceWithFrames) { - SourceId s_id = SetUpSingleSourceTree(); - SourceId new_s_id = geometry_state_.RegisterNewSource("new_source"); - FrameId fid = geometry_state_.RegisterFrame(new_s_id, *frame_.get()); + const SourceId s_id = SetUpSingleSourceTree(); + const SourceId new_s_id = geometry_state_.RegisterNewSource("new_source"); + const FrameId fid = geometry_state_.RegisterFrame(new_s_id, *frame_); EXPECT_EQ(fid, frame_->id()); // Confirm addition. EXPECT_TRUE(geometry_state_.BelongsToSource(fid, new_s_id)); @@ -923,8 +941,8 @@ TEST_F(GeometryStateTest, AddFrameToNewSourceWithFrames) { // Tests the functionality of adding a frame to another frame. TEST_F(GeometryStateTest, AddFrameOnFrame) { - SourceId s_id = SetUpSingleSourceTree(); - FrameId fid = geometry_state_.RegisterFrame(s_id, frames_[0], *frame_); + const SourceId s_id = SetUpSingleSourceTree(); + const FrameId fid = geometry_state_.RegisterFrame(s_id, frames_[0], *frame_); EXPECT_EQ(fid, frame_->id()); // Includes the kFrameCount frames added in SetUpSingleSourceTree, the // frame we just added above (fid), and the world frame. @@ -946,8 +964,8 @@ TEST_F(GeometryStateTest, AddFrameOnFrame) { // Confirms that adding two frames with the same id causes an error. TEST_F(GeometryStateTest, AddFrameWithDuplicateId) { - SourceId s_id = NewSource(); - FrameId f_id = geometry_state_.RegisterFrame(s_id, *frame_.get()); + const SourceId s_id = NewSource(); + const FrameId f_id = geometry_state_.RegisterFrame(s_id, *frame_); DRAKE_EXPECT_THROWS_MESSAGE( geometry_state_.RegisterFrame(s_id, *frame_), std::logic_error, "Registering frame with an id that has already been registered: \\d+"); @@ -959,7 +977,7 @@ TEST_F(GeometryStateTest, AddFrameWithDuplicateId) { // Tests the frame iterator, confirming that it iterates through all frames. TEST_F(GeometryStateTest, FrameIdRange) { SetUpSingleSourceTree(); - std::unordered_set all_frames(frames_.begin(), frames_.end()); + unordered_set all_frames(frames_.begin(), frames_.end()); for (FrameId id : geometry_state_.get_frame_ids()) { // This should remove exactly one element. The world frame is *not* stored // in frames_. @@ -975,15 +993,15 @@ TEST_F(GeometryStateTest, FrameIdRange) { // correctness of GeometryState::BelongsToSource(GeometryId, SourceId) and // GeometryState::GetFrameId(GeometryId) and, therefore, implicitly tests them. TEST_F(GeometryStateTest, RegisterGeometryGoodSource) { - SourceId s_id = NewSource(); - FrameId f_id = geometry_state_.RegisterFrame(s_id, *frame_); - GeometryId expected_g_id = instance_->id(); - GeometryId g_id = geometry_state_.RegisterGeometry(s_id, f_id, + const SourceId s_id = NewSource(); + const FrameId f_id = geometry_state_.RegisterFrame(s_id, *frame_); + const GeometryId expected_g_id = instance_->id(); + const GeometryId g_id = geometry_state_.RegisterGeometry(s_id, f_id, move(instance_)); EXPECT_EQ(g_id, expected_g_id); EXPECT_EQ(geometry_state_.GetFrameId(g_id), f_id); EXPECT_TRUE(geometry_state_.BelongsToSource(g_id, s_id)); - Isometry3 X_FG = geometry_state_.GetPoseInFrame(g_id); + const Isometry3d& X_FG = geometry_state_.GetPoseInFrame(g_id); EXPECT_TRUE(CompareMatrices(X_FG.matrix(), instance_pose_.matrix())); EXPECT_TRUE(gs_tester_.get_frames().at(f_id).has_child(g_id)); @@ -994,8 +1012,8 @@ TEST_F(GeometryStateTest, RegisterGeometryGoodSource) { // Confirms that registering two geometries with the same id causes failure. TEST_F(GeometryStateTest, RegisterDuplicateGeometry) { - SourceId s_id = NewSource(); - FrameId f_id = geometry_state_.RegisterFrame(s_id, *frame_); + const SourceId s_id = NewSource(); + const FrameId f_id = geometry_state_.RegisterFrame(s_id, *frame_); auto instance_copy = make_unique(*instance_); geometry_state_.RegisterGeometry(s_id, f_id, move(instance_)); DRAKE_EXPECT_THROWS_MESSAGE( @@ -1006,8 +1024,8 @@ TEST_F(GeometryStateTest, RegisterDuplicateGeometry) { // Tests registration of geometry on invalid source. TEST_F(GeometryStateTest, RegisterGeometryMissingSource) { - SourceId s_id = SourceId::get_new_id(); - FrameId f_id = FrameId::get_new_id(); + const SourceId s_id = SourceId::get_new_id(); + const FrameId f_id = FrameId::get_new_id(); DRAKE_EXPECT_THROWS_MESSAGE( geometry_state_.RegisterGeometry(s_id, f_id, move(instance_)), std::logic_error, "Referenced geometry source \\d+ is not registered."); @@ -1015,9 +1033,8 @@ TEST_F(GeometryStateTest, RegisterGeometryMissingSource) { // Tests registration of geometry on valid source and non-existent frame. TEST_F(GeometryStateTest, RegisterGeometryMissingFrame) { - SourceId s_id = NewSource(); - - FrameId f_id = FrameId::get_new_id(); + const SourceId s_id = NewSource(); + const FrameId f_id = FrameId::get_new_id(); DRAKE_EXPECT_THROWS_MESSAGE( geometry_state_.RegisterGeometry(s_id, f_id, move(instance_)), std::logic_error, @@ -1027,8 +1044,8 @@ TEST_F(GeometryStateTest, RegisterGeometryMissingFrame) { // Tests error resulting from passing a null GeometryInstance. TEST_F(GeometryStateTest, RegisterNullGeometry) { - SourceId s_id = NewSource(); - FrameId f_id = geometry_state_.RegisterFrame(s_id, *frame_); + const SourceId s_id = NewSource(); + const FrameId f_id = geometry_state_.RegisterFrame(s_id, *frame_); unique_ptr null_geometry; DRAKE_EXPECT_THROWS_MESSAGE( geometry_state_.RegisterGeometry(s_id, f_id, move(null_geometry)), @@ -1039,12 +1056,11 @@ TEST_F(GeometryStateTest, RegisterNullGeometry) { // Tests the logic for hanging a geometry on another geometry. This confirms // topology and pose values. TEST_F(GeometryStateTest, RegisterGeometryonValidGeometry) { - SourceId s_id = SetUpSingleSourceTree(); + const SourceId s_id = SetUpSingleSourceTree(); const double x = 3; const double y = 2; const double z = 1; - Isometry3 pose = Isometry3::Identity(); - pose.translation() << x, y, z; + Isometry3d pose{Translation3d{x, y, z}}; const int parent_index = 0; const GeometryId parent_id = geometries_[parent_index]; const FrameId frame_id = geometry_state_.GetFrameId(parent_id); @@ -1061,14 +1077,14 @@ TEST_F(GeometryStateTest, RegisterGeometryonValidGeometry) { // geometry is at [parent_index + 1, 0, 0] and this is at [3, 2, 1]. They // simply sum up. The parent has *no* rotation so the resultant transform is // simply the sum of the translation vectors. - Isometry3 expected_pose_in_frame = Isometry3::Identity(); - expected_pose_in_frame.translation() << (parent_index + 1) + x, y, z; + const Isometry3d expected_pose_in_frame{ + Translation3d{(parent_index + 1) + x, y, z}}; EXPECT_EQ(frame_id, geometry_state_.GetFrameId(g_id)); - Isometry3 X_FG = geometry_state_.GetPoseInFrame(g_id); + const Isometry3d& X_FG = geometry_state_.GetPoseInFrame(g_id); EXPECT_TRUE(CompareMatrices(X_FG.matrix(), expected_pose_in_frame.matrix(), 1e-14, MatrixCompareType::absolute)); - Isometry3 X_PG = geometry_state_.GetPoseInParent(g_id); + const Isometry3d& X_PG = geometry_state_.GetPoseInParent(g_id); EXPECT_TRUE(CompareMatrices(X_PG.matrix(), pose.matrix(), 1e-14, MatrixCompareType::absolute)); @@ -1088,11 +1104,10 @@ TEST_F(GeometryStateTest, RegisterGeometryonValidGeometry) { // Tests the response to the erroneous action of trying to hang a new geometry // on a non-existent geometry id. TEST_F(GeometryStateTest, RegisterGeometryonInvalidGeometry) { - SourceId s_id = SetUpSingleSourceTree(); - Isometry3 pose = Isometry3::Identity(); + const SourceId s_id = SetUpSingleSourceTree(); auto instance = make_unique( - pose, make_unique(1), "sphere"); - GeometryId junk_id = GeometryId::get_new_id(); + Isometry3d::Identity(), make_unique(1), "sphere"); + const GeometryId junk_id = GeometryId::get_new_id(); DRAKE_EXPECT_THROWS_MESSAGE( geometry_state_.RegisterGeometryWithParent(s_id, junk_id, move(instance)), std::logic_error, @@ -1101,7 +1116,7 @@ TEST_F(GeometryStateTest, RegisterGeometryonInvalidGeometry) { // Tests the response to passing a null pointer as a GeometryInstance. TEST_F(GeometryStateTest, RegisterNullGeometryonGeometry) { - SourceId s_id = SetUpSingleSourceTree(); + const SourceId s_id = SetUpSingleSourceTree(); unique_ptr instance; DRAKE_EXPECT_THROWS_MESSAGE( geometry_state_.RegisterGeometryWithParent(s_id, geometries_[0], @@ -1112,12 +1127,12 @@ TEST_F(GeometryStateTest, RegisterNullGeometryonGeometry) { // Tests the registration of anchored geometry. TEST_F(GeometryStateTest, RegisterAnchoredGeometry) { - SourceId s_id = NewSource("new source"); - Isometry3 pose = Isometry3::Identity(); + const SourceId s_id = NewSource("new source"); auto instance = make_unique( - pose, make_unique(1), "sphere"); - GeometryId expected_g_id = instance->id(); - auto g_id = geometry_state_.RegisterAnchoredGeometry(s_id, move(instance)); + Isometry3d::Identity(), make_unique(1), "sphere"); + const GeometryId expected_g_id = instance->id(); + const auto g_id = + geometry_state_.RegisterAnchoredGeometry(s_id, move(instance)); EXPECT_EQ(g_id, expected_g_id); EXPECT_TRUE(geometry_state_.BelongsToSource(g_id, s_id)); const InternalGeometry* g = gs_tester_.GetGeometry(g_id); @@ -1132,9 +1147,8 @@ TEST_F(GeometryStateTest, RegisterAnchoredGeometry) { // Tests the registration of a new geometry on another geometry. TEST_F(GeometryStateTest, RegisterAnchoredOnAnchoredGeometry) { // Add an anchored geometry. - SourceId s_id = NewSource("new source"); - Isometry3 pose = Isometry3::Identity(); - pose.translation() << 1, 2, 3; + const SourceId s_id = NewSource("new source"); + Isometry3d pose{Translation3d{1, 2, 3}}; auto instance = make_unique( pose, make_unique(1), "sphere1"); auto parent_id = geometry_state_.RegisterAnchoredGeometry(s_id, @@ -1151,14 +1165,14 @@ TEST_F(GeometryStateTest, RegisterAnchoredOnAnchoredGeometry) { EXPECT_TRUE(static_cast(child->parent_id())); EXPECT_EQ(parent_id, *child->parent_id()); EXPECT_TRUE(CompareMatrices(pose.matrix(), child->X_PG().matrix())); - EXPECT_TRUE(CompareMatrices((parent->X_FG() * pose).matrix(), - child->X_FG().matrix())); + const Isometry3d& X_FP = parent->X_FG(); + EXPECT_TRUE(CompareMatrices((X_FP * pose).matrix(), child->X_FG().matrix())); EXPECT_EQ(InternalFrame::world_frame_id(), parent->frame_id()); } // Confirms that registering two geometries with the same id causes failure. TEST_F(GeometryStateTest, RegisterDuplicateAnchoredGeometry) { - SourceId s_id = NewSource(); + const SourceId s_id = NewSource(); auto instance_copy = make_unique(*instance_); geometry_state_.RegisterAnchoredGeometry(s_id, move(instance_)); DRAKE_EXPECT_THROWS_MESSAGE( @@ -1170,9 +1184,8 @@ TEST_F(GeometryStateTest, RegisterDuplicateAnchoredGeometry) { // Tests the attempt to register anchored geometry on an invalid source. TEST_F(GeometryStateTest, RegisterAnchoredGeometryInvalidSource) { - Isometry3 pose = Isometry3::Identity(); auto instance = make_unique( - pose, make_unique(1), "sphere"); + Isometry3d::Identity(), make_unique(1), "sphere"); DRAKE_EXPECT_THROWS_MESSAGE( geometry_state_.RegisterAnchoredGeometry(SourceId::get_new_id(), move(instance)), @@ -1191,27 +1204,36 @@ TEST_F(GeometryStateTest, RegisterAnchoredNullGeometry) { "Registering null geometry to frame \\d+, on source \\d+."); } -// Tests the RemoveGeometry functionality. Ultimately, this confirms that the -// re-ordering of geometries is correct. By removing geometry 0 (the first -// registered geometry (and first dynamic geometry), two indices will change: -// GeometryIndex(0) will refer to the anchored geometry (the geometry which -// previously had the highest GeometryIndex value). -// ProximityIndex(0) will refer to the last _dynamic_ geometry (the -// dynamic geometry that previously had the highest ProximityIndex value.) +// Tests the RemoveGeometry() functionality. This action will have several +// effects which will all be confirmed. By removing geometry 0 (the first +// registered geometry and first dynamic geometry), the following effects are +// expected: +// 1. GeometryState moves geometries around to maintain compact distribution +// of GeometryIndex values. Therefore, the GeometryIndex(0) (which +// previously referred to the removed geometry now refers to the anchored +// geometry (the *last* geometry added to the state in the tree). +// 2. The proximity engine will likewise shuffle indices. Therefore, +// ProximityIndex(0) will now refer to the last _dynamic_ geometry (the +// dynamic geometry that previously had the highest ProximityIndex value +// because the proximity engine only moves dynamic geometries in place of +// dynamic geometries and anchored for anchored). TEST_F(GeometryStateTest, RemoveGeometry) { - SourceId s_id = SetUpSingleSourceTree(true); + const SourceId s_id = SetUpSingleSourceTree(Assign::kProximity); + // Pose all of the frames to the specified poses in their parent frame. FramePoseVector poses; for (int f = 0; f < static_cast(frames_.size()); ++f) { - poses.set_value(frames_[f], X_PF_[f]); + poses.set_value(frames_[f], X_PFs_[f]); } gs_tester_.SetFramePoses(s_id, poses); gs_tester_.FinalizePoseUpdate(); // The geometry to remove, its parent frame, and its engine index. - GeometryId g_id = geometries_[0]; - FrameId f_id = frames_[0]; - auto proximity_index = gs_tester_.get_geometries().at(g_id).proximity_index(); + const GeometryId g_id = geometries_[0]; + const FrameId f_id = frames_[0]; + const ProximityIndex proximity_index = + gs_tester_.get_geometries().at(g_id).proximity_index(); + // Confirm initial state. ASSERT_EQ(geometry_state_.GetFrameId(g_id), f_id); EXPECT_EQ(geometry_state_.get_num_geometries(), @@ -1233,7 +1255,7 @@ TEST_F(GeometryStateTest, RemoveGeometry) { EXPECT_EQ(gs_tester_.get_geometries().count(g_id), 0); // Confirm GeometryIndex(0) now maps to the anchored geometry. - GeometryId last_geometry_id = anchored_geometry_; + const GeometryId last_geometry_id = anchored_geometry_; const auto& last_geometry = gs_tester_.get_geometries().at(last_geometry_id); EXPECT_EQ(last_geometry.proximity_index(), proximity_index); @@ -1245,7 +1267,7 @@ TEST_F(GeometryStateTest, RemoveGeometry) { EXPECT_EQ( gs_tester_.get_geometries().at(geometries_.back()).proximity_index(), proximity_index); - Isometry3 X_WG = X_WF_.back() * X_FG_.back(); + const Isometry3d X_WG = X_WFs_.back() * X_FGs_.back(); EXPECT_TRUE(CompareMatrices(gs_tester_.get_geometry_world_poses()[0].matrix(), X_WG.matrix())); @@ -1254,7 +1276,7 @@ TEST_F(GeometryStateTest, RemoveGeometry) { // Adding a new geometry should bring the number of total geometries back to // the original count. - GeometryId added_id = geometry_state_.RegisterGeometry( + const GeometryId added_id = geometry_state_.RegisterGeometry( source_id_, frames_[0], make_unique(Isometry3d::Identity(), make_unique(1), "newest")); @@ -1282,7 +1304,7 @@ TEST_F(GeometryStateTest, RemoveGeometry) { // make sure the geometry moves, it needs to _not_ be the last added geometry. // So, we add the geometry we're going to remove _and_ an additional geometry // after it. - GeometryId no_role_id = geometry_state_.RegisterGeometry( + const GeometryId no_role_id = geometry_state_.RegisterGeometry( source_id_, frames_[0], make_unique( Isometry3d::Identity(), make_unique(1), "no_role_geometry")); @@ -1297,26 +1319,26 @@ TEST_F(GeometryStateTest, RemoveGeometry) { // Tests the RemoveGeometry functionality in which the geometry removed has // geometry children. TEST_F(GeometryStateTest, RemoveGeometryTree) { - SourceId s_id = SetUpSingleSourceTree(true); + const SourceId s_id = SetUpSingleSourceTree(Assign::kProximity); // Pose all of the frames to the specified poses in their parent frame. FramePoseVector poses; for (int f = 0; f < static_cast(frames_.size()); ++f) { - poses.set_value(frames_[f], X_PF_[f]); + poses.set_value(frames_[f], X_PFs_[f]); } gs_tester_.SetFramePoses(s_id, poses); gs_tester_.FinalizePoseUpdate(); // The geometry to remove, its parent frame, and its proximity index. - GeometryId root_id = geometries_[0]; - FrameId f_id = frames_[0]; + const GeometryId root_id = geometries_[0]; + const FrameId f_id = frames_[0]; auto proximity_index = gs_tester_.get_geometries().at(root_id).proximity_index(); // Confirm that the first geometry belongs to the first frame. ASSERT_EQ(geometry_state_.GetFrameId(root_id), f_id); // Hang geometry from the first geometry. - GeometryId g_id = geometry_state_.RegisterGeometryWithParent( + const GeometryId g_id = geometry_state_.RegisterGeometryWithParent( s_id, root_id, - make_unique(Isometry3::Identity(), + make_unique(Isometry3d::Identity(), unique_ptr(new Sphere(1)), "leaf")); geometry_state_.AssignRole(s_id, g_id, ProximityProperties()); @@ -1347,7 +1369,7 @@ TEST_F(GeometryStateTest, RemoveGeometryTree) { // swapping akin to that in the RemoveGeometry() test. // Confirm GeometryIndex(0) now maps to the anchored geometry. - GeometryId last_geometry_id = anchored_geometry_; + const GeometryId last_geometry_id = anchored_geometry_; const auto& last_geometry = gs_tester_.get_geometries().at(last_geometry_id); EXPECT_EQ(last_geometry.proximity_index(), proximity_index); @@ -1359,7 +1381,7 @@ TEST_F(GeometryStateTest, RemoveGeometryTree) { EXPECT_EQ( gs_tester_.get_geometries().at(geometries_.back()).proximity_index(), proximity_index); - Isometry3 X_WG = X_WF_.back() * X_FG_.back(); + const Isometry3d X_WG = X_WFs_.back() * X_FGs_.back(); EXPECT_TRUE(CompareMatrices(gs_tester_.get_geometry_world_poses()[0].matrix(), X_WG.matrix())); } @@ -1367,16 +1389,16 @@ TEST_F(GeometryStateTest, RemoveGeometryTree) { // Tests the RemoveGeometry functionality in which the geometry is a child of // another geometry (and has no child geometries itself). TEST_F(GeometryStateTest, RemoveChildLeaf) { - SourceId s_id = SetUpSingleSourceTree(true); + const SourceId s_id = SetUpSingleSourceTree(Assign::kProximity); // The geometry parent and frame to which it belongs. - GeometryId parent_id = geometries_[0]; - FrameId frame_id = frames_[0]; + const GeometryId parent_id = geometries_[0]; + const FrameId frame_id = frames_[0]; // Confirm that the first geometry belongs to the first frame. ASSERT_EQ(geometry_state_.GetFrameId(parent_id), frame_id); // Hang geometry from the first geometry. - GeometryId g_id = geometry_state_.RegisterGeometryWithParent( + const GeometryId g_id = geometry_state_.RegisterGeometryWithParent( s_id, parent_id, - make_unique(Isometry3::Identity(), + make_unique(Isometry3d::Identity(), unique_ptr(new Sphere(1)), "leaf")); EXPECT_EQ(geometry_state_.get_num_geometries(), single_tree_total_geometry_count() + 1); @@ -1403,7 +1425,7 @@ TEST_F(GeometryStateTest, RemoveChildLeaf) { // Tests the response to invalid use of RemoveGeometry. TEST_F(GeometryStateTest, RemoveGeometryInvalid) { - SourceId s_id = SetUpSingleSourceTree(); + const SourceId s_id = SetUpSingleSourceTree(); // Case: Invalid source id, valid geometry id. DRAKE_EXPECT_THROWS_MESSAGE( @@ -1419,12 +1441,12 @@ TEST_F(GeometryStateTest, RemoveGeometryInvalid) { "Referenced geometry \\d+ has not been registered."); // Case: Valid geometry and source, but geometry belongs to different source. - SourceId s_id2 = geometry_state_.RegisterNewSource("new_source"); - FrameId frame_id = geometry_state_.RegisterFrame(s_id2, *frame_); + const SourceId s_id2 = geometry_state_.RegisterNewSource("new_source"); + const FrameId frame_id = geometry_state_.RegisterFrame(s_id2, *frame_); EXPECT_EQ(geometry_state_.get_num_frames(), single_tree_frame_count() + 1); - GeometryId g_id = geometry_state_.RegisterGeometry( + const GeometryId g_id = geometry_state_.RegisterGeometry( s_id2, frame_id, - make_unique(Isometry3::Identity(), + make_unique(Isometry3d::Identity(), unique_ptr(new Sphere(1)), "new")); EXPECT_EQ(geometry_state_.get_num_geometries(), single_tree_total_geometry_count() + 1); @@ -1437,7 +1459,7 @@ TEST_F(GeometryStateTest, RemoveGeometryInvalid) { // Tests removal of anchored geometry. TEST_F(GeometryStateTest, RemoveAnchoredGeometry) { - SourceId s_id = SetUpSingleSourceTree(true); + const SourceId s_id = SetUpSingleSourceTree(Assign::kProximity); const Vector3 normal{0, 1, 0}; const Vector3 point{1, 1, 1}; @@ -1473,7 +1495,7 @@ TEST_F(GeometryStateTest, RemoveAnchoredGeometry) { // Tests removal of geometry when collision filters are present. As geometries // move around, their filter semantics should follow. TEST_F(GeometryStateTest, RemoveGeometryWithCollisionFilters) { - SourceId s_id = SetUpSingleSourceTree(true); + const SourceId s_id = SetUpSingleSourceTree(Assign::kProximity); auto is_filtered = [this](int index0, int index1) { return geometry_state_.CollisionFiltered(geometries_[index0], geometries_[index1]); @@ -1542,7 +1564,7 @@ TEST_F(GeometryStateTest, GetPoseForBadGeometryId) { // source id is invalid. Whether or not the frame/geometry ids are valid, the // bad source should dominate. TEST_F(GeometryStateTest, SourceOwnershipInvalidSource) { - SourceId source_id = SourceId::get_new_id(); + const SourceId source_id = SourceId::get_new_id(); // Invalid frame/geometry ids. DRAKE_EXPECT_THROWS_MESSAGE( geometry_state_.BelongsToSource(FrameId::get_new_id(), source_id), @@ -1551,9 +1573,9 @@ TEST_F(GeometryStateTest, SourceOwnershipInvalidSource) { geometry_state_.BelongsToSource(GeometryId::get_new_id(), source_id), std::logic_error, "Referenced geometry source \\d+ is not registered."); SetUpSingleSourceTree(); - GeometryId anchored_id = geometry_state_.RegisterAnchoredGeometry( + const GeometryId anchored_id = geometry_state_.RegisterAnchoredGeometry( source_id_, - make_unique(Isometry3::Identity(), + make_unique(Isometry3d::Identity(), make_unique(1), "sphere")); // Valid frame/geometry ids. @@ -1571,7 +1593,7 @@ TEST_F(GeometryStateTest, SourceOwnershipInvalidSource) { // This tests the source ownership functionality for frames - a function which // reports if a frame belongs to the specified source. TEST_F(GeometryStateTest, SourceOwnershipFrameId) { - SourceId s_id = SetUpSingleSourceTree(); + const SourceId s_id = SetUpSingleSourceTree(); // Test for invalid frame. DRAKE_EXPECT_THROWS_MESSAGE( geometry_state_.BelongsToSource(FrameId::get_new_id(), s_id), @@ -1584,9 +1606,9 @@ TEST_F(GeometryStateTest, SourceOwnershipFrameId) { // reports if a geometry belongs to the specified source. It examines dynamic // and anchored geometry. TEST_F(GeometryStateTest, SourceOwnershipGeometryId) { - SourceId s_id = SetUpSingleSourceTree(); - GeometryId anchored_id = geometry_state_.RegisterAnchoredGeometry( - s_id, make_unique(Isometry3::Identity(), + const SourceId s_id = SetUpSingleSourceTree(); + const GeometryId anchored_id = geometry_state_.RegisterAnchoredGeometry( + s_id, make_unique(Isometry3d::Identity(), make_unique(1), "sphere")); // Test for invalid geometry. @@ -1608,10 +1630,10 @@ TEST_F(GeometryStateTest, GetFrameIdFromBadId) { // Tests the validation of the ids provided in a frame kinematics vector. TEST_F(GeometryStateTest, ValidateFrameIds) { - SourceId s_id = SetUpSingleSourceTree(); + const SourceId s_id = SetUpSingleSourceTree(); FramePoseVector frame_set; for (auto frame_id : frames_) { - frame_set.set_value(frame_id, Isometry3::Identity()); + frame_set.set_value(frame_id, Isometry3d::Identity()); } // Case: frame ids are valid. EXPECT_NO_THROW(gs_tester_.ValidateFrameIds(s_id, frame_set)); @@ -1629,7 +1651,7 @@ TEST_F(GeometryStateTest, ValidateFrameIds) { // Case: Too few frames. FramePoseVector frame_set_3; for (int i = 0; i < kFrameCount - 1; ++i) { - frame_set.set_value(frames_[i], Isometry3::Identity()); + frame_set.set_value(frames_[i], Isometry3d::Identity()); } DRAKE_EXPECT_THROWS_MESSAGE( gs_tester_.ValidateFrameIds(s_id, frame_set_3), std::runtime_error, @@ -1645,11 +1667,11 @@ TEST_F(GeometryStateTest, ValidateFrameIds) { // that the right matrix multiplications are performed based on the hierarchy // of constructs. TEST_F(GeometryStateTest, SetFramePoses) { - SourceId s_id = SetUpSingleSourceTree(); + const SourceId s_id = SetUpSingleSourceTree(); // A vector of poses we will use to populate FramePoseVectors. - vector> frame_poses; + vector frame_poses; for (int i = 0; i < kFrameCount; ++i) { - frame_poses.push_back(Isometry3::Identity()); + frame_poses.push_back(Isometry3d::Identity()); } auto make_pose_vector = @@ -1672,14 +1694,13 @@ TEST_F(GeometryStateTest, SetFramePoses) { const auto& world_poses = gs_tester_.get_geometry_world_poses(); for (int i = 0; i < kFrameCount * kGeometryCount; ++i) { EXPECT_TRUE(CompareMatrices(world_poses[i].matrix().block<3, 4>(0, 0), - X_FG_[i].matrix().block<3, 4>(0, 0))); + X_FGs_[i].matrix().block<3, 4>(0, 0))); } // Case 2: Move the two *root* frames 1 unit in the +y direction. The f2 will // stay at the identity. // The final geometry poses should all be offset by 1 unit in the y. - Isometry3 offset = Isometry3::Identity(); - offset.translation() << 0, 1, 0; + const Isometry3d offset{Translation3d{0, 1, 0}}; frame_poses[0] = offset; frame_poses[1] = offset; FramePoseVector poses2 = make_pose_vector(); @@ -1687,7 +1708,7 @@ TEST_F(GeometryStateTest, SetFramePoses) { for (int i = 0; i < kFrameCount * kGeometryCount; ++i) { EXPECT_TRUE( CompareMatrices(world_poses[i].matrix().block<3, 4>(0, 0), - (offset * X_FG_[i].matrix()).block<3, 4>(0, 0))); + (offset * X_FGs_[i].matrix()).block<3, 4>(0, 0))); } // Case 3: All frames get set to move up one unit. This will leave geometries @@ -1698,19 +1719,19 @@ TEST_F(GeometryStateTest, SetFramePoses) { for (int i = 0; i < (kFrameCount - 1) * kGeometryCount; ++i) { EXPECT_TRUE( CompareMatrices(world_poses[i].matrix().block<3, 4>(0, 0), - (offset * X_FG_[i].matrix()).block<3, 4>(0, 0))); + (offset * X_FGs_[i].matrix()).block<3, 4>(0, 0))); } for (int i = (kFrameCount - 1) * kGeometryCount; i < kFrameCount * kGeometryCount; ++i) { EXPECT_TRUE(CompareMatrices( world_poses[i].matrix().block<3, 4>(0, 0), - (offset * offset * X_FG_[i].matrix()).block<3, 4>(0, 0))); + (offset * offset * X_FGs_[i].matrix()).block<3, 4>(0, 0))); } } // Test various frame property queries. TEST_F(GeometryStateTest, QueryFrameProperties) { - SourceId s_id = SetUpSingleSourceTree(); + const SourceId s_id = SetUpSingleSourceTree(); const FrameId world = InternalFrame::world_frame_id(); // Query frame group. @@ -1730,12 +1751,12 @@ TEST_F(GeometryStateTest, QueryFrameProperties) { // Set the frame poses to query geometry and frame poses. FramePoseVector poses; - for (int i = 0; i < kFrameCount; ++i) poses.set_value(frames_[i], X_PF_[i]); + for (int i = 0; i < kFrameCount; ++i) poses.set_value(frames_[i], X_PFs_[i]); gs_tester_.SetFramePoses(s_id, poses); EXPECT_TRUE( CompareMatrices(geometry_state_.get_pose_in_world(frames_[0]).matrix(), - X_WF_[0].matrix())); + X_WFs_[0].matrix())); EXPECT_TRUE( CompareMatrices(geometry_state_.get_pose_in_world(world).matrix(), Isometry3d::Identity().matrix())); @@ -1744,10 +1765,10 @@ TEST_F(GeometryStateTest, QueryFrameProperties) { std::logic_error, "No world pose available for invalid frame id: \\d+"); // This assumes that geometry parent belongs to frame 0. - Isometry3 geometry_pose = X_WF_[0] * X_FG_[0]; + const Isometry3d X_WG = X_WFs_[0] * X_FGs_[0]; EXPECT_TRUE(CompareMatrices( geometry_state_.get_pose_in_world(geometries_[0]).matrix(), - geometry_pose.matrix())); + X_WG.matrix())); DRAKE_EXPECT_THROWS_MESSAGE( geometry_state_.get_pose_in_world(GeometryId::get_new_id()), std::logic_error, @@ -1758,7 +1779,7 @@ TEST_F(GeometryStateTest, QueryFrameProperties) { EXPECT_TRUE( CompareMatrices(geometry_state_.get_pose_in_parent(frames_[0]).matrix(), - X_PF_[0].matrix())); + X_PFs_[0].matrix())); EXPECT_TRUE( CompareMatrices(geometry_state_.get_pose_in_parent(world).matrix(), Isometry3d::Identity().matrix())); @@ -1768,12 +1789,12 @@ TEST_F(GeometryStateTest, QueryFrameProperties) { } TEST_F(GeometryStateTest, TestCollisionCandidates) { - SetUpSingleSourceTree(true /* assign proximity roles */); + SetUpSingleSourceTree(Assign::kProximity); // The explicit enumeration of candidate pairs. geometries 0 & 1, 2 & 3, and // 4 & 5 are siblings, therefore they are not valid candidate pairs. All other // combinations _are_. - std::set> expected_candidates = + set> expected_candidates = {{geometries_[0], geometries_[2]}, {geometries_[0], geometries_[3]}, {geometries_[0], geometries_[4]}, {geometries_[0], geometries_[5]}, {geometries_[0], anchored_geometry_}, @@ -1788,14 +1809,14 @@ TEST_F(GeometryStateTest, TestCollisionCandidates) { {geometries_[5], anchored_geometry_}}; auto candidates_in_set = - [](const std::set>& candidates, - const std::set>& expected) { + [](const set>& candidates, + const set>& expected) { ::testing::AssertionResult result = ::testing::AssertionSuccess(); if (candidates != expected) { result = ::testing::AssertionFailure(); auto print_difference = [&result](const auto& set1, const auto& set2, const char* msg) { - std::set> diff; + set> diff; std::set_difference(set1.begin(), set1.end(), set2.begin(), set2.end(), std::inserter(diff, diff.begin())); if (!diff.empty()) { @@ -1820,7 +1841,7 @@ TEST_F(GeometryStateTest, TestCollisionCandidates) { // This assumes that ExcludeCollisionsBetween() (tested below) works. while (!expected_candidates.empty()) { - auto pair = expected_candidates.begin(); + const auto pair = expected_candidates.begin(); geometry_state_.ExcludeCollisionsBetween(GeometrySet{pair->first}, GeometrySet{pair->second}); expected_candidates.erase(pair); @@ -1834,12 +1855,12 @@ TEST_F(GeometryStateTest, TestCollisionCandidates) { // Test disallowing collisions among members of a group (self collisions). TEST_F(GeometryStateTest, ExcludeCollisionsWithin) { - SetUpSingleSourceTree(true /* assign proximity roles */); + SetUpSingleSourceTree(Assign::kProximity); // Pose all of the frames to the specified poses in their parent frame. FramePoseVector poses; for (int f = 0; f < static_cast(frames_.size()); ++f) { - poses.set_value(frames_[f], X_PF_[f]); + poses.set_value(frames_[f], X_PFs_[f]); } gs_tester_.SetFramePoses(source_id_, poses); gs_tester_.FinalizePoseUpdate(); @@ -1906,12 +1927,12 @@ TEST_F(GeometryStateTest, ExcludeCollisionsWithin) { // Test disallowing collision between members fo two groups. TEST_F(GeometryStateTest, ExcludeCollisionsBetween) { - SetUpSingleSourceTree(true /* add proximity roles */); + SetUpSingleSourceTree(Assign::kProximity); // Pose all of the frames to the specified poses in their parent frame. FramePoseVector poses; for (int f = 0; f < static_cast(frames_.size()); ++f) { - poses.set_value(frames_[f], X_PF_[f]); + poses.set_value(frames_[f], X_PFs_[f]); } gs_tester_.SetFramePoses(source_id_, poses); gs_tester_.FinalizePoseUpdate(); @@ -1945,12 +1966,12 @@ TEST_F(GeometryStateTest, ExcludeCollisionsBetween) { // Test collision filtering configuration when the input GeometrySet includes // geometries that *do not* have a proximity role. TEST_F(GeometryStateTest, NonProximityRoleInCollisionFilter) { - SetUpSingleSourceTree(true /* add proximity roles */); + SetUpSingleSourceTree(Assign::kProximity); // Pose all of the frames to the specified poses in their parent frame. FramePoseVector poses; for (int f = 0; f < static_cast(frames_.size()); ++f) { - poses.set_value(frames_[f], X_PF_[f]); + poses.set_value(frames_[f], X_PFs_[f]); } gs_tester_.SetFramePoses(source_id_, poses); gs_tester_.FinalizePoseUpdate(); @@ -1973,12 +1994,12 @@ TEST_F(GeometryStateTest, NonProximityRoleInCollisionFilter) { // between the new geometry and the previous geometries should be // automatically filtered because they are rigidly affixed to the same // frame. - Isometry3 pose = Isometry3::Identity(); + // Documentation on the single source tree indicates that the previous spheres // are at (5, 0, 0) and (6, 0, 0), respectively. Split the distance to put the // new geometry in a penetrating configuration. - pose.translation() << 5.5, 0, 0; - const std::string name("added_sphere"); + Isometry3d pose{Translation3d{5.5, 0, 0}}; + const string name("added_sphere"); GeometryId added_id = geometry_state_.RegisterGeometry( source_id_, frames_[2], make_unique(pose, make_unique(1), name)); @@ -2016,13 +2037,13 @@ TEST_F(GeometryStateTest, SelfCollisionFilterExceptions) { // NOTE: a collision group with a single frame or geometry doesn't exercise // self-collision filtering logic. - GeometrySet set_bad_frame{FrameId::get_new_id(), FrameId::get_new_id()}; + const GeometrySet set_bad_frame{FrameId::get_new_id(), FrameId::get_new_id()}; DRAKE_EXPECT_THROWS_MESSAGE( geometry_state_.ExcludeCollisionsWithin(set_bad_frame), std::logic_error, "Referenced frame \\d+ has not been registered."); - GeometrySet set_bad_geometry{GeometryId::get_new_id(), - GeometryId::get_new_id()}; + const GeometrySet set_bad_geometry{GeometryId::get_new_id(), + GeometryId::get_new_id()}; DRAKE_EXPECT_THROWS_MESSAGE( geometry_state_.ExcludeCollisionsWithin(set_bad_geometry), std::logic_error, @@ -2034,8 +2055,8 @@ TEST_F(GeometryStateTest, SelfCollisionFilterExceptions) { TEST_F(GeometryStateTest, CrossCollisionFilterExceptions) { SetUpSingleSourceTree(); - GeometrySet set_bad_frame{FrameId::get_new_id()}; - GeometrySet set_good_frame{frames_[0]}; + const GeometrySet set_bad_frame{FrameId::get_new_id()}; + const GeometrySet set_good_frame{frames_[0]}; DRAKE_EXPECT_THROWS_MESSAGE( geometry_state_.ExcludeCollisionsBetween(set_bad_frame, set_good_frame), @@ -2047,8 +2068,8 @@ TEST_F(GeometryStateTest, CrossCollisionFilterExceptions) { std::logic_error, "Referenced frame \\d+ has not been registered."); - GeometrySet set_bad_geometry{GeometryId::get_new_id()}; - GeometrySet set_good_geometry{geometries_[0]}; + const GeometrySet set_bad_geometry{GeometryId::get_new_id()}; + const GeometrySet set_good_geometry{geometries_[0]}; DRAKE_EXPECT_THROWS_MESSAGE( geometry_state_.ExcludeCollisionsBetween(set_bad_geometry, set_good_geometry), @@ -2098,7 +2119,7 @@ TEST_F(GeometryStateTest, CollisionFilteredExceptions) { EXPECT_TRUE( geometry_state_.CollisionFiltered(geometries_[0], geometries_[1])); - GeometryId bad_id = GeometryId::get_new_id(); + const GeometryId bad_id = GeometryId::get_new_id(); // Case: First argument is bad. DRAKE_EXPECT_THROWS_MESSAGE( @@ -2123,12 +2144,13 @@ TEST_F(GeometryStateTest, CollisionFilteredExceptions) { // Tests the ability to query for a geometry from the name of a geometry. TEST_F(GeometryStateTest, GetGeometryIdFromName) { - SetUpSingleSourceTree(true /* initialize with proximity role */); + SetUpSingleSourceTree(Assign::kProximity); + // Frame i has geometries f * kFrameCount + g, where g ∈ [0, kGeometryCount). for (int f = 0; f < kFrameCount; ++f) { for (int g = 0; g < kGeometryCount; ++g) { int g_index = f * kGeometryCount + g; - GeometryId expected_id = geometries_[g_index]; + const GeometryId expected_id = geometries_[g_index]; // Look up with the canonical name. EXPECT_EQ(geometry_state_.GetGeometryFromName(frames_[f], Role::kProximity, @@ -2174,13 +2196,12 @@ TEST_F(GeometryStateTest, GetGeometryIdFromName) { // Multiple unassigned geometries with the same name. - const std::string dummy_name("duplicate"); + const string dummy_name("duplicate"); for (int i = 0; i < 2; ++i) { - const Isometry3 pose = Isometry3::Identity(); geometry_state_.RegisterGeometry( source_id_, frames_[0], - make_unique(pose, make_unique(1), - dummy_name)); + make_unique(Isometry3d::Identity(), + make_unique(1), dummy_name)); } DRAKE_EXPECT_THROWS_MESSAGE( geometry_state_.GetGeometryFromName(frames_[0], Role::kUnassigned, @@ -2195,32 +2216,31 @@ TEST_F(GeometryStateTest, GetGeometryIdFromName) { TEST_F(GeometryStateTest, GeometryNameStorage) { SetUpSingleSourceTree(); - const Isometry3d X_FG = Isometry3d::Identity(); - const std::string name = "unique test name"; + const string name = "unique test name"; // White space trimmed off and new string stored. { - GeometryId id = geometry_state_.RegisterGeometry( + const GeometryId id = geometry_state_.RegisterGeometry( source_id_, frames_[0], make_unique( - X_FG, make_unique(1), " " + name)); + Isometry3d::Identity(), make_unique(1), " " + name)); EXPECT_EQ(geometry_state_.get_name(id), name); } // Valid name that is unchanged after trimming is stored as is. // Note: This assigns a geometry fo the *same* name to a *different* frame. { - GeometryId id = geometry_state_.RegisterGeometry( + const GeometryId id = geometry_state_.RegisterGeometry( source_id_, frames_[1], make_unique( - X_FG, make_unique(1), name)); + Isometry3d::Identity(), make_unique(1), name)); EXPECT_EQ(geometry_state_.get_name(id), name); } } // Tests the logic for confirming if a name is valid or not. TEST_F(GeometryStateTest, GeometryNameValidation) { - SetUpSingleSourceTree(true /* Assign proximity roles */); + SetUpSingleSourceTree(Assign::kProximity); // Case: Invalid frame should throw (regardless of name contents or role). DRAKE_EXPECT_THROWS_MESSAGE( @@ -2228,9 +2248,9 @@ TEST_F(GeometryStateTest, GeometryNameValidation) { Role::kProximity, ""), std::logic_error, "Given frame id is not valid: \\d+"); - auto expect_bad_name = [this](const std::string& name, - const std::string& exception_message, - const std::string& printable_name) { + auto expect_bad_name = [this](const string& name, + const string& exception_message, + const string& printable_name) { EXPECT_FALSE(geometry_state_.IsValidGeometryName(frames_[0], Role::kProximity, name)) << "Failed on input name: " << printable_name; @@ -2250,19 +2270,19 @@ TEST_F(GeometryStateTest, GeometryNameValidation) { expect_bad_name("", "The proposed geometry name is empty", ""); // Nothing but whitespace. - const std::string whitespace_message{ + const string whitespace_message{ "The proposed geometry name contains only whitespace"}; expect_bad_name(" ", whitespace_message, "' '"); expect_bad_name("\t", whitespace_message, "'\\t'"); expect_bad_name(" \t", whitespace_message, "' \\t'"); // Case: Valid (as a control case). - const std::string unique = "unique"; + const string unique = "unique"; EXPECT_TRUE(geometry_state_.IsValidGeometryName(frames_[0], Role::kProximity, unique)); // Querying with non-canonical names test as the canonical name. - vector names{" " + unique, unique + " ", " " + unique + " "}; + const vector names{" " + unique, unique + " ", " " + unique + " "}; for (const auto& name : names) { EXPECT_TRUE(geometry_state_.IsValidGeometryName(frames_[0], Role::kProximity, name)); @@ -2283,7 +2303,7 @@ TEST_F(GeometryStateTest, GeometryNameValidation) { // Case: Whitespace that SDF nevertheless considers not whitespace. // Update this when the following sdformat issue is resolved: // https://bitbucket.org/osrf/sdformat/issues/194/string-trimming-only-considers-space-and - for (const std::string& s : {"\n", " \n\t", " \f", "\v", "\r", "\ntest"}) { + for (const string& s : {"\n", " \n\t", " \f", "\v", "\r", "\ntest"}) { EXPECT_TRUE(geometry_state_.IsValidGeometryName(frames_[0], Role::kProximity, s)); } @@ -2296,9 +2316,9 @@ TEST_F(GeometryStateTest, AssignRolesToGeometry) { // We need at least 8 geometries to run through all role permutations. Add // geometries until we're there. - const Isometry3 pose = Isometry3::Identity(); + const Isometry3d pose = Isometry3d::Identity(); for (int i = 0; i < 8 - single_tree_dynamic_geometry_count(); ++i) { - const std::string name = "new_geom" + std::to_string(i); + const string name = "new_geom" + std::to_string(i); geometries_.push_back(geometry_state_.RegisterGeometry( source_id_, frames_[0], make_unique(pose, make_unique(1), name))); @@ -2317,20 +2337,19 @@ TEST_F(GeometryStateTest, AssignRolesToGeometry) { auto has_expected_roles = [this]( GeometryId id, bool has_proximity, bool has_illustration) -> ::testing::AssertionResult { - const internal::InternalGeometry* geometry = - gs_tester_.GetGeometry(id); + const InternalGeometry* geometry = gs_tester_.GetGeometry(id); bool passes = true; ::testing::AssertionResult failure = ::testing::AssertionFailure(); if (has_proximity != (geometry->proximity_properties() != nullptr)) { failure << "Proximity role: " - << (has_proximity ? " expected, but not found" - : " not expected, but found. "); + << (has_proximity ? "expected, but not found" + : "not expected, but found. "); passes = false; } if (has_illustration != (geometry->illustration_properties() != nullptr)) { failure << "Illustration role: " - << (has_illustration ? " expected, but not found" - : " not expected, but found. "); + << (has_illustration ? "expected, but not found" + : "not expected, but found. "); passes = false; } if (passes) @@ -2363,20 +2382,20 @@ TEST_F(GeometryStateTest, AssignRolesToGeometry) { // Tests that properties assigned to a geometry instance lead to the resulting // geometry having the appropriate roles assigned. TEST_F(GeometryStateTest, InstanceRoleAssignment) { - SourceId s_id = NewSource(); - FrameId f_id = geometry_state_.RegisterFrame( - s_id, GeometryFrame("frame")); + const SourceId s_id = NewSource(); + const FrameId f_id = + geometry_state_.RegisterFrame(s_id, GeometryFrame("frame")); - auto make_instance = [this](const std::string& name) { + auto make_instance = [this](const string& name) { return make_unique( instance_pose_, make_unique(1.0), name); }; // Case: no properties assigned leaves geometry with no roles. { - GeometryId g_id = geometry_state_.RegisterGeometry( + const GeometryId g_id = geometry_state_.RegisterGeometry( s_id, f_id, make_instance("instance1")); - const internal::InternalGeometry* geometry = gs_tester_.GetGeometry(g_id); + const InternalGeometry* geometry = gs_tester_.GetGeometry(g_id); EXPECT_FALSE(geometry->has_proximity_role()); EXPECT_FALSE(geometry->has_illustration_role()); } @@ -2385,10 +2404,10 @@ TEST_F(GeometryStateTest, InstanceRoleAssignment) { { auto instance = make_instance("instance2"); instance->set_proximity_properties(ProximityProperties()); - GeometryId g_id = + const GeometryId g_id = geometry_state_.RegisterGeometry(s_id, f_id, move(instance)); - const internal::InternalGeometry* geometry = gs_tester_.GetGeometry(g_id); + const InternalGeometry* geometry = gs_tester_.GetGeometry(g_id); EXPECT_TRUE(geometry->has_proximity_role()); EXPECT_FALSE(geometry->has_illustration_role()); } @@ -2397,10 +2416,10 @@ TEST_F(GeometryStateTest, InstanceRoleAssignment) { { auto instance = make_instance("instance3"); instance->set_illustration_properties(IllustrationProperties()); - GeometryId g_id = + const GeometryId g_id = geometry_state_.RegisterGeometry(s_id, f_id, move(instance)); - const internal::InternalGeometry* geometry = gs_tester_.GetGeometry(g_id); + const InternalGeometry* geometry = gs_tester_.GetGeometry(g_id); EXPECT_FALSE(geometry->has_proximity_role()); EXPECT_TRUE(geometry->has_illustration_role()); } @@ -2410,10 +2429,10 @@ TEST_F(GeometryStateTest, InstanceRoleAssignment) { auto instance = make_instance("instance4"); instance->set_proximity_properties(ProximityProperties()); instance->set_illustration_properties(IllustrationProperties()); - GeometryId g_id = + const GeometryId g_id = geometry_state_.RegisterGeometry(s_id, f_id, move(instance)); - const internal::InternalGeometry* geometry = gs_tester_.GetGeometry(g_id); + const InternalGeometry* geometry = gs_tester_.GetGeometry(g_id); EXPECT_TRUE(geometry->has_proximity_role()); EXPECT_TRUE(geometry->has_illustration_role()); } @@ -2427,10 +2446,10 @@ TEST_F(GeometryStateTest, RolePropertyValueAssignment) { // other role property types. ProximityProperties source; - const std::string& default_group = source.default_group_name(); + const string& default_group = source.default_group_name(); source.AddProperty(default_group, "prop1", 7); source.AddProperty(default_group, "prop2", 10); - const std::string group1("group1"); + const string group1("group1"); source.AddProperty(group1, "propA", 7.5); source.AddProperty(group1, "propB", "test"); @@ -2446,7 +2465,7 @@ TEST_F(GeometryStateTest, RolePropertyValueAssignment) { // Utility for counting properties in a group. auto num_group_properties = [](const ProximityProperties& properties, - const std::string& group_name) { + const string& group_name) { const auto& group = properties.GetPropertiesInGroup(group_name); return static_cast(group.size()); }; @@ -2475,8 +2494,8 @@ TEST_F(GeometryStateTest, RolePropertyValueAssignment) { num_group_properties(*read, group1)); EXPECT_EQ(source.GetProperty(group1, "propA"), read->GetProperty(group1, "propA")); - EXPECT_EQ(source.GetProperty(group1, "propB"), - read->GetProperty(group1, "propB")); + EXPECT_EQ(source.GetProperty(group1, "propB"), + read->GetProperty(group1, "propB")); } // Tests the conditions in which `AssignRole()` throws an exception. @@ -2503,7 +2522,7 @@ TEST_F(GeometryStateTest, RoleAssignExceptions) { "Referenced geometry \\d+ has not been registered."); // Geometry not owned by source. - SourceId other_source = geometry_state_.RegisterNewSource("alt_source"); + const SourceId other_source = geometry_state_.RegisterNewSource("alt_source"); DRAKE_EXPECT_THROWS_MESSAGE( geometry_state_.AssignRole(other_source, geometries_[0], ProximityProperties()), @@ -2530,11 +2549,10 @@ TEST_F(GeometryStateTest, RoleAssignExceptions) { // Addition of geometry with duplicate name -- no problem. Assigning it a // duplicate role -- bad. - const Isometry3 pose = Isometry3::Identity(); - GeometryId new_id = geometry_state_.RegisterGeometry( + const GeometryId new_id = geometry_state_.RegisterGeometry( source_id_, frames_[0], - make_unique(pose, make_unique(1), - geometry_names_[0])); + make_unique( + Isometry3d::Identity(), make_unique(1), geometry_names_[0])); DRAKE_EXPECT_THROWS_MESSAGE( geometry_state_.AssignRole(source_id_, new_id, ProximityProperties()), std::logic_error, @@ -2548,24 +2566,6 @@ TEST_F(GeometryStateTest, RoleAssignExceptions) { "role."); } -// Confirms that assigning a proximity role to a mesh is a no-op. If it -// *weren't* no-op, the ProximityEngine would abort; so not aborting is -// correlated with its no-op-ness. This test will go away when meshes are fully -// supported in collision. -TEST_F(GeometryStateTest, ProximityRoleOnMesh) { - SetUpSingleSourceTree(); - - // Add a mesh to a frame. - GeometryId mesh_id = geometry_state_.RegisterGeometry( - source_id_, frames_[0], - make_unique(Isometry3d::Identity(), - make_unique("path", 1.0), "mesh")); - const InternalGeometry* mesh = gs_tester_.GetGeometry(mesh_id); - ASSERT_FALSE(mesh->has_proximity_role()); - geometry_state_.AssignRole(source_id_, mesh_id, ProximityProperties()); - ASSERT_FALSE(mesh->has_proximity_role()); -} - // Tests the functionality that counts the number of children geometry a frame // has for each role. TEST_F(GeometryStateTest, ChildGeometryRoleCount) { @@ -2577,7 +2577,7 @@ TEST_F(GeometryStateTest, ChildGeometryRoleCount) { int num_illustration) -> ::testing::AssertionResult { bool success = true; ::testing::AssertionResult failure = ::testing::AssertionFailure(); - std::vector> roles{ + vector> roles{ {Role::kProximity, num_proximity}, {Role::kIllustration, num_illustration}}; for (const auto& pair : roles) { @@ -2600,12 +2600,12 @@ TEST_F(GeometryStateTest, ChildGeometryRoleCount) { // Assert initial conditions. int proximity_count = 0; int illustration_count = 0; - FrameId f_id = frames_[0]; + const FrameId f_id = frames_[0]; ASSERT_TRUE(expected_roles(f_id, proximity_count, illustration_count)); // Confirm the two geometries I'm going to play with belong to the same frame. - GeometryId g_id1 = geometries_[0]; - GeometryId g_id2 = geometries_[1]; + const GeometryId g_id1 = geometries_[0]; + const GeometryId g_id2 = geometries_[1]; ASSERT_EQ(f_id, geometry_state_.GetFrameId(g_id1)); ASSERT_EQ(f_id, geometry_state_.GetFrameId(g_id2)); @@ -2626,7 +2626,7 @@ TEST_F(GeometryStateTest, ChildGeometryRoleCount) { ++proximity_count; ASSERT_TRUE(expected_roles(f_id, proximity_count, illustration_count)); // Now test against anchored geometry by passing in the world frame. - FrameId world_id = InternalFrame::world_frame_id(); + const FrameId world_id = InternalFrame::world_frame_id(); ASSERT_TRUE(expected_roles(world_id, 0, 0)); geometry_state_.AssignRole(source_id_, anchored_geometry_, ProximityProperties()); @@ -2636,6 +2636,25 @@ TEST_F(GeometryStateTest, ChildGeometryRoleCount) { ASSERT_TRUE(expected_roles(world_id, 1, 1)); } +// Confirms that assigning a proximity role to a mesh is a no-op. If it +// *weren't* no-op, the ProximityEngine would abort; so not aborting is +// correlated with its no-op-ness. This test will go away when meshes are fully +// supported in collision. +TEST_F(GeometryStateTest, ProximityRoleOnMesh) { + SetUpSingleSourceTree(); + + // Add a mesh to a frame. + const GeometryId mesh_id = geometry_state_.RegisterGeometry( + source_id_, frames_[0], + make_unique(Isometry3d::Identity(), + make_unique("path", 1.0), "mesh")); + const InternalGeometry* mesh = gs_tester_.GetGeometry(mesh_id); + ASSERT_NE(mesh, nullptr); + ASSERT_FALSE(mesh->has_proximity_role()); + geometry_state_.AssignRole(source_id_, mesh_id, ProximityProperties()); + ASSERT_FALSE(mesh->has_proximity_role()); +} + } // namespace } // namespace geometry } // namespace drake