Merge pull request #10 from clearpathrobotics/ros_parameters

ROS Parameters

clearpath_config/parser.py

@@ -37,9 +37,27 @@
 
 
 class BaseConfigParser:
+    @staticmethod
+    def is_nested_key(key: str) -> bool:
+        return "." in key
+
+    @staticmethod
+    def get_nested_keys(key: str) -> list:
+        return key.split(".")
+
     @staticmethod
     def check_key_exists(key: str, config: dict) -> bool:
-        return key in config
+        if BaseConfigParser.is_nested_key(key):
+            keys = BaseConfigParser.get_nested_keys(key)
+            key = keys.pop(0)
+            if key in config:
+                return False
+            else:
+                return BaseConfigParser.check_key_exists(
+                    ".".join(keys), config
+                )
+        else:
+            return key in config
 
     @staticmethod
     def assert_key_exists(key: str, config: dict) -> None:
@@ -50,12 +68,28 @@ def assert_key_exists(key: str, config: dict) -> None:
     @staticmethod
     def get_required_val(key: str, config: dict):
         BaseConfigParser.assert_key_exists(key, config)
+        if BaseConfigParser.is_nested_key(key):
+            keys = BaseConfigParser.get_nested_keys(key)
+            key = keys.pop(0)
+            return BaseConfigParser.get_required_val(
+                key=".".join(keys),
+                config=config[key]
+            )
         return config[key]
 
     @staticmethod
     def get_optional_val(key: str, config: dict, default=None):
         if BaseConfigParser.check_key_exists(key, config):
-            return config[key]
+            if BaseConfigParser.is_nested_key(key):
+                keys = BaseConfigParser.get_nested_keys(key)
+                key = keys.pop(0)
+                return BaseConfigParser.get_optional_val(
+                    key=".".join(keys),
+                    config=config[key],
+                    default=default
+                )
+            else:
+                return config[key]
         else:
             return default
 
@@ -476,6 +510,7 @@ class BaseSensorParser(BaseConfigParser):
     # Keys
     URDF_ENABLED = "urdf_enabled"
     LAUNCH_ENABLED = "launch_enabled"
+    ROS_PARAMETERS = "ros_parameters"
 
     def __new__(cls, config: dict) -> BaseSensor:
         parent = cls.get_optional_val(
@@ -488,11 +523,14 @@ def __new__(cls, config: dict) -> BaseSensor:
             BaseSensorParser.URDF_ENABLED, config, BaseSensor.URDF_ENABLED)
         launch_enabled = cls.get_optional_val(
             BaseSensorParser.LAUNCH_ENABLED, config, BaseSensor.LAUNCH_ENABLED)
+        ros_parameters = cls.get_optional_val(
+            BaseSensorParser.ROS_PARAMETERS, config, BaseSensor.ROS_PARAMETERS)
         return BaseSensor(
             parent=parent,
             xyz=xyz, rpy=rpy,
             urdf_enabled=urdf_enabled,
-            launch_enabled=launch_enabled
+            launch_enabled=launch_enabled,
+            ros_parameters=ros_parameters
         )
 
 
@@ -519,6 +557,7 @@ def __new__(cls, config: dict) -> BaseLidar2D:
             rpy=sensor.get_rpy(),
             urdf_enabled=sensor.get_urdf_enabled(),
             launch_enabled=sensor.get_launch_enabled(),
+            ros_parameters=sensor.get_ros_parameters(),
             ip=ip,
             port=port,
             min_angle=min_angle,
@@ -543,6 +582,7 @@ def __new__(cls, config: dict) -> BaseLidar2D:
         lidar2d.set_port(base.get_port())
         lidar2d.set_min_angle(base.get_min_angle())
         lidar2d.set_max_angle(base.get_max_angle())
+        lidar2d.set_ros_parameters(base.get_ros_parameters())
         return lidar2d
 
 
@@ -562,6 +602,7 @@ def __new__(cls, config: dict) -> BaseCamera:
             rpy=sensor.get_rpy(),
             urdf_enabled=sensor.get_urdf_enabled(),
             launch_enabled=sensor.get_launch_enabled(),
+            ros_parameters=sensor.get_ros_parameters(),
             fps=fps,
             serial=serial
         )
@@ -575,25 +616,23 @@ class CameraParser(BaseConfigParser):
     ENCODING = "encoding"
 
     # Realsense Parameters
-    WIDTH = "width"
-    HEIGHT = "height"
+    COLOR_ENABLE = "color_enabled"
+    COLOR_FPS = "color_fps"
+    COLOR_WIDTH = "color_width"
+    COLOR_HEIGHT = "color_height"
     DEPTH_ENABLED = "depth_enabled"
     DEPTH_FPS = "depth_fps"
     DEPTH_WIDTH = "depth_width"
     DEPTH_HEIGHT = "depth_height"
+    POINTCLOUD_ENABLED = "pointcloud_enabled"
+
+    # ROS Parameters
+    ROS_PARAMETERS = "ros_parameters"
 
     def __new__(cls, config: dict) -> BaseLidar2D:
         base = BaseCameraParser(config)
         model = cls.get_required_val(CameraParser.MODEL, config)
         camera = Camera(model)
-        # Set Base Parameters
-        camera.set_parent(base.get_parent())
-        camera.set_xyz(base.get_xyz())
-        camera.set_rpy(base.get_rpy())
-        camera.set_urdf_enabled(base.get_urdf_enabled())
-        camera.set_launch_enabled(base.get_launch_enabled())
-        camera.set_fps(base.get_fps())
-        camera.set_serial(base.get_serial())
         # Set Specific Parameters
         if model == Camera.FLIR_BLACKFLY:
             camera.set_connection_type(
@@ -611,18 +650,32 @@ def __new__(cls, config: dict) -> BaseLidar2D:
                 )
             )
         elif model == Camera.INTEL_REALSENSE:
-            camera.set_width(
+            camera.set_color_enabled(
                 cls.get_optional_val(
-                    CameraParser.WIDTH,
+                    CameraParser.COLOR_ENABLE,
                     config,
-                    IntelRealsense.WIDTH
+                    IntelRealsense.COLOR_ENABLED
                 )
             )
-            camera.set_height(
+            camera.set_color_fps(
                 cls.get_optional_val(
-                    CameraParser.HEIGHT,
+                    CameraParser.COLOR_FPS,
                     config,
-                    IntelRealsense.HEIGHT
+                    IntelRealsense.COLOR_FPS
+                )
+            )
+            camera.set_color_width(
+                cls.get_optional_val(
+                    CameraParser.COLOR_WIDTH,
+                    config,
+                    IntelRealsense.COLOR_WIDTH
+                )
+            )
+            camera.set_color_height(
+                cls.get_optional_val(
+                    CameraParser.COLOR_HEIGHT,
+                    config,
+                    IntelRealsense.COLOR_HEIGHT
                 )
             )
             camera.set_depth_enabled(
@@ -653,6 +706,22 @@ def __new__(cls, config: dict) -> BaseLidar2D:
                     IntelRealsense.DEPTH_HEIGHT
                 )
             )
+            camera.set_pointcloud_enabled(
+                cls.get_optional_val(
+                    CameraParser.POINTCLOUD_ENABLED,
+                    config,
+                    IntelRealsense.POINTCLOUD_ENABLED
+                )
+            )
+        # Set Base Parameters
+        camera.set_parent(base.get_parent())
+        camera.set_xyz(base.get_xyz())
+        camera.set_rpy(base.get_rpy())
+        camera.set_urdf_enabled(base.get_urdf_enabled())
+        camera.set_launch_enabled(base.get_launch_enabled())
+        camera.set_fps(base.get_fps())
+        camera.set_serial(base.get_serial())
+        camera.set_ros_parameters(base.get_ros_parameters())
         return camera
 
 

clearpath_config/sample/a200_config.yaml

@@ -41,6 +41,9 @@ mounts:
         angle: 0.0
         xyz: [0.0, 0.0, 0.0]
         rpy: [0.0, 0.0, 0.0]
+    flir_ptu:
+      - {}
+      - {}
     riser:
       - rows: 8
         columns: 7
@@ -74,45 +77,52 @@ sensors:
       - model: flir_blackfly
         urdf_enabled: true
         launch_enabled: true
-        connection_type: USB3
-        encoding: BayerRG8
-        fps: 30
-        serial: 0
         parent: base_link
         xyz: [0.0, 0.0, 0.0]
         rpy: [0.0, 0.0, 0.0]
+        ros_parameters:
+          connection_type: USB3
+          encoding: BayerRG8
+          fps: 30
+          serial: 0
       - model: intel_realsense
         urdf_enabled: true
         launch_enabled: true
-        fps: 30
-        serial: 0
-        width: 640
-        height: 480
-        depth_enabled: true
-        depth_fps: 30
-        depth_width: 640
-        depth_height: 480
         parent: base_link
         xyz: [0.0, 0.0, 0.0]
         rpy: [0.0, 0.0, 0.0]
+        ros_parameters:
+          serial_no: 12345
+          device_type: d435
+          enable_color: true
+          rgb_camera:
+            profile: "640,480,30"
+          enable_depth: true
+          depth_module:
+            profile: "640,480,30"
+          pointcloud:
+            enable: true
+          anything: true
     lidar2d:
       - model: sick_lms1xx
         urdf_enabled: true
         launch_enabled: true
-        ip: 192.168.131.20
-        port: 6000
-        min_angle: -3.1415
-        max_angle: 3.1415
         parent: base_link
         xyz: [0.0, 0.0, 0.0]
         rpy: [0.0, 0.0, 0.0]
+        ros_parameters:
+          hostname: 192.168.131.20
+          port: 6000
+          min_ang: -3.1415
+          max_ang: 3.1415
       - model: hokuyo_ust10
         urdf_enabled: true
         launch_enabled: true
-        ip: 192.168.131.21
-        port: 6000
-        min_angle: -3.1415
-        max_angle: 3.1415
         parent: base_link
         xyz: [0.0, 0.0, 0.0]
         rpy: [0.0, 0.0, 0.0]
+        ros_parameters:
+          ip_address: 192.168.131.21
+          ip_port: 6000
+          angle_max: -3.1415
+          angle_min: 3.1415

clearpath_config/sensors/base.py

@@ -1,5 +1,6 @@
 from clearpath_config.common import Accessory, IndexedAccessory
-from typing import List
+from typing import List, Callable
+import copy
 
 
 class BaseSensor(IndexedAccessory):
@@ -13,6 +14,18 @@ class BaseSensor(IndexedAccessory):
     TOPIC = "base"
     URDF_ENABLED = True
     LAUNCH_ENABLED = True
+    ROS_PARAMETERS = {}
+
+    class ROSParameter:
+        def __init__(
+                self,
+                key: str,
+                get: Callable,
+                set: Callable
+                ) -> None:
+            self.key = key
+            self.get = get
+            self.set = set
 
     def __init__(
             self,
@@ -21,6 +34,7 @@ def __init__(
             topic: str = TOPIC,
             urdf_enabled: bool = URDF_ENABLED,
             launch_enabled: bool = LAUNCH_ENABLED,
+            ros_parameters: str = ROS_PARAMETERS,
             parent: str = Accessory.PARENT,
             xyz: List[float] = Accessory.XYZ,
             rpy: List[float] = Accessory.RPY,
@@ -38,6 +52,11 @@ def __init__(
         # - enables the sensor launch in the generated launch
         self.launch_enabled = True
         self.enable_launch if launch_enabled else self.disable_launch()
+        # ROS Parameters
+        # - dictionary with parameters for launch file
+        self.ros_parameters = {}
+        self.ros_parameter_pairs = {}
+        self.set_ros_parameters(ros_parameters)
         super().__init__(idx, name, parent, xyz, rpy)
 
     @classmethod
@@ -101,3 +120,46 @@ def set_launch_enabled(self, enabled: bool) -> None:
 
     def get_launch_enabled(self) -> bool:
         return self.launch_enabled
+
+    @staticmethod
+    def __get_key_recursive(keys: list, config: dict) -> any:
+        key = keys.pop(0)
+        if key in config:
+            if keys:
+                return BaseSensor.__get_key_recursive(
+                    keys,
+                    config[key]
+                )
+            else:
+                return config[key]
+        else:
+            return None
+
+    def set_ros_parameters(self, ros_parameters: dict) -> None:
+        assert isinstance(ros_parameters, dict), (
+            "ROS paramaters must be a dictionary")
+        for key, val in ros_parameters.items():
+            for _, param in self.ros_parameter_pairs.items():
+                if "." in param.key:
+                    val = BaseSensor.__get_key_recursive(
+                        param.key.split("."),
+                        ros_parameters
+                    )
+                    if val is not None:
+                        param.set(self, val)
+                else:
+                    if key == param.key:
+                        param.set(self, val)
+        self.ros_parameters = ros_parameters
+
+    def get_ros_parameters(self) -> dict:
+        for _, param in self.ros_parameter_pairs.items():
+            if "." in param.key:
+                keys = param.key.split(".")
+                params = {keys.pop(): param.get(self)}
+                for i in range(len(keys) - 1, -1, -1):
+                    params = {keys[i]: params}
+                self.ros_parameters.update(params)
+            else:
+                self.ros_parameters[param.key] = param.get(self)
+        return self.ros_parameters

clearpath_config/sensors/cameras.py

@@ -14,6 +14,10 @@ class BaseCamera(BaseSensor):
     FPS = 30
     SERIAL = "0"
 
+    class ROS_PARAMETER_KEYS:
+        FPS = "fps"
+        SERIAL = "serial"
+
     def __init__(
             self,
             idx: int = None,
@@ -23,6 +27,7 @@ def __init__(
             serial: str = SERIAL,
             urdf_enabled: bool = BaseSensor.URDF_ENABLED,
             launch_enabled: bool = BaseSensor.LAUNCH_ENABLED,
+            ros_parameters: dict = BaseSensor.ROS_PARAMETERS,
             parent: str = Accessory.PARENT,
             xyz: List[float] = Accessory.XYZ,
             rpy: List[float] = Accessory.RPY
@@ -33,6 +38,7 @@ def __init__(
             topic,
             urdf_enabled,
             launch_enabled,
+            ros_parameters,
             parent,
             xyz,
             rpy,
@@ -84,30 +90,52 @@ class IntelRealsense(BaseCamera):
     """
     SENSOR_MODEL = "intel_realsense"
 
-    FPS = 30
-    WIDTH = 640
-    HEIGHT = 480
+    D415 = "d415"
+    D435 = "d435"
+    D435i = "d435i"
+    DEVICE_TYPE = D435
+    DEVICE_TYPES = [D415, D435, D435i]
+
+    COLOR_ENABLED = True
+    COLOR_FPS = 30
+    COLOR_WIDTH = 640
+    COLOR_HEIGHT = 480
 
     DEPTH_ENABLED = True
     DEPTH_FPS = 30
     DEPTH_WIDTH = 640
     DEPTH_HEIGHT = 480
 
+    POINTCLOUD_ENABLED = True
+
+    class ROS_PARAMETER_KEYS:
+        SERIAL = "serial_no"
+        DEVICE_TYPE = "device_type"
+        DEPTH_PROFILE = "depth_module.profile"
+        DEPTH_ENABLE = "enable_depth"
+        COLOR_PROFILE = "rgb_camera.profile"
+        COLOR_ENABLE = "enable_color"
+        POINTCLOUD_ENABLE = "pointcloud.enable"
+
     def __init__(
             self,
             idx: int = None,
             name: str = None,
             topic: str = BaseCamera.TOPIC,
-            fps: int = FPS,
             serial: str = BaseCamera.SERIAL,
-            width: int = WIDTH,
-            height: int = HEIGHT,
+            device_type: str = DEVICE_TYPE,
+            color_enabled: bool = COLOR_ENABLED,
+            color_fps: bool = COLOR_FPS,
+            color_width: int = COLOR_WIDTH,
+            color_height: int = COLOR_HEIGHT,
             depth_enabled: bool = DEPTH_ENABLED,
             depth_fps: int = DEPTH_FPS,
             depth_width: int = DEPTH_WIDTH,
             depth_height: int = DEPTH_HEIGHT,
+            pointcloud_enabled: bool = POINTCLOUD_ENABLED,
             urdf_enabled: bool = BaseSensor.URDF_ENABLED,
             launch_enabled: bool = BaseSensor.LAUNCH_ENABLED,
+            ros_parameters: dict = BaseSensor.ROS_PARAMETERS,
             parent: str = Accessory.PARENT,
             xyz: List[float] = Accessory.XYZ,
             rpy: List[float] = Accessory.RPY
@@ -116,26 +144,115 @@ def __init__(
             idx,
             name,
             topic,
-            fps,
+            color_fps,
             serial,
             urdf_enabled,
             launch_enabled,
+            ros_parameters,
             parent,
             xyz,
             rpy
         )
-        self.width: int = IntelRealsense.WIDTH
-        self.set_width(width)
-        self.height: int = IntelRealsense.HEIGHT
-        self.set_height(height)
+        self.device_type: str = IntelRealsense.DEVICE_TYPE
+        self.set_device_type(device_type)
+        # Color Image
+        self.color_enabled: bool = IntelRealsense.COLOR_ENABLED
+        self.color_width: int = IntelRealsense.COLOR_WIDTH
+        self.color_height: int = IntelRealsense.COLOR_HEIGHT
+        self.set_color_enabled(color_enabled)
+        self.set_color_width(color_width)
+        self.set_color_height(color_height)
+        # Depth Image
         self.depth_enabled: bool = IntelRealsense.DEPTH_ENABLED
-        self.enable_depth() if depth_enabled else self.disable_depth()
-        self.depth_fps: int = IntelRealsense.DEPTH_FPS
-        self.set_depth_fps(depth_fps)
         self.depth_width: int = IntelRealsense.DEPTH_WIDTH
+        self.depth_height: int = IntelRealsense.DEPTH_HEIGHT
+        self.depth_fps: int = IntelRealsense.DEPTH_FPS
+        self.set_depth_enabled(depth_enabled)
         self.set_depth_width(depth_width)
-        self.detph_height: int = IntelRealsense.DEPTH_HEIGHT
         self.set_depth_height(depth_height)
+        self.set_depth_fps(depth_fps)
+        # Pointcloud
+        self.pointcloud_enabled: bool = IntelRealsense.POINTCLOUD_ENABLED
+        self.set_pointcloud_enabled(pointcloud_enabled)
+        # ROS Parameter Keys
+        pairs = {
+            # Device Type
+            self.ROS_PARAMETER_KEYS.DEVICE_TYPE: (
+                BaseSensor.ROSParameter(
+                    key=self.ROS_PARAMETER_KEYS.DEVICE_TYPE,
+                    get=lambda obj: obj.get_device_type(),
+                    set=lambda obj, val: obj.set_device_type(val)
+                )
+            ),
+            # Device Serial
+            self.ROS_PARAMETER_KEYS.SERIAL: (
+                BaseSensor.ROSParameter(
+                    key=self.ROS_PARAMETER_KEYS.SERIAL,
+                    get=lambda obj: obj.get_serial(),
+                    set=lambda obj, val: obj.set_serial(val)
+                )
+            ),
+            # Color Enable
+            self.ROS_PARAMETER_KEYS.COLOR_ENABLE: (
+                BaseSensor.ROSParameter(
+                    key=self.ROS_PARAMETER_KEYS.COLOR_ENABLE,
+                    get=lambda obj: obj.get_color_enabled(),
+                    set=lambda obj, val: obj.set_color_enabled(val)
+                )
+            ),
+            # Color Profile
+            self.ROS_PARAMETER_KEYS.COLOR_PROFILE: (
+                BaseSensor.ROSParameter(
+                    key=self.ROS_PARAMETER_KEYS.COLOR_PROFILE,
+                    get=lambda obj: obj.get_color_profile(),
+                    set=lambda obj, val: obj.set_color_profile(val)
+                )
+            ),
+            # Depth Enable
+            self.ROS_PARAMETER_KEYS.DEPTH_ENABLE: (
+                BaseSensor.ROSParameter(
+                    key=self.ROS_PARAMETER_KEYS.DEPTH_ENABLE,
+                    get=lambda obj: obj.get_depth_enabled(),
+                    set=lambda obj, val: obj.set_depth_enabled(val)
+                )
+            ),
+            # Depth Profile
+            self.ROS_PARAMETER_KEYS.DEPTH_PROFILE: (
+                BaseSensor.ROSParameter(
+                    key=self.ROS_PARAMETER_KEYS.DEPTH_PROFILE,
+                    get=lambda obj: obj.get_depth_profile(),
+                    set=lambda obj, val: obj.set_depth_profile(val)
+                )
+            ),
+            # Pointcloud Enable
+            self.ROS_PARAMETER_KEYS.POINTCLOUD_ENABLE: (
+                BaseSensor.ROSParameter(
+                    key=self.ROS_PARAMETER_KEYS.POINTCLOUD_ENABLE,
+                    get=lambda obj: obj.get_pointcloud_enabled(),
+                    set=lambda obj, val: obj.set_pointcloud_enabled(val)
+                )
+            ),
+        }
+        self.ros_parameter_pairs.update(pairs)
+        self.set_ros_parameters(ros_parameters)
+
+    @staticmethod
+    def clean_profile(profile: str | list) -> list:
+        if isinstance(profile, str):
+            profile = profile.split(",")
+            assert len(profile) == 3, (
+                "Profile '%s' is not three comma separated values")
+            try:
+                profile = [int(entry) for entry in profile]
+            except ValueError:
+                raise AssertionError(
+                    "Profile '%s' cannot be cast to integer")
+        else:
+            assert len(profile) == 3, (
+                "Profile '%s' is not three integer values")
+            assert all([isinstance(entry, int) for entry in profile]), (
+                "Profile '%s' is not three integer values")
+        return profile
 
     def assert_pixel_length(
             self,
@@ -148,19 +265,65 @@ def assert_pixel_length(
             "Pixel length must be positive"
         )
 
-    def set_height(self, height: int) -> None:
+    def get_device_type(self) -> str:
+        return self.device_type
+
+    def set_device_type(self, device_type: str) -> None:
+        assert device_type in self.DEVICE_TYPES, (
+            "Device type '%s' is not one of '%s'" % (
+                device_type,
+                self.DEVICE_TYPES
+            )
+        )
+        self.device_type = device_type
+
+    def enable_color(self) -> None:
+        self.color_enabled = True
+
+    def disable_color(self) -> None:
+        self.color_enabled = False
+
+    def is_color_enabled(self) -> bool:
+        return self.color_enabled
+
+    def get_color_enabled(self) -> bool:
+        return self.color_enabled
+
+    def set_color_enabled(self, enable: bool) -> None:
+        self.color_enabled = bool(enable)
+
+    def set_color_fps(self, fps: int) -> None:
+        self.set_fps(fps)
+
+    def get_color_fps(self) -> int:
+        return self.get_fps()
+
+    def set_color_height(self, height: int) -> None:
         self.assert_pixel_length(height)
         self.height = height
 
-    def get_height(self) -> int:
-        return self.get_height
+    def get_color_height(self) -> int:
+        return self.color_height
 
-    def set_width(self, width: int) -> None:
+    def set_color_width(self, width: int) -> None:
         self.assert_pixel_length(width)
         self.width = width
 
-    def get_width(self) -> int:
-        return self.get_width
+    def get_color_width(self) -> int:
+        return self.color_width
+
+    def set_color_profile(self, profile: str | list) -> None:
+        profile = self.clean_profile(profile)
+        self.set_color_width(profile[0])
+        self.set_color_height(profile[1])
+        self.set_color_fps(profile[2])
+
+    def get_color_profile(self) -> str:
+        return "%s,%s,%s" % (
+            self.get_color_width(),
+            self.get_color_height(),
+            self.get_color_fps()
+        )
 
     def enable_depth(self) -> None:
         self.depth_enabled = True
@@ -171,6 +334,9 @@ def disable_depth(self) -> None:
     def is_depth_enabled(self) -> bool:
         return self.depth_enabled
 
+    def get_depth_enabled(self) -> bool:
+        return self.depth_enabled
+
     def set_depth_enabled(self, enable: bool) -> None:
         self.depth_enabled = bool(enable)
 
@@ -195,6 +361,34 @@ def set_depth_height(self, height: int) -> None:
     def get_depth_height(self) -> int:
         return self.depth_height
 
+    def set_depth_profile(self, profile: str | list) -> None:
+        profile = self.clean_profile(profile)
+        self.set_depth_width(profile[0])
+        self.set_depth_height(profile[1])
+        self.set_depth_fps(profile[2])
+
+    def get_depth_profile(self) -> str:
+        return "%s,%s,%s" % (
+            self.get_depth_width(),
+            self.get_depth_height(),
+            self.get_depth_fps()
+        )
+
+    def enable_pointcloud(self) -> None:
+        self.pointcloud_enabled = True
+
+    def disable_pointcloud(self) -> None:
+        self.pointcloud_enabled = False
+
+    def is_pointcloud_enabled(self) -> bool:
+        return self.pointcloud_enabled
+
+    def get_pointcloud_enabled(self) -> bool:
+        return self.pointcloud_enabled
+
+    def set_pointcloud_enabled(self, enable: bool) -> None:
+        self.pointcloud_enabled = bool(enable)
+
 
 class FlirBlackfly(BaseCamera):
     """
@@ -284,6 +478,7 @@ def __init__(
             serial: str = BaseCamera.SERIAL,
             urdf_enabled: bool = BaseSensor.URDF_ENABLED,
             launch_enabled: bool = BaseSensor.LAUNCH_ENABLED,
+            ros_parameters: str = BaseSensor.ROS_PARAMETERS,
             parent: str = Accessory.PARENT,
             xyz: List[float] = Accessory.XYZ,
             rpy: List[float] = Accessory.RPY
@@ -296,6 +491,7 @@ def __init__(
             serial,
             urdf_enabled,
             launch_enabled,
+            ros_parameters,
             parent,
             xyz,
             rpy

clearpath_config/sensors/lidars_2d.py

@@ -18,6 +18,12 @@ class BaseLidar2D(BaseSensor):
     MIN_ANGLE = -pi
     MAX_ANGLE = pi
 
+    class ROS_PARAMETER_KEYS:
+        IP_ADDRESS = "ip_address"
+        IP_PORT = "ip_port"
+        MIN_ANGLE = "min_angle"
+        MAX_ANGLE = "max_angle"
+
     def __init__(
             self,
             idx: int = None,
@@ -29,6 +35,7 @@ def __init__(
             max_angle: float = MAX_ANGLE,
             urdf_enabled: bool = BaseSensor.URDF_ENABLED,
             launch_enabled: bool = BaseSensor.LAUNCH_ENABLED,
+            ros_parameters: dict = BaseSensor.ROS_PARAMETERS,
             parent: str = Accessory.PARENT,
             xyz: List[float] = Accessory.XYZ,
             rpy: List[float] = Accessory.RPY,
@@ -52,10 +59,40 @@ def __init__(
             topic,
             urdf_enabled,
             launch_enabled,
+            ros_parameters,
             parent,
             xyz,
             rpy,
             )
+        # ROS Parameter Keys
+        pairs = {
+            # IP Address
+            self.ROS_PARAMETER_KEYS.IP_ADDRESS: (
+                BaseSensor.ROSParameter(
+                    key=self.ROS_PARAMETER_KEYS.IP_ADDRESS,
+                    get=lambda obj: obj.get_ip(),
+                    set=lambda obj, val: obj.set_ip(val))),
+            # IP Port
+            self.ROS_PARAMETER_KEYS.IP_PORT: (
+                BaseSensor.ROSParameter(
+                    key=self.ROS_PARAMETER_KEYS.IP_PORT,
+                    get=lambda obj: obj.get_port(),
+                    set=lambda obj, val: obj.set_port(val))),
+            # Min. Angle
+            self.ROS_PARAMETER_KEYS.MIN_ANGLE: (
+                BaseSensor.ROSParameter(
+                    key=self.ROS_PARAMETER_KEYS.MIN_ANGLE,
+                    get=lambda obj: obj.get_min_angle(),
+                    set=lambda obj, val: obj.set_min_angle(val))),
+            # Max. Angle
+            self.ROS_PARAMETER_KEYS.MAX_ANGLE: (
+                BaseSensor.ROSParameter(
+                    key=self.ROS_PARAMETER_KEYS.MIN_ANGLE,
+                    get=lambda obj: obj.get_max_angle(),
+                    set=lambda obj, val: obj.set_max_angle(val))),
+        }
+        self.ros_parameter_pairs.update(pairs)
+        self.set_ros_parameters(ros_parameters)
 
     @classmethod
     def get_ip_from_idx(cls, idx: int) -> str:
@@ -96,12 +133,13 @@ def get_max_angle(self) -> float:
     def set_max_angle(self, angle: float) -> None:
         if angle > self.MAX_ANGLE:
             angle = self.MAX_ANGLE
-        self.min_angle = angle
+        self.max_angle = angle
 
 
 class HokuyoUST10(BaseLidar2D):
     SENSOR_MODEL = "hokuyo_ust10"
 
+    IP_PORT = 10940
     MIN_ANGLE = -pi
     MAX_ANGLE = pi
 
@@ -111,11 +149,12 @@ def __init__(
             name: str = None,
             topic: str = BaseLidar2D.TOPIC,
             ip: str = BaseLidar2D.IP_ADDRESS,
-            port: int = BaseLidar2D.IP_PORT,
+            port: int = IP_PORT,
             min_angle: float = MIN_ANGLE,
             max_angle: float = MAX_ANGLE,
             urdf_enabled: bool = BaseSensor.URDF_ENABLED,
             launch_enabled: bool = BaseSensor.LAUNCH_ENABLED,
+            ros_parameters: dict = BaseSensor.ROS_PARAMETERS,
             parent: str = Accessory.PARENT,
             xyz: List[float] = Accessory.XYZ,
             rpy: List[float] = Accessory.RPY
@@ -130,15 +169,26 @@ def __init__(
             max_angle,
             urdf_enabled,
             launch_enabled,
+            ros_parameters,
             parent,
             xyz,
             rpy
         )
+        # ROS Parameter Keys
+        self.ros_parameter_pairs[
+            BaseLidar2D.ROS_PARAMETER_KEYS.IP_ADDRESS].key = "ip_address"
+        self.ros_parameter_pairs[
+            BaseLidar2D.ROS_PARAMETER_KEYS.IP_PORT].key = "ip_port"
+        self.ros_parameter_pairs[
+            BaseLidar2D.ROS_PARAMETER_KEYS.MIN_ANGLE].key = "angle_min"
+        self.ros_parameter_pairs[
+            BaseLidar2D.ROS_PARAMETER_KEYS.MAX_ANGLE].key = "angle_max"
 
 
 class SickLMS1XX(BaseLidar2D):
     SENSOR_MODEL = "sick_lms1xx"
 
+    IP_PORT = 2112
     MIN_ANGLE = -2.391
     MAX_ANGLE = 2.391
 
@@ -148,11 +198,12 @@ def __init__(
             name: str = None,
             topic: str = BaseLidar2D.TOPIC,
             ip: str = BaseLidar2D.IP_ADDRESS,
-            port: int = BaseLidar2D.IP_PORT,
+            port: int = IP_PORT,
             min_angle: float = MIN_ANGLE,
             max_angle: float = MAX_ANGLE,
             urdf_enabled: bool = BaseSensor.URDF_ENABLED,
             launch_enabled: bool = BaseSensor.LAUNCH_ENABLED,
+            ros_parameters: dict = BaseSensor.ROS_PARAMETERS,
             parent: str = Accessory.PARENT,
             xyz: List[float] = Accessory.XYZ,
             rpy: List[float] = Accessory.RPY
@@ -167,7 +218,17 @@ def __init__(
             max_angle,
             urdf_enabled,
             launch_enabled,
+            ros_parameters,
             parent,
             xyz,
             rpy
         )
+        # ROS Parameter Keys
+        self.ros_parameter_pairs[
+            BaseLidar2D.ROS_PARAMETER_KEYS.IP_ADDRESS].key = "hostname"
+        self.ros_parameter_pairs[
+            BaseLidar2D.ROS_PARAMETER_KEYS.IP_PORT].key = "port"
+        self.ros_parameter_pairs[
+            BaseLidar2D.ROS_PARAMETER_KEYS.MIN_ANGLE].key = "min_ang"
+        self.ros_parameter_pairs[
+            BaseLidar2D.ROS_PARAMETER_KEYS.MAX_ANGLE].key = "max_ang"

clearpath_config/sensors/sensors.py

@@ -253,6 +253,7 @@ def add_camera(
             serial: str = BaseCamera.SERIAL,
             urdf_enabled: bool = BaseSensor.URDF_ENABLED,
             launch_enabled: bool = BaseSensor.LAUNCH_ENABLED,
+            ros_parameters: dict = BaseSensor.ROS_PARAMETERS,
             parent: str = Accessory.PARENT,
             xyz: List[float] = Accessory.XYZ,
             rpy: List[float] = Accessory.RPY
@@ -266,6 +267,7 @@ def add_camera(
             camera.set_serial(serial)
             camera.set_urdf_enabled(urdf_enabled)
             camera.set_launch_enabled(launch_enabled)
+            camera.set_ros_parameters(ros_parameters)
             camera.set_parent(parent)
             camera.set_xyz(xyz)
             camera.set_rpy(rpy)
@@ -310,32 +312,40 @@ def add_realsense(
             # By Object
             realsense: IntelRealsense = None,
             # By Parameters
-            fps: int = IntelRealsense.FPS,
             serial: str = BaseCamera.SERIAL,
-            width: int = IntelRealsense.WIDTH,
-            height: int = IntelRealsense.HEIGHT,
+            device_type: str = IntelRealsense.DEVICE_TYPE,
+            color_enabled: bool = IntelRealsense.COLOR_ENABLED,
+            color_fps: bool = IntelRealsense.COLOR_FPS,
+            color_width: int = IntelRealsense.COLOR_WIDTH,
+            color_height: int = IntelRealsense.COLOR_HEIGHT,
             depth_enabled: bool = IntelRealsense.DEPTH_ENABLED,
             depth_fps: int = IntelRealsense.DEPTH_FPS,
             depth_width: int = IntelRealsense.DEPTH_WIDTH,
             depth_height: int = IntelRealsense.DEPTH_HEIGHT,
+            pointcloud_enabled: bool = IntelRealsense.POINTCLOUD_ENABLED,
             urdf_enabled: bool = BaseSensor.URDF_ENABLED,
             launch_enabled: bool = BaseSensor.LAUNCH_ENABLED,
+            ros_parameters: dict = BaseSensor.ROS_PARAMETERS,
             parent: str = Accessory.PARENT,
             xyz: List[float] = Accessory.XYZ,
             rpy: List[float] = Accessory.RPY
             ) -> None:
         if realsense is None:
             realsense = IntelRealsense(
-                fps=fps,
                 serial=serial,
-                width=width,
-                height=height,
+                device_type=device_type,
+                color_enabled=color_enabled,
+                color_fps=color_fps,
+                color_width=color_width,
+                color_height=color_height,
                 depth_enabled=depth_enabled,
                 depth_fps=depth_fps,
                 depth_width=depth_width,
                 depth_height=depth_height,
+                pointcloud_enabled=pointcloud_enabled,
                 urdf_enabled=urdf_enabled,
                 launch_enabled=launch_enabled,
+                ros_parameters=ros_parameters,
                 parent=parent,
                 xyz=xyz,
                 rpy=rpy,
@@ -364,3 +374,20 @@ def set_camera(self, camera: BaseCamera) -> None:
     # Camera: Set All
     def set_all_camera(self, cameras: List[BaseCamera]) -> None:
         self.__cameras.set_all(cameras)
+
+    # Camera: Get All Objects of a Specified Model
+    def get_all_cameras_by_model(self, model: str) -> List[BaseLidar2D]:
+        Camera.assert_model(model)
+        all_model_camera = []
+        for camera in self.get_all_cameras():
+            if camera.SENSOR_MODEL == model:
+                all_model_camera.append(camera)
+        return all_model_camera
+
+    # Lidar2D: Get All Objects of Model UST10
+    def get_all_realsense(self) -> List[IntelRealsense]:
+        return self.get_all_cameras_by_model(Camera.INTEL_REALSENSE)
+
+    # Lidar2D: Get All Objects of Model LMS1XX
+    def get_all_blackfly(self) -> List[FlirBlackfly]:
+        return self.get_all_cameras_by_model(Camera.FLIR_BLACKFLY)