From 7521bb031f53fea047b07ced845b53cd0b64aaa4 Mon Sep 17 00:00:00 2001
From: Steve Peters <scpeters@openrobotics.org>
Date: Thu, 29 Oct 2020 23:29:31 -0700
Subject: [PATCH] Prefix nested model names when flattening (#399)

Currently the addNestedModel function in parser.cc prefixes
link, joint, and frame names with the flattened model name
delimited by "::". This applies the same prefix to the names
of nested models within the flattened model as well.

Signed-off-by: Steve Peters <scpeters@openrobotics.org>
---
 src/parser.cc                                               | 3 ++-
 test/integration/model/test_model_with_frames/model.sdf     | 6 ++++++
 test/integration/nested_model_with_frames_expected.sdf      | 6 ++++++
 .../two_level_nested_model_with_frames_expected.sdf         | 6 ++++++
 4 files changed, 20 insertions(+), 1 deletion(-)

diff --git a/src/parser.cc b/src/parser.cc
index eaff2a603..b94d08222 100644
--- a/src/parser.cc
+++ b/src/parser.cc
@@ -1285,6 +1285,7 @@ void addNestedModel(ElementPtr _sdf, ElementPtr _includeSDF, Errors &_errors)
   while (elem)
   {
     if ((elem->GetName() == "link") ||
+        (elem->GetName() == "model") ||
         (elem->GetName() == "joint") ||
         (elem->GetName() == "frame"))
     {
@@ -1293,7 +1294,7 @@ void addNestedModel(ElementPtr _sdf, ElementPtr _includeSDF, Errors &_errors)
       replace[elemName] = newName;
     }
 
-    if ((elem->GetName() == "link"))
+    if ((elem->GetName() == "link") || (elem->GetName() == "model"))
     {
       // Add a pose element even if the element doesn't originally have one
       auto elemPose = elem->GetElement("pose");
diff --git a/test/integration/model/test_model_with_frames/model.sdf b/test/integration/model/test_model_with_frames/model.sdf
index f085ec4b8..b34e0375c 100644
--- a/test/integration/model/test_model_with_frames/model.sdf
+++ b/test/integration/model/test_model_with_frames/model.sdf
@@ -59,5 +59,11 @@
       <parent>L3</parent>
       <child>L4</child>
     </joint>
+    <model name="M2">
+      <pose relative_to="F1">1 0 0 0 0 0</pose>
+      <link name="L5">
+        <pose>1 0 0 0 0 0</pose>
+      </link>
+    </model>
   </model>
 </sdf>
diff --git a/test/integration/nested_model_with_frames_expected.sdf b/test/integration/nested_model_with_frames_expected.sdf
index 6e3f1dcbe..2385f000f 100644
--- a/test/integration/nested_model_with_frames_expected.sdf
+++ b/test/integration/nested_model_with_frames_expected.sdf
@@ -63,6 +63,12 @@
         <parent>M1::L3</parent>
         <child>M1::L4</child>
       </joint>
+      <model name='M1::M2'>
+        <pose relative_to='M1::F1'>1 0 0 0 -0 0</pose>
+        <link name='L5'>
+          <pose>1 0 0 0 -0 0</pose>
+        </link>
+      </model>
     </model>
   </world>
 </sdf>
diff --git a/test/integration/two_level_nested_model_with_frames_expected.sdf b/test/integration/two_level_nested_model_with_frames_expected.sdf
index f96fae7ba..807fa6d6c 100644
--- a/test/integration/two_level_nested_model_with_frames_expected.sdf
+++ b/test/integration/two_level_nested_model_with_frames_expected.sdf
@@ -66,6 +66,12 @@
         <parent>M1::test_model_with_frames::L3</parent>
         <child>M1::test_model_with_frames::L4</child>
       </joint>
+      <model name='M1::test_model_with_frames::M2'>
+        <pose relative_to='M1::test_model_with_frames::F1'>1 0 0 0 -0 0</pose>
+        <link name='L5'>
+          <pose>1 0 0 0 -0 0</pose>
+        </link>
+      </model>
     </model>
   </world>
 </sdf>