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 22, 2025
1 parent 1faf681 commit c9f27d8
Show file tree
Hide file tree
Showing 6 changed files with 145 additions and 173 deletions.
123 changes: 78 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 = {
self.TOPICS.COLOR_IMAGE: BaseCamera.fps,
self.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 @@ -351,13 +349,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 = {
self.TOPICS.COLOR_IMAGE: IntelRealsense.color_fps,
self.TOPICS.COLOR_CAMERA_INFO: IntelRealsense.color_fps,
self.TOPICS.DEPTH_IMAGE: IntelRealsense.depth_fps,
self.TOPICS.DEPTH_CAMERA_INFO: IntelRealsense.depth_fps,
self.TOPICS.POINTCLOUD: IntelRealsense.depth_fps,
self.TOPICS.IMU: IntelRealsense.color_fps
}

@staticmethod
def clean_profile(profile: str | list) -> list:
if isinstance(profile, str):
Expand Down Expand Up @@ -464,7 +466,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 = fps

@property
Expand Down Expand Up @@ -591,9 +593,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 @@ -639,6 +641,10 @@ def __init__(
)

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

@property
def fps(self) -> float:
Expand Down Expand Up @@ -726,13 +732,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 @@ -784,6 +790,16 @@ def __init__(
rpy
)

# Topic Rates
self.rates = {
self.TOPICS.COLOR_IMAGE: StereolabsZed.fps,
self.TOPICS.COLOR_CAMERA_INFO: StereolabsZed.fps,
self.TOPICS.DEPTH_IMAGE: StereolabsZed.fps,
self.TOPICS.DEPTH_CAMERA_INFO: StereolabsZed.fps,
self.TOPICS.POINTCLOUD: StereolabsZed.fps,
self.TOPICS.IMU: StereolabsZed.fps,
}

@property
def device_type(self) -> str:
return self._device_type
Expand Down Expand Up @@ -858,10 +874,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 Down Expand Up @@ -908,9 +927,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 = {
self.TOPICS.COLOR_IMAGE: LuxonisOAKD.fps,
self.TOPICS.COLOR_CAMERA_INFO: LuxonisOAKD.fps,
self.TOPICS.STEREO_IMAGE: LuxonisOAKD.fps,
self.TOPICS.STEREO_CAMERA_INFO: LuxonisOAKD.fps,
self.TOPICS.POINTCLOUD: LuxonisOAKD.fps,
self.TOPICS.IMU: LuxonisOAKD.fps,
}

@property
def width(self) -> int:
Expand Down Expand Up @@ -1060,6 +1084,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 @@ -1234,6 +1262,11 @@ def __init__(
xyz,
rpy
)
# Topic Rates
self.rates = {
self.TOPICS.COLOR_IMAGE: AxisCamera.fps,
self.TOPICS.COLOR_CAMERA_INFO: AxisCamera.fps,
}

@property
def device_type(self) -> str:
Expand Down
34 changes: 12 additions & 22 deletions clearpath_config/sensors/types/gps.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,8 +50,8 @@ class TOPICS:
NAME = {
FIX: 'fix',
}
RATE = {
FIX: 60,
TYPE = {
FIX: 'sensor_msgs/msg/NavSatFix',
}

def __init__(
Expand Down Expand Up @@ -87,6 +87,10 @@ def __init__(
xyz,
rpy
)
# Topic Rates
self.rates = {
self.TOPICS.FIX: 1,
}

@classmethod
def get_frame_id_from_idx(cls, idx: int) -> str:
Expand Down Expand Up @@ -134,15 +138,6 @@ class ROS_PARAMETER_KEYS:
IP_ADDRESS = 'duro_node.ip_address'
IP_PORT = 'duro_node.port'

class TOPICS:
FIX = 'fix'
NAME = {
FIX: 'fix',
}
RATE = {
FIX: 60,
}

def __init__(
self,
idx: int = None,
Expand Down Expand Up @@ -217,8 +212,8 @@ class TOPICS:
NAME = {
FIX: 'fix',
}
RATE = {
FIX: 60,
TYPE = {
FIX: 'sensor_msgs/msg/NavSatFix',
}

def __init__(
Expand Down Expand Up @@ -258,6 +253,10 @@ def __init__(
xyz,
rpy
)
# Topic Rates
self.rates = {
self.TOPICS.FIX: 1,
}

@property
def port(self) -> str:
Expand Down Expand Up @@ -293,15 +292,6 @@ class ROS_PARAMETER_KEYS:
PORT = 'nmea_navsat_driver.port'
BAUD = 'nmea_navsat_driver.baud'

class TOPICS:
FIX = 'fix'
NAME = {
FIX: 'fix',
}
RATE = {
FIX: 60,
}

def __init__(
self,
idx: int = None,
Expand Down
Loading

0 comments on commit c9f27d8

Please sign in to comment.