Windows で ROS2 のシミュレーションを行う (4)
TurtleBot3 をプログラムで制御する
ここからは、TurtleBot3 をプログラムで制御する方法を学ぶ。
TurtleBot3 とその周囲の環境をシミュレートするために、新しく起動したターミナルの一つのタブで以下のコマンドを実行する。
ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py
初回の起動には時間がかかるが、ロボットシミュレーション用の Gazebo というアプリケーションが起動し、以下のように表示される。
９本の柱がある仮想空間の右下に灰色の四角形が見える。これが TurtleBot3 である。
TurtleBot3 から放射状に出ている青い線分は、TurtleBot3 上の LiDAR の出すレーザーの軌跡をイメージしたものであろう。
なお、Gazebo の画面が現われない場合、「ROS2 のセットアップ」で行った「source /usr/share/gazebo/setup.bash を ~/.bashrc に取り込む」という作業を行っていないのかもしれない。確かめてみよう。
なお、検索すると「GAZEBO_MODEL_PATH の設定が必要」というアドバイスが見付かるが、2024年5月現在 GAZEBO_MODEL_PATH の設定は不要だった。
(このように基本的な仕様がコロコロ変わるのが ROS2 を使う上で難しいところ)
なお、この仮想空間の拡大率や位置を変更したい場合、以下を行う。
- 仮想空間の拡大率の変更: マウスホイールの回転
- 画面内での仮想空間の位置の変更: マウスを左クリックしながらのドラッグ
しかし、どちらの操作もかなり遅い。PC のパワーが足りていないと感じた場合、
無理にこの操作を行わず、可能な限りデフォルトの状態のまま利用するのが良いかもしれない。
なお、ROS のバージョン 1 では、Gazebo アプリケーション上でロボットを表す黒い四角形が画面奥に向かって落ちて行く様子 (すなわち、床がない状態) が見られることがあった。
私自身は ROS2 でそのような経験をしたことはないが、もし、ROS2 でもこの問題が見付かったら、新しいターミナルで以下の２つのコマンドを順に実行すると解決するかもしれない。
うまくいけば、上記の turtlebot3_gazebo のコマンドを実行しなおせば問題は解決されているはずである。
(とはいえ、Gazebo のインストールに問題があるのかもしれず、これだけで問題が解決するとは限らないと思う)
wget http://brain.cc.kogakuin.ac.jp/~kanamaru/lecture/ROS/download_models.sh
sh download_models.sh
さて、Gazebo シミュレータが正常に起動した時点で気づくと思うが、この Gazebo によるシミュレーションの動作はかなり重く、
本ページ以降のシミュレーションの実行には根気が要求される。
これは、Gazebo が要求するハードウェアのスペックが高いことや、
仮想マシンで Ubuntu を動かしていることが原因であるので御了承頂きたい。
なお、シミュレーションではなく TurtleBot3 の実機を制御するときはここまで処理が重いことはない。
なお、９本の柱の空間ではなく家のような環境を試したい場合、上のコマンドを Ctrl-C で終了した上で下記コマンドを試すと良い。
ただし、これを仮想マシンの Ubuntu で試すにはさらに荷が重い。
ros2 launch turtlebot3_gazebo turtlebot3_house.launch.py
さて、この仮想空間上の TurtleBot3 を キーボードで操作してみよう。上記２コマンドのうち、どちらか一つだけが実行されている状態にすること。
そしてターミナルに新たなタブを作成し、以下のコマンドを実行する。Turtlesim でも用いたキーボード操作用のノードが実行される。
ros2 run teleop_twist_keyboard teleop_twist_keyboard
主に使用するキーは以下の通りである。Turtlesim と異なり、動き始めると k を入力するまで動き続けるので注意すること。
- i : 前進。一度押す k を入力するまで動き続ける
- j : 左旋回。一度押すと k を入力するまで動き続ける
- k : 停止。一度押すとすぐに停止する
- l : 右旋回。一度押すと k を入力するまで動き続ける
- < : 後退。一度押すと k を入力するまで動き続ける
さらに、速度変更は以下で行えるのだった。必要に応じて速度を変更すること。
- q : 直進速度と回転速度が 10% アップ
- z : 直進速度と回転速度が 10% ダウン
- w : 直進速度のみ 10% アップ
- x : 直進速度のみ 10% ダウン
- e : 回転速度のみ 10% アップ
- c : 回転速度のみ 10% ダウン
動かして操作に慣れてみよう。
これまで通り、下図のようにコマンドを入力するターミナルにフォーカスがあっていないと TurtleBot3 を操作できないので注意すること。
なお、既に述べたようにシミュレーターが重いので、キーを入力してから TurtleBot3 が動き出すまでかなりのタイムラグがある場合がある。
TurtleBot3 を仮想空間内で動かしてラグの感覚をつかみ、壁や柱にぶつからないように操作することに慣れよう。
ところで、デフォルトの速度は直進速度 0.5、回転速度 1.0 となっている。シミュレーションならこれで構わないが、
TurtleBot3 の実機を動かす場合この速度では速すぎるし、場合によってはTurtleBot3 が動作しないこともあるので注意して欲しい
TurtleBot3 実機が teleop_twist_keyboard で動かないのは、多くの場合、速度 (直進速度) の設定が速すぎるためであろう。
(本ページではシミュレーションしか行わないのであまり関係がないが)。
なお、新たなタブで下記コマンドを実行し、ノード間の関係を表示すると下図のようになる。
ros2 run rqt_graph rqt_graph
「Nodes only」を選択して最読み込みボタンをクリックした場合を表示した。
Turtlesim の場合と同様、「/teleop_twist_keyboard」から「/cmd_vel」というトピックを通して「/turtlebot3_diff_drive」を制御していることが読み取れれば良い。
以上を確認したら、ターミナルの全てのタブで Ctrl-C を入力してプログラムを終了しよう。
TurtleBot3 とその周囲の環境をシミュレートするために、新しく起動したターミナルの一つのタブで以下のコマンドを実行しよう。
先程と同じく仮想環境に TurtleBot3 が表示される。
ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py
プログラムを自分で作成して実行することで、この仮想環境内で TurtleBot3 を制御してみよう。
作成するプログラムは、仮想環境上の TurtleBot3 だけではなく、TurtleBot3 の実機を制御することも出来る。
ここでは、「TurtleBot3 が 2 秒間前進して から静止する」という Python プログラム move1.py を作成するものとする。
ROS 上で動作するプログラムを初めて書いたのは
「(3) Turtlesim で ROS2 を体験する # Turtlesim の自律制御」であるので忘れている場合は復習するとよい。
Turtlesim での場合と同様に、テキストエディターを起動し、以下のプログラムを記述しよう。
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
import sys
from geometry_msgs.msg import Twist
from rclpy.clock import Clock, ClockType
from rclpy.time import Duration
class Publisher(Node):
def __init__(self):
super().__init__('publisher')
self.publisher = self.create_publisher(Twist, 'cmd_vel', 10)
self.move_cmd = Twist()
self.stop_cmd = Twist()
self.move_cmd.linear.x = 0.2
self.move_cmd.linear.y = 0.0
self.move_cmd.linear.z = 0.0
self.move_cmd.angular.x = 0.0
self.move_cmd.angular.y = 0.0
self.move_cmd.angular.z = 0.0
self.duration = 2 # sec
self.stop_time = Clock(clock_type=ClockType.ROS_TIME).now() + Duration(seconds=self.duration)
self.timer_period = 0.1 # seconds
self.timer = self.create_timer(self.timer_period, self.timer_callback)
def __del__(self):
self.publisher.publish(self.stop_cmd)
def timer_callback(self):
self.publisher.publish(self.move_cmd)
if Clock(clock_type=ClockType.ROS_TIME).now() > self.stop_time:
self.publisher.publish(self.stop_cmd)
self.timer.cancel()
sys.exit(0)
def main():
rclpy.init(args=sys.argv)
pub = Publisher()
rclpy.spin(pub)
pub.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
記述したら、以下の手順で保存を行う。
- 「保存」ボタンをクリック
- 「colcon_ws」→「src」→「ros_practice」→「ros_practice」とダブルクリックしてディレクトリをたどる
- 最上部の「無題のドキュメント」の部分を「move1.py」と書き換える。これが保存するファイル名となり、つづりを間違えると後で困るのでコピー＆貼り付けが確実
- 右上の「保存」ボタンで保存完了。
保存が終了したら、テキストエディタを閉じよう。
さて、ROS2 用のプログラムを書くのは「(3) Turtlesim で ROS2 を体験する # Turtlesim の自律制御」に続き二度目である。
ROS2 では、新しいプログラムを書いた場合、そのプログラムを利用できるよう設定が必要となる。その設定を行おう。
まず、新しいテキストエディタを新規で開いて欲しい。そして、「開く」ボタンから
「colcon_ws」→「src」→「ros_practice」→「setup.py」の順でダブルクリックしてファイル setup.py を開いて欲しい。
このファイルの末尾は以下のようになっているはずである。
(略)
entry_points={
'console_scripts': [
'autonomous_controller = ros_practice.autonomous_controller:main'
],
},
)
これを、以下のように編集して欲しい。
(略)
entry_points={
'console_scripts': [
'autonomous_controller = ros_practice.autonomous_controller:main',
'move1 = ros_practice.move1:main'
],
},
)
ポイントは
- 「'autonomous_controller' ...」の行の末尾にコンマ「,」を追記する。
- 「'autonomous_controller' ...」の次の行に行が追加されている
の二点である。1. の「コンマ」は忘れがちなので注意すること。
「保存」ボタンで上書き保存したらテキストエディタを閉じて構わない。
そして、ターミナルの新たなタブで以下のコマンドを実行してビルドを行おう。
colcon_ws ディレクトリに移動してからビルドを行っている。
この２コマンドの実行は、プログラム (ここでは move1.py) を変更する度に必要になるので注意すること。
cd ~/colcon_ws/
colcon build
以上で、「TurtleBot3 が 2 秒間前進して から静止する」という Python プログラムが完成したので早速実行してみよう。
ターミナルの新たなタブで以下のコマンドを実行しよう。
ros2 run ros_practice move1
仮想環境内では、TurtleBot3 がシミュレーション時間で 2 秒間前進して静止したはずである。
静止したと同時にプログラムの実行も終了しているはずである。
以下では、この内容を簡単に説明する。
まず、「前進」という動作を決め、実行しているのが以下の命令である。
self.move_cmd = Twist()
(中略)
self.move_cmd.linear.x = 0.2
self.move_cmd.linear.y = 0.0
self.move_cmd.linear.z = 0.0
self.move_cmd.angular.x = 0.0
self.move_cmd.angular.y = 0.0
self.move_cmd.angular.z = 0.0
(中略)
self.publisher.publish(self.move_cmd)
まず、前進動作として定義する変数 move_cmd を「self.move_cmd = Twist()」により定義している。
self がつくのは、move_cmd がクラスのメンバだからであるが、詳細は省略する。
そして、この move_cmd の linear.x のみに正の値を与えることで、 move_cmd は「前進」という命令になる。
「0.2」は向きのある速さを表しており、ここを 0.1 にすればゆっくり前進、-0.2 にすれば後退、となる。
シミュレータ上での動作を確認する目的ならばある程度自由な値を設定して良いが、
TurtleBot3 の実機では、あまり大きな値を設定すると動作が速すぎる恐れがあるので注意すること。
この move_cmd 変数を実際にロボットに適用しているのが「self.publisher.publish(self.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 に初期化されるためである。
self.stop_cmd = Twist()
(中略)
self.publisher.publish(self.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
全体として、「通常は 0.1 秒間隔のタイマ内で move_cmd を送信し、動き出してから 2 秒間経過したら stop_cmd を送信してプログラムを終了する」
というプログラムになっているので確認しておこう。
以上を確認したら、ターミナルの全てのタブで Ctrl-C を入力してプログラムを終了しよう。
次に、「仮想空間内で TurtleBot3 が障害物を避けながら前進し続ける」というプログラムを実行してみよう。
「(3) Turtlesim で ROS2 を体験する # Turtlesim の自律制御」で作成したプログラムの TurtleBot3 版である。
TurtleBot3 とその周囲の環境をシミュレートするために、新しく起動したターミナルの一つのタブで以下のコマンドを実行しよう。
ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py
いつも通り仮想空間内に TurtleBot3 が表示される。
プログラムを自分で作成して実行することで、この仮想環境内で TurtleBot3 を自律制御してみよう。
ここでは、「TurtleBot3 は障害物がなければ前進し、前方に障害物があったら、前方から障害物がなくなるまで反時計周りにその場回転する」
という Python プログラム auto.py を作成するものとする。
TurtleBot3 には 360 Laser Distance Sensor LDS-01 という LiDAR が搭載されているので、
その出力を障害物を検知するためのセンサとして用いるのである。
これまでと同様に、テキストエディターを新規に起動し、以下のプログラムを記述しよう。
#!/usr/bin/env python3
import rclpy
from rclpy.executors import ExternalShutdownException
from rclpy.node import Node
import sys, select
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
from rclpy.qos import qos_profile_sensor_data
from rclpy.clock import Clock, ClockType
from rclpy.time import Duration
import numpy as np
key = 'n'
class KeyReader():
def __init__(self, node):
timer_period = 0.1
self.timer = node.create_timer(timer_period, self.timer_callback)
def timer_callback(self):
global key
i, o, e = select.select( [sys.stdin], [], [], 0.01)
if i:
key = sys.stdin.readline().strip()
class PublisherSubscriber():
def __init__(self, node):
self.publisher = node.create_publisher(Twist, 'cmd_vel', 10)
self.subscription = node.create_subscription(
LaserScan,
'scan',
self.subscription_callback,
qos_profile=qos_profile_sensor_data)
self.subscription # prevent unused variable warning
self.move_cmd = Twist()
self.rotate_cmd = Twist()
self.stop_cmd = Twist()
self.move_cmd.linear.x = 0.2
self.move_cmd.linear.y = 0.0
self.move_cmd.linear.z = 0.0
self.move_cmd.angular.x = 0.0
self.move_cmd.angular.y = 0.0
self.move_cmd.angular.z = 0.0
self.rotate_cmd.linear.x = 0.0
self.rotate_cmd.linear.y = 0.0
self.rotate_cmd.linear.z = 0.0
self.rotate_cmd.angular.x = 0.0
self.rotate_cmd.angular.y = 0.0
self.rotate_cmd.angular.z = 0.2
self.distance = 100
def subscription_callback(self, data):
if key != 'q':
data_float = np.array(data.ranges)
data_float = np.where(data_float<data.range_min, data.range_max, data_float)
self.distance = min(min(data_float[0:30]), min(data_float[330:360]))
if self.distance < 0.5:
self.publisher.publish(self.rotate_cmd)
else:
self.publisher.publish(self.move_cmd)
else:
self.publisher.publish(self.stop_cmd)
sys.exit()
def main():
rclpy.init(args=sys.argv)
node1 = rclpy.create_node('Node1')
pubsub = PublisherSubscriber(node1)
keyread = KeyReader(node1)
try:
rclpy.spin(node1)
except (KeyboardInterrupt, ExternalShutdownException):
pass
finally:
node1.destroy_node()
rclpy.try_shutdown()
if __name__ == '__main__':
main()
記述したら、以下の手順で保存を行う。
- 「保存」ボタンをクリック
- 「colcon_ws」→「src」→「ros_practice」→「ros_practice」とダブルクリックしてディレクトリをたどる
- 最上部の「無題のドキュメント」の部分を「auto.py」と書き換える。これが保存するファイル名となり、つづりを間違えると後で困るのでコピー＆貼り付けが確実
- 右上の「保存」ボタンで保存完了。
保存が終了したら、テキストエディタを閉じよう。
次に、プログラム auto.py を利用できるよう設定を行おう。
新しいテキストエディタを新規で開き、「開く」ボタンから
「colcon_ws」→「src」→「ros_practice」→「setup.py」の順でダブルクリックしてファイル setup.py を開く。
これまでの作業を行っていれば、このファイルの末尾は以下のようになっているはずである。
(略)
entry_points={
'console_scripts': [
'autonomous_controller = ros_practice.autonomous_controller:main',
'move1 = ros_practice.move1:main'
],
},
)
これを、以下のように編集して欲しい。
(略)
entry_points={
'console_scripts': [
'autonomous_controller = ros_practice.autonomous_controller:main',
'move1 = ros_practice.move1:main',
'auto = ros_practice.auto:main'
],
},
)
ポイントは
- 「'move1' ...」の行の末尾にコンマ「,」を追記する。
- 「'move1' ...」の次の行に行が追加されている
の二点である。1. の「コンマ」は忘れがちなので注意すること。
「保存」ボタンで上書き保存したらテキストエディタを閉じて構わない。
そして、ターミナルの新たなタブで以下のコマンドを実行してビルドを行おう。
colcon_ws ディレクトリに移動してからビルドを行っている。
この２コマンドの実行は、プログラム (ここでは auto.py) を変更する度に必要になるので注意すること。
cd ~/colcon_ws/
colcon build
以上で、「TurtleBot3 が障害物を避けて自律移動する」という Python プログラムが完成したので早速実行してみよう。
ターミナルの新たなタブで以下のコマンドを実行しよう。
ros2 run ros_practice auto
仮想環境内で、「TurtleBot3 は障害物がなければ前進し、前方に障害物があったら、前方から障害物がなくなるまで反時計周りにその場回転する」
という動作が実現されたはずである。
なお、このプログラムを終了する際は、Ctrl-C ではなく、ターミナル上で「キーボードで q を入力→Enter」を実行して欲しい。
「Ctrl-C でプログラムを停止しても、TurtleBot3 を止めることができない」ためである。
キーボードの q → Enter でなら、TurtleBot3 を止めることができるはずである。
なお、Ubuntu 20.04 + ROS2 Foxy でなら、Ctrl-C で TurtleBot3 を止めることができていた (Foxy 用のプログラム)。
ROS2 Humble で Ctrl-C などに対する仕様が変わったため、TurtleBot3 を止めることができなくなったようである。
(このように基本的な仕様がコロコロ変わるのが ROS2 を使う上で難しいところ)
以下では、この内容を簡単に説明する。
プログラムの基本構造は、前進を意味する変数 move_cmd とその場回転を意味する変数 rotate_cmd を定義し、
前方障害物までの距離 distance の値に応じて上記二つの動作を切替える、というものである。
前方障害物までの距離が短ければ前進をやめ回転を始める。
回転により、前方に障害物がなくなれば再び前進が始まる。
LiDAR からの情報はプログラムの以下の部分で取得している。
self.subscription = self.create_subscription(
LaserScan,
'scan',
self.subscription_callback,
qos_profile=qos_profile_sensor_data)
(中略)
def subscription_callback(self, data):
data_float = np.array(data.ranges)
data_float = np.where(data_float<data.range_min, data.range_max, data_float)
self.distance = min(min(data_float[0:30]), min(data_float[330:360]))
これにより、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_float = np.array(data.ranges)
data_float = np.where(data_float<data.range_min, data.range_max, data_float)
上の auto.py で、LiDAR の出力から障害物までの最小距離を求め、TurtleBot3 の制御に利用することができた。
では、その最小距離となる障害物はどの方向にあるかを知りたいときはどうすべきだろうか？
最小距離を求める際、上のプログラムでは、距離データの配列 msg.ranges[] を range_min 以上の距離に加工した配列 data_float[] を用意し、
data_float[0:30] および data_float[330:360] の範囲 (前方 60 度) の最小値として最小距離を定義した。
その最小距離をとる配列のインデックスを求めれば良い。
具体的には以下のようにする。まず、「self.distance = 100」の行の次に
「self.close_position = 0」という行を追加する。これは、最小距離の位置 self.close_position を定義し、0 で初期化している。
self.distance = 100
self.close_position = 0
そして、「self.distance = min(min ...(中略)」の次の行に以下の 4 行を追加する。
左 30 度の最小位置 pos_left と 右 30 度の最小位置 pos_right を定義し、そのうち距離の小さい方を self.close_position として設定している。
「#」で無効化されている行を有効にすると、TurtleBot3 動作時に、最小位置と最小距離がコンソールに表示される。
この例のように左周りで障害物を回避するプログラムの場合、ほとんどの時間で close_position は右端位置を表す 330 度付近の位置を示しているであろう。
self.distance = min(min(data_float[0:30]), min(data_float[330:360]))
pos_left = np.argmin(data_float[0:30])
pos_right = 330 + np.argmin(data_float[330:360])
self.close_position = pos_left if data_float[pos_left] < data_float[pos_right] else pos_right
#self.get_logger().info('{} {}'.format(self.close_position, self.distance))
なお、上記の記述は「前方 60 度の範囲の障害物の位置」のため、左右の場合分けが必要となりやや面倒だったが、
「周囲全体での最も近い障害物の位置」ならば下記の記述で済む。
self.close_position = np.argmin(data_float)
前のページ「Windows で ROS2 のシミュレーションを行う (3) Turtlesim で ROS を体験する」／
次のページ「Windows で ROS2 のシミュレーションを行う (5) TurtleBot3 で地図を作成する」
トップページに戻る