diff --git a/clearpath_config/clearpath_config.py b/clearpath_config/clearpath_config.py index fa12f40..ba301ce 100644 --- a/clearpath_config/clearpath_config.py +++ b/clearpath_config/clearpath_config.py @@ -178,7 +178,7 @@ def mounts(self) -> MountsConfig: @mounts.setter def mounts(self, config: dict) -> None: - self._sensors.config = config + self._mounts.config = config @property def sensors(self) -> SensorConfig: diff --git a/clearpath_config/mounts/mounts.py b/clearpath_config/mounts/mounts.py index ae5cfae..50e1f96 100644 --- a/clearpath_config/mounts/mounts.py +++ b/clearpath_config/mounts/mounts.py @@ -32,6 +32,8 @@ from clearpath_config.mounts.types.mount import BaseMount from clearpath_config.mounts.types.fath_pivot import FathPivot from clearpath_config.mounts.types.flir_ptu import FlirPTU +from clearpath_config.mounts.types.sick import SICKStand +from clearpath_config.mounts.types.tower import Tower from clearpath_config.mounts.types.pacs import PACS from typing import List @@ -73,15 +75,19 @@ def to_dict(self) -> List[dict]: class MountsConfig(BaseConfig): MOUNTS = "mounts" - BRACKET = "bracket" - FATH_PIVOT = "fath_pivot" - RISER = "riser" + BRACKET = PACS.Bracket.MOUNT_MODEL + FATH_PIVOT = FathPivot.MOUNT_MODEL + RISER = PACS.Riser.MOUNT_MODEL + SICK = SICKStand.MOUNT_MODEL + TOWER = Tower.MOUNT_MODEL TEMPLATE = { MOUNTS: { BRACKET: BRACKET, FATH_PIVOT: FATH_PIVOT, RISER: RISER, + SICK: SICK, + TOWER: TOWER, } } @@ -91,6 +97,8 @@ class MountsConfig(BaseConfig): BRACKET: [], FATH_PIVOT: [], RISER: [], + SICK: [], + TOWER: [], } def __init__( @@ -98,17 +106,23 @@ def __init__( config: dict = {}, bracket: List[PACS.Bracket] = DEFAULTS[BRACKET], fath_pivot: List[FathPivot] = DEFAULTS[FATH_PIVOT], - riser: List[PACS.Riser] = DEFAULTS[RISER] + riser: List[PACS.Riser] = DEFAULTS[RISER], + sick_stand: List[SICKStand] = DEFAULTS[SICK], + tower: List[Tower] = DEFAULTS[TOWER], ) -> None: # Initialization self.bracket = bracket self.fath_pivot = fath_pivot self.riser = riser + self.sick_stand = sick_stand + self.tower = tower # Template template = { self.KEYS[self.BRACKET]: MountsConfig.bracket, self.KEYS[self.FATH_PIVOT]: MountsConfig.fath_pivot, - self.KEYS[self.RISER]: MountsConfig.riser + self.KEYS[self.RISER]: MountsConfig.riser, + self.KEYS[self.SICK]: MountsConfig.sick_stand, + self.KEYS[self.TOWER]: MountsConfig.tower, } super().__init__(template, config, self.MOUNTS) @@ -181,12 +195,60 @@ def fath_pivot(self, value: List[dict]) -> None: mounts.set_all(mount_list) self._fath_pivot = mounts + @property + def sick_stand(self) -> OrderedListConfig: + self.set_config_param( + key=self.KEYS[self.SICK], + value=self._sick.to_dict() + ) + return self._sick + + @sick_stand.setter + def sick_stand(self, value: List[dict]) -> None: + assert isinstance(value, list), ( + "Mounts must be list of 'dict'") + assert all([isinstance(i, dict) for i in value]), ( + "Mounts must be list of 'dict'") + mounts = MountListConfig() + mount_list = [] + for d in value: + mount = SICKStand() + mount.from_dict(d) + mount_list.append(mount) + mounts.set_all(mount_list) + self._sick = mounts + + @property + def tower(self) -> OrderedListConfig: + self.set_config_param( + key=self.KEYS[self.TOWER], + value=self._tower.to_dict() + ) + return self._tower + + @tower.setter + def tower(self, value: List[dict]) -> None: + assert isinstance(value, list), ( + "Mounts must be list of 'dict'") + assert all([isinstance(i, dict) for i in value]), ( + "Mounts must be list of 'dict'") + mounts = MountListConfig() + mount_list = [] + for d in value: + mount = Tower() + mount.from_dict(d) + mount_list.append(mount) + mounts.set_all(mount_list) + self._tower = mounts + # Get All Mounts def get_all_mounts(self) -> List[BaseMount]: mounts = [] mounts.extend(self.get_fath_pivots()) mounts.extend(self.get_risers()) mounts.extend(self.get_brackets()) + mounts.extend(self.get_sick_stands()) + mounts.extend(self.get_towers()) return mounts # FathPivot: Add @@ -362,3 +424,113 @@ def set_brackets( brackets: List[PACS.Bracket], ) -> None: self._bracket.set_all(brackets) + + # Sick Stand: Add + def add_sick_stand( + self, + # By Object + sick_stand: SICKStand = None, + # By Parameters + parent: str = "base_link", + model: str = SICKStand.INVERTED, + xyz: List[float] = [0.0, 0.0, 0.0], + rpy: List[float] = [0.0, 0.0, 0.0] + ) -> None: + if not sick_stand: + sick_stand = SICKStand( + parent=parent, + model=model, + xyz=xyz, + rpy=rpy + ) + self._sick.add(sick_stand) + + # Sick Stand: Remove + def remove_sick_stand( + self, + # By Object or Name + sick_stand: SICKStand | int, + ) -> None: + self._sick.remove(sick_stand) + + # Sick Stand: Get + def get_sick_stand( + self, + idx: int, + ) -> SICKStand: + return self._sick.get(idx) + + # Sick Stand: Get All + def get_sick_stands( + self, + ) -> List[SICKStand]: + return self._sick.get_all() + + # Sick Stand: Set + def set_sick_stand( + self, + sick_stand: SICKStand, + ) -> None: + self._sick.set(sick_stand) + + # Sick Stand: Set All + def set_sick_stands( + self, + sick_stands: List[SICKStand], + ) -> None: + self._sick.set_all(sick_stands) + + # Tower: Add + def add_tower( + self, + # By Object + tower: Tower = None, + # By Parameters + parent: str = "base_link", + model: str = Tower.SINGLE, + xyz: List[float] = [0.0, 0.0, 0.0], + rpy: List[float] = [0.0, 0.0, 0.0] + ) -> None: + if not tower: + tower = Tower( + parent=parent, + model=model, + xyz=xyz, + rpy=rpy + ) + self._tower.add(tower) + + # Tower: Remove + def remove_tower( + self, + # By Object or Name + tower: Tower | int, + ) -> None: + self._tower.remove(tower) + + # Tower: Get + def get_tower( + self, + idx: int, + ) -> Tower: + return self._tower.get(idx) + + # Tower: Get All + def get_towers( + self, + ) -> List[Tower]: + return self._tower.get_all() + + # Tower: Set + def set_tower( + self, + tower: Tower, + ) -> None: + self._tower.set(tower) + + # Tower: Set All + def set_towers( + self, + towers: List[Tower], + ) -> None: + self._tower.set_all(towers) diff --git a/clearpath_config/mounts/types/sick.py b/clearpath_config/mounts/types/sick.py new file mode 100644 index 0000000..6edff7b --- /dev/null +++ b/clearpath_config/mounts/types/sick.py @@ -0,0 +1,69 @@ +# Software License Agreement (BSD) +# +# @author Luis Camero +# @copyright (c) 2023, Clearpath Robotics, Inc., All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# * Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# * Neither the name of Clearpath Robotics nor the names of its contributors +# may be used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# 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. +from clearpath_config.common.types.accessory import Accessory +from clearpath_config.mounts.types.mount import BaseMount +from typing import List + + +class SICKStand(BaseMount): + MOUNT_MODEL = "sick" + UPRIGHT = "upright" + INVERTED = "inverted" + MODELS = [UPRIGHT, INVERTED] + + def __init__( + self, + idx: int = None, + name: str = None, + model: str = INVERTED, + parent: str = Accessory.PARENT, + xyz: List[float] = Accessory.XYZ, + rpy: List[float] = Accessory.RPY + ) -> None: + self.model = model + super().__init__(idx, name, parent, xyz, rpy) + + def to_dict(self) -> dict: + d = super().to_dict() + d['model'] = self.get_model() + return d + + def from_dict(self, d: dict) -> None: + super().from_dict(d) + if 'model' in d: + self.set_model(d['model']) + + def get_model(self) -> str: + return self.model + + def set_model(self, model: str) -> None: + assert model in self.MODELS, " ".join([ + "Unexpected SICK Stand model '%s'," % model, + "it must be one of the following: %s" % self.MODELS + ]) + self.model = model diff --git a/clearpath_config/mounts/types/tower.py b/clearpath_config/mounts/types/tower.py new file mode 100644 index 0000000..f1e1d7c --- /dev/null +++ b/clearpath_config/mounts/types/tower.py @@ -0,0 +1,70 @@ +# Software License Agreement (BSD) +# +# @author Luis Camero +# @copyright (c) 2023, Clearpath Robotics, Inc., All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# * Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# * Neither the name of Clearpath Robotics nor the names of its contributors +# may be used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# 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. +from clearpath_config.common.types.accessory import Accessory +from clearpath_config.mounts.types.mount import BaseMount +from typing import List + + +class Tower(BaseMount): + MOUNT_MODEL = "tower" + SINGLE = "single" + DUAL = "dual" + QUAD = "quad" + MODELS = [SINGLE, DUAL, QUAD] + + def __init__( + self, + idx: int = None, + name: str = None, + model: str = SINGLE, + parent: str = Accessory.PARENT, + xyz: List[float] = Accessory.XYZ, + rpy: List[float] = Accessory.RPY + ) -> None: + self.model = model + super().__init__(idx, name, parent, xyz, rpy) + + def to_dict(self) -> dict: + d = super().to_dict() + d['model'] = self.get_model() + return d + + def from_dict(self, d: dict) -> None: + super().from_dict(d) + if 'model' in d: + self.set_model(d['model']) + + def get_model(self) -> str: + return self.model + + def set_model(self, model: str) -> None: + assert model in self.MODELS, " ".join([ + "Unexpected Tower model '%s'," % model, + "it must be one of the following: %s" % self.MODELS + ]) + self.model = model