Camera Sim Update
This commit is contained in:
@@ -1,227 +1,152 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Camera Processor Node
|
||||
Handles camera feed processing for GPS-denied navigation
|
||||
Camera Processor - Handles Gazebo camera feeds for GPS-denied navigation.
|
||||
Subscribes to gz-transport topics, processes frames, and displays via OpenCV.
|
||||
Downstream modules (ArUco detector, optical flow) register as callbacks.
|
||||
"""
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import Image, CameraInfo
|
||||
from std_msgs.msg import Header
|
||||
from cv_bridge import CvBridge
|
||||
import cv2
|
||||
import sys
|
||||
import signal
|
||||
import time
|
||||
import numpy as np
|
||||
import cv2
|
||||
from gz.transport13 import Node
|
||||
from gz.msgs10.image_pb2 import Image
|
||||
|
||||
# PixelFormatType enum values (from gz.msgs10 Image proto)
|
||||
PF_RGB_INT8 = 3
|
||||
PF_BGR_INT8 = 8
|
||||
PF_R_FLOAT32 = 13
|
||||
|
||||
|
||||
class CameraProcessor(Node):
|
||||
"""
|
||||
Processes camera feeds for visual odometry and obstacle detection.
|
||||
Outputs processed images for downstream nodes.
|
||||
"""
|
||||
|
||||
def __init__(self):
|
||||
super().__init__('camera_processor')
|
||||
|
||||
self.bridge = CvBridge()
|
||||
|
||||
# Camera parameters
|
||||
self.camera_matrix = None
|
||||
self.dist_coeffs = None
|
||||
self.image_size = None
|
||||
|
||||
# Image processing parameters
|
||||
self.declare_parameter('undistort', True)
|
||||
self.declare_parameter('grayscale_output', True)
|
||||
self.declare_parameter('histogram_equalization', True)
|
||||
self.declare_parameter('resize_factor', 1.0)
|
||||
|
||||
self.undistort = self.get_parameter('undistort').value
|
||||
self.grayscale_output = self.get_parameter('grayscale_output').value
|
||||
self.histogram_eq = self.get_parameter('histogram_equalization').value
|
||||
self.resize_factor = self.get_parameter('resize_factor').value
|
||||
|
||||
# Undistort maps (computed once)
|
||||
self.map1 = None
|
||||
self.map2 = None
|
||||
|
||||
# Subscribers - Forward camera
|
||||
self.forward_image_sub = self.create_subscription(
|
||||
Image,
|
||||
'/uav/camera/forward/image_raw',
|
||||
self.forward_image_callback,
|
||||
10
|
||||
)
|
||||
|
||||
self.forward_info_sub = self.create_subscription(
|
||||
CameraInfo,
|
||||
'/uav/camera/forward/camera_info',
|
||||
self.camera_info_callback,
|
||||
10
|
||||
)
|
||||
|
||||
# Subscribers - Downward camera
|
||||
self.downward_image_sub = self.create_subscription(
|
||||
Image,
|
||||
'/uav/camera/downward/image_raw',
|
||||
self.downward_image_callback,
|
||||
10
|
||||
)
|
||||
|
||||
# Publishers - Processed images
|
||||
self.forward_processed_pub = self.create_publisher(
|
||||
Image,
|
||||
'/uav/camera/forward/image_processed',
|
||||
10
|
||||
)
|
||||
|
||||
self.downward_processed_pub = self.create_publisher(
|
||||
Image,
|
||||
'/uav/camera/downward/image_processed',
|
||||
10
|
||||
)
|
||||
|
||||
# Debug visualization
|
||||
self.debug_pub = self.create_publisher(
|
||||
Image,
|
||||
'/uav/camera/debug',
|
||||
10
|
||||
)
|
||||
|
||||
self.get_logger().info('Camera Processor Node Started')
|
||||
|
||||
def camera_info_callback(self, msg: CameraInfo):
|
||||
"""Extract and store camera calibration parameters."""
|
||||
if self.camera_matrix is None:
|
||||
self.camera_matrix = np.array(msg.k).reshape(3, 3)
|
||||
self.dist_coeffs = np.array(msg.d)
|
||||
self.image_size = (msg.width, msg.height)
|
||||
|
||||
# Compute undistortion maps
|
||||
if self.undistort and self.dist_coeffs is not None:
|
||||
new_camera_matrix, _ = cv2.getOptimalNewCameraMatrix(
|
||||
self.camera_matrix,
|
||||
self.dist_coeffs,
|
||||
self.image_size,
|
||||
alpha=0
|
||||
class CameraProcessor:
|
||||
def __init__(self, topics=None, show_gui=True):
|
||||
self.node = Node()
|
||||
self.show_gui = show_gui
|
||||
self.frames = {}
|
||||
self.callbacks = {}
|
||||
self.running = True
|
||||
|
||||
if topics is None:
|
||||
topics = {
|
||||
"downward": "/uav/camera/downward",
|
||||
"gimbal": "/world/uav_ugv_search/model/iris_with_gimbal/model/gimbal/link/pitch_link/sensor/camera/image",
|
||||
}
|
||||
|
||||
self.topics = topics
|
||||
|
||||
for name, topic in self.topics.items():
|
||||
self.callbacks[name] = []
|
||||
ok = self.node.subscribe(Image, topic, self._make_gz_callback(name))
|
||||
if ok:
|
||||
print(f"[CAM] Subscribed: {name} -> {topic}")
|
||||
else:
|
||||
print(f"[CAM] Failed: {topic}")
|
||||
|
||||
signal.signal(signal.SIGINT, lambda s, f: self.stop())
|
||||
|
||||
def _make_gz_callback(self, name):
|
||||
def cb(msg):
|
||||
fmt = msg.pixel_format_type
|
||||
if fmt == PF_RGB_INT8:
|
||||
arr = np.frombuffer(msg.data, dtype=np.uint8).reshape(
|
||||
msg.height, msg.width, 3
|
||||
)
|
||||
self.map1, self.map2 = cv2.initUndistortRectifyMap(
|
||||
self.camera_matrix,
|
||||
self.dist_coeffs,
|
||||
None,
|
||||
new_camera_matrix,
|
||||
self.image_size,
|
||||
cv2.CV_16SC2
|
||||
bgr = cv2.cvtColor(arr, cv2.COLOR_RGB2BGR)
|
||||
elif fmt == PF_BGR_INT8:
|
||||
arr = np.frombuffer(msg.data, dtype=np.uint8).reshape(
|
||||
msg.height, msg.width, 3
|
||||
)
|
||||
|
||||
self.get_logger().info(
|
||||
f'Camera calibration received: {self.image_size[0]}x{self.image_size[1]}'
|
||||
)
|
||||
|
||||
def process_image(self, image: np.ndarray) -> np.ndarray:
|
||||
"""
|
||||
Apply image processing pipeline.
|
||||
|
||||
Args:
|
||||
image: Input BGR image
|
||||
|
||||
Returns:
|
||||
Processed image
|
||||
"""
|
||||
bgr = arr
|
||||
elif fmt == PF_R_FLOAT32:
|
||||
arr = np.frombuffer(msg.data, dtype=np.float32).reshape(
|
||||
msg.height, msg.width
|
||||
)
|
||||
normalized = cv2.normalize(arr, None, 0, 255, cv2.NORM_MINMAX)
|
||||
bgr = cv2.cvtColor(normalized.astype(np.uint8), cv2.COLOR_GRAY2BGR)
|
||||
else:
|
||||
return
|
||||
|
||||
processed = self.process_image(bgr)
|
||||
self.frames[name] = processed
|
||||
|
||||
for fn in self.callbacks.get(name, []):
|
||||
fn(name, processed)
|
||||
|
||||
return cb
|
||||
|
||||
def process_image(self, image):
|
||||
processed = image.copy()
|
||||
|
||||
# 1. Undistort if calibration available
|
||||
if self.undistort and self.map1 is not None:
|
||||
processed = cv2.remap(processed, self.map1, self.map2, cv2.INTER_LINEAR)
|
||||
|
||||
# 2. Resize if needed
|
||||
if self.resize_factor != 1.0:
|
||||
new_size = (
|
||||
int(processed.shape[1] * self.resize_factor),
|
||||
int(processed.shape[0] * self.resize_factor)
|
||||
)
|
||||
processed = cv2.resize(processed, new_size, interpolation=cv2.INTER_AREA)
|
||||
|
||||
# 3. Convert to grayscale if requested
|
||||
if self.grayscale_output:
|
||||
if len(processed.shape) == 3:
|
||||
processed = cv2.cvtColor(processed, cv2.COLOR_BGR2GRAY)
|
||||
|
||||
# 4. Histogram equalization for better feature detection
|
||||
if self.histogram_eq:
|
||||
if len(processed.shape) == 2:
|
||||
# CLAHE for better results than standard histogram equalization
|
||||
clahe = cv2.createCLAHE(clipLimit=2.0, tileGridSize=(8, 8))
|
||||
processed = clahe.apply(processed)
|
||||
else:
|
||||
# For color images, apply to L channel in LAB
|
||||
lab = cv2.cvtColor(processed, cv2.COLOR_BGR2LAB)
|
||||
clahe = cv2.createCLAHE(clipLimit=2.0, tileGridSize=(8, 8))
|
||||
lab[:, :, 0] = clahe.apply(lab[:, :, 0])
|
||||
processed = cv2.cvtColor(lab, cv2.COLOR_LAB2BGR)
|
||||
|
||||
clahe = cv2.createCLAHE(clipLimit=2.0, tileGridSize=(8, 8))
|
||||
lab = cv2.cvtColor(processed, cv2.COLOR_BGR2LAB)
|
||||
lab[:, :, 0] = clahe.apply(lab[:, :, 0])
|
||||
processed = cv2.cvtColor(lab, cv2.COLOR_LAB2BGR)
|
||||
return processed
|
||||
|
||||
def forward_image_callback(self, msg: Image):
|
||||
"""Process forward camera images for visual odometry."""
|
||||
try:
|
||||
# Convert ROS Image to OpenCV
|
||||
cv_image = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
|
||||
|
||||
# Process image
|
||||
processed = self.process_image(cv_image)
|
||||
|
||||
# Convert back to ROS Image
|
||||
if len(processed.shape) == 2:
|
||||
encoding = 'mono8'
|
||||
else:
|
||||
encoding = 'bgr8'
|
||||
|
||||
processed_msg = self.bridge.cv2_to_imgmsg(processed, encoding)
|
||||
processed_msg.header = msg.header
|
||||
|
||||
# Publish processed image
|
||||
self.forward_processed_pub.publish(processed_msg)
|
||||
|
||||
except Exception as e:
|
||||
self.get_logger().error(f'Forward image processing error: {e}')
|
||||
|
||||
def downward_image_callback(self, msg: Image):
|
||||
"""Process downward camera images for optical flow."""
|
||||
try:
|
||||
# Convert ROS Image to OpenCV
|
||||
cv_image = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
|
||||
|
||||
# Process image
|
||||
processed = self.process_image(cv_image)
|
||||
|
||||
# Convert back to ROS Image
|
||||
if len(processed.shape) == 2:
|
||||
encoding = 'mono8'
|
||||
else:
|
||||
encoding = 'bgr8'
|
||||
|
||||
processed_msg = self.bridge.cv2_to_imgmsg(processed, encoding)
|
||||
processed_msg.header = msg.header
|
||||
|
||||
# Publish processed image
|
||||
self.downward_processed_pub.publish(processed_msg)
|
||||
|
||||
except Exception as e:
|
||||
self.get_logger().error(f'Downward image processing error: {e}')
|
||||
|
||||
def register_callback(self, camera_name, fn):
|
||||
if camera_name in self.callbacks:
|
||||
self.callbacks[camera_name].append(fn)
|
||||
print(f"[CAM] Registered callback for {camera_name}: {fn.__name__}")
|
||||
|
||||
def stop(self):
|
||||
self.running = False
|
||||
|
||||
def spin(self):
|
||||
print("[CAM] Running. Press 'q' to quit, 's' to screenshot.")
|
||||
while self.running:
|
||||
for name, frame in list(self.frames.items()):
|
||||
cv2.imshow(name, frame)
|
||||
|
||||
key = cv2.waitKey(33) & 0xFF
|
||||
if key == ord("q"):
|
||||
break
|
||||
elif key == ord("s"):
|
||||
for name, frame in self.frames.items():
|
||||
fname = f"{name}_{int(time.time())}.png"
|
||||
cv2.imwrite(fname, frame)
|
||||
print(f"[CAM] Saved {fname}")
|
||||
|
||||
cv2.destroyAllWindows()
|
||||
print("[CAM] Stopped.")
|
||||
|
||||
def spin_headless(self):
|
||||
print("[CAM] Running headless (no GUI).")
|
||||
while self.running:
|
||||
time.sleep(0.1)
|
||||
print("[CAM] Stopped.")
|
||||
|
||||
def get_frame(self, camera_name):
|
||||
return self.frames.get(camera_name)
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = CameraProcessor()
|
||||
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
def main():
|
||||
cameras = "both"
|
||||
show_gui = True
|
||||
|
||||
if len(sys.argv) > 1:
|
||||
cameras = sys.argv[1].lower()
|
||||
if "--headless" in sys.argv:
|
||||
show_gui = False
|
||||
|
||||
all_topics = {
|
||||
"downward": "/uav/camera/downward",
|
||||
"gimbal": "/world/uav_ugv_search/model/iris_with_gimbal/model/gimbal/link/pitch_link/sensor/camera/image",
|
||||
}
|
||||
|
||||
if cameras == "down":
|
||||
topics = {"downward": all_topics["downward"]}
|
||||
elif cameras == "gimbal":
|
||||
topics = {"gimbal": all_topics["gimbal"]}
|
||||
else:
|
||||
topics = all_topics
|
||||
|
||||
proc = CameraProcessor(topics=topics, show_gui=show_gui)
|
||||
|
||||
if show_gui:
|
||||
proc.spin()
|
||||
else:
|
||||
proc.spin_headless()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
||||
Reference in New Issue
Block a user