From b923b2d889ca9239558764befda83753da7b3f8c Mon Sep 17 00:00:00 2001
From: Hilary Luo <hluo@clearpathrobotics.com>
Date: Fri, 29 Dec 2023 15:22:25 -0500
Subject: [PATCH] Added package parameter to mesh object so that visual paths
 can be relative to the package location

---
 clearpath_config/links/links.py      |  4 +++-
 clearpath_config/links/types/mesh.py | 29 ++++++++++++++++++++--------
 2 files changed, 24 insertions(+), 9 deletions(-)

diff --git a/clearpath_config/links/links.py b/clearpath_config/links/links.py
index 4923e5b..778598e 100644
--- a/clearpath_config/links/links.py
+++ b/clearpath_config/links/links.py
@@ -497,7 +497,8 @@ def add_mesh(
             mesh: Mesh = None,
             # By Parameters
             name: str = None,
-            visual: float = Mesh.VISUAL,
+            visual: str = Mesh.VISUAL,
+            package: str = Mesh.PACKAGE,
             parent: str = Accessory.PARENT,
             xyz: List[float] = Accessory.XYZ,
             rpy: List[float] = Accessory.RPY,
@@ -511,6 +512,7 @@ def add_mesh(
             mesh = Mesh(
                 name=name,
                 visual=visual,
+                package=package,
                 parent=parent,
                 xyz=xyz,
                 rpy=rpy,
diff --git a/clearpath_config/links/types/mesh.py b/clearpath_config/links/types/mesh.py
index 6707be1..ead6e60 100644
--- a/clearpath_config/links/types/mesh.py
+++ b/clearpath_config/links/types/mesh.py
@@ -25,8 +25,9 @@
 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 # POSSIBILITY OF SUCH DAMAGE.
+import os
+
 from clearpath_config.common.types.accessory import Accessory
-from clearpath_config.common.types.file import File
 from clearpath_config.links.types.link import BaseLink
 from typing import List
 
@@ -34,13 +35,15 @@
 class Mesh(BaseLink):
     LINK_TYPE = "mesh"
     VISUAL = "empty.stl"
+    PACKAGE = ""
     # COLLISION = "empty.stl"
 
     def __init__(
             self,
             name: str,
             parent: str = Accessory.PARENT,
-            visual: float = VISUAL,
+            visual: str = VISUAL,
+            package: str = PACKAGE,
             # collision: float = COLLISION,
             xyz: List[float] = Accessory.XYZ,
             rpy: List[float] = Accessory.RPY,
@@ -55,8 +58,9 @@ def __init__(
             offset_xyz,
             offset_rpy
         )
-        self.visual: File = File(Mesh.VISUAL)
-        self.set_visual(visual)
+
+        self.visual: str = Mesh.VISUAL
+        self.set_visual(visual, package)
 
     def to_dict(self) -> dict:
         d = super().to_dict()
@@ -66,10 +70,19 @@ def to_dict(self) -> dict:
     def from_dict(self, d: dict) -> None:
         super().from_dict(d)
         if 'visual' in d:
-            self.set_visual(d['visual'])
+            if 'package' in d:
+                self.set_visual(d['visual'], d['package'])
+            else:
+                self.set_visual(d['visual'])
 
-    def set_visual(self, visual: str) -> None:
-        self.visual = File(visual)
+    def set_visual(self, visual: str, package: str = "") -> None:
+        visual_temp = os.path.expanduser(visual)
+        visual_temp = os.path.normpath(visual_temp)
+        if package:
+            visual_temp = os.path.join("$(find " + package + ")", visual_temp)
+        else:
+            visual_temp = os.path.abspath(visual_temp)
+        self.visual = visual_temp
 
     def get_visual(self) -> str:
-        return self.visual.get_path()
+        return self.visual