mbedとrosでドローンを飛ばす(1)~環境構築とmbedでpubsub~

# モチベ
初投稿になります。あるきっかけでrosを勉強しているうちに実際にモノを動かしてみたくなりました。
2年前くらいに買って挫折したドローンの作成キットがあったので、これのフライトコントローラを使用せず、mbedで制御をして、それらしいものを作るのが目標です。
mbedは1年前くらいにやっていた学生プロジェクトで使ったことがあり、rosとの通信もサポートされているという理由から使いました。
バージョン管理にも慣れたいので、gitを使います。
本格的なロボットの開発経験がなかったので、これを作ることを通じていろいろ身につけたいと思っています。
拙い文章ですが誰かのお役に立てたら嬉しいです。

# 目的
- gitに慣れる
- rosに慣れる
- センサの通信や無線、シリアル通信など一通り経験する
- 姿勢制御の実装を経験する

などです。また増えたり減ったりするかもです。

# 環境
- ubuntu 18.04
- ros melodic
- mbed LPC1768

※ドローンを操作するコントローラーとしてps3コントローラを使います。

# 環境構築
rosとmbedの環境構築を行います。
### ros
いっぱいある記事やチュートリアルを見てインストール
特に[この記事](https://qiita.com/yamanotaka2020/items/ab3228048415a0f5d8)を参考にさせていただきました。


### mbedとros
[公式のチュートリアル](http://wiki.ros.org/rosserial_mbed/Tutorials/rosserial_mbed%20Setup)通りにインストール
オフラインコンパイルの方でやりました

# mbedでpublishされたhelloworldをpcで見る

`~/catkin_ws/src/rosserial/rosserial_mbed/src/examples/HelloWorld`
のコンパイルを行う。
LPC1768しか使わないのでmakefileを以下のように修正します。

```makefile
修正前
DEVICES         := LPC1768 KL25Z NUCLEO_F401RE
修正後
DEVICES         := LPC1768
```
makefileをこのようにしておかないと、後々、mbedのピンを指定するときにコンパイルエラーになってしまいます。修正後makeし、バイナリをmbedにコピーします。

別ターミナルで

```terminal
roscore
```
デバイスの権限を変更します。デバイスは適宜読み替えます。

```terminal
sudo chmod a+rw /dev/ttyACM0
```

mbedとシリアル通信を開始します。

```terminal
rosrun rosserial_python serial_node.py /dev/ttyACM0
```
mbedからのpublishを確認します。

```terminal
rostopic echo chatter
```
ターミナルにhello_worldが表示されればokです。

# mbedにpublishする
pc側から値をいじれるように(デューティー比など)雛形を作ります。

```pub_sub.cpp
#include "mbed.h"
#include <ros.h>
#include <geometry_msgs/Pose2D.h>
#include <std_msgs/Float32.h>
#include <std_msgs/UInt32.h>

#define PERIOD  20

ros::NodeHandle nh;

std_msgs::Float32 duty;
ros::Publisher pub("now_duty", &duty);

DigitalOut led1 = LED1;
DigitalOut led2 = LED2;

void update_duty(const std_msgs::Float32& input_duty)
{
    duty.data = input_duty.data;  //  &input_duty だとダメ？？？
    pub.publish(&duty);  //  &input_duty だとダメ？？？
    led1 = !led1;   
}

ros::Subscriber<std_msgs::Float32> sub("input_duty", &update_duty);

int main()
{
    nh.initNode();
    nh.advertise(pub);
    nh.subscribe(sub);
    while(1)
    {
        nh.spinOnce();
        wait_ms(10);
        led2 = !led2;
    }
}
```
makeしてmbedに書き込みます。
その後mbedとのシリアル通信を開始します。

```terminal
roscore
rosrun rosserial_python serial_node.py /dev/ttyACM0
```
mbedからのpublishを確認する準備をします。

```terminal
rostopic echo now_duty
```
pcからmbedに値をpublishします。

```terminal
rostopic pub input_duty std_msgs/Float32 '0.01'
```
これでrostopic echo しているターミナルに0.01が表示されるはずですが、10回に8回くらいの頻度でコールバック関数が呼ばれなくなる不具合が発生しました。

https://groups.google.com/g/ros-japan-users/c/En428bXdCRk
↑によるとメモリ関係のエラーの可能性があるようです。

https://qiita.com/motoms/items/c81d85b2e7967a780eb9
↑によるとros.hでnodehandleのメモリの設定を変更できるようなので確認してみました。
`~/ros/lib/ros_lib/ros.h(43行目)`

```ros.h
修正前
typedef NodeHandle_<MbedHardware> NodeHandle;
修正後
typedef NodeHandle_<MbedHardware, 25, 25, 2048, 2048> NodeHandle;
}
```
デフォルトは左から、 `25, 25, 512, 512`です。
これで毎回コールバック関数が呼ばれるようになりました。

また[この記事](https://qiita.com/motoms/items/c81d85b2e7967a780eb9)にbaud_rateは高いほうが良いと書かれていたので、baud_rateをmaxにして同じように通信ができるかを確認しました。

[ここ](https://os.mbed.com/forum/mbed/topic/893/?page=1#comment-4415)によるとmbedの最大ボードレートは`921600`らしいです。

`~/ros/lib/ros_lib/MbedHardware.h`　MbedHardwareクラスのコンストラクタでボードレートとピンの設定ができるっぽいです。

```MbedHardware.h
class MbedHardware {
  public:
    MbedHardware(PinName tx, PinName rx, long baud = 57600)
      :iostream(tx, rx){
      baud_ = baud;
      t.start();
    }

    MbedHardware()
      :iostream(USBTX, USBRX) {
        //baud_ = 57600;
        baud_ = 921600;
        t.start();
    }
(以下略)
```

とりあえず今はusb接続しているので、引数なしコンストラクタのbaud_を`921600`に変更しました。
将来的にはxbeeで無線シリアル通信を行うので、引数ありコンストラクタを使えば良さそうです。

修正後makeし直して、

```terminal
roscore
rosrun rosserial_python serial_node.py _port:=/dev/ttyACM0 _baud:=921600
```
rostopicで通信できることを確認しました。

これでps3コントローラでmbedに入力を行う準備ができました。
次回はgitにバージョン管理ができるディレクトリを作製してみます。

ここまでお読み頂きありがとうございました。