物体を追いかけて映すカメラ
前回、物体を色相を特徴として追跡する方法を紹介しました。これにより、対象物がカメラ画像のどこに映っているのか、追跡できるようになりました。
今回は、その追跡情報を利用し、物体を追いかけて映すパンチルトカメラを作ってみます。
カメラが物体を追跡する様子
カメラが物体を追跡する様子を上の動画に記録しました。
左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トピックにより、現在のパン/チルト角度を得ることが可能であり、これが制御に大いに役立っています。
もっと色々やるには
うまく動き、目的も達成してはいるものの、技術的な物足りなさも感じます。
それは、「空間における物体の位置を認識して、その方向にカメラを向けるように制御している」わけではないことが、物足りなさの原因ではないでしょうか。
現在のカメラの向きと画像内の物体の位置が判っているので、パンチルトカメラ本体に対する物体の方向も推定できるはずです。
このあたりのことを課題に、ロボットビジョンの他の技術を何か、また紹介したいと思います。