Skip to content

Commit

Permalink
Add topic message type and correct rates so they are not shared mutea…
Browse files Browse the repository at this point in the history 
…ble variables
  • Loading branch information
@hilary-luo
hilary-luo committed Jan 23, 2025
1 parent dae356e commit 53f5ca5
Show file tree
Hide file tree
Showing 6 changed files with 304 additions and 163 deletions.
144 changes: 99 additions & 45 deletions clearpath_config/sensors/types/cameras.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -127,9 +127,9 @@ class TOPICS:
COLOR_IMAGE: 'color/image',
COLOR_CAMERA_INFO: 'color/camera_info'
}
RATE = {
COLOR_IMAGE: 30,
COLOR_CAMERA_INFO: 30,
TYPE = {
COLOR_IMAGE: 'sensor_msgs/msg/Image',
COLOR_CAMERA_INFO: 'sensor_msgs/msg/CameraInfo',
}

def __init__(
Expand Down Expand Up @@ -174,6 +174,13 @@ def __init__(
xyz,
rpy,
)
# Rates:
# - should be a dictionary matching those in the TOPICS class
# - should be updated as necessary to reflect current operation
self.rates = {
BaseCamera.TOPICS.COLOR_IMAGE: BaseCamera.fps,
BaseCamera.TOPICS.COLOR_CAMERA_INFO: BaseCamera.fps,
}

@property
def camera_name(self) -> str:
Expand All @@ -189,17 +196,8 @@ def fps(self) -> int:

@fps.setter
def fps(self, fps: int) -> None:
BaseCamera.assert_valid_fps(fps)
self._fps = fps

@staticmethod
def assert_valid_fps(fps: int) -> None:
assert isinstance(fps, int), (
'FPS "%s" is invalid, must be an integer.' % fps
)
assert 0 <= fps, (
'FPS "%s" must be a positive integer.' % fps
)
BaseSensor.assert_valid_rate(fps)
self._fps = int(fps)

@property
def serial(self) -> str:
Expand Down Expand Up @@ -279,13 +277,13 @@ class TOPICS:
POINTCLOUD: 'points',
IMU: 'imu'
}
RATE = {
COLOR_IMAGE: BaseCamera.FPS,
COLOR_CAMERA_INFO: BaseCamera.FPS,
DEPTH_IMAGE: BaseCamera.FPS,
DEPTH_CAMERA_INFO: BaseCamera.FPS,
POINTCLOUD: BaseCamera.FPS,
IMU: BaseCamera.FPS
TYPE = {
COLOR_IMAGE: 'sensor_msgs/msg/Image',
COLOR_CAMERA_INFO: 'sensor_msgs/msg/CameraInfo',
DEPTH_IMAGE: 'sensor_msgs/msg/Image',
DEPTH_CAMERA_INFO: 'sensor_msgs/msg/CameraInfo',
POINTCLOUD: 'sensor_msgs/msg/PointCloud2',
IMU: 'sensor_msgs/msg/Imu'
}

def __init__(
Expand Down Expand Up @@ -353,13 +351,17 @@ def __init__(
xyz,
rpy
)

# Topic Rates
self.TOPICS.RATE[self.TOPICS.COLOR_IMAGE] = self.fps
self.TOPICS.RATE[self.TOPICS.COLOR_CAMERA_INFO] = self.fps
self.TOPICS.RATE[self.TOPICS.DEPTH_IMAGE] = self.fps
self.TOPICS.RATE[self.TOPICS.DEPTH_CAMERA_INFO] = self.fps
self.TOPICS.RATE[self.TOPICS.POINTCLOUD] = self.fps
self.TOPICS.RATE[self.TOPICS.IMU] = self.fps
self.rates = {
IntelRealsense.TOPICS.COLOR_IMAGE: IntelRealsense.color_fps,
IntelRealsense.TOPICS.COLOR_CAMERA_INFO: IntelRealsense.color_fps,
IntelRealsense.TOPICS.DEPTH_IMAGE: IntelRealsense.depth_fps,
IntelRealsense.TOPICS.DEPTH_CAMERA_INFO: IntelRealsense.depth_fps,
IntelRealsense.TOPICS.POINTCLOUD: IntelRealsense.depth_fps,
# Tracking of the IMU rate is not currently supported
}

@staticmethod
def clean_profile(profile: str | list) -> list:
if isinstance(profile, str):
Expand Down Expand Up @@ -466,7 +468,7 @@ def depth_fps(self) -> int:

@depth_fps.setter
def depth_fps(self, fps: int) -> None:
self.assert_valid_fps(fps)
BaseSensor.assert_valid_rate(fps)
self._depth_fps = int(fps)

@property
Expand Down Expand Up @@ -593,9 +595,9 @@ class TOPICS:
COLOR_IMAGE: 'color/image',
COLOR_CAMERA_INFO: 'color/camera_info'
}
RATE = {
COLOR_IMAGE: BaseCamera.FPS,
COLOR_CAMERA_INFO: BaseCamera.FPS,
TYPE = {
COLOR_IMAGE: 'sensor_msgs/msg/Image',
COLOR_CAMERA_INFO: 'sensor_msgs/msg/CameraInfo',
}

def __init__(
Expand Down Expand Up @@ -641,6 +643,10 @@ def __init__(
)

# Topic Rates
self.rates = {
FlirBlackfly.TOPICS.COLOR_IMAGE: FlirBlackfly.fps,
FlirBlackfly.TOPICS.COLOR_CAMERA_INFO: FlirBlackfly.fps,
}

@property
def fps(self) -> float:
Expand Down Expand Up @@ -728,13 +734,13 @@ class TOPICS:
POINTCLOUD: 'points',
IMU: 'imu'
}
RATE = {
COLOR_IMAGE: BaseCamera.FPS,
COLOR_CAMERA_INFO: BaseCamera.FPS,
DEPTH_IMAGE: BaseCamera.FPS,
DEPTH_CAMERA_INFO: BaseCamera.FPS,
POINTCLOUD: BaseCamera.FPS,
IMU: BaseCamera.FPS
TYPE = {
COLOR_IMAGE: 'sensor_msgs/msg/Image',
COLOR_CAMERA_INFO: 'sensor_msgs/msg/CameraInfo',
DEPTH_IMAGE: 'sensor_msgs/msg/Image',
DEPTH_CAMERA_INFO: 'sensor_msgs/msg/CameraInfo',
POINTCLOUD: 'sensor_msgs/msg/PointCloud2',
IMU: 'sensor_msgs/msg/Imu'
}

@staticmethod
Expand Down Expand Up @@ -786,6 +792,16 @@ def __init__(
rpy
)

# Topic Rates
self.rates = {
StereolabsZed.TOPICS.COLOR_IMAGE: StereolabsZed.fps,
StereolabsZed.TOPICS.COLOR_CAMERA_INFO: StereolabsZed.fps,
StereolabsZed.TOPICS.DEPTH_IMAGE: StereolabsZed.fps,
StereolabsZed.TOPICS.DEPTH_CAMERA_INFO: StereolabsZed.fps,
StereolabsZed.TOPICS.POINTCLOUD: StereolabsZed.fps,
# Tracking of the IMU rate is not currently supported
}

@property
def device_type(self) -> str:
return self._device_type
Expand Down Expand Up @@ -840,6 +856,7 @@ class LuxonisOAKD(BaseCamera):

class ROS_PARAMETER_KEYS:
FPS = 'oakd.rgb.i_fps'
STEREO_FPS = 'oakd.stereo.i_fps'
SERIAL = 'oakd.rgb.i_usb_port_id'
HEIGHT = 'oakd.rgb.i_height'
WIDTH = 'oakd.rgb.i_width'
Expand All @@ -860,10 +877,13 @@ class TOPICS:
POINTCLOUD: 'points',
IMU: 'imu',
}
RATE = {
COLOR_IMAGE: BaseCamera.FPS,
COLOR_CAMERA_INFO: BaseCamera.FPS,
IMU: BaseCamera.FPS
TYPE = {
COLOR_IMAGE: 'sensor_msgs/msg/Image',
COLOR_CAMERA_INFO: 'sensor_msgs/msg/CameraInfo',
STEREO_IMAGE: 'sensor_msgs/msg/Image',
STEREO_CAMERA_INFO: 'sensor_msgs/msg/CameraInfo',
POINTCLOUD: 'sensor_msgs/msg/PointCloud2',
IMU: 'sensor_msgs/msg/Imu'
}

def __init__(
Expand All @@ -872,6 +892,7 @@ def __init__(
name: str = None,
topic: str = BaseCamera.TOPIC,
fps: int = FPS,
stereo_fps: int = FPS,
serial: str = BaseCamera.SERIAL,
device_type: str = DEVICE_TYPE,
urdf_enabled: bool = BaseSensor.URDF_ENABLED,
Expand All @@ -886,10 +907,12 @@ def __init__(
# Resolution
self.height = LuxonisOAKD.HEIGHT
self.width = LuxonisOAKD.WIDTH
self.stereo_fps = stereo_fps

# ROS Parameter Template
ros_parameters_template = {
self.ROS_PARAMETER_KEYS.FPS: LuxonisOAKD.fps,
self.ROS_PARAMETER_KEYS.STEREO_FPS: LuxonisOAKD.stereo_fps,
self.ROS_PARAMETER_KEYS.SERIAL: LuxonisOAKD.serial,
self.ROS_PARAMETER_KEYS.HEIGHT: LuxonisOAKD.height,
self.ROS_PARAMETER_KEYS.WIDTH: LuxonisOAKD.width,
Expand All @@ -910,9 +933,14 @@ def __init__(
)

# Topic Rates
self.TOPICS.RATE[self.TOPICS.COLOR_IMAGE] = self.fps
self.TOPICS.RATE[self.TOPICS.COLOR_CAMERA_INFO] = self.fps
self.TOPICS.RATE[self.TOPICS.IMU] = self.fps
self.rates = {
LuxonisOAKD.TOPICS.COLOR_IMAGE: LuxonisOAKD.fps,
LuxonisOAKD.TOPICS.COLOR_CAMERA_INFO: LuxonisOAKD.fps,
LuxonisOAKD.TOPICS.STEREO_IMAGE: LuxonisOAKD.stereo_fps,
LuxonisOAKD.TOPICS.STEREO_CAMERA_INFO: LuxonisOAKD.stereo_fps,
LuxonisOAKD.TOPICS.POINTCLOUD: LuxonisOAKD.stereo_fps,
# Tracking of the IMU rate is not currently supported
}

@property
def width(self) -> int:
Expand All @@ -930,6 +958,23 @@ def height(self) -> int:
def height(self, height: int) -> None:
self._height = height

@property
def fps(self) -> float:
return self._fps

@fps.setter
def fps(self, fps: float) -> None:
BaseSensor.assert_valid_rate(fps)
self._fps = float(fps)

@property
def stereo_fps(self) -> float:
return self._stereo_fps

@stereo_fps.setter
def stereo_fps(self, fps: float) -> None:
BaseSensor.assert_valid_rate(fps)
self._stereo_fps = float(fps)

class AxisCamera(BaseCamera):
"""PTZ and fixed cameras that use the axis_camera driver."""
Expand Down Expand Up @@ -1062,6 +1107,10 @@ class TOPICS:
COLOR_IMAGE: 'color/image/compressed',
COLOR_CAMERA_INFO: 'color/camera_info',
}
TYPE = {
COLOR_IMAGE: 'sensor_msgs/msg/CompressedImage',
COLOR_CAMERA_INFO: 'sensor_msgs/msg/CameraInfo',
}

def __init__(
self,
Expand Down Expand Up @@ -1236,6 +1285,11 @@ def __init__(
xyz,
rpy
)
# Topic Rates
self.rates = {
AxisCamera.TOPICS.COLOR_IMAGE: AxisCamera.fps,
AxisCamera.TOPICS.COLOR_CAMERA_INFO: AxisCamera.fps,
}

@property
def device_type(self) -> str:
Expand Down
Loading

0 comments on commit 53f5ca5

Please sign in to comment.