前回は、「13．速度制御プログラムの作成」を行いましたが、駆動情報や制御情報の変更・速度情報の配信のテストなどはテストに必要なプログラムが未作成なので行えませんでした。今回は、ラズパイ・ゼロの最後のプログラムで「RosRemoCon.py」となります。
１．リモコン信号の流れの復習
２．ソースコードの説明と入力
３．プログラムの実行とテスト
１．リモコン信号の流れの復習： 目次に戻る
プログラムの説明に入る前に、以前の記事で説明したラジコン・カーに付属していたリモコンの信号の流れと改造後の信号の流れ、そしてソフトウェアの分散処理の流れを復習しておきます。プログラムの理解の参考にして下さい。
①ラジコン・カーに付属のリモコンを図に示しましたが、左タイヤの前後操作と右タイヤの前後操作が左右のレバースイッチで分かれています。
プレステ等のゲームリモコンと異なり、ジョイスティックによる操作角度による連続的な強弱指示が出来ず、オンかオフの指示のみ可能です。
右肩の「SLOWLY」スイッチを押すと「電源表示LED」が緑色に変わり低速モードとなります。左肩の「SPEEDY」スイッチを押すと「電源表示LED」が赤色に変わり高速モードに変わります。
②この図は「４．ラジコン・カーの改造（その３：ラズパイ・ゼロを搭載）」で使用したブロック図ですが、ラジコン・カーの内部にある制御基板には２個のＩＣが図のように接続されています。
先の図の付属リモコンで操作されたスイッチ情報は、左側の「リモコン受信制御ＩＣ」で処理され４本の信号となって右側の「モーター駆動ＩＣ」に入力されます。
③この図は、前回の「１．ＰＷＭ駆動による回転制御：」で使用した真理値表ですが、右側の「モーター駆動ＩＣ」のＭＸ１９１９Ｌは、入力Ａまたは入力ＢにＰＷＭ信号を入力することで駆動力を可変させることが出来ます。
④そこで、「４．ラジコン・カーの改造（その３：ラズパイ・ゼロを搭載）」で行った改造によって、付属リモコンからの４本の信号をラズパイ・ゼロのＧＰＩＯポートに取り込みプログラムで処理した後、ＧＰＩＯポートから「モーター駆動ＩＣ」に出力しています。
⑤この図は、「６．ラズパイ・ゼロとノートパソコンの分散処理」で使用したソフトウェア構成図ですが、今回の付属リモコンは左上の「RosRemoCon.py」で処理され、ＲＯＳのトピックでノートパソコン側の「RosBrdgRcGuiV2.py」でＲＯＳ標準の速度情報「/cmd_vel」トピックに変換されてラズパイ・ゼロ側の「RosSpConV2.py」でＰＩＤ制御されモーター駆動ＩＣに出力されます。
２．ソースコードの説明と入力： 目次に戻る
・１行目と２行目：パイソンのプログラムのお決まり事でしたね。
・４行目から４１行目：コメント行です。ご自身の理解した内容でしっかりと記載しておきましょう。
・４６行目：パイソン・プログラムからＲＯＳを使用する時に必須になります。
・４７行目：ＲＯＳのトピックを使用して付属リモコンのスイッチ情報を送信するために８ビットの１番短いメッセージの形式を使用しています。４つのスイッチの状態を２ビットずつにエンコードしてパッキングしています。
・４８行目：ラズベリーパイのＧＰＩＯをパイソン・プログラムから使用するときに必須となります。このプログラムでは、リモコン受信ＩＣからの入力ポートに使用しています。
・４９行目：スイッチが押された時刻や離された時刻を記録することで、５ｍｓ周期のＰＷＭ信号なのか通常のスイッチ操作なのかを区別するのに使用しています。
・５０行目：配列（リスト）の複製（コピー）に使用しています。リストのコピーについては、使用している１６７行目で説明します。
・５６行目から５９行目：リモコン受信ＩＣから出力されるスイッチの捜査情報を入力するＧＰＩＯポート番号をグローバル変数に設定しています。プログラムでは「IN_R_D」からの連続するポート番号を想定していますから連番である必要があります。
連番が出来ないケースでは「RotaryEncoder.py」の６９行目のようなコードにする必要があります。
・６４行目から６６行目：スイッチの状態を３つの数字で定義しています。スイッチのオンとオフの２値だけであれば１ビットで表現できますが、ＰＷＭの状態を追加したので３値になり２ビット必要になります。
・１８９行目から１９０行目：今回も、いきなり最終行に来てしまいましたが、このファイルから実行されたときだけ「main」関数を実行します。パイソン・プログラムの常とう手法ですね。
・１０９行目：最初に実行される「main」関数で１８３行目までのメインブロックを構成します。
・１１０行目：使用するＧＰＩＯポート番号をＢＣＭ番号で指定するように設定しています。
・１１４行目から１１７行目：４つのスイッチを入力するＧＰＩＯポートを入力に設定しています。
・１２２行目から１２５行目：４つのスイッチが押された時と離された時の両方のタイミングで割り込み処理関数の「switch_on」を呼ぶように設定しています。「RotaryEncoder.py」の１６７行目から１７０行目と同じ設定になります。
・１２９行目：現在の時刻を保持するローカル変数の「ctm」を作成しています。
・１３０行目：前回ＲＯＳのトピックで配信したスイッチの変化情報を、ローカル変数の「msw」配列に作成しています。
・１３１行目：割り込み処理関数の「switch_on」で変化したスイッチを検出するために前回処理したスイッチの状態を、グローバル変数の「psw」配列に作成しています。変数名の前に関数名を付加するとグローバル変数として作成されます。
・１３２行目：割り込み処理関数の「switch_on」で変化したスイッチの前回処理した時刻を、グローバル変数の「ptm」配列に作成しています。
・１３７行目：「rospy」ライブラリの「init_node」関数を使用して「/rc_remocon」と言う名称のノードを作成しています。
・１３８行目：「rospy」ライブラリの「loginfo」関数を使用してＲＯＳノードの実行開始をログ端末に表示しておきましょう。
・１４２行目：「rospy」ライブラリの「Publisher」クラスを使用してリモコンのスイッチが変化した時に配信するためのメッセージのオブジェクト「pub」を作成します。トピック名は「/remocon_info」でトピックのメッセージ型にはＲＯＳ標準の「UInt8」を使用し、バッファのサイズを１０個使用します。
・１４３行目：「rospy」ライブラリの「Rate」クラスを使用して１００ヘルツ（Hz）のタイマーオブジェクト「rate」を作成しています。
・１４４行目：ＲＯＳ標準のメッセージ型「UInt8」を使用し、メッセージオブジェクト「u8」を作成しています。
・１４５行目：スイッチの変化をトピックに配信するタイミングを１０ｍｓ遅らせるためのフラグをローカル変数の「s1」に作成しています。スイッチ情報がＰＷＭの場合は、５ｍｓ周期で「ハイ」と「ロー」を繰り返すため１０ｍｓ遅延させることによって、スイッチオンとＰＷＭを分離するのに使用しています。
・１４９行目：１７７行目の「except」文と対になって、１５０行目から１７３行目のブロック内で発生した例外事項をキャッチさせます。例外事項とは、キーボードからの「Ctrl＋C」入力を検出して終了処理を実行させます。
・１５０行目：作成したＲＯＳのノードが実行中は、以降の「while」ブロックを実行します。
・１５１行目：現在時刻を再取得して前回スイッチ情報が変化してからの時間を計算するのに使用します。
・１５５行目：現在のスイッチがＰＷＭ状態のスイッチの検索し、そのスイッチのインデックスをリストしてＰＷＭ信号の終了を検出しています。ちょっと複雑な「for」文なので説明しておきます。
「for」文の基本は、次のような構文になります。
結果、１５５行目は、「switch_on.psw」にある４つのスイッチ要素を変数「x」にその要素のインデックスを変数「i」にセットで取り出して、要素の「x」がＰＷＭ状態を示す「SW_PWM」と一致したインデックス「i」をリストに設定しています。作成したリストから要素を１つずつ変数「idx」に取り出し「for」文を実行しています。
・１５６行目：１５５行目で見つけたＰＷＭ状態のスイッチが前回の変化から１０ｍｓ（０．０１秒）以上経過していないかチェックしています。
・１５７行目：ＰＷＭ信号なのに１０ｍｓ以上変化していないという事は、スイッチを押すのを止めてオフ状態になったか、「SLOWLY」から「SPEEDY」に変化してオン状態になったと判断して新しく現在の状態をポートから取り込みます。入力ポートのＧＰＩＯ番号を「idx + IN_R_D」としているため、「IN_R_D」ポートから４つの入力ポートは連番である必要があります。
・１５８行目：コメントにしていますが、ＰＷＭのスイッチ状態を解除した時刻とスイッチ状態をデバッグ時に表示しています。「str」関数は、引数を文字列に変換して文字列を連結できるようにしています。
・１６２行目：前回、ＲＯＳのトピックで配信したスイッチ情報から現在のスイッチ情報が変化しているか確認します。配列の変数名で比較すると配列の要素全てが等しいかどうかチェックします。
・１６３行目：スイッチ情報が変化しているようであれば、変数「s1」をチェックして最初の変化なのか変化から２回目なのか調べます。
・１６４行目：スイッチ情報の変化から１回目の時は、変数「s1」を「False」に設定して２回目を待ちます。
・１６５行目：スイッチ情報の変化から２回目の時は、次のブロックを実行します。
・１６６行目：スイッチ情報の変化から２回目の時は、変数「s1」を「True」に戻して置き次は１回目にします。
・１６７行目：現在のスイッチ状態「switch_on.psw」をローカル変数の「msw」に保存して、次回のスイッチ状態と比べて変化を確認できるようにします。
ここで、配列をコピーするのに「msw = switch_on.psw」とせずに、５０行目でインポートしている「copy」ライブラリの「deepcopy」関数を使用して実行しています。
前者の場合は「配列を共有する」事になるため「switch_on.psw」の要素を変更すると「msw」の同じ要素も変更され、逆に「msw」の要素を変更すると「switch_on.psw」の同じ要素も変更されてしまいます。一方、後者の場合は別の配列として全ての要素をコピーしますから一方の要素を変更しても、もう一方の要素は影響されません。
・１６８行目：現在のスイッチ状態を８ビットの変数「u8」にパッキングしてＲＯＳのメッセージにエンコードしています。図のように、２ビットずつのスイッチ情報はｘ４倍していくことで上位のビットにパックすることが出来ます。
・１６９行目：ＲＯＳのメッセージ型「u8」にパッキングした最新のスイッチ状態を、１４２行目で作成した「pub」オブジェクトの「publish」メソッドを使用して配信しています。
・１７０行目：コメントにしていますが、配信したスイッチ状態をデバッグ時にログしています。「str」関数は、引数を文字列に変換して文字列を連結できるようにしています。
・１７２行目：１４３行目で作成した「rate」オブジェクトの「sleep」メソッドを使用してオブジェクト作成時に指定した１０ｍｓの残り時間をこの関数で消費します。例えば、ループのコード実行に３ｍｓかかっていたとすると残りの７ｍｓを「sleep」関数が消費します。
・１７３行目：コメントにしていますが、ループの動作時間をログしてみると５０ｍｓ程度ループの実行にかかっているため、１０ｍｓの「sleep」関数は機能していませんでした。
・１７７行目：１４９行目の「try」文とセットになって、「try」ブロック内で発生した「Ctrl＋C」をキャッチして以降のブロックを実行します。
・１７８行目：１７７行目の「except」ブロックを作成するために何も実行しない「pass」文を挿入しています。
・１７９行目：「Ctrl＋C」の入力によってＲＯＳのノードが終了したことをログに表示しています。
・１８３行目：プログラムの終了の前に使用したＧＰＩＯポートを元に戻しておきます。
・７９行目：スイッチの状態が変化した時に実行されるように１２２行目から１２５行目で設定した割り込み処理関数です。引数に変化したＧＰＩＯポート番号が保持されています。
・８０行目：変化した対象のスイッチ状態をローカル変数の「csw」に取り込みます。
・８１行目：変化した時刻をローカル変数の「ctm」に取り込みます。
・８２行目：対象のスイッチをＧＰＩＯポート番号から配列のインデックスに変換し、ローカル変数の「n」に設定します。１５７行目でも記載しましたが、「IN_R_D」ポートから４つの入力ポートは連番である必要があります。
・８３行目：コメントにしていますが、出力ポートに設定した「OT_DEB」ポートに割り込みの度に反転された信号をデバッグ出力します。
・８４行目：グローバル変数「switch_on.psw」に保存されている前回のスイッチ状態と今回のスイッチ状態を比較し同じ状態だったら次のブロックを処理します。
・８５行目：グローバル変数「switch_on.ptm」に現在の時刻だけ変更しておきます。
・８６行目：コメントにしていますが、処理内容をデバッグ表示しています。
・８７行目：スイッチ状態に変化が無かった時は、時刻情報だけ更新してこの「return」文で関数から抜けて元の処理に戻ります。
・８８行目：スイッチ状態が変化した時は、先の変化から１０ｍｓ経っていなければＰＷＭ信号と判断して、次のブロックを実行します。
・８９行目：グローバル変数「switch_on.ptm」を現在の時刻に変更しておきます。
・９０行目：グローバル変数「switch_on.psw」を「SW_PWM」状態変数に変更しておきます。
・９１行目：コメントにしていますが、処理内容をデバッグ表示しています。
・９２行目：関数から抜けて元の処理に戻ります。
・９３行目：スイッチ状態が先の変化から１０ｍｓ以上経っていれば、通常のスイッチ処理として８０行目で取り込んだスイッチの状態を保存します。
・９４行目：グローバル変数「switch_on.ptm」を現在の時刻に変更しておきます。
・９５行目：コメントにしていますが、処理内容をデバッグ表示しています。
コードの説明は、以上です。次に、簡単なテストをしてみましょう。
３．プログラムの実行とテスト： 目次に戻る
それでは、作成した「RosRemoCon-new.py」の実行とテストを以前の「RotaryEncoder-new.py」で行った「３．プログラムの実行とテスト：」の手順と同じように行っていきます。
①「Windowsターミナル」から「roscore」の実行や４分割画面を作成して置いてください。
②４分割画面の左上の「端末」からラズパイ・ゼロにログインして置いてください。
③ラズパイ・ゼロの「端末」から「roscd rc_car/scripts/」と入力してスクリプト用ディレクトリに移動して下さい。
④４分割画面の右上の「端末」から「cd ~/Python/rc_car/raspi/」と入力して作成したプログラムのディレクトリに移動して下さい。
⑤Ubuntu18で作成したプログラムをラズパイ・ゼロにコピーします。右上の「端末」から「sput RosRemoCon-new.py」と入力して下さい。
⑥コピーしたファイルに実行権限を追加します。左上の「端末」で「chmod +x RosRemoCon-new.py」を実行して下さい。
⑦左下の「端末」からもラズパイ・ゼロにログインして「rosrun rc_car RosRemoCon-new.py」と入力して実行して下さい。エラー無く実行出来たでしょうか？
⑧左下の「端末」からは、１３８行目のログが「ROSノード[rc_remocon]を開始しました。」表示されたでしょうか？「rc_remocon」ノードからの表示は、以上です。
⑨ラジコン・カーに付属のリモコンの電源を入れて下さい。赤いＬＥＤが点灯したら準備ＯＫです。
⑩右下の「端末」から「rostopic echo /remocon_info」と入力して「Enter」を入力して下さい。
⑪リモコンのスイッチを操作して、右下の「端末」に表示されるメッセージを確認してみて下さい。
図は、右のスイッチを上側に倒して「data: 4」が表示され、離して「data: 0」が表示されています。続けて、右のスイッチを下側に倒して「data: 1」が表示され、離して「data: 0」が表示されています。同じように、左スイッチの上で「data: 64」が、下で「data: 16」が表示されています。
⑫リモコンの右肩のスイッチを押して、電源ＬＥＤが赤色から緑色に変わればＯＫです。右下の「端末」に表示されるメッセージを確認してみて下さい。
図のように、右スイッチの上で「data: 8」が表示され、右スイッチの下で「data: 2」が表示され、左スイッチの上で「data: 128」が、下で「data: 32」が表示されたでしょうか。
「SPEEDY」モードは「1」に「SLOWLY」モードは「2」にエンコードしているので、表示が各々のスイッチで２倍になっていますね。
以上で動作確認とテストを終了します。右下の「端末」と左下の「端末」に「Ctrl＋C」を入力してプログラムを終了させてください。
今回でラズパイ・ゼロ側の４つのプログラムは終了です。次回からはノートパソコン側のプログラムについて紹介していきます。
目次に戻る
・・・
１．リモコン信号の流れの復習
２．ソースコードの説明と入力
３．プログラムの実行とテスト
１．リモコン信号の流れの復習： 目次に戻る
プログラムの説明に入る前に、以前の記事で説明したラジコン・カーに付属していたリモコンの信号の流れと改造後の信号の流れ、そしてソフトウェアの分散処理の流れを復習しておきます。プログラムの理解の参考にして下さい。
①ラジコン・カーに付属のリモコンを図に示しましたが、左タイヤの前後操作と右タイヤの前後操作が左右のレバースイッチで分かれています。プレステ等のゲームリモコンと異なり、ジョイスティックによる操作角度による連続的な強弱指示が出来ず、オンかオフの指示のみ可能です。
右肩の「SLOWLY」スイッチを押すと「電源表示LED」が緑色に変わり低速モードとなります。左肩の「SPEEDY」スイッチを押すと「電源表示LED」が赤色に変わり高速モードに変わります。
②この図は「４．ラジコン・カーの改造（その３：ラズパイ・ゼロを搭載）」で使用したブロック図ですが、ラジコン・カーの内部にある制御基板には２個のＩＣが図のように接続されています。先の図の付属リモコンで操作されたスイッチ情報は、左側の「リモコン受信制御ＩＣ」で処理され４本の信号となって右側の「モーター駆動ＩＣ」に入力されます。
③この図は、前回の「１．ＰＷＭ駆動による回転制御：」で使用した真理値表ですが、右側の「モーター駆動ＩＣ」のＭＸ１９１９Ｌは、入力Ａまたは入力ＢにＰＷＭ信号を入力することで駆動力を可変させることが出来ます。
④そこで、「４．ラジコン・カーの改造（その３：ラズパイ・ゼロを搭載）」で行った改造によって、付属リモコンからの４本の信号をラズパイ・ゼロのＧＰＩＯポートに取り込みプログラムで処理した後、ＧＰＩＯポートから「モーター駆動ＩＣ」に出力しています。
⑤この図は、「６．ラズパイ・ゼロとノートパソコンの分散処理」で使用したソフトウェア構成図ですが、今回の付属リモコンは左上の「RosRemoCon.py」で処理され、ＲＯＳのトピックでノートパソコン側の「RosBrdgRcGuiV2.py」でＲＯＳ標準の速度情報「/cmd_vel」トピックに変換されてラズパイ・ゼロ側の「RosSpConV2.py」でＰＩＤ制御されモーター駆動ＩＣに出力されます。２．ソースコードの説明と入力： 目次に戻る
もうお馴染みと思いますが、前回の「RosSpConV2-new.py」の作成と同じ手順で進めていきます。新規ファイルを作成して、空白の編集エリアにキーボードからの入力やコピー＆ペーストでパイソン・プログラムを完成させてから、実行して動作確認を行っていきます。
①最初に、以前の「７．Geanyの操作と新規パイソン・プログラムの作成：」と同じように、「Geany」を開いて「RosRemoCon.py」をエクスプローラーからドラッグ＆ドロップして下さい。前回使用した「RosSpConV2.py」や「RosSpConV2-new.py」が開いていたら紛らわしいので閉じておきましょう。
②「新しいファイルを作成」アイコンを使用して新しいファイルを「RosRemoCon-new.py」で保存しておいてください。保存するときに、保存ディレクトリが「RosRemoCon.py」と同じディレクトリになるように注意して下さい。
③ここからは「RosRemoCon.py」のプログラム解説を行番号を指定して行っていきます。「RosRemoCon.py」のタブをクリックして内容を確認しながら、「RosRemoCon-new.py」のタブをクリックしてコピー＆ペーストを活用しながら書き込んで行って下さい。
・１行目と２行目：パイソンのプログラムのお決まり事でしたね。
・４行目から４１行目：コメント行です。ご自身の理解した内容でしっかりと記載しておきましょう。
・４６行目：パイソン・プログラムからＲＯＳを使用する時に必須になります。
・４７行目：ＲＯＳのトピックを使用して付属リモコンのスイッチ情報を送信するために８ビットの１番短いメッセージの形式を使用しています。４つのスイッチの状態を２ビットずつにエンコードしてパッキングしています。
・４８行目：ラズベリーパイのＧＰＩＯをパイソン・プログラムから使用するときに必須となります。このプログラムでは、リモコン受信ＩＣからの入力ポートに使用しています。
・４９行目：スイッチが押された時刻や離された時刻を記録することで、５ｍｓ周期のＰＷＭ信号なのか通常のスイッチ操作なのかを区別するのに使用しています。
・５０行目：配列（リスト）の複製（コピー）に使用しています。リストのコピーについては、使用している１６７行目で説明します。
・５６行目から５９行目：リモコン受信ＩＣから出力されるスイッチの捜査情報を入力するＧＰＩＯポート番号をグローバル変数に設定しています。プログラムでは「IN_R_D」からの連続するポート番号を想定していますから連番である必要があります。
連番が出来ないケースでは「RotaryEncoder.py」の６９行目のようなコードにする必要があります。
・６４行目から６６行目：スイッチの状態を３つの数字で定義しています。スイッチのオンとオフの２値だけであれば１ビットで表現できますが、ＰＷＭの状態を追加したので３値になり２ビット必要になります。
・１８９行目から１９０行目：今回も、いきなり最終行に来てしまいましたが、このファイルから実行されたときだけ「main」関数を実行します。パイソン・プログラムの常とう手法ですね。
・１０９行目：最初に実行される「main」関数で１８３行目までのメインブロックを構成します。
・１１０行目：使用するＧＰＩＯポート番号をＢＣＭ番号で指定するように設定しています。
・１１４行目から１１７行目：４つのスイッチを入力するＧＰＩＯポートを入力に設定しています。
・１２２行目から１２５行目：４つのスイッチが押された時と離された時の両方のタイミングで割り込み処理関数の「switch_on」を呼ぶように設定しています。「RotaryEncoder.py」の１６７行目から１７０行目と同じ設定になります。
・１２９行目：現在の時刻を保持するローカル変数の「ctm」を作成しています。
・１３０行目：前回ＲＯＳのトピックで配信したスイッチの変化情報を、ローカル変数の「msw」配列に作成しています。
・１３１行目：割り込み処理関数の「switch_on」で変化したスイッチを検出するために前回処理したスイッチの状態を、グローバル変数の「psw」配列に作成しています。変数名の前に関数名を付加するとグローバル変数として作成されます。
・１３２行目：割り込み処理関数の「switch_on」で変化したスイッチの前回処理した時刻を、グローバル変数の「ptm」配列に作成しています。
・１３７行目：「rospy」ライブラリの「init_node」関数を使用して「/rc_remocon」と言う名称のノードを作成しています。
・１３８行目：「rospy」ライブラリの「loginfo」関数を使用してＲＯＳノードの実行開始をログ端末に表示しておきましょう。
・１４２行目：「rospy」ライブラリの「Publisher」クラスを使用してリモコンのスイッチが変化した時に配信するためのメッセージのオブジェクト「pub」を作成します。トピック名は「/remocon_info」でトピックのメッセージ型にはＲＯＳ標準の「UInt8」を使用し、バッファのサイズを１０個使用します。
・１４３行目：「rospy」ライブラリの「Rate」クラスを使用して１００ヘルツ（Hz）のタイマーオブジェクト「rate」を作成しています。
・１４４行目：ＲＯＳ標準のメッセージ型「UInt8」を使用し、メッセージオブジェクト「u8」を作成しています。
・１４５行目：スイッチの変化をトピックに配信するタイミングを１０ｍｓ遅らせるためのフラグをローカル変数の「s1」に作成しています。スイッチ情報がＰＷＭの場合は、５ｍｓ周期で「ハイ」と「ロー」を繰り返すため１０ｍｓ遅延させることによって、スイッチオンとＰＷＭを分離するのに使用しています。
・１４９行目：１７７行目の「except」文と対になって、１５０行目から１７３行目のブロック内で発生した例外事項をキャッチさせます。例外事項とは、キーボードからの「Ctrl＋C」入力を検出して終了処理を実行させます。
・１５０行目：作成したＲＯＳのノードが実行中は、以降の「while」ブロックを実行します。
・１５１行目：現在時刻を再取得して前回スイッチ情報が変化してからの時間を計算するのに使用します。
・１５５行目：現在のスイッチがＰＷＭ状態のスイッチの検索し、そのスイッチのインデックスをリストしてＰＷＭ信号の終了を検出しています。ちょっと複雑な「for」文なので説明しておきます。
「for」文の基本は、次のような構文になります。
- for ＜変数＞ in ＜配列＞ :
- [ ＜変数２＞ for ＜変数２＞ in ＜配列２＞ if ＜条件＞ ]
- [ ＜変数３＞ for ＜変数３＞, ＜変数４＞ in enumerate(＜配列３＞) if ＜条件２＞ ]
結果、１５５行目は、「switch_on.psw」にある４つのスイッチ要素を変数「x」にその要素のインデックスを変数「i」にセットで取り出して、要素の「x」がＰＷＭ状態を示す「SW_PWM」と一致したインデックス「i」をリストに設定しています。作成したリストから要素を１つずつ変数「idx」に取り出し「for」文を実行しています。
・１５６行目：１５５行目で見つけたＰＷＭ状態のスイッチが前回の変化から１０ｍｓ（０．０１秒）以上経過していないかチェックしています。
・１５７行目：ＰＷＭ信号なのに１０ｍｓ以上変化していないという事は、スイッチを押すのを止めてオフ状態になったか、「SLOWLY」から「SPEEDY」に変化してオン状態になったと判断して新しく現在の状態をポートから取り込みます。入力ポートのＧＰＩＯ番号を「idx + IN_R_D」としているため、「IN_R_D」ポートから４つの入力ポートは連番である必要があります。
・１５８行目：コメントにしていますが、ＰＷＭのスイッチ状態を解除した時刻とスイッチ状態をデバッグ時に表示しています。「str」関数は、引数を文字列に変換して文字列を連結できるようにしています。
・１６２行目：前回、ＲＯＳのトピックで配信したスイッチ情報から現在のスイッチ情報が変化しているか確認します。配列の変数名で比較すると配列の要素全てが等しいかどうかチェックします。
・１６３行目：スイッチ情報が変化しているようであれば、変数「s1」をチェックして最初の変化なのか変化から２回目なのか調べます。
・１６４行目：スイッチ情報の変化から１回目の時は、変数「s1」を「False」に設定して２回目を待ちます。
・１６５行目：スイッチ情報の変化から２回目の時は、次のブロックを実行します。
・１６６行目：スイッチ情報の変化から２回目の時は、変数「s1」を「True」に戻して置き次は１回目にします。
・１６７行目：現在のスイッチ状態「switch_on.psw」をローカル変数の「msw」に保存して、次回のスイッチ状態と比べて変化を確認できるようにします。
ここで、配列をコピーするのに「msw = switch_on.psw」とせずに、５０行目でインポートしている「copy」ライブラリの「deepcopy」関数を使用して実行しています。
前者の場合は「配列を共有する」事になるため「switch_on.psw」の要素を変更すると「msw」の同じ要素も変更され、逆に「msw」の要素を変更すると「switch_on.psw」の同じ要素も変更されてしまいます。一方、後者の場合は別の配列として全ての要素をコピーしますから一方の要素を変更しても、もう一方の要素は影響されません。
・１６８行目：現在のスイッチ状態を８ビットの変数「u8」にパッキングしてＲＯＳのメッセージにエンコードしています。図のように、２ビットずつのスイッチ情報はｘ４倍していくことで上位のビットにパックすることが出来ます。・１６９行目：ＲＯＳのメッセージ型「u8」にパッキングした最新のスイッチ状態を、１４２行目で作成した「pub」オブジェクトの「publish」メソッドを使用して配信しています。
・１７０行目：コメントにしていますが、配信したスイッチ状態をデバッグ時にログしています。「str」関数は、引数を文字列に変換して文字列を連結できるようにしています。
・１７２行目：１４３行目で作成した「rate」オブジェクトの「sleep」メソッドを使用してオブジェクト作成時に指定した１０ｍｓの残り時間をこの関数で消費します。例えば、ループのコード実行に３ｍｓかかっていたとすると残りの７ｍｓを「sleep」関数が消費します。
・１７３行目：コメントにしていますが、ループの動作時間をログしてみると５０ｍｓ程度ループの実行にかかっているため、１０ｍｓの「sleep」関数は機能していませんでした。
・１７７行目：１４９行目の「try」文とセットになって、「try」ブロック内で発生した「Ctrl＋C」をキャッチして以降のブロックを実行します。
・１７８行目：１７７行目の「except」ブロックを作成するために何も実行しない「pass」文を挿入しています。
・１７９行目：「Ctrl＋C」の入力によってＲＯＳのノードが終了したことをログに表示しています。
・１８３行目：プログラムの終了の前に使用したＧＰＩＯポートを元に戻しておきます。
・７９行目：スイッチの状態が変化した時に実行されるように１２２行目から１２５行目で設定した割り込み処理関数です。引数に変化したＧＰＩＯポート番号が保持されています。
・８０行目：変化した対象のスイッチ状態をローカル変数の「csw」に取り込みます。
・８１行目：変化した時刻をローカル変数の「ctm」に取り込みます。
・８２行目：対象のスイッチをＧＰＩＯポート番号から配列のインデックスに変換し、ローカル変数の「n」に設定します。１５７行目でも記載しましたが、「IN_R_D」ポートから４つの入力ポートは連番である必要があります。
・８３行目：コメントにしていますが、出力ポートに設定した「OT_DEB」ポートに割り込みの度に反転された信号をデバッグ出力します。
・８４行目：グローバル変数「switch_on.psw」に保存されている前回のスイッチ状態と今回のスイッチ状態を比較し同じ状態だったら次のブロックを処理します。
・８５行目：グローバル変数「switch_on.ptm」に現在の時刻だけ変更しておきます。
・８６行目：コメントにしていますが、処理内容をデバッグ表示しています。
・８７行目：スイッチ状態に変化が無かった時は、時刻情報だけ更新してこの「return」文で関数から抜けて元の処理に戻ります。
・８８行目：スイッチ状態が変化した時は、先の変化から１０ｍｓ経っていなければＰＷＭ信号と判断して、次のブロックを実行します。
・８９行目：グローバル変数「switch_on.ptm」を現在の時刻に変更しておきます。
・９０行目：グローバル変数「switch_on.psw」を「SW_PWM」状態変数に変更しておきます。
・９１行目：コメントにしていますが、処理内容をデバッグ表示しています。
・９２行目：関数から抜けて元の処理に戻ります。
・９３行目：スイッチ状態が先の変化から１０ｍｓ以上経っていれば、通常のスイッチ処理として８０行目で取り込んだスイッチの状態を保存します。
・９４行目：グローバル変数「switch_on.ptm」を現在の時刻に変更しておきます。
・９５行目：コメントにしていますが、処理内容をデバッグ表示しています。
コードの説明は、以上です。次に、簡単なテストをしてみましょう。
３．プログラムの実行とテスト： 目次に戻る
それでは、作成した「RosRemoCon-new.py」の実行とテストを以前の「RotaryEncoder-new.py」で行った「３．プログラムの実行とテスト：」の手順と同じように行っていきます。
①「Windowsターミナル」から「roscore」の実行や４分割画面を作成して置いてください。
②４分割画面の左上の「端末」からラズパイ・ゼロにログインして置いてください。
③ラズパイ・ゼロの「端末」から「roscd rc_car/scripts/」と入力してスクリプト用ディレクトリに移動して下さい。
④４分割画面の右上の「端末」から「cd ~/Python/rc_car/raspi/」と入力して作成したプログラムのディレクトリに移動して下さい。
⑤Ubuntu18で作成したプログラムをラズパイ・ゼロにコピーします。右上の「端末」から「sput RosRemoCon-new.py」と入力して下さい。
⑥コピーしたファイルに実行権限を追加します。左上の「端末」で「chmod +x RosRemoCon-new.py」を実行して下さい。
⑦左下の「端末」からもラズパイ・ゼロにログインして「rosrun rc_car RosRemoCon-new.py」と入力して実行して下さい。エラー無く実行出来たでしょうか？
⑧左下の「端末」からは、１３８行目のログが「ROSノード[rc_remocon]を開始しました。」表示されたでしょうか？「rc_remocon」ノードからの表示は、以上です。
⑨ラジコン・カーに付属のリモコンの電源を入れて下さい。赤いＬＥＤが点灯したら準備ＯＫです。
⑩右下の「端末」から「rostopic echo /remocon_info」と入力して「Enter」を入力して下さい。
⑪リモコンのスイッチを操作して、右下の「端末」に表示されるメッセージを確認してみて下さい。図は、右のスイッチを上側に倒して「data: 4」が表示され、離して「data: 0」が表示されています。続けて、右のスイッチを下側に倒して「data: 1」が表示され、離して「data: 0」が表示されています。同じように、左スイッチの上で「data: 64」が、下で「data: 16」が表示されています。
⑫リモコンの右肩のスイッチを押して、電源ＬＥＤが赤色から緑色に変わればＯＫです。右下の「端末」に表示されるメッセージを確認してみて下さい。図のように、右スイッチの上で「data: 8」が表示され、右スイッチの下で「data: 2」が表示され、左スイッチの上で「data: 128」が、下で「data: 32」が表示されたでしょうか。
「SPEEDY」モードは「1」に「SLOWLY」モードは「2」にエンコードしているので、表示が各々のスイッチで２倍になっていますね。
以上で動作確認とテストを終了します。右下の「端末」と左下の「端末」に「Ctrl＋C」を入力してプログラムを終了させてください。
今回でラズパイ・ゼロ側の４つのプログラムは終了です。次回からはノートパソコン側のプログラムについて紹介していきます。
目次に戻る
・・・
コメント