物体を追いかけて映すカメラ

前回、物体を色相を特徴として追跡する方法を紹介しました。これにより、対象物がカメラ画像のどこに映っているのか、追跡できるようになりました。

今回は、その追跡情報を利用し、物体を追いかけて映すパンチルトカメラを作ってみます。

カメラが物体を追跡する様子

カメラが物体を追跡する様子を上の動画に記録しました。

左2つのウインドウはCamShiftノードによるもの、右はRVizで実機の状態をモニタリングしているところです。

カメラのパン/チルトが間に合わないくらい速く物体を動かしても、追跡が途絶えなければ、カメラが物体を追いかけていることがわかります。

制御方法

前回製作した、物体追跡を行うCamShiftノードは、追跡結果を"/roi"トピックにpublishします。これをsubscribeして追跡結果をもとにカメラを制御するfollowerノードを作りました。

カメラノード、CamShiftノード、followerノードを起動すると、パンチルトカメラが物体を追跡します。

$roslaunch ptcam_vision usb_cam.launch
$roslaunch ptcam_vision camshift.launch
$roslaunch ptcam_arduino ptcam_follower.launch
		

followerノードは、どのようにカメラを制御しているのでしょう? 種明かしに、そのソースコードを載せます。

#!/usr/bin/env python

''' follower.py - Version 1.0 2016-03-27

'''

import rospy
from std_msgs.msg import Float64
from sensor_msgs.msg import RegionOfInterest, JointState, CameraInfo
from math import radians, degrees
from ros_arduino_python.servo_controller import Servo
import dynamic_reconfigure.server
from ptcam_arduino.cfg import FollowerConfig

class Follower(object):
    def __init__(self, name, cmdpub, reso):
        self.name = name
        self.cmdpub = cmdpub
        prmns = "~follower/" + name + "/"
        self.reso_2 = reso / 2
        self.range_max = radians(rospy.get_param(prmns + "range_max", 90.0))
        self.range_min = radians(rospy.get_param(prmns + "range_min", -90.0))
        self.setparam(rospy.get_param(prmns + "gain", 30.0), rospy.get_param(prmns + "dzone", 10.0))
        
    def follow(self, roipos, crrpos):
        fb = (roipos - self.reso_2) * self.gain
        if abs(fb) > self.dzone:
            cmd = crrpos - fb
            cmd = min(self.range_max, cmd)
            cmd = max(self.range_min, cmd)
            try:
                self.cmdpub.publish(cmd)
            except:
                rospy.loginfo("Publishing command failed")
                
    def setparam(self, gain, dzone):
        self.gain = radians(gain) / self.reso_2
        self.dzone = radians(dzone)


class FollowServo(object):
    def __init__(self, node_name):
        self.node_name = node_name
        self.roi_minarea = rospy.get_param("~follower/roi_minratio", 0.07)  # area ratio in %
        rospy.init_node(node_name)
        rospy.loginfo("Starting node " + str(node_name))
        rospy.Subscriber("/camera/rgb/camera_info", CameraInfo, self.camera_info_cb)
        self.pancmd_pub = rospy.Publisher("/pan/command", Float64, queue_size=1)
        self.tilcmd_pub = rospy.Publisher("/tilt/command", Float64, queue_size=1)
        self.crrpos = (0.0, 0.0)
        self.camreso = None
        while not rospy.is_shutdown() and self.camreso is None:
            rospy.sleep(0.1)
        self.follow_pan = Follower("pan", self.pancmd_pub, self.camreso[0])
        self.follow_til = Follower("tilt", self.tilcmd_pub, self.camreso[1])
        self.roi_minarea = self.roi_minarea * self.camreso[0] * self.camreso[1] / 100
        rospy.Subscriber("/roi", RegionOfInterest, self.roi_cb)
        rospy.Subscriber("/joint_states", JointState, self.jointstat_cb)
        dyncfg_srv = dynamic_reconfigure.server.Server(FollowerConfig, self.dyncfg_cb)
        
    def roi_cb(self, roi):
        if roi.width * roi.height > self.roi_minarea:
            x = roi.x_offset + roi.width / 2
            y = roi.y_offset + roi.height / 2
            self.follow_pan.follow(x, self.crrpos[1])
            self.follow_til.follow(y, self.crrpos[0])

    def jointstat_cb(self, msg):
        self.crrpos = msg.position

    def camera_info_cb(self, msg):
        self.camreso = (msg.width, msg.height)
        
    def dyncfg_cb(self, config, level):
        if self.camreso is not None:
            self.roi_minarea = config.roi_minarea * self.camreso[0] * self.camreso[1] / 100
        self.follow_pan.setparam(config.pan_gain, config.pan_dzone)
        self.follow_til.setparam(config.tilt_gain, config.tilt_dzone)
        return config
        
if __name__ == '__main__':
    try:
        node_name = "follower"
        FollowServo(node_name)
        rospy.spin()
    except KeyboardInterrupt:
        print"Shutting down " + str(node_name)
		

このうち、制御のキモは次の数行です。

    def follow(self, roipos, crrpos):
        fb = (roipos - self.reso_2) * self.gain
        if abs(fb) > self.dzone:
            cmd = crrpos - fb
            cmd = min(self.range_max, cmd)
            cmd = max(self.range_min, cmd)
            try:
                self.cmdpub.publish(cmd)
		

fb=・・・のところで対象物の画面中央からの変位を計算、それにゲインをかけてフィードバック量を求めています。

cmd = crrpos - fbで、現在位置crrposからfbを引いてNFBをかけています。cmdは、サーボの目標角度です。

実はフィードバック制御

カメラは物体を追いかけていますが、実は画面の中央に物体がくるように、パンとチルトをそれぞれフィードバック制御しているだけなのです。

このような簡単な制御でも、案外うまくいくものですね。

ros_arduino_bridgeがpublishするjoint_statesトピックにより、現在のパン/チルト角度を得ることが可能であり、これが制御に大いに役立っています。

もっと色々やるには

うまく動き、目的も達成してはいるものの、技術的な物足りなさも感じます。

それは、「空間における物体の位置を認識して、その方向にカメラを向けるように制御している」わけではないことが、物足りなさの原因ではないでしょうか。

現在のカメラの向きと画像内の物体の位置が判っているので、パンチルトカメラ本体に対する物体の方向も推定できるはずです。

このあたりのことを課題に、ロボットビジョンの他の技術を何か、また紹介したいと思います。