こんにちは。突然ですがros_controlってすごくややこしいですよね・・・仕方がないのはわかるのですが、僕のような人間からするともう少し楽に制御系のノードを作りたい・・・
そういう方にとって使いやすいツールの一つにMATLAB/Simulinkがあります。
今回はMATLAB/SimulinkからC++へのコンバート機能を利用してC++のROSノードを作成する手法について書き残したいと思います。
詳細な手順はこちらを参考にしてください。
この手順実行には以下のtoolboxが必要です。
Robotics System Toolbox, Simulink Coder,Simulink

matlabとROSを接続

roscoreを実行し、その後matlabのコマンドラインにrosinitと打ち込んでROSとmatlabを接続します。

Simulinkモデルの作成

今回は非常にシンプルなノードを作成してみたいと思います。
/inputという名前のstd_msgs/Float64型メッセージを受け取り、メッセージを受け取った時それと同じ値を/outputというトピックで出力します。

Enabled Subsystemの中身は以下のようになっています。

Subscriber,Publisherブロックの受信、送信トピック、その型などはブロックをダブルクリックすると編集できます。

Simulinkモデルの動作確認

以下のようなlaunchフアイルを作ってlaunchしてください、100Hzで/inputというトピックにstd_msgs/Float64型で1.0という値が送信され続けます。

<?xml version="1.0" encoding="UTF-8"?>
<launch>
  <node name="input_node" pkg="rostopic" type="rostopic" args="pub -r 100 /input std_msgs/Float64 -- 1" output="screen" respawn="true">
  </node>
</launch>

モデルを保存して、Simulinkの実行ボタンを押してみましょう。この時シミュレーション終了時間をInf（無限）にするのを忘れないでください！さもないと実行が一瞬で終わってしまいます！

C++へのコンバート

Simulinkモデルができて動作確認が取れたところでC++のROSノードへコンバートしてみましょう
Simulinkのバーにある歯車のマークをクリックしてコンフィギュレーションパラメータを設定しましょう

ここでソルバーオプションを固定ステップにしてください
その後固定ステップサイズを自分の作りたい制御系に合わせて設定してください

次に、ハードウェア実行のタブを開きます

ハードウェアボードにROSを選択します。
設定が終わったらソースコードに展開します。
ctrl+Bのショートカットを使うかコード->C/C++コード->ハードウェアに展開を選びます。
しばらくするとこんな画面が出てきてソースコードが生成されます。

生成したディレクトリを確認するとなんかいろいろ増えてると思います。

./build_ros_model.sh.shの指示どおりにビルドしていきましょう。

このコマンドを実行すると、なんかいろいろ警告はでるのですが、ビルドに成功しました！！

それでは早速テストしてみましょう。

生成したROSノードの実行テスト

以下のようなlaunchファイルを作成してroslaunchしてください。

<?xml version="1.0" encoding="UTF-8"?>
<launch>
    <node name="input_node" pkg="rostopic" type="rostopic" args="pub -r 100 /input std_msgs/Float64 -- 1" output="screen" respawn="true">
  </node>

  <node name="sample_node" pkg="sample" type="sample_node" args="" output="screen" respawn="true">
  </node>
</launch>

rqt_graphで確認したところノード同士は通信できていますね！！

受け取ってるデータにも異常はなく、topicのパブリッシュレートも正確です。

まとめ

以上のような感じでSimulinkモデルからROSのノードを生成することが可能です。
手順はややこしいですが、グラフィカルに制御系が組めることは大きな利点であると思います。
あと、確認はしていませんがWindows上でもおそらくコード生成まではできると思います。