From 7fcdc946afed3e29943ef408881aa769d115b664 Mon Sep 17 00:00:00 2001
From: Steve Peters <scpeters@openrobotics.org>
Date: Thu, 10 Mar 2022 08:09:14 -0800
Subject: [PATCH] Support plotting for entities with / in the name (#3187)

I noticed while testing the rexrov models from the rexrov
model in Field-Robotics-Lab/dave that the plotting window
does not work properly when elements of a model include
a forward-slash `/` in the entity name. To fix the problem,
the `/` characters are encoded as %2f in physics::Base::URI().
Corresponding decoding is added to the gui/plot/Palette class.

* Escape '%' as "%25" as well

This would prevent a string containing "%2f" from
incorrectly being unescaped to "/".

Signed-off-by: Steve Peters <scpeters@openrobotics.org>
---
 gazebo/gui/plot/Palette.cc           |  5 ++
 gazebo/gui/plot/Palette_TEST.cc      | 67 +++++++++++++++++++++
 gazebo/gui/plot/Palette_TEST.hh      |  3 +
 gazebo/physics/Base.cc               | 11 +++-
 test/worlds/names_with_slashes.world | 88 ++++++++++++++++++++++++++++
 5 files changed, 172 insertions(+), 2 deletions(-)
 create mode 100644 test/worlds/names_with_slashes.world

diff --git a/gazebo/gui/plot/Palette.cc b/gazebo/gui/plot/Palette.cc
index 4ab3cf4ba2..7a02848d1d 100644
--- a/gazebo/gui/plot/Palette.cc
+++ b/gazebo/gui/plot/Palette.cc
@@ -870,6 +870,11 @@ void Palette::IntrospectionUpdateSlot(const std::set<std::string> &_items)
 
     // Process path
     auto pathParts = common::split(pathStr, "/");
+    for (auto & part : pathParts)
+    {
+      part = common::replaceAll(part, "%2f", "/");
+      part = common::replaceAll(part, "%25", "%");
+    }
 
     QStandardItem *previousItem = nullptr;
     unsigned int i = 0;
diff --git a/gazebo/gui/plot/Palette_TEST.cc b/gazebo/gui/plot/Palette_TEST.cc
index e303657ef6..22a4ed4d5a 100644
--- a/gazebo/gui/plot/Palette_TEST.cc
+++ b/gazebo/gui/plot/Palette_TEST.cc
@@ -112,5 +112,72 @@ void Palette_TEST::ModelsTab()
   delete palette;
 }
 
+/////////////////////////////////////////////////
+void Palette_TEST::ModelsTabNamesWithSlashes()
+{
+  this->resMaxPercentChange = 5.0;
+  this->shareMaxPercentChange = 2.0;
+
+  this->Load("worlds/names_with_slashes.world");
+
+  // Get the number of models in the world
+  auto world = gazebo::physics::get_world("default");
+  QVERIFY(world != nullptr);
+
+  auto count = world->ModelCount();
+
+  // Create a new plot window widget
+  auto palette = new gazebo::gui::Palette(nullptr);
+  QVERIFY(palette != nullptr);
+
+  // Get the models model
+  auto modelsModel =
+      palette->findChild<QStandardItemModel *>("plotModelsModel");
+  QVERIFY(modelsModel != nullptr);
+
+  // Check the model has as many rows as there are top level models,
+  // plus the title
+  QCOMPARE(modelsModel->rowCount(), static_cast<int>(count + 1));
+
+  auto model = modelsModel->item(1);
+  QCOMPARE(0, model->text().toStdString().compare("double/pendulum_%2f"));
+  QVERIFY(model->hasChildren());
+
+  auto jointsLabel = model->child(3);
+  QVERIFY(!jointsLabel->hasChildren());
+
+  auto joint1 = model->child(4);
+  QCOMPARE(0, joint1->text().toStdString().compare("lower/joint"));
+  QCOMPARE(2, joint1->rowCount());
+
+  auto joint2 = model->child(5);
+  QCOMPARE(0, joint2->text().toStdString().compare("upper/joint"));
+  QCOMPARE(2, joint1->rowCount());
+
+  auto linksLabel = model->child(6);
+  QVERIFY(!linksLabel->hasChildren());
+
+  auto link1 = model->child(7);
+  QCOMPARE(0, link1->text().toStdString().compare("lower/link"));
+  QCOMPARE(3, link1->rowCount());
+
+  auto link2 = model->child(8);
+  QCOMPARE(0, link2->text().toStdString().compare("upper/link"));
+  QCOMPARE(3, link2->rowCount());
+
+  this->ProcessEventsAndDraw();
+
+  // Get the new models model
+  modelsModel =
+      palette->findChild<QStandardItemModel *>("plotModelsModel");
+  QVERIFY(modelsModel != nullptr);
+
+  // Check the model has as many rows as there are top level models,
+  // plus the title
+  QCOMPARE(modelsModel->rowCount(), static_cast<int>(count + 1));
+
+  delete palette;
+}
+
 // Generate a main function for the test
 QTEST_MAIN(Palette_TEST)
diff --git a/gazebo/gui/plot/Palette_TEST.hh b/gazebo/gui/plot/Palette_TEST.hh
index 9593c78c37..55a088405a 100644
--- a/gazebo/gui/plot/Palette_TEST.hh
+++ b/gazebo/gui/plot/Palette_TEST.hh
@@ -30,5 +30,8 @@ class Palette_TEST : public QTestFixture
 
   /// \brief Test the models tab.
   private slots: void ModelsTab();
+
+  /// \brief test that models with slashes in name are handled.
+  private slots: void ModelsTabNamesWithSlashes();
 };
 #endif
diff --git a/gazebo/physics/Base.cc b/gazebo/physics/Base.cc
index 2abd43d0d2..b13b90f5b6 100644
--- a/gazebo/physics/Base.cc
+++ b/gazebo/physics/Base.cc
@@ -15,6 +15,7 @@
  *
 */
 #include "gazebo/common/Assert.hh"
+#include "gazebo/common/CommonIface.hh"
 #include "gazebo/common/Console.hh"
 #include "gazebo/common/Exception.hh"
 #include "gazebo/common/SdfFrameSemantics.hh"
@@ -383,7 +384,10 @@ common::URI Base::URI() const
   {
     if (p->GetParent())
     {
-      uri.Path().PushFront(p->GetName());
+      std::string escapedParentName = p->GetName();
+      escapedParentName = common::replaceAll(escapedParentName, "%", "%25");
+      escapedParentName = common::replaceAll(escapedParentName, "/", "%2f");
+      uri.Path().PushFront(escapedParentName);
       uri.Path().PushFront(p->TypeStr());
     }
 
@@ -391,7 +395,10 @@ common::URI Base::URI() const
   }
 
   uri.Path().PushBack(this->TypeStr());
-  uri.Path().PushBack(this->GetName());
+  std::string escapedName = this->GetName();
+  escapedName = common::replaceAll(escapedName, "%", "%25");
+  escapedName = common::replaceAll(escapedName, "/", "%2f");
+  uri.Path().PushBack(escapedName);
   uri.Path().PushFront(this->world->Name());
   uri.Path().PushFront("world");
 
diff --git a/test/worlds/names_with_slashes.world b/test/worlds/names_with_slashes.world
new file mode 100644
index 0000000000..32cde37c70
--- /dev/null
+++ b/test/worlds/names_with_slashes.world
@@ -0,0 +1,88 @@
+<?xml version="1.0" ?>
+<sdf version="1.4">
+  <world name="default">
+    <physics type="ode">
+      <gravity>1 0 -9.81</gravity>
+      <max_step_size>0.00101</max_step_size>
+    </physics>
+    <include>
+      <uri>model://sun</uri>
+    </include>
+    <!-- add %2f to ensure it is escaped properly -->
+    <model name="double/pendulum_%2f">
+      <pose>0 0 0.9  0.7853981633974483 0 0</pose>
+      <link name="upper/link">
+        <pose>0 0 -0.05  0 0 0</pose>
+        <inertial>
+          <mass>0.02700000000000001</mass>
+          <inertia>
+            <ixx>2.272500000000001e-05</ixx>
+            <iyy>2.272500000000001e-05</iyy>
+            <izz>4.5000000000000035e-07</izz>
+            <ixy>0.0</ixy>
+            <ixz>0.0</ixz>
+            <iyz>0.0</iyz>
+          </inertia>
+        </inertial>
+        <collision name="collision">
+          <geometry>
+            <box>
+              <size>0.010000000000000002 0.010000000000000002 0.1</size>
+            </box>
+          </geometry>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <box>
+              <size>0.010000000000000002 0.010000000000000002 0.1</size>
+            </box>
+          </geometry>
+        </visual>
+      </link>
+      <joint name="upper/joint" type="revolute">
+        <pose>0 0 0.05  0 0 0</pose>
+        <parent>world</parent>
+        <child>upper/link</child>
+        <axis>
+          <xyz>1 0 0</xyz>
+        </axis>
+      </joint>
+      <link name="lower/link">
+        <pose>0 0.17677669529663687 -0.2767766952966369  0.7853981633974483 0 0</pose>
+        <inertial>
+          <mass>3.375</mass>
+          <inertia>
+            <ixx>0.071015625</ixx>
+            <iyy>0.071015625</iyy>
+            <izz>0.0014062500000000004</izz>
+            <ixy>0.0</ixy>
+            <ixz>0.0</ixz>
+            <iyz>0.0</iyz>
+          </inertia>
+        </inertial>
+        <collision name="collision">
+          <geometry>
+            <box>
+              <size>0.05 0.05 0.5</size>
+            </box>
+          </geometry>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <box>
+              <size>0.05 0.05 0.5</size>
+            </box>
+          </geometry>
+        </visual>
+      </link>
+      <joint name="lower/joint" type="revolute">
+        <pose>0 0 0.25  0 0 0</pose>
+        <parent>upper/link</parent>
+        <child>lower/link</child>
+        <axis>
+          <xyz>1 0 0</xyz>
+        </axis>
+      </joint>
+    </model>
+  </world>
+</sdf>