@@ -114,33 +114,33 @@ TEST(NestedModel, State)
114114 sdfStr << " <sdf version ='" << SDF_VERSION << " '>"
115115 << " <world name='default'>"
116116 << " <state world_name='default'>"
117- << " <model name='model_00'>"
117+ << " <model_state name='model_00'>"
118118 << " <pose>0 0 0.5 0 0 0</pose>"
119- << " <link name='link_00'>"
119+ << " <link_state name='link_00'>"
120120 << " <pose>0 0 0.5 0 0 0</pose>"
121121 << " <velocity>0.001 0 0 0 0 0</velocity>"
122122 << " <acceleration>0 0.006121 0 0.012288 0 0.001751</acceleration>"
123123 << " <wrench>0 0.006121 0 0 0 0</wrench>"
124- << " </link >"
125- << " <model name='model_01'>"
124+ << " </link_state >"
125+ << " <model_state name='model_01'>"
126126 << " <pose>1 0 0.5 0 0 0</pose>"
127- << " <link name='link_01'>"
127+ << " <link_state name='link_01'>"
128128 << " <pose>1.25 0 0.5 0 0 0</pose>"
129129 << " <velocity>0 -0.001 0 0 0 0</velocity>"
130130 << " <acceleration>0 0.000674 0 -0.001268 0 0</acceleration>"
131131 << " <wrench>0 0.000674 0 0 0 0</wrench>"
132- << " </link >"
133- << " <model name='model_02'>"
132+ << " </link_state >"
133+ << " <model_state name='model_02'>"
134134 << " <pose>1 1 0.5 0 0 0</pose>"
135- << " <link name='link_02'>"
135+ << " <link_state name='link_02'>"
136136 << " <pose>1.25 1 0.5 0 0 0</pose>"
137137 << " <velocity>0 0 0.001 0 0 0</velocity>"
138138 << " <acceleration>0 0 0 0 0 0</acceleration>"
139139 << " <wrench>0 0 0 0 0 0</wrench>"
140- << " </link >"
141- << " </model >"
142- << " </model >"
143- << " </model >"
140+ << " </link_state >"
141+ << " </model_state >"
142+ << " </model_state >"
143+ << " </model_state >"
144144 << " </state>"
145145 << " </world>"
146146 << " </sdf>" ;
@@ -154,21 +154,22 @@ TEST(NestedModel, State)
154154 sdf::ElementPtr worldElem = sdfParsed->Root ()->GetElement (" world" );
155155 EXPECT_TRUE (worldElem->HasElement (" state" ));
156156 sdf::ElementPtr stateElem = worldElem->GetElement (" state" );
157- EXPECT_TRUE (stateElem->HasElement (" model " ));
157+ EXPECT_TRUE (stateElem->HasElement (" model_state " ));
158158
159- sdf::ElementPtr modelStateElem = stateElem->GetElement (" model " );
159+ sdf::ElementPtr modelStateElem = stateElem->GetElement (" model_state " );
160160
161161 // model sdf
162162 EXPECT_TRUE (modelStateElem->HasAttribute (" name" ));
163163 EXPECT_EQ (modelStateElem->Get <std::string>(" name" ), " model_00" );
164164 EXPECT_TRUE (modelStateElem->HasElement (" pose" ));
165165 EXPECT_EQ (modelStateElem->Get <gz::math::Pose3d>(" pose" ),
166166 gz::math::Pose3d (0 , 0 , 0.5 , 0 , 0 , 0 ));
167- EXPECT_TRUE (!modelStateElem->HasElement (" joint" ));
167+ EXPECT_FALSE (modelStateElem->HasElement (" joint" ));
168+ EXPECT_FALSE (modelStateElem->HasElement (" joint_state" ));
168169
169170 // link sdf
170- EXPECT_TRUE (modelStateElem->HasElement (" link " ));
171- sdf::ElementPtr linkStateElem = modelStateElem->GetElement (" link " );
171+ EXPECT_TRUE (modelStateElem->HasElement (" link_state " ));
172+ sdf::ElementPtr linkStateElem = modelStateElem->GetElement (" link_state " );
172173 EXPECT_TRUE (linkStateElem->HasAttribute (" name" ));
173174 EXPECT_EQ (linkStateElem->Get <std::string>(" name" ), " link_00" );
174175 EXPECT_TRUE (linkStateElem->HasElement (" pose" ));
@@ -185,20 +186,21 @@ TEST(NestedModel, State)
185186 gz::math::Pose3d (0 , 0.006121 , 0 , 0 , 0 , 0 ));
186187
187188 // nested model sdf
188- EXPECT_TRUE (modelStateElem->HasElement (" model " ));
189+ EXPECT_TRUE (modelStateElem->HasElement (" model_state " ));
189190 sdf::ElementPtr nestedModelStateElem =
190- modelStateElem->GetElement (" model " );
191+ modelStateElem->GetElement (" model_state " );
191192 EXPECT_TRUE (nestedModelStateElem->HasAttribute (" name" ));
192193 EXPECT_EQ (nestedModelStateElem->Get <std::string>(" name" ), " model_01" );
193194 EXPECT_TRUE (nestedModelStateElem->HasElement (" pose" ));
194195 EXPECT_EQ (nestedModelStateElem->Get <gz::math::Pose3d>(" pose" ),
195196 gz::math::Pose3d (1 , 0 , 0.5 , 0 , 0 , 0 ));
196- EXPECT_TRUE (!nestedModelStateElem->HasElement (" joint" ));
197+ EXPECT_FALSE (nestedModelStateElem->HasElement (" joint" ));
198+ EXPECT_FALSE (nestedModelStateElem->HasElement (" joint_state" ));
197199
198200 // nested model's link sdf
199- EXPECT_TRUE (nestedModelStateElem->HasElement (" link " ));
201+ EXPECT_TRUE (nestedModelStateElem->HasElement (" link_state " ));
200202 sdf::ElementPtr nestedLinkStateElem =
201- nestedModelStateElem->GetElement (" link " );
203+ nestedModelStateElem->GetElement (" link_state " );
202204 EXPECT_TRUE (nestedLinkStateElem->HasAttribute (" name" ));
203205 EXPECT_EQ (nestedLinkStateElem->Get <std::string>(" name" ), " link_01" );
204206 EXPECT_TRUE (nestedLinkStateElem->HasElement (" pose" ));
@@ -215,18 +217,19 @@ TEST(NestedModel, State)
215217 gz::math::Pose3d (0 , 0.000674 , 0 , 0 , 0 , 0 ));
216218
217219 // double nested model sdf
218- EXPECT_TRUE (nestedModelStateElem->HasElement (" model " ));
219- nestedModelStateElem = nestedModelStateElem->GetElement (" model " );
220+ EXPECT_TRUE (nestedModelStateElem->HasElement (" model_state " ));
221+ nestedModelStateElem = nestedModelStateElem->GetElement (" model_state " );
220222 EXPECT_TRUE (nestedModelStateElem->HasAttribute (" name" ));
221223 EXPECT_EQ (nestedModelStateElem->Get <std::string>(" name" ), " model_02" );
222224 EXPECT_TRUE (nestedModelStateElem->HasElement (" pose" ));
223225 EXPECT_EQ (nestedModelStateElem->Get <gz::math::Pose3d>(" pose" ),
224226 gz::math::Pose3d (1 , 1 , 0.5 , 0 , 0 , 0 ));
225- EXPECT_TRUE (!nestedModelStateElem->HasElement (" joint" ));
227+ EXPECT_FALSE (nestedModelStateElem->HasElement (" joint" ));
228+ EXPECT_FALSE (nestedModelStateElem->HasElement (" joint_state" ));
226229
227230 // double nested model's link sdf
228- EXPECT_TRUE (nestedModelStateElem->HasElement (" link " ));
229- nestedLinkStateElem = nestedModelStateElem->GetElement (" link " );
231+ EXPECT_TRUE (nestedModelStateElem->HasElement (" link_state " ));
232+ nestedLinkStateElem = nestedModelStateElem->GetElement (" link_state " );
230233 EXPECT_TRUE (nestedLinkStateElem->HasAttribute (" name" ));
231234 EXPECT_EQ (nestedLinkStateElem->Get <std::string>(" name" ), " link_02" );
232235 EXPECT_TRUE (nestedLinkStateElem->HasElement (" pose" ));
0 commit comments