Merge pull request #24 from clearpathrobotics/new_mounts

New Mounts

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:

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,24 +97,32 @@ class MountsConfig(BaseConfig):
         BRACKET: [],
         FATH_PIVOT: [],
         RISER: [],
+        SICK: [],
+        TOWER: [],
     }
 
     def __init__(
             self,
             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)

clearpath_config/mounts/types/sick.py

@@ -0,0 +1,69 @@
+# Software License Agreement (BSD)
+#
+# @author    Luis Camero <lcamero@clearpathrobotics.com>
+# @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

clearpath_config/mounts/types/tower.py

@@ -0,0 +1,70 @@
+# Software License Agreement (BSD)
+#
+# @author    Luis Camero <lcamero@clearpathrobotics.com>
+# @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