multiple input images implementation

config/image_object_detection.yaml

@@ -6,7 +6,20 @@ image_object_detection_node:
  model.weights_file: yolov7-tiny.pt
  model.device: '0'
 
- selected_detections: ['person']
+ selected_detections: ['person'] # Classes to detect ['person', 'car']
 
  show_image: False
  publish_debug_image: True
+
+ # Lists of topics to subscribe
+ camera_topics:
+ - '/camera/image_raw'
+ # - '/camera1/image_raw'
+ # - '/camera2/image_raw'
+ # - '/camera3/image_raw'
+
+ # QoS policy for the image subscriber
+ subscribers.qos_policy: 'best_effort'
+
+ # QoS policy for the image debug publisher
+ image_debug_publisher.qos_policy: 'best_effort'

src/image_object_detection/image_object_detection_node.py

@@ -19,6 +19,7 @@
 
 import std_srvs.srv
 from sensor_msgs.msg import CompressedImage, Image
+from vision_msgs.msg import Detection2D, ObjectHypothesisWithPose
 
 import torch
 import torch.backends.cudnn as cudnn
@@ -27,17 +28,17 @@
 from utils.general import check_img_size, non_max_suppression, scale_coords, xyxy2xywh, set_logging
 from utils.plots import plot_one_box
 from utils.torch_utils import select_device
-from vision_msgs.msg import Detection2D, Detection2DArray, ObjectHypothesisWithPose
+from vision_msgs.msg import Detection2DArray, Detection2D
 from ament_index_python.packages import get_package_share_directory
 
 PACKAGE_NAME = "image_object_detection"
 
 
 class ImageDetectObjectNode(Node):
  def __init__(self):
-  super().__init__("image_object_detection_node")
+ super().__init__("image_object_detection_node")
 
- # parametros
+ # Model parameters
  self.declare_parameter("model.image_size", 640)
  self.model_image_size = (
  self.get_parameter("model.image_size").get_parameter_value().integer_value
@@ -129,60 +130,59 @@ def __init__(self):
 
  self.bridge = cv_bridge.CvBridge()
 
- self.image_sub = self.create_subscription(
- msg_type=Image, topic="image", callback=self.image_callback, qos_profile=self.qos
+ # Get the list of camera topics from the config file
+ self.declare_parameter("camera_topics", [])
+ self.camera_topics = (
+ self.get_parameter("camera_topics").get_parameter_value().string_array_value
  )
-
- self.image_compressed_sub = self.create_subscription(
- msg_type=CompressedImage,
- topic="image/compressed",
- callback=self.image_compressed_callback,
- qos_profile=self.qos,
- )
-
- self.detection_publisher = self.create_publisher(
- msg_type=Detection2DArray, topic="detections", qos_profile=self.qos
- )
-
- if self.enable_publish_debug_image:
- if self.qos_policy == "best_effort":
- self.get_logger().info("Using best effort qos policy for debug image publisher")
- self.qos = QoSProfile(
- reliability=QoSReliabilityPolicy.BEST_EFFORT,
- history=QoSHistoryPolicy.KEEP_LAST,
- depth=1,
- )
- else:
- self.get_logger().info("Using reliable qos policy for debug image publisher")
- self.qos = QoSProfile(
- reliability=QoSReliabilityPolicy.RELIABLE,
- history=QoSHistoryPolicy.KEEP_LAST,
- depth=1,
+ self.get_logger().info(f"Subscribed to topics: {self.camera_topics}")
+
+ # Initialize subscribers and publishers for each camera topic
+ self.subscribers = []
+ self.detection_publishers = {}
+ self.debug_image_publishers = {}
+
+ for topic in self.camera_topics:
+ # Create a subscriber for each camera topic
+ self.subscribers.append(
+ self.create_subscription(
+ Image,
+ topic,
+ callback=self.image_callback_factory(topic),
+ qos_profile=self.qos,
  )
+ )
 
- self.debug_image_publisher = self.create_publisher(
- msg_type=Image, topic="debug_image", qos_profile=self.qos
+ # Create a detection publisher for each camera
+ detection_topic = f"{topic}/detections"
+ self.detection_publishers[topic] = self.create_publisher(
+ Detection2DArray, detection_topic, self.qos
  )
 
+ # Create a debug image publisher for each camera (if enabled)
+ if self.enable_publish_debug_image:
+ debug_image_topic = f"{topic}/debug_image"
+ self.debug_image_publishers[topic] = self.create_publisher(
+ Image, debug_image_topic, self.qos
+ )
+
  self.initialize_model()
 
  def initialize_model(self):
  with torch.no_grad():
- # Initialize
  set_logging()
  self.device = select_device(self.device)
  self.half = self.device.type != "cpu"
 
- # Load model
  self.model = attempt_load(
  self.model_weights_file, map_location=self.device
- ) # load FP32 model
+ )
  self.stride = int(self.model.stride.max())
 
  self.imgsz = check_img_size(self.model_image_size, s=self.stride)
 
  if self.half:
- self.model.half() # to FP16
+ self.model.half()
 
  cudnn.benchmark = True
 
@@ -215,17 +215,13 @@ def accomodate_image_to_model(self, img0):
  def image_compressed_callback(self, msg):
  if not self.processing_enabled:
  return
-
- try:
- self.cv_img = self.bridge.compressed_imgmsg_to_cv2(msg, self.debug_image_output_format)
- img = self.accomodate_image_to_model(self.cv_img)
 
- detections_msg, debugimg = self.predict(img, self.cv_img)
+ self.cv_img = self.bridge.compressed_imgmsg_to_cv2(msg, self.debug_image_output_format)
+ img = self.accomodate_image_to_model(self.cv_img)
 
- self.detection_publisher.publish(detections_msg)
- except CvBridgeError as e:
- self.get_logger().error(f"Error converting image: {e}")
- return
+ detections_msg, debugimg = self.predict(img, self.cv_img)
+
+ self.detection_publisher.publish(detections_msg)
 
  if debugimg is not None:
  self.publish_debug_image(debugimg)
@@ -234,25 +230,43 @@ def image_compressed_callback(self, msg):
  cv2.imshow("Compressed Image", debugimg)
  cv2.waitKey(1)
 
- def image_callback(self, msg):
- if not self.processing_enabled:
- return
+ def image_callback_factory(self, topic):
+ def callback(msg):
+ try:
+ cv_img = self.bridge.imgmsg_to_cv2(msg, "bgr8")
+ self.image_queue[topic] = cv_img
+ except CvBridgeError as e:
+ self.get_logger().error(f"Error converting image from {topic}: {e}")
 
- self.cv_img = self.bridge.imgmsg_to_cv2(msg, "bgr8")
- img = self.accomodate_image_to_model(self.cv_img)
+ return callback
+
+ def image_callback_factory(self, topic):
+ def callback(msg):
+ if not self.processing_enabled:
+ return
 
- detections_msg, debugimg = self.predict(img, self.cv_img)
+ try:
+ cv_img = self.bridge.imgmsg_to_cv2(msg, "bgr8")
+ img = self.accomodate_image_to_model(cv_img)
 
- self.detection_publisher.publish(detections_msg)
+  detections_msg, debugimg = self.predict(img, cv_img)
 
- if debugimg is not None:
- self.publish_debug_image(debugimg)
+  # Publish detections for the current camera
+  self.detection_publishers[topic].publish(detections_msg)
 
- if self.show_image:
- cv2.imshow("Detection", debugimg)
- cv2.waitKey(1)
+  # Publish debug image for the current camera (if enabled)
+  if self.enable_publish_debug_image and topic in self.debug_image_publishers:
+  self.publish_debug_image(debugimg, topic)
 
- def publish_debug_image(self, debugimg):
+ if self.show_image:
+ cv2.imshow(f"Detection from {topic}", debugimg)
+ cv2.waitKey(1)
+ except CvBridgeError as e:
+ self.get_logger().error(f"Error converting image from {topic}: {e}")
+
+ return callback
+
+ def publish_debug_image(self, debugimg, topic):
  if self.debug_image_output_format == "mono8":
  debugimg = cv2.cvtColor(debugimg, cv2.COLOR_RGB2GRAY)
  elif self.debug_image_output_format == "rgb8":
@@ -261,11 +275,12 @@ def publish_debug_image(self, debugimg):
  debugimg = cv2.cvtColor(debugimg, cv2.COLOR_BGR2RGBA)
  else:
  self.get_logger().error(
- "Unsupported debug image output format: {}".format(self.debug_image_output_format)
+ f"Unsupported debug image output format: {self.debug_image_output_format}"
  )
  return
 
- self.debug_image_publisher.publish(
+ # Publish the debug image for the current camera
+ self.debug_image_publishers[topic].publish(
  self.bridge.cv2_to_imgmsg(debugimg, self.debug_image_output_format)
  )
 
@@ -294,7 +309,6 @@ def predict(self, model_img, original_image):
  ).round()
 
  for *xyxy, conf, cls in reversed(det):
- # clase clases deseadas
  if self.names[int(cls)] in self.selected_detections:
  detection2D_msg = Detection2D()
  xywh = (xyxy2xywh(torch.tensor(xyxy).view(1, 4)) / gn).view(-1).tolist()
@@ -322,7 +336,6 @@ def predict(self, model_img, original_image):
 
  return detections_msg, original_image
 
-
 def main(args=None):
  print(args)
  rclpy.init(args=sys.argv)