ROS2 카메라&서보모터 :: object tracking (2)

1. Bag file을 이미지로 변환

---

객체 인식을 위해 Yolov8이라는 모델을 쓸 것이다.
학습을 위해서는 영상 데이터를 프레임으로 나누어 이미지를 획득할 것이다.

ROS1에서는 ROS자체에서 제공되는 비디오 변환 명령어가 존재한다.
하지만, ROS2에서는 찾아볼 수 없었다.

따라서, 파이썬이나 C언어로 구성된 비디오변환 Node가 필요했다.

나는 다음 사이트에서 코드를 추출하여 사용하였다.
내가 원한 것은 /image_raw를 저장한 백파일을 통해 비디오를 추출하는 것이었다.
하지만, 토픽명과 형식에 대한 파라미터를 노드 내에서 변경한 결과 오류가 떴다.
이 문제는 추후에 해결해야 할 것이다.

<Node 출처>
링크텍스트

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import CompressedImage
import cv_bridge
import cv2
import numpy as np


class ImageToVideoConverter(Node):
  def __init__(self):
      super().__init__('image_to_video_converter')
      self.bridge = cv_bridge.CvBridge()
      self.subscription = self.create_subscription(
          CompressedImage,
          '/image_raw/compressed',
          self.image_callback,
          10
      )
      self.video_writer = None

  def image_callback(self, msg):
      try:
          np_arr = np.frombuffer(msg.data, np.uint8)
          cv_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
          if self.video_writer is None:
              self.init_video_writer(cv_image)
          self.video_writer.write(cv_image)
      except Exception as e:
          self.get_logger().error('Error processing image: %s' % str(e))

  def init_video_writer(self, image):
      try:
          height, width, _ = image.shape
          video_format = 'mp4'  # or any other video format supported by OpenCV
          video_filename = 'output_video.' + video_format
          fourcc = cv2.VideoWriter_fourcc(*'mp4v')
          fps = 30  # Frames per second
          self.video_writer = cv2.VideoWriter(video_filename, fourcc, fps, (width, height))
      except Exception as e:
          self.get_logger().error('Error initializing video writer: %s' % str(e))

  def destroy_node(self):
      if self.video_writer is not None:
          self.video_writer.release()
      super().destroy_node()


def main(args=None):
  rclpy.init(args=args)
  image_to_video_converter = ImageToVideoConverter()
  rclpy.spin(image_to_video_converter)
  image_to_video_converter.destroy_node()
  rclpy.shutdown()


if __name__ == '__main__':
  main()

위 노드는 파이썬 코드로 작성된 것이기 떄문에

chmod +x <파이썬 코드 제목>

위와 같은 명령어를 통해 권한을 부여하여 실행하도록 한다.

<[사진] 백파일 재생 & 백파일 영상으로 변환하는 노드 실행>

2. 학습 데이터 취득

---

/image/compressed 토픽에 대한 영상을 추출했으니 프레임에 따라 이미지로 나누어 해당 이미지를 통해 Labeling 작업을 하도록 한다.
http://www.viconvert.kro.kr:8501/ --> 영상을 이미지로 변환해주는 무료사이트

라벨링 작업은 Roboflow라는 공식 사이트에서 진행하도록 한다.
https://app.roboflow.com/ --> Labeling 사이트 & 학습을 위한 이미지 데이터 획득

3. openCV 윤곽선 & HSV 색상 검출

---

현재 사용 중인 노트북이 nvidia 드라이버가 지원되지 않는 관계로 yolov8을 통한 학습이 불가능하다고 판단하였고, 대안으로 파이썬 코드를 통해 윤곽선, 색상 검출을 하도록 하였다.
기본적인 코드 구성은 아래와 같다.

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from geometry_msgs.msg import Point 
from cv_bridge import CvBridge
import cv2
import numpy as np

class ShapeColorDetector(Node):
  def __init__(self):
      super().__init__('shape_color_detector')
      self.subscription = self.create_subscription(
          Image,
          '/image_raw',
          self.image_callback,
          10)
      self.publisher = self.create_publisher(Point, 'shape_centroid', 10)
      self.bridge = CvBridge()

  def image_callback(self, msg):
      try:
          cv_image = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
      except CvBridgeError as e:
          self.get_logger().error(f"Error converting ROS2 image: {e}")
          return

      processed_image = self.detect_shapes_and_colors(cv_image)

      cv2.imshow('Processed Image', processed_image)
      cv2.waitKey(1)

  def detect_shapes_and_colors(self, image):
      hsv_image = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)

      lower_color = np.array([0, 100, 100])  # HSV 최저
      upper_color = np.array([5, 255, 255])  # HSV 최고

      color_mask = cv2.inRange(hsv_image, lower_color, upper_color)

      contours, _ = cv2.findContours(color_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)

      for contour in contours:
          epsilon = 0.04 * cv2.arcLength(contour, True)
          approx = cv2.approxPolyDP(contour, epsilon, True)

          num_vertices = len(approx)

          if num_vertices == 3: # 절점 개수
              shape = "Triangle"
          else:
              continue

          M = cv2.moments(approx)
          if M["m00"] != 0:
              cx = int(M["m10"] / M["m00"])
              cy = int(M["m01"] / M["m00"])

              centroid_msg = Point()
              centroid_msg.x = float(cx)
              centroid_msg.y = float(cy)
              centroid_msg.z = 0.0  
              self.publisher.publish(centroid_msg)

          cv2.drawContours(image, [approx], 0, (0, 255, 0), 2)

          cv2.putText(image, shape, tuple(approx[0][0]), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2)

      return image

def main(args=None):
  rclpy.init(args=args)
  shape_color_detector = ShapeColorDetector()
  rclpy.spin(shape_color_detector)
  shape_color_detector.destroy_node()
  rclpy.shutdown()

if __name__ == '__main__':
  main()

4. 코드 분석

---

self.subscription = self.create_subscription(
          Image, '/image_raw', image topic, self.image_callback, 10)
  ## 센서로부터 받은 이미지 토픽을 해당 코드에서 사용할 것

self.publisher = self.create_publisher(Point, 'shape_centroid', 10)
  ## 해당 코드 내에서 가공한 데이터(검출 도형의 중심)를 특정한 토픽명("shape_centroid")으로 내보냄

def image_callback(self, msg):
      try:
          cv_image = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
      except CvBridgeError as e:
          self.get_logger().error(f"Error converting ROS2 image: {e}")
          return
		processed_image = self.detect_shapes_and_colors(cv_image)
  ## USB 카메라로 획득한 이미지 데이터를 openCV 타입의 이미지 데이터로 변환
  ## processed_image : detect_shpaes_and_colors 함수를 통해 가공한 이미지 데이터
  ## cv2.imshow() : 괄호 안의 이미지 데이터를 시각화해주는 기능
  ## cv2.waitKey() : keyboard 입력까지 대기를 명령

def detect_shapes_and_colors(self, image)
  ## 카메라로부터 받은 이미지 데이터를 가공하는 함수
   # 도형의 윤곽선 그리기
   # 원하는 색깔 검출
   # 검출한 도형의 중심 파악
  
hsv_image = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
  ## 이미지의 색상을 BGR계열에서 HSV계열로 변환

lower_color = np.array([0, 100, 100])
upper_color = np.array([5, 255, 255])
  ## 검출하려는 도형의 HSV범위를 numpy array형태의 파라미터로 지정
  
contours, _ = cv2.findContours(color_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
  ## 설정한 HSV계열에 포함되어 검출된 도형의 외곽선
  
for contour in contours:
  epsilon = 0.04*cv2.arcLength(contour, True)
  approx = cv2.approxPloyDP(contour, epsilon, True)
  
  num_vertices = len(approx)
  ## cv2.arcLength : 검출한 외곽선의 길이 반환
  ## cv2.approxPolyDP : 윤곽선들의 윤곽점들로 근사해 근사 다각형으로 변환
   # cv2.approxPolyDP(윤곽선[contour], 근사치 정확도, 폐곡선)
   # 근사치 정확도의 값이 작을수록 contour 형상과 비슷하게 근사함(정확도 상승)
  ## num_vertices : 도형을 구성하는 외곽선의 개수 계산
   # 나는 외곽선의 개수가 3개인 삼각형만을 검출하기로 하였다.(편의상)
  
  M = cv2.moments(approx)
  ## cv2.moments : 이미지 모멘트를 계산하여 딕셔너리 형태로 저장하는 함수
  
  cx = int(M["m10"] / M["m00"])
  cy = int(M["m01"] / M["m00"])
  ## contour의 무게중심을 구하는 식 (변하지 않는 식 -> contour의 무게중심을 구하려면 위의 식을 활용하면 됨)
  
  centroid_msg = Point()
  centroid_msg.x = float(cx)
  centroid_msg.y = float(cy)
  ## cx, cy를 Point라는 메시지 타입으로 토픽 형성
  
  cv2.drawContours(image, [approx], 0, (0, 255, 0), 2)
  ## 검출된 contour의 외곽선을 따라 그림을 그려줌
  
  cv2.putText(image, shape, tuple(approx[0][0]), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2)
  ## 검출된 contour에 원하는 텍스트를 달아줌

  

---

• 검출하고자 하는 도형에 대한 HSV 색상에 맞게 파라미터를 조절하는 것이 쉽지않다.
• 또한, yolo에 비해 정확도도 엄청나게 떨어지는 편이기에 빨간색 삼각형만을 검출하는 것으로 했다.
• 빨간색 삼각형을 움직이며 카메라 트래킹할 수 있도록 모터를 제어하는 것이 나의 목표라고 볼 수 있겠다.