ROS講座121 rosbridge経由でpub/subをする

# 環境
この記事は以下の環境で動いています。

| 項目 | 値 |
|:-:|:-:|
| CPU | Core i5-8250U |
| Ubuntu | 18.04 |
| ROS | Melodic |

インストールについては[ROS講座02 インストール](https://qiita.com/srs/items/e0e0a9dc3f94c2d3348e)を参照してください。
またこの記事のプログラムはgithubにアップロードされています。[ROS講座11 gitリポジトリ](https://qiita.com/srs/items/25a56a6abda41dcf396e)を参照してください。

# 概要
ROSを使えばpub-subを簡単に行うことが出来ますが、例えばロボットのほかのPCで操作用のGUIを表示したいというときには複数のPC間でROSが動くように設定をしなければなりません（[20 複数のPCでROS接続1](https://qiita.com/srs/items/7d4aeb5e44138f97c770)）ただしこのの方法では。

* PCのIP設定などの設定が煩雑
* rosmasterが一度落ちるとシステム全体を再起動しないといけない
* ROSのそのままの通信なのでランダムに多数のポートを使った通信をする
* 同一のセグメントにいないと使用不可
* 複数のマシンに同時につなぐなどの運用が不可

等のように実際に使おうとすると多くの制約がつきます。
そこでWEbからのアクセスの時に使ったようにrosbridgeを使ってこれ越しにpub,subする方法を解説します。

## jsonパーサー
rosbridgeではデータをjson形式にしてやり取りします。json parserは数々のものがありますが、ROSパッケージとして以下のjson transportがあるので今回はこれを使います。これはnlohmannをラップしているものです。以下のコマンドでインストールできます。

```shell:jsonパーサーのインストール
sudo apt install ros-melodic-json-transport
```

## Simple-WebSocket-Server
今回rosbridge_serverにはwebsocketという通信方式でアクセスします。websocketとはtcp上で実装されているプロトコルで、双方向通信を簡単に行うためのものです。今回Simple-WebSocket-Serverというライブラリを使用します。

## ライブラリのインストール
このライブラリはバイナリではなくソースで提供しているので、git submoduleで抱えてみましょう

```shell:submoduleの追加
cd ros_lecture/app_lecture
git submodule add https://gitlab.com/eidheim/Simple-WebSocket-Server.git
```

# ソースコード

## cpp

```cpp:app_lecture/src/rosbridge_client_example.cpp
#include <stdio.h>
#include <client_ws.hpp>
#include <memory>
#include <thread>

#include <ros/static_assert.h>
#include <json_transport/json_transport.hpp>

using WsClient = SimpleWeb::SocketClient<SimpleWeb::WS>;

class RosBrigeClientExample
{
public:
  RosBrigeClientExample(std::string target) : client_(target)
  {
    using namespace std::placeholders;
    client_.on_open = std::bind(&RosBrigeClientExample::onOpen, this, _1);
    client_.on_message = std::bind(&RosBrigeClientExample::onMessage, this, _1, _2);
    client_.on_close = std::bind(&RosBrigeClientExample::onClose, this, _1, _2, _3);
    client_.on_error = std::bind(&RosBrigeClientExample::onError, this, _1, _2);
    send_thread_ = std::thread(std::bind(&RosBrigeClientExample::sendThread, this));
    ws_thread_ = std::thread([&] { client_.start(); });
  }

private:
  void onOpen(std::shared_ptr<WsClient::Connection> connection)
  {
    std::cout << "Client: Opened connection" << std::endl;
    connection_ = connection;

    // advertise
    json_transport::json_t command;
    command["op"] = "advertise";
    command["topic"] = "/hello_from_client";
    command["type"] = "std_msgs/String";
    connection_->send(command.dump());

    // subscribe
    command.clear();
    command["op"] = "subscribe";
    command["topic"] = "/hello_from_ros";
    command["type"] = "std_msgs/String";
    connection_->send(command.dump());
  };

  void onMessage(std::shared_ptr<WsClient::Connection> connection, std::shared_ptr<WsClient::InMessage> in_message)
  {
    json_transport::json_t recv = json_transport::json_t::parse(in_message->string());
    if (recv["topic"].is_string() && recv["topic"] == "/hello_from_ros")
    {
      std::cout << "recv: " << recv["msg"]["data"] << std::endl;
    }
  };

  void onClose(std::shared_ptr<WsClient::Connection> connection, int status, const std::string& reason)
  {
    std::cout << "Client: Closed connection with status code " << status << std::endl;
  };

  void onError(std::shared_ptr<WsClient::Connection> connection, const SimpleWeb::error_code& ec)
  {
    std::cout << "Client: Error: " << ec << ", error message: " << ec.message() << std::endl;
  };

  void sendThread(void)
  {
    while (true)
    {
      if (connection_)
      {
        json_transport::json_t command, msg;
        command["op"] = "publish";
        command["topic"] = "/hello_from_client";
        msg["data"] = "hello client->ros";
        command["msg"] = msg;
        const std::string output = command.dump();
        connection_->send(output);
      }
      sleep(2);
    }
  }

  WsClient client_;
  std::shared_ptr<WsClient::Connection> connection_;
  std::thread send_thread_;
  std::thread ws_thread_;
};

int main(int argc, char** argv)
{
  printf("start\n");
  RosBrigeClientExample rosbridge_client_example("localhost:9090");
  while (true)
  {
    sleep(1);
  }
  return 0;
}
```

* `SimpleWeb::SocketClient<SimpleWeb::WS>;`がwebsocketのクライアントを操作するクラスです。
    * `SimpleWeb::SocketClient<SimpleWeb::WS>("localhost:9090/hoge")`とインスタンス化します。引数はターゲットとなるWebSocketのサーバーのアドレスです。
    * on_open、on_message、on_close、on_errorの4つのメソッドを登録します。Websocketのサーバーにつながるとon_openが、サーバーからのメッセージが来ればon_messageが呼び出されます。
    * `start()`はブロッキングなメソッドなので別スレッドに今回しました。
* WebSocket自体はただの文字列の送受信をするにとどまります。rosbridgeではjson形式の文字列を送受信して通信を行います。
    * `{"op":"advertise","topic":"/hello_from_client","type":"std_msgs/String"}`でpublishの開始（advertise）をします。
    * `{"op":"subscribe","topic":"/hello_from_ros","type":"std_msgs/String"}`でsubscribeの開始をします。
    * 実際にtopicを送る際は`{"op":"publish","topic":"/hello_from_client","type":"std_msgs/String","msg":"*******"}`とします。msgの内容はmsgフィールド以下に詰めます。

## CMakeList

```Cmake:app_lecture/CMakeLists.txtの編集事項
find_package(catkin REQUIRED COMPONENTS
  roscpp
  json_transport # この行を追加(1)
)
add_subdirectory(Simple-WebSocket-Server) # この行を追加(2)

include_directories(
# include
  ${catkin_INCLUDE_DIRS}
  Simple-WebSocket-Server　# この行を追加(3)
)

add_executable(rosbridge_client_example src/rosbridge_client_example.cpp)

target_link_libraries(rosbridge_client_example
  simple-websocket-server　# この行を追加(4)
)
```

* (1)json_transport が必要なので追加します。
* (2)submoduleとして取得したSimple-WebSocket-Serverをビルドするために必要です。
* (3)Simple-WebSocket-Serverディレクトリのヘッダファイルをインクルードするのに必要です。
* (4)Simple-WebSocket-Serverをビルドしてできるライブラリのバイナリをリンクするのに必要です。

# ビルド
```shell
cd ~/catkin_ws
catkin_make
```

# 実行

```shell:1つ目のターミナル
 roslaunch rosbridge_server rosbridge_websocket.launch 
```

```shell:2つ目のターミナル
rosrun app_lecture rosbridge_client_example
```

clientのプログラムがrosbridge_serverに接続すると1つ目のターミナルのほうに`[INFO] [....]: Client connected.  1 clients total.`と表示されます。
この状態で`rostopic list`とすると`/hello_from_client`と`/hello_from_ros`の2つのROSトピックがあることが分かります。

## client->rosbridgeの通信の確認
`rostopic echo /hello_from_client`とするとclient由来のmsgが流れてくることが分かります。

## rosbridge->clientの通信の確認
`rostopic pub -1 /hello_from_ros std_msgs/String "data: 'sample'"`とするとclient側の画面にdataの内容が表示されます。

# 参考
[nlohmannのAPI説明](https://github.com/nlohmann/json#stl-like-access)
[Simple-WebSocket-Server](https://gitlab.com/eidheim/Simple-WebSocket-Server)


# 目次ページへのリンク
[ROS講座の目次へのリンク](https://qiita.com/srs/items/5f44440afea0eb616b4a) c