From ed1b9e8effd0adbe770c3c73874e500bd71d5e14 Mon Sep 17 00:00:00 2001 From: Jeremy Nimmer Date: Tue, 8 Feb 2022 09:25:26 -0800 Subject: [PATCH] fixup! [geometry] Add support for SceneGraph --- geometry/test/scene_graph_test.cc | 98 +++++++++++++++---------------- 1 file changed, 47 insertions(+), 51 deletions(-) diff --git a/geometry/test/scene_graph_test.cc b/geometry/test/scene_graph_test.cc index e544540c6e73..2d09d7d1dba2 100644 --- a/geometry/test/scene_graph_test.cc +++ b/geometry/test/scene_graph_test.cc @@ -285,85 +285,81 @@ TEST_F(SceneGraphTest, FullPoseUpdateAnchoredOnly) { SceneGraphTester::FullPoseUpdate(scene_graph_, *context_)); } +template +class TypedSceneGraphTest : public SceneGraphTest { + public: + TypedSceneGraphTest() = default; +}; + +TYPED_TEST_SUITE_P(TypedSceneGraphTest); + // Tests operations on a transmogrified SceneGraph. Whether a context has been // allocated or not, subsequent operations should be allowed. -template -void TransmogrifyWithoutAllocation(SceneGraph* scene_graph) { - SourceId s_id = scene_graph->RegisterSource(); +TYPED_TEST_P(TypedSceneGraphTest, TransmogrifyWithoutAllocation) { + using U = TypeParam; + SourceId s_id = this->scene_graph_.RegisterSource(); // This should allow additional geometry registration. - std::unique_ptr> system_T = scene_graph->ToScalarType(); - SceneGraph& scene_graph_T = *dynamic_cast*>(system_T.get()); + auto scene_graph_U = System::ToScalarType(this->scene_graph_); DRAKE_EXPECT_NO_THROW( - scene_graph_T.RegisterAnchoredGeometry(s_id, make_sphere_instance())); + scene_graph_U->RegisterAnchoredGeometry(s_id, make_sphere_instance())); // After allocation, registration should _still_ be valid. - scene_graph->CreateDefaultContext(); - system_T = scene_graph->ToScalarType(); - SceneGraph& scene_graph_T2 = *dynamic_cast*>(system_T.get()); + this->CreateDefaultContext(); + auto scene_graph_U2 = System::ToScalarType(this->scene_graph_); DRAKE_EXPECT_NO_THROW( - scene_graph_T2.RegisterAnchoredGeometry(s_id, make_sphere_instance())); -} - -TEST_F(SceneGraphTest, TransmogrifyWithoutAllocation) { - TransmogrifyWithoutAllocation(&scene_graph_); - TransmogrifyWithoutAllocation(&scene_graph_); -} - -template -void TransmogrifyPorts(SceneGraph* scene_graph) { - SourceId s_id = scene_graph->RegisterSource(); - scene_graph->CreateDefaultContext(); - std::unique_ptr> system_T = - scene_graph->ToScalarType(); - SceneGraph& scene_graph_T = *dynamic_cast*>(system_T.get()); - EXPECT_EQ(scene_graph_T.num_input_ports(), scene_graph->num_input_ports()); - EXPECT_EQ(scene_graph_T.get_source_pose_port(s_id).get_index(), - scene_graph->get_source_pose_port(s_id).get_index()); - EXPECT_NO_THROW(scene_graph_T.CreateDefaultContext()); + scene_graph_U2->RegisterAnchoredGeometry(s_id, make_sphere_instance())); } // Tests that the ports are correctly mapped. -TEST_F(SceneGraphTest, TransmogrifyPorts) { - TransmogrifyPorts(&scene_graph_); - TransmogrifyPorts(&scene_graph_); +TYPED_TEST_P(TypedSceneGraphTest, TransmogrifyPorts) { + using U = TypeParam; + SourceId s_id = this->scene_graph_.RegisterSource(); + this->CreateDefaultContext(); + auto scene_graph_U = System::ToScalarType(this->scene_graph_); + EXPECT_EQ(scene_graph_U->num_input_ports(), + this->scene_graph_.num_input_ports()); + EXPECT_EQ(scene_graph_U->get_source_pose_port(s_id).get_index(), + this->scene_graph_.get_source_pose_port(s_id).get_index()); + EXPECT_NO_THROW(scene_graph_U->CreateDefaultContext()); } // Tests that the work to "set" the context values for the transmogrified system // behaves correctly. -template -void TransmogrifyContext() { - SceneGraph sg; - SourceId s_id = sg.RegisterSource(); +TYPED_TEST_P(TypedSceneGraphTest, TransmogrifyContext) { + using U = TypeParam; + SourceId s_id = this->scene_graph_.RegisterSource(); // Register geometry that should be successfully transmogrified. - GeometryId g_id = sg.RegisterAnchoredGeometry(s_id, make_sphere_instance()); - std::unique_ptr> context = sg.CreateDefaultContext(); + GeometryId g_id = this->scene_graph_.RegisterAnchoredGeometry( + s_id, make_sphere_instance()); + this->CreateDefaultContext(); + const Context& context_T = *this->context_; // This should transmogrify the internal *model*, so when I allocate the // transmogrified context, I should get the "same" values (considering type // change). - std::unique_ptr> system_T = sg.ToScalarType(); - SceneGraph& scene_graph_T = - *dynamic_cast*>(system_T.get()); - std::unique_ptr> context_T = - scene_graph_T.CreateDefaultContext(); + auto scene_graph_U = System::ToScalarType(this->scene_graph_); + std::unique_ptr> context_U = scene_graph_U->CreateDefaultContext(); // Extract the GeometryState and query some invariants on it directly. - const GeometryState& geo_state_T = - SceneGraphTester::GetGeometryState(scene_graph_T, *context_T); + const GeometryState& geo_state_U = + SceneGraphTester::GetGeometryState(*scene_graph_U, *context_U); // If the anchored geometry were not ported over, this would throw an // exception. - EXPECT_TRUE(geo_state_T.BelongsToSource(g_id, s_id)); - EXPECT_THROW(geo_state_T.BelongsToSource(GeometryId::get_new_id(), s_id), + EXPECT_TRUE(geo_state_U.BelongsToSource(g_id, s_id)); + EXPECT_THROW(geo_state_U.BelongsToSource(GeometryId::get_new_id(), s_id), std::logic_error); // Quick reality check that this is still valid although unnecessary vis a // vis the GeometryState. - DRAKE_EXPECT_NO_THROW(context_T->SetTimeStateAndParametersFrom(*context)); + DRAKE_EXPECT_NO_THROW(context_U->SetTimeStateAndParametersFrom(context_T)); } -TEST_F(SceneGraphTest, TransmogrifyContext) { - TransmogrifyContext(); - TransmogrifyContext(); -} +REGISTER_TYPED_TEST_SUITE_P(TypedSceneGraphTest, + TransmogrifyWithoutAllocation, + TransmogrifyPorts, + TransmogrifyContext); + +using NonDoubleScalarTypes = ::testing::Types; +INSTANTIATE_TYPED_TEST_SUITE_P(My, TypedSceneGraphTest, NonDoubleScalarTypes); // Tests the model inspector. Exercises a token piece of functionality. The // inspector is a wrapper on the GeometryState. It is assumed that GeometryState