Add rgba, collision attributes to links

clearpath_config/common/types/accessory.py

@@ -131,6 +131,17 @@ def assert_valid_triplet(tri: List[float], msg: str = None) -> None:
         # Triplet must be all floats
         assert all([isinstance(i, float) for i in tri])  # noqa:C419
 
+    @staticmethod
+    def assert_valid_quadruple(quad: List[float], msg: str = None) -> None:
+        if msg is None:
+            msg = 'Quadruple must be a list of four float values'
+        # Quad must be a list
+        assert isinstance(quad, list), msg
+        # Quad must have a length of 4
+        assert len(quad) == 4, msg
+        # Quad must be all floats
+        assert all([isinstance(i, float) for i in quad])  # noqa:C419
+
     @staticmethod
     def assert_is_supported():
         """

clearpath_config/links/types/link.py

@@ -59,28 +59,36 @@ class BaseLink(Accessory):
     LINK_TYPE = 'base'
     OFFSET_XYZ = [0.0, 0.0, 0.0]
     OFFSET_RPY = [0.0, 0.0, 0.0]
+    RGBA = [0.2, 0.2, 0.2, 1.0]  # clearpath_dark_grey
+    COLLISION_ENABLE = True
 
     def __init__(
-            self,
-            name: str,
-            parent: str = Accessory.PARENT,
-            xyz: List[float] = Accessory.XYZ,
-            rpy: List[float] = Accessory.RPY,
-            offset_xyz: List[float] = OFFSET_XYZ,
-            offset_rpy: List[float] = OFFSET_RPY
-            ) -> None:
+        self,
+        name: str,
+        parent: str = Accessory.PARENT,
+        xyz: List[float] = Accessory.XYZ,
+        rpy: List[float] = Accessory.RPY,
+        offset_xyz: List[float] = OFFSET_XYZ,
+        offset_rpy: List[float] = OFFSET_RPY,
+        rgba: List[float] = RGBA,
+        collision_enable: bool = COLLISION_ENABLE,
+    ) -> None:
         super().__init__(name, parent, xyz, rpy)
         self.offset_xyz: List[float] = BaseLink.OFFSET_XYZ
         self.set_offset_xyz(offset_xyz)
         self.offset_rpy: List[float] = BaseLink.OFFSET_RPY
         self.set_offset_rpy(offset_rpy)
+        self.set_rgba(rgba)
+        self.set_collision_enable(collision_enable)
 
     def to_dict(self) -> dict:
         d = {}
         d['name'] = self.get_name()
         d['parent'] = self.get_parent()
         d['xyz'] = self.get_xyz()
         d['rpy'] = self.get_rpy()
+        d['rgba'] = self.get_rgba()
+        d['collision'] = self.get_collision_enable()
         return d
 
     def from_dict(self, d: dict) -> None:
@@ -92,6 +100,10 @@ def from_dict(self, d: dict) -> None:
             self.set_xyz(d['xyz'])
         if 'rpy' in d:
             self.set_rpy(d['rpy'])
+        if 'rgba' in d:
+            self.set_rgba(d['rgba'])
+        if 'collision' in d:
+            self.set_collision_enable(d['collision'])
 
     @classmethod
     def get_link_type(cls) -> str:
@@ -113,3 +125,19 @@ def set_offset_rpy(self, rpy: List[float]) -> None:
             'Offset RPY must be a list of exactly three float values'
         )
         self.offset_rpy = rpy
+
+    def get_rgba(self) -> List[float]:
+        return self.rgba
+
+    def set_rgba(self, rgba: List[float]) -> None:
+        Accessory.assert_valid_quadruple(
+            rgba,
+            'RGBA must be a list of exactly 4 float values'
+        )
+        self.rgba = rgba
+
+    def get_collision_enable(self) -> bool:
+        return self.collision_enable
+
+    def set_collision_enable(self, enable: bool) -> None:
+        self.collision_enable = enable