Windows で ROS のシミュレーションを行う (4)
TurtleBot3 をプログラムで制御する
ここからは、TurtleBot3 をプログラムで制御する方法を学ぶ。
TurtleBot3 とその周囲の環境をシミュレートするために、新しく起動したターミナルの一つのタブで以下のコマンドを実行する。
roslaunch turtlebot3_gazebo turtlebot3_world.launch
初回の起動には時間がかかるが、ロボットシミュレーション用の Gazebo というアプリケーションが起動し、下図の表示が現われる。
９本の柱がある仮想空間の右下に黒い四角形が見える。これが TurtleBot3 (Waffle Pi) である。
もし、ここでロボットを表す黒い四角形が画面奥に向かって落ちて行く様子が見られたら (すなわち、床がない状態が見られたら)、
まず、ターミナルに赤字で以下のエラーメッセージが表示されていないか、ターミナルの表示をスクロールしてチェックして欲しい。
Error [parser.cc:581] Unable to find uri[model://sun]
Error [parser.cc:581] Unable to find uri[model://ground_plane]
もし、このエラーが見付かったら、新しいターミナルで以下の２つのコマンドを順に実行して欲しい。その後、上記の turtlebot3_gazebo のコマンドを実行しなおせば問題は解決されているはずである。
wget http://brain.cc.kogakuin.ac.jp/~kanamaru/lecture/ROS/download_models.sh
sh download_models.sh
なお、うまく実行できた時点で気づくと思うが、この Gazebo によるシミュレーションの動作はかなり重く、
本ページと次ページのシミュレーションにはかなりの根気が要求される。
これは、Gazebo が要求するハードウェアのスペックが高いことや、
仮想マシンで Ubuntu を動かしていることが原因であるので御了承頂きたい。
なお、シミュレーションではなく TurtleBot3 の実機を制御するときはここまで処理が重いことはない。
なお、９本の柱の空間ではなく家のような環境を試したい場合、上のコマンドを Ctrl-c で終了した上で下記コマンドを試すと良い。
ただし、これを仮想マシンの Ubuntu で試すにはさらに荷が重い。
roslaunch turtlebot3_gazebo turtlebot3_house.launch
さて、この仮想空間上の TurtleBot3 を キーボードで操作してみよう。上記２コマンドのうち、どちらか一つだけが実行されている状態にすること。
そしてターミナルに新たなタブを作成し、以下のコマンドを実行する。Turtlesim でも用いたキーボード操作用のノードが実行される。
rosrun teleop_twist_keyboard teleop_twist_keyboard.py _speed:=0.5 _turn:=1.0
主に使用するキーは以下の通りである。Turtlesim と異なり、動き始めると k を入力するまで動き続けるので注意すること。
- i : 前進。一度押す k を入力するまで動き続ける
- j : 左旋回。一度押すと k を入力するまで動き続ける
- k : 停止。一度押すとすぐに停止する
- l : 右旋回。一度押すと k を入力するまで動き続ける
- < : 後退。一度押すと k を入力するまで動き続ける
動かして操作になれてみよう。これまで通り、コマンドを入力したターミナルにフォーカスがあっていないと TurtleBot3 を操作できないので注意すること。
なお、既に述べたようにシミュレーターが重いので、キーを入力してから TurtleBot3 が動き出すまでかなりのタイムラグがある場合がある。
TurtleBot3 を仮想空間内で動かしてラグの感覚をつかみ、壁や柱にぶつからないように操作することに慣れよう。
コマンドに含まれる「0.5」や「1.0」は、それぞれ直進速度と回転速度なので、速すぎると思ったら (例えば「0.2」などに) 小さくしてもよい。
なお、この命令を TurtleBot3 実機で行うときは、0.5 や 1.0 では動作が速すぎることがあるので、0.1 や 0.2 など、まずは小さい値から試すのが安全である。
なお、新たなタブで下記コマンドを実行し、ノード間の関係を表示すると下図のようになる。
rosrun rqt_graph rqt_graph
「Nodes only」の場合を表示した。Turtlesim の場合と大きな違いはない。
以上を確認したら、ターミナルの全てのタブで Ctrl-c を入力してプログラムを終了しよう。
TurtleBot3 とその周囲の環境をシミュレートするために、新しく起動したターミナルの一つのタブで以下のコマンドを実行しよう。
先程と同じく仮想環境に TurtleBot3 が表示される。
roslaunch turtlebot3_gazebo turtlebot3_world.launch
プログラムを自分で作成して実行することで、この仮想環境内で TurtleBot3 を制御してみよう。
作成するプログラムは、仮想環境上の TurtleBot3 だけではなく、TurtleBot3 の実機を制御することも出来る。
ここでは、「TurtleBot3 が 2 秒間前進して から静止する」という Python プログラム move1.py を作成するものとする。
ROS 上で動作するプログラムを初めて書いたのは
「(3) Turtlesim で ROS を体験する # Turtlesim の自律制御」であるので忘れている場合は復習するとよい。
Turtlesim での場合と同様に、テキストエディターを起動し、以下のプログラムを記述しよう。
#! /usr/bin/env python3
import rospy
from geometry_msgs.msg import Twist
cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size = 1)
rospy.init_node('move')
move_cmd = Twist()
stop_cmd = Twist()
move_cmd.linear.x = 0.2
move_cmd.linear.y = 0
move_cmd.linear.z = 0
move_cmd.angular.x = 0
move_cmd.angular.y = 0
move_cmd.angular.z = 0
duration = 2 # sec
rate = rospy.Rate(10)
# required for stabilizing rospy.Time.now() in Gazebo simulation
rospy.sleep(0.0)
stop_time = rospy.Time.now() + rospy.Duration(duration)
while not rospy.is_shutdown():
cmd_vel_pub.publish(move_cmd)
if rospy.Time.now() > stop_time:
cmd_vel_pub.publish(stop_cmd)
break
rate.sleep()
記述したらファイルを保存する。
「保存」ボタンを押して (あるいは Ctrl-S で) 保存する際、保存先のディレクトリを
「catkin_ws」→「src」→「ros_practice」→「scripts」とダブルクリックしてたどる。
そして、「名前」欄にファイル名として move1.py と記入して「保存」ボタンを押すこと。
なお、ros_practice ディレクトリやその中の scripts ディレクトリは
「(3) Turtlesim で ROS を体験する # Turtlesim の自律制御」で作成したものをそのまま流用している。
基本的には catkin_ws 以下のパッケージの scripts ディレクトリならどこでも良いのだが、
上記の保存場所を変えると以下のプログラム実行時のコマンドが変化するので注意して欲しい。
保存したファイルを実行可能なプログラムにするため、ターミナルで以下のコマンドを実行する。
(これは一回実行するだけで良い)
chmod 755 ~/catkin_ws/src/ros_practice/scripts/move1.py
以上で、「TurtleBot3 が 2 秒間前進して から静止する」という Python プログラムが完成したので早速実行してみよう。
ターミナルの新たなタブで以下のコマンドを実行しよう。
rosrun ros_practice move1.py
仮想環境内では、TurtleBot3 がシミュレーション時間で 2 秒間前進して静止したはずである。
静止したと同時にプログラムの実行も終了しているはずである。
以下では、この内容を簡単に説明する。
まず、「前進」という動作を決め、実行しているのが以下の命令である。
move_cmd = Twist()
(中略)
move_cmd.linear.x = 0.2
move_cmd.linear.y = 0
move_cmd.linear.z = 0
move_cmd.angular.x = 0
move_cmd.angular.y = 0
move_cmd.angular.z = 0
(中略)
cmd_vel_pub.publish(move_cmd)
まず、前進動作として定義する変数 move_cmd を「move_cmd = Twist()」により定義している。
そして、この move_cmd の linear.x のみに正の値を与えることで、 move_cmd は「前進」という命令になる。
「0.2」は向きのある速さを表しており、ここを 0.1 にすればゆっくり前進、-0.2 にすれば後退、となる。
シミュレータ上での動作を確認する目的ならばある程度自由な値を設定して良いが、
TurtleBot3 の実機では、あまり大きな値を設定すると動作が速すぎる恐れがあるので注意すること。
この move_cmd 変数を実際にロボットに適用しているのが「cmd_vel_pub.publish(move_cmd)」、ということになる。
なお、「前進」ではなく「その場で回転」の動作をさせたければ、move_cmd の angular.z のみに非 0 の値を与えれば良い。
0.2 などのように正ならば反時計周り、-0.2 などのように負ならば時計周りに TurtleBot3 がその場で回転する。
move_cmd の linear.x と angular.z の両方に非 0 の値を与えれば、二つの動作の合成動作となる。
なお、プログラム終了時に実行される静止コマンド stop_cmd は、以下の命令が与えられているだけで、linear.x や angular.z などの値がセットされていない。
これは、以下の宣言で全ての速さの値が 0 に初期化されるためである。
stop_cmd = Twist()
(中略)
cmd_vel_pub.publish(stop_cmd)
そのことはROS公式ページ内の
「Note that all fields in a message are currently initialized to a default value of 0 (or empty for strings and variable-length arrays).」
より確認できる (クラスライブラリのドキュメントなど、もっと分かりやすい場所に書くべきだと思う)。
また、「2 秒間前進」の「2 秒間」は以下の変数でセットされている。
duration = 2 # sec
while 文の中身は、「通常は move_cmd を送信、動き出してから 2 秒間経過したら stop_cmd を送信して while 文を抜ける」
というプログラムになっているので確認しておこう。
以上を確認したら、ターミナルの全てのタブで Ctrl-c を入力してプログラムを終了しよう。
次に、「仮想空間内で TurtleBot3 が障害物を避けながら前進し続ける」というプログラムを実行してみよう。
「(3) Turtlesim で ROS を体験する # Turtlesim の自律制御」で作成したプログラムの TurtleBot3 版である。
TurtleBot3 とその周囲の環境をシミュレートするために、新しく起動したターミナルの一つのタブで以下のコマンドを実行しよう。
roslaunch turtlebot3_gazebo turtlebot3_world.launch
いつも通り仮想空間内に TurtleBot3 が表示される。
プログラムを自分で作成して実行することで、この仮想環境内で TurtleBot3 を自律制御してみよう。
ここでは、「TurtleBot3 は障害物がなければ前進し、前方に障害物があったら、前方から障害物がなくなるまで反時計周りにその場回転する」
という Python プログラム auto.py を作成するものとする。
TurtleBot3 には 360 Laser Distance Sensor LDS-01 という LiDAR が搭載されているので、
その出力を障害物を検知するためのセンサとして用いるのである。
これまでと同様に、テキストエディターを起動し、以下のプログラムを記述しよう。
#! /usr/bin/env python3
import rospy
import numpy as np
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
def callback_laser(msg):
global distance
data = np.array(msg.ranges)
data = np.where(data<msg.range_min, msg.range_max, data)
distance = min(min(data[0:30]), min(data[330:360]))
def callback_shutdown():
cmd_vel_pub.publish(stop_cmd)
cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size = 1)
scan_sub = rospy.Subscriber('scan', LaserScan, callback_laser)
rospy.init_node('auto')
move_cmd = Twist()
rotate_cmd = Twist()
stop_cmd = Twist()
move_cmd.linear.x = 0.2
move_cmd.linear.y = 0
move_cmd.linear.z = 0
move_cmd.angular.x = 0
move_cmd.angular.y = 0
move_cmd.angular.z = 0
rotate_cmd.linear.x = 0
rotate_cmd.linear.y = 0
rotate_cmd.linear.z = 0
rotate_cmd.angular.x = 0
rotate_cmd.angular.y = 0
rotate_cmd.angular.z = 0.2
distance = 100
rate = rospy.Rate(10)
rospy.on_shutdown(callback_shutdown)
# required for stabilizing rospy.Time.now() in Gazebo simulation
rospy.sleep(0.0)
while not rospy.is_shutdown():
if distance < 0.5:
cmd_vel_pub.publish(rotate_cmd)
else:
cmd_vel_pub.publish(move_cmd)
rate.sleep()
記述したらファイルを保存する。
「保存」ボタンを押して (あるいは Ctrl-S で) 保存する際、保存先のディレクトリを
「catkin_ws」→「src」→「ros_practice」→「scripts」とダブルクリックしてたどる。
そして、「名前」欄にファイル名として auto.py と記入して「保存」ボタンを押すこと。
なお、ros_practice ディレクトリやその中の scripts ディレクトリは
「(3) Turtlesim で ROS を体験する # Turtlesim の自律制御」で作成したものをそのまま流用している。
基本的には catkin_ws 以下のパッケージの scripts ディレクトリならどこでも良いのだが、
上記の保存場所を変えると以下のプログラム実行時のコマンドが変化するので注意して欲しい。
保存したファイルを実行可能なプログラムにするため、ターミナルで以下のコマンドを実行する。
(これは一回実行するだけで良い)
chmod 755 ~/catkin_ws/src/ros_practice/scripts/auto.py
以上で、「TurtleBot3 は障害物がなければ前進し、前方に障害物があったら、前方から障害物がなくなるまで反時計周りにその場回転する」
という Python プログラムが完成したので早速実行してみよう。
ターミナルの新たなタブで以下のコマンドを実行しよう。
rosrun ros_practice auto.py
仮想環境内で、「TurtleBot3 は障害物がなければ前進し、前方に障害物があったら、前方から障害物がなくなるまで反時計周りにその場回転する」
という動作が実現されたはずである。
以下では、この内容を簡単に説明する。
プログラムの基本構造は、前進を意味する変数 move_cmd とその場回転を意味する変数 rotate_cmd を定義し、
前方障害物までの距離 distance の値に応じて上記二つの動作を切替える、というものである。
前方障害物までの距離が短ければ前進をやめ回転を始める。
回転によって前方に障害物がなくなっていれば再び前進が始まる。
LiDAR からの情報はプログラムの以下の部分で取得している。
def callback_laser(msg):
global distance
data = np.array(msg.ranges)
data = np.where(data<msg.range_min, msg.range_max, data)
distance = min(min(data[0:30]), min(data[330:360]))
(中略)
scan_sub = rospy.Subscriber('scan', LaserScan, callback_laser)
これにより、LiDAR から距離データが得られる度に、前方障害物までの距離 distance が更新されるようになる。
なお、LiDAR からの距離データは、 LDS-01 の場合、下図のように msg.ranges[0] ～ msg.ranges[359] までの範囲の数値として取得でき、
前方のデータが msg.ranges[0] および msg.ranges[359] となっていた (これは LiDAR により異なるので注意)。
そこで、「前方障害物までの距離」を
msg.ranges[0]～msg.ranges[29] および msg.ranges[330]～msg.ranges[359] の最小値として定める。
前方 60 度の範囲の最小値、ということになる。
ただし、LiDAR の出力する距離の値は msg.range_min から msg.range_max の値に制限されているため、
その範囲外の値が得られた場合はエラー値として除外すべきである。今回の場合、 msg.range_min より小さい値 (例えば 0.0) を
下記の命令で強制的に msg.range_max に書き換え、最小値の計算に影響が及ばないようにしている。
data = np.array(msg.ranges)
data = np.where(data<msg.range_min, msg.range_max, data)
また、このプログラムを動作させる際、Ctrl-c で制御を終了させたときに TurtleBot3 が確実に停止していることが重要である。
なぜなら、制御プログラムを停止させた後も TurtleBot3 が前進や回転し続けていたら困るからである。
(仮想環境によるシミュレーションならばそれほど問題にならないが、TurtleBot3 の実機が静止しないのは大問題)。
それを実現しているのがプログラムの以下の部分である。ROS プログラムの終了時に callback_shutdown という関数が呼ばれるようにし、
その関数内で stop_cmd という静止コマンドを TurtleBot3 に与えている。
def callback_shutdown():
cmd_vel_pub.publish(stop_cmd)
(中略)
rospy.on_shutdown(callback_shutdown)
(中略)
stop_cmd = Twist()
以上を確認したら、ターミナルの全てのタブで Ctrl-c を入力してプログラムを終了しよう。
上の auto.py で、LiDAR の出力から障害物までの最小距離を求め、TurtleBot3 の制御に利用することができた。
では、その最小距離となる障害物はどの方向にあるかを知りたいときはどうすべきだろうか？
最小距離を求める際、上のプログラムでは、距離データの配列 msg.ranges[] を range_min 以上の距離に加工した配列 data[] を用意し、
data[0:30] および data[330:360] の範囲 (前方 60 度) の最小値として最小距離を定義した。
その最小距離をとる配列のインデックスを求めれば良い。
具体的には以下のようにする。まず、「global distance」というの行の次に
「global close_position」という行を追加する。
global distance
global close_position
同様に、「distance = 100」というの行の次に
「close_position = 0」という行を追加する。
以上で、最小距離の位置を示す変数 close_position を定義できた。
distance = 100
close_position = 0
そして、「distance = min(min ...(中略)」の次の行に以下の 3 行を追加する。
左 30 度の最小位置 pos_left と 右 30 度の最小位置 pos_right を定義し、そのうち距離の小さい方を close_position として設定している。
distance = min(min(data[0:30]), min(data[330:360]))
pos_left = np.argmin(data[0:30])
pos_right = 330 + np.argmin(data[330:360])
close_position = pos_left if data[pos_left] < data[pos_right] else pos_right
もし、close_position と distance の値を知りたければ、以下の位置に一行追加すれば良い。
TurtleBot3 動作時に、最小位置と最小距離がコンソールに表示される。
この例のように左周りで障害物を回避するプログラムの場合、ほとんどの時間で close_position は右端位置を表す 330 度付近の位置を示しているであろう。
while not rospy.is_shutdown():
rospy.loginfo('{} {}'.format(close_position, distance))
なお、上記の記述は「前方 60 度の範囲の障害物の位置」のため、左右の場合分けが必要となりやや面倒だったが、
「周囲全体での最も近い障害物の位置」ならば下記の記述で済む。
close_position = np.argmin(data)
前のページ「Windows で ROS のシミュレーションを行う (3) Turtlesim で ROS を体験する」／
次のページ「Windows で ROS のシミュレーションを行う (5) TurtleBot3 で地図を作成する」
トップページに戻る