ROS2 OpenCV 활용

OpenCV 활용

pip install opencv-python

get_blue.py

origin.png에는 blue, green, red 색깔이 있는 도형이 있음

import cv2
import numpy as np
import time

def main(args=None):

    while(1):
        frame = cv2.imread("origin.png", cv2.IMREAD_COLOR)     #  read camera frame
        hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)    # Convert from BGR to HSV

        # define range of blue color in HSV
        lower_blue = np.array([100,100,120])          # range of blue
        upper_blue = np.array([150,255,255])

        lower_green = np.array([50, 150, 50])        # range of green
        upper_green = np.array([80, 255, 255])

        lower_red = np.array([0, 50, 50])        # range of red
        upper_red = np.array([30, 255, 255])

        # Threshold the HSV image to get only blue colors
        mask = cv2.inRange(hsv, lower_blue, upper_blue)     # color range of blue
        mask1 = cv2.inRange(hsv, lower_green, upper_green)  # color range of green
        mask2 = cv2.inRange(hsv, lower_red, upper_red)      # color range of red

        # Bitwise-AND mask and original image
        res = cv2.bitwise_and(frame, frame, mask=mask)      # apply blue mask
        res1 = cv2.bitwise_and(frame, frame, mask=mask1)    # apply green mask
        res2 = cv2.bitwise_and(frame, frame, mask=mask2)    # apply red mask
        
        gray = cv2.cvtColor(res1, cv2.COLOR_BGR2GRAY)    
        _, bin = cv2.threshold(gray, 30, 255, cv2.THRESH_BINARY)
            
        contours, _ = cv2.findContours(bin, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
        
        largest_contour = None
        largest_area = 0    
        
        COLOR = (0, 255, 0)
        for cnt in contours:                # find largest blue object
            area = cv2.contourArea(cnt)
            if area > largest_area:
                largest_area = area
                largest_contour = cnt
                
        # draw bounding box with green line
        if largest_contour is not None:
            #area = cv2.contourArea(cnt)
            if largest_area > 500:  # draw only larger than 500
                x, y, width, height = cv2.boundingRect(largest_contour)       
                cv2.rectangle(frame, (x, y), (x + width, y + height), COLOR, 2)
                center_x = x + width//2
                center_y = y + height//2
                #print("center: ( %s, %s )"%(center_x, center_y)) 
        cv2.imshow("VideoFrame",frame)       # show original frame
        cv2.imshow('blue', res)           # show applied blue mask
        cv2.imwrite("blue.png", res)
        cv2.imshow('Green', res1)          # show appliedgreen mask
        cv2.imwrite("green.png", res1)
        cv2.imshow('red', res2)          # show applied red mask
        cv2.imwrite("red.png", res2)

        k = cv2.waitKey(5) & 0xFF
            
        if k == 27:
            break

    cv2.destroyAllWindows()

if __name__ == '__main__':
    main()

webcam.py

import cv2
from time import sleep

webcam = cv2.VideoCapture(0)

if not webcam.isOpened():
    print("Could not open webcam")
    exit()

while webcam.isOpened():
    status, frame = webcam.read()
    print(status)
    sleep(0.1)

    if status:
        cv2.imshow("test", frame)

    if cv2.waitKey(1) & 0xFF == ord('q'):
        break

webcam.release()
cv2.destroyAllWindows()

webcam_blue.py

'''
    0 <------ pan left(pan++) ------- > 300   320 <------ pan right(pan--) ------------> 640
  0 +------------------------------------+-----+-----+------------------------------------+
    |                                    |     |     |                ^                   | 
    |                                    |     |     |                |                   | 
    |                                    |     |     |          tilt up(tilt--)           | 
    |                                    |     |     |                |                   | 
    |                                    |     |     |                v                   | 
220 +------------------------------------+-----+-----+------------------------------------+ 
    |                                    |     |     |                                    | 
    |                                    |     |     |                                    |  
240 +------------------------------------+-----+-----+------------------------------------+ 
    |                                    |     |     |                                    | 
    |                                    |     |     |                                    |  
260 +------------------------------------+-----+-----+------------------------------------+ 
    |                                    |     |     |                ^                   | 
    |                                    |     |     |                |                   | 
    |                                    |     |     |                                    | 
    |                                    |     |     |          tilt down(tilt++)         | 
    |                                    |     |     |                |                   | 
    |                                    |     |     |                v                   |  
480 +------------------------------------+-----+-----+------------------------------------+ 

'''

import cv2
import numpy as np

webcam = cv2.VideoCapture(0)

margin_x = 20
margin_y = 20

if not webcam.isOpened():
    print("Could not open webcam")
    exit()

while webcam.isOpened():
    status, frame = webcam.read()
    hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
    lower_blue = np.array([100,100,120])
    upper_blue = np.array([150,255,255])
    mask = cv2.inRange(hsv, lower_blue, upper_blue)
    res = cv2.bitwise_and(frame, frame, mask=mask)

    gray = cv2.cvtColor(res, cv2.COLOR_BGR2GRAY)
    _, bin = cv2.threshold(gray, 30, 255, cv2.THRESH_BINARY)
    contours, _ = cv2.findContours(bin, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
    
    
    largest_contour = None
    largest_area = 0    
    
    COLOR = (0, 255, 0)
    for cnt in contours:                # find largest blue object
        area = cv2.contourArea(cnt)
        if area > largest_area:
            largest_area = area
            largest_contour = cnt
            
     # draw bounding box with green line
    if largest_contour is not None:
        #area = cv2.contourArea(cnt)
        if largest_area > 500:  # draw only larger than 500
            x, y, width, height = cv2.boundingRect(largest_contour)       
            cv2.rectangle(frame, (x, y), (x + width, y + height), COLOR, 2)
            center_x = x + width//2
            center_y = y + height//2
            print("center: ( %s, %s )"%(center_x, center_y)) 
            
            if center_x < 300:#320 - margin_x:
                print("pan left")
            elif center_x > 340:#320 + margin_x:
                print("pan right")
            else:
                print("pan stop")
            if center_y < 240 - margin_y:
                print("tilt up")
            elif center_y > 240 + margin_y:
                print("tilt down")
            else:
                print("tilt stop")
            
            
    cv2.imshow("VideoFrame",frame)       # show original frame
    '''
    cv2.imshow('blue', res)           # show applied blue mask
    cv2.imwrite("blue.png", res)
    cv2.imshow('Green', res1)          # show applied green mask
    cv2.imwrite("green.png", res1)
    cv2.imshow('red', res2)          # show applied red mask
    cv2.imwrite("red.png", res2)
    '''
    k = cv2.waitKey(5) & 0xFF
        
    if k == 27:
        break
   
        
webcam.release()
cv2.destroyAllWindows()

usb_cam 활용해서 ros2

ros2 pkg create opencv --build-type ament_python --dependencies rclpy

sudo apt install ros-foxy-cv-bridge

grayscale.py

import rclpy 
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2
 
class ImageConvertor(Node):
  def __init__(self):
    super().__init__('image_subscriber')
    self.subscription = self.create_subscription(
      Image, 
      '/image_raw', 
      self.get_img_cb, 
      10)
    self.img_pub = self.create_publisher(Image, 'image_gray', 10)
    self.subscription # prevent unused variable warning
      
    # Used to convert between ROS and OpenCV images
    self.bridge = CvBridge()
   
  def get_img_cb(self, msg):
    #self.get_logger().info('---')
 
    cv_img = self.bridge.imgmsg_to_cv2(msg, "bgr8")
    img_gray = cv2.cvtColor(cv_img, cv2.COLOR_BGR2GRAY)
    '''
    cv2.imshow("grayscale", img_gray)
    
    cv2.waitKey(1)
    '''
    img_msg = self.bridge.cv2_to_imgmsg(img_gray)
    self.img_pub.publish(img_msg)
def main(args=None):
  
  # Initialize the rclpy library
  rclpy.init(args=args)
  
  # Create the node
  node = ImageConvertor()
  
  # Spin the node so the callback function is called.
  rclpy.spin(node)
  
  # Destroy the node explicitly
  # (optional - otherwise it will be done automatically
  # when the garbage collector destroys the node object)
  image_subscriber.destroy_node()
  
  # Shutdown the ROS client library for Python
  rclpy.shutdown()
  
if __name__ == '__main__':
  main()

setup.py에서 ‘grayscale = opencv.script.grayscale:main’, 하고
빌드 소스 하고

1번 터미널

ros2 run usb_cam usb_cam_node_exe

2번 터미널

ros2 run opencv grayscale

3번 터미널

ros2 run rqt_image_view rqt_image_view

확인 가능

ros2 topic list

/*
/camera_info
/image_gray
/image_raw
/image_raw/compressed
/image_raw/compressedDepth
/image_raw/theora
/parameter_events
/rosout
*/

터틀봇3 카메라로 get_blue.py

터틀봇3 카메라로 파란색 인식하기

import rclpy 
from rclpy.node import Node
from sensor_msgs.msg import Image
from sensor_msgs.msg import CompressedImage

from cv_bridge import CvBridge
import cv2
import numpy as np
 
class FindBlue(Node):
    def __init__(self):
        super().__init__('find_blue')
        self.create_subscription(CompressedImage, '/camera/image/compressed', self.get_img_cb, 10)
        self.cv_img = cv2.imread("/home/lej/blank.png", cv2.IMREAD_COLOR)
        self.bridge = CvBridge()
   
    def get_img_cb(self, msg):
        #self.get_logger().info('---')
        self.cv_img = self.bridge.compressed_imgmsg_to_cv2(msg, "bgr8")
    
def main(args=None):
  
    # Initialize the rclpy library
    rclpy.init(args=args)
  
    # Create the node
    node = FindBlue()
  
    try:    
        while rclpy.ok():
            rclpy.spin_once(node, timeout_sec=0.1)
            
            frame = node.cv_img
            
            hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
            lower_blue = np.array([100,100,120])
            upper_blue = np.array([150,255,255])
            mask = cv2.inRange(hsv, lower_blue, upper_blue)
            res = cv2.bitwise_and(frame, frame, mask=mask)
            
            gray = cv2.cvtColor(res, cv2.COLOR_BGR2GRAY)
            _, bin = cv2.threshold(gray, 30, 255, cv2.THRESH_BINARY)
            contours, _ = cv2.findContours(bin, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
            
            largest_contour = None
            largest_area = 0    
            COLOR = (0, 255, 0)
            for cnt in contours:
                area = cv2.contourArea(cnt)
                if area > largest_area:
                    largest_area = area
                    largest_contour = cnt                
                    # draw bounding box with green line
                if largest_contour is not None:
                    #area = cv2.contourArea(cnt)
                    if largest_area > 500:  # draw only larger than 500
                        x, y, width, height = cv2.boundingRect(largest_contour)       
                        cv2.rectangle(frame, (x, y), (x + width, y + height), COLOR, 2)
                        center_x = x + width//2
                        center_y = y + height//2
                        print("center: ( %s, %s )"%(center_x, center_y))
            cv2.imshow("VideoFrame",frame)       # show original frame
            k = cv2.waitKey(5) & 0xFF
            if k == 27:
                break
        node.destroy_node()
        rclpy.shutdown()
            
    except KeyboardInterrupt:
        node.get_logger().info('Keyboard Interrupt(SIGINT)')
        
    finally:
        # (optional - otherwise it will be done automatically
        # when the garbage collector destroys the node object)
        node.destroy_node()
        
        # Shutdown the ROS client library for Python
        rclpy.shutdown()
    
if __name__ == '__main__':
  main()

파란색 따라가기 track_blue2.py

터틀봇3 카메라로 파란색 따라가기

import rclpy 
from rclpy.node import Node
from sensor_msgs.msg import Image
from sensor_msgs.msg import CompressedImage
from geometry_msgs.msg import Twist

from cv_bridge import CvBridge
import cv2
import numpy as np

# Turtlebot3 Specification
MAX_LIN_SPEED =  0.22
MAX_ANG_SPEED =  2.84

# make default speed of linear & angular
LIN_SPEED = MAX_LIN_SPEED * 0.075
ANG_SPEED = MAX_ANG_SPEED * 0.075

class TrackBlue(Node):
    def __init__(self):
        super().__init__('track_blue')
        self.create_subscription(CompressedImage, '/camera/image/compressed', self.get_img_cb, 10)
        self.cv_img = cv2.imread("/home/lej/blank.png", cv2.IMREAD_COLOR)
        self.bridge = CvBridge()
        self.x = self.y = self.w = self.h = 0
        self.object_detected = False
   
    def get_img_cb(self, msg):
        self.cv_img = self.bridge.compressed_imgmsg_to_cv2(msg, "bgr8")
    
def main(args=None):
  
    # Initialize the rclpy library
    rclpy.init(args=args)
  
    # Create the node
    node = TrackBlue()
    tw = Twist()
    pub = node.create_publisher(Twist, '/cmd_vel', 10)
  
    try:    
        while rclpy.ok():
            rclpy.spin_once(node, timeout_sec=0.1)
            
            frame = node.cv_img
            
            hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
            lower_blue = np.array([100,100,120])
            upper_blue = np.array([150,255,255])
            mask = cv2.inRange(hsv, lower_blue, upper_blue)
            res = cv2.bitwise_and(frame, frame, mask=mask)
            
            gray = cv2.cvtColor(res, cv2.COLOR_BGR2GRAY)
            _, bin = cv2.threshold(gray, 30, 255, cv2.THRESH_BINARY)
            contours, _ = cv2.findContours(bin, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
            
            largest_contour = None
            largest_area = 0    
            COLOR = (0, 255, 0)
            for cnt in contours:
                area = cv2.contourArea(cnt)
                if area > largest_area:
                    largest_area = area
                    largest_contour = cnt                

            if largest_contour is not None:
                if largest_area > 500:  # draw only larger than 500
                    node.object_detected = True
                    x, y, width, height = cv2.boundingRect(largest_contour)
                    node.x = x; node.y = y
                    node.w = width; node.h = height
                    cv2.rectangle(frame, (x, y), (x + width, y + height), COLOR, 2)
                    center_x = x + width//2
                    center_y = y + height//2
                    print(f"center: ({center_x}, {center_y}), area: {largest_area}")

                    # Adjust angular velocity based on horizontal position
                    if node.x > 180:
                        tw.angular.z = -ANG_SPEED * 0.5
                    elif node.x < 140:
                        tw.angular.z = ANG_SPEED * 0.5
                    else:
                        tw.angular.z = 0.0

                    # Adjust linear velocity based on area (distance)
                    if largest_area < 1500:  # Too far
                        tw.linear.x = LIN_SPEED
                    elif largest_area > 3000:  # Too close
                        tw.linear.x = -LIN_SPEED
                    else:
                        tw.linear.x = 0.0

                    pub.publish(tw)
                
            else:
                # No blue object detected
                if node.object_detected:
                    print("No blue object detected. Stopping.")
                    tw.linear.x = 0.0
                    tw.angular.z = 0.0
                    pub.publish(tw)
                    node.object_detected = False

            cv2.imshow("VideoFrame", frame)  # show original frame
            k = cv2.waitKey(5) & 0xFF
            if k == 27:
                break

        tw.linear.x = tw.angular.z = 0.0
        pub.publish(tw)
        node.destroy_node()
        rclpy.shutdown()
            
    except KeyboardInterrupt:
        node.get_logger().info('Keyboard Interrupt(SIGINT)')
        
    finally:
        tw.linear.x = tw.angular.z = 0.0
        pub.publish(tw)
        node.destroy_node()
        rclpy.shutdown()
    
if __name__ == '__main__':
  main()