ROS+Matlab まとめと連携

# はじめに
ROS＋MATLABを使用する機会があったので
何かの参考になれば幸いです

https://jp.mathworks.com/help/robotics/examples/get-started-with-ros.html

を参考にしました

github:https://github.com/Shunichi09

# 環境
- Windows10
- Matlab2018a(Robotics_tool_boxが入ってることを確認してください)
- ubuntu 16.04 LTS
- ROS kinetic

# 目標
この図のようにすることが目標です
![image.png](https://qiita-image-store.s3.amazonaws.com/0/261584/48cf6440-07f9-d67d-06ee-5517ac711281.png)


# 準備（ubuntu側）
- 複数台PCを使う場合は，ubuntuPCのホームで

```
pluma .bashrc
```

と実行
なにやらテキスト画面が出てくるので，
その中に

```
export ROS_IP=123.456.7.891
export ROS_MASTER_URI=http://123.456.7.891:11311
```

を追加（IPは適当なので各自で確認してください）
これでIPが設定され，ほかのPCからこのPCのマスターにアクセスできます
もし，マスターを変えたい場合はこの

```
ROS_MASTER_URI=http://123.456.7.891:11311
```

のIP（123.456.7.891）を変更すればよいです（11311は変更しないでください）

ちなみにIPの確認方法は，基本的には

```
ifconfig
```

でみれます
自分の使いたいのがどのネット設定かみて設定してください
イーサネットなのか，それとも，Wifiなのか
お任せしますが

# 準備（Windows）
とくにありません
matlabでrosinitとためしに打ってみて，
以下のメッセージがでれば大丈夫です

```
Initializing ROS master on http://「PC名」:11311/.
Initializing global node /matlab_global_node_12345 with NodeURI http://「PC名」:53112/
```

## 補足
先日VMのubuntuとMainで回っているWindowsのmatlabで通信できることが分かりました
これによって，Kinectなどを1台のWindowsで回したい（openniは精度がそんなによくないので）ものがある場合でも対応できるようになります
つまり
ROSCORE(ubuntu)⇔Matlab(Windows)⇔Kinect(windows)が可能になります
するとロボットなどにも応用が可能になります
モータの指令はubuntuでだしながらkinectで．．．みたいな

# MatlabでのROSの使用方法
MatlabでROSできるとはいえどぶっちゃけそんなにできません
たぶん調べた感じgazeboとrvizは回らないのでお絵かきは別にしないといけません　まぁ当然ですが
ただ，Matlabで処理したいこと（最適化のツールボックスなど）があれば使用することができます
なので基本は
Publish
と
Subscribe
のみです！
では，ノードの立ち上げから説明をしていきます

# ノードの立ち上げ(matlab)
まず

```
rosinit
```
で立ち上げます　ですがこれだけだとubuntu側にはアクセスできないので

```
rosinit('http://123.456.7.891:11311')
```

です

# Publish(Matlab)
Publisherを作ります

```matlab
rospublisher
```

を使用します
以下はサンプルです

```matlab
% Publisher_1
% Topic Name : /cmd_vel
% Topic Type : geometry_msgs/Twist
matlab_pub = rospublisher('/cmd_vel', 'geometry_msgs/Twist');%cmd_vel を送信するPublisher%送信→Publisher（初期化）
publish_command = rosmessage(matlab_pub);
```

publishする際は

```matlab
send(matlab_pub, publish_command);
```

で送ってください

## メッセージの作り方

```matlab
% Topic name : publish_command
% publisher : matlab_pub
publish_command = rosmessage(matlab_pub);
```

これで作成することができます
中身にアクセスする際は .X や .theta や .Pose.X などなど型によって異なるので各自確認をお願いいたします

# Subscribe(Matlab)
基本的には2つあります！

```matlab
receive()
```

を使用する場合と

callback

を使用する場合です．
それぞれによって良いところと悪いところがあります
まず，receiveの場合のSubscriberの宣言についてみていきましょう

```matlab
% Subscriber
% Topic Name : /pose
% Topic Type : geometry_msgs/Pose2D (MatlabはSubscribeする場合はTopic名のみで受信可能)
matlab_sub = rossubscriber('/pose'); 
```

こうすれば処理の中で

```matlab
data = receive(matlab_sub_node, 0.01)
```

とすれば値を受け取ることができます！簡単
dataの中身はTopicの型によっても異なるので，data.Xとかdata.Yとかでアクセスできます　上手く行かない時はワークスペース内のメッセージを追ってみてください

ちなみに0.01はタイムアウトです（つまり0.01秒まってこなかったらこの処理は飛ばします）

次にcallbackを使う場合です

```matlab
% Subscriber
% Topic Name : /pose
% Topic Type : geometry_msgs/Pose2D (MatlabはSubscribeする場合はTopic名のみで受信可能)
matlab_sub = rossubscriber('/pose', @control_func); 
```

になりますね！
異なるのは@control_funcのところ
これでcallback関数を定義できています

callbackの関数でははじめに

```matlab
function control_func(obj, message, src);

%% global変数
global matlab_pub
global publish_command

send(matlab_pub, publish_command);
```

と宣言してください
messageの中に，messageが入っているので後は好きにできます．
そのままmessageをつかってpubしたい場合は，以下に書いたように，global変数でｓpublishするメッセージとpublisherを置いておけば，それを使ってsendできます

```matlab
function control_func(obj, message, src);

%% global変数
global matlab_pub
global publish_command

send(matlab_pub, publish_command);
```

## 疑問
このcallback関数の問題は，並行処理できてるのかという疑問が発生しました
つまり，publishしながら，subscribeしたいとき（受け取ってからpubではありません）
調べてみたら．．．
~~どうやらできないみたいですね~~
~~なので，pubとsubを同時にやりたかったら値は初期化しておいて，while文の中にreceiveを入れてtimeoutさせるのがよさそうです~~

ごめんなさいできるみたいです
なので，普通にsubscribeすれば別の処理で回ってくれるそうです

# 参考プログラム
まとめをのせておきます
while文版

```matlab
%% サンプル
%% 初期化
clear
close all

% ROS初期化/IPを確認
rosinit('http://「IP」:11311')

% Publisher
% Topic Name : /cmd_vel
% Topic Type : geometry_msgs/Twist
matlab_pub = rospublisher('/cmd_vel', 'geometry_msgs/Twist');%cmd_vel を送信するPublisher%送信→Publisher（初期化）
% message
command = rosmessage(matlab_pub);

% Subscriber
% Topic Name : /pose
% Topic Type : geometry_msgs/Pose2D (MatlabはSubscribeする場合はTopic名のみで受信可能)
matlab_sub = rossubscriber('/pose');


% Publish_message と　Subscribe_messageの初期設定を各自書いてください

%% Main loop
while 1 
    % data 受信
    data = receive(matlab_sub, 0.01); % timeout = 10ms   
    
    
    % main処理
    
    % publish_message作成
    
    % message送信
    send(matlab_pub ,command);
    
end
```

callback版

```matlab
%% サンプル
%% 初期化
clear
close all

% global変数
global matlab_pub_node
global command

% ROS初期化/IPを確認
rosinit('http://「IP」:11311')

% Publisher
% Topic Name : /cmd_vel
% Topic Type : geometry_msgs/Twist
matlab_pub = rospublisher('/cmd_vel', 'geometry_msgs/Twist');%cmd_vel を送信するPublisher%送信→Publisher（初期化）
% message
command = rosmessage(matlab_pub);

%% Subscriber
% Topic Name : /pose
% Topic Type : geometry_msgs/Pose2D (MatlabはSubscribeする場合はTopic名のみで受信可能)
% Callback_func : control_func
% Topicを受信し次第@control_funcを実行
matlab_sub_node = rossubscriber('/pose', @control_func);
```

control_func内の関数

```matlab
function control_func(obj, message, src);

%% global変数
global matlab_pub
global publish_command

send(matlab_pub, publish_command);
```


# いろいろ確認するときに便利なコマンド集（Matlab）
- しっかりトピックが飛んでいるか
    - 飛んでいるトピックをすべて見れます

```matlab
rostopic list
```

これぐらしかない笑
調べてみていただけると嬉しいです

# いろいろ確認するときに便利なコマンド集（Ubuntu）
- しっかりトピックが飛んでいるか
    - 飛んでいるトピックの数値が分かります

```
rostopic echo 「トピック名」
```
- 
    - 飛んでいるトピックをすべて見れます

```
rostopic list
```

- ROSトピックを試しにPublishしたいとき

```
rostopic pub 「トピック名」 「メッセージの型」 '数値'
```

間違ってるとエラー出してくれるのでそこのexpectedとかみてください
以下が具体例です

```py
# Topic_name : motor/twist/cmd_vel
# Topic type : geometry_msgs/Twist

rostopic pub motor/twist/cmd_vel geometry_msgs/Twist '[1.0, 0.0, 0.0]' '[0.0, 0.0, 0.0]'
```

- ノードの構成をみたいとき（グラフがでてきます）

```
rqt_graph
```