참고

Gazebo, Ros, Yolov5를 활용한 군집드론추적 - 1 | Notion

Gazebo, Ros, Yolov5를 활용한 군집 드론추적 - 2 (성공) | Notion

1. 드론 준비

1. 리더드론(카메라 촬영 후 자폭 드론에게 명령 전송(ros토픽을 통한 명령))
2. 자폭 드론

1-1. 리더드론 카메라 위치 변경

<?xml version='1.0'?>
<!-- DO NOT EDIT: Generated from iris.sdf.jinja -->
<sdf version='1.6'>
  <model name='iris_camera'>
    <link name='camera::link'>
      <pose frame=''>0 0 -0.1 0 0.523599 0</pose>
      <inertial>
        <pose frame=''>0.01 0.025 0.025 0 -0 0</pose>
        <mass>0.01</mass>
        <inertia>
          <ixx>4.15e-06</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>2.407e-06</iyy>
          <iyz>0</iyz>
          <izz>2.407e-06</izz>
        </inertia>
        .
        .
        . 
        <중략>

카메라를 30도 각도로 설정

<pose frame=''>0 0 -0.1 0 0.523599 0</pose>

//pose가 iris에 카메라의 각도 및 위치를 설정할 수 있는 부분

결과

1-2 goturn추적기로 다중 객체 추적(타겟 군집 드론)

import cv2
import rospy
import numpy as np
import torch
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image
from std_msgs.msg import String
import time

class RealTimeObjectDetection:
    
    def __init__(self):
        self.average_bounding_box_size = 0   
        # ROS 노드 초기화
        rospy.init_node('dronedetection', anonymous=True)

        # CvBridge 인스턴스 생성
        self.bridge = CvBridge()
        # 상대경로
        weights = './drone_yolov5.pt' 

        # 모델 로드
        self.model = torch.hub.load('ultralytics/yolov5', 'custom', path=weights)
        # GoTURN 추적기 초기화
        self.tracker = cv2.TrackerGOTURN_create()
        # 추적 상태 및 첫 번째 프레임 플래그
        self.initialized = False
        # 이미지 토픽 구독
        self.subscriber = rospy.Subscriber("iris0/flow_camera/image_raw", Image, self.callback)
        
       # Detection 정보 발행을 위한 Publisher
       
        self.detection_pub = rospy.Publisher("detection_info", String, queue_size=10)
        
        # FPS 계산을 위한 변수
        self.frame_count = 0
        self.start_time = time.time()
        
    def callback(self, data):
        
        bounding_box_size = 0
        object_position = [0, 0]
        try:
            cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
        except CvBridgeError as e:
            rospy.logerr(e)
            return

        self.frame_count += 1
        # 매 30 프레임마다 재탐지를 시도
        if not self.initialized or self.frame_count % 30 == 0:
            # YOLOv5를 이용한 객체 탐지 수행
            results = self.model(cv_image)
            if len(results.xyxy[0]) > 0:
                # 각 객체의 바운딩 박스 크기를 저장할 리스트 초기화
                bounding_box_sizes = []
                
                # 여러 객체의 최소 및 최대 좌표를 찾습니다.
                for i in range(len(results.xyxy[0])):
                    x1, y1, x2, y2 = results.xyxy[0][i][:4]
                    width = x2 - x1
                    height = y2 - y1
                    # 바운딩 박스 크기 계산 (여기서는 너비 * 높이로 계산)
                    bounding_box_size = width * height
                    # 리스트에 바운딩 박스 크기 추가
                    bounding_box_sizes.append(bounding_box_size)
                
                # 평균 바운딩 박스 크기 계산
                if bounding_box_sizes:
                    self.average_bounding_box_size = sum(bounding_box_sizes) / len(bounding_box_sizes)
                else:
                    self.average_bounding_box_size = 0

                
                # 최소 및 최대 좌표를 사용해 전체 객체를 포함하는 하나의 바운딩 박스를 생성합니다.
                min_x = results.xyxy[0][:, 0].min().item()
                min_y = results.xyxy[0][:, 1].min().item()
                max_x = results.xyxy[0][:, 2].max().item()
                max_y = results.xyxy[0][:, 3].max().item()
                init_bb = (min_x, min_y, max_x - min_x, max_y - min_y)
                self.tracker.init(cv_image, tuple(map(int, init_bb)))
                self.initialized = True
        else:
            # 이미 추적기가 초기화되었다면, 추적을 수행
            success, box = self.tracker.update(cv_image)
            if success:
                x, y, w, h = map(int, box)
                cv2.rectangle(cv_image, (x, y), (x + w, y + h), (0, 255, 0), 2, 1)
                object_position = (x + w / 2, y + h / 2)
                bounding_box_size = h
                detection_info = f"{object_position[0]} {object_position[1]} {bounding_box_size}"
                self.detection_pub.publish(detection_info)
            else:
                # 추적 실패 시 추적기 재초기화를 위해 self.initialized를 False로 설정
                self.initialized = False

        # FPS 및 탐지 정보를 화면에 표시
        cv2.putText(cv_image, "x : {:.2f}".format(object_position[0]), (140, 20), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (50, 170, 50), 1)
        cv2.putText(cv_image, "y : {:.2f}".format(object_position[1]), (140, 40), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (50, 170, 50), 1)
        cv2.putText(cv_image, "size : {:.2f}".format(bounding_box_size), (140, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (50, 170, 50), 1)
        # 평균 바운딩 박스 크기를 화면에 표시
        cv2.putText(cv_image, "avg size : {:.2f}".format(self.average_bounding_box_size), (120, 80), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (50, 170, 50), 1)

        cv_image_resized = cv2.resize(cv_image, None, fx=2.0, fy=2.0, interpolation=cv2.INTER_LINEAR)
        # 탐지 및 추적 결과를 OpenCV 이미지로 변환하여 보여줌
        cv2.imshow('YOLOv5 & GoTURN Tracking', cv_image_resized)
        key = cv2.waitKey(1) & 0xFF
        if key == 27:  # ESC 키
            cv2.destroyAllWindows()
            rospy.signal_shutdown('ESC pressed')
if __name__ == '__main__':
    rtod = RealTimeObjectDetection()
    try:
        rospy.spin()
    except KeyboardInterrupt:
        cv2.destroyAllWindows()

기존에 goturn추적기에서

if len(results.xyxy[0]) > 0:
                # 여러 객체의 최소 및 최대 좌표를 찾기
                min_x = results.xyxy[0][:, 0].min().item()
                min_y = results.xyxy[0][:, 1].min().item()
                max_x = results.xyxy[0][:, 2].max().item()
                max_y = results.xyxy[0][:, 3].max().item()
                # 모든 객체를 포함하는 하나의 바운딩 박스를 생성
                init_bb = (min_x, min_y, max_x - min_x, max_y - min_y)
                self.tracker.init(cv_image, tuple(map(int, init_bb)))
                self.initialized = True