🐢

ROSによるロボット操作法 Part-2 シミュレーション(Turtle)編

2023/12/18に公開

0.記事の内容

群馬県産業技術センターではAI技術等をはじめとしたデジタル技術を広く県内企業の方にも活用頂けるようデジタルソリューションラボDSL[外部リンク] 等を通じて情報等の提供を行っています。この記事では、ROSの基本的な考え方とDSLに整備されたTHK(株)社製の移動台車を備えた単腕のロボットのシミュレーションによる操作の手順等についてまとめています。

このPart-2は文字数の制限から1つの内容を前半のTurtle編と後半のRobot編に分けています。Part-1およびPart-2の後半の内容は次のリンクからご覧頂けます(別のページか開きます)。
Part-1 実機編 [リンク]
Part-2 シミュレーション(Robot)編 [リンク]

1.はじめに

近年、人に代わって資材や部品等の運搬を自動で行うAGVやAMRなどの機器が製造業や物流業の現場等で広く用いられ、生産性の向上や作業者の負担軽減、人件費の削減、人手不足の解消などに寄与しています。一方、ロボットの移動では、LiDARやカメラ等を利用した自己位置推定や移動範囲の地図の作成などを行うSLAM技術が用いられ、AI技術等も活用して人や障害物との衝突を回避するような制御や効率的な移動ルートの設定などを行うことにより、動きの定まらない作業員等との協働も可能となってきています。そして、今後も自動搬送ロボットはより高機能なものへと進化し、適用範囲の増加等による市場の拡大も期待されています。

また、これらのロボットのソフト開発ではフレームワークを提供するフリーのロボット用ソフトのROSが広く用いられています。その理由の一つとして、ROSには様々なライブラリがパッケージとして用意されているため、すべてをはじめから開発する場合と比べて、自動搬送ロボットなどの開発を容易にすることがあります。

群馬産業技術センターでは、中小企業のDX化を体験できる場である「デジタルソリューションラボ DSL」にTHK(株)社製の移動台車を備えた単腕のアームロボットを整備しました。そして、実際にロボットを操作することによりROSの機能やSLAMによる地図の作成と移動等を確認できるように手順書「ROSによるロボットの操作法 Part-1 実機編」を作成しました。そこでは、導入したロボットを実際に動かすことをテーマに、必要なROSの環境設定やロボットのマニュアル操作、SLAMによる地図の作成、Pythonのプログラムによる制御等の手順を示しました。

一方、本手順書「Part-2 シミュレーション編」では、Part-1で設定したROSの環境を用いて、実際のロボットではなく、シミュレーションにより表示されたロボットをパソコン上の3Dビジュアライザー(Rviz)や3Dモデルのバーチャルシミュレータ(Gazebo)により操作して確認を行います。そのため、ロボットの意図しない動きによる危険性等はなく、ROSに慣れていない方でもロボットの制御の仕組みやPythonによるロボットのプログラム開発の手順を容易に確認することができます。用いたROSはPart-1と同様にROS1と呼ばれるものですが、ROSに関心をもち、概要を知りたい技術者の参考になればと思います。

            AGV    :  Automatic Guided Vehicle (無人搬送車)
            AMR    :  Autonomous Mobile Robot (自律走行搬送ロボット)
            LiDAR  :  Light Detection And Ranging (光による検知と測距)
            SLAM   :  Simultaneous Localization and Mapping (自己位置推定とマッピングの同時実行)
            ROS    :  Robot Operating System (ロボット開発プラットフォーム)
            DSL    :  Digital Solution Laboratory

2.ROSの環境

Part-2で利用するROSの環境は、Part-1で設定したものを、そのまま利用します。必要なソフトもすでにインストールされており、新たなソフトの導入等は必要ありません。

実際にDSLを利用できない方など、本手順書Part-2から利用される場合でも、Part-1の以下の部分を参考に、環境の設定を行うことで対応可能です。

(1) Live USB
Part-1の第2章に沿ったLive USBの作成とUbuntuの起動を確認して下さい。NEDO特別講座用ISOファイルのダウンロード先のホームページの様子を図2-1に、作成したLive USBによるUbuntu18.04の起動時のデスクトップの画面を図2-2にそれぞれ示します。

(2) プログラム環境
Part-1の第6章の6-2節のPythonによるROSプログラムの開発環境の設定と6-3節の端末にメッセージを表示する送信と受信のプログラムの動作確認を行って下さい。確認を行うときに実行する3つのプロクラムのコマンド入力の画面を図2-3に示します。


図2-1 NEDO特別講座用ISOファイルのダウンロード先Webページ


図2-2 Live USBによるUbuntu18.04のデスクトップ


図2-3 確認用プログラムのコマンド入力の画面

3.CUIによるTurtleの操作

この章では、ROSの仕組みを理解するためにTurtlesimというTurtle(亀)のキャラクターを簡単な操作で移動と回転ができるプログラムを用います。

3-1.Turtlesimの実行

(1) ROSシステムの環境
Live USBをパソコンに挿入して、ROSシステムの設定されたUbuntu18.04の環境を起動します。

(2) Byobuの起動
Part-1の4-2節の「(1) Byobuターミナルマルチプレクサ」で紹介のByobuを起動します。
Desktopの左側にあるタスクバーから「ターミナル」を起動し、実行環境のフォルダを /home/seed/rosに変更して「byobu」コマンドを入力します。

            cd /home/seed/ros
            byobu

主に用いるキー操作は、ウインドウの「追加」、「切り替え」、「閉じる」です。

            新規ウインドウの追加 : F2
            前のウインドウに移動 : F3
            後のウインドウに移動 : F4
            ウインドウを閉じる   : exit または CTRL+D

また、現在のウインドウは、ターミナルの下段に表示される 0 からはじまるウインドウの番号により識別できます。

(3) roscoreの起動
Byobuの最初のウインドウ0で次のコマンドを入力します。

            roscore

これは、ROSで通信を行うために必要なもので、ROSのプログラムの実行に先立って起動しておく必要があります。

(4) Turtlesimの起動
F2を押して、Byobuにウインドウ1を追加し、次のコマンドを入力してTurtlesimプログラムを起動します(図3-1)。

            rosrun turtlesim turtlesim_node

新たに画面が現れ、中央にTurtleが表示されます。


図3-1 turtlesim_nodeの起動

(5) Turtle制御プログラムの起動
Turtleの移動を制御する2つのプログラムがあります。どちらを利用しても構いません。

a. 矢印キーで制御
F2を押して、Byobuにウインドウ2を追加し、次のコマンドを入力します(図3-2)。

            rosrun turtlesim turtle_teleop_key

Turtleの操作は、次のキーを用いて行います。

            上 : 前進
            下 : 後退
            左 : 左回転
            右 : 右回転
            k  : 停止

このプログラムを起動したウインドウにフォーカスがあるときに、キーを押すと、Turtleが少し動いて停止します。プログラムの終了はCTRL+Cを押します。


図3-2 turtle_teleop_keyの起動

b. ijkl等のキーで制御
F2を押して、Byobuにウインドウ3を追加し、次のコマンドを入力します(図3-3)。
入力は一行で行って下さい。

            rosrun teleop_twist_keyboard teleop_twist_keyboard.py cmd_vel:=/turtle1/cmd_vel

画面に操作用のキーの割り当てが表示されます。主に用いるキーは、以下のものです。

            i : 前進
            j : 左回転
            k : 停止
            l : 右回転
            q : 速度のUp
            z : 速度のDown

このプログラムを起動したウインドウにフォーカスがあるときに、キーを押すと、Turtleが少し動いて停止します。押し続けると連続で移動します。プログラムの終了はCTRL+Cを押します。


図3-3 teleop_twist_keyboardの起動

(6) Turtleの移動による軌跡の確認
キー操作により、Turtleが移動や回転して軌跡が描かれます。動作中でも別のキーを受け付けますので、いろいろな線を一筆書きで描けます。

3-2.ROSの通信

ROSで制御を行うためのメッセージ通信について示します。

(1) ノード
ROSのプログラムでは、最小のプログラムであるプロセスをノード(Node)と呼び、それぞれのノード間の通信により必要なメッセージの交換を行います。メッセージは、通信により交換されるデータで、それぞれ型を持っています。

また、Masterと呼ばれるプロセスは、ROSプログラムの実行時にノードとメッセージの名前を管理するネームサーバーで、ROSでは、最初にroscoreコマンドにより起動する必要があります。このとき、/rosoutというノード名のノードが作られます。
ちなみに、ROS2では、システムの構成が異なりroscoreを起動する必要はありません。

(2) 通信の種類
ノード間で行われるメッセージ通信には、以下のものがあります。
ここで、アクション通信とパラメータ通信を通信に含めるかは、文献により異なります。

a. トピック通信 (Topic)
Publisher/Subscriber型の一方向の非同期通信です。
送信側のノードは、データを設定してから指定されたトピック名のメッセージを送信(Publish)します。受信側のノードは、指定されたトピック名のメッセージを受信(Subscribe)して、データを受取ります。

b. サービス通信 (Service)
Server/Client型の双方向の同期通信です。
送信側(クライアント、Client)から送信されたリクエスト(要求)メッセージを、受信側(サーバー、Server)が受信して、結果をレスポンス(応答)メッセージとして送信側に返します。受信側は、その間、待ちの状態となるため、サーバー側での処理に時間の掛からないときの通信として用いられます。

c. アクション通信 (Action)
サービス通信と同様に双方向の同期通信ですが、サーバーがクライアントに途中経過等のフィードバックを返す点が異なります。

送信側(アクションクライアント)は、ゴールサービス通信により目標値のリクエストメッセージを送信し、受信側(アクションサーバー)は、レスポンスメッセージを返します。
次に、受信側は、フィードバックトピック通信により途中経過を送信側に繰返し送信します。
そして、目標値が達成されたことを送信側が認識すると、送信側はリザルトサービス通信により、受信側にリクエストメッセージを送信し、受信側は、レスポンスメッセージを返します。
アクションサーバー側での処理に時間が掛かり、途中の経過を知りたいときに利用される通信です。

d. パラメータ通信 (Parameter)
ROSプログラムの実行中にノードの設定値(パラメータ)を変更するときに用いる通信です。
ロボットの動作や環境などを実行中に変更したいときなどに利用されます。

3-3.Turtlesimの仕組み

Turtlesimで行われている通信を利用して、その仕組みを確認します。

(1) ノードとメッセージ
3-1節のTurtleの制御ができる状態で、F2を押して、新たにByobuにウインドウ4を追加します。

a. ノード名
次のコマンドを入力すると起動されているノードのノード名のリストが表示されます(図3-4)。

            rosnode list

ノードとプログラムの対応は以下のようになります。

            /turtlesim              : Turtlesim_nodeプログラムのノード名
            /teleop_turtle          : turtle_teleop_keyプログラムのノード名
            /teleop_twist_keyboard  : teleop_twist_keyboardプログラムのノード名
            /rosout                 : roscoreプログラムにより起動したノードのノード名

b. トピックメッセージ
次のコマンドを入力すると現在稼働中のROSプログラムで通信されているトピックメッセージの一覧が表示されます(図3-4)。

            rostopic list

リストの中の/turtle1/cmd_velが、/teleop_turtleノードから送信(Publish)されるトピックメッセージで、turtleの速度指令(command velocity)になります。そして、このメッセージは、/turtlesimに受信(Subscribe)されturtleを制御します。


図3-4 ノード名とトピック名のリスト

c. グラフ表示
Byobuのウインドウ4で次のコマンドを入力するとノード間のメッセージの送信と受信の関係が図示されます(図3-5)。

            rqt_graph

左上の設定を「Nodes only」とします。楕円で囲まれたものがノード、矢印がトピックメッセージで、2つのノードから速度指令のメッセージの矢印が/turtlesimに繋がっていることが分かります。


図3-5 ノードとトピックの関係図

d. ノードの情報
通信を行うためには、同じ名前のメッセージを送信、受信する必要があります。
F2を押して、新たにByobuにウインドウ5を追加します。ノードが送信、受信するメッセージ名、及びサービス名を、次のコマンドにより確認できます(図3-6)。

            rosnode info /teleop_turtle
            rosnode info /turtlesim

表示される画面からノードごとに以下の項目が確認できます。

            Publications    : 送信するトピックメッセージ名と型
            Subscriptions   : 受信するトピックメッセージ名と型
            Services        : 提供するサービス名

これより、送信側の/teleop_turtleのPublicationsと受信側の/turtlesimのSubscriptionsに同じ /turtle1/cmd_vel [geometry_msgs/Twist]のトピックメッセージのあることが分かります。また、[ ]内はメッセージの型を表します。

ROSでは、プログラムを変更することなく、プログラムの起動時にリマップの機能を用いてメッセージ名やノード名を変更することができます。これにより、通信をしようとしているノード間で、送受信するメッセージ名が異なるときでも、同じ名前に変更して通信をさせることができます。


(a) /teleop_turtleノード


(b) /turtlesimノード
図3-6 ノードの情報

【参考】
手順書Part-1の第6章6-4節の(4)で作成したプログラムrobot_mover.pyは、実際のロボットを矩形に移動させるものです。このプログラムは、ロボットを移動させるために/cmd_velのトピック名のメッセージを送信していますので、リマップによりトピック名を変更して合わせることにより、TurtlesimのTurtleを矩形に移動させることができます。

Byobuのウインドウ5で次のコマンドを入力することにより、Turtleの軌跡が消去され、リマップによりrobot_mover.pyが矩形に移動します(図3-7)。

            rosserivce call /clear
            rosrun dsl_ros_pkg robot_mover.py cmd_vel:=/turtle1/cmd_vel


図3-7 robot_mover.pyによるTurtleの制御

e. cmd_velのリマップ
Byobuのウインドウ5で次のコマンドを入力するとGUIによりロボットの速度指令を送信するプログラムが起動します(図3-8)。

            rosrun rqt_robot_steering rqt_robot_steering

現れた画面の上段のトピック名のところを/turtle1/cmd_velに設定します。縦のスライダーで前進、後退の速度、水平のスライダーで左右の回転速度のトピックメッセージを送信し、TurtlesimのTurtleの動作を制御できます。一方の動作をしているときに、他方の動作を同時にさせることも可能です。


図3-8 rqt_robot_steeringによるTurtleの制御

【参考】
rqt_robot_steeringのノード名はrqt_gui_py_node_4835 (数字は異なる)で、2つ作成されます。F2を押して、新たにByobuにウインドウ6を追加し、rosnode listで確認できますが、rqt_graphでは表示されない様です(図3-9)。


図3-9 rqt_robot_steeringのノード名

次に、GUIの上段のトピック名のところを/cmd_velに設定すると、スライダーを操作しても、Turtleは動作しないことが分かります。また、rqt_robot_steeringのプログラムを起動したウインドウにWarningが出力されます。

rqt_robot_steeringのプログラムを起動してGUIの画面でトピック名を設定することは、リマップにより移動速度と回転速度の指示の送信先を/turtle1/cmd_vel等に変更することに対応します。
また、以下のようにプログラムの起動時にコマンドでリマップを設定することもできます。この場合は、GUIの画面でトピック名を変更する必要はありません。

            rosrun rqt_robot_steering rqt_robot_steering cmd_vel:=/turtle1/cmd_vel

【参考】
トピック名/cmd_velの前の/turtle1は、同一のトピック名を区別するための名前空間です。例えば、Turtlesimで複数のTurtleを識別することができます。(3-3節の(2) c項)

f. メッセージの型
(ア) メッセージの型の具体的内容は、F2を押して、Byobuにウインドウ7を追加し、次のコマンドを入力することにより確認できます(図3-10)。

            rostopic type /turtle1/cmd_vel

これより /turtle1/cmd_velは、geometry_msgs/Twist型のトピックであることが分かります。

(イ) その型の具体的な内容は、次のコマンドを入力することにより確認できます(図3-10)。

            rosmsg show geometry_msgs/Twist

対応する変数名と型が示されます。
型名は表示せずに、メッセージ名から直接、変数名と型を表示させることもできます(図3-10)。

            rostopic type /turtle1/cmd_vel | rosmsg show

(ウ) 次のコマンドにより、実際に通信されるメッセージの内容をモニタすることでも分かります(図3-11)。

            rostopic echo /turtle1/cmd_vel

表示は、コマンドを入力してから、速度指令を送信するためにByobuのウインドウ3のteleop_twist_keyboardプログラムでキーを押します。モニタの終了はCTRL+Cです。
これよりgeometry_msgs/Twist型の/turtle1/cmd_velは、linearの直進速度[m/s]のx,y,z成分とangularの回転速度[rad/s]のx,y,z成分を持つ型であることが分かります。図3-11で送信されるx: 0.405は、図3-3の最下段のspeed 0.405の値です。

(エ) ROSの3次元の座標系は、図3-12のように設定されています。Turtleの頭の進む方向がx方向です。Turtlesimでは、Turtleは2次元平面上で移動と回転を行いますので、速度はx,y成分、回転はz成分の指定が可能で、負の値も設定できます。


図3-10 トピックの型と内容


図3-11 トピックメッセージのモニタ


図3-12 ROSの3次元の座標系

(2) コマンドによるTurtlesimの制御
a. Turtleが描いた軌跡の消去
Turtleの描いた軌跡は、Byobuのウインドウ7で、次のコマンドを入力することで消去できます。

            rosservice call /clear

これはサービス通信で、送信したメッセージはTurtlesimノードが受信し、図3-6(b)で確認したTurtlesimのServicesの/clearを呼び出(リクエスト)しています。

b. トピックの送信
プログラムではなく、コマンドにより、直接、/turtle1/cmd_velのトピックメッセージを送信(Publish)して、Turtleを移動させることができます。Byobuのウインドウ7で、次のコマンドを入力します(図3-13)。

            rostopic pub /turtle/cmd_vel geometry_msgs/Twist -- '[2.0, 0.0, 0.0]' '[0.0, 0.0, 1.8]'

トピック名の/turtle/cmd_velとその型geometry_msgs/Twistを指定して、-- 以降に移動速度のx,y,z成分と回転速度のx,y,z成分を設定します。停止はCTRL+Cを押します。

このコマンドにより、実際にTurtleが移動しますが、すぐに止まってしまいます。
これは、1回のコマンドでは数秒しか移動しないためです。連続的にコマンドを送信するためには、次ようにコマンドの繰返し周波数を -r に続けて設定する必要があります(図3-13)。

            rostopic pub -r 1 /turtle/cmd_vel geometry_msgs/Twist -- '[2.0, 0.0, 0.0]' '[0.0, 0.0, 1.8]'

これより、1Hz(=1sec)の間隔でコマンドが送信され、Turtleは移動し続けて円を描きます。Turtleの停止はCTRL+Cです。


(a) コマンドの単発実行


(b) コマンドの連続実行
図3-13 コマンドによるトピックメッセージの送信

c. 複数のTurtle
サービス通信を利用して、TurtlesimにTurtleを追加することができます。

(ア) Byobuのウインドウ7で、次のコマンドを入力します(図3-14)。
/spawnがTurtleを発生させるサービスで、続いてTurtleの発生する位置x,yと向きzを設定します。

            rosservice call /spawn 2 3 1 ""

このとき、次の応答が表示されます。

            name: “turtle2”

(イ) 位置(x,y)は、Turtlesimの画面の左下隅が(0,0)で、右上隅が(11,11)です。
zは、π/2(≒1.5)のときに左回りに90度、回転します。""にはTurtleを識別する名前空間を設定しますが、何も設定しないときは、自動でTurtle + 番号が設定されます。これは、サービス通信のレスポンスメッセージとして戻され、画面に表示されます。

(ウ) この新たに生成されたTurtleを移動させるときは、turtle_teleop_key、teleop_twist_keyboard、rqt_robot_steeringのプログラムの起動時に、トピック名の/cmd_velを/turtle2/cmd_velにリマップをする必要があります。

(エ) Byobuのウインドウ7で、次のコマンドを入力して、rqt_robot_steeringを起動し、GUIの上段のトピック名のところを/turtle2/cmd_velに設定します。ウインドウ5で起動したGUIと並べると2つTurtleの操作がし易くなります(図3-15)。

            rosrun rqt_robot_steering rqt_robot_steering


図3-14 /spawnによるTurtleの発生


図3-15 rqt_robot_steeringを並べて配置

また、turtle2を削除するときは、F2を押して、Byobuにウインドウ8を追加し、以下のコマンドを入力します(図3-16)。

            rosservice call /kill “turtle2”


図3-16 Turtleの削除

(3) その他の通信のコマンド
a. Turtlesimの背景色
パラメータ通信により、Turtlesimの背景色を設定できます。

(ア) Byobuのウインドウ8で、次のコマンドを入力することにより、稼働中のROSのパラメータサーバーにあるパラメータの一覧を表示できます(図3-17)。

            rosparam list

リストの中の次のものが、背景色の3原色RBGのパラメータです。

            /turtlesim/background_b 
            /turtlesim/background_g 
            /turtlesim/background_r 

(イ) 現在の青色の設定値は、次のコマンドにより取得できます。

            rosparam get /turtlesim/background_b 

次のコマンドにより、パラメータの設定値の一覧を表示することもできます。

            rosparam get /

(ウ) 青色の値の変更は、次のコマンドにより行えます。

            rosparam set /turtlesim/background_b 150

(エ) 但し、設定した色を反映させるためには、軌跡の消去のときに用いた/clearサービスを実行する必要があります。

            rosservice call /clear


図3-17 パラメータの表示と設定除

b. Turtlesimの軌跡の設定
サービス通信の/turtle1/set_penサービスを用いて、軌跡の設定ができます。

(ア) 次のコマンドにより、/turtle1/set_penサービスのパラメータが表示されます(図3-18)。

            rosservice args /turtle1/set_pen

これより、次のパラメータの設定ができます。

            軌跡の色           : r,g,b
            軌跡の太さ         : width
            軌跡の表示/非表示  : off

(イ) Byobuのウインドウ8で、次のコマンドにより、軌跡の色を赤、太さを5、線を表示するように設定できます(図3-18)。

            rosservice call /turtle1/set_pen 255 0 0 5 0

設定後、Turtleを移動して設定の確認ができます。


図3-18 軌跡の色と太さの変更

3-4.トピックの記録と再生

稼働中のROSで通信されているトピックメッセージとその発生タイミングを記録、表示、再生することができます。ここでは、TurtlesimのTurtleの移動を記録します。
次の手順によりこれまでの設定をすべて終了し、改めてROSを起動します。

a. Byobuのウインドウで CTRL+Cを押してプログラムを終了し、CTRL+Dを押してByobuのウインドウを閉じる操作を繰り返します。
b. Byobuの最後のウインドウを閉じて、Byobuを終了します。
c. ターミナルをCTRL+Dを押して終了し、閉じます。

(1) roscoreの起動
「ターミナル」を起動し、実行環境のフォルダを/home/seed/rosに変更して「byobu」コマンドを入力します。

            cd /home/seed/ros
            byobu

Byobuの最初のウインドウ0で、次のコマンドを入力します。

            roscore

(2) Turtlesimの起動
F2を押して、Byobuにウインドウ1を追加し、次のコマンドを入力してTurtlesimプログラムを起動し、Turtlesimの画面を表示させます。中央にTurtleが表示されます(図3-19)。

            rosrun turtlesim turtlesim_node


図3-19 turtlesim_nodeの起動

(3) 操作用プログラムの起動
F2を押して、Byobuにウインドウ2を追加し、次のコマンドを入力して操作用のGUIを表示させます(図3-20)。

            rosrun rqt_robot_steering rqt_robot_steering

GUI画面でトピック名を/turtle1/cmd_velに設定します。


図3-20 rqt_robot_steeringの起動

(4) 記録の開始
F2を押して、Byobuにウインドウ3を追加し、次のコマンドを入力してフォルダを変更し、記録を開始します。

            cd /home/seed/ros
            rosbag record -a -o turtle.bag

ここで、オプションの設定は以下の通りです。

            -a : すべてのトピックメッセージを記録
            -o : 保存ファイル名を指定

保存ファイルは「ファイル名+年月日時間.bag」のファイル名でコマンドを起動したフォルダに保存されます。

(5) Turtleの移動
(3)のウインドウ2で起動したGUIプログラムにより、Turtleの移動等を行います。

(6) 記録の停止
(4)のウインドウ3で起動したrosbagのプログラムを CTRL+C で停止させます(図3-21)。


図3-21 rosbagによる記録の停止

(7) 記録情報の確認
(4)のウインドウ3で、次のコマンドを入力すると記録された情報の確認が行えます(図3-22)。
ファイル名は、実行の日時により異なります。

            rosbag info turtle_2023-mm-dd-hh-mm-ss.bag


図3-22 記録情報の確認

(8) 記録の再生
(4)のウインドウ3で、次のコマンドを入力すると記録された内容が再生されます(図3-23)。

            rosbag play turtle_2023-mm-dd-hh-mm-ss.bag

但し、今回、記録したTurtlesimの内容は、Turtleへの速度指令のため、再生するときのTurtleの位置から、移動と回転の速度のトピックが送信されるだけで、記録したときと同じ軌跡になるとは限りません。
スペースキーで再生の停止/再開、sキーでステップ実行ができます。また、コマンドに再生速度のオプションを追加できます。

            rosbag play turtle_2023-mm-dd-hh-mm-ss.bag -r 2.0

-r 2.0 と再生速度を約2倍にすると、トピックの送信間隔が半分になります。
但し、実行時間が半分になったことに対応して軌跡の移動距離も約半分になります。


図3-23 記録の再生

(9) タイミングチャートの表示
(4)のウインドウ3で、次のコマンドを入力するとトピックの送信されたタイミングチャートが現れます(図3-24)。

            rqt_bag turtle_2023-mm-dd-hh-mm-ss.bag

a. チャートの拡大縮小
チャートの上でマウスのホイールを操作すると、時間軸の拡大、縮小ができます。

b. トピックメッセージのグラフ表示
トピック名/turtle1/poseまたは対応するチャートの上で、マウスの右クリックメニューから View > Plot を選択するとグラフが現れます。

・グラフの右上の「閉じる✕」の隣の〇をクリックするとフローティングとなり、マウスでの移動やタイミングチャートの画面への挿入ができます。
・境界をドラッグすると大きさの変更ができます。
・トピックのx,yをチェックすると、時間に対する各値のグラフが表示されます。
・軸や曲線の設定は、Editボタンを押して、現れた画面で行うことができます。
・グラフの上の十字のアイコンをクリックした後、グラフ上でのマウス操作により移動(左ドラッグ)や拡大縮小(右ドラッグ)ができます。

c. タイミングチャートの再生
画面の上段の三角のPlay/Pauseボタンをクリックすると、チャート内の縦棒が移動し、同期してグラフ内の縦棒も移動し、値を確認できます。


(a) 起動時のタイミングチャート


(b) タイミングチャートの拡大


(c) 右クリックメニュー( View > Plot)の選択


(d) グラフの表示


(e) グラフの表示位置の変更とx,y座標の経過プロット


(f) グラフの設定変更画面


(g) グラフの表示範囲の変更後


(h) タイミングチャートの再生(赤色縦棒の移動)
図3-24 タイミングチャートの画面

(10) 終了
rqt_bagは、起動したByobuのウインドウでCTRL+Cを押して終了します。
また、ROSの終了は、これまでと同様に、各Byobuのウインドウで CTRL+Cを押してプログラムを終了し、CTRL+Dを押してByobuのウインドウを閉じます。最後に、ターミナルもCTRL+Dで終了します。

4.GUIによるTurtleの操作

この章では、第3章で行ったターミナルでのコマンド入力によるTurtlesimのTurtleの移動や回転の制御をGUIツールのRQtプログラムを用いて行います。
RQtは、第3章で利用したコマンドによるROSのノードやトピック、サービスメッセージの確認、値の変更、メッセージの送信などを視覚的に行うことができます。

4-1.RQtの起動

3-1節と同様に改めてROSのシステムを起動して、RQtのプログラムを起動します。

(1) ROSシステムの環境
Live USBをパソコンに挿入して、ROSシステムの設定されたUbuntu18.04の環境を起動し、Desktopの画面とします。

(2) Byobuの起動
Desktopの左側にあるタスクバーから「ターミナル」を起動し、フォルダを/home/seed/rosに変えて、「byobu」コマンドを入力します。

            cd /home/seed/ros
            byobu

(3) roscoreの起動
Byobuの最初のウインドウ0で次のコマンドを入力します。

            roscore

(4) Turtlesimの起動
F2を押して、Byobuにウインドウ1を追加し、次のコマンドを入力してTurtlesimプログラムを起動します。

            rosrun turtlesim turtlesim_node

(5) RQtの起動
F2を押して、Byobuにウインドウ2を追加し、次のコマンドを入力してRQtプログラムを起動します(図4-1)。

            rqt

表示された画面に何かのペインが表示されているときは、右上の「Close」のボタンを押して、すべて閉じます。


図4-1 RQtの起動

4-2.Turtleの起動と制御

(1) 移動命令
第3章3-3「(2) コマンドによるTurtlesimの制御」b.のコマンドによるトピックメッセージの送信に対応して、次の手順でTurtleを移動させることができます。

a. RQtの画面で、上段のメニューから次の項目を選択し、Message Publisherを起動します(図4-2)。

            Plugins > Topics > Message Publisher


図4-2 Message Publisherの起動

b. 画面のペインに項目が表示されているときは、右上端の「Clear all publishers」のボタンを押します。

c. Topicの項目からトピックメッセージ/turtle1/cmd_velを選択すると、Typeの項目がメッセージの型geometry_msgs/Twistになります(図4-3)。


図4-3 トピックメッセージの選択

d. 右側の「Add new publisher」のボタンを押すと、トピックメッセージがMessage Publisherのペインに追加されます(図4-4)。


(a) 追加ボタン


(b) Message Publisherペイン
図4-4 トピックメッセージのペインへの追加

e. トピックメッセージ/turtle1/cmd_velの先頭の「右向き三角形」をクリックするとlinearとangularの項目が現れ、さらに、それらの「右向き三角形」をクリックするとx,y,zの成分が表示されます(図4-5)。


図4-5 トピックメッセージ/turtle1/cmd_velの成分

f. RQtでのトピックメッセージの送信では、ペインのlinearのx,yをダブルクリックして移動速度の値を、angularのzをダブルクリックして回転速度の値を、それぞれ設定します。

g. トピックメッセージ/turtle1/cmd_velの行で、マウスの右クリックメニューから、「Publish Selected Once」を選択すると、Turtleは少し移動して停止します(図4-6)。


図4-6 トピックメッセージの値の設定と送信

h. トピックメッセージ/turtle1/cmd_velの行の先頭の四角形をクリックして、チェックすると、その行のRateで設定された周波数で、繰返しメッセージが送信されます。停止は、四角形をクリックして、チェックを外します(図4-7)。


図4-7 メッセージ送信の繰返し

(2) 移動情報のモニタ
第3章3-3「(1) ノードとメッセージ」a.のノード名のリスト表示や f.のrostopic echoによるメッセージ内容の表示に対応して、前項(1)で送信されたメッセージを、次の手順で確認できます。

a. 上段のメニューから次の項目を選択します。

            Plugins > Topics > Topic Monitor

追加されたTopic Monitorのペインは、右上端の「Toggle Floating」のボタンを押すと独立した画面となり、移動したり、改めてRQtの画面に挿入したりすることができます(図4-8)。また、ペインの境界をドラッグしてサイズの変更も可能です。


(a) Topic Monitorの起動


(b) ペインの切り離し(Floating)


(c) ペインの挿入
図4-8 ペインの切り離しと挿入

b. ペインに表示のメッセージトピックのリストから、/turtle1/cmd_velおよび/turtle1/poseの行の先頭の四角形をチェックして、モニタするトピックメッセージを設定します。また、非表示のメッセージの成分を展開して表示させます。
/turtle1/cmd_velは、Turtleへの速度指令の情報で、/turtle1/poseは、Turtleの現在の位置と角度の情報です。

c. Message Publisherの画面から、トピックメッセージ/turtle1/cmd_velを送信します。

d. Turtleの移動に対応して、Topic Monitorの画面の/turtle1/cmd_velのlinearのx,yとangularのz、および/turtle1/poseのtheta,x,yの値が変化します(図4-9)。


図4-9 トピックメッセージのモニタ

(3) メッセージの型
第3章3-3「(1) ノードとメッセージ」a.のノード名のリスト表示やf.のrosmsg showによるトピックメッセージの型の内容表示に対応して、次の手順でリストと型の内容を確認できます。

a. 上段のメニューから次の項目を選択します(図4-10)。

            Plugins > Topics > Message Type Browser


図4-10 Message Type Browserの起動

b. メッセージ/turtle1/cmd_velの型geometry_msgs/Twistの内容を確認するために、Message Type BrowserのMessageの行の「Package」にgeometry_msgsを、「Message Name」にTwistを設定し、「Add Currently Selected Message」のボタンを押してペインに追加します(図4-11)。


(a) Packageの選択


(b) Message Nameの選択
図4-11 Message Type Browserでの型(Type)の設定

c. geometry_msgs/Twistの行の「右向き三角形」を展開し、内容を確認できます(図4-12)。
実際には、(2)のTopic Monitorで、同様の内容が確認できますので、確認後は、ペイン右上の「Close」のボタンで画面を閉じて構いません。


図4-12 トピックメッセージの型(Type)の内容

(4) ノードのグラフ表示
第3章3-3「(1) ノードとメッセージ」c.の関係図を次の手順で表示できます。

a. 上段のメニューから次の項目を選択します(図4-13)。

            Plugins > Introspection > Node Graph


図4-13 Node Graphの起動

b. 現れた画面の左上の「Graph Type」を「Nodes/Topics (active)」とします(図4-14)。


図4-14 Nodes/Topics (active)の選択

c. 表示の大きさは、「Fit Graph in view」のボタンで全体表示、マウスのホイールで拡大、縮小の調整ができます(図4-15)。


図4-15 表示サイズの調整

利用しないペインは、右上の「Minimize」のボタンを押してアイコン化し、画面を広くできます。表示を再開するときは、アイコン化されたアイコンをクリックします(図4-16)。また、ペインを重ねることでタブによりペインを選択できるようになります。


図4-16 ペインのMinimizeによるアイコン化

(5) GUIによる速度指令の送信
第3章3-3「(1) ノードとメッセージ」e.で起動したrqt_robot_steeringプログラムを
次の手順で起動できます。上段のメニューから次の項目を選択します。

            Plugins > Robot Tools > Robot Steering

操作は、第3章の場合と同様で、GUIの上段のトピック名を/turtle1/cmd_velに設定し、スライダーで直進と回転の速度を設定します(図4-17、図4-18)。


図4-17 Robot Steeringの起動化


図4-18 Robot Steeringペインの表示位置調整

(6) サービスメッセージの送信
第3章3-3のサービス通信に対応して、次の手順でメッセージを送信してTurtlesimの設定を行うことができます。

a. 上段のメニューから次の項目を選択します。

            Plugins > Service > Service Caller

Service CallerのペインをMessage Publisherのペインに重ねて、タグにより切替えるようにします(図4-19、図4-20)。


図4-19 Service Callerの起動


図4-20 Service CallerとMessage Publisherのタブでの切換え

b. Turtleの軌跡の消去
第3章3-3「(2) コマンドによるTurtlesimの制御」a.で行ったコマンドによるTurtleの軌跡の消去に対応して、次の手順でサービスメッセージを送信することかできます。

(ア) Serviceの行で/clearを選択するとRequestペインに対応したサービスメッセージが表示されます。

(イ) 行末の「Call selected service」のCallのボタンを押すと、サービスメッセージが送信され、軌跡が消去されます。また、サービス通信の応答がResponseペインに表示されます(図4-21)。

(ウ) 同様に、/resetを選択して、行末のCallのボタンを押すとTurtleと軌跡が消去され、改めて絵柄の変わったTurtleが中央に現れます。

(エ) ここで、サービスの対象となるノードは、Service Callerを起動したときのものになります。ノードを追加したときは、Serviceの行の先頭の「Refresh service list」のボタンを押して更新する必要があります。


図4-21 軌跡の消去

c. Turtleの軌跡の設定
第3章3-3「(3) その他の通信のコマンド」b.のTurtleの軌跡の設定に対応し、次の手順でサービスメッセージを送信することができます。

(ア) Serviceの行で/turtle1/set_penを選択します。
Requestのペインに現れた項目を展開すると、色の設定のr,g,b、太さのwidth、表示と非表示のoffが表示されます(図4-22)。

(イ) 値をダブルクリックして、次のように変更します。

            r=255, g=0, b=0, width=5, off=0

行末の「Call selected service」のCallのボタンを押すと、サービスメッセージが送信されて設定されます。

(ウ) TurtleをRobot Steeringで移動させると、赤い太い軌跡が描かれ設定の変更が確認できます。


(a) 軌跡の色と太さの変更


(b) 変更の確認
図4-22 軌跡の設定

d. Turtleの絶対/相対座標での移動
(ア) Turtleの位置を指定してTurtleを移動させることができます。
座標はTurtlesimの画面の左下の(0,0)から右上の(11,11)の範囲で指定します。

(イ) 絶対座標で設定する場合は、Serviceで/turtle1/teleport_absoluteを選択します。
相対座標で設定する場合は、Serviceの行で/turtle1/teleport_relativeを選択します。
ここでは、前者の絶対座標の場合を選択し、Requestのペインに現れた項目を展開すると、座標x,yと角度thetaが表示されます。

(ウ) 値をダブルクリックして、次のように変更します。

            x=8.0,y=3.0

「Call」のアイコンをクリックすると、サービスメッセージが送信されてTurtleが移動します。
但し、軌跡の設定でoff=0のときは、軌跡を描いて移動します(図4-23(a))。

(エ) 軌跡を描かずに移動させるときは、b.の/turtle1/set_penの設定でoff=1として、移動させます。

            x=3.0,y=3.0

と設定して、軌跡を描かずに移動する場合の例を図4-23(b)に示します。
軌跡を表示に戻すときは、off=0とします。


(a) 軌跡の描画あり(off=0)


(b) 軌跡の描画なし(off=1)
図4-23 Turtleの移動(絶対座標)

e. 複数のTurtle
第3章3-3「(2) コマンドによるTurtlesimの制御」c.でのTurtleの追加に対応し、次の手順でTurtleを追加することができます(図4-24)。

(ア) Serviceの行で/spawnを選択します。
Requestのペインに現れた項目を展開すると、座標x,yと角度theta、nameが表示されます。

(イ) 値をダブルクリックして、次のように変更します。

            x=3.0,y=4.0,theta=0.0,name=''

「Call selected service」のCallのボタンを押すと、サービスメッセージが送信されてTurtleが現れます。’’ はシングルコーテーションを2個入力します。
このとき、応答としてResponseのペインにnameの値が'Turtle2'と表示されます。このTurtle2は名前空間で、メッセージを送信するときにTurtle1と区別できます。


図4-24 Turtleの追加

(ウ) 2つのTurtleは、既に起動されているRobot Steeringと、新たに追加したものとでそれぞれ移動の制御を行うことが出来ます(図4-25)。

            Plugins > Robot Tools > Robot Steering

但し、GUIの上段のトピック名の/cmd_velをそれぞれ/turtle1/cmd_velと/turtle2/cmd_velにする必要があります。


図4-25 複数のTurtleの制御(2つのTurtle)

(エ) Turtleの制御を停止し、Node Graphによりメッセージとノードのグラフ表示の確認をすると/turtle1と/turtle2で、速度指令のcmd_velが送信されていることが分かります(図4-26)。
また、/turtle2では、はじめTopic Monitorで/turtle2/poseにチェックしていないと、 /turtle1/poseがグラフに現れていませんが、Topic Monitorでチェックをして、改めてNode Graphの「Refresh ROS Graph」のボダンを押すと確認できます。

Topic Monitorは、追加したTurtleの項目が自動で追加されますが、Message PublisherやServer Caller、Node Graphは、リフレッシュのボダンを押して更新する必要があります。


図4-26 複数のTurtleを制御したときのNode Graph

(オ) 6つのTurtleを独立に移動させているときの様子を図4-27に示します。
Robot Steeringも追加して起動し、トピック名の/cmd_velを適切に変更します。


図4-27 複数のTurtleを制御(6つのTurtle)

f. Turtleの削除
第3章3-3「(2) コマンドによるTurtlesimの制御」c.でのTurtleの削除に対応し、次の手順でTurtleを削除することができます(図4-28)。

(ア) Serviceの行で/clearを選択し、行末の「Call」を押すと、サービスメッセージが送信され、全ての軌跡が消去されます。

(イ) Serviceの行で/killを選択し、Requestのペインに現れた項目を展開すると、nameが表示されます。
値をダブルクリックして、次のように変更します。

            name='turtle2'

「Call」を押すと、サービスメッセージが送信されて指定したTurtleが削除されます。


図4-28 Turtleの削除

5.Turtleのプログラム制御

この章では、Part-1 第6章でPythonのプログラムを作成しロボットの制御を行ったのと同様に、TurtlesimのTurtleを制御するPythonのプログラムを作成します。
これまで、Turtleを制御するプログラムとして、第3章3-3(1)d.【参考】で、Part-1で実際のロボットの動きを制御したプログラムrobot_mover.pyで、Turtleの移動もそのまま制御できることを示しました。

5-1.Pythonプログラムの設定

Part-1で設定したPythonのプログラム環境をそのまま利用できます。プログラムの作成、実行の手順(Part-1の第6章6-3)を改めて以下に示します。

a. パッケージdsl_ros_pkgの設定は終了しているとします。

b. Turtlesimを起動します。
(ア) 「ターミナル」を起動し、フォルダを/home/seed/ros/melodic/src/dsl_ros_pkg/scriptに変更して「byobu」コマンドを入力します。

(イ) Byobuの最初のウインドウ0で、「roscore」コマンドを入力します。

(ウ) F2を押して、Byobuにウインドウ1を追加し、「rosrun turtlesim turtlesim_node」とコマンドを入力します。

c. F2を押して、Byobuにウインドウ2を追加し、以下の手順でプログラムの入力と実行を行います。
(ア) フォルダを/home/seed/ros/melodic/src/dsl_ros_pkg/scriptに変更して、エディタのgeditプログラムを起動します。

            gedit プログラム名.py

(イ) エディタにプログラムを入力して、以下のフォルダに保存します(拡張子は.py)。

            /home/seed/ros/melodic/src/dsl_ros_pkg/script

(ウ) 保存したファイルに実行権を設定します。

            chmod a+x プログラム名.py

(エ) rosrunコマンドにより実行します。

            rosrun dsl_ros_pkg プログラム名.py

(オ) プログラムの終了はCTRL+Cを押します。

(カ) エラー等でプロクラムを修正するときは、geditを起動して、修正、保存を行い、rosrunコマンドによる実行を行います。

5-2.Turtleの移動

(1) Turtleが円状に移動するプログラム
Turtleの速度指令で、前進のlinear.x=2.0と回転のangle.z=1.8を設定することで、円軌道を描きます。プログラムでは、実行したときのノード名をturtle_move01_nodeとし、トピックメッセージのメッセージ名を/turtle1/cmd_velとして、起動時にリマップの設定は行いません(リスト5-1、図5-1、図5-2)。

プログラムの実行中に、F2を押して、Byobuにウインドウ3を追加し、「rqt_graph」のコマンドを入力するとノード名とトピックメッセージ名を確認できます(図5-3)。左上の設定は、「Nodes/Topics (all)」とします。

#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
from geometry_msgs.msg import Twist

if __name__ == "__main__":
    rospy.init_node('turtle_move01_node')
    kame = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=1)
    loop = rospy.Rate(1)
    vel = Twist()
    vel.linear.x =  2.0
    vel.linear.y =  0.0
    vel.linear.z =  0.0
    vel.angular.x = 0.0
    vel.angular.y = 0.0
    vel.angular.z = 1.8
    while not rospy.is_shutdown():
        kame.publish(vel)
        loop.sleep()


リスト5-1 turtle_move01.py


図5-1 プログラムの入力 (turtle_move01.py)


図5-2 プログラムの実行 (turtle_move01.py)


図5-3 ノードとトピックメッセージ (turtle_move01.py)

(2) 2つのTurtleの移動を制御
Turtleを追加して、2つのTurtleを制御します。実行したときのノード名をturtle_move02_nodeとし、トピックメッセージのメッセージ名を/turtle1/cmd_velと/turtle2/cmd_velとして、それぞれのTurtleにトピックメッセージを送信します(リスト5-2、図5-4、図5-5)。

実行の前に、次のコマンドを1回だけ入力し、Turtleを追加します。

            rosservice call /spawn  3.0 4.0 1.0 ""
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
from geometry_msgs.msg import Twist

if __name__ == "__main__":
    rospy.init_node('turtle_move02_node')
    kame01 = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=1)
    kame02 = rospy.Publisher('/turtle2/cmd_vel', Twist, queue_size=1)
    loop = rospy.Rate(1)
    vel01 = Twist()
    vel01.linear.x =  2.0
    vel01.linear.y =  0.0
    vel01.linear.z =  0.0
    vel01.angular.x = 0.0
    vel01.angular.y = 0.0
    vel01.angular.z = 1.8
    vel02 = Twist()
    vel02.linear.x =  2.0
    vel02.linear.y =  0.0
    vel02.linear.z =  0.0
    vel02.angular.x = 0.0
    vel02.angular.y = 0.0
    vel02.angular.z =-1.8
    while not rospy.is_shutdown():
        kame01.publish(vel01)
        kame02.publish(vel02)
        loop.sleep()


リスト5-2 turtle_move02.py

プログラムの実行中にrqt_graphの「Refresh ROS graph」のボタンを押して、画面を更新するとノード名とトピックメッセージ名を確認できます。


図5-4 2つのTutrleの制御 (turtle_move02.py)


図5-5 2つのTurtleのノードとトピックメッセージ (turtle_move02.py)

【参考】Turtleの八の字の移動
Turtleの軌跡が八の字を描くように制御します。2つの円の交点で回転速度angle.zを変更しますので、Turtleが交点の座標の位置に来たことを調べる必要があります。このTurtleの現在位置は /turtle1/poseのx,y成分により取得できます。リスト5-3に示すプログラムの概要を以下に示します(図5-6、図5-7)。

(ア) ノード名、トピックメッセージ名
ノード名の設定

            rospy.init_node('turtle_move03_node')

送信するトピックメッセージ名と型の設定

            pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)

受信するトピックメッセージ名と型の設定

            sub = rospy.Subscriber('/turtle1/pose', Pose, update_pose)

(イ) Turtleの位置情報
turtlesimがTurtleの位置情報の/turtle1/poseを送信するとプログラムのsubがこれを受信し、関数update_pose()が呼び出されます。このとき、位置情報は引数のPose型の変数dataに入っています。

(ウ) 2つの円の交点
プログラムの開始時のTurtleの位置を交点として変数pose_Cに設定し、Turtleがこの位置に来たときに、速度指令のcmd_vel.angular.zの値を変更します。
但し、Turtleの位置は交点と一致しませんので、交点を中心とする円状の範囲を考えます。さらに、この範囲に入った回数を変数num_inでカウントして交点に近づくようにしています。

(エ) 例外処理を考慮して、プログラム本体は関数turtle()にまとめています。

実行するときは、改めてrosの環境を立ち上げ直すか、Turtleが2つある場合は、次のコマンドを入力して、Turtle2の削除と軌跡の消去を行います。

            rosservice call /kill "turtle2"
            rosservice call /clear
            rosservice call /reset
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose
cmd_vel = Twist()
pose    = Pose()
pose_C  = Pose()
def update_pose(data):
    global pose
    pose.x = data.x
    pose.y = data.y
def turtle():
    global cmd_vel
    global pose_C
    rospy.init_node('turtle_move03_node')
    pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
    sub = rospy.Subscriber('/turtle1/pose', Pose, update_pose)
    rospy.sleep(1.0)
    pose_C.x = pose.x
    pose_C.y = pose.y
    cmd_vel.linear.x  = 2.0
    cmd_vel.angular.z = 1.8
    num_in = 0
    rate = rospy.Rate(1000)
    while not rospy.is_shutdown():
        if abs(pose_C.x - pose.x) < 0.02 and abs(pose_C.y - pose.y) < 0.02:
            if num_in == 4:
                cmd_vel.angular.z *= -1.0
            num_in += 1
        else:
            num_in = 0
        pub.publish(cmd_vel)
        rate.sleep()
if __name__ == '__main__':
    try:
        turtle()
    except rospy.ROSInterruptException:
        pass


リスト5-3 turtle_move03.py


図5-6 Tutrleの制御 (turtle_move03.py)


図5-7 ノードとトピックメッセージ (turtle_move03.py)

6章以降は「ROSによるロボット操作法  Part-2 シミュレーション(Robot)編」に続きます。
(別のページか開きます)

https://zenn.dev/dsl_gunma/articles/2add9d5f362af4
https://zenn.dev/dsl_gunma/articles/1cd52b90ed52fe

Discussion