MATLAB/SimulinkでC++のROSノードを作る

こんにちは。突然ですがros_controlってすごくややこしいですよね・・・仕方がないのはわかるのですが、僕のような人間からするともう少し楽に制御系のノードを作りたい・・・
そういう方にとって使いやすいツールの一つにMATLAB/Simulinkがあります。
今回はMATLAB/SimulinkからC++へのコンバート機能を利用してC++のROSノードを作成する手法について書き残したいと思います。
詳細な手順は[こちら](https://jp.mathworks.com/help/robotics/examples/generate-a-standalone-ros-node-from-simulink.html)を参考にしてください。
この手順実行には以下のtoolboxが必要です。
Robotics System Toolbox, Simulink Coder,Simulink
# matlabとROSを接続
roscoreを実行し、その後matlabのコマンドラインにrosinitと打ち込んでROSとmatlabを接続します。
![Screenshot from 2017-12-31 20-44-33.png](https://qiita-image-store.s3.amazonaws.com/0/160346/d7081a65-e55a-c10d-f710-4e41e7d13048.png)

# Simulinkモデルの作成
今回は非常にシンプルなノードを作成してみたいと思います。
/inputという名前のstd_msgs/Float64型メッセージを受け取り、メッセージを受け取った時それと同じ値を/outputというトピックで出力します。
![Screenshot from 2017-12-31 20-51-17.png](https://qiita-image-store.s3.amazonaws.com/0/160346/65888af6-1bc7-dd4e-b087-fc4c62c355e3.png)
Enabled Subsystemの中身は以下のようになっています。
![Screenshot from 2017-12-31 20-51-22.png](https://qiita-image-store.s3.amazonaws.com/0/160346/abe18d70-80bd-465b-3d9a-fbb0a9545519.png)
Subscriber,Publisherブロックの受信、送信トピック、その型などはブロックをダブルクリックすると編集できます。

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

```xml:sample.launch
<?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（無限）にするのを忘れないでください！さもないと実行が一瞬で終わってしまいます！
![Screenshot from 2017-12-31 20-56-10.png](https://qiita-image-store.s3.amazonaws.com/0/160346/3b7f2546-6a28-6539-c2f4-74593d3dd46d.png)

# C++へのコンバート
Simulinkモデルができて動作確認が取れたところでC++のROSノードへコンバートしてみましょう
Simulinkのバーにある歯車のマークをクリックしてコンフィギュレーションパラメータを設定しましょう
![Screenshot from 2017-12-31 21-09-02.png](https://qiita-image-store.s3.amazonaws.com/0/160346/95fab4bf-0ff9-639a-3e8b-dc35474841f2.png)
ここでソルバーオプションを<font color="Red">固定ステップ</font>にしてください
その後固定ステップサイズを自分の作りたい制御系に合わせて設定してください

次に、ハードウェア実行のタブを開きます
![Screenshot from 2017-12-31 21-13-42.png](https://qiita-image-store.s3.amazonaws.com/0/160346/db66a9c8-e9e1-dfa5-2d56-d69f616ec9dd.png)
ハードウェアボードにROSを選択します。
設定が終わったらソースコードに展開します。
ctrl+Bのショートカットを使うかコード->C/C++コード->ハードウェアに展開を選びます。
しばらくするとこんな画面が出てきてソースコードが生成されます。
![Screenshot from 2017-12-31 21-19-46.png](https://qiita-image-store.s3.amazonaws.com/0/160346/8f3091b1-6afb-4f09-929d-26732b049537.png)

生成したディレクトリを確認するとなんかいろいろ増えてると思います。
![Screenshot from 2017-12-31 21-22-46.png](https://qiita-image-store.s3.amazonaws.com/0/160346/e05d08b7-f860-b417-20bf-322578977db1.png)

./build_ros_model.sh.shの指示どおりにビルドしていきましょう。
![Screenshot from 2017-12-31 21-24-35.png](https://qiita-image-store.s3.amazonaws.com/0/160346/89d13aaf-9d80-0bf0-9b32-b4e1c10c12da.png)

このコマンドを実行すると、なんかいろいろ警告はでるのですが、ビルドに成功しました！！
![Screenshot from 2017-12-31 21-26-00.png](https://qiita-image-store.s3.amazonaws.com/0/160346/71d0f119-5275-d0ec-2e2b-75635448720b.png)
それでは早速テストしてみましょう。

# 生成したROSノードの実行テスト
以下のようなlaunchファイルを作成してroslaunchしてください。

```xml:sample.launch
<?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>
```

![Screenshot from 2017-12-31 21-30-41.png](https://qiita-image-store.s3.amazonaws.com/0/160346/acf227da-e04d-9880-a767-319ae89a581e.png)
rqt_graphで確認したところノード同士は通信できていますね！！

![Screenshot from 2017-12-31 21-33-08.png](https://qiita-image-store.s3.amazonaws.com/0/160346/5b47ce45-69b5-c477-ea60-5d5537fc7d83.png)
受け取ってるデータにも異常はなく、topicのパブリッシュレートも正確です。

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