Added package parameter to mesh object so that visual paths can be re…

…lative to the package location
clearpathrobotics · Dec 29, 2023 · cfe02de · cfe02de
1 parent 382219f
commit cfe02de
Show file tree

Hide file tree

Showing 2 changed files with 25 additions and 9 deletions.
diff --git 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
@@ -25,22 +25,25 @@
 # 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
 
 
 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,21 +58,32 @@ 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()
         d['visual'] = self.get_visual()
+        print("Visual is", self.get_visual())
         return d
 
     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