PythonでTurtlebot3を動かしてみた(gazeboを使って)

学生の頃、ROSを触っていたのですがあの頃は何をやっているのかよく分からず使っていてあまり理解できていませんでした。

あれから5年、また久々に触ってみたくなり、プログラミングROS ―Pythonによるロボットアプリケーション開発で再入門中です。

今回はPythonでTurtlebot3のgazeboシミュレータを動かしてみようと思います。

本ではTurtlebot2を題材にしていますが、今回の内容はTurtlebot3でもそのまま使うことができます。(一部アレンジ加えてます)

前提条件

• Ubuntu 20.04
• ROS Noetic desktop-full(インストール方法はこちら)
• 追加の依存パッケージを以下のコマンドでインストール

sudo apt install python3-rosdep python3-rosinstall python3-rosinstall-generator python3-wstool build-essential

パッケージの追加

まずは今回使うTurtlebot3のパッケージを追加します。

以下のコマンドを実行してワークスペースを作成します。

mkdir -p ~/wanderbot_ws/src
cd ~/wanderbot_ws/src
catkin_init_workspace

続いてROSのプログラムを実行するためのパッケージを作成します。

cd ~/wanderbot_ws/src
catkin_create_pkg wanderbot rospy geometry_msgs sensor_msgs

続いて以下のコマンドでTurtlebot3のパッケージとgazeboのシミュレータをレポジトリからインストールします(turtlebot3の本体とメッセージのパッケージはROSのバージョンごとにブランチが切られているようなのでnoetic用のブランチを指定してクローンしています)。

cd ~/wanderbot_ws/src
git clone -b noetic-devel https://github.com/ROBOTIS-GIT/turtlebot3_msgs.git
git clone -b noetic-devel https://github.com/ROBOTIS-GIT/turtlebot3.git
git clone https://github.com/ROBOTIS-GIT/turtlebot3_simulations.git
git clone https://github.com/ROBOTIS-GIT/turtlebot3_gazebo_plugin.git
cd ~/wanderbot_ws && catkin_make

Turtlebot3にはBurger, Waffle, Waffle Piの3種類があり、どれかを環境変数で指定する必要があります。

そこで、以下のコマンドで環境変数を指定します。

echo "export TURTLEBOT3_MODEL=burger" >> ~/.bashrc

とりあえず動かす

とりあえず導入として一定間隔で前進、停止を繰り返すだけのコードを書いてみます。

これはプログラミングROS ―Pythonによるロボットアプリケーション開発を参考にしてますが、若干アレンジしています。

Ctrl + Cでプログラムを止めたときにrospy.on_shutdownでコールバック関数を呼び出してTurtlebot3を止めるように追加してみました。

ソースコード

wanderbot/scripts/red_light_green_light.py

#!/usr/bin/env python3
import rospy
from rospy.exceptions import ROSInterruptException
from geometry_msgs.msg import Twist

def shutdown_turtlebot():
    rospy.loginfo('Stop Turtlebot')
    cmd_vel_pub.publish(Twist())
    rospy.sleep(1)

cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
rospy.init_node('red_light_green_light')
rospy.on_shutdown(shutdown_turtlebot)

red_light_twist = Twist()
green_light_twist = Twist()
green_light_twist.linear.x = 0.5

driving_forward = False
light_change_time = rospy.Time.now()
rate = rospy.Rate(1)

while not rospy.is_shutdown():
    try:
        if driving_forward:
            cmd_vel_pub.publish(green_light_twist)
        else:
            cmd_vel_pub.publish(red_light_twist)
        if light_change_time > rospy.Time.now():
            driving_forward = not driving_forward
        light_change_time = rospy.Time.now() + rospy.Duration(3)
        rate.sleep()
    except ROSInterruptException:
        rospy.loginfo('Finish')

プログラムを実行するときには、以下のコマンドを実行して実行権限を変えたあとにrosrunを実行します。

chmod u+x red_light_green_light.py
rosrun wanderbot red_light_green_light.py

動かしてみると一定感間隔でロボットが前進と停止を繰り返します。

真っ直ぐに走行しないのが少し気になります…

障害物回避をする

先程のように動き回るのを見るだけでも満足してしまいますが、今度はロボットらしくセンサーを使ってみます。

今回はTurtlebot3にデフォルトでついているLiDARセンサーを使って障害物回避をしてみます。

本当は障害物回避するなら他のセンサーを使ったほうがいいのですが、Gazeboで使えるセンサーがこれぐらいしかなさそうなので、妥協しました。

ソースコード

wanderbot/scripts/wander.py

#!/usr/bin/env python3
import rospy
from rospy.exceptions import ROSInterruptException
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan

def scan_callback(msg):
    global g_range_ahead
    g_range_ahead = min(msg.ranges)

def shutdown_turtlebot():
    rospy.loginfo('Stop Turtlebot')
    cmd_vel_pub.publish(Twist())
    rospy.sleep(1)

g_range_ahead = 1
scan_sub = rospy.Subscriber('scan', LaserScan, scan_callback)
cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
rospy.init_node('wander')
rospy.on_shutdown(shutdown_turtlebot)

driving_forward = True
state_change_time = rospy.Time.now()
rate = rospy.Rate(10)

while not rospy.is_shutdown():
    try:
        rospy.loginfo(g_range_ahead)
        twist = Twist()
        if driving_forward:
            if (g_range_ahead < 0.12 or rospy.Time.now() > state_change_time):
                driving_forward = False
                state_change_time = rospy.Time.now() + rospy.Duration(5)
                twist.angular.z = 1
            else:
                twist.linear.x = 1
        else:
            if rospy.Time.now() > state_change_time:
                driving_forward = True
                state_change_time = rospy.Time.now() + rospy.Duration(30)
                twist.linear.x = 1
            else:
                twist.angular.z = 1
        cmd_vel_pub.publish(twist)
        rate.sleep()
    except ROSInterruptException:
        rospy.loginfo('Finish')

それでは先程と同様にプログラムを実行してみましょう。

chmod u+x wander.py
rosrun wanderbot wander.py

今度は障害物に当たったときにぐるぐるまわりながら移動を続けます。

一度障害物に当たるとぐるぐる回り続けますが、いいタイミングで障害物のない方向で逃げるようになります。

これでやっとロボットプログラムのはじめの一歩を踏み出せました(というよりも再入門できたｗ)。

今度はセンサーをシミュレータで動かせないか模索したいところです…