LoginSignup
7
9

More than 1 year has passed since last update.

@Yuya-Shimizu(清水 優椰)

ROSの勉強　第24弾：ライントレース（OpenCV）

Last updated at Posted at 2021-03-15

#プログラミング ROS< ライントレース（OpenCV） >

はじめに

1つの参考書に沿って，ROS(Robot Operating System)を難なく扱えるようになることが目的である．その第24弾として，「ライントレース（OpenCV）」を扱う．

環境

仮想環境
ソフト VMware Workstation 15
実装RAM 2 GB
OS Ubuntu 64 ビット
isoファイル ubuntu-mate-20.04.1-desktop-amd64.iso
コンピュータ
デバイス MSI
プロセッサ Intel(R) Core(TM) i5-7300HQ CPU @ 2.50GHz 2.50GHz
実装RAM 8.00 GB (7.89 GB 使用可能)
OS Windows (Windows 10 Home, バージョン：20H2)
ROS
Distribution noetic
プログラミング言語 Python 3.8.5
シミュレーション gazebo

タスク

以下に示すようなコースにおいて，黄色の線上を移動するプログラムを組む．ここで使えるものとしては，カメラ画像とする．
course

OpenCV

画像に対する処理としては，OpenCVが有名である．ROSには，OpenCV間でデータを受け渡すためのパッケージが用意されているため，ここでもOpenCVを活用し，タスクを達成することとする．OpenCVについては過去にチュートリアルでひと通り学習した．今回使う内容についてのみ，改めて簡単に示しておく．

画像の取得

画像データについて

まず，画像を取得するにあたって，使用するカメラについて知る必要がある．今回はgazeboのwaffleというロボットに搭載されているカメラを使用する．ここで，配信されている画像データについて確認する．以下に
roslaunch turtlebot3_gazebo turtlebot3_world.launchを実行後にrostopic listを実行した結果を示す．
camera_topic
上では，カメラに関する部分のみ示している．それぞれについて簡単にまとめておく．
camera/depth/：キャリブレーションデータと深度センサのデータを扱う
camera/rgb/image_raw：一般的なカラー画像
camera/rgb/image_raw/compressed：圧縮画像（画質劣化）
camera/rgb/image_raw/theora：ビデオストリームとして圧縮（効率的な圧縮）

※圧縮は，Wi-Fi接続など帯域幅の制限のある接続を使って制御する場合に使われることがある．これは，圧縮によりネットワーク帯域をかなり節約する効果をもたらすからであるが，画質の劣化とおそらく処理負荷という遅延という犠牲を払うことになる．
※一般に，人間の遠隔操作をサポートするのが目的であれば，圧縮ビデオストリームを使うことは理にかなっている．しかしながら，画像処理を行う場合には，できる限り非圧縮の画像を使うほうが良い性能が得られる．

実装

先ほどの説明で，画像データはcamera/rgb/image_rawトピックで入手できることがわかったので，このデータを購読する最小限のノードを書くことができる．以下では，ソースコードと実際に購読できているかの確認を示す．

ソースコード
follower.py
#! /usr/bin/env python3

import rospy
from sensor_msgs.msg import Image

def image_callback(msg):
	#ここに画像データに対する処理を記述する
	pass

rospy.init_node('follower')	#'follower'という名前でノードを初期化
image_sub = rospy.Subscriber('camera/rgb/image_raw', Image, image_callback)	#Image型で画像トピックを購読し，コールバック関数を呼ぶ
rospy.spin()	#ループ

これは，とりあえず画像データを購読するということを行うだけである．これを実行して，rosnode topicを別のターミナル(端末)で実行したときの様子を示す．
rosnode
確かに，followというノードが作成されている．次に本当に画像データを購読できているのかをrosnode info /followerにより確認する．その結果を次に示す．
follower_node
購読できていると確認できる．

OpenCVの適用

先ほどのプログラムでは，画像データを購読することができた．続いて，その購読した画像データに対して，表示させるという簡単な処理をして，OpenCVの適用を学ぶ．

実装

ソースコード
follower_opencv.py
#! /usr/bin/env python3

import rospy
from sensor_msgs.msg import Image
import cv2 as cv
import cv_bridge	#ROSとOpenCV間でデータを受け渡すためのパッケージ

class Follower:
	def __init__(self):
		self.bridge = cv_bridge.CvBridge()
		cv.namedWindow('window', 1)	#'window'という名前の画像表示のウィンドウを作成
		self.image_sub = rospy.Subscriber('camera/rgb/image_raw', Image, self.image_callback)	#Image型で画像トピックを購読し，コールバック関数を呼ぶ

	def image_callback(self, msg):
		image = self.bridge.imgmsg_to_cv2(msg, desired_encoding = 'bgr8')	#画像データをOpenCVに受け渡す
		image = cv.resize(image, (image.shape[1]//2, image.shape[0]//2))	#大きすぎるため，サイズ調整
		cv.imshow('window', image)	#'window'ウィンドウにimageを表示
		cv.waitKey(3)	#3ミリ秒待つ

rospy.init_node('follower')	#'follower'という名前でノードを初期化
follower = Follower()	#Followerクラスのインスタンスを作成（init関数が実行される）
rospy.spin()	#ループ
出力

方向を変えた出力結果を2つ示す．なお，実行中にgazeboのGUI機能を使って方向を変えた．
camera_image
camera_image2

走るコース

教材のサンプルにコースを実行するlaunchファイルがあったが，少し修正する部分があったため，ここでロボットを走らせるコースを構築する作業を記録しておく．

ワールドファイル

用意されていたワールドファイルを次に示す．

course.world
<?xml version="1.0"?> 
<sdf version="1.4">
  <world name="default">
    <scene>
      <ambient>0 0 0 1</ambient>
      <shadows>0</shadows>
      <grid>0</grid>
      <background>0.7 0.7 0.7 1</background>
    </scene>
    <!--
    <physics type="ode">
      <gravity>0 0 -9.8</gravity>
      <ode>
        <solver>
          <type>quick</type>
          <iters>10</iters>
          <sor>1.3</sor>
        </solver>
        <constraints>
          <cfm>0</cfm>
          <erp>0.1</erp>
          <contact_max_correcting_vel>10</contact_max_correcting_vel>
          <contact_surface_layer>0.001</contact_surface_layer>
        </constraints>
      </ode>
      <real_time_update_rate>1000</real_time_update_rate>
      <max_step_size>0.001</max_step_size>
      <real_time_factor>1</real_time_factor>
    </physics>
    -->
    <include>
      <uri>model://sun</uri>
    </include>
    <model name="ground">
      <pose>1 2.3 -.1 0 0 0</pose>
      <static>1</static>
      <link name="ground">
        <collision name="ground_coll">
          <geometry>
            <box>
              <size>10 10 .1</size>
            </box>
          </geometry>
          <surface>
            <contact>
              <ode/>
            </contact>
          </surface>
        </collision>
        <visual name="ground_vis">
          <geometry>
            <box>
              <size>10 10 .1</size>
            </box>
          </geometry>
          <material>
            <script>
              <uri>file://course.material</uri>
              <name>course</name>
            </script>
          </material>
        </visual>
      </link>
    </model>
  </world>
</sdf>

このファイル内にあるcourse.matrialについても与えられていた．

material course
{
  receive_shadows on
  technique
  {
    pass
    {
      ambient 0.5 0.5 0.5 1.0
      texture_unit
      {
        texture course.png
      }
    }
  }
}

ここで，ある画像ファイルをテクスチャとしている．以下の画像である．
course
この画像の部分を変えるだけでコース変更は可能であると分かる．

※注意
これら3つのファイルは同じディレクトリ内にある必要がある．
ここでは，follow_botのパッケージファイルの直下に配置している．

XML

launchファイルをいじる中で，以下の変更も伴った．以下のファイルは，follow_botパッケージ直下にあるものである．なお，変更部分を切り取って示している．

package.xml

  <exec_depend>gazebo_ros</exec_depend>


  <!-- The export tag contains other, unspecified, tags -->
  <export>
    <gazebo_ros gazebo_media_path="${prefix}"/>
  </export>
launchファイル

コースとロボットを配置するという最小限のlaunchファイルにした．以下にソースコードそのときの出力を示す．

course.launch
<launch>
  <arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>
  <arg name="x_pos" default="0.0"/>
  <arg name="y_pos" default="0.0"/>
  <arg name="z_pos" default="0.0"/>
  
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="use_sim_time" value="true"/>
    <arg name="debug" value="false"/>
    <arg name="world_name" value="$(find follow_bot)/course.world"/>
  </include>

  <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
    <param name="publish_frequency" type="double" value="30.0" />
  </node>
  <param name="robot_description" command="$(find xacro)/xacro --inorder $(find turtlebot3_description)/urdf/turtlebot3_$(arg model).urdf.xacro" />

  <node pkg="gazebo_ros" type="spawn_model" name="spawn_urdf" args="-urdf -model turtlebot3_$(arg model) -x $(arg x_pos) -y $(arg y_pos) -z $(arg z_pos) -param robot_description" />

</launch>

※実行環境がnoeticでmelodic以降であるから，--inorderは必要ないが，一部は別のファイルからコピーしてきたもので，特に変更しなかったため消去していない．

コースの様子

course

これで，実行環境の構築が完了した．

線の検出

上の方で示したOpenCVを活用して，黄色の線を検出するプログラムを組む．線を検出するにあたって，画像の前処理が必要となってくる．ここでの前処理は，HSV画像を用いて黄色とそれ以外を識別できるようにすることである．それから，検出に入る．以下にソースコードとそのときの出力を示す．なお，プログラムの説明については，ソースコード内に記述している．
#####ソースコード（前処理まで）

follower_color_filter.py
#! /usr/bin/env python3

import rospy, cv_bridge	#ROSとOpenCV間でデータを受け渡すためのパッケージ
from sensor_msgs.msg import Image
import cv2 as cv
import numpy as np

class Follower:
	def __init__(self):
		self.bridge = cv_bridge.CvBridge()
		cv.namedWindow('BGR Image', 1)	#'BGR Image'という名前の画像表示のウィンドウを作成
		cv.namedWindow('MASK', 1)	#'MASK'という名前の画像表示のウィンドウを作成
		cv.namedWindow('MASKED', 1)	#'MASK'という名前の画像表示のウィンドウを作成
		self.image_sub = rospy.Subscriber('camera/rgb/image_raw', Image, self.image_callback)	#Image型で画像トピックを購読し，コールバック関数を呼ぶ

	def image_callback(self, msg):
		image = self.bridge.imgmsg_to_cv2(msg, desired_encoding = 'bgr8')
		h, w = image.shape[:2]
		RESIZE = (w//3, h//3)
		hsv = cv.cvtColor(image, cv.COLOR_BGR2HSV)	#色空間の変換(BGR→HSV)
		lower_yellow = np.array([10, 10, 10])		#黄色の閾値（下限）
		upper_yellow = np.array([255, 255, 250])	#黄色の閾値（上限）
		mask = cv.inRange(hsv, lower_yellow, upper_yellow)	#閾値によるHSV画像の2値化（マスク画像生成）
		masked = cv.bitwise_and(image, image, mask = mask)	#mask画像において，1である部分だけが残る（フィルタに通している）

		#大きすぎるため，サイズ調整
		display_mask = cv.resize(mask, RESIZE)
		display_masked = cv.resize(masked, RESIZE)
		display_image = cv.resize(image, RESIZE)
		
		#表示
		cv.imshow('BGR Image', display_image)	#'BGR Image'ウィンドウにimageを表示
		cv.imshow('MASK', display_mask)			#'MASK'ウィンドウにimageを表示
		cv.imshow('MASKED', display_masked)		#'MASKED'ウィンドウにimageを表示
		cv.waitKey(3)	#3ミリ秒待つ

rospy.init_node('follower')	#'follower'という名前でノードを初期化
follower = Follower()	#Followerクラスのインスタンスを作成（init関数が実行される）
rospy.spin()	#ループ
出力

all_camera_image

黄色の線のみ抽出できている様子が観察できる．

続いて，線の検出に移る．ある特定のものを検出するとき，OpenCVについてのスライド示しているが，領域を絞ることで効率的になり精度の向上を見込めるということで，以下のプログラムでもその処理を行っていることが分かる．

ソースコード（線の検出）
follower_line_finder.py
#! /usr/bin/env python3

import rospy, cv_bridge	#ROSとOpenCV間でデータを受け渡すためのパッケージ
import cv2 as cv
import numpy as np
from sensor_msgs.msg import Image
from geometry_msgs.msg import Twist

class Follower:
	def __init__(self):
		self.bridge = cv_bridge.CvBridge()
		cv.namedWindow('BGR Image', 1)	#'BGR Image'という名前の画像表示のウィンドウを作成
		cv.namedWindow('MASK', 1)	#'MASK'という名前の画像表示のウィンドウを作成
		cv.namedWindow('MASKED', 1)	#'MASK'という名前の画像表示のウィンドウを作成
		self.image_sub = rospy.Subscriber('camera/rgb/image_raw', Image, self.image_callback)	#Image型で画像トピックを購読し，コールバック関数を呼ぶ

		self.twist = Twist()	#Twistインスタンス生成

	def image_callback(self, msg):
		image = self.bridge.imgmsg_to_cv2(msg, desired_encoding = 'bgr8')

		hsv = cv.cvtColor(image, cv.COLOR_BGR2HSV)	#色空間の変換(BGR→HSV)
		lower_yellow = np.array([10, 10, 10])		#黄色の閾値（下限）
		upper_yellow = np.array([255, 255, 250])	#黄色の閾値（上限）
		mask = cv.inRange(hsv, lower_yellow, upper_yellow)	#閾値によるHSV画像の2値化（マスク画像生成）
		masked = cv.bitwise_and(image, image, mask = mask)	#mask画像において，1である部分だけが残る（フィルタに通している）

		h, w = image.shape[:2]
		RESIZE = (w//3, h//3)
		search_top = (h//4)*3
		search_bot = search_top + 20	#目の前の線にだけに興味がある→20行分くらいに絞る
		mask[0:search_top, 0:w] = 0
		mask[search_bot:h, 0:w] = 0


		M = cv.moments(mask)	#maskにおける1の部分の重心
		if M['m00'] > 0:	#重心が存在する
			cx = int(M['m10']/M['m00'])	#重心のx座標
			cy = int(M['m01']/M['m00'])	#重心のy座標
			cv.circle(image, (cx, cy), 20, (0, 0, 255), -1)	#赤丸を画像に描画

		#大きすぎるため，サイズ調整
		display_mask = cv.resize(mask, RESIZE)
		display_masked = cv.resize(masked, RESIZE)
		display_image = cv.resize(image, RESIZE)
		
		#表示
		cv.imshow('BGR Image', display_image)	#'BGR Image'ウィンドウにimageを表示
		cv.imshow('MASK', display_mask)			#'MASK'ウィンドウにimageを表示
		cv.imshow('MASKED', display_masked)		#'MASKED'ウィンドウにimageを表示
		cv.waitKey(3)	#3ミリ秒待つ

rospy.init_node('follower')	#'follower'という名前でノードを初期化
follower = Follower()	#Followerクラスのインスタンスを作成（init関数が実行される）
rospy.spin()	#ループ
出力

all_line_finder_image

赤丸により，線をとらえられていることが確認できる．

線の追跡（ライントレース）

以上のことを踏まえて，ロボットを線上に沿って動かすプログラムを組む．ここでは，P制御を用いて線上から離れないようにフィードバックにより実現する．なお，P制御については，次の記事にまとめている．(https://qiita.com/Yuya-Shimizu/items/8570640e6e03c3d1e09a#p%E5%88%B6%E5%BE%A1 )

以下にソースコードとそのときの出力を示す．なお，出力においては，ゲインを変更した2つの出力を示す．

ソースコード
follower_P.py
#! /usr/bin/env python3

import rospy, cv_bridge	#ROSとOpenCV間でデータを受け渡すためのパッケージ
import cv2 as cv
import numpy as np
from sensor_msgs.msg import Image
from geometry_msgs.msg import Twist

class Follower:
	def __init__(self):
		self.bridge = cv_bridge.CvBridge()
		cv.namedWindow('BGR Image', 1)	#'BGR Image'という名前の画像表示のウィンドウを作成
		cv.namedWindow('MASK', 1)	#'MASK'という名前の画像表示のウィンドウを作成
		cv.namedWindow('MASKED', 1)	#'MASK'という名前の画像表示のウィンドウを作成
		self.image_sub = rospy.Subscriber('camera/rgb/image_raw', Image, self.image_callback)	#Image型で画像トピックを購読し，コールバック関数を呼ぶ
		self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size = 1)

		self.twist = Twist()	#Twistインスタンス生成

	def image_callback(self, msg):
		image = self.bridge.imgmsg_to_cv2(msg, desired_encoding = 'bgr8')

		hsv = cv.cvtColor(image, cv.COLOR_BGR2HSV)	#色空間の変換(BGR→HSV)
		lower_yellow = np.array([10, 10, 10])		#黄色の閾値（下限）
		upper_yellow = np.array([255, 255, 250])	#黄色の閾値（上限）
		mask = cv.inRange(hsv, lower_yellow, upper_yellow)	#閾値によるHSV画像の2値化（マスク画像生成）
		masked = cv.bitwise_and(image, image, mask = mask)	#mask画像において，1である部分だけが残る（フィルタに通している）

		h, w = image.shape[:2]
		RESIZE = (w//3, h//3)
		search_top = (h//4)*3
		search_bot = search_top + 20	#目の前の線にだけに興味がある→20行分くらいに絞る
		mask[0:search_top, 0:w] = 0
		mask[search_bot:h, 0:w] = 0


		M = cv.moments(mask)	#maskにおける1の部分の重心
		if M['m00'] > 0:	#重心が存在する
			cx = int(M['m10']/M['m00'])	#重心のx座標
			cy = int(M['m01']/M['m00'])	#重心のy座標
			cv.circle(image, (cx, cy), 20, (0, 0, 255), -1)	#赤丸を画像に描画

			##P制御
			err = cx - w//2	#黄色の先の重心座標(x)と画像の中心(x)との差
			self.twist.linear.x = 0.2
			#self.twist.angular.z = -float(err)/100	#画像が大きいためか，-1/100では絶対値がまだ十分に大きく，ロボットが暴れてしまう
			self.twist.angular.z = -float(err)/1000	#誤差にあわせて回転速度を変化させる（-1/1000がP制御でいうところの比例ゲインにあたる）
			self.cmd_vel_pub.publish(self.twist)
		#大きすぎるため，サイズ調整
		display_mask = cv.resize(mask, RESIZE)
		display_masked = cv.resize(masked, RESIZE)
		display_image = cv.resize(image, RESIZE)
		
		#表示
		cv.imshow('BGR Image', display_image)	#'BGR Image'ウィンドウにimageを表示
		cv.imshow('MASK', display_mask)			#'MASK'ウィンドウにimageを表示
		cv.imshow('MASKED', display_masked)		#'MASKED'ウィンドウにimageを表示
		cv.waitKey(3)	#3ミリ秒待つ

rospy.init_node('follower')	#'follower'という名前でノードを初期化
follower = Follower()	#Followerクラスのインスタンスを作成（init関数が実行される）
rospy.spin()	#ループ
出力

ゲイン=-1/100

処理する画像のサイズが大きいため，ゲインの絶対値が相対的に大きくなってしまうと，上の動画のように暴れてしまい，うまくライン追従できていないということが観察できる．

ゲイン=-1/1000

適当に桁を1つ下げてみたところうまく追従できた．また，並進速度が0.2m/sで，ゆっくりであるため，P制御でも十分に追従できているように見える．

感想

まとめが非常に長くなってしまったが，今回は過去に学んできたOpenCVをはじめ，今まさに学んでいる制御部分も含めた，総合的実践的な学習であったため，非常によい復習と共に，実装の手順に対する理解も深められたと思う．

参考文献

プログラミングROS Pythonによるロボットアプリケーション開発
　　　　　　　　Morgan Quigley, Brian Gerkey, William D.Smart 著
　　　　　　　　　　　　　　　　　　　　　　　河田　卓志　監訳
　　　　　　　　　　 　松田　晃一，福地　正樹，由谷　哲夫　訳
　　　　　　　　　　　　　　　　　　オイラリー・ジャパン　発行

7
9
1

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
7
9