ROSで動かすロボットカー作り(センサーを使って実機走行してみた編)

前回は旋回の機能と超音波センサーの配信機能を実装しました。

supernove.hatenadiary.jp

これでロボットを動かすために必要なものが用意できたので、今回は前回までに用意した2つのコード(ロボットの車体を操作するコードと超音波センサーの配信を行うコード)を組み合わせてロボットをコントロールできるようにしていきます。

そして、超音波センサーを使って障害物を回避できるようにコードを実装していきます。

今回の内容も例の如く以下のレポジトリの backlog/step3ブランチで切っています。

github.com

launchファイル

今回のポイントになるのがlauchファイルです。

launchファイルは複数のROSノードを同時に起動させるためのファイルです。

このlaunchファイルは roslaunchコマンドを使うことで起動することができます。

これにより、複数ノードを動かすたびにrosrunコマンドを使わなくても一回のコマンドで毎回実行する必要のある複数のノードを起動できるのでとても便利です。

ちなみにroslaunchコマンドを実行時にroscoreが起動してなければそのまま実行してくれるという便利機能まであるんです!

今回は、前回までに作成した車体を動かすコード(rober.py)と超音波センサーを配信するコード(sonar_sensor.py)を同時に動かすlaunchファイルを用意します。

パッケージのディレクトリ上に以下の内容をmy_rober.launchとして保存します。

<launch>
    <node name="rober" pkg="my_rober" type="rober.py" output="screen" />
    <node name="sonar_sensor" pkg="my_rober" type="sonar_sensor.py" />
</launch>

各属性について軽く説明します。

name属性にはノードの名前を定義します。ここではファイル名から.pyを抜いたものをそのまま使います。

pkg属性には動かすノードが属しているパッケージ名を指定します。

type属性には実行するノードのファイル名を指定します。今回はそれぞれ起動するPythonコードを指定します。

output属性はログを出力したいときに設定します。ログを出力しない場合はこの属性はなくてもいいです。今回は配信されたtwistメッセージを表示するためにroberノードにだけこの属性を設定します。

動かすときには以下のコマンドを実行します。

roslaunch my_rober my_rober.lauch

実行すると以下のような画面を表示します。roscoreを起動していなければここで立ち上がります。

... logging to /home/pi/.ros/log/b4fc8916-779e-11ec-94df-dca63297671e/roslaunch-raspberrypi-4035.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://raspberrypi:37883/

SUMMARY
========

PARAMETERS
 * /rosdistro: kinetic
 * /rosversion: 1.12.17

NODES
  /
    rober (my_rober/rober.py)
    sonar_sensor (my_rober/sonar_sensor.py)

auto-starting new master
process[master]: started with pid [4045]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to b4fc8916-779e-11ec-94df-dca63297671e
process[rosout-1]: started with pid [4058]
started core service [/rosout]
process[rober-2]: started with pid [4061]
process[sonar_sensor-3]: started with pid [4063]
[INFO] [1642428409.977015]: Startup

ロボットの制御用コード

これでロボットのノードは起動したので、制御用のコードを用意します。

前回作成したcontrol_rober.pyを以下のコードに変更します。

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


class Contorller:
    def __init__(self):
        rospy.init_node('controller', anonymous=True)
        self.distance = 100
        self.pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
        self.sub = rospy.Subscriber('scan', Range, self.scan_callback)
        # プログラムを終了したときに実行するコールバック関数
        rospy.on_shutdown(self.shutdown_callback)
        rospy.Timer(rospy.Duration(1.0), self.timer_callback)

        # Twistの初期化
        twiet = Twist()
        twiet.linear.x = 0
        twiet.angular.z = 0
        self.pub.publish(twiet)

    def scan_callback(self, data):
        # センサーの値を受け取る
        self.distance = data.range
        rospy.loginfo("distance: %f", self.distance)

    def timer_callback(self, event):
        # Twistの初期化
        twiet = Twist()
        # センサーの距離に応じて、旋回する
        if self.distance < 30:
            twiet.linear.x = 0
            twiet.angular.z = 0.5
        else:
            twiet.linear.x = 0.5
            twiet.angular.z = 0
        rate = rospy.Rate(10)
        for i in range(5):
            self.pub.publish(twiet)
            rate.sleep()

    def shutdown_callback(self):
        rospy.loginfo("Stop")
        rospy.sleep(1)
        twiet = Twist()
        twiet.linear.x = 0
        twiet.angular.z = 0
        self.pub.publish(twiet)


if __name__ == '__main__':
    try:
        Contorller()
        rospy.spin()
    except ROSInterruptException:
        pass

実装は以前Turtlebot3につけた超音波センサーを動かすときに使った以下の記事のコードを参考にしました。

zenn.dev

動かしてみる

それでは、実際に動かしてみます。

以下のコマンドを実行するとロボットが走行を始めます。

rosrun my_rober control_rober.py

これで以下のTweetのように障害物が近づいたら旋回してまた別の方向に走行するようになります。

終わりに

今回はロボットカーにつけた超音波センサーで実際に障害物を回避できるようにしてみました。

そして実機で走らせてみましたが、ようやくここまでこれたと満足しています。

これでやっとロボットカーとしての最低限の機能を実装できた気がします。

ただ、これはできているようにできていないことがあります。

それはROSの特徴である複数のマシンでできるトピック通信ができていないんです。

というわけで次回はマスターとスレーブに挑戦しようと思います。