HARD2021:ルンバをPythonプログラムでナビゲーションさせよう!

この記事はHARD2021(Home AI Robot Development)スプリングワークショップ用です。今回はナビゲーション用のPythonプログラムを作りましょう。このナビゲーションはロボットが通過する地点(ウェイポイント)を人間が設定し、それを順番にたどるウェイポイントナビゲーションとよばれているもっとも基本的なものです。ROSのナビゲーションの機能も使っているので途中で障害物があれば、ロボットがそれを避けてウェイポイントまで行ってくれる優れものです。

ナビゲーションのPythonプログラムを簡単にするためにROSのActionlibを使います。ROSには非同期で片方向通信のトピック通信、同期で双方向通信のサービス通信があります。あるノード(クライアント)が別のノード(サーバー)にロボットのあるタスクを依頼する場合、メッセージをやり取りする必要があるので双方向通信のService通信の方が都合が良いわけですが一つ問題があります。それはサーバーの処理が遅い場合、クライアントがその処理が終わるまでブロック(待機)させられてしまいます。さらに、クライアントが途中でそのタスクをキャンセルしたい場合や途中結果などを聞きたい場合がありますが、Service通信では対応していません。それに対応するためにROSには、アクション通信があります。

アクション通信 サービス通信と同様にタスクを依頼するアクションクライアント(ActionClient)と処理をするアクションサーバー(ActionServer)があり、サービス通信と異なる点は非同期通信ということです。actionlibパッケージはアクション通信のためのツールを提供しています。

まずは、次のROS Wiki、ROSチュートリアルを読んでActionLibの詳細を理解しましょう。

次のソースコードはこのC++プログラムのPython版です。プログラム的にはクラス、例外処理なども入ってきて高度になってきています。クラスを使うとグローバル変数なども使う必要がなくシンプルにコーディングできます。

サンプルプログラム

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy
import tf
import actionlib
from actionlib_msgs.msg import *
from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Point, Quaternion\
, Twist
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from math import pi


# クラス
class WpNavi():
    def __init__(self):  # コンストラクタ
        rospy.init_node('wp_navi')  # ノードの初期化
        rospy.on_shutdown(self.shutdown)  # シャットダウン時の処理

        # アクションクライアントの生成
        self.ac = actionlib.SimpleActionClient('move_base', MoveBaseAction)

        # アクションサーバーが起動するまで待つ。引数はタイムアウトの時間(秒)
        while not self.ac.wait_for_server(rospy.Duration(5)):
            rospy.loginfo("Waiting for the move_base action server to come up")

        rospy.loginfo("The server comes up")

        self.goal = MoveBaseGoal()  # ゴールの生成
        self.goal.target_pose.header.frame_id = 'map'  # 地図座標系
        self.goal.target_pose.header.stamp = rospy.Time.now()  # 現在時刻

        way_point = [[-1.0, -1.0, 0.0 * pi], [0.0, -5.0, 0.5 * pi],
                     [3.0, -5.0, -1.0 * pi], [3.0, 3.0, -0.5 * pi],
                     [-2.0, 2.0, 0.0 * pi], [999, 999, 999]]

        # メインループ。ウェイポイントを順番に通過
        i = 0
        while not rospy.is_shutdown():
            # ROSではロボットの進行方向がx座標、左方向がy座標、上方向がz座標
            self.goal.target_pose.pose.position.x = way_point[i][0]
            self.goal.target_pose.pose.position.y = way_point[i][1]

            if way_point[i][0] == 999:
                break

            q = tf.transformations.quaternion_from_euler(0, 0, way_point[i][2])
            self.goal.target_pose.pose.orientation = Quaternion(
                q[0], q[1], q[2], q[3])
            rospy.loginfo("Sending goal: No" + str(i + 1))

            self.ac.send_goal(self.goal)
            # サーバーにgoalを送信

            # 結果が返ってくるまで30.0[s] 待つ。ここでブロックされる。
            succeeded = self.ac.wait_for_result(rospy.Duration(30))

            # 結果を見て、成功ならSucceeded、失敗ならFailedと表示
            state = self.ac.get_state()
            if succeeded:
                rospy.loginfo(
                    "Succeeded: No." + str(i + 1) + "(" + str(state) + ")")
            else:
                rospy.loginfo(
                    "Failed: No." + str(i + 1) + "(" + str(state) + ")")

            i = i + 1

    # シャットダウン時の処理
    def shutdown(self):
        rospy.loginfo("The robot was terminated")
        self.ac.cancel_goal()  # ゴールをキャンセル


if __name__ == '__main__':
    # 例外処理。rospy.ROSInterruptExceptionを捕まえる。
    # この例外はCrl+cキーが押されるときに発生するので、
    # この例外処理によりこのノードが終了する。
    try:
        WpNavi()
        rospy.spin()
    except rospy.ROSInterruptException:
        rospy.loginfo("WP navigation finished.")

Launchファイル

  • ノードを実行するためにrosrunコマンドを実行しました。複数のノードを立ち上げたり、複数の変数を設定するのをコマンドでいちいち実行するのは面倒です。それを解決してくれるのがlaunchファイルよばれるファイルです。
  • launchファイルを作りたい場合、パッケージディレクトリの中にlaunchというディレクトリを作り、そのなかに拡張子がlaunchというファイルを作ります。それがlaunchファイルです。以下のコードがその例です。launchファイルはXML形式で書きます。XML形式はいろいろなアプリでよく使われています。
    • launchタグ: launchファイルは<launch>で始まり</launch>で終わります。その中に書いていきます。
    • argタグ:引数を指定する。
      • name: 引数名。
      • default:引数のデフォルト(規定)値
      • value: 引数の値
    • nodeタグ:ノードを設定する。
      • pkg:パッケージ名。この例ではwp_navi。
      • type:実行ファイル名。この例ではwp_navi.py。
      • name:ノード名。この例ではwp_navi。パッケージ名とノード名は同じ場合が多い。
      • ns: 名前空間(name space)。create_autonomyパッケージでは複数台のロボットを扱うために名前空間を使っている。ns = $(args ns)は3行目で設定した値 create1を名前空間としている意味。これがないとwp_navi.pyは動かない。
      • output:screenの場合はノードからの出力を端末に表示する。指定しないとログファイルだけに記録される。

 

パッケージの作成

  • パッケージは次のcatkin_create_pkgコマンドで作ります。依存パッケージはそのパッケージを作るために必要なパッケージです。
    • $ cd ~/catkin_ws/src/hard2021
    • $ catkin_create_pkg wp_navi roscpp rospy std_msgs

ソースコードの作成

  • 次のコマンドでgeditを開き、上のソースコード(wp_navi.py)をコピペしてセーブする。
    • $ gedit ~/catkin_ws/src/hard2021/wp_navi/src/wp_navi.py &
  • wp_navi.pyに実行権を与える。
    • $ cd ~/catkin_ws/src/hard2021/wp_navi/src
    • $ chmod u+x wp_navi.py

Launchファイルの作成

  • ディレクトリの作成と移動
    • $ cd ~/catkin_ws/src/hard2021/wp_navi
    • $ mkdir launch
    • $ cd launch
  • 次のコマンドでgeditを開き、上図のソースコード(wp_navi.launch)を打ち込んでwp_navi.launchというファイル名をつけてlaunchディレクトリの中にセーブする。全部打ち込むのがつらい場合は、~/catkin_ws/src/create_autonomy/navigation/ca_move_base/launch/send_goal.launchファイルをコピーして改変すると楽できます。
    • $ gedit ~/catkin_ws/src/hard2021/wp_navi/launch/wp_navi.launch &

ビルド

  • $ cd ~/catkin_ws
  • $ catkin build wp_navi

実 行

  • 端末を2つ開く
  • 1番目の端末
    • $ export LOCALIZATION=amcl
    • 新しいcreate_playgroundの3Dモデルをネットから読み込むので表示に数分かかる。
      • $ roslaunch ca_gazebo create_playground.launch
  • 2番目の端末
    • $ source ~/.bashrc
    • 上で作成したwp_navi.launchファイルを使います。
      • $ roslaunch wp_navi wp_navi.launch
    • 上で作成したlaunchファイルを使わない場合は、wp_navi.pyの22行目の’move_base’を’create1/move_base’に変更して次のコマンドを実行する。
      • $ rosrun wp_navi wp_navi.py
  • 成功するとロボットがウェイポイント順番に通過し、ぐるっと1周してスタート地点に戻ります。

ハンズオン

  1. サンプルプログラムの実行
    • 上の説明に従ってサンプルプログラムを実行して動作を確認しよう!
  2. 指定した位置(x, y)へ移動する
    • 指定した位置へ移動するプログラムを作ろう。そのためには、指定位置と現在位置を比較して、現在位置が指定位置に近づく(その差が0に近づく)ようにロボットの並進速度と角速度を制御する必要があります。
    • ロボットの現在位置は/odomトピックからの情報を使ってください。
  3. ウェイポイントナビゲーションの実装
    • ROSのActionLibを使ったウェイポイントナビゲーションのPythonプログラムを説明しました。ActionLibを使わないで実装しよう。
    • 自己位置は自作オドメトリ関数をできれば使うおう。無理な場合は/odomトピックの情報を使ってもかまいません。
    • LIDARなどのセンサ情報処理についてはまだ学んでいないので障害物回避はしなくてもOKです。

終わり

HARD
スポンサーリンク
シェアする
demura.netをフォローする
demura.net

コメント

  1. goodnature より:

    上梓、おめでとうございます。
    首を長くして待っていたので、早速購入しましまた。
    これでルンバとLIDARの埃を払えるかと期待しています。
    今後も興味深い情報の提供、よろしくお願いします。

    • demu より:

      コメントありがとうございます.返事が遅くなりすみません.AIロボット入門を執筆する当初はルンバベースのサンプルプログラムを提供する予定でしたが,最近のルンバはiシリーズになり従来のROSドライバが使えなくなったことと,ROS2のナビゲーション関連のパッケージが整備されていない2つの理由から,拙著ではTurtlebot3ベースのサンプロプログラムになっています.ご期待にそえず申し訳ございません.

  2. goodnature より:

    希望の持てる回答ありがとうございます。
    出版を楽しみにしています。
    それまでの間は、pythonで動かして遊んでいます。

  3. goodnature より:

    当サイトは、差し当たり理屈は分からなくても指示された通りに入力すれば動作するので素人には助かっています。「HARD2021: ルンバをPythonプログラムで動かそう!」は実機(Roomba692)でも動かすことができましたが、「HARD2021:ルンバをPythonプログラムでナビゲーションさせよう!」は、実機で動かすことは可能でしょうか。三番目のターミナルで「 roslaunch ca_driver create_2.launch」を起動し実機に繋いでみると、訳の分からない挙動をします。メッセージは[CREATE] Loop running slowly.が出ます。
    LIDARはありません。「1万円ぐらいで購入できるとっても安価なLIDAR、SLAMTEC社のRPLIDAR A1M8」が魅力的なのて、組み合わせてナビゲーションができるようになればいいなと思っています。実機の場合はどうしたらよいかを追記する予定はございますでしょうか。

    • demu より:

      コメントありがとうございます.このサイトでは理論的な記事はないですが,基礎理論とROS2の実践を兼ねた入門書を有志の先生方と執筆中です.その本では,実機で動かすこと前提にしています.ナビゲーションは私の担当で,RPLIDAR A1M8も所有しているので,実機での動かし方も記事にしていく予定です.もう,少々お待ちください.

タイトルとURLをコピーしました