はじめに
本記事では、移動型ロボットが事前に登録されたパスをたどりつつも後方から追随するユーザーを置いてけぼりにしないような「道案内ロボット」の実装について紹介します。
前半では、指標物体の検知とその結果をトピックとしてパブリッシュする方法を紹介し、後半では、得られたデータをサブスクライブし、ゴールを更新したりキャンセルしたりする方法について紹介します。
なお、後半はこちら
実行環境
PC
| 項目 | スペック |
|---|---|
| CPU | Core i7-9750H |
| OS | Ubuntu 20.04LTS |
| ROS2 | Foxy Fitzroy |
Raspberry Pi 4
| 項目 | スペック |
|---|---|
| CPU | 1.5GHz クアッドコア Cortex-A72(ARMv8、64bit L1=32KB、L2=1M) |
| OS | Ubuntu 20.04LTS |
| ROS2 | Foxy Fitzroy |
| Turtlebot3 | Waffle Pi |
1.画像処理の結果を送信するメッセージを作成する
今回は指標物体にピンポン玉を選択し、ウェブカメラの画像上のピンポン玉の位置と半径をパブリッシュするためにメッセージを作成しました。
新しくImgproc.msgを作成し3つのfloatで構成しました。
float64 x
float64 y
float64 radius
ビルドして. install/setup.bashしてから、ros2 interface listで imgproc_msgs/msg/Imgprocがあることを確認してください。
2.OpenCVで画像処理
カラーボールの検知を行っていたこちらのサイトに掲載されていたプログラムを参考に画像処理ノードをつくりました。
import rclpy
from rclpy.node import Node
from std_msgs.msg import String
from imgproc_msgs.msg import Imgproc
import cv2
import numpy as np
class ImageProcessor(Node):
def __init__(self):
super().__init__('imageProcessor')
self.publisher_ = self.create_publisher(Imgproc, 'img_data', 10)
timer_period = 0.125 # seconds
self.timer = self.create_timer(timer_period, self.timer_callback)
self.cap = cv2.VideoCapture(0)
self.marker_x = 0.0
self.marker_y = 0.0
self.marker_radius = 0.0
def timer_callback(self):
self._, self.frame = self.cap.read()
self.frame = cv2.resize(self.frame, (int(self.frame.shape[1]/2), int(self.frame.shape[0]/2)))
self.res_orange = self.getMask([0,50,100], [25,255,255])
self.contours_frame = self.getContours(self.res_orange, 50, 30)
#cv2.imshow('video',self.contours_frame)
msg = Imgproc()
msg.x = float(self.marker_x)
msg.y = float(self.marker_y)
msg.radius = float(self.marker_radius)
self.publisher_.publish(msg)
self.get_logger().info('Publishing (radius): "%s"' % msg.radius)
def getMask(self, l, u):
hsv = cv2.cvtColor(self.frame, cv2.COLOR_BGR2HSV)
lower = np.array(l)
upper = np.array(u)
if(lower[0] >= 0):
mask = cv2.inRange(hsv, lower, upper)
else:
h = hsv[:, :, 0]
s = hsv[:, :, 1]
v = hsv[:, :, 2]
mask = np.zeros(h.shape, dtype=np.uint8)
mask[((h < lower[0]*-1) | h > upper[0]) & (s > lower[1]) & (s < upper[1]) & (v > lower[2]) & (v < upper[2])] = 255
return cv2.bitwise_and(self.frame,self.frame, mask= mask)
def getContours(self, img, t, r):
gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
ret, thresh = cv2.threshold(gray, t, 255, cv2.THRESH_BINARY)
contours, _= cv2.findContours(thresh, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
largest_radius = 0.0
largest_x = 0.0
largest_y = 0.0
if len(contours) > 0:
for cnt in contours:
(x,y), radius = cv2.minEnclosingCircle(cnt)
center = (int(x),int(y))
radius = int(radius)
radius_frame = cv2.circle(self.frame,center,0,(0,255,0),0)
if radius > r:
if radius > largest_radius:
largest_radius = radius
largest_x = x
largest_y = y
radius_frame = cv2.circle(self.frame,center,radius,(0,255,0),2)
self.marker_radius = float(largest_radius)
self.marker_x = float(largest_x)
self.marker_y = float(largest_y)
return radius_frame
else:
self.marker_radius = 0.0
return self.frame
def __del__(self):
self.cap.release()
cv2.destroyAllWindows()
self.get_logger().info('Closing camera')
def main(args=None):
rclpy.init(args=args)
imgproc = ImageProcessor()
rclpy.spin(imgproc)
imgproc.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
画像処理の中の内容については引用元を参照いただきたいですが、ざっくり説明すると、特定の色（HSV形式）でマスクをしてから輪郭抽出するという流れになります。
また、from imgproc_msgs.msg import Imgprocで作成したImgproc型のimportをしています。publishする際のトピック名はimg_dataとしています。TB3のラズパイで回すことを想定しているのでcv2.imshowはコメントアウトにしています。
なお、画像処理のノードを立てるパッケージはこちらにまとめてあります。
ノードを立てるとこのようにデータがPublishされているのがわかると思います。
次回
次回後編では、画像データをサブスクライブしてナビゲーションの実行やキャンセルさせる方法について紹介します。
参考資料