diff --git a/clearpath_config/common/types/accessory.py b/clearpath_config/common/types/accessory.py index 0bd60cb2..24a22c49 100644 --- a/clearpath_config/common/types/accessory.py +++ b/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(): """ diff --git a/clearpath_config/links/types/link.py b/clearpath_config/links/types/link.py index 0e4cffba..feb410fd 100644 --- a/clearpath_config/links/types/link.py +++ b/clearpath_config/links/types/link.py @@ -59,21 +59,27 @@ 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 = {} @@ -81,6 +87,8 @@ def to_dict(self) -> dict: 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