@@ -114,33 +114,33 @@ TEST(NestedModel, State)
114
114
sdfStr << " <sdf version ='" << SDF_VERSION << " '>"
115
115
<< " <world name='default'>"
116
116
<< " <state world_name='default'>"
117
- << " <model name='model_00'>"
117
+ << " <model_state name='model_00'>"
118
118
<< " <pose>0 0 0.5 0 0 0</pose>"
119
- << " <link name='link_00'>"
119
+ << " <link_state name='link_00'>"
120
120
<< " <pose>0 0 0.5 0 0 0</pose>"
121
121
<< " <velocity>0.001 0 0 0 0 0</velocity>"
122
122
<< " <acceleration>0 0.006121 0 0.012288 0 0.001751</acceleration>"
123
123
<< " <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'>"
126
126
<< " <pose>1 0 0.5 0 0 0</pose>"
127
- << " <link name='link_01'>"
127
+ << " <link_state name='link_01'>"
128
128
<< " <pose>1.25 0 0.5 0 0 0</pose>"
129
129
<< " <velocity>0 -0.001 0 0 0 0</velocity>"
130
130
<< " <acceleration>0 0.000674 0 -0.001268 0 0</acceleration>"
131
131
<< " <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'>"
134
134
<< " <pose>1 1 0.5 0 0 0</pose>"
135
- << " <link name='link_02'>"
135
+ << " <link_state name='link_02'>"
136
136
<< " <pose>1.25 1 0.5 0 0 0</pose>"
137
137
<< " <velocity>0 0 0.001 0 0 0</velocity>"
138
138
<< " <acceleration>0 0 0 0 0 0</acceleration>"
139
139
<< " <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 >"
144
144
<< " </state>"
145
145
<< " </world>"
146
146
<< " </sdf>" ;
@@ -154,21 +154,22 @@ TEST(NestedModel, State)
154
154
sdf::ElementPtr worldElem = sdfParsed->Root ()->GetElement (" world" );
155
155
EXPECT_TRUE (worldElem->HasElement (" state" ));
156
156
sdf::ElementPtr stateElem = worldElem->GetElement (" state" );
157
- EXPECT_TRUE (stateElem->HasElement (" model " ));
157
+ EXPECT_TRUE (stateElem->HasElement (" model_state " ));
158
158
159
- sdf::ElementPtr modelStateElem = stateElem->GetElement (" model " );
159
+ sdf::ElementPtr modelStateElem = stateElem->GetElement (" model_state " );
160
160
161
161
// model sdf
162
162
EXPECT_TRUE (modelStateElem->HasAttribute (" name" ));
163
163
EXPECT_EQ (modelStateElem->Get <std::string>(" name" ), " model_00" );
164
164
EXPECT_TRUE (modelStateElem->HasElement (" pose" ));
165
165
EXPECT_EQ (modelStateElem->Get <gz::math::Pose3d>(" pose" ),
166
166
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" ));
168
169
169
170
// 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 " );
172
173
EXPECT_TRUE (linkStateElem->HasAttribute (" name" ));
173
174
EXPECT_EQ (linkStateElem->Get <std::string>(" name" ), " link_00" );
174
175
EXPECT_TRUE (linkStateElem->HasElement (" pose" ));
@@ -185,20 +186,21 @@ TEST(NestedModel, State)
185
186
gz::math::Pose3d (0 , 0.006121 , 0 , 0 , 0 , 0 ));
186
187
187
188
// nested model sdf
188
- EXPECT_TRUE (modelStateElem->HasElement (" model " ));
189
+ EXPECT_TRUE (modelStateElem->HasElement (" model_state " ));
189
190
sdf::ElementPtr nestedModelStateElem =
190
- modelStateElem->GetElement (" model " );
191
+ modelStateElem->GetElement (" model_state " );
191
192
EXPECT_TRUE (nestedModelStateElem->HasAttribute (" name" ));
192
193
EXPECT_EQ (nestedModelStateElem->Get <std::string>(" name" ), " model_01" );
193
194
EXPECT_TRUE (nestedModelStateElem->HasElement (" pose" ));
194
195
EXPECT_EQ (nestedModelStateElem->Get <gz::math::Pose3d>(" pose" ),
195
196
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" ));
197
199
198
200
// nested model's link sdf
199
- EXPECT_TRUE (nestedModelStateElem->HasElement (" link " ));
201
+ EXPECT_TRUE (nestedModelStateElem->HasElement (" link_state " ));
200
202
sdf::ElementPtr nestedLinkStateElem =
201
- nestedModelStateElem->GetElement (" link " );
203
+ nestedModelStateElem->GetElement (" link_state " );
202
204
EXPECT_TRUE (nestedLinkStateElem->HasAttribute (" name" ));
203
205
EXPECT_EQ (nestedLinkStateElem->Get <std::string>(" name" ), " link_01" );
204
206
EXPECT_TRUE (nestedLinkStateElem->HasElement (" pose" ));
@@ -215,18 +217,19 @@ TEST(NestedModel, State)
215
217
gz::math::Pose3d (0 , 0.000674 , 0 , 0 , 0 , 0 ));
216
218
217
219
// 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 " );
220
222
EXPECT_TRUE (nestedModelStateElem->HasAttribute (" name" ));
221
223
EXPECT_EQ (nestedModelStateElem->Get <std::string>(" name" ), " model_02" );
222
224
EXPECT_TRUE (nestedModelStateElem->HasElement (" pose" ));
223
225
EXPECT_EQ (nestedModelStateElem->Get <gz::math::Pose3d>(" pose" ),
224
226
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" ));
226
229
227
230
// 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 " );
230
233
EXPECT_TRUE (nestedLinkStateElem->HasAttribute (" name" ));
231
234
EXPECT_EQ (nestedLinkStateElem->Get <std::string>(" name" ), " link_02" );
232
235
EXPECT_TRUE (nestedLinkStateElem->HasElement (" pose" ));
0 commit comments