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

22322 ワード
C++ ROS rosbridge C++ テキストリンク

環境

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

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

インストールについてはROS講座02 インストールを参照してください。
またこの記事のプログラムはgithubにアップロードされています。ROS講座11 gitリポジトリを参照してください。

概要

ROSを使えばpub-subを簡単に行うことが出来ますが、例えばロボットのほかのPCで操作用のGUIを表示したいというときには複数のPC間でROSが動くように設定をしなければなりません（20 複数のPCでROS接続1）ただしこのの方法では。

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

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

jsonパーサー

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

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

Simple-WebSocket-Server

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

ライブラリのインストール

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

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

ソースコード

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

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をビルドしてできるライブラリのバイナリをリンクするのに必要です。

ビルド

cd ~/catkin_ws
catkin_make

実行

1つ目のターミナル
 roslaunch rosbridge_server rosbridge_websocket.launch
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説明
Simple-WebSocket-Server

目次ページへのリンク

ROS講座の目次へのリンク c

英語の勉強/ファーウェイ試験(C/C++)
Could not get unknown property 'release' for SigningConfig container of type org.gradle.api.internal