24. [ROS RC카] 응용 실습, Object Following - 2

계속해서 다른 node들도 분석해 보겠습니다.

그 전에! 복습을 하고 넘어가겠습니다 😎

rqt_graph

csi_pub : 카메라 이미지를 계속해서 송출해주는 노드이지요?

blob_detector : 컴퓨터 비전이 적용된 노드로, 녹색 물체를 찾아내고, 지속적으로 추적하는 노드

chase_ball : 이전 노드에서 녹색 물체의 위치가 픽셀 비율 단위로 오게 됩니다. 이 픽셀의 위치에 따라서 제어 신호를 Twist 형식으로 변환합니다.

image_view : 이전에도 살펴보았던, 이미지 토픽을 볼 수 있게 해주는 노드입니다.

blob_chase_node : 실질적인 PWM 제어를 담당하는 노드입니다.

chase_the_ball.py

#!/usr/bin/python
"""
Gets the position of the blob and it commands to steer the wheels

referenced from tizianofiorenzani/ros_tutorials
url: https://github.com/tizianofiorenzani/ros_tutorials

Subscribes to 
    /blob/point_blob
    
Publishes commands to 
    /dkcar/control/cmd_vel    

"""
import math, time
import rospy
from geometry_msgs.msg import Twist
from geometry_msgs.msg import Point

K_LAT_DIST_TO_STEER = 2.0


def saturate(value, min, max):
    if value <= min:
        return min
    elif value >= max:
        return max
    else:
        return value


class ChaseBall:
    def __init__(self):

        self.blob_x = 0.0
        self.blob_y = 0.0
        self._time_detected = 0.0

        self.sub_center = rospy.Subscriber("/blob/point_blob", Point, self.update_ball)
        rospy.loginfo("Subscribers set")

        self.pub_twist = rospy.Publisher("/dkcar/control/cmd_vel", Twist, queue_size=5)
        rospy.loginfo("Publisher set")

        self._message = Twist()

        self._time_steer = 0
        self._steer_sign_prev = 0

    @property
    def is_detected(self):
        return time.time() - self._time_detected < 1.0

    def update_ball(self, message):
        self.blob_x = message.x
        self.blob_y = message.y
        self._time_detected = time.time()
        # rospy.loginfo("Ball detected: %.1f  %.1f "%(self.blob_x, self.blob_y))

    def get_control_action(self):
        """
        Based on the current ranges, calculate the command

        Steer will be added to the commanded throttle
        throttle will be multiplied by the commanded throttle
        """
        steer_action = 0.0
        throttle_action = 0.0

        if self.is_detected:
            # --- Apply steering, proportional to how close is the object
            steer_action = -K_LAT_DIST_TO_STEER * self.blob_x
            steer_action = saturate(steer_action, -1.5, 1.5)
            rospy.loginfo("Steering command %.2f" % steer_action)
            throttle_action = 1.0

        return (steer_action, throttle_action)

    def run(self):

        # --- Set the control rate
        rate = rospy.Rate(5)

        while not rospy.is_shutdown():
            # -- Get the control action
            steer_action, throttle_action = self.get_control_action()

            rospy.loginfo("Steering = %3.1f" % (steer_action))

            # -- update the message
            self._message.linear.x = throttle_action
            self._message.angular.z = steer_action

            # -- publish it
            self.pub_twist.publish(self._message)

            rate.sleep()


if __name__ == "__main__":

    rospy.init_node("chase_ball")

    chase_ball = ChaseBall()
    chase_ball.run()

주석에도 나와 있듯이 이 node는 ⇒ /blob/point_blob를 Subscribes 받아서

⇒ Twist 형식으로 변환한 후,

⇒ /dkcar/control/cmd_vel에게 Publish 합니다.

import

import math, time
import rospy
from geometry_msgs.msg import Twist
from geometry_msgs.msg import Point

/blob/point_blob 는 geometry_msgs/Point형식을 갖는데요, 어떻게 생겼을까요?

$ rosmsg show geometry_msgs/Point
float64 x
float64 y
float64 z

들어오게 되는 x,y,z 가 어떠한 의미를 갖는지는 그 앞단인 find_ball에서 의미를 갖습니다.

지금은 우선 전체적인 흐름을 살펴보겠습니다.

main & run

    def run(self):

        # --- Set the control rate
        rate = rospy.Rate(5)

        while not rospy.is_shutdown():
            # -- Get the control action
            steer_action, throttle_action = self.get_control_action()

            rospy.loginfo("Steering = %3.1f" % (steer_action))

            # -- update the message
            self._message.linear.x = throttle_action
            self._message.angular.z = steer_action

            # -- publish it
            self.pub_twist.publish(self._message)

            rate.sleep()


if __name__ == "__main__":

    rospy.init_node("chase_ball")

    chase_ball = ChaseBall()
    chase_ball.run()

run의 하단부는 msg를 채우고, publish를 하는 부분이기에 topic 강의를 수강하셨다면 모두 이해하실 수 있으며, 우리는 상단의 get_control_action 함수를 중점적으로 살피겠습니다.

get_control_action

    def get_control_action(self):
        """
        Based on the current ranges, calculate the command

        Steer will be added to the commanded throttle
        throttle will be multiplied by the commanded throttle
        """
        steer_action = 0.0
        throttle_action = 0.0

        if self.is_detected:
            # --- Apply steering, proportional to how close is the object
            steer_action = -K_LAT_DIST_TO_STEER * self.blob_x
            steer_action = saturate(steer_action, -1.5, 1.5)
            rospy.loginfo("Steering command %.2f" % steer_action)
            throttle_action = 1.0

        return (steer_action, throttle_action)

K_LAT_DIST_TO_STEER 가 무엇일까요? 예를 들어 살펴보겠습니다.

결론부터 말씀드리자면, blob_x는 -1 ~ 1을 넘을 수 없는 값입니다.

그렇다면, K_LAT_DIST_TO_STEER = 2.0인 현 상황에서

⇒ steer_action는 -2.0 ~ 2.0의 값을 가진다고 할 수 있는데요, 이것을 saturate시키므로, 다시

⇒ -1.5 ~ 1.5의 값을 갖는 steer_action을 얻게 됩니다.

            # -- update the message
            self._message.linear.x = throttle_action
            self._message.angular.z = steer_action

            # -- publish it
            self.pub_twist.publish(self._message)

이 steer_action 값을 Twist에 담아 최종적으로 publish하게 되며,

그렇다면 K_LAT_DIST_TO_STEER란,

얼마나 민감하게 젯슨 박스에 반응할 것인지를 결정짓는 상수라고 할 수 있습니다!

💡

K_LAT_DIST_TO_STEER 가 크다면, 젯슨 박스가 조금만 중심에서 오른쪽으로 이동해도 바로 바퀴가 오른쪽으로 확 꺾이게 됩니다.

아직까진 크게 와닿지 않으실 것 같은데요, 서둘러 마지막 find_ball로 넘어가 보도록 하겠습니다.

find_ball

#!/usr/bin/env python

"""
ON THE RASPI: roslaunch raspicam_node camerav2_320x240.launch enable_raw:=true

   0------------------> x (cols) Image Frame
   |
   |        c    Camera frame
   |         o---> x
   |         |
   |         V y
   |
   V y (rows)


referenced from tizianofiorenzani/ros_tutorials
url: https://github.com/tizianofiorenzani/ros_tutorials

SUBSCRIBES TO:
    /raspicam_node/image: Source image topic
    
PUBLISHES TO:
    /blob/image_blob : image with detected blob and search window
    /blob/image_mask : masking    
    /blob/point_blob : blob position in adimensional values wrt. camera frame

"""


# --- Allow relative importing
if __name__ == "__main__" and __package__ is None:
    from os import sys, path

    sys.path.append(path.dirname(path.dirname(path.abspath(__file__))))

import sys
import rospy
import cv2
import time

from std_msgs.msg import String
from sensor_msgs.msg import Image
from geometry_msgs.msg import Point
from cv_bridge import CvBridge, CvBridgeError
from include.blob_detector import *


class BlobDetector:
    def __init__(
        self, thr_min, thr_max, blur=15, blob_params=None, detection_window=None
    ):

        self.set_threshold(thr_min, thr_max)
        self.set_blur(blur)
        self.set_blob_params(blob_params)
        self.detection_window = detection_window

        self._t0 = time.time()

        self.blob_point = Point()

        print(">> Publishing image to topic image_blob")
        self.image_pub = rospy.Publisher("/blob/image_blob", Image, queue_size=1)
        self.mask_pub = rospy.Publisher("/blob/image_mask", Image, queue_size=1)
        print(">> Publishing position to topic point_blob")
        self.blob_pub = rospy.Publisher("/blob/point_blob", Point, queue_size=1)

        self.bridge = CvBridge()
        self.image_sub = rospy.Subscriber("/csi_image", Image, self.callback)
        print("<< Subscribed to topic /csi_image")

(...)

def main(args):

    pink_min = (135, 41, 95)
    pink_max = (255, 196, 255)

    green_min = (39, 81, 71)
    green_max = (75, 255, 255)

    blur = 5
    min_size = 10
    max_size = 40

    # --- detection window respect to camera frame in [x_min, y_min, x_max, y_max] adimensional (0 to 1)
    x_min = 0.1
    x_max = 0.9
    y_min = 0.1
    y_max = 0.9

    detection_window = [x_min, y_min, x_max, y_max]

    params = cv2.SimpleBlobDetector_Params()

    # Change thresholds
    params.minThreshold = 0
    params.maxThreshold = 200

    # Filter by Area.
    params.filterByArea = True
    params.minArea = 2000
    params.maxArea = 70000  # 640 * 480 = 307,200

    # Filter by Circularity
    params.filterByCircularity = True
    params.minCircularity = 0.1

    # Filter by Convexity
    params.filterByConvexity = True
    params.minConvexity = 0.2

    # Filter by Inertia
    params.filterByInertia = True
    params.minInertiaRatio = 0.7

    rospy.init_node("blob_detector", anonymous=True)
    ic = BlobDetector(green_min, green_max, blur, params, detection_window)
    try:
        rospy.spin()
    except KeyboardInterrupt:
        print("Shutting down")

    cv2.destroyAllWindows()


if __name__ == "__main__":
    main(sys.argv)

이번 코드도 길이가 굉장히 길지요? 😂😂

코드 분석 전에, 우리 재미있는 실습을 하나 해보아요

Blob Thresholding

$ cd ~/catkin_ws/src/donkey_ros/donkey_cv/include/
$ python range_detector.py --image frame0000.jpg --filter HSV --preview

하단에 제가 해둔 답을 보기 전에, H, S, V Min/Max 값들을 계속해서 바꿔가면서, 어떠한 변화가 생기는지 살펴보세요!!

my_value

HSV가 뭔가요 ❓❓❓

색상을 표현하는 대표적인 방식으로, 모두가 알고 있는 RGB방식이 있지요? 이와 더불어,

색상(Hue), 채도(Saturation), 명도(Value) 의 값으로 색상을 표현하는 체계가 있답니다.

image from : wikipedia

ppt나 포토샵을 사용하다보면, 이렇게 색상을 고르는 gradation을 많이 보셨지요?

바로 이 것이 Hue/Saturation에 해당합니다.

image from : wikipedia

이 세가지 값을 수정하면서, 젯슨 박스만을 걸러주는 범위를 선별하고, 이것을 통해 물체를 따라가도록 할 수 있었던 것입니다. 😉😉 코드상에서는 다음과 같이 표현되었지요.

    green_min = (39, 81, 71)
    green_max = (75, 255, 255)

여러분들도 직접 사진을 촬영하여 다른 필터를 만들어보세요

$ roscore
$ rosrun csi_camera csi_pub.py
$ rosrun image_view image_view image:=/csi_image

저작자표시 비영리 동일조건

Swiming_Lab

24. [ROS RC카] 응용 실습, Object Following - 2

24. [ROS RC카] 응용 실습, Object Following - 2

rqt_graph

chase_the_ball.py

find_ball

Blob Thresholding

HSV가 뭔가요 ❓❓❓

티스토리툴바