Skip to content

Commit

Permalink
Merge pull request #10 from clearpathrobotics/ros_parameters
Browse files Browse the repository at this point in the history
ROS Parameters
luis-camero authored Apr 26, 2023
2 parents d6c8969 + b2a75bd commit d76770e
Showing 6 changed files with 495 additions and 70 deletions.
107 changes: 88 additions & 19 deletions clearpath_config/parser.py
Original file line number Diff line number Diff line change
@@ -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


50 changes: 30 additions & 20 deletions clearpath_config/sample/a200_config.yaml
Original file line number Diff line number Diff line change
@@ -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
64 changes: 63 additions & 1 deletion clearpath_config/sensors/base.py
Original file line number Diff line number Diff line change
@@ -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
238 changes: 217 additions & 21 deletions clearpath_config/sensors/cameras.py
Original file line number Diff line number Diff line change
@@ -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
67 changes: 64 additions & 3 deletions clearpath_config/sensors/lidars_2d.py
Original file line number Diff line number Diff line change
@@ -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"
39 changes: 33 additions & 6 deletions clearpath_config/sensors/sensors.py
Original file line number Diff line number Diff line change
@@ -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)

0 comments on commit d76770e

Please sign in to comment.