[ROS] Tracking a visual target on ros by example vol 1

안녕하세요.

물체 추적하는것 관련으로 검색을 하다가 꽤 괜찮은 방법이 있어 정리해보게 됐습니다.

아래 책에 물체추적하는 파트가 있는데 제 하드웨어에 맞춰서 코드를 작성해봤습니다.

아 그리고, 이 글의 특징은 ROS 상에서 동작한 다는 것입니다.

ROS 관련 예제 관련 책, 무료로 제공되는 pdf

https://github.com/StevenShiChina/books/blob/master/ros%20by%20example%20vol%201%20indigo.pdf

챕터 12.11 tracking a visual target

To track the target, the script uses a kind of “speed tracking”. If you move the camera to
the position where the tracked object is now, it may have moved by the time the camera
gets there. You might think you could just update the camera’s target position at a high
rate and therefore keep up with the object. However, this kind of position tracking will
result in a jerky staccato-like motion of the camera. A better strategy is to always aim
the camera ahead of the target, but adjust the servo speeds to be proportional to the
displacement of the target from the center of view. This results in much smoother
camera motion and will ensure that it moves quickly if the target is far off-center and
more slowly if the displacement is small. When the target is centered, the servo speeds
will be zero and so the camera will not move.

이 책에서는 트래킹을 하기 위한 방법을 설명한다.

트래킹 하는 물체가 항상 가운데가 올 수 있도록 한다. 물체의 위치를 계속 업데이트 하면서 그 위치를 추적한다.

하지만, 단순히 물체를 업데이트하고 물체의 위치가 가운데가 될 수 있도록 움직인다면,

갑자기 움직이거나, 혹은 렉걸린것 같은? 툭툭툭, 자연스럽게 스무디하게 움직이지 못한다.

그래서 아래와 같은 방법을 제안한다.

A better strategy is to always aim the camera ahead of the target, but adjust the servo speeds to be proportional to the
displacement of the target from the center of view.

화면중심으로부터 타겟 물체가 얼마나 떨어져있는지 거리에 따라 비례하여 모터의 속도를 정한다.

그렇게 함으로써, 좀더 스무스하게 움직일 수 있다.


얼굴 추적을 위해 필요한 입력은 아래와 같다.

  1. 이미지 상에서 타겟 물체의 u,v 위치
  2. 모터의 현재 각도

필요한 출력은 아래와 같다.

  1. 모터의 각도 설정 (pan 은 30도, tilt는 20도로 돌아라)
  2. 모터의 속도 설정 (속도는 5rad/s로 돌아라!)
    • 저는 따로 속도는 설정하지 않았습니다.
    • 얼굴 검출 주기가 빠르다면, 얼굴이 중심으로 많이 벗어날 일이 없을 것이라고 가정했습니다.

필요한 파라미터는 아래와 같다.

  1. delta_x, delta_y : 현재 모터의 각도에서 한번에 움직일 최소 각도 설정
  2. pan_threshold, tilt_threshold : 중심에서 어느정도 벗어나있으면 모터를 움직일 것인가에 대한 임계값

ROS 코드로 보면 아주 단순합니다.

아래 코드를 보면,

  • 얼굴 정보를 가져오는 subscriber, 얼굴 정보가 들어올 때마다 faceCallback 함수가 실행됩니다.
  • 현재 모터의 각도를 가져오는 subscriber, 현재 pan-tilt 컨트롤러의 각 joint 각도를 가져옵니다.
  • 모터 컨트롤러에게 각 조인트가 움직여야할 목적 각도를 설정, 전송합니다.

[python]
# get face info
rospy.Subscriber("/face", Objects, self.facesCallback, queue_size = 1)
# get current joint
rospy.Subscriber("/joint/angles", Float32MultiArray, self.currentJointCallback, queue_size = 10)
# publish position directly
self.jointAnglePub = rospy.Publisher("/joint/cmd", Float32MultiArray, queue_size = 10)
[/python]


실제 Callback 메서드를 봐보겠습니다.

currentJointCallback에서는 현재 pan, tilt의 각도를 저장하고 있습니다.

facesCallback에서는 검출된 얼굴 위치에 따라, pan, tilt가 움직여야할 각도를 설정하고 있습니다.

여기서 핵심은 얼굴을 가운데로 움직이게 하기 위해 delta만큼 조금씩 움직여서 얼굴을 가운데로 맞춥니다.

[python]

def facesCallback(self, faces):
print ‘facesCallback’
faces = faces.objects # detected
face = faces[0] # one face

face_x = (face.xmin + face.xmax) / 2.0
face_y = (face.ymin + face.ymax) / 2.0
image_width = 640
image_height = 480

target_offset_x = face_x – image_width/2
target_offset_y = face_y – image_height/2

percent_offset_x = float(target_offset_x) / (float(image_width) / 2.0)
percent_offset_y = float(target_offset_y) / (float(image_height) / 2.0)
# print str(percent_offset_x)+ ‘ ‘ + str(percent_offset_y)

if abs(percent_offset_x) > self.pan_threshold:
if target_offset_x >0:
self.cmd_pan = self.cur_pan_angle – self.delta_x
else:
self.cmd_pan = self.cur_pan_angle + self.delta_x

if abs(percent_offset_y) > self.tilt_threshold:
if target_offset_y > 0:
self.cmd_tilt = self.cur_tilt_angle – self.delta_y
else:
self.cmd_tilt = self.cur_tilt_angle + self.delta_y

joints = Float32MultiArray()
joints.data = [self.cmd_pan, self.cmd_tilt]
self.jointAnglePub.publish(joints)
print joints.data

def currentJointCallback(self, joint_angles):
self.cur_pan_angle = joint_angles.data[0]
self.cur_tilt_angle = joint_angles.data[1]

[/python]


영상

 

 

결론

항상 말했던 것 같지만, ROS를 쓰면 모든 하드웨어, 소프트웨어 모듈을 분리, 각각 관리할 수 있습니다.

여기서 얼굴을 subscribe 했지만, 얼굴이 아니라 사람이여도 되고, 물체여도 되고, 우리가 원하는 형태의 인풋만 받으면 됩니다.

마찬가지로 어떤 모터컨트롤러인지 상관없이 현재 모터의 각도를 받을 수 있고, 모터에게 특정 각도로 가라는 명령을 할 수 있다면

어떤 모터컨트롤러든지 사용할 수 있습니다. 자기가 직접 만든 모터컨트롤러든, 다이나믹셀이든, 아두이노든 어떤 걸 써도

얼굴 검출의 코드는 전혀 바뀌지 않습니다.

글 읽어주셔서 감사합니다.

댓글 남기기

이메일은 공개되지 않습니다. 필수 입력창은 * 로 표시되어 있습니다