From 49de232c10afa76541b03221bd14778cfb29a9d6 Mon Sep 17 00:00:00 2001
From: Steve Peters <scpeters@openrobotics.org>
Date: Thu, 2 Nov 2023 12:57:12 -0700
Subject: [PATCH] Add test to show seg-fault with unsorted links

Test for #562.

Signed-off-by: Steve Peters <scpeters@openrobotics.org>
---
 test/common_test/world_features.cc            | 41 +++++++++++++++++++
 .../worlds/world_unsorted_links.world         | 40 ++++++++++++++++++
 2 files changed, 81 insertions(+)
 create mode 100644 test/common_test/worlds/world_unsorted_links.world

diff --git a/test/common_test/world_features.cc b/test/common_test/world_features.cc
index b4d3b784d..e9aa3436a 100644
--- a/test/common_test/world_features.cc
+++ b/test/common_test/world_features.cc
@@ -179,6 +179,47 @@ TEST_F(WorldFeaturesTestGravity, GravityFeatures)
   }
 }
 
+using ConstructModelFeatures = gz::physics::FeatureList<
+  gz::physics::GetEngineInfo,
+  gz::physics::sdf::ConstructSdfWorld,
+  gz::physics::sdf::ConstructSdfModel,
+  gz::physics::GetModelFromWorld,
+  gz::physics::ForwardStep
+>;
+
+using WorldFeaturesTestConstructModel =
+  WorldFeaturesTest<ConstructModelFeatures>;
+
+/////////////////////////////////////////////////
+TEST_F(WorldFeaturesTestConstructModel, ConstructModelUnsortedLinks)
+{
+  for (const std::string &name : this->pluginNames)
+  {
+    std::cout << "Testing plugin: " << name << std::endl;
+    gz::plugin::PluginPtr plugin = this->loader.Instantiate(name);
+
+    auto engine =
+      gz::physics::RequestEngine3d<ConstructModelFeatures>::From(plugin);
+    ASSERT_NE(nullptr, engine);
+    EXPECT_TRUE(engine->GetName().find(this->PhysicsEngineName(name)) !=
+                std::string::npos);
+
+    sdf::Root root;
+    const sdf::Errors errors = root.Load(
+      gz::common::joinPaths(TEST_WORLD_DIR, "world_unsorted_links.world"));
+    EXPECT_TRUE(errors.empty()) << errors;
+    const sdf::World *sdfWorld = root.WorldByIndex(0);
+    EXPECT_NE(nullptr, sdfWorld);
+
+    // Per https://github.com/gazebosim/gz-physics/pull/562, there is a
+    // seg-fault in the bullet-featherstone implementation of ConstructSdfModel
+    // (called by ConstructSdfWorld). So loading the world successfully
+    // is enough for this test.
+    auto world = engine->ConstructWorld(*root.WorldByIndex(0));
+    EXPECT_NE(nullptr, world);
+  }
+}
+
 using WorldModelFeatureList = gz::physics::FeatureList<
   GravityFeatures,
   gz::physics::WorldModelFeature,
diff --git a/test/common_test/worlds/world_unsorted_links.world b/test/common_test/worlds/world_unsorted_links.world
new file mode 100644
index 000000000..667b62559
--- /dev/null
+++ b/test/common_test/worlds/world_unsorted_links.world
@@ -0,0 +1,40 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+  <world name="default">
+    <plugin
+      filename="gz-sim-physics-system"
+      name="gz::sim::systems::v0::Physics">
+    </plugin>
+    <model name="unsorted_links">
+      <link name="child_linkC" />
+      <link name="parent_link" />
+      <link name="child_linkA" />
+      <link name="child_linkB" />
+      <joint name="world_joint" type="fixed">
+        <parent>world</parent>
+        <child>parent_link</child>
+      </joint>
+      <joint name="joint1" type="prismatic">
+        <parent>parent_link</parent>
+        <child>child_linkA</child>
+        <axis>
+            <xyz>0 0 1</xyz>
+        </axis>
+      </joint>
+      <joint name="joint2" type="prismatic">
+        <parent>child_linkA</parent>
+        <child>child_linkB</child>
+        <axis>
+            <xyz>0 0 1</xyz>
+        </axis>
+      </joint>
+      <joint name="joint3" type="prismatic">
+        <parent>child_linkB</parent>
+        <child>child_linkC</child>
+        <axis>
+            <xyz>0 0 1</xyz>
+        </axis>
+      </joint>
+    </model>
+  </world>
+</sdf>