From f1da95cbc9b221f609936f8ee531e17fb1b12248 Mon Sep 17 00:00:00 2001 From: luis-camero Date: Wed, 8 Mar 2023 13:49:14 -0500 Subject: [PATCH 1/5] Remove old test config yaml --- test_config.yaml | 71 ------------------------------------------------ 1 file changed, 71 deletions(-) delete mode 100644 test_config.yaml diff --git a/test_config.yaml b/test_config.yaml deleted file mode 100644 index 9330ecd..0000000 --- a/test_config.yaml +++ /dev/null @@ -1,71 +0,0 @@ -version: 0 - -system: # These are system level configs - self: J100-1 - hosts: # These are the hosts that are involved in this system - platform: # The main computer for this system, ie the robot's computer - J100-1: 192.168.131.1 - onboard: # These are additional on-board computers such as a secondary computer or software kit - J100-1B: 192.168.131.5 - ONAV-KIT: 192.168.131.1 - remote: # These are remote machines which need to interact with this system such as laptops or other robots - CPR12345: 10.10.10.101 - A200-1: 192.168.1.111 - ros2: - namespace: HOSTNAME - domain_id: 123 - rmw_implementation: rmw_fastrtps_cpp - -platform: # These are are parameters specific to a a platform - serial_number: J100-XXXXX - model: J100 - decorations: # Platform specific accessories - fenders: - wibotic_mount: true - pacs: - risers - partial_riser - brackets - extras: - control: PATH_TO_CONTROL_EXTRAS_YAML - -mounts: - front_pivot: - enabled: true - parent_link: front_mount - accessory_link: front_pivot_mount - model: fath_pivot - angle: 0 - xyz: [0, 0, 0] - rpy: [0, 0, 0] - -sensors: # Various sensors - lidars: - 0: - model: hokuyo_ust10 # model of the sensor - driver: true # If the driver should be launched - host: J100-1 # The host where the driver will launched on - description: true # If the description is enabled - parent_link: front_pivot_mount - xyz: [0, 0, 0] - rpy: [0, 0, 0] - # Below are sensor specific params - error_limit: 5 - 1: - model: sick-lms100 - driver: true - host: J100-1B - description: true - parent_link: front_pivot_1 - xyz: [0, 0, 0] - rpy: [0, 0, 0] - error_limit: 5 - gps: - 0: - model: nmea - driver: true - host: J100-1 - description: true - parent_link: front_gps_mount - xyz: [0, 0, 0] - rpy: [0, 0, 0] \ No newline at end of file From 6d839058ba236a6955e7514cf8d629fa71e438d4 Mon Sep 17 00:00:00 2001 From: luis-camero Date: Fri, 10 Mar 2023 15:37:46 -0500 Subject: [PATCH 2/5] Added check to Accessory --- clearpath_config/clearpath_config.py | 3 +- clearpath_config/common.py | 48 +++++++++++++++++++--------- 2 files changed, 35 insertions(+), 16 deletions(-) diff --git a/clearpath_config/clearpath_config.py b/clearpath_config/clearpath_config.py index dce216d..41567e7 100644 --- a/clearpath_config/clearpath_config.py +++ b/clearpath_config/clearpath_config.py @@ -1,5 +1,6 @@ from clearpath_config.system.system import SystemConfig from clearpath_config.platform.platform import PlatformConfig +from clearpath_config.mounts.mounts import MountsConfig # ClearpathConfig: # - top level configurator @@ -11,5 +12,5 @@ def __init__(self, config: dict = None) -> None: self.version = 0 self.system = SystemConfig() self.platform = PlatformConfig() - #self.mounts = MountsConfig() + self.mounts = MountsConfig() #self.sensors = SensorsConfig() diff --git a/clearpath_config/common.py b/clearpath_config/common.py index 4f84cf9..dc8c305 100644 --- a/clearpath_config/common.py +++ b/clearpath_config/common.py @@ -131,12 +131,11 @@ def assert_valid(ip: str) -> None: class File(): def __init__(self, path: str, creatable=False, exists=False) -> None: - path = File.clean(path) if creatable: assert File.is_creatable(path) if exists: assert File.is_exists(path) - self.path = path + self.path = File.clean(path) def __str__(self) -> str: return self.path @@ -169,6 +168,9 @@ def is_exists(path: str) -> bool: path = File.clean(path) return os.path.exists(path) + def get_path(self) -> str: + return self.path + # SerialNumber # - Clearpath Robots Serial Number # - ex. cpr-j100-0100 @@ -213,12 +215,18 @@ def get_serial(self, prefix = False) -> str: class Accessory(): - - def __init__(self, - name: str = "", - parent: str = "base_link", - xyz: List[float] = [0.0, 0.0, 0.0], - rpy: List[float] = [0.0, 0.0, 0.0]) -> None: + # Defaults + PARENT = "base_link" + XYZ = [0.0, 0.0, 0.0] + RPY = [0.0, 0.0, 0.0] + + def __init__( + self, + name: str, + parent: str = PARENT, + xyz: List[float] = XYZ, + rpy: List[float] = RPY + ) -> None: self.name = str() self.parent = str() self.xyz = list() @@ -232,18 +240,14 @@ def get_name(self) -> str: return self.name def set_name(self, name: str) -> None: - assert isinstance(name, str), "Name must be a string" - assert name != "", "Name cannot be empty" - assert not name[0].isdigit(), "Name cannot start with a digit" + self.assert_valid_link(name) self.name = name def get_parent(self) -> str: return self.parent - def set_parent(self, parent:str) -> None: - assert isinstance(parent, str), "Parent must be a string" - assert parent != "", "Parent cannot be empty" - assert not parent[0].isdigit(), "Parent cannot start with a digit" + def set_parent(self, parent: str) -> None: + self.assert_valid_link(parent) self.parent = parent def get_xyz(self) -> List[float]: @@ -261,3 +265,17 @@ def set_rpy(self, rpy: List[float]) -> None: assert all([isinstance(i, float) for i in rpy]), "RPY must have all float entries" assert len(rpy) == 3, "RPY must be a list of exactly three float values" self.rpy = rpy + + def assert_valid_link(self, link: str) -> None: + # Link name must be a string + assert (isinstance(link, str) + ), "Link name '%s' must be string" % link + # Link name must not be empty + assert (link + ), "Link name '%s' must not be empty" % link + # Link name must not have spaces + assert (" " not in link + ), "Link name '%s' must no have spaces" % link + # Link name must not start with a digit + assert (not link[0].isdigit() + ), "Link name '%s' must not start with a digit" % link From 1a31d66ae1d75feccdbcdb4d5f394130062e05f8 Mon Sep 17 00:00:00 2001 From: luis-camero Date: Mon, 13 Mar 2023 09:58:44 -0400 Subject: [PATCH 3/5] Added platform parser --- clearpath_config/parser.py | 154 ++++++++++++++++++++++++++++++++++++- 1 file changed, 153 insertions(+), 1 deletion(-) diff --git a/clearpath_config/parser.py b/clearpath_config/parser.py index 2040608..7fb3a42 100644 --- a/clearpath_config/parser.py +++ b/clearpath_config/parser.py @@ -1,5 +1,6 @@ -from clearpath_config.common import Platform +from clearpath_config.common import Platform, Accessory from clearpath_config.clearpath_config import ClearpathConfig +from clearpath_config.mounts.mounts import MountsConfig, Mount from clearpath_config.platform.decorations import Decorations, BaseDecorationsConfig from clearpath_config.platform.pacs import PACS from clearpath_config.platform.platform import PlatformConfig, DecorationsConfig @@ -296,6 +297,155 @@ def __new__(cls, config: dict) -> PlatformConfig: pfmconfig.extras.set_control_extras(cls.get_optional_val(cls.CONTROL, extras, "")) return pfmconfig +class AccessoryParser(BaseConfigParser): + # Keys + NAME = "name" + PARENT = "parent" + XYZ = "xyz" + RPY = "rpy" + + def __new__(cls, config: dict) -> Accessory: + name = cls.get_required_val(AccessoryParser.NAME, config) + parent = cls.get_optional_val(AccessoryParser.PARENT, config, Accessory.PARENT) + xyz = cls.get_optional_val(AccessoryParser.XYZ, config, Accessory.XYZ) + rpy = cls.get_optional_val(AccessoryParser.RPY, config, Accessory.RPY) + return Accessory(name, parent, xyz, rpy) + + +class MountParser(BaseConfigParser): + + class Base(BaseConfigParser): + # Keys + MODEL = "model" + MOUNTING_LINK = "mounting_link" + + def __new__(cls, config: dict) -> Mount.Base: + a = AccessoryParser(config) + model = cls.get_required_val( + MountParser.Base.MODEL, + config, + ) + mounting_link = cls.get_optional_val( + MountParser.Base.MOUNTING_LINK, + config, + Mount.Base.MOUNTING_LINK, + ) + return Mount.Base( + name=a.get_name(), + parent=a.get_parent(), + xyz=a.get_xyz(), + rpy=a.get_rpy(), + model=model, + mounting_link=mounting_link, + ) + + class FathPivot(BaseConfigParser): + # Keys + ANGLE = "angle" + + def __new__(cls, config:dict) -> Mount.FathPivot: + b = MountParser.Base(config) + # Pivot Angle + angle = cls.get_optional_val( + MountParser.FathPivot.ANGLE, + config, + Mount.FathPivot.ANGLE, + ) + return Mount.FathPivot( + name=b.get_name(), + parent=b.get_parent(), + xyz=b.get_xyz(), + rpy=b.get_rpy(), + mounting_link=b.get_mounting_link(), + angle=angle, + ) + + class FlirPTU(BaseConfigParser): + # Keys + TTY_PORT = "tty_port" + TCP_PORT = "tcp_port" + IP_ADDRESS = "ip" + CONNECTION_TYPE = "connection_type" + LIMITS_ENABLED = "limits_enabled" + + def __new__(cls, config: dict) -> Mount.Base: + b = MountParser.Base(config) + # TTY Port + tty_port = cls.get_optional_val( + MountParser.FlirPTU.TTY_PORT, + config, + Mount.FlirPTU.TTY_PORT, + ) + # TCP Port + tcp_port = cls.get_optional_val( + MountParser.FlirPTU.TCP_PORT, + config, + Mount.FlirPTU.TCP_PORT, + ) + # IP Address + ip = cls.get_optional_val( + MountParser.FlirPTU.IP_ADDRESS, + config, + Mount.FlirPTU.IP_ADDRESS, + ) + # Connection Type + connection_type = cls.get_optional_val( + MountParser.FlirPTU.CONNECTION_TYPE, + config, + Mount.FlirPTU.CONNECTION_TYPE + ) + # Limits Enabled + limits_enabled = cls.get_optional_val( + MountParser.FlirPTU.LIMITS_ENABLED, + config, + Mount.FlirPTU.LIMITS_ENABLED, + ) + return Mount.FlirPTU( + name=b.get_name(), + parent=b.get_parent(), + xyz=b.get_xyz(), + rpy=b.get_rpy(), + mounting_link=b.get_mounting_link(), + tty_port=tty_port, + tcp_port=tcp_port, + ip=ip, + connection_type=connection_type, + limits_enabled=limits_enabled, + ) + + MODELS = { + Mount.FATH_PIVOT: FathPivot, + Mount.FLIR_PTU: FlirPTU, + } + def __new__(cls, config: dict) -> Mount.Base: + model = cls.get_required_val( + MountParser.Base.MODEL, + config + ) + return cls.MODELS[model](config) + + +class MountsConfigParser(BaseConfigParser): + # Key + MOUNTS = "mounts" + MOUNT_CONFIG = {} + + def __new__(cls, config: dict) -> MountsConfig: + mntconfig = MountsConfig() + # Mounts + mounts = cls.get_optional_val(cls.MOUNTS, config) + mntconfig.set_mounts(cls.get_mounts(mounts)) + return mntconfig + + @staticmethod + def get_mounts(config: list) -> List[Mount]: + # Assert List of Dictionaries + assert (isinstance(config, list) + ), "Config must be a list of dictionaries" + assert ((all([isinstance(c, dict) for c in config])) + ), "Config must be a list of dictionaries" + return [MountParser(c) for c in config] + # Clearpath Configuration Parser class ClearpathConfigParser(BaseConfigParser): @@ -354,4 +504,6 @@ def __new__(self, config): cprconfig.system = SystemConfigParser(config) # PlatformConfig cprconfig.platform = PlatformConfigParser(config) + # MountConfig + cprconfig.mounts = MountsConfigParser(config) return cprconfig From 748d6694ccfcc14e2082e572e73209db758ec34c Mon Sep 17 00:00:00 2001 From: luis-camero Date: Mon, 13 Mar 2023 09:59:07 -0400 Subject: [PATCH 4/5] Added mounts to sample config --- clearpath_config/sample/a200_config.yaml | 37 ++++++++++++++++++------ 1 file changed, 28 insertions(+), 9 deletions(-) diff --git a/clearpath_config/sample/a200_config.yaml b/clearpath_config/sample/a200_config.yaml index e195f56..b539995 100644 --- a/clearpath_config/sample/a200_config.yaml +++ b/clearpath_config/sample/a200_config.yaml @@ -62,15 +62,34 @@ platform: # These are are parameters specific to a a platform mounts: - front_pivot: - enabled: true - parent_link: front_mount - accessory_link: front_pivot_mount - model: fath_pivot - angle: 0 - xyz: [0, 0, 0] - rpy: [0, 0, 0] - + - name: "front_pivot" + model: "fath_pivot" + parent: "base_link" + mounting_link: "front_pivot_mount" + angle: 0.0 + xyz: [0.0, 0.0, 0.0] + rpy: [0.0, 0.0, 0.0] + + - name: "ptu" + model: "flir_ptu" + parent: "base_link" + mounting_link: "ptu_mount" + xyz: [0.0, 0.0, 0.0] + rpy: [0.0, 0.0, 0.0] + tty_port: "/dev/ptu" + tcp_port: 4000 + ip: "192.168.131.70" + connection_type: "tty" + limits_enabled: False +# front_pivot: +# enabled: true +# parent_link: front_mount +# accessory_link: front_pivot_mount +# model: fath_pivot +# angle: 0 +# xyz: [0, 0, 0] +# rpy: [0, 0, 0] + sensors: # Various sensors lidars: 0: From ddea64b264a7fe617c4c29b3b52a1e5f860e9e69 Mon Sep 17 00:00:00 2001 From: luis-camero Date: Mon, 13 Mar 2023 10:02:10 -0400 Subject: [PATCH 5/5] Added mounts config --- clearpath_config/mounts/mounts.py | 194 ++++++++++++++++++++++++++++++ 1 file changed, 194 insertions(+) create mode 100644 clearpath_config/mounts/mounts.py diff --git a/clearpath_config/mounts/mounts.py b/clearpath_config/mounts/mounts.py new file mode 100644 index 0000000..93fd3f4 --- /dev/null +++ b/clearpath_config/mounts/mounts.py @@ -0,0 +1,194 @@ +from clearpath_config.common import Accessory, File, IP, List +from copy import deepcopy +from math import pi + +class Mount(): + FATH_PIVOT = "fath_pivot" + FLIR_PTU = "flir_ptu" + ALL = [FATH_PIVOT, FLIR_PTU] + + class Base(Accessory): + MOUNTING_LINK = None + + def __init__( + self, + name: str, + model: str, + parent: str = Accessory.PARENT, + mounting_link: str = MOUNTING_LINK, + xyz: List[float] = Accessory.XYZ, + rpy: List[float] = Accessory.RPY, + ) -> None: + super().__init__(name, parent, xyz, rpy) + self.model = str() + self.set_model(model) + self.mounting_link = "%s_mount" % self.get_name() + if mounting_link: + self.set_mounting_link(mounting_link) + + def get_model(self) -> str: + return self.model + + def set_model(self, model: str) -> None: + assert (model in Mount.ALL + ), "Model '%s' must be one of '%s'" % (model, self.MODELS) + self.model = model + + def get_mounting_link(self) -> str: + return self.mounting_link + + def set_mounting_link(self, mounting_link: str) -> None: + self.assert_valid_link(mounting_link) + self.mounting_link = mounting_link + + + class FathPivot(Base): + MOUNTING_LINK = None + ANGLE = 0.0 + + def __init__( + self, + name: str, + parent: str = Accessory.PARENT, + mounting_link: str = MOUNTING_LINK, + angle: float = ANGLE, + xyz: List[float] = Accessory.XYZ, + rpy: List[float] = Accessory.RPY, + ) -> None: + super().__init__(name, Mount.FATH_PIVOT, parent, mounting_link, xyz, rpy) + self.angle = 0.0 + if angle: + self.set_angle(angle) + + def get_angle(self) -> float: + return self.angle + + def set_angle(self, angle: float) -> None: + assert(-pi < angle <= pi + ), "Angle '%s' must be in radian and between pi and -pi" + self.angle = angle + + + class FlirPTU(Base): + # Default Values + MOUNTING_LINK = None + TTY_PORT = "/dev/ptu" + TCP_PORT = 4000 + IP_ADDRESS = "192.168.131.70" + LIMITS_ENABLED = False + TTY = "tty" + TCP = "tcp" + CONNECTION_TYPE = TTY + # TTY (uses tty_port) + # TCP (uses ip_addr and tcp_port) + CONNECTION_TYPES = [TTY, TCP] + + def __init__( + self, + name: str, + parent: str = Accessory.PARENT, + mounting_link: str = MOUNTING_LINK, + xyz: List[float] = Accessory.XYZ, + rpy: List[float] = Accessory.RPY, + tty_port: str = TTY_PORT, + tcp_port: int = TCP_PORT, + ip: str = IP_ADDRESS, + connection_type: str = CONNECTION_TYPE, + limits_enabled: bool = LIMITS_ENABLED, + ) -> None: + super().__init__(name, Mount.FLIR_PTU, parent, mounting_link, xyz, rpy) + # Serial Port + self.tty_port = File(self.TTY_PORT) + self.set_tty_port(tty_port) + # TCP Port + self.tcp_port = self.TCP_PORT + self.set_tcp_port(tcp_port) + # IP + self.ip = IP() + self.set_ip(ip) + # Connection Type + self.connection_type = self.TTY + self.set_connection_type(connection_type) + # Limits + self.limits_enabled = False + self.set_limits_enabled(limits_enabled) + + def get_tty_port(self) -> str: + return self.tty_port.get_path() + + def set_tty_port(self, tty_port: str) -> None: + self.tty_port = File(tty_port) + + def get_tcp_port(self) -> str: + return self.tcp_port + + def set_tcp_port(self, tcp_port: int) -> None: + assert(1024 < tcp_port < 65536 + ), "TCP port '%s' must be in range 1024 to 65536" % tcp_port + self.tcp_port = tcp_port + + def get_ip(self) -> str: + return str(self.ip) + + def set_ip(self, ip: str) -> None: + self.ip = IP(ip) + + def get_connection_type(self) -> str: + return self.connection_type + + def set_connection_type(self, connection_type: str) -> None: + assert(connection_type in self.CONNECTION_TYPES + ), "Connection type '%s' must be one of '%s'" % ( + connection_type, self.CONNECTION_TYPES + ) + self.connection_type = connection_type + + def get_limits_enabled(self) -> bool: + return self.limits_enabled + + def set_limits_enabled(self, limits_enabled: bool) -> None: + self.limits_enabled = limits_enabled + + MODEL = { + FATH_PIVOT: FathPivot, + FLIR_PTU: FlirPTU, + } + + def __new__(cls, name: str, model: str) -> Base: + return Mount.MODEL[model](name) + + +class MountsConfig: + + def __init__(self, mounts: List[Mount.Base] = []) -> None: + self.mounts = list() + self.set_mounts(mounts) + + def get_mounts(self) -> List[Mount.Base]: + return self.mounts + + def set_mounts(self, mounts: List[Mount.Base]) -> None: + assert (isinstance(mounts, list) + ), "Mounts must be a list of Mount objects" + assert (all([isinstance(m, Mount.Base) for m in mounts]) + ), "Mounts must be a list of Mount objects" + temp = deepcopy(self.mounts) + self.mounts.clear() + for mount in mounts: + self.add_mount(mount) + + def add_mount(self, mount: Mount.Base) -> None: + assert (isinstance(mount, Mount.Base) + ), "Mount '%s' must be of type Mount." % mount + assert (mount.get_name() not in [m.get_name() for m in self.mounts] + ), "Mount name '%s' must be unique." % mount.get_name() + self.mounts.append(mount) + + def remove_mount(self, mount: Mount.Base = None, name: str = None) -> None: + assert (mount or name + ), "Mount or name of mount must be passed to 'remove_mount'" + if mount: + name = mount.get_name() + for mount in self.mounts: + if mount.get_name() == name: + self.mounts.remove(mount)