Test if joint state is loaded for a model

Because it's currently not loaded.
gazebosim · Jan 8, 2021 · b9d93b1 · b9d93b1
1 parent 29111d9
commit b9d93b1
Show file tree

Hide file tree

Showing 2 changed files with 71 additions and 0 deletions.
diff --git a/test/integration/model.cc b/test/integration/model.cc
@@ -51,6 +51,20 @@ TEST_F(ModelTest, GetLinksV)
   EXPECT_EQ(model->GetLinks().size(), 1u);
 }
 
+TEST_F(ModelTest, LoadJointState)
+{
+  Load("worlds/joint_state_test.world");
+  physics::ModelPtr const model = GetModel("model_3");
+
+  ASSERT_EQ(model->GetJointCount(), 2u);
+
+  physics::JointPtr const joint_1 = model->GetJoint("joint_1");
+  EXPECT_NEAR(joint_1->Position(), 0.7854, 0.0001);
+
+  physics::JointPtr const joint_2 = model->GetJoint("joint_2");
+  EXPECT_NEAR(joint_2->Position(), 0.7854, 0.0001);
+}
+
 /////////////////////////////////////////////////
 // This tests getting the scoped name of a model.
 TEST_F(ModelTest, GetScopedName)

diff --git a/test/worlds/joint_state_test.world b/test/worlds/joint_state_test.world
@@ -0,0 +1,57 @@
+<?xml version="1.0" ?>
+<sdf version="1.7">
+    <world name="default">
+        <!-- A global light source -->
+        <include>
+            <uri>model://sun</uri>
+        </include>
+        <model name="model_3">
+            <pose>0 0 0 0 0 0</pose>
+            <link name="link_1">
+                <visual name="link_1_box">
+                    <pose>0 0.5 0 0 0 0</pose>
+                    <geometry>
+                        <box>
+                            <size>0.125 0.95 0.125</size>
+                        </box>
+                    </geometry>
+                </visual>
+            </link>
+            <link name="link_2">
+                <visual name="link_2_visual">
+                    <pose>0 1.5 0 0 0 0</pose>
+                    <geometry>
+                        <box>
+                            <size>0.125 0.95 0.125</size>
+                        </box>
+                    </geometry>
+                </visual>
+            </link>
+            <joint name="joint_1" type="revolute">
+                <parent>world</parent>
+                <child>link_1</child>
+                <axis>
+                    <xyz>0 0 1</xyz>
+                </axis>
+            </joint>
+            <joint name="joint_2" type="revolute">
+                <parent>link_1</parent>
+                <child>link_2</child>
+                <axis>
+                    <xyz>0 0 1</xyz>
+                </axis>
+                <pose>0 1 0 0 0 0</pose>
+            </joint>
+        </model>
+        <state world_name="default">
+            <model name="model_3">
+                <joint name="joint_1">
+                    <angle axis=0>0.7854</angle>
+                </joint>
+                <joint name="joint_2">
+                    <angle axis=0>0.7854</angle>
+                </joint>
+            </model>
+        </state>
+    </world>
+</sdf>