Fast Forward Feature: Add delay to manipulator controller

Author: luis-camero

clearpath_config/manipulators/manipulators.py

@@ -41,6 +41,78 @@
 from clearpath_config.manipulators.types.manipulator import BaseManipulator
 
 
+class MoveItConfig(BaseConfig):
+    ENABLE = 'enable'
+    DELAY = 'delay'
+    ROS_PARAMETERS = 'ros_parameters'
+
+    TEMPLATE = {
+        ENABLE: ENABLE,
+        DELAY: DELAY,
+        ROS_PARAMETERS: ROS_PARAMETERS
+    }
+
+    KEYS = flip_dict(TEMPLATE)
+
+    DEFAULTS = {
+        ENABLE: False,
+        DELAY: 5.0,
+        ROS_PARAMETERS: {}
+    }
+
+    def __init__(
+            self,
+            config: dict = {},
+            enable: bool = DEFAULTS[ENABLE],
+            delay: float = DEFAULTS[DELAY],
+            ros_parameters: dict = DEFAULTS[ROS_PARAMETERS]
+            ) -> None:
+        self.enable = enable
+        self.delay = delay
+        self.ros_parameters = ros_parameters
+        if config:
+            self.from_dict(config)
+
+    @property
+    def enable(self) -> bool:
+        return self._enable
+
+    @enable.setter
+    def enable(self, value: bool) -> None:
+        self._enable = bool(value)
+
+    @property
+    def delay(self) -> float:
+        return self._delay
+
+    @delay.setter
+    def delay(self, value: float) -> None:
+        assert value > 0, f'MoveIt delay must be greater than 0. Got {value}'
+        self._delay = value
+
+    @property
+    def ros_parameters(self) -> dict:
+        return self._ros_parameters
+
+    @ros_parameters.setter
+    def ros_parameters(self, value: dict) -> None:
+        assert isinstance(value, dict), (
+            f'MoveIt ROS parameters must be a dictionary. Got {value} instead.')
+        self._ros_parameters = value
+
+    def from_dict(self, d: dict) -> None:
+        if self.ENABLE in d:
+            self.enable = d[self.ENABLE]
+        if self.ROS_PARAMETERS in d:
+            self.ros_parameters = d[self.ROS_PARAMETERS]
+
+    def to_dict(self) -> dict:
+        return {
+            self.ENABLE: self.enable,
+            self.ROS_PARAMETERS: self.ros_parameters
+        }
+
+
 class ManipulatorListConfig(OrderedListConfig[BaseManipulator]):
 
     def __init__(self) -> None: