diff --git a/clearpath_config/manipulators/manipulators.py b/clearpath_config/manipulators/manipulators.py index 93ed8df..ecab9ce 100644 --- a/clearpath_config/manipulators/manipulators.py +++ b/clearpath_config/manipulators/manipulators.py @@ -34,18 +34,22 @@ Arm, BaseArm, ) +from clearpath_config.manipulators.types.lifts import ( + Lift, + BaseLift +) from clearpath_config.manipulators.types.manipulator import BaseManipulator -class ArmListConfig(OrderedListConfig[BaseArm]): +class ManipulatorListConfig(OrderedListConfig[BaseManipulator]): def __init__(self) -> None: - super().__init__(obj_type=BaseArm) + super().__init__(obj_type=BaseManipulator) def to_dict(self) -> List[dict]: d = [] - for arm in self.get_all(): - d.append(arm.to_dict()) + for manipulator in self.get_all(): + d.append(manipulator.to_dict()) return d @@ -56,6 +60,7 @@ class ManipulatorConfig(BaseConfig): TEMPLATE = { MANIPULATORS: { ARMS: ARMS, + LIFTS: LIFTS } } @@ -63,6 +68,7 @@ class ManipulatorConfig(BaseConfig): DEFAULTS = { ARMS: [], + LIFTS: [], } def __init__( @@ -70,9 +76,11 @@ def __init__( config: dict = {}, ) -> None: # List Initialization - self._arms = ArmListConfig() + self._arms = ManipulatorListConfig() + self._lifts = ManipulatorListConfig() template = { self.KEYS[self.ARMS]: ManipulatorConfig.arms, + self.KEYS[self.LIFTS]: ManipulatorConfig.lifts } super().__init__(template, config, self.MANIPULATORS) @@ -97,11 +105,36 @@ def arms(self, value: List[dict]) -> None: arms_list.append(arm) self._arms.set_all(arms_list) + @property + def lifts(self) -> OrderedListConfig: + self.set_config_param( + key=self.KEYS[self.LIFTS], + value=self._lifts.to_dict() + ) + return self._lifts + + @lifts.setter + def lifts(self, value: List[dict]) -> None: + assert isinstance(value, list), ( + "Manipulators must be list of 'dict'") + assert all([isinstance(i, dict) for i in value]), ( + "Manipulators must be list of 'dict'") + lifts_list = [] + for d in value: + lift = Lift(d['model']) + lift.from_dict(d) + lifts_list.append(lift) + self._lifts.set_all(lifts_list) + def get_all_manipulators(self) -> List[BaseManipulator]: manipulators = [] # Arms manipulators.extend(self.get_all_arms()) + manipulators.extend(self.get_all_lifts()) return manipulators def get_all_arms(self) -> List[BaseArm]: return self._arms.get_all() + + def get_all_lifts(self) -> List[BaseLift]: + return self._lifts.get_all() diff --git a/clearpath_config/manipulators/types/lifts.py b/clearpath_config/manipulators/types/lifts.py new file mode 100644 index 0000000..c187b88 --- /dev/null +++ b/clearpath_config/manipulators/types/lifts.py @@ -0,0 +1,125 @@ +# Software License Agreement (BSD) +# +# @author Luis Camero +# @copyright (c) 2024, 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 typing import List +from clearpath_config.common.types.accessory import Accessory +from clearpath_config.manipulators.types.manipulator import BaseManipulator + + +class BaseLift(BaseManipulator): + MANIPULATOR_MODEL = 'base' + MANIPULATOR_TYPE = 'lift' + + URDF_PARAMETERS = {} + + def __init__( + self, + idx: int = None, + name: str = None, + ros_parameters: dict = BaseManipulator.ROS_PARAMETERS, + ros_parameters_template: dict = BaseManipulator.ROS_PARAMETERS_TEMPLATE, + parent: str = Accessory.PARENT, + xyz: List[float] = Accessory.XYZ, + rpy: List[float] = Accessory.RPY + ) -> None: + super().__init__( + idx, name, ros_parameters, ros_parameters_template, parent, xyz, rpy) + self.urdf_parameters = dict(self.URDF_PARAMETERS) + + def to_dict(self) -> dict: + d = super().to_dict() + for k, v in self.urdf_parameters.items(): + if v: + d[k] = v + return d + + def from_dict(self, d: dict) -> None: + self.config = d + super().from_dict(d) + for k in self.urdf_parameters: + if k in d: + self.urdf_parameters[k] = d[k] + + def get_urdf_parameters(self) -> dict: + d = {} + for k, v in self.urdf_parameters.items(): + if v: + d[k] = v + return d + + +class Ewellix(BaseLift): + MANIPULATOR_MODEL = 'ewellix' + + EWELLIX_TYPE = 'ewellix_type' + ADD_PLATE = 'add_plate' + PARAMETERS_FILE = 'parameters_file' + INITIAL_POSITIONS = 'initial_positions' + INITIAL_POSITIONS_FILE = 'initial_positions_file' + GENERATE_ROS2_CONTROL_TAG = 'generate_ros2_control_tag' + USE_FAKE_HARDWARE = 'use_fake_hardware' + SIM_IGNITION = 'sim_ignition' + PORT = 'port' + BAUD = 'baud' + TIMEOUT = 'timeout' + CONVERSION = 'conversion' + RATED_EFFORT = 'rated_effort' + TOLERANCE = 'tolerance' + + URDF_PARAMETERS = { + EWELLIX_TYPE: '', + ADD_PLATE: '', + PARAMETERS_FILE: '', + INITIAL_POSITIONS: '', + INITIAL_POSITIONS_FILE: '', + GENERATE_ROS2_CONTROL_TAG: '', + USE_FAKE_HARDWARE: '', + SIM_IGNITION: '', + PORT: '', + BAUD: '', + TIMEOUT: '', + CONVERSION: '', + RATED_EFFORT: '', + TOLERANCE: '', + } + + +class Lift(): + EWELLIX = Ewellix.MANIPULATOR_MODEL + + MODEL = { + EWELLIX: Ewellix + } + + @classmethod + def assert_model(cls, model: str) -> None: + assert model in cls.MODEL, ( + f'Lift model {model} must be one of {cls.MODEL.keys()}') + + def __new__(cls, model: str) -> BaseLift: + cls.assert_model(model) + return cls.MODEL[model]() diff --git a/clearpath_config/platform/platform.py b/clearpath_config/platform/platform.py index cb72c13..21b89c1 100644 --- a/clearpath_config/platform/platform.py +++ b/clearpath_config/platform/platform.py @@ -84,6 +84,7 @@ def parameters(self, value: dict) -> None: class PlatformConfig(BaseConfig): PLATFORM = "platform" + # Controllers PS4 = "ps4" LOGITECH = "logitech" @@ -101,6 +102,8 @@ class PlatformConfig(BaseConfig): BATTERY = "battery" # Wheel WHEEL = "wheel" + # Enable/disable EKF + ENABLE_EKF = 'enable_ekf' TEMPLATE = { PLATFORM: { @@ -113,6 +116,7 @@ class PlatformConfig(BaseConfig): CONTROL: CONTROL, BATTERY: BATTERY, WHEEL: WHEEL, + ENABLE_EKF: ENABLE_EKF } } @@ -129,6 +133,7 @@ class PlatformConfig(BaseConfig): CONTROL: "", BATTERY: BatteryConfig.DEFAULTS, WHEEL: "default", + ENABLE_EKF: True, } def __init__( @@ -140,6 +145,7 @@ def __init__( battery: dict = DEFAULTS[BATTERY], extras: dict = DEFAULTS[EXTRAS], wheel: dict = DEFAULTS[WHEEL], + enable_ekf: bool = DEFAULTS[ENABLE_EKF], ) -> None: # Initialization self._config = {} @@ -151,7 +157,8 @@ def __init__( self.description = self.DEFAULTS[self.DESCRIPTION] self.launch = self.DEFAULTS[self.LAUNCH] self.control = self.DEFAULTS[self.CONTROL] - self.wheel = self.DEFAULTS[self.WHEEL] + self.wheel = wheel + self.enable_ekf = enable_ekf # Setter Template setters = { self.KEYS[self.CONTROLLER]: PlatformConfig.controller, @@ -160,6 +167,7 @@ def __init__( self.KEYS[self.BATTERY]: PlatformConfig.battery, self.KEYS[self.EXTRAS]: PlatformConfig.extras, self.KEYS[self.WHEEL]: PlatformConfig.wheel, + self.KEYS[self.ENABLE_EKF]: PlatformConfig.enable_ekf } super().__init__(setters, config, self.PLATFORM) @@ -334,3 +342,15 @@ def wheel(self) -> str: @wheel.setter def wheel(self, value: str) -> None: self._wheel = value + + @property + def enable_ekf(self) -> bool: + self.set_config_param( + key=self.KEYS[self.ENABLE_EKF], + value=self._enable_ekf + ) + return self._enable_ekf + + @enable_ekf.setter + def enable_ekf(self, value: bool) -> None: + self._enable_ekf = value diff --git a/clearpath_config/sensors/types/cameras.py b/clearpath_config/sensors/types/cameras.py index d433e6e..81b4edb 100644 --- a/clearpath_config/sensors/types/cameras.py +++ b/clearpath_config/sensors/types/cameras.py @@ -1078,6 +1078,7 @@ class AxisCamera(BaseCamera): DOME_PTZ, DOME_FIXED, ] + DEFAULT = DOME_FIXED HOSTNAME = '192.168.10.0' HTTP_PORT = 80 @@ -1123,6 +1124,8 @@ class AxisCamera(BaseCamera): class ROS_PARAMETER_KEYS: SERIAL = 'axis_camera.serial' # required by superclass, not used locally + DEVICE_TYPE = 'axis_camera.device_type' + TF_PREFIX = 'axis_camera.tf_prefix' HOSTNAME = 'axis_camera.hostname' @@ -1190,7 +1193,7 @@ def __init__( topic: str = BaseCamera.TOPIC, fps: int = FPS, serial: str = SERIAL, - device_type: str = Q62, + device_type: str = DEFAULT, hostname: str = HOSTNAME, http_port: int = HTTP_PORT, @@ -1245,6 +1248,8 @@ def __init__( ros_parameters_template = { self.ROS_PARAMETER_KEYS.SERIAL: AxisCamera.serial, + self.ROS_PARAMETER_KEYS.DEVICE_TYPE: AxisCamera.device_type, + self.ROS_PARAMETER_KEYS.TF_PREFIX: AxisCamera.tf_prefix, self.ROS_PARAMETER_KEYS.HOSTNAME: AxisCamera.hostname,